Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Added libspe2 parallel Cell SPE support contribution by IBM Germany 'Extreme Blue' project:

Browse Source 
Thanks to Benjamin Hoeferlin, Minh Cuong Tran,Martina Huellmann,Frederick Roth.
This commit is contained in:
ejcoumans
2007-09-26 23:43:51 +00:00
parent 5ebab3e59b
commit 29dd9681f4
15 changed files with 4046 additions and 3507 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
Extras/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuContactResult.cpp
View File

@@ -19,9 +19,6 @@ subject to the following restrictions:
//#define DEBUG_SPU_COLLISION_DETECTION 1 //#define DEBUG_SPU_COLLISION_DETECTION 1
#include "SpuContactResult.h"
SpuContactResult::SpuContactResult() SpuContactResult::SpuContactResult()
{ {
m_manifoldAddress = 0; m_manifoldAddress = 0;

102
Extras/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuConvexPenetrationDepthSolver.h
View File

@@ -1,51 +1,51 @@
/* /*
Bullet Continuous Collision Detection and Physics Library Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty. This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software. In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose, Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely, including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions: subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution. 3. This notice may not be removed or altered from any source distribution.
*/ */
#ifndef CONVEX_PENETRATION_DEPTH_H #ifndef CONVEX_PENETRATION_DEPTH_H
#define CONVEX_PENETRATION_DEPTH_H #define CONVEX_PENETRATION_DEPTH_H
class btStackAlloc; class btStackAlloc;
class btIDebugDraw; class btIDebugDraw;
class SpuVoronoiSimplexSolver; class SpuVoronoiSimplexSolver;
#include <LinearMath/btTransform.h> #include <LinearMath/btTransform.h>
#include <LinearMath/btPoint3.h> #include <LinearMath/btPoint3.h>
///ConvexPenetrationDepthSolver provides an interface for penetration depth calculation. ///ConvexPenetrationDepthSolver provides an interface for penetration depth calculation.
class SpuConvexPenetrationDepthSolver class SpuConvexPenetrationDepthSolver
{ {
public: public:
virtual ~SpuConvexPenetrationDepthSolver() {}; virtual ~SpuConvexPenetrationDepthSolver() {};
virtual bool calcPenDepth( SpuVoronoiSimplexSolver& simplexSolver, virtual bool calcPenDepth( SpuVoronoiSimplexSolver& simplexSolver,
void* convexA,void* convexB,int shapeTypeA, int shapeTypeB, float marginA, float marginB, void* convexA,void* convexB,int shapeTypeA, int shapeTypeB, float marginA, float marginB,
btTransform& transA,const btTransform& transB, btTransform& transA,const btTransform& transB,
btVector3& v, btPoint3& pa, btPoint3& pb, btVector3& v, btPoint3& pa, btPoint3& pb,
class btIDebugDraw* debugDraw,btStackAlloc* stackAlloc, class btIDebugDraw* debugDraw,btStackAlloc* stackAlloc,
struct SpuConvexPolyhedronVertexData* convexVertexData struct SpuConvexPolyhedronVertexData* convexVertexData
) const = 0; ) const = 0;
}; };
#endif //CONVEX_PENETRATION_DEPTH_H #endif //CONVEX_PENETRATION_DEPTH_H

2887
Extras/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuGatheringCollisionTask.cpp
View File

File diff suppressed because it is too large Load Diff

153
Extras/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuGatheringCollisionTask.h
View File

@@ -1,25 +1,128 @@
/* /*
Bullet Continuous Collision Detection and Physics Library Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty. This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software. In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose, Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely, including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions: subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution. 3. This notice may not be removed or altered from any source distribution.
*/ */
#ifndef SPU_GATHERING_COLLISION_TASK_H #ifndef SPU_GATHERING_COLLISION_TASK_H
#define SPU_GATHERING_COLLISION_TASK_H #define SPU_GATHERING_COLLISION_TASK_H
struct SpuGatherAndProcessPairsTaskDesc; #include "../PlatformDefinitions.h"
//#define DEBUG_SPU_COLLISION_DETECTION 1
void processCollisionTask(void* userPtr, void* lsMemory);
void* createCollisionLocalStoreMemory(); ///Task Description for SPU collision detection
struct SpuGatherAndProcessPairsTaskDesc
#endif //SPU_GATHERING_COLLISION_TASK_H {
uint64_t inPtr;//m_pairArrayPtr;
//mutex variable
uint32_t m_someMutexVariableInMainMemory;
uint64_t m_dispatcher;
uint32_t numOnLastPage;
uint16_t numPages;
uint16_t taskId;
struct CollisionTask_LocalStoreMemory* m_lsMemory;
}
#if defined(__CELLOS_LV2__) || defined(USE_LIBSPE2)
__attribute__ ((aligned (128)))
#endif
;
void processCollisionTask(void* userPtr, void* lsMemory);
void* createCollisionLocalStoreMemory();
#if defined(USE_LIBSPE2) && defined(__SPU__)
#include "../SpuLibspe2Support.h"
#include <spu_intrinsics.h>
#include <spu_mfcio.h>
#include <SpuFakeDma.h>
int main(unsigned long long speid, addr64 argp, addr64 envp)
{
printf("SPU: hello \n");
ATTRIBUTE_ALIGNED128(btSpuStatus status);
ATTRIBUTE_ALIGNED16( SpuGatherAndProcessPairsTaskDesc taskDesc ) ;
unsigned int received_message = Spu_Mailbox_Event_Nothing;
bool shutdown = false;
cellDmaGet(&status, argp.ull, sizeof(btSpuStatus), DMA_TAG(3), 0, 0);
cellDmaWaitTagStatusAll(DMA_MASK(3));
status.m_status = Spu_Status_Free;
status.m_lsMemory.p = createCollisionLocalStoreMemory();
cellDmaLargePut(&status, argp.ull, sizeof(btSpuStatus), DMA_TAG(3), 0, 0);
cellDmaWaitTagStatusAll(DMA_MASK(3));
while ( btLikely( !shutdown ) )
{
received_message = spu_read_in_mbox();
if( btLikely( received_message == Spu_Mailbox_Event_Task ))
{
printf("SPU: received Spu_Mailbox_Event_Task\n");
// refresh the status
cellDmaGet(&status, argp.ull, sizeof(btSpuStatus), DMA_TAG(3), 0, 0);
cellDmaWaitTagStatusAll(DMA_MASK(3));
btAssert(status.m_status==Spu_Status_Occupied);
cellDmaGet(&taskDesc, status.m_taskDesc.p, sizeof(SpuGatherAndProcessPairsTaskDesc), DMA_TAG(3), 0, 0);
cellDmaWaitTagStatusAll(DMA_MASK(3));
printf("SPU:processCollisionTask\n");
processCollisionTask((void*)&taskDesc, taskDesc.m_lsMemory);
printf("SPU:finished processCollisionTask\n");
}
else
{
printf("SPU: received ShutDown\n");
if( btLikely( received_message == Spu_Mailbox_Event_Shutdown ) )
{
shutdown = true;
}
else
{
//printf("SPU - Sth. recieved\n");
}
}
// set to status free and wait for next task
status.m_status = Spu_Status_Free;
cellDmaLargePut(&status, argp.ull, sizeof(btSpuStatus), DMA_TAG(3), 0, 0);
cellDmaWaitTagStatusAll(DMA_MASK(3));
}
printf("SPU: shutdown\n");
return 0;
}
#endif // USE_LIBSPE2
#endif //SPU_GATHERING_COLLISION_TASK_H

622
Extras/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuGjkPairDetector.cpp
View File

@@ -1,311 +1,311 @@
/* /*
Bullet Continuous Collision Detection and Physics Library Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty. This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software. In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose, Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely, including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions: subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution. 3. This notice may not be removed or altered from any source distribution.
*/ */
#include "SpuGjkPairDetector.h" #include "SpuGjkPairDetector.h"
#include "SpuConvexPenetrationDepthSolver.h" #include "SpuConvexPenetrationDepthSolver.h"
#include "SpuLocalSupport.h" #include "SpuLocalSupport.h"
#if defined(DEBUG) || defined (_DEBUG) #if defined(DEBUG) || defined (_DEBUG)
#include <stdio.h> //for debug printf #include <stdio.h> //for debug printf
#ifdef __SPU__ #ifdef __SPU__
#include <spu_printf.h> #include <spu_printf.h>
#define printf spu_printf #define printf spu_printf
#endif //__SPU__ #endif //__SPU__
#endif #endif
//must be above the machine epsilon //must be above the machine epsilon
#define REL_ERROR2 btScalar(1.0e-6) #define REL_ERROR2 btScalar(1.0e-6)
//temp globals, to improve GJK/EPA/penetration calculations //temp globals, to improve GJK/EPA/penetration calculations
int gSpuNumDeepPenetrationChecks = 0; int gSpuNumDeepPenetrationChecks = 0;
int gSpuNumGjkChecks = 0; int gSpuNumGjkChecks = 0;
SpuGjkPairDetector::SpuGjkPairDetector(void* objectA,void* objectB,int shapeTypeA, int shapeTypeB, float marginA,float marginB,SpuVoronoiSimplexSolver* simplexSolver, const SpuConvexPenetrationDepthSolver* penetrationDepthSolver) SpuGjkPairDetector::SpuGjkPairDetector(void* objectA,void* objectB,int shapeTypeA, int shapeTypeB, float marginA,float marginB,SpuVoronoiSimplexSolver* simplexSolver, const SpuConvexPenetrationDepthSolver* penetrationDepthSolver)
:m_cachedSeparatingAxis(float(0.),float(0.),float(1.)), :m_cachedSeparatingAxis(float(0.),float(0.),float(1.)),
m_penetrationDepthSolver(penetrationDepthSolver), m_penetrationDepthSolver(penetrationDepthSolver),
m_simplexSolver(simplexSolver), m_simplexSolver(simplexSolver),
m_minkowskiA(objectA), m_minkowskiA(objectA),
m_minkowskiB(objectB), m_minkowskiB(objectB),
m_shapeTypeA(shapeTypeA), m_shapeTypeA(shapeTypeA),
m_shapeTypeB(shapeTypeB), m_shapeTypeB(shapeTypeB),
m_marginA(marginA), m_marginA(marginA),
m_marginB(marginB), m_marginB(marginB),
m_ignoreMargin(false), m_ignoreMargin(false),
m_lastUsedMethod(-1), m_lastUsedMethod(-1),
m_catchDegeneracies(1) m_catchDegeneracies(1)
{ {
} }
void SpuGjkPairDetector::getClosestPoints(const SpuClosestPointInput& input,SpuContactResult& output) void SpuGjkPairDetector::getClosestPoints(const SpuClosestPointInput& input,SpuContactResult& output)
{ {
btScalar distance=btScalar(0.); btScalar distance=btScalar(0.);
btVector3 normalInB(btScalar(0.),btScalar(0.),btScalar(0.)); btVector3 normalInB(btScalar(0.),btScalar(0.),btScalar(0.));
btVector3 pointOnA,pointOnB; btVector3 pointOnA,pointOnB;
btTransform localTransA = input.m_transformA; btTransform localTransA = input.m_transformA;
btTransform localTransB = input.m_transformB; btTransform localTransB = input.m_transformB;
btVector3 positionOffset = (localTransA.getOrigin() + localTransB.getOrigin()) * btScalar(0.5); btVector3 positionOffset = (localTransA.getOrigin() + localTransB.getOrigin()) * btScalar(0.5);
localTransA.getOrigin() -= positionOffset; localTransA.getOrigin() -= positionOffset;
localTransB.getOrigin() -= positionOffset; localTransB.getOrigin() -= positionOffset;
btScalar marginA = m_marginA; btScalar marginA = m_marginA;
btScalar marginB = m_marginB; btScalar marginB = m_marginB;
gSpuNumGjkChecks++; gSpuNumGjkChecks++;
//for CCD we don't use margins //for CCD we don't use margins
if (m_ignoreMargin) if (m_ignoreMargin)
{ {
marginA = btScalar(0.); marginA = btScalar(0.);
marginB = btScalar(0.); marginB = btScalar(0.);
} }
m_curIter = 0; m_curIter = 0;
int gGjkMaxIter = 1000;//this is to catch invalid input, perhaps check for #NaN? int gGjkMaxIter = 1000;//this is to catch invalid input, perhaps check for #NaN?
m_cachedSeparatingAxis.setValue(0,1,0); m_cachedSeparatingAxis.setValue(0,1,0);
bool isValid = false; bool isValid = false;
bool checkSimplex = false; bool checkSimplex = false;
bool checkPenetration = true; bool checkPenetration = true;
m_degenerateSimplex = 0; m_degenerateSimplex = 0;
m_lastUsedMethod = -1; m_lastUsedMethod = -1;
{ {
btScalar squaredDistance = SIMD_INFINITY; btScalar squaredDistance = SIMD_INFINITY;
btScalar delta = btScalar(0.); btScalar delta = btScalar(0.);
btScalar margin = marginA + marginB; btScalar margin = marginA + marginB;
m_simplexSolver->reset(); m_simplexSolver->reset();
for ( ; ; ) for ( ; ; )
//while (true) //while (true)
{ {
btVector3 seperatingAxisInA = (-m_cachedSeparatingAxis)* input.m_transformA.getBasis(); btVector3 seperatingAxisInA = (-m_cachedSeparatingAxis)* input.m_transformA.getBasis();
btVector3 seperatingAxisInB = m_cachedSeparatingAxis* input.m_transformB.getBasis(); btVector3 seperatingAxisInB = m_cachedSeparatingAxis* input.m_transformB.getBasis();
// btVector3 pInA = m_minkowskiA->localGetSupportingVertexWithoutMargin(seperatingAxisInA); // btVector3 pInA = m_minkowskiA->localGetSupportingVertexWithoutMargin(seperatingAxisInA);
// btVector3 qInB = m_minkowskiB->localGetSupportingVertexWithoutMargin(seperatingAxisInB); // btVector3 qInB = m_minkowskiB->localGetSupportingVertexWithoutMargin(seperatingAxisInB);
btVector3 pInA = localGetSupportingVertexWithoutMargin(m_shapeTypeA, m_minkowskiA, seperatingAxisInA,input.m_convexVertexData);//, &featureIndexA); btVector3 pInA = localGetSupportingVertexWithoutMargin(m_shapeTypeA, m_minkowskiA, seperatingAxisInA,input.m_convexVertexData);//, &featureIndexA);
btVector3 qInB = localGetSupportingVertexWithoutMargin(m_shapeTypeB, m_minkowskiB, seperatingAxisInB,input.m_convexVertexData);//, &featureIndexB); btVector3 qInB = localGetSupportingVertexWithoutMargin(m_shapeTypeB, m_minkowskiB, seperatingAxisInB,input.m_convexVertexData);//, &featureIndexB);
btPoint3 pWorld = localTransA(pInA); btPoint3 pWorld = localTransA(pInA);
btPoint3 qWorld = localTransB(qInB); btPoint3 qWorld = localTransB(qInB);
btVector3 w = pWorld - qWorld; btVector3 w = pWorld - qWorld;
delta = m_cachedSeparatingAxis.dot(w); delta = m_cachedSeparatingAxis.dot(w);
// potential exit, they don't overlap // potential exit, they don't overlap
if ((delta > btScalar(0.0)) && (delta * delta > squaredDistance * input.m_maximumDistanceSquared)) if ((delta > btScalar(0.0)) && (delta * delta > squaredDistance * input.m_maximumDistanceSquared))
{ {
checkPenetration = false; checkPenetration = false;
break; break;
} }
//exit 0: the new point is already in the simplex, or we didn't come any closer //exit 0: the new point is already in the simplex, or we didn't come any closer
if (m_simplexSolver->inSimplex(w)) if (m_simplexSolver->inSimplex(w))
{ {
m_degenerateSimplex = 1; m_degenerateSimplex = 1;
checkSimplex = true; checkSimplex = true;
break; break;
} }
// are we getting any closer ? // are we getting any closer ?
btScalar f0 = squaredDistance - delta; btScalar f0 = squaredDistance - delta;
btScalar f1 = squaredDistance * REL_ERROR2; btScalar f1 = squaredDistance * REL_ERROR2;
if (f0 <= f1) if (f0 <= f1)
{ {
if (f0 <= btScalar(0.)) if (f0 <= btScalar(0.))
{ {
m_degenerateSimplex = 2; m_degenerateSimplex = 2;
} }
checkSimplex = true; checkSimplex = true;
break; break;
} }
//add current vertex to simplex //add current vertex to simplex
m_simplexSolver->addVertex(w, pWorld, qWorld); m_simplexSolver->addVertex(w, pWorld, qWorld);
//calculate the closest point to the origin (update vector v) //calculate the closest point to the origin (update vector v)
if (!m_simplexSolver->closest(m_cachedSeparatingAxis)) if (!m_simplexSolver->closest(m_cachedSeparatingAxis))
{ {
m_degenerateSimplex = 3; m_degenerateSimplex = 3;
checkSimplex = true; checkSimplex = true;
break; break;
} }
btScalar previousSquaredDistance = squaredDistance; btScalar previousSquaredDistance = squaredDistance;
squaredDistance = m_cachedSeparatingAxis.length2(); squaredDistance = m_cachedSeparatingAxis.length2();
//redundant m_simplexSolver->compute_points(pointOnA, pointOnB); //redundant m_simplexSolver->compute_points(pointOnA, pointOnB);
//are we getting any closer ? //are we getting any closer ?
if (previousSquaredDistance - squaredDistance <= SIMD_EPSILON * previousSquaredDistance) if (previousSquaredDistance - squaredDistance <= SIMD_EPSILON * previousSquaredDistance)
{ {
m_simplexSolver->backup_closest(m_cachedSeparatingAxis); m_simplexSolver->backup_closest(m_cachedSeparatingAxis);
checkSimplex = true; checkSimplex = true;
break; break;
} }
//degeneracy, this is typically due to invalid/uninitialized worldtransforms for a btCollisionObject //degeneracy, this is typically due to invalid/uninitialized worldtransforms for a btCollisionObject
if (m_curIter++ > gGjkMaxIter) if (m_curIter++ > gGjkMaxIter)
{ {
#if defined(DEBUG) || defined (_DEBUG) #if defined(DEBUG) || defined (_DEBUG)
printf("SpuGjkPairDetector maxIter exceeded:%i\n",m_curIter); printf("SpuGjkPairDetector maxIter exceeded:%i\n",m_curIter);
printf("sepAxis=(%f,%f,%f), squaredDistance = %f, shapeTypeA=%i,shapeTypeB=%i\n", printf("sepAxis=(%f,%f,%f), squaredDistance = %f, shapeTypeA=%i,shapeTypeB=%i\n",
m_cachedSeparatingAxis.getX(), m_cachedSeparatingAxis.getX(),
m_cachedSeparatingAxis.getY(), m_cachedSeparatingAxis.getY(),
m_cachedSeparatingAxis.getZ(), m_cachedSeparatingAxis.getZ(),
squaredDistance, squaredDistance,
m_shapeTypeA, m_shapeTypeA,
m_shapeTypeB); m_shapeTypeB);
#endif #endif
break; break;
} }
bool check = (!m_simplexSolver->fullSimplex()); bool check = (!m_simplexSolver->fullSimplex());
//bool check = (!m_simplexSolver->fullSimplex() && squaredDistance > SIMD_EPSILON * m_simplexSolver->maxVertex()); //bool check = (!m_simplexSolver->fullSimplex() && squaredDistance > SIMD_EPSILON * m_simplexSolver->maxVertex());
if (!check) if (!check)
{ {
//do we need this backup_closest here ? //do we need this backup_closest here ?
m_simplexSolver->backup_closest(m_cachedSeparatingAxis); m_simplexSolver->backup_closest(m_cachedSeparatingAxis);
break; break;
} }
} }
if (checkSimplex) if (checkSimplex)
{ {
m_simplexSolver->compute_points(pointOnA, pointOnB); m_simplexSolver->compute_points(pointOnA, pointOnB);
normalInB = pointOnA-pointOnB; normalInB = pointOnA-pointOnB;
btScalar lenSqr = m_cachedSeparatingAxis.length2(); btScalar lenSqr = m_cachedSeparatingAxis.length2();
//valid normal //valid normal
if (lenSqr < 0.0001) if (lenSqr < 0.0001)
{ {
m_degenerateSimplex = 5; m_degenerateSimplex = 5;
} }
if (lenSqr > SIMD_EPSILON*SIMD_EPSILON) if (lenSqr > SIMD_EPSILON*SIMD_EPSILON)
{ {
btScalar rlen = btScalar(1.) / btSqrt(lenSqr ); btScalar rlen = btScalar(1.) / btSqrt(lenSqr );
normalInB *= rlen; //normalize normalInB *= rlen; //normalize
btScalar s = btSqrt(squaredDistance); btScalar s = btSqrt(squaredDistance);
btAssert(s > btScalar(0.0)); btAssert(s > btScalar(0.0));
pointOnA -= m_cachedSeparatingAxis * (marginA / s); pointOnA -= m_cachedSeparatingAxis * (marginA / s);
pointOnB += m_cachedSeparatingAxis * (marginB / s); pointOnB += m_cachedSeparatingAxis * (marginB / s);
distance = ((btScalar(1.)/rlen) - margin); distance = ((btScalar(1.)/rlen) - margin);
isValid = true; isValid = true;
m_lastUsedMethod = 1; m_lastUsedMethod = 1;
} else } else
{ {
m_lastUsedMethod = 2; m_lastUsedMethod = 2;
} }
} }
bool catchDegeneratePenetrationCase = bool catchDegeneratePenetrationCase =
(m_catchDegeneracies && m_penetrationDepthSolver && m_degenerateSimplex && ((distance+margin) < 0.01)); (m_catchDegeneracies && m_penetrationDepthSolver && m_degenerateSimplex && ((distance+margin) < 0.01));
//if (checkPenetration && !isValid) //if (checkPenetration && !isValid)
if (checkPenetration && (!isValid || catchDegeneratePenetrationCase )) if (checkPenetration && (!isValid || catchDegeneratePenetrationCase ))
{ {
//penetration case //penetration case
//if there is no way to handle penetrations, bail out //if there is no way to handle penetrations, bail out
if (m_penetrationDepthSolver) if (m_penetrationDepthSolver)
{ {
// Penetration depth case. // Penetration depth case.
btVector3 tmpPointOnA,tmpPointOnB; btVector3 tmpPointOnA,tmpPointOnB;
gSpuNumDeepPenetrationChecks++; gSpuNumDeepPenetrationChecks++;
bool isValid2 = m_penetrationDepthSolver->calcPenDepth( bool isValid2 = m_penetrationDepthSolver->calcPenDepth(
*m_simplexSolver, *m_simplexSolver,
m_minkowskiA,m_minkowskiB, m_minkowskiA,m_minkowskiB,
m_shapeTypeA, m_shapeTypeB, m_shapeTypeA, m_shapeTypeB,
marginA, marginB, marginA, marginB,
localTransA,localTransB, localTransA,localTransB,
m_cachedSeparatingAxis, tmpPointOnA, tmpPointOnB, m_cachedSeparatingAxis, tmpPointOnA, tmpPointOnB,
0,input.m_stackAlloc,input.m_convexVertexData 0,input.m_stackAlloc,input.m_convexVertexData
); );
if (isValid2) if (isValid2)
{ {
btVector3 tmpNormalInB = tmpPointOnB-tmpPointOnA; btVector3 tmpNormalInB = tmpPointOnB-tmpPointOnA;
btScalar lenSqr = tmpNormalInB.length2(); btScalar lenSqr = tmpNormalInB.length2();
if (lenSqr > (SIMD_EPSILON*SIMD_EPSILON)) if (lenSqr > (SIMD_EPSILON*SIMD_EPSILON))
{ {
tmpNormalInB /= btSqrt(lenSqr); tmpNormalInB /= btSqrt(lenSqr);
btScalar distance2 = -(tmpPointOnA-tmpPointOnB).length(); btScalar distance2 = -(tmpPointOnA-tmpPointOnB).length();
//only replace valid penetrations when the result is deeper (check) //only replace valid penetrations when the result is deeper (check)
if (!isValid || (distance2 < distance)) if (!isValid || (distance2 < distance))
{ {
distance = distance2; distance = distance2;
pointOnA = tmpPointOnA; pointOnA = tmpPointOnA;
pointOnB = tmpPointOnB; pointOnB = tmpPointOnB;
normalInB = tmpNormalInB; normalInB = tmpNormalInB;
isValid = true; isValid = true;
m_lastUsedMethod = 3; m_lastUsedMethod = 3;
} else } else
{ {
} }
} else } else
{ {
//isValid = false; //isValid = false;
m_lastUsedMethod = 4; m_lastUsedMethod = 4;
} }
} else } else
{ {
m_lastUsedMethod = 5; m_lastUsedMethod = 5;
} }
} }
} }
} }
if (isValid) if (isValid)
{ {
#ifdef __SPU__ #ifdef __SPU__
//spu_printf("distance\n"); //spu_printf("distance\n");
#endif //__CELLOS_LV2__ #endif //__SPU__
output.addContactPoint( output.addContactPoint(
normalInB, normalInB,
pointOnB+positionOffset, pointOnB+positionOffset,
distance); distance);
//printf("gjk add:%f",distance); //printf("gjk add:%f",distance);
} }
} }

186
Extras/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuGjkPairDetector.h
View File

@@ -1,93 +1,93 @@
/* /*
Bullet Continuous Collision Detection and Physics Library Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty. This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software. In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose, Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely, including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions: subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution. 3. This notice may not be removed or altered from any source distribution.
*/ */
#ifndef SPU_GJK_PAIR_DETECTOR_H #ifndef SPU_GJK_PAIR_DETECTOR_H
#define SPU_GJK_PAIR_DETECTOR_H #define SPU_GJK_PAIR_DETECTOR_H
#include "SpuContactResult.h" #include "SpuContactResult.h"
#include "SpuVoronoiSimplexSolver.h" #include "SpuVoronoiSimplexSolver.h"
class SpuConvexPenetrationDepthSolver; class SpuConvexPenetrationDepthSolver;
/// btGjkPairDetector uses GJK to implement the btDiscreteCollisionDetectorInterface /// btGjkPairDetector uses GJK to implement the btDiscreteCollisionDetectorInterface
class SpuGjkPairDetector class SpuGjkPairDetector
{ {
btVector3 m_cachedSeparatingAxis; btVector3 m_cachedSeparatingAxis;
const SpuConvexPenetrationDepthSolver* m_penetrationDepthSolver; const SpuConvexPenetrationDepthSolver* m_penetrationDepthSolver;
SpuVoronoiSimplexSolver* m_simplexSolver; SpuVoronoiSimplexSolver* m_simplexSolver;
void* m_minkowskiA; void* m_minkowskiA;
void* m_minkowskiB; void* m_minkowskiB;
int m_shapeTypeA; int m_shapeTypeA;
int m_shapeTypeB; int m_shapeTypeB;
float m_marginA; float m_marginA;
float m_marginB; float m_marginB;
bool m_ignoreMargin; bool m_ignoreMargin;
public: public:
//some debugging to fix degeneracy problems //some debugging to fix degeneracy problems
int m_lastUsedMethod; int m_lastUsedMethod;
int m_curIter; int m_curIter;
int m_degenerateSimplex; int m_degenerateSimplex;
int m_catchDegeneracies; int m_catchDegeneracies;
SpuGjkPairDetector(void* objectA,void* objectB,int m_shapeTypeA, int m_shapeTypeB, float marginA, float marginB, SpuVoronoiSimplexSolver* simplexSolver, const SpuConvexPenetrationDepthSolver* penetrationDepthSolver); SpuGjkPairDetector(void* objectA,void* objectB,int m_shapeTypeA, int m_shapeTypeB, float marginA, float marginB, SpuVoronoiSimplexSolver* simplexSolver, const SpuConvexPenetrationDepthSolver* penetrationDepthSolver);
virtual ~SpuGjkPairDetector() {}; virtual ~SpuGjkPairDetector() {};
virtual void getClosestPoints(const SpuClosestPointInput& input,SpuContactResult& output); virtual void getClosestPoints(const SpuClosestPointInput& input,SpuContactResult& output);
void setMinkowskiA(void* minkA) void setMinkowskiA(void* minkA)
{ {
m_minkowskiA = minkA; m_minkowskiA = minkA;
} }
void setMinkowskiB(void* minkB) void setMinkowskiB(void* minkB)
{ {
m_minkowskiB = minkB; m_minkowskiB = minkB;
} }
void setCachedSeperatingAxis(const btVector3& seperatingAxis) void setCachedSeperatingAxis(const btVector3& seperatingAxis)
{ {
m_cachedSeparatingAxis = seperatingAxis; m_cachedSeparatingAxis = seperatingAxis;
} }
void setPenetrationDepthSolver(SpuConvexPenetrationDepthSolver* penetrationDepthSolver) void setPenetrationDepthSolver(SpuConvexPenetrationDepthSolver* penetrationDepthSolver)
{ {
m_penetrationDepthSolver = penetrationDepthSolver; m_penetrationDepthSolver = penetrationDepthSolver;
} }
///don't use setIgnoreMargin, it's for Bullet's internal use ///don't use setIgnoreMargin, it's for Bullet's internal use
void setIgnoreMargin(bool ignoreMargin) void setIgnoreMargin(bool ignoreMargin)
{ {
m_ignoreMargin = ignoreMargin; m_ignoreMargin = ignoreMargin;
} }
}; };
#endif //SPU_GJK_PAIR_DETECTOR_H #endif //SPU_GJK_PAIR_DETECTOR_H

498
Extras/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuLocalSupport.h
View File

@@ -1,249 +1,249 @@
/* /*
Bullet Continuous Collision Detection and Physics Library Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty. This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software. In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose, Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely, including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions: subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution. 3. This notice may not be removed or altered from any source distribution.
*/ */
#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
#include "BulletCollision/CollisionShapes/btConvexInternalShape.h" #include "BulletCollision/CollisionShapes/btConvexInternalShape.h"
#include "BulletCollision/CollisionShapes/btCylinderShape.h" #include "BulletCollision/CollisionShapes/btCylinderShape.h"
#define MAX_NUM_SPU_CONVEX_POINTS 128 #define MAX_NUM_SPU_CONVEX_POINTS 128
struct SpuConvexPolyhedronVertexData struct SpuConvexPolyhedronVertexData
{ {
void* gSpuConvexShapePtr0; void* gSpuConvexShapePtr0;
void* gSpuConvexShapePtr1; void* gSpuConvexShapePtr1;
btPoint3* gConvexPoints0; btPoint3* gConvexPoints0;
btPoint3* gConvexPoints1; btPoint3* gConvexPoints1;
int gNumConvexPoints0; int gNumConvexPoints0;
int gNumConvexPoints1; int gNumConvexPoints1;
ATTRIBUTE_ALIGNED16(btPoint3 g_convexPointBuffer0[MAX_NUM_SPU_CONVEX_POINTS]); ATTRIBUTE_ALIGNED16(btPoint3 g_convexPointBuffer0[MAX_NUM_SPU_CONVEX_POINTS]);
ATTRIBUTE_ALIGNED16(btPoint3 g_convexPointBuffer1[MAX_NUM_SPU_CONVEX_POINTS]); ATTRIBUTE_ALIGNED16(btPoint3 g_convexPointBuffer1[MAX_NUM_SPU_CONVEX_POINTS]);
}; };
inline btPoint3 localGetSupportingVertexWithoutMargin(int shapeType, void* shape, btVector3& localDir,struct SpuConvexPolyhedronVertexData* convexVertexData)//, int *featureIndex) inline btPoint3 localGetSupportingVertexWithoutMargin(int shapeType, void* shape, btVector3 localDir,struct SpuConvexPolyhedronVertexData* convexVertexData)//, int *featureIndex)
{ {
switch (shapeType) switch (shapeType)
{ {
case SPHERE_SHAPE_PROXYTYPE: case SPHERE_SHAPE_PROXYTYPE:
{ {
return btPoint3(0,0,0); return btPoint3(0,0,0);
} }
case BOX_SHAPE_PROXYTYPE: case BOX_SHAPE_PROXYTYPE:
{ {
// spu_printf("SPU: getSupport BOX_SHAPE_PROXYTYPE\n"); // spu_printf("SPU: getSupport BOX_SHAPE_PROXYTYPE\n");
btConvexInternalShape* convexShape = (btConvexInternalShape*)shape; btConvexInternalShape* convexShape = (btConvexInternalShape*)shape;
btVector3 halfExtents = convexShape->getImplicitShapeDimensions(); btVector3 halfExtents = convexShape->getImplicitShapeDimensions();
float margin = convexShape->getMarginNV(); float margin = convexShape->getMarginNV();
halfExtents -= btVector3(margin,margin,margin); halfExtents -= btVector3(margin,margin,margin);
return btPoint3( return btPoint3(
localDir.getX() < 0.0f ? -halfExtents.x() : halfExtents.x(), localDir.getX() < 0.0f ? -halfExtents.x() : halfExtents.x(),
localDir.getY() < 0.0f ? -halfExtents.y() : halfExtents.y(), localDir.getY() < 0.0f ? -halfExtents.y() : halfExtents.y(),
localDir.getZ() < 0.0f ? -halfExtents.z() : halfExtents.z()); localDir.getZ() < 0.0f ? -halfExtents.z() : halfExtents.z());
} }
case TRIANGLE_SHAPE_PROXYTYPE: case TRIANGLE_SHAPE_PROXYTYPE:
{ {
btVector3 dir(localDir.getX(),localDir.getY(),localDir.getZ()); btVector3 dir(localDir.getX(),localDir.getY(),localDir.getZ());
btVector3* vertices = (btVector3*)shape; btVector3* vertices = (btVector3*)shape;
btVector3 dots(dir.dot(vertices[0]), dir.dot(vertices[1]), dir.dot(vertices[2])); btVector3 dots(dir.dot(vertices[0]), dir.dot(vertices[1]), dir.dot(vertices[2]));
btVector3 sup = vertices[dots.maxAxis()]; btVector3 sup = vertices[dots.maxAxis()];
return btPoint3(sup.getX(),sup.getY(),sup.getZ()); return btPoint3(sup.getX(),sup.getY(),sup.getZ());
break; break;
} }
case CYLINDER_SHAPE_PROXYTYPE: case CYLINDER_SHAPE_PROXYTYPE:
{ {
btCylinderShape* cylShape = (btCylinderShape*)shape; btCylinderShape* cylShape = (btCylinderShape*)shape;
//mapping of halfextents/dimension onto radius/height depends on how cylinder local orientation is (upAxis) //mapping of halfextents/dimension onto radius/height depends on how cylinder local orientation is (upAxis)
btVector3 halfExtents = cylShape->getImplicitShapeDimensions(); btVector3 halfExtents = cylShape->getImplicitShapeDimensions();
btVector3 v(localDir.getX(),localDir.getY(),localDir.getZ()); btVector3 v(localDir.getX(),localDir.getY(),localDir.getZ());
int cylinderUpAxis = cylShape->getUpAxis(); int cylinderUpAxis = cylShape->getUpAxis();
int XX(1),YY(0),ZZ(2); int XX(1),YY(0),ZZ(2);
switch (cylinderUpAxis) switch (cylinderUpAxis)
{ {
case 0: case 0:
{ {
XX = 1; XX = 1;
YY = 0; YY = 0;
ZZ = 2; ZZ = 2;
break; break;
} }
case 1: case 1:
{ {
XX = 0; XX = 0;
YY = 1; YY = 1;
ZZ = 2; ZZ = 2;
break; break;
} }
case 2: case 2:
{ {
XX = 0; XX = 0;
YY = 2; YY = 2;
ZZ = 1; ZZ = 1;
break; break;
} }
default: default:
btAssert(0); btAssert(0);
//printf("SPU:localGetSupportingVertexWithoutMargin unknown Cylinder up-axis\n"); //printf("SPU:localGetSupportingVertexWithoutMargin unknown Cylinder up-axis\n");
}; };
btScalar radius = halfExtents[XX]; btScalar radius = halfExtents[XX];
btScalar halfHeight = halfExtents[cylinderUpAxis]; btScalar halfHeight = halfExtents[cylinderUpAxis];
btVector3 tmp; btVector3 tmp;
btScalar d ; btScalar d ;
btScalar s = btSqrt(v[XX] * v[XX] + v[ZZ] * v[ZZ]); btScalar s = btSqrt(v[XX] * v[XX] + v[ZZ] * v[ZZ]);
if (s != btScalar(0.0)) if (s != btScalar(0.0))
{ {
d = radius / s; d = radius / s;
tmp[XX] = v[XX] * d; tmp[XX] = v[XX] * d;
tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight; tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight;
tmp[ZZ] = v[ZZ] * d; tmp[ZZ] = v[ZZ] * d;
return btPoint3(tmp.getX(),tmp.getY(),tmp.getZ()); return btPoint3(tmp.getX(),tmp.getY(),tmp.getZ());
} }
else else
{ {
tmp[XX] = radius; tmp[XX] = radius;
tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight; tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight;
tmp[ZZ] = btScalar(0.0); tmp[ZZ] = btScalar(0.0);
return btPoint3(tmp.getX(),tmp.getY(),tmp.getZ()); return btPoint3(tmp.getX(),tmp.getY(),tmp.getZ());
} }
} }
case CAPSULE_SHAPE_PROXYTYPE: case CAPSULE_SHAPE_PROXYTYPE:
{ {
//spu_printf("SPU: todo: getSupport CAPSULE_SHAPE_PROXYTYPE\n"); //spu_printf("SPU: todo: getSupport CAPSULE_SHAPE_PROXYTYPE\n");
btVector3 vec0(localDir.getX(),localDir.getY(),localDir.getZ()); btVector3 vec0(localDir.getX(),localDir.getY(),localDir.getZ());
btConvexInternalShape* cnvxShape = (btConvexInternalShape*)shape; btConvexInternalShape* cnvxShape = (btConvexInternalShape*)shape;
btVector3 halfExtents = cnvxShape->getImplicitShapeDimensions(); btVector3 halfExtents = cnvxShape->getImplicitShapeDimensions();
btScalar halfHeight = halfExtents.getY(); btScalar halfHeight = halfExtents.getY();
btScalar radius = halfExtents.getX(); btScalar radius = halfExtents.getX();
btVector3 supVec(0,0,0); btVector3 supVec(0,0,0);
btScalar maxDot(btScalar(-1e30)); btScalar maxDot(btScalar(-1e30));
btVector3 vec = vec0; btVector3 vec = vec0;
btScalar lenSqr = vec.length2(); btScalar lenSqr = vec.length2();
if (lenSqr < btScalar(0.0001)) if (lenSqr < btScalar(0.0001))
{ {
vec.setValue(1,0,0); vec.setValue(1,0,0);
} else } else
{ {
btScalar rlen = btScalar(1.) / btSqrt(lenSqr ); btScalar rlen = btScalar(1.) / btSqrt(lenSqr );
vec *= rlen; vec *= rlen;
} }
btVector3 vtx; btVector3 vtx;
btScalar newDot; btScalar newDot;
{ {
btVector3 pos(0,halfHeight,0); btVector3 pos(0,halfHeight,0);
vtx = pos +vec*(radius); vtx = pos +vec*(radius);
newDot = vec.dot(vtx); newDot = vec.dot(vtx);
if (newDot > maxDot) if (newDot > maxDot)
{ {
maxDot = newDot; maxDot = newDot;
supVec = vtx; supVec = vtx;
} }
} }
{ {
btVector3 pos(0,-halfHeight,0); btVector3 pos(0,-halfHeight,0);
vtx = pos +vec*(radius); vtx = pos +vec*(radius);
newDot = vec.dot(vtx); newDot = vec.dot(vtx);
if (newDot > maxDot) if (newDot > maxDot)
{ {
maxDot = newDot; maxDot = newDot;
supVec = vtx; supVec = vtx;
} }
} }
return btPoint3(supVec.getX(),supVec.getY(),supVec.getZ()); return btPoint3(supVec.getX(),supVec.getY(),supVec.getZ());
break; break;
}; };
case CONVEX_HULL_SHAPE_PROXYTYPE: case CONVEX_HULL_SHAPE_PROXYTYPE:
{ {
//spu_printf("SPU: todo: getSupport CONVEX_HULL_SHAPE_PROXYTYPE\n"); //spu_printf("SPU: todo: getSupport CONVEX_HULL_SHAPE_PROXYTYPE\n");
btPoint3* points = 0; btPoint3* points = 0;
int numPoints = 0; int numPoints = 0;
if (shape==convexVertexData->gSpuConvexShapePtr0) if (shape==convexVertexData->gSpuConvexShapePtr0)
{ {
points = convexVertexData->gConvexPoints0; points = convexVertexData->gConvexPoints0;
numPoints = convexVertexData->gNumConvexPoints0; numPoints = convexVertexData->gNumConvexPoints0;
} }
if (shape == convexVertexData->gSpuConvexShapePtr1) if (shape == convexVertexData->gSpuConvexShapePtr1)
{ {
points = convexVertexData->gConvexPoints1; points = convexVertexData->gConvexPoints1;
numPoints = convexVertexData->gNumConvexPoints1; numPoints = convexVertexData->gNumConvexPoints1;
} }
// spu_printf("numPoints = %d\n",numPoints); // spu_printf("numPoints = %d\n",numPoints);
btVector3 supVec(btScalar(0.),btScalar(0.),btScalar(0.)); btVector3 supVec(btScalar(0.),btScalar(0.),btScalar(0.));
btScalar newDot,maxDot = btScalar(-1e30); btScalar newDot,maxDot = btScalar(-1e30);
btVector3 vec0(localDir.getX(),localDir.getY(),localDir.getZ()); btVector3 vec0(localDir.getX(),localDir.getY(),localDir.getZ());
btVector3 vec = vec0; btVector3 vec = vec0;
btScalar lenSqr = vec.length2(); btScalar lenSqr = vec.length2();
if (lenSqr < btScalar(0.0001)) if (lenSqr < btScalar(0.0001))
{ {
vec.setValue(1,0,0); vec.setValue(1,0,0);
} else } else
{ {
btScalar rlen = btScalar(1.) / btSqrt(lenSqr ); btScalar rlen = btScalar(1.) / btSqrt(lenSqr );
vec *= rlen; vec *= rlen;
} }
for (int i=0;i<numPoints;i++) for (int i=0;i<numPoints;i++)
{ {
btPoint3 vtx = points[i];// * m_localScaling; btPoint3 vtx = points[i];// * m_localScaling;
newDot = vec.dot(vtx); newDot = vec.dot(vtx);
if (newDot > maxDot) if (newDot > maxDot)
{ {
maxDot = newDot; maxDot = newDot;
supVec = vtx; supVec = vtx;
} }
} }
return btPoint3(supVec.getX(),supVec.getY(),supVec.getZ()); return btPoint3(supVec.getX(),supVec.getY(),supVec.getZ());
break; break;
}; };
default: default:
//spu_printf("SPU:(type %i) missing support function\n",shapeType); //spu_printf("SPU:(type %i) missing support function\n",shapeType);
#if __ASSERT #if __ASSERT
spu_printf("localGetSupportingVertexWithoutMargin() - Unsupported bound type: %d.\n", shapeType); spu_printf("localGetSupportingVertexWithoutMargin() - Unsupported bound type: %d.\n", shapeType);
#endif // __ASSERT #endif // __ASSERT
return btPoint3(0.f, 0.f, 0.f); return btPoint3(0.f, 0.f, 0.f);
} }
} }

684
Extras/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuMinkowskiPenetrationDepthSolver.cpp
View File

@@ -1,342 +1,342 @@
/* /*
Bullet Continuous Collision Detection and Physics Library Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty. This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software. In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose, Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely, including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions: subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution. 3. This notice may not be removed or altered from any source distribution.
*/ */
#include "SpuMinkowskiPenetrationDepthSolver.h" #include "SpuMinkowskiPenetrationDepthSolver.h"
#include "SpuVoronoiSimplexSolver.h" #include "SpuVoronoiSimplexSolver.h"
#include "SpuGjkPairDetector.h" #include "SpuGjkPairDetector.h"
#include "SpuContactResult.h" #include "SpuContactResult.h"
#include "SpuPreferredPenetrationDirections.h" #include "SpuPreferredPenetrationDirections.h"
#include "SpuLocalSupport.h" #include "SpuLocalSupport.h"
#define NUM_UNITSPHERE_POINTS 42 #define NUM_UNITSPHERE_POINTS 42
static btVector3 sPenetrationDirections[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2] = static btVector3 sPenetrationDirections[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2] =
{ {
btVector3(btScalar(0.000000) , btScalar(-0.000000),btScalar(-1.000000)), btVector3(btScalar(0.000000) , btScalar(-0.000000),btScalar(-1.000000)),
btVector3(btScalar(0.723608) , btScalar(-0.525725),btScalar(-0.447219)), btVector3(btScalar(0.723608) , btScalar(-0.525725),btScalar(-0.447219)),
btVector3(btScalar(-0.276388) , btScalar(-0.850649),btScalar(-0.447219)), btVector3(btScalar(-0.276388) , btScalar(-0.850649),btScalar(-0.447219)),
btVector3(btScalar(-0.894426) , btScalar(-0.000000),btScalar(-0.447216)), btVector3(btScalar(-0.894426) , btScalar(-0.000000),btScalar(-0.447216)),
btVector3(btScalar(-0.276388) , btScalar(0.850649),btScalar(-0.447220)), btVector3(btScalar(-0.276388) , btScalar(0.850649),btScalar(-0.447220)),
btVector3(btScalar(0.723608) , btScalar(0.525725),btScalar(-0.447219)), btVector3(btScalar(0.723608) , btScalar(0.525725),btScalar(-0.447219)),
btVector3(btScalar(0.276388) , btScalar(-0.850649),btScalar(0.447220)), btVector3(btScalar(0.276388) , btScalar(-0.850649),btScalar(0.447220)),
btVector3(btScalar(-0.723608) , btScalar(-0.525725),btScalar(0.447219)), btVector3(btScalar(-0.723608) , btScalar(-0.525725),btScalar(0.447219)),
btVector3(btScalar(-0.723608) , btScalar(0.525725),btScalar(0.447219)), btVector3(btScalar(-0.723608) , btScalar(0.525725),btScalar(0.447219)),
btVector3(btScalar(0.276388) , btScalar(0.850649),btScalar(0.447219)), btVector3(btScalar(0.276388) , btScalar(0.850649),btScalar(0.447219)),
btVector3(btScalar(0.894426) , btScalar(0.000000),btScalar(0.447216)), btVector3(btScalar(0.894426) , btScalar(0.000000),btScalar(0.447216)),
btVector3(btScalar(-0.000000) , btScalar(0.000000),btScalar(1.000000)), btVector3(btScalar(-0.000000) , btScalar(0.000000),btScalar(1.000000)),
btVector3(btScalar(0.425323) , btScalar(-0.309011),btScalar(-0.850654)), btVector3(btScalar(0.425323) , btScalar(-0.309011),btScalar(-0.850654)),
btVector3(btScalar(-0.162456) , btScalar(-0.499995),btScalar(-0.850654)), btVector3(btScalar(-0.162456) , btScalar(-0.499995),btScalar(-0.850654)),
btVector3(btScalar(0.262869) , btScalar(-0.809012),btScalar(-0.525738)), btVector3(btScalar(0.262869) , btScalar(-0.809012),btScalar(-0.525738)),
btVector3(btScalar(0.425323) , btScalar(0.309011),btScalar(-0.850654)), btVector3(btScalar(0.425323) , btScalar(0.309011),btScalar(-0.850654)),
btVector3(btScalar(0.850648) , btScalar(-0.000000),btScalar(-0.525736)), btVector3(btScalar(0.850648) , btScalar(-0.000000),btScalar(-0.525736)),
btVector3(btScalar(-0.525730) , btScalar(-0.000000),btScalar(-0.850652)), btVector3(btScalar(-0.525730) , btScalar(-0.000000),btScalar(-0.850652)),
btVector3(btScalar(-0.688190) , btScalar(-0.499997),btScalar(-0.525736)), btVector3(btScalar(-0.688190) , btScalar(-0.499997),btScalar(-0.525736)),
btVector3(btScalar(-0.162456) , btScalar(0.499995),btScalar(-0.850654)), btVector3(btScalar(-0.162456) , btScalar(0.499995),btScalar(-0.850654)),
btVector3(btScalar(-0.688190) , btScalar(0.499997),btScalar(-0.525736)), btVector3(btScalar(-0.688190) , btScalar(0.499997),btScalar(-0.525736)),
btVector3(btScalar(0.262869) , btScalar(0.809012),btScalar(-0.525738)), btVector3(btScalar(0.262869) , btScalar(0.809012),btScalar(-0.525738)),
btVector3(btScalar(0.951058) , btScalar(0.309013),btScalar(0.000000)), btVector3(btScalar(0.951058) , btScalar(0.309013),btScalar(0.000000)),
btVector3(btScalar(0.951058) , btScalar(-0.309013),btScalar(0.000000)), btVector3(btScalar(0.951058) , btScalar(-0.309013),btScalar(0.000000)),
btVector3(btScalar(0.587786) , btScalar(-0.809017),btScalar(0.000000)), btVector3(btScalar(0.587786) , btScalar(-0.809017),btScalar(0.000000)),
btVector3(btScalar(0.000000) , btScalar(-1.000000),btScalar(0.000000)), btVector3(btScalar(0.000000) , btScalar(-1.000000),btScalar(0.000000)),
btVector3(btScalar(-0.587786) , btScalar(-0.809017),btScalar(0.000000)), btVector3(btScalar(-0.587786) , btScalar(-0.809017),btScalar(0.000000)),
btVector3(btScalar(-0.951058) , btScalar(-0.309013),btScalar(-0.000000)), btVector3(btScalar(-0.951058) , btScalar(-0.309013),btScalar(-0.000000)),
btVector3(btScalar(-0.951058) , btScalar(0.309013),btScalar(-0.000000)), btVector3(btScalar(-0.951058) , btScalar(0.309013),btScalar(-0.000000)),
btVector3(btScalar(-0.587786) , btScalar(0.809017),btScalar(-0.000000)), btVector3(btScalar(-0.587786) , btScalar(0.809017),btScalar(-0.000000)),
btVector3(btScalar(-0.000000) , btScalar(1.000000),btScalar(-0.000000)), btVector3(btScalar(-0.000000) , btScalar(1.000000),btScalar(-0.000000)),
btVector3(btScalar(0.587786) , btScalar(0.809017),btScalar(-0.000000)), btVector3(btScalar(0.587786) , btScalar(0.809017),btScalar(-0.000000)),
btVector3(btScalar(0.688190) , btScalar(-0.499997),btScalar(0.525736)), btVector3(btScalar(0.688190) , btScalar(-0.499997),btScalar(0.525736)),
btVector3(btScalar(-0.262869) , btScalar(-0.809012),btScalar(0.525738)), btVector3(btScalar(-0.262869) , btScalar(-0.809012),btScalar(0.525738)),
btVector3(btScalar(-0.850648) , btScalar(0.000000),btScalar(0.525736)), btVector3(btScalar(-0.850648) , btScalar(0.000000),btScalar(0.525736)),
btVector3(btScalar(-0.262869) , btScalar(0.809012),btScalar(0.525738)), btVector3(btScalar(-0.262869) , btScalar(0.809012),btScalar(0.525738)),
btVector3(btScalar(0.688190) , btScalar(0.499997),btScalar(0.525736)), btVector3(btScalar(0.688190) , btScalar(0.499997),btScalar(0.525736)),
btVector3(btScalar(0.525730) , btScalar(0.000000),btScalar(0.850652)), btVector3(btScalar(0.525730) , btScalar(0.000000),btScalar(0.850652)),
btVector3(btScalar(0.162456) , btScalar(-0.499995),btScalar(0.850654)), btVector3(btScalar(0.162456) , btScalar(-0.499995),btScalar(0.850654)),
btVector3(btScalar(-0.425323) , btScalar(-0.309011),btScalar(0.850654)), btVector3(btScalar(-0.425323) , btScalar(-0.309011),btScalar(0.850654)),
btVector3(btScalar(-0.425323) , btScalar(0.309011),btScalar(0.850654)), btVector3(btScalar(-0.425323) , btScalar(0.309011),btScalar(0.850654)),
btVector3(btScalar(0.162456) , btScalar(0.499995),btScalar(0.850654)) btVector3(btScalar(0.162456) , btScalar(0.499995),btScalar(0.850654))
}; };
bool SpuMinkowskiPenetrationDepthSolver::calcPenDepth( SpuVoronoiSimplexSolver& simplexSolver, bool SpuMinkowskiPenetrationDepthSolver::calcPenDepth( SpuVoronoiSimplexSolver& simplexSolver,
void* convexA,void* convexB,int shapeTypeA, int shapeTypeB, float marginA, float marginB, void* convexA,void* convexB,int shapeTypeA, int shapeTypeB, float marginA, float marginB,
btTransform& transA,const btTransform& transB, btTransform& transA,const btTransform& transB,
btVector3& v, btPoint3& pa, btPoint3& pb, btVector3& v, btPoint3& pa, btPoint3& pb,
class btIDebugDraw* debugDraw,btStackAlloc* stackAlloc, class btIDebugDraw* debugDraw,btStackAlloc* stackAlloc,
struct SpuConvexPolyhedronVertexData* convexVertexData struct SpuConvexPolyhedronVertexData* convexVertexData
) const ) const
{ {
(void)stackAlloc; (void)stackAlloc;
(void)v; (void)v;
struct btIntermediateResult : public SpuContactResult struct btIntermediateResult : public SpuContactResult
{ {
btIntermediateResult():m_hasResult(false) btIntermediateResult():m_hasResult(false)
{ {
} }
btVector3 m_normalOnBInWorld; btVector3 m_normalOnBInWorld;
btVector3 m_pointInWorld; btVector3 m_pointInWorld;
btScalar m_depth; btScalar m_depth;
bool m_hasResult; bool m_hasResult;
virtual void setShapeIdentifiers(int partId0,int index0, int partId1,int index1) virtual void setShapeIdentifiers(int partId0,int index0, int partId1,int index1)
{ {
(void)partId0; (void)partId0;
(void)index0; (void)index0;
(void)partId1; (void)partId1;
(void)index1; (void)index1;
} }
void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth) void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth)
{ {
m_normalOnBInWorld = normalOnBInWorld; m_normalOnBInWorld = normalOnBInWorld;
m_pointInWorld = pointInWorld; m_pointInWorld = pointInWorld;
m_depth = depth; m_depth = depth;
m_hasResult = true; m_hasResult = true;
} }
}; };
//just take fixed number of orientation, and sample the penetration depth in that direction //just take fixed number of orientation, and sample the penetration depth in that direction
btScalar minProj = btScalar(1e30); btScalar minProj = btScalar(1e30);
btVector3 minNorm; btVector3 minNorm;
btVector3 minVertex; btVector3 minVertex;
btVector3 minA,minB; btVector3 minA,minB;
btVector3 seperatingAxisInA,seperatingAxisInB; btVector3 seperatingAxisInA,seperatingAxisInB;
btVector3 pInA,qInB,pWorld,qWorld,w; btVector3 pInA,qInB,pWorld,qWorld,w;
//#define USE_BATCHED_SUPPORT 1 //#define USE_BATCHED_SUPPORT 1
#ifdef USE_BATCHED_SUPPORT #ifdef USE_BATCHED_SUPPORT
btVector3 supportVerticesABatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2]; btVector3 supportVerticesABatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2];
btVector3 supportVerticesBBatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2]; btVector3 supportVerticesBBatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2];
btVector3 seperatingAxisInABatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2]; btVector3 seperatingAxisInABatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2];
btVector3 seperatingAxisInBBatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2]; btVector3 seperatingAxisInBBatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2];
int i; int i;
int numSampleDirections = NUM_UNITSPHERE_POINTS; int numSampleDirections = NUM_UNITSPHERE_POINTS;
for (i=0;i<numSampleDirections;i++) for (i=0;i<numSampleDirections;i++)
{ {
const btVector3& norm = sPenetrationDirections[i]; const btVector3& norm = sPenetrationDirections[i];
seperatingAxisInABatch[i] = (-norm) * transA.getBasis() ; seperatingAxisInABatch[i] = (-norm) * transA.getBasis() ;
seperatingAxisInBBatch[i] = norm * transB.getBasis() ; seperatingAxisInBBatch[i] = norm * transB.getBasis() ;
} }
{ {
int numPDA = convexA->getNumPreferredPenetrationDirections(); int numPDA = convexA->getNumPreferredPenetrationDirections();
if (numPDA) if (numPDA)
{ {
for (int i=0;i<numPDA;i++) for (int i=0;i<numPDA;i++)
{ {
btVector3 norm; btVector3 norm;
convexA->getPreferredPenetrationDirection(i,norm); convexA->getPreferredPenetrationDirection(i,norm);
norm = transA.getBasis() * norm; norm = transA.getBasis() * norm;
sPenetrationDirections[numSampleDirections] = norm; sPenetrationDirections[numSampleDirections] = norm;
seperatingAxisInABatch[numSampleDirections] = (-norm) * transA.getBasis(); seperatingAxisInABatch[numSampleDirections] = (-norm) * transA.getBasis();
seperatingAxisInBBatch[numSampleDirections] = norm * transB.getBasis(); seperatingAxisInBBatch[numSampleDirections] = norm * transB.getBasis();
numSampleDirections++; numSampleDirections++;
} }
} }
} }
{ {
int numPDB = convexB->getNumPreferredPenetrationDirections(); int numPDB = convexB->getNumPreferredPenetrationDirections();
if (numPDB) if (numPDB)
{ {
for (int i=0;i<numPDB;i++) for (int i=0;i<numPDB;i++)
{ {
btVector3 norm; btVector3 norm;
convexB->getPreferredPenetrationDirection(i,norm); convexB->getPreferredPenetrationDirection(i,norm);
norm = transB.getBasis() * norm; norm = transB.getBasis() * norm;
sPenetrationDirections[numSampleDirections] = norm; sPenetrationDirections[numSampleDirections] = norm;
seperatingAxisInABatch[numSampleDirections] = (-norm) * transA.getBasis(); seperatingAxisInABatch[numSampleDirections] = (-norm) * transA.getBasis();
seperatingAxisInBBatch[numSampleDirections] = norm * transB.getBasis(); seperatingAxisInBBatch[numSampleDirections] = norm * transB.getBasis();
numSampleDirections++; numSampleDirections++;
} }
} }
} }
convexA->batchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInABatch,supportVerticesABatch,numSampleDirections); convexA->batchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInABatch,supportVerticesABatch,numSampleDirections);
convexB->batchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInBBatch,supportVerticesBBatch,numSampleDirections); convexB->batchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInBBatch,supportVerticesBBatch,numSampleDirections);
for (i=0;i<numSampleDirections;i++) for (i=0;i<numSampleDirections;i++)
{ {
const btVector3& norm = sPenetrationDirections[i]; const btVector3& norm = sPenetrationDirections[i];
seperatingAxisInA = seperatingAxisInABatch[i]; seperatingAxisInA = seperatingAxisInABatch[i];
seperatingAxisInB = seperatingAxisInBBatch[i]; seperatingAxisInB = seperatingAxisInBBatch[i];
pInA = supportVerticesABatch[i]; pInA = supportVerticesABatch[i];
qInB = supportVerticesBBatch[i]; qInB = supportVerticesBBatch[i];
pWorld = transA(pInA); pWorld = transA(pInA);
qWorld = transB(qInB); qWorld = transB(qInB);
w = qWorld - pWorld; w = qWorld - pWorld;
btScalar delta = norm.dot(w); btScalar delta = norm.dot(w);
//find smallest delta //find smallest delta
if (delta < minProj) if (delta < minProj)
{ {
minProj = delta; minProj = delta;
minNorm = norm; minNorm = norm;
minA = pWorld; minA = pWorld;
minB = qWorld; minB = qWorld;
} }
} }
#else #else
int numSampleDirections = NUM_UNITSPHERE_POINTS; int numSampleDirections = NUM_UNITSPHERE_POINTS;
///this is necessary, otherwise the normal is not correct, and sphere will rotate forever on a sloped triangle mesh ///this is necessary, otherwise the normal is not correct, and sphere will rotate forever on a sloped triangle mesh
#define DO_PREFERRED_DIRECTIONS 1 #define DO_PREFERRED_DIRECTIONS 1
#ifdef DO_PREFERRED_DIRECTIONS #ifdef DO_PREFERRED_DIRECTIONS
{ {
int numPDA = spuGetNumPreferredPenetrationDirections(shapeTypeA,convexA); int numPDA = spuGetNumPreferredPenetrationDirections(shapeTypeA,convexA);
if (numPDA) if (numPDA)
{ {
for (int i=0;i<numPDA;i++) for (int i=0;i<numPDA;i++)
{ {
btVector3 norm; btVector3 norm;
spuGetPreferredPenetrationDirection(shapeTypeA,convexA,i,norm); spuGetPreferredPenetrationDirection(shapeTypeA,convexA,i,norm);
norm = transA.getBasis() * norm; norm = transA.getBasis() * norm;
sPenetrationDirections[numSampleDirections] = norm; sPenetrationDirections[numSampleDirections] = norm;
numSampleDirections++; numSampleDirections++;
} }
} }
} }
{ {
int numPDB = spuGetNumPreferredPenetrationDirections(shapeTypeB,convexB); int numPDB = spuGetNumPreferredPenetrationDirections(shapeTypeB,convexB);
if (numPDB) if (numPDB)
{ {
for (int i=0;i<numPDB;i++) for (int i=0;i<numPDB;i++)
{ {
btVector3 norm; btVector3 norm;
spuGetPreferredPenetrationDirection(shapeTypeB,convexB,i,norm); spuGetPreferredPenetrationDirection(shapeTypeB,convexB,i,norm);
norm = transB.getBasis() * norm; norm = transB.getBasis() * norm;
sPenetrationDirections[numSampleDirections] = norm; sPenetrationDirections[numSampleDirections] = norm;
numSampleDirections++; numSampleDirections++;
} }
} }
} }
#endif //DO_PREFERRED_DIRECTIONS #endif //DO_PREFERRED_DIRECTIONS
for (int i=0;i<numSampleDirections;i++) for (int i=0;i<numSampleDirections;i++)
{ {
const btVector3& norm = sPenetrationDirections[i]; const btVector3& norm = sPenetrationDirections[i];
seperatingAxisInA = (-norm)* transA.getBasis(); seperatingAxisInA = (-norm)* transA.getBasis();
seperatingAxisInB = norm* transB.getBasis(); seperatingAxisInB = norm* transB.getBasis();
pInA = localGetSupportingVertexWithoutMargin(shapeTypeA, convexA, seperatingAxisInA,convexVertexData);//, NULL); pInA = localGetSupportingVertexWithoutMargin(shapeTypeA, convexA, seperatingAxisInA,convexVertexData);//, NULL);
qInB = localGetSupportingVertexWithoutMargin(shapeTypeB, convexB, seperatingAxisInB,convexVertexData);//, NULL); qInB = localGetSupportingVertexWithoutMargin(shapeTypeB, convexB, seperatingAxisInB,convexVertexData);//, NULL);
// pInA = convexA->localGetSupportingVertexWithoutMargin(seperatingAxisInA); // pInA = convexA->localGetSupportingVertexWithoutMargin(seperatingAxisInA);
// qInB = convexB->localGetSupportingVertexWithoutMargin(seperatingAxisInB); // qInB = convexB->localGetSupportingVertexWithoutMargin(seperatingAxisInB);
pWorld = transA(pInA); pWorld = transA(pInA);
qWorld = transB(qInB); qWorld = transB(qInB);
w = qWorld - pWorld; w = qWorld - pWorld;
btScalar delta = norm.dot(w); btScalar delta = norm.dot(w);
//find smallest delta //find smallest delta
if (delta < minProj) if (delta < minProj)
{ {
minProj = delta; minProj = delta;
minNorm = norm; minNorm = norm;
minA = pWorld; minA = pWorld;
minB = qWorld; minB = qWorld;
} }
} }
#endif //USE_BATCHED_SUPPORT #endif //USE_BATCHED_SUPPORT
//add the margins //add the margins
minA += minNorm*marginA; minA += minNorm*marginA;
minB -= minNorm*marginB; minB -= minNorm*marginB;
//no penetration //no penetration
if (minProj < btScalar(0.)) if (minProj < btScalar(0.))
return false; return false;
minProj += (marginA + marginB); minProj += (marginA + marginB);
//#define DEBUG_DRAW 1 //#define DEBUG_DRAW 1
#ifdef DEBUG_DRAW #ifdef DEBUG_DRAW
if (debugDraw) if (debugDraw)
{ {
btVector3 color(0,1,0); btVector3 color(0,1,0);
debugDraw->drawLine(minA,minB,color); debugDraw->drawLine(minA,minB,color);
color = btVector3 (1,1,1); color = btVector3 (1,1,1);
btVector3 vec = minB-minA; btVector3 vec = minB-minA;
btScalar prj2 = minNorm.dot(vec); btScalar prj2 = minNorm.dot(vec);
debugDraw->drawLine(minA,minA+(minNorm*minProj),color); debugDraw->drawLine(minA,minA+(minNorm*minProj),color);
} }
#endif //DEBUG_DRAW #endif //DEBUG_DRAW
SpuGjkPairDetector gjkdet(convexA,convexB,shapeTypeA,shapeTypeB,marginA,marginB,&simplexSolver,0); SpuGjkPairDetector gjkdet(convexA,convexB,shapeTypeA,shapeTypeB,marginA,marginB,&simplexSolver,0);
btScalar offsetDist = minProj; btScalar offsetDist = minProj;
btVector3 offset = minNorm * offsetDist; btVector3 offset = minNorm * offsetDist;
SpuClosestPointInput input; SpuClosestPointInput input;
input.m_convexVertexData = convexVertexData; input.m_convexVertexData = convexVertexData;
btVector3 newOrg = transA.getOrigin() + offset; btVector3 newOrg = transA.getOrigin() + offset;
btTransform displacedTrans = transA; btTransform displacedTrans = transA;
displacedTrans.setOrigin(newOrg); displacedTrans.setOrigin(newOrg);
input.m_transformA = displacedTrans; input.m_transformA = displacedTrans;
input.m_transformB = transB; input.m_transformB = transB;
input.m_maximumDistanceSquared = btScalar(1e30);//minProj; input.m_maximumDistanceSquared = btScalar(1e30);//minProj;
btIntermediateResult res; btIntermediateResult res;
gjkdet.getClosestPoints(input,res); gjkdet.getClosestPoints(input,res);
btScalar correctedMinNorm = minProj - res.m_depth; btScalar correctedMinNorm = minProj - res.m_depth;
//the penetration depth is over-estimated, relax it //the penetration depth is over-estimated, relax it
btScalar penetration_relaxation= btScalar(1.); btScalar penetration_relaxation= btScalar(1.);
minNorm*=penetration_relaxation; minNorm*=penetration_relaxation;
if (res.m_hasResult) if (res.m_hasResult)
{ {
pa = res.m_pointInWorld - minNorm * correctedMinNorm; pa = res.m_pointInWorld - minNorm * correctedMinNorm;
pb = res.m_pointInWorld; pb = res.m_pointInWorld;
#ifdef DEBUG_DRAW #ifdef DEBUG_DRAW
if (debugDraw) if (debugDraw)
{ {
btVector3 color(1,0,0); btVector3 color(1,0,0);
debugDraw->drawLine(pa,pb,color); debugDraw->drawLine(pa,pb,color);
} }
#endif//DEBUG_DRAW #endif//DEBUG_DRAW
} }
return res.m_hasResult; return res.m_hasResult;
} }

92
Extras/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuMinkowskiPenetrationDepthSolver.h
View File

@@ -1,46 +1,46 @@
/* /*
Bullet Continuous Collision Detection and Physics Library Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty. This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software. In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose, Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely, including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions: subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution. 3. This notice may not be removed or altered from any source distribution.
*/ */
#ifndef MINKOWSKI_PENETRATION_DEPTH_SOLVER_H #ifndef MINKOWSKI_PENETRATION_DEPTH_SOLVER_H
#define MINKOWSKI_PENETRATION_DEPTH_SOLVER_H #define MINKOWSKI_PENETRATION_DEPTH_SOLVER_H
#include "SpuConvexPenetrationDepthSolver.h" #include "SpuConvexPenetrationDepthSolver.h"
class btStackAlloc; class btStackAlloc;
class btIDebugDraw; class btIDebugDraw;
class SpuVoronoiSimplexSolver; class SpuVoronoiSimplexSolver;
///MinkowskiPenetrationDepthSolver implements bruteforce penetration depth estimation. ///MinkowskiPenetrationDepthSolver implements bruteforce penetration depth estimation.
///Implementation is based on sampling the depth using support mapping, and using GJK step to get the witness points. ///Implementation is based on sampling the depth using support mapping, and using GJK step to get the witness points.
class SpuMinkowskiPenetrationDepthSolver : public SpuConvexPenetrationDepthSolver class SpuMinkowskiPenetrationDepthSolver : public SpuConvexPenetrationDepthSolver
{ {
public: public:
virtual bool calcPenDepth( SpuVoronoiSimplexSolver& simplexSolver, virtual bool calcPenDepth( SpuVoronoiSimplexSolver& simplexSolver,
void* convexA,void* convexB,int shapeTypeA, int shapeTypeB, float marginA, float marginB, void* convexA,void* convexB,int shapeTypeA, int shapeTypeB, float marginA, float marginB,
btTransform& transA,const btTransform& transB, btTransform& transA,const btTransform& transB,
btVector3& v, btPoint3& pa, btPoint3& pb, btVector3& v, btPoint3& pa, btPoint3& pb,
class btIDebugDraw* debugDraw,btStackAlloc* stackAlloc, class btIDebugDraw* debugDraw,btStackAlloc* stackAlloc,
struct SpuConvexPolyhedronVertexData* convexVertexData struct SpuConvexPolyhedronVertexData* convexVertexData
) const; ) const;
}; };
#endif //MINKOWSKI_PENETRATION_DEPTH_SOLVER_H #endif //MINKOWSKI_PENETRATION_DEPTH_SOLVER_H

140
Extras/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuPreferredPenetrationDirections.h
View File

@@ -1,70 +1,70 @@
/* /*
Bullet Continuous Collision Detection and Physics Library Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty. This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software. In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose, Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely, including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions: subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution. 3. This notice may not be removed or altered from any source distribution.
*/ */
#ifndef _SPU_PREFERRED_PENETRATION_DIRECTIONS_H #ifndef _SPU_PREFERRED_PENETRATION_DIRECTIONS_H
#define _SPU_PREFERRED_PENETRATION_DIRECTIONS_H #define _SPU_PREFERRED_PENETRATION_DIRECTIONS_H
#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
int spuGetNumPreferredPenetrationDirections(int shapeType, void* shape) int spuGetNumPreferredPenetrationDirections(int shapeType, void* shape)
{ {
switch (shapeType) switch (shapeType)
{ {
case TRIANGLE_SHAPE_PROXYTYPE: case TRIANGLE_SHAPE_PROXYTYPE:
{ {
return 2; return 2;
//spu_printf("2\n"); //spu_printf("2\n");
break; break;
} }
default: default:
{ {
#if __ASSERT #if __ASSERT
spu_printf("spuGetNumPreferredPenetrationDirections() - Unsupported bound type: %d.\n", shapeType); spu_printf("spuGetNumPreferredPenetrationDirections() - Unsupported bound type: %d.\n", shapeType);
#endif // __ASSERT #endif // __ASSERT
} }
} }
return 0; return 0;
} }
void spuGetPreferredPenetrationDirection(int shapeType, void* shape, int index, btVector3& penetrationVector) void spuGetPreferredPenetrationDirection(int shapeType, void* shape, int index, btVector3& penetrationVector)
{ {
switch (shapeType) switch (shapeType)
{ {
case TRIANGLE_SHAPE_PROXYTYPE: case TRIANGLE_SHAPE_PROXYTYPE:
{ {
btVector3* vertices = (btVector3*)shape; btVector3* vertices = (btVector3*)shape;
///calcNormal ///calcNormal
penetrationVector = (vertices[1]-vertices[0]).cross(vertices[2]-vertices[0]); penetrationVector = (vertices[1]-vertices[0]).cross(vertices[2]-vertices[0]);
penetrationVector.normalize(); penetrationVector.normalize();
if (index) if (index)
penetrationVector *= btScalar(-1.); penetrationVector *= btScalar(-1.);
break; break;
} }
default: default:
{ {
#if __ASSERT #if __ASSERT
spu_printf("spuGetNumPreferredPenetrationDirections() - Unsupported bound type: %d.\n", shapeType); spu_printf("spuGetNumPreferredPenetrationDirections() - Unsupported bound type: %d.\n", shapeType);
#endif // __ASSERT #endif // __ASSERT
} }
} }
} }
#endif //_SPU_PREFERRED_PENETRATION_DIRECTIONS_H #endif //_SPU_PREFERRED_PENETRATION_DIRECTIONS_H

1212
Extras/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuVoronoiSimplexSolver.cpp
View File

File diff suppressed because it is too large Load Diff

312
Extras/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuVoronoiSimplexSolver.h
View File

@@ -1,156 +1,156 @@
/* /*
Bullet Continuous Collision Detection and Physics Library Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty. This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software. In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose, Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely, including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions: subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution. 3. This notice may not be removed or altered from any source distribution.
*/ */
#ifndef SPUVoronoiSimplexSolver_H #ifndef SPUVoronoiSimplexSolver_H
#define SPUVoronoiSimplexSolver_H #define SPUVoronoiSimplexSolver_H
#include <LinearMath/btTransform.h> #include <LinearMath/btTransform.h>
#include <LinearMath/btPoint3.h> #include <LinearMath/btPoint3.h>
#define VORONOI_SIMPLEX_MAX_VERTS 5 #define VORONOI_SIMPLEX_MAX_VERTS 5
struct SpuUsageBitfield{ struct SpuUsageBitfield{
SpuUsageBitfield() SpuUsageBitfield()
{ {
reset(); reset();
} }
void reset() void reset()
{ {
usedVertexA = false; usedVertexA = false;
usedVertexB = false; usedVertexB = false;
usedVertexC = false; usedVertexC = false;
usedVertexD = false; usedVertexD = false;
} }
unsigned short usedVertexA : 1; unsigned short usedVertexA : 1;
unsigned short usedVertexB : 1; unsigned short usedVertexB : 1;
unsigned short usedVertexC : 1; unsigned short usedVertexC : 1;
unsigned short usedVertexD : 1; unsigned short usedVertexD : 1;
unsigned short unused1 : 1; unsigned short unused1 : 1;
unsigned short unused2 : 1; unsigned short unused2 : 1;
unsigned short unused3 : 1; unsigned short unused3 : 1;
unsigned short unused4 : 1; unsigned short unused4 : 1;
}; };
struct SpuSubSimplexClosestResult struct SpuSubSimplexClosestResult
{ {
btVector3 m_closestPointOnSimplex; btVector3 m_closestPointOnSimplex;
//MASK for m_usedVertices //MASK for m_usedVertices
//stores the simplex vertex-usage, using the MASK, //stores the simplex vertex-usage, using the MASK,
// if m_usedVertices & MASK then the related vertex is used // if m_usedVertices & MASK then the related vertex is used
SpuUsageBitfield m_usedVertices; SpuUsageBitfield m_usedVertices;
float m_barycentricCoords[4]; float m_barycentricCoords[4];
bool m_degenerate; bool m_degenerate;
void reset() void reset()
{ {
m_degenerate = false; m_degenerate = false;
setBarycentricCoordinates(); setBarycentricCoordinates();
m_usedVertices.reset(); m_usedVertices.reset();
} }
bool isValid() bool isValid()
{ {
bool valid = (m_barycentricCoords[0] >= float(0.)) && bool valid = (m_barycentricCoords[0] >= float(0.)) &&
(m_barycentricCoords[1] >= float(0.)) && (m_barycentricCoords[1] >= float(0.)) &&
(m_barycentricCoords[2] >= float(0.)) && (m_barycentricCoords[2] >= float(0.)) &&
(m_barycentricCoords[3] >= float(0.)); (m_barycentricCoords[3] >= float(0.));
return valid; return valid;
} }
void setBarycentricCoordinates(float a=float(0.),float b=float(0.),float c=float(0.),float d=float(0.)) void setBarycentricCoordinates(float a=float(0.),float b=float(0.),float c=float(0.),float d=float(0.))
{ {
m_barycentricCoords[0] = a; m_barycentricCoords[0] = a;
m_barycentricCoords[1] = b; m_barycentricCoords[1] = b;
m_barycentricCoords[2] = c; m_barycentricCoords[2] = c;
m_barycentricCoords[3] = d; m_barycentricCoords[3] = d;
} }
}; };
/// SpuVoronoiSimplexSolver is an implementation of the closest point distance algorithm from a 1-4 points simplex to the origin. /// SpuVoronoiSimplexSolver is an implementation of the closest point distance algorithm from a 1-4 points simplex to the origin.
/// Can be used with GJK, as an alternative to Johnson distance algorithm. /// Can be used with GJK, as an alternative to Johnson distance algorithm.
class SpuVoronoiSimplexSolver class SpuVoronoiSimplexSolver
{ {
public: public:
int m_numVertices; int m_numVertices;
btVector3 m_simplexVectorW[VORONOI_SIMPLEX_MAX_VERTS]; btVector3 m_simplexVectorW[VORONOI_SIMPLEX_MAX_VERTS];
btVector3 m_simplexPointsP[VORONOI_SIMPLEX_MAX_VERTS]; btVector3 m_simplexPointsP[VORONOI_SIMPLEX_MAX_VERTS];
btVector3 m_simplexPointsQ[VORONOI_SIMPLEX_MAX_VERTS]; btVector3 m_simplexPointsQ[VORONOI_SIMPLEX_MAX_VERTS];
int m_VertexIndexA[VORONOI_SIMPLEX_MAX_VERTS]; int m_VertexIndexA[VORONOI_SIMPLEX_MAX_VERTS];
int m_VertexIndexB[VORONOI_SIMPLEX_MAX_VERTS]; int m_VertexIndexB[VORONOI_SIMPLEX_MAX_VERTS];
btVector3 m_cachedP1; btVector3 m_cachedP1;
btVector3 m_cachedP2; btVector3 m_cachedP2;
btVector3 m_cachedV; btVector3 m_cachedV;
btVector3 m_lastW; btVector3 m_lastW;
bool m_cachedValidClosest; bool m_cachedValidClosest;
SpuSubSimplexClosestResult m_cachedBC; SpuSubSimplexClosestResult m_cachedBC;
bool m_needsUpdate; bool m_needsUpdate;
void removeVertex(int index); void removeVertex(int index);
void reduceVertices (const SpuUsageBitfield& usedVerts); void reduceVertices (const SpuUsageBitfield& usedVerts);
bool updateClosestVectorAndPoints(); bool updateClosestVectorAndPoints();
bool closestPtPointTetrahedron(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d, SpuSubSimplexClosestResult& finalResult); bool closestPtPointTetrahedron(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d, SpuSubSimplexClosestResult& finalResult);
int pointOutsideOfPlane(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d); int pointOutsideOfPlane(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d);
bool closestPtPointTriangle(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c,SpuSubSimplexClosestResult& result); bool closestPtPointTriangle(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c,SpuSubSimplexClosestResult& result);
int RemoveDegenerateIndices (const int *inArray, int numIndices, int *outArray) const; int RemoveDegenerateIndices (const int *inArray, int numIndices, int *outArray) const;
public: public:
void reset(); void reset();
void addVertex(const btVector3& w, const btPoint3& p, const btPoint3& q); void addVertex(const btVector3& w, const btPoint3& p, const btPoint3& q);
bool closest(btVector3& v); bool closest(btVector3& v);
btScalar maxVertex(); btScalar maxVertex();
bool fullSimplex() const bool fullSimplex() const
{ {
return (m_numVertices == 4); return (m_numVertices == 4);
} }
int getSimplex(btVector3 *pBuf, btVector3 *qBuf, btVector3 *yBuf) const; int getSimplex(btVector3 *pBuf, btVector3 *qBuf, btVector3 *yBuf) const;
bool inSimplex(const btVector3& w); bool inSimplex(const btVector3& w);
void backup_closest(btVector3& v) ; void backup_closest(btVector3& v) ;
bool emptySimplex() const ; bool emptySimplex() const ;
void compute_points(btVector3& p1, btVector3& p2) ; void compute_points(btVector3& p1, btVector3& p2) ;
int numVertices() const int numVertices() const
{ {
return m_numVertices; return m_numVertices;
} }
}; };
#endif //SpuVoronoiSimplexSolver #endif //SpuVoronoiSimplexSolver

401
Extras/BulletMultiThreaded/SpuSampleTaskProcess.cpp
View File

@@ -1,196 +1,205 @@
/* /*
Bullet Continuous Collision Detection and Physics Library Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
This software is provided 'as-is', without any express or implied warranty. This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software. In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose, Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely, including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions: subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution. 3. This notice may not be removed or altered from any source distribution.
*/ */
//#define __CELLOS_LV2__ 1 //#define __CELLOS_LV2__ 1
#define USE_SAMPLE_PROCESS 1 #define USE_SAMPLE_PROCESS 1
#ifdef USE_SAMPLE_PROCESS #ifdef USE_SAMPLE_PROCESS
#include "btThreadSupportInterface.h" #include "SpuSampleTaskProcess.h"
#include <stdio.h>
//#include "SPUAssert.h"
#include <string.h> #ifdef __SPU__
#include "SpuSampleTaskProcess.h"
void SampleThreadFunc(void* userPtr,void* lsMemory)
{
#include <stdio.h> //do nothing
printf("hello world\n");
}
void SampleThreadFunc(void* userPtr,void* lsMemory)
{
//do nothing void* SamplelsMemoryFunc()
printf("hello world\n"); {
} //don't create local store memory, just return 0
return 0;
void* SamplelsMemoryFunc() }
{
//don't create local store memory, just return 0
return 0; #else
}
#include "btThreadSupportInterface.h"
extern "C" { //# include "SPUAssert.h"
extern char SPU_SAMPLE_ELF_SYMBOL[]; #include <string.h>
};
extern "C" {
extern char SPU_SAMPLE_ELF_SYMBOL[];
};
SpuSampleTaskProcess::SpuSampleTaskProcess(btThreadSupportInterface* threadInterface, unsigned int maxNumOutstandingTasks)
:m_threadInterface(threadInterface),
m_maxNumOutstandingTasks(maxNumOutstandingTasks)
{
m_taskBusy.resize(m_maxNumOutstandingTasks); SpuSampleTaskProcess::SpuSampleTaskProcess(btThreadSupportInterface* threadInterface, unsigned int maxNumOutstandingTasks)
m_spuSampleTaskDesc.resize(m_maxNumOutstandingTasks); :m_threadInterface(threadInterface),
m_maxNumOutstandingTasks(maxNumOutstandingTasks)
for (int i = 0; i < m_maxNumOutstandingTasks; i++) {
{
m_taskBusy[i] = false; m_taskBusy.resize(m_maxNumOutstandingTasks);
} m_spuSampleTaskDesc.resize(m_maxNumOutstandingTasks);
m_numBusyTasks = 0;
m_currentTask = 0; for (int i = 0; i < m_maxNumOutstandingTasks; i++)
{
m_initialized = false; m_taskBusy[i] = false;
}
m_threadInterface->startSPU(); m_numBusyTasks = 0;
m_currentTask = 0;
} m_initialized = false;
SpuSampleTaskProcess::~SpuSampleTaskProcess() m_threadInterface->startSPU();
{
m_threadInterface->stopSPU();
}
}
SpuSampleTaskProcess::~SpuSampleTaskProcess()
{
m_threadInterface->stopSPU();
void SpuSampleTaskProcess::initialize()
{ }
#ifdef DEBUG_SPU_TASK_SCHEDULING
printf("SpuSampleTaskProcess::initialize()\n");
#endif //DEBUG_SPU_TASK_SCHEDULING
void SpuSampleTaskProcess::initialize()
for (int i = 0; i < m_maxNumOutstandingTasks; i++) {
{ #ifdef DEBUG_SPU_TASK_SCHEDULING
m_taskBusy[i] = false; printf("SpuSampleTaskProcess::initialize()\n");
} #endif //DEBUG_SPU_TASK_SCHEDULING
m_numBusyTasks = 0;
m_currentTask = 0; for (int i = 0; i < m_maxNumOutstandingTasks; i++)
m_initialized = true; {
m_taskBusy[i] = false;
} }
m_numBusyTasks = 0;
m_currentTask = 0;
void SpuSampleTaskProcess::issueTask(void* sampleMainMemPtr,int sampleValue) m_initialized = true;
{
}
#ifdef DEBUG_SPU_TASK_SCHEDULING
printf("SpuSampleTaskProcess::issueTask (m_currentTask= %d\)n", m_currentTask);
#endif //DEBUG_SPU_TASK_SCHEDULING void SpuSampleTaskProcess::issueTask(void* sampleMainMemPtr,int sampleValue)
{
m_taskBusy[m_currentTask] = true;
m_numBusyTasks++; #ifdef DEBUG_SPU_TASK_SCHEDULING
printf("SpuSampleTaskProcess::issueTask (m_currentTask= %d\)n", m_currentTask);
SpuSampleTaskDesc& taskDesc = m_spuSampleTaskDesc[m_currentTask]; #endif //DEBUG_SPU_TASK_SCHEDULING
{
// send task description in event message m_taskBusy[m_currentTask] = true;
// no error checking here... m_numBusyTasks++;
// but, currently, event queue can be no larger than NUM_WORKUNIT_TASKS.
SpuSampleTaskDesc& taskDesc = m_spuSampleTaskDesc[m_currentTask];
taskDesc.m_mainMemoryPtr = reinterpret_cast<uint64_t>(sampleMainMemPtr); {
taskDesc.m_sampleValue = sampleValue; // send task description in event message
// no error checking here...
//some bookkeeping to recognize finished tasks // but, currently, event queue can be no larger than NUM_WORKUNIT_TASKS.
taskDesc.m_taskId = m_currentTask;
} taskDesc.m_mainMemoryPtr = reinterpret_cast<uint64_t>(sampleMainMemPtr);
taskDesc.m_sampleValue = sampleValue;
m_threadInterface->sendRequest(CMD_SAMPLE_TASK_COMMAND, (uint32_t) &taskDesc, m_currentTask); //some bookkeeping to recognize finished tasks
taskDesc.m_taskId = m_currentTask;
// if all tasks busy, wait for spu event to clear the task. }
if (m_numBusyTasks >= m_maxNumOutstandingTasks)
{ m_threadInterface->sendRequest(CMD_SAMPLE_TASK_COMMAND, (uint32_t) &taskDesc, m_currentTask);
unsigned int taskId;
unsigned int outputSize; // if all tasks busy, wait for spu event to clear the task.
m_threadInterface->waitForResponse(&taskId, &outputSize); if (m_numBusyTasks >= m_maxNumOutstandingTasks)
{
//printf("PPU: after issue, received event: %u %d\n", taskId, outputSize); unsigned int taskId;
unsigned int outputSize;
postProcess(taskId, outputSize);
m_threadInterface->waitForResponse(&taskId, &outputSize);
m_taskBusy[taskId] = false;
//printf("PPU: after issue, received event: %u %d\n", taskId, outputSize);
m_numBusyTasks--;
} postProcess(taskId, outputSize);
// find new task buffer m_taskBusy[taskId] = false;
for (unsigned int i = 0; i < m_maxNumOutstandingTasks; i++)
{ m_numBusyTasks--;
if (!m_taskBusy[i]) }
{
m_currentTask = i; // find new task buffer
break; for (unsigned int i = 0; i < m_maxNumOutstandingTasks; i++)
} {
} if (!m_taskBusy[i])
} {
m_currentTask = i;
break;
///Optional PPU-size post processing for each task }
void SpuSampleTaskProcess::postProcess(int taskId, int outputSize) }
{ }
}
///Optional PPU-size post processing for each task
void SpuSampleTaskProcess::postProcess(int taskId, int outputSize)
void SpuSampleTaskProcess::flush() {
{
#ifdef DEBUG_SPU_TASK_SCHEDULING }
printf("\nSpuCollisionTaskProcess::flush()\n");
#endif //DEBUG_SPU_TASK_SCHEDULING
void SpuSampleTaskProcess::flush()
{
// all tasks are issued, wait for all tasks to be complete #ifdef DEBUG_SPU_TASK_SCHEDULING
while(m_numBusyTasks > 0) printf("\nSpuCollisionTaskProcess::flush()\n");
{ #endif //DEBUG_SPU_TASK_SCHEDULING
// Consolidating SPU code
unsigned int taskId;
unsigned int outputSize; // all tasks are issued, wait for all tasks to be complete
while(m_numBusyTasks > 0)
{ {
// Consolidating SPU code
m_threadInterface->waitForResponse(&taskId, &outputSize); unsigned int taskId;
} unsigned int outputSize;
//printf("PPU: flushing, received event: %u %d\n", taskId, outputSize); {
postProcess(taskId, outputSize); m_threadInterface->waitForResponse(&taskId, &outputSize);
}
m_taskBusy[taskId] = false;
//printf("PPU: flushing, received event: %u %d\n", taskId, outputSize);
m_numBusyTasks--;
} postProcess(taskId, outputSize);
m_taskBusy[taskId] = false;
}
#endif //USE_SAMPLE_PROCESS m_numBusyTasks--;
}
}
#endif
#endif //USE_SAMPLE_PROCESS

246
Extras/BulletMultiThreaded/SpuSampleTaskProcess.h
View File

@@ -1,87 +1,159 @@
/* /*
Bullet Continuous Collision Detection and Physics Library Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
This software is provided 'as-is', without any express or implied warranty. This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software. In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose, Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely, including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions: subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution. 3. This notice may not be removed or altered from any source distribution.
*/ */
#ifndef SPU_SAMPLE_TASK_PROCESS_H #ifndef SPU_SAMPLE_TASK_PROCESS_H
#define SPU_SAMPLE_TASK_PROCESS_H #define SPU_SAMPLE_TASK_PROCESS_H
#include <assert.h> #include <assert.h>
#include "PlatformDefinitions.h" #include "PlatformDefinitions.h"
#include <stdlib.h> #include <stdlib.h>
#include "LinearMath/btAlignedObjectArray.h" #include "LinearMath/btAlignedObjectArray.h"
///SpuSampleTaskDesc ///SpuSampleTaskDesc
struct SpuSampleTaskDesc struct SpuSampleTaskDesc
{ {
uint64_t m_mainMemoryPtr; uint64_t m_mainMemoryPtr;
int m_sampleValue; int m_sampleValue;
uint16_t m_taskId; uint16_t m_taskId;
} }
#ifdef __CELLOS_LV2__ #if defined(__CELLOS_LV2__) || defined(USE_LIBSPE2)
__attribute__ ((aligned (16))) __attribute__ ((aligned (16)))
#endif #endif
; ;
//just add your commands here, try to keep them globally unique for debugging purposes //just add your commands here, try to keep them globally unique for debugging purposes
#define CMD_SAMPLE_TASK_COMMAND 10 #define CMD_SAMPLE_TASK_COMMAND 10
/// SpuSampleTaskProcess handles SPU processing of collision pairs. /// SpuSampleTaskProcess handles SPU processing of collision pairs.
/// When PPU issues a task, it will look for completed task buffers /// When PPU issues a task, it will look for completed task buffers
/// PPU will do postprocessing, dependent on workunit output (not likely) /// PPU will do postprocessing, dependent on workunit output (not likely)
class SpuSampleTaskProcess class SpuSampleTaskProcess
{ {
// track task buffers that are being used, and total busy tasks // track task buffers that are being used, and total busy tasks
btAlignedObjectArray<bool> m_taskBusy; btAlignedObjectArray<bool> m_taskBusy;
btAlignedObjectArray<SpuSampleTaskDesc>m_spuSampleTaskDesc; btAlignedObjectArray<SpuSampleTaskDesc>m_spuSampleTaskDesc;
unsigned int m_numBusyTasks; unsigned int m_numBusyTasks;
// the current task and the current entry to insert a new work unit // the current task and the current entry to insert a new work unit
unsigned int m_currentTask; unsigned int m_currentTask;
bool m_initialized; bool m_initialized;
void postProcess(int taskId, int outputSize); void postProcess(int taskId, int outputSize);
class btThreadSupportInterface* m_threadInterface; class btThreadSupportInterface* m_threadInterface;
unsigned int m_maxNumOutstandingTasks; unsigned int m_maxNumOutstandingTasks;
public: public:
SpuSampleTaskProcess(btThreadSupportInterface* threadInterface, unsigned int maxNumOutstandingTasks); SpuSampleTaskProcess(btThreadSupportInterface* threadInterface, unsigned int maxNumOutstandingTasks);
~SpuSampleTaskProcess(); ~SpuSampleTaskProcess();
///call initialize in the beginning of the frame, before addCollisionPairToTask ///call initialize in the beginning of the frame, before addCollisionPairToTask
void initialize(); void initialize();
void issueTask(void* sampleMainMemPtr,int sampleValue); void issueTask(void* sampleMainMemPtr,int sampleValue);
///call flush to submit potential outstanding work to SPUs and wait for all involved SPUs to be finished ///call flush to submit potential outstanding work to SPUs and wait for all involved SPUs to be finished
void flush(); void flush();
}; };
#endif // SPU_SAMPLE_TASK_PROCESS_H #if defined(USE_LIBSPE2) && defined(__SPU__)
////////////////////MAIN/////////////////////////////
#include "../SpuLibspe2Support.h"
#include <spu_intrinsics.h>
#include <spu_mfcio.h>
#include <SpuFakeDma.h>
void * SamplelsMemoryFunc();
void SampleThreadFunc(void* userPtr,void* lsMemory);
int main(unsigned long long speid, addr64 argp, addr64 envp)
{
printf("SPU is up \n");
ATTRIBUTE_ALIGNED128(btSpuStatus status);
ATTRIBUTE_ALIGNED16( SpuSampleTaskDesc taskDesc ) ;
unsigned int received_message = Spu_Mailbox_Event_Nothing;
bool shutdown = false;
cellDmaGet(&status, argp.ull, sizeof(btSpuStatus), DMA_TAG(3), 0, 0);
cellDmaWaitTagStatusAll(DMA_MASK(3));
status.m_status = Spu_Status_Free;
status.m_lsMemory.p = SamplelsMemoryFunc();
cellDmaLargePut(&status, argp.ull, sizeof(btSpuStatus), DMA_TAG(3), 0, 0);
cellDmaWaitTagStatusAll(DMA_MASK(3));
while (!shutdown)
{
received_message = spu_read_in_mbox();
switch(received_message)
{
case Spu_Mailbox_Event_Shutdown:
shutdown = true;
break;
case Spu_Mailbox_Event_Task:
// refresh the status
printf("SPU recieved Task \n");
cellDmaGet(&status, argp.ull, sizeof(btSpuStatus), DMA_TAG(3), 0, 0);
cellDmaWaitTagStatusAll(DMA_MASK(3));
btAssert(status.m_status==Spu_Status_Occupied);
cellDmaGet(&taskDesc, status.m_taskDesc.p, sizeof(SpuSampleTaskDesc), DMA_TAG(3), 0, 0);
cellDmaWaitTagStatusAll(DMA_MASK(3));
SampleThreadFunc((void*)&taskDesc, reinterpret_cast<void*> (taskDesc.m_mainMemoryPtr) );
break;
case Spu_Mailbox_Event_Nothing:
default:
break;
}
// set to status free and wait for next task
status.m_status = Spu_Status_Free;
cellDmaLargePut(&status, argp.ull, sizeof(btSpuStatus), DMA_TAG(3), 0, 0);
cellDmaWaitTagStatusAll(DMA_MASK(3));
}
return 0;
}
//////////////////////////////////////////////////////
#endif
#endif // SPU_SAMPLE_TASK_PROCESS_H

15
Extras/BulletMultiThreaded/SpuSolverTask/SpuParallellSolverTask.h
View File

@@ -25,16 +25,21 @@ Written by: Marten Svanfeldt
#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h" #include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
#include "../SpuSync.h" #include "../SpuSync.h"
#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h" #include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
#include "LinearMath/btAlignedAllocator.h"
ATTRIBUTE_ALIGNED16(struct) ManifoldCellHolder ATTRIBUTE_ALIGNED16(struct) ManifoldCellHolder
{ {
BT_DECLARE_ALIGNED_ALLOCATOR();
uint32_t m_hashCellIndex; uint32_t m_hashCellIndex;
class btPersistentManifold* m_manifold; class btPersistentManifold* m_manifold;
}; };
ATTRIBUTE_ALIGNED16(struct) ConstraintCellHolder ATTRIBUTE_ALIGNED16(struct) ConstraintCellHolder
{ {
BT_DECLARE_ALIGNED_ALLOCATOR();
uint32_t m_hashCellIndex; uint32_t m_hashCellIndex;
uint32_t m_constraintType; uint32_t m_constraintType;
class btTypedConstraint* m_constraint; class btTypedConstraint* m_constraint;
@@ -103,6 +108,8 @@ inline unsigned int spuGetHashCellIndex(int x, int y, int z)
ATTRIBUTE_ALIGNED16(struct) SpuSolverBody ATTRIBUTE_ALIGNED16(struct) SpuSolverBody
{ {
BT_DECLARE_ALIGNED_ALLOCATOR();
btVector3 m_linearVelocity; btVector3 m_linearVelocity;
btVector3 m_angularVelocity; btVector3 m_angularVelocity;
@@ -113,6 +120,8 @@ ATTRIBUTE_ALIGNED16(struct) SpuSolverBody
ATTRIBUTE_ALIGNED16(struct) SpuSolverInternalConstraint ATTRIBUTE_ALIGNED16(struct) SpuSolverInternalConstraint
{ {
BT_DECLARE_ALIGNED_ALLOCATOR();
uint32_t m_localOffsetBodyA; uint32_t m_localOffsetBodyA;
uint32_t m_localOffsetBodyB; uint32_t m_localOffsetBodyB;
@@ -135,6 +144,8 @@ ATTRIBUTE_ALIGNED16(struct) SpuSolverInternalConstraint
ATTRIBUTE_ALIGNED16(struct) SpuSolverConstraint ATTRIBUTE_ALIGNED16(struct) SpuSolverConstraint
{ {
BT_DECLARE_ALIGNED_ALLOCATOR();
uint16_t m_localOffsetBodyA; uint16_t m_localOffsetBodyA;
uint16_t m_localOffsetBodyB; uint16_t m_localOffsetBodyB;
@@ -204,6 +215,8 @@ ATTRIBUTE_ALIGNED16(struct) SpuSolverConstraint
ATTRIBUTE_ALIGNED16(struct) SpuSolverDataDesc ATTRIBUTE_ALIGNED16(struct) SpuSolverDataDesc
{ {
BT_DECLARE_ALIGNED_ALLOCATOR();
SpuSolverHash* m_solverHash; SpuSolverHash* m_solverHash;
SpuSolverBody* m_solverBodyList; SpuSolverBody* m_solverBodyList;
SpuSolverInternalConstraint* m_solverInternalConstraintList; SpuSolverInternalConstraint* m_solverInternalConstraintList;
@@ -214,6 +227,8 @@ ATTRIBUTE_ALIGNED16(struct) SpuSolverDataDesc
ATTRIBUTE_ALIGNED16(struct) SpuSolverTaskDesc ATTRIBUTE_ALIGNED16(struct) SpuSolverTaskDesc
{ {
BT_DECLARE_ALIGNED_ALLOCATOR();
uint32_t m_solverCommand; uint32_t m_solverCommand;
uint32_t m_taskId; uint32_t m_taskId;
SpuSolverDataDesc m_solverData; SpuSolverDataDesc m_solverData;