Added libspe2 parallel Cell SPE support contribution by IBM Germany 'Extreme Blue' project:
Thanks to Benjamin Hoeferlin, Minh Cuong Tran,Martina Huellmann,Frederick Roth.
This commit is contained in:
ejcoumans
@@ -19,9 +19,6 @@ subject to the following restrictions:
|
||||
//#define DEBUG_SPU_COLLISION_DETECTION 1
|
||||
|
||||
|
||||
#include "SpuContactResult.h"
|
||||
|
||||
|
||||
SpuContactResult::SpuContactResult()
|
||||
{
|
||||
m_manifoldAddress = 0;
|
||||
|
||||
@@ -1,51 +1,51 @@
|
||||
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
|
||||
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.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
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.
|
||||
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.
|
||||
*/
|
||||
|
||||
|
||||
#ifndef CONVEX_PENETRATION_DEPTH_H
|
||||
#define CONVEX_PENETRATION_DEPTH_H
|
||||
|
||||
|
||||
|
||||
class btStackAlloc;
|
||||
class btIDebugDraw;
|
||||
class SpuVoronoiSimplexSolver;
|
||||
|
||||
#include <LinearMath/btTransform.h>
|
||||
#include <LinearMath/btPoint3.h>
|
||||
|
||||
|
||||
///ConvexPenetrationDepthSolver provides an interface for penetration depth calculation.
|
||||
class SpuConvexPenetrationDepthSolver
|
||||
{
|
||||
public:
|
||||
|
||||
virtual ~SpuConvexPenetrationDepthSolver() {};
|
||||
virtual bool calcPenDepth( SpuVoronoiSimplexSolver& simplexSolver,
|
||||
void* convexA,void* convexB,int shapeTypeA, int shapeTypeB, float marginA, float marginB,
|
||||
btTransform& transA,const btTransform& transB,
|
||||
btVector3& v, btPoint3& pa, btPoint3& pb,
|
||||
class btIDebugDraw* debugDraw,btStackAlloc* stackAlloc,
|
||||
struct SpuConvexPolyhedronVertexData* convexVertexData
|
||||
) const = 0;
|
||||
|
||||
|
||||
};
|
||||
|
||||
|
||||
|
||||
#endif //CONVEX_PENETRATION_DEPTH_H
|
||||
|
||||
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
|
||||
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.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
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.
|
||||
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.
|
||||
*/
|
||||
|
||||
|
||||
#ifndef CONVEX_PENETRATION_DEPTH_H
|
||||
#define CONVEX_PENETRATION_DEPTH_H
|
||||
|
||||
|
||||
|
||||
class btStackAlloc;
|
||||
class btIDebugDraw;
|
||||
class SpuVoronoiSimplexSolver;
|
||||
|
||||
#include <LinearMath/btTransform.h>
|
||||
#include <LinearMath/btPoint3.h>
|
||||
|
||||
|
||||
///ConvexPenetrationDepthSolver provides an interface for penetration depth calculation.
|
||||
class SpuConvexPenetrationDepthSolver
|
||||
{
|
||||
public:
|
||||
|
||||
virtual ~SpuConvexPenetrationDepthSolver() {};
|
||||
virtual bool calcPenDepth( SpuVoronoiSimplexSolver& simplexSolver,
|
||||
void* convexA,void* convexB,int shapeTypeA, int shapeTypeB, float marginA, float marginB,
|
||||
btTransform& transA,const btTransform& transB,
|
||||
btVector3& v, btPoint3& pa, btPoint3& pb,
|
||||
class btIDebugDraw* debugDraw,btStackAlloc* stackAlloc,
|
||||
struct SpuConvexPolyhedronVertexData* convexVertexData
|
||||
) const = 0;
|
||||
|
||||
|
||||
};
|
||||
|
||||
|
||||
|
||||
#endif //CONVEX_PENETRATION_DEPTH_H
|
||||
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -1,25 +1,128 @@
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
|
||||
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.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
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.
|
||||
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.
|
||||
*/
|
||||
|
||||
#ifndef SPU_GATHERING_COLLISION_TASK_H
|
||||
#define SPU_GATHERING_COLLISION_TASK_H
|
||||
|
||||
struct SpuGatherAndProcessPairsTaskDesc;
|
||||
|
||||
void processCollisionTask(void* userPtr, void* lsMemory);
|
||||
|
||||
void* createCollisionLocalStoreMemory();
|
||||
|
||||
#endif //SPU_GATHERING_COLLISION_TASK_H
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
|
||||
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.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
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.
|
||||
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.
|
||||
*/
|
||||
|
||||
#ifndef SPU_GATHERING_COLLISION_TASK_H
|
||||
#define SPU_GATHERING_COLLISION_TASK_H
|
||||
|
||||
#include "../PlatformDefinitions.h"
|
||||
//#define DEBUG_SPU_COLLISION_DETECTION 1
|
||||
|
||||
|
||||
///Task Description for SPU collision detection
|
||||
struct SpuGatherAndProcessPairsTaskDesc
|
||||
{
|
||||
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
|
||||
|
||||
|
||||
|
||||
@@ -1,311 +1,311 @@
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
|
||||
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.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
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.
|
||||
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.
|
||||
*/
|
||||
|
||||
#include "SpuGjkPairDetector.h"
|
||||
#include "SpuConvexPenetrationDepthSolver.h"
|
||||
#include "SpuLocalSupport.h"
|
||||
|
||||
|
||||
|
||||
#if defined(DEBUG) || defined (_DEBUG)
|
||||
#include <stdio.h> //for debug printf
|
||||
#ifdef __SPU__
|
||||
#include <spu_printf.h>
|
||||
#define printf spu_printf
|
||||
#endif //__SPU__
|
||||
#endif
|
||||
|
||||
//must be above the machine epsilon
|
||||
#define REL_ERROR2 btScalar(1.0e-6)
|
||||
|
||||
//temp globals, to improve GJK/EPA/penetration calculations
|
||||
int gSpuNumDeepPenetrationChecks = 0;
|
||||
int gSpuNumGjkChecks = 0;
|
||||
|
||||
|
||||
|
||||
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_penetrationDepthSolver(penetrationDepthSolver),
|
||||
m_simplexSolver(simplexSolver),
|
||||
m_minkowskiA(objectA),
|
||||
m_minkowskiB(objectB),
|
||||
m_shapeTypeA(shapeTypeA),
|
||||
m_shapeTypeB(shapeTypeB),
|
||||
m_marginA(marginA),
|
||||
m_marginB(marginB),
|
||||
m_ignoreMargin(false),
|
||||
m_lastUsedMethod(-1),
|
||||
m_catchDegeneracies(1)
|
||||
{
|
||||
}
|
||||
|
||||
void SpuGjkPairDetector::getClosestPoints(const SpuClosestPointInput& input,SpuContactResult& output)
|
||||
{
|
||||
btScalar distance=btScalar(0.);
|
||||
btVector3 normalInB(btScalar(0.),btScalar(0.),btScalar(0.));
|
||||
btVector3 pointOnA,pointOnB;
|
||||
btTransform localTransA = input.m_transformA;
|
||||
btTransform localTransB = input.m_transformB;
|
||||
btVector3 positionOffset = (localTransA.getOrigin() + localTransB.getOrigin()) * btScalar(0.5);
|
||||
localTransA.getOrigin() -= positionOffset;
|
||||
localTransB.getOrigin() -= positionOffset;
|
||||
|
||||
btScalar marginA = m_marginA;
|
||||
btScalar marginB = m_marginB;
|
||||
|
||||
gSpuNumGjkChecks++;
|
||||
|
||||
//for CCD we don't use margins
|
||||
if (m_ignoreMargin)
|
||||
{
|
||||
marginA = btScalar(0.);
|
||||
marginB = btScalar(0.);
|
||||
}
|
||||
|
||||
m_curIter = 0;
|
||||
int gGjkMaxIter = 1000;//this is to catch invalid input, perhaps check for #NaN?
|
||||
m_cachedSeparatingAxis.setValue(0,1,0);
|
||||
|
||||
bool isValid = false;
|
||||
bool checkSimplex = false;
|
||||
bool checkPenetration = true;
|
||||
m_degenerateSimplex = 0;
|
||||
|
||||
m_lastUsedMethod = -1;
|
||||
|
||||
{
|
||||
btScalar squaredDistance = SIMD_INFINITY;
|
||||
btScalar delta = btScalar(0.);
|
||||
|
||||
btScalar margin = marginA + marginB;
|
||||
|
||||
|
||||
|
||||
m_simplexSolver->reset();
|
||||
|
||||
for ( ; ; )
|
||||
//while (true)
|
||||
{
|
||||
|
||||
btVector3 seperatingAxisInA = (-m_cachedSeparatingAxis)* input.m_transformA.getBasis();
|
||||
btVector3 seperatingAxisInB = m_cachedSeparatingAxis* input.m_transformB.getBasis();
|
||||
|
||||
// btVector3 pInA = m_minkowskiA->localGetSupportingVertexWithoutMargin(seperatingAxisInA);
|
||||
// btVector3 qInB = m_minkowskiB->localGetSupportingVertexWithoutMargin(seperatingAxisInB);
|
||||
|
||||
btVector3 pInA = localGetSupportingVertexWithoutMargin(m_shapeTypeA, m_minkowskiA, seperatingAxisInA,input.m_convexVertexData);//, &featureIndexA);
|
||||
btVector3 qInB = localGetSupportingVertexWithoutMargin(m_shapeTypeB, m_minkowskiB, seperatingAxisInB,input.m_convexVertexData);//, &featureIndexB);
|
||||
|
||||
|
||||
btPoint3 pWorld = localTransA(pInA);
|
||||
btPoint3 qWorld = localTransB(qInB);
|
||||
|
||||
btVector3 w = pWorld - qWorld;
|
||||
delta = m_cachedSeparatingAxis.dot(w);
|
||||
|
||||
// potential exit, they don't overlap
|
||||
if ((delta > btScalar(0.0)) && (delta * delta > squaredDistance * input.m_maximumDistanceSquared))
|
||||
{
|
||||
checkPenetration = false;
|
||||
break;
|
||||
}
|
||||
|
||||
//exit 0: the new point is already in the simplex, or we didn't come any closer
|
||||
if (m_simplexSolver->inSimplex(w))
|
||||
{
|
||||
m_degenerateSimplex = 1;
|
||||
checkSimplex = true;
|
||||
break;
|
||||
}
|
||||
// are we getting any closer ?
|
||||
btScalar f0 = squaredDistance - delta;
|
||||
btScalar f1 = squaredDistance * REL_ERROR2;
|
||||
|
||||
if (f0 <= f1)
|
||||
{
|
||||
if (f0 <= btScalar(0.))
|
||||
{
|
||||
m_degenerateSimplex = 2;
|
||||
}
|
||||
checkSimplex = true;
|
||||
break;
|
||||
}
|
||||
//add current vertex to simplex
|
||||
m_simplexSolver->addVertex(w, pWorld, qWorld);
|
||||
|
||||
//calculate the closest point to the origin (update vector v)
|
||||
if (!m_simplexSolver->closest(m_cachedSeparatingAxis))
|
||||
{
|
||||
m_degenerateSimplex = 3;
|
||||
checkSimplex = true;
|
||||
break;
|
||||
}
|
||||
|
||||
btScalar previousSquaredDistance = squaredDistance;
|
||||
squaredDistance = m_cachedSeparatingAxis.length2();
|
||||
|
||||
//redundant m_simplexSolver->compute_points(pointOnA, pointOnB);
|
||||
|
||||
//are we getting any closer ?
|
||||
if (previousSquaredDistance - squaredDistance <= SIMD_EPSILON * previousSquaredDistance)
|
||||
{
|
||||
m_simplexSolver->backup_closest(m_cachedSeparatingAxis);
|
||||
checkSimplex = true;
|
||||
break;
|
||||
}
|
||||
|
||||
//degeneracy, this is typically due to invalid/uninitialized worldtransforms for a btCollisionObject
|
||||
if (m_curIter++ > gGjkMaxIter)
|
||||
{
|
||||
#if defined(DEBUG) || defined (_DEBUG)
|
||||
|
||||
printf("SpuGjkPairDetector maxIter exceeded:%i\n",m_curIter);
|
||||
printf("sepAxis=(%f,%f,%f), squaredDistance = %f, shapeTypeA=%i,shapeTypeB=%i\n",
|
||||
m_cachedSeparatingAxis.getX(),
|
||||
m_cachedSeparatingAxis.getY(),
|
||||
m_cachedSeparatingAxis.getZ(),
|
||||
squaredDistance,
|
||||
m_shapeTypeA,
|
||||
m_shapeTypeB);
|
||||
|
||||
#endif
|
||||
break;
|
||||
|
||||
}
|
||||
|
||||
|
||||
bool check = (!m_simplexSolver->fullSimplex());
|
||||
//bool check = (!m_simplexSolver->fullSimplex() && squaredDistance > SIMD_EPSILON * m_simplexSolver->maxVertex());
|
||||
|
||||
if (!check)
|
||||
{
|
||||
//do we need this backup_closest here ?
|
||||
m_simplexSolver->backup_closest(m_cachedSeparatingAxis);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (checkSimplex)
|
||||
{
|
||||
m_simplexSolver->compute_points(pointOnA, pointOnB);
|
||||
normalInB = pointOnA-pointOnB;
|
||||
btScalar lenSqr = m_cachedSeparatingAxis.length2();
|
||||
//valid normal
|
||||
if (lenSqr < 0.0001)
|
||||
{
|
||||
m_degenerateSimplex = 5;
|
||||
}
|
||||
if (lenSqr > SIMD_EPSILON*SIMD_EPSILON)
|
||||
{
|
||||
btScalar rlen = btScalar(1.) / btSqrt(lenSqr );
|
||||
normalInB *= rlen; //normalize
|
||||
btScalar s = btSqrt(squaredDistance);
|
||||
|
||||
btAssert(s > btScalar(0.0));
|
||||
pointOnA -= m_cachedSeparatingAxis * (marginA / s);
|
||||
pointOnB += m_cachedSeparatingAxis * (marginB / s);
|
||||
distance = ((btScalar(1.)/rlen) - margin);
|
||||
isValid = true;
|
||||
|
||||
m_lastUsedMethod = 1;
|
||||
} else
|
||||
{
|
||||
m_lastUsedMethod = 2;
|
||||
}
|
||||
}
|
||||
|
||||
bool catchDegeneratePenetrationCase =
|
||||
(m_catchDegeneracies && m_penetrationDepthSolver && m_degenerateSimplex && ((distance+margin) < 0.01));
|
||||
|
||||
//if (checkPenetration && !isValid)
|
||||
if (checkPenetration && (!isValid || catchDegeneratePenetrationCase ))
|
||||
{
|
||||
//penetration case
|
||||
|
||||
//if there is no way to handle penetrations, bail out
|
||||
if (m_penetrationDepthSolver)
|
||||
{
|
||||
// Penetration depth case.
|
||||
btVector3 tmpPointOnA,tmpPointOnB;
|
||||
|
||||
gSpuNumDeepPenetrationChecks++;
|
||||
|
||||
bool isValid2 = m_penetrationDepthSolver->calcPenDepth(
|
||||
*m_simplexSolver,
|
||||
m_minkowskiA,m_minkowskiB,
|
||||
m_shapeTypeA, m_shapeTypeB,
|
||||
marginA, marginB,
|
||||
localTransA,localTransB,
|
||||
m_cachedSeparatingAxis, tmpPointOnA, tmpPointOnB,
|
||||
0,input.m_stackAlloc,input.m_convexVertexData
|
||||
);
|
||||
|
||||
if (isValid2)
|
||||
{
|
||||
btVector3 tmpNormalInB = tmpPointOnB-tmpPointOnA;
|
||||
btScalar lenSqr = tmpNormalInB.length2();
|
||||
if (lenSqr > (SIMD_EPSILON*SIMD_EPSILON))
|
||||
{
|
||||
tmpNormalInB /= btSqrt(lenSqr);
|
||||
btScalar distance2 = -(tmpPointOnA-tmpPointOnB).length();
|
||||
//only replace valid penetrations when the result is deeper (check)
|
||||
if (!isValid || (distance2 < distance))
|
||||
{
|
||||
distance = distance2;
|
||||
pointOnA = tmpPointOnA;
|
||||
pointOnB = tmpPointOnB;
|
||||
normalInB = tmpNormalInB;
|
||||
isValid = true;
|
||||
m_lastUsedMethod = 3;
|
||||
} else
|
||||
{
|
||||
|
||||
}
|
||||
} else
|
||||
{
|
||||
//isValid = false;
|
||||
m_lastUsedMethod = 4;
|
||||
}
|
||||
} else
|
||||
{
|
||||
m_lastUsedMethod = 5;
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (isValid)
|
||||
{
|
||||
#ifdef __SPU__
|
||||
//spu_printf("distance\n");
|
||||
#endif //__CELLOS_LV2__
|
||||
|
||||
|
||||
output.addContactPoint(
|
||||
normalInB,
|
||||
pointOnB+positionOffset,
|
||||
distance);
|
||||
//printf("gjk add:%f",distance);
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
|
||||
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.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
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.
|
||||
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.
|
||||
*/
|
||||
|
||||
#include "SpuGjkPairDetector.h"
|
||||
#include "SpuConvexPenetrationDepthSolver.h"
|
||||
#include "SpuLocalSupport.h"
|
||||
|
||||
|
||||
|
||||
#if defined(DEBUG) || defined (_DEBUG)
|
||||
#include <stdio.h> //for debug printf
|
||||
#ifdef __SPU__
|
||||
#include <spu_printf.h>
|
||||
#define printf spu_printf
|
||||
#endif //__SPU__
|
||||
#endif
|
||||
|
||||
//must be above the machine epsilon
|
||||
#define REL_ERROR2 btScalar(1.0e-6)
|
||||
|
||||
//temp globals, to improve GJK/EPA/penetration calculations
|
||||
int gSpuNumDeepPenetrationChecks = 0;
|
||||
int gSpuNumGjkChecks = 0;
|
||||
|
||||
|
||||
|
||||
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_penetrationDepthSolver(penetrationDepthSolver),
|
||||
m_simplexSolver(simplexSolver),
|
||||
m_minkowskiA(objectA),
|
||||
m_minkowskiB(objectB),
|
||||
m_shapeTypeA(shapeTypeA),
|
||||
m_shapeTypeB(shapeTypeB),
|
||||
m_marginA(marginA),
|
||||
m_marginB(marginB),
|
||||
m_ignoreMargin(false),
|
||||
m_lastUsedMethod(-1),
|
||||
m_catchDegeneracies(1)
|
||||
{
|
||||
}
|
||||
|
||||
void SpuGjkPairDetector::getClosestPoints(const SpuClosestPointInput& input,SpuContactResult& output)
|
||||
{
|
||||
btScalar distance=btScalar(0.);
|
||||
btVector3 normalInB(btScalar(0.),btScalar(0.),btScalar(0.));
|
||||
btVector3 pointOnA,pointOnB;
|
||||
btTransform localTransA = input.m_transformA;
|
||||
btTransform localTransB = input.m_transformB;
|
||||
btVector3 positionOffset = (localTransA.getOrigin() + localTransB.getOrigin()) * btScalar(0.5);
|
||||
localTransA.getOrigin() -= positionOffset;
|
||||
localTransB.getOrigin() -= positionOffset;
|
||||
|
||||
btScalar marginA = m_marginA;
|
||||
btScalar marginB = m_marginB;
|
||||
|
||||
gSpuNumGjkChecks++;
|
||||
|
||||
//for CCD we don't use margins
|
||||
if (m_ignoreMargin)
|
||||
{
|
||||
marginA = btScalar(0.);
|
||||
marginB = btScalar(0.);
|
||||
}
|
||||
|
||||
m_curIter = 0;
|
||||
int gGjkMaxIter = 1000;//this is to catch invalid input, perhaps check for #NaN?
|
||||
m_cachedSeparatingAxis.setValue(0,1,0);
|
||||
|
||||
bool isValid = false;
|
||||
bool checkSimplex = false;
|
||||
bool checkPenetration = true;
|
||||
m_degenerateSimplex = 0;
|
||||
|
||||
m_lastUsedMethod = -1;
|
||||
|
||||
{
|
||||
btScalar squaredDistance = SIMD_INFINITY;
|
||||
btScalar delta = btScalar(0.);
|
||||
|
||||
btScalar margin = marginA + marginB;
|
||||
|
||||
|
||||
|
||||
m_simplexSolver->reset();
|
||||
|
||||
for ( ; ; )
|
||||
//while (true)
|
||||
{
|
||||
|
||||
btVector3 seperatingAxisInA = (-m_cachedSeparatingAxis)* input.m_transformA.getBasis();
|
||||
btVector3 seperatingAxisInB = m_cachedSeparatingAxis* input.m_transformB.getBasis();
|
||||
|
||||
// btVector3 pInA = m_minkowskiA->localGetSupportingVertexWithoutMargin(seperatingAxisInA);
|
||||
// btVector3 qInB = m_minkowskiB->localGetSupportingVertexWithoutMargin(seperatingAxisInB);
|
||||
|
||||
btVector3 pInA = localGetSupportingVertexWithoutMargin(m_shapeTypeA, m_minkowskiA, seperatingAxisInA,input.m_convexVertexData);//, &featureIndexA);
|
||||
btVector3 qInB = localGetSupportingVertexWithoutMargin(m_shapeTypeB, m_minkowskiB, seperatingAxisInB,input.m_convexVertexData);//, &featureIndexB);
|
||||
|
||||
|
||||
btPoint3 pWorld = localTransA(pInA);
|
||||
btPoint3 qWorld = localTransB(qInB);
|
||||
|
||||
btVector3 w = pWorld - qWorld;
|
||||
delta = m_cachedSeparatingAxis.dot(w);
|
||||
|
||||
// potential exit, they don't overlap
|
||||
if ((delta > btScalar(0.0)) && (delta * delta > squaredDistance * input.m_maximumDistanceSquared))
|
||||
{
|
||||
checkPenetration = false;
|
||||
break;
|
||||
}
|
||||
|
||||
//exit 0: the new point is already in the simplex, or we didn't come any closer
|
||||
if (m_simplexSolver->inSimplex(w))
|
||||
{
|
||||
m_degenerateSimplex = 1;
|
||||
checkSimplex = true;
|
||||
break;
|
||||
}
|
||||
// are we getting any closer ?
|
||||
btScalar f0 = squaredDistance - delta;
|
||||
btScalar f1 = squaredDistance * REL_ERROR2;
|
||||
|
||||
if (f0 <= f1)
|
||||
{
|
||||
if (f0 <= btScalar(0.))
|
||||
{
|
||||
m_degenerateSimplex = 2;
|
||||
}
|
||||
checkSimplex = true;
|
||||
break;
|
||||
}
|
||||
//add current vertex to simplex
|
||||
m_simplexSolver->addVertex(w, pWorld, qWorld);
|
||||
|
||||
//calculate the closest point to the origin (update vector v)
|
||||
if (!m_simplexSolver->closest(m_cachedSeparatingAxis))
|
||||
{
|
||||
m_degenerateSimplex = 3;
|
||||
checkSimplex = true;
|
||||
break;
|
||||
}
|
||||
|
||||
btScalar previousSquaredDistance = squaredDistance;
|
||||
squaredDistance = m_cachedSeparatingAxis.length2();
|
||||
|
||||
//redundant m_simplexSolver->compute_points(pointOnA, pointOnB);
|
||||
|
||||
//are we getting any closer ?
|
||||
if (previousSquaredDistance - squaredDistance <= SIMD_EPSILON * previousSquaredDistance)
|
||||
{
|
||||
m_simplexSolver->backup_closest(m_cachedSeparatingAxis);
|
||||
checkSimplex = true;
|
||||
break;
|
||||
}
|
||||
|
||||
//degeneracy, this is typically due to invalid/uninitialized worldtransforms for a btCollisionObject
|
||||
if (m_curIter++ > gGjkMaxIter)
|
||||
{
|
||||
#if defined(DEBUG) || defined (_DEBUG)
|
||||
|
||||
printf("SpuGjkPairDetector maxIter exceeded:%i\n",m_curIter);
|
||||
printf("sepAxis=(%f,%f,%f), squaredDistance = %f, shapeTypeA=%i,shapeTypeB=%i\n",
|
||||
m_cachedSeparatingAxis.getX(),
|
||||
m_cachedSeparatingAxis.getY(),
|
||||
m_cachedSeparatingAxis.getZ(),
|
||||
squaredDistance,
|
||||
m_shapeTypeA,
|
||||
m_shapeTypeB);
|
||||
|
||||
#endif
|
||||
break;
|
||||
|
||||
}
|
||||
|
||||
|
||||
bool check = (!m_simplexSolver->fullSimplex());
|
||||
//bool check = (!m_simplexSolver->fullSimplex() && squaredDistance > SIMD_EPSILON * m_simplexSolver->maxVertex());
|
||||
|
||||
if (!check)
|
||||
{
|
||||
//do we need this backup_closest here ?
|
||||
m_simplexSolver->backup_closest(m_cachedSeparatingAxis);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (checkSimplex)
|
||||
{
|
||||
m_simplexSolver->compute_points(pointOnA, pointOnB);
|
||||
normalInB = pointOnA-pointOnB;
|
||||
btScalar lenSqr = m_cachedSeparatingAxis.length2();
|
||||
//valid normal
|
||||
if (lenSqr < 0.0001)
|
||||
{
|
||||
m_degenerateSimplex = 5;
|
||||
}
|
||||
if (lenSqr > SIMD_EPSILON*SIMD_EPSILON)
|
||||
{
|
||||
btScalar rlen = btScalar(1.) / btSqrt(lenSqr );
|
||||
normalInB *= rlen; //normalize
|
||||
btScalar s = btSqrt(squaredDistance);
|
||||
|
||||
btAssert(s > btScalar(0.0));
|
||||
pointOnA -= m_cachedSeparatingAxis * (marginA / s);
|
||||
pointOnB += m_cachedSeparatingAxis * (marginB / s);
|
||||
distance = ((btScalar(1.)/rlen) - margin);
|
||||
isValid = true;
|
||||
|
||||
m_lastUsedMethod = 1;
|
||||
} else
|
||||
{
|
||||
m_lastUsedMethod = 2;
|
||||
}
|
||||
}
|
||||
|
||||
bool catchDegeneratePenetrationCase =
|
||||
(m_catchDegeneracies && m_penetrationDepthSolver && m_degenerateSimplex && ((distance+margin) < 0.01));
|
||||
|
||||
//if (checkPenetration && !isValid)
|
||||
if (checkPenetration && (!isValid || catchDegeneratePenetrationCase ))
|
||||
{
|
||||
//penetration case
|
||||
|
||||
//if there is no way to handle penetrations, bail out
|
||||
if (m_penetrationDepthSolver)
|
||||
{
|
||||
// Penetration depth case.
|
||||
btVector3 tmpPointOnA,tmpPointOnB;
|
||||
|
||||
gSpuNumDeepPenetrationChecks++;
|
||||
|
||||
bool isValid2 = m_penetrationDepthSolver->calcPenDepth(
|
||||
*m_simplexSolver,
|
||||
m_minkowskiA,m_minkowskiB,
|
||||
m_shapeTypeA, m_shapeTypeB,
|
||||
marginA, marginB,
|
||||
localTransA,localTransB,
|
||||
m_cachedSeparatingAxis, tmpPointOnA, tmpPointOnB,
|
||||
0,input.m_stackAlloc,input.m_convexVertexData
|
||||
);
|
||||
|
||||
if (isValid2)
|
||||
{
|
||||
btVector3 tmpNormalInB = tmpPointOnB-tmpPointOnA;
|
||||
btScalar lenSqr = tmpNormalInB.length2();
|
||||
if (lenSqr > (SIMD_EPSILON*SIMD_EPSILON))
|
||||
{
|
||||
tmpNormalInB /= btSqrt(lenSqr);
|
||||
btScalar distance2 = -(tmpPointOnA-tmpPointOnB).length();
|
||||
//only replace valid penetrations when the result is deeper (check)
|
||||
if (!isValid || (distance2 < distance))
|
||||
{
|
||||
distance = distance2;
|
||||
pointOnA = tmpPointOnA;
|
||||
pointOnB = tmpPointOnB;
|
||||
normalInB = tmpNormalInB;
|
||||
isValid = true;
|
||||
m_lastUsedMethod = 3;
|
||||
} else
|
||||
{
|
||||
|
||||
}
|
||||
} else
|
||||
{
|
||||
//isValid = false;
|
||||
m_lastUsedMethod = 4;
|
||||
}
|
||||
} else
|
||||
{
|
||||
m_lastUsedMethod = 5;
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (isValid)
|
||||
{
|
||||
#ifdef __SPU__
|
||||
//spu_printf("distance\n");
|
||||
#endif //__SPU__
|
||||
|
||||
|
||||
output.addContactPoint(
|
||||
normalInB,
|
||||
pointOnB+positionOffset,
|
||||
distance);
|
||||
//printf("gjk add:%f",distance);
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -1,93 +1,93 @@
|
||||
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
|
||||
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.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
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.
|
||||
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.
|
||||
*/
|
||||
|
||||
|
||||
|
||||
|
||||
#ifndef SPU_GJK_PAIR_DETECTOR_H
|
||||
#define SPU_GJK_PAIR_DETECTOR_H
|
||||
|
||||
|
||||
|
||||
#include "SpuContactResult.h"
|
||||
|
||||
|
||||
#include "SpuVoronoiSimplexSolver.h"
|
||||
class SpuConvexPenetrationDepthSolver;
|
||||
|
||||
/// btGjkPairDetector uses GJK to implement the btDiscreteCollisionDetectorInterface
|
||||
class SpuGjkPairDetector
|
||||
{
|
||||
|
||||
|
||||
btVector3 m_cachedSeparatingAxis;
|
||||
const SpuConvexPenetrationDepthSolver* m_penetrationDepthSolver;
|
||||
SpuVoronoiSimplexSolver* m_simplexSolver;
|
||||
void* m_minkowskiA;
|
||||
void* m_minkowskiB;
|
||||
int m_shapeTypeA;
|
||||
int m_shapeTypeB;
|
||||
float m_marginA;
|
||||
float m_marginB;
|
||||
bool m_ignoreMargin;
|
||||
|
||||
|
||||
public:
|
||||
|
||||
//some debugging to fix degeneracy problems
|
||||
int m_lastUsedMethod;
|
||||
int m_curIter;
|
||||
int m_degenerateSimplex;
|
||||
int m_catchDegeneracies;
|
||||
|
||||
|
||||
SpuGjkPairDetector(void* objectA,void* objectB,int m_shapeTypeA, int m_shapeTypeB, float marginA, float marginB, SpuVoronoiSimplexSolver* simplexSolver, const SpuConvexPenetrationDepthSolver* penetrationDepthSolver);
|
||||
virtual ~SpuGjkPairDetector() {};
|
||||
|
||||
virtual void getClosestPoints(const SpuClosestPointInput& input,SpuContactResult& output);
|
||||
|
||||
void setMinkowskiA(void* minkA)
|
||||
{
|
||||
m_minkowskiA = minkA;
|
||||
}
|
||||
|
||||
void setMinkowskiB(void* minkB)
|
||||
{
|
||||
m_minkowskiB = minkB;
|
||||
}
|
||||
|
||||
void setCachedSeperatingAxis(const btVector3& seperatingAxis)
|
||||
{
|
||||
m_cachedSeparatingAxis = seperatingAxis;
|
||||
}
|
||||
|
||||
void setPenetrationDepthSolver(SpuConvexPenetrationDepthSolver* penetrationDepthSolver)
|
||||
{
|
||||
m_penetrationDepthSolver = penetrationDepthSolver;
|
||||
}
|
||||
|
||||
///don't use setIgnoreMargin, it's for Bullet's internal use
|
||||
void setIgnoreMargin(bool ignoreMargin)
|
||||
{
|
||||
m_ignoreMargin = ignoreMargin;
|
||||
}
|
||||
|
||||
|
||||
};
|
||||
|
||||
|
||||
|
||||
#endif //SPU_GJK_PAIR_DETECTOR_H
|
||||
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
|
||||
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.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
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.
|
||||
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.
|
||||
*/
|
||||
|
||||
|
||||
|
||||
|
||||
#ifndef SPU_GJK_PAIR_DETECTOR_H
|
||||
#define SPU_GJK_PAIR_DETECTOR_H
|
||||
|
||||
|
||||
|
||||
#include "SpuContactResult.h"
|
||||
|
||||
|
||||
#include "SpuVoronoiSimplexSolver.h"
|
||||
class SpuConvexPenetrationDepthSolver;
|
||||
|
||||
/// btGjkPairDetector uses GJK to implement the btDiscreteCollisionDetectorInterface
|
||||
class SpuGjkPairDetector
|
||||
{
|
||||
|
||||
|
||||
btVector3 m_cachedSeparatingAxis;
|
||||
const SpuConvexPenetrationDepthSolver* m_penetrationDepthSolver;
|
||||
SpuVoronoiSimplexSolver* m_simplexSolver;
|
||||
void* m_minkowskiA;
|
||||
void* m_minkowskiB;
|
||||
int m_shapeTypeA;
|
||||
int m_shapeTypeB;
|
||||
float m_marginA;
|
||||
float m_marginB;
|
||||
bool m_ignoreMargin;
|
||||
|
||||
|
||||
public:
|
||||
|
||||
//some debugging to fix degeneracy problems
|
||||
int m_lastUsedMethod;
|
||||
int m_curIter;
|
||||
int m_degenerateSimplex;
|
||||
int m_catchDegeneracies;
|
||||
|
||||
|
||||
SpuGjkPairDetector(void* objectA,void* objectB,int m_shapeTypeA, int m_shapeTypeB, float marginA, float marginB, SpuVoronoiSimplexSolver* simplexSolver, const SpuConvexPenetrationDepthSolver* penetrationDepthSolver);
|
||||
virtual ~SpuGjkPairDetector() {};
|
||||
|
||||
virtual void getClosestPoints(const SpuClosestPointInput& input,SpuContactResult& output);
|
||||
|
||||
void setMinkowskiA(void* minkA)
|
||||
{
|
||||
m_minkowskiA = minkA;
|
||||
}
|
||||
|
||||
void setMinkowskiB(void* minkB)
|
||||
{
|
||||
m_minkowskiB = minkB;
|
||||
}
|
||||
|
||||
void setCachedSeperatingAxis(const btVector3& seperatingAxis)
|
||||
{
|
||||
m_cachedSeparatingAxis = seperatingAxis;
|
||||
}
|
||||
|
||||
void setPenetrationDepthSolver(SpuConvexPenetrationDepthSolver* penetrationDepthSolver)
|
||||
{
|
||||
m_penetrationDepthSolver = penetrationDepthSolver;
|
||||
}
|
||||
|
||||
///don't use setIgnoreMargin, it's for Bullet's internal use
|
||||
void setIgnoreMargin(bool ignoreMargin)
|
||||
{
|
||||
m_ignoreMargin = ignoreMargin;
|
||||
}
|
||||
|
||||
|
||||
};
|
||||
|
||||
|
||||
|
||||
#endif //SPU_GJK_PAIR_DETECTOR_H
|
||||
|
||||
@@ -1,249 +1,249 @@
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
|
||||
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.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
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.
|
||||
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.
|
||||
*/
|
||||
|
||||
|
||||
|
||||
|
||||
#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
|
||||
#include "BulletCollision/CollisionShapes/btConvexInternalShape.h"
|
||||
#include "BulletCollision/CollisionShapes/btCylinderShape.h"
|
||||
|
||||
#define MAX_NUM_SPU_CONVEX_POINTS 128
|
||||
|
||||
struct SpuConvexPolyhedronVertexData
|
||||
{
|
||||
void* gSpuConvexShapePtr0;
|
||||
void* gSpuConvexShapePtr1;
|
||||
btPoint3* gConvexPoints0;
|
||||
btPoint3* gConvexPoints1;
|
||||
int gNumConvexPoints0;
|
||||
int gNumConvexPoints1;
|
||||
ATTRIBUTE_ALIGNED16(btPoint3 g_convexPointBuffer0[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)
|
||||
{
|
||||
switch (shapeType)
|
||||
{
|
||||
case SPHERE_SHAPE_PROXYTYPE:
|
||||
{
|
||||
return btPoint3(0,0,0);
|
||||
}
|
||||
case BOX_SHAPE_PROXYTYPE:
|
||||
{
|
||||
// spu_printf("SPU: getSupport BOX_SHAPE_PROXYTYPE\n");
|
||||
btConvexInternalShape* convexShape = (btConvexInternalShape*)shape;
|
||||
btVector3 halfExtents = convexShape->getImplicitShapeDimensions();
|
||||
float margin = convexShape->getMarginNV();
|
||||
halfExtents -= btVector3(margin,margin,margin);
|
||||
return btPoint3(
|
||||
localDir.getX() < 0.0f ? -halfExtents.x() : halfExtents.x(),
|
||||
localDir.getY() < 0.0f ? -halfExtents.y() : halfExtents.y(),
|
||||
localDir.getZ() < 0.0f ? -halfExtents.z() : halfExtents.z());
|
||||
}
|
||||
|
||||
case TRIANGLE_SHAPE_PROXYTYPE:
|
||||
{
|
||||
|
||||
btVector3 dir(localDir.getX(),localDir.getY(),localDir.getZ());
|
||||
btVector3* vertices = (btVector3*)shape;
|
||||
btVector3 dots(dir.dot(vertices[0]), dir.dot(vertices[1]), dir.dot(vertices[2]));
|
||||
btVector3 sup = vertices[dots.maxAxis()];
|
||||
return btPoint3(sup.getX(),sup.getY(),sup.getZ());
|
||||
break;
|
||||
}
|
||||
|
||||
case CYLINDER_SHAPE_PROXYTYPE:
|
||||
{
|
||||
btCylinderShape* cylShape = (btCylinderShape*)shape;
|
||||
|
||||
//mapping of halfextents/dimension onto radius/height depends on how cylinder local orientation is (upAxis)
|
||||
|
||||
btVector3 halfExtents = cylShape->getImplicitShapeDimensions();
|
||||
btVector3 v(localDir.getX(),localDir.getY(),localDir.getZ());
|
||||
|
||||
int cylinderUpAxis = cylShape->getUpAxis();
|
||||
int XX(1),YY(0),ZZ(2);
|
||||
|
||||
switch (cylinderUpAxis)
|
||||
{
|
||||
case 0:
|
||||
{
|
||||
XX = 1;
|
||||
YY = 0;
|
||||
ZZ = 2;
|
||||
break;
|
||||
}
|
||||
case 1:
|
||||
{
|
||||
XX = 0;
|
||||
YY = 1;
|
||||
ZZ = 2;
|
||||
break;
|
||||
}
|
||||
case 2:
|
||||
{
|
||||
XX = 0;
|
||||
YY = 2;
|
||||
ZZ = 1;
|
||||
break;
|
||||
}
|
||||
default:
|
||||
btAssert(0);
|
||||
//printf("SPU:localGetSupportingVertexWithoutMargin unknown Cylinder up-axis\n");
|
||||
};
|
||||
|
||||
btScalar radius = halfExtents[XX];
|
||||
btScalar halfHeight = halfExtents[cylinderUpAxis];
|
||||
|
||||
btVector3 tmp;
|
||||
btScalar d ;
|
||||
|
||||
btScalar s = btSqrt(v[XX] * v[XX] + v[ZZ] * v[ZZ]);
|
||||
if (s != btScalar(0.0))
|
||||
{
|
||||
d = radius / s;
|
||||
tmp[XX] = v[XX] * d;
|
||||
tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight;
|
||||
tmp[ZZ] = v[ZZ] * d;
|
||||
return btPoint3(tmp.getX(),tmp.getY(),tmp.getZ());
|
||||
}
|
||||
else
|
||||
{
|
||||
tmp[XX] = radius;
|
||||
tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight;
|
||||
tmp[ZZ] = btScalar(0.0);
|
||||
return btPoint3(tmp.getX(),tmp.getY(),tmp.getZ());
|
||||
}
|
||||
}
|
||||
|
||||
case CAPSULE_SHAPE_PROXYTYPE:
|
||||
{
|
||||
//spu_printf("SPU: todo: getSupport CAPSULE_SHAPE_PROXYTYPE\n");
|
||||
btVector3 vec0(localDir.getX(),localDir.getY(),localDir.getZ());
|
||||
|
||||
btConvexInternalShape* cnvxShape = (btConvexInternalShape*)shape;
|
||||
btVector3 halfExtents = cnvxShape->getImplicitShapeDimensions();
|
||||
btScalar halfHeight = halfExtents.getY();
|
||||
btScalar radius = halfExtents.getX();
|
||||
btVector3 supVec(0,0,0);
|
||||
|
||||
btScalar maxDot(btScalar(-1e30));
|
||||
|
||||
btVector3 vec = vec0;
|
||||
btScalar lenSqr = vec.length2();
|
||||
if (lenSqr < btScalar(0.0001))
|
||||
{
|
||||
vec.setValue(1,0,0);
|
||||
} else
|
||||
{
|
||||
btScalar rlen = btScalar(1.) / btSqrt(lenSqr );
|
||||
vec *= rlen;
|
||||
}
|
||||
btVector3 vtx;
|
||||
btScalar newDot;
|
||||
{
|
||||
btVector3 pos(0,halfHeight,0);
|
||||
vtx = pos +vec*(radius);
|
||||
newDot = vec.dot(vtx);
|
||||
if (newDot > maxDot)
|
||||
{
|
||||
maxDot = newDot;
|
||||
supVec = vtx;
|
||||
}
|
||||
}
|
||||
{
|
||||
btVector3 pos(0,-halfHeight,0);
|
||||
vtx = pos +vec*(radius);
|
||||
newDot = vec.dot(vtx);
|
||||
if (newDot > maxDot)
|
||||
{
|
||||
maxDot = newDot;
|
||||
supVec = vtx;
|
||||
}
|
||||
}
|
||||
return btPoint3(supVec.getX(),supVec.getY(),supVec.getZ());
|
||||
break;
|
||||
};
|
||||
|
||||
case CONVEX_HULL_SHAPE_PROXYTYPE:
|
||||
{
|
||||
//spu_printf("SPU: todo: getSupport CONVEX_HULL_SHAPE_PROXYTYPE\n");
|
||||
|
||||
|
||||
|
||||
btPoint3* points = 0;
|
||||
int numPoints = 0;
|
||||
if (shape==convexVertexData->gSpuConvexShapePtr0)
|
||||
{
|
||||
points = convexVertexData->gConvexPoints0;
|
||||
numPoints = convexVertexData->gNumConvexPoints0;
|
||||
}
|
||||
if (shape == convexVertexData->gSpuConvexShapePtr1)
|
||||
{
|
||||
points = convexVertexData->gConvexPoints1;
|
||||
numPoints = convexVertexData->gNumConvexPoints1;
|
||||
}
|
||||
|
||||
// spu_printf("numPoints = %d\n",numPoints);
|
||||
|
||||
btVector3 supVec(btScalar(0.),btScalar(0.),btScalar(0.));
|
||||
btScalar newDot,maxDot = btScalar(-1e30);
|
||||
|
||||
btVector3 vec0(localDir.getX(),localDir.getY(),localDir.getZ());
|
||||
btVector3 vec = vec0;
|
||||
btScalar lenSqr = vec.length2();
|
||||
if (lenSqr < btScalar(0.0001))
|
||||
{
|
||||
vec.setValue(1,0,0);
|
||||
} else
|
||||
{
|
||||
btScalar rlen = btScalar(1.) / btSqrt(lenSqr );
|
||||
vec *= rlen;
|
||||
}
|
||||
|
||||
|
||||
for (int i=0;i<numPoints;i++)
|
||||
{
|
||||
btPoint3 vtx = points[i];// * m_localScaling;
|
||||
|
||||
newDot = vec.dot(vtx);
|
||||
if (newDot > maxDot)
|
||||
{
|
||||
maxDot = newDot;
|
||||
supVec = vtx;
|
||||
}
|
||||
}
|
||||
return btPoint3(supVec.getX(),supVec.getY(),supVec.getZ());
|
||||
|
||||
break;
|
||||
};
|
||||
|
||||
default:
|
||||
|
||||
//spu_printf("SPU:(type %i) missing support function\n",shapeType);
|
||||
|
||||
|
||||
#if __ASSERT
|
||||
spu_printf("localGetSupportingVertexWithoutMargin() - Unsupported bound type: %d.\n", shapeType);
|
||||
#endif // __ASSERT
|
||||
return btPoint3(0.f, 0.f, 0.f);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
|
||||
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.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
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.
|
||||
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.
|
||||
*/
|
||||
|
||||
|
||||
|
||||
|
||||
#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
|
||||
#include "BulletCollision/CollisionShapes/btConvexInternalShape.h"
|
||||
#include "BulletCollision/CollisionShapes/btCylinderShape.h"
|
||||
|
||||
#define MAX_NUM_SPU_CONVEX_POINTS 128
|
||||
|
||||
struct SpuConvexPolyhedronVertexData
|
||||
{
|
||||
void* gSpuConvexShapePtr0;
|
||||
void* gSpuConvexShapePtr1;
|
||||
btPoint3* gConvexPoints0;
|
||||
btPoint3* gConvexPoints1;
|
||||
int gNumConvexPoints0;
|
||||
int gNumConvexPoints1;
|
||||
ATTRIBUTE_ALIGNED16(btPoint3 g_convexPointBuffer0[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)
|
||||
{
|
||||
switch (shapeType)
|
||||
{
|
||||
case SPHERE_SHAPE_PROXYTYPE:
|
||||
{
|
||||
return btPoint3(0,0,0);
|
||||
}
|
||||
case BOX_SHAPE_PROXYTYPE:
|
||||
{
|
||||
// spu_printf("SPU: getSupport BOX_SHAPE_PROXYTYPE\n");
|
||||
btConvexInternalShape* convexShape = (btConvexInternalShape*)shape;
|
||||
btVector3 halfExtents = convexShape->getImplicitShapeDimensions();
|
||||
float margin = convexShape->getMarginNV();
|
||||
halfExtents -= btVector3(margin,margin,margin);
|
||||
return btPoint3(
|
||||
localDir.getX() < 0.0f ? -halfExtents.x() : halfExtents.x(),
|
||||
localDir.getY() < 0.0f ? -halfExtents.y() : halfExtents.y(),
|
||||
localDir.getZ() < 0.0f ? -halfExtents.z() : halfExtents.z());
|
||||
}
|
||||
|
||||
case TRIANGLE_SHAPE_PROXYTYPE:
|
||||
{
|
||||
|
||||
btVector3 dir(localDir.getX(),localDir.getY(),localDir.getZ());
|
||||
btVector3* vertices = (btVector3*)shape;
|
||||
btVector3 dots(dir.dot(vertices[0]), dir.dot(vertices[1]), dir.dot(vertices[2]));
|
||||
btVector3 sup = vertices[dots.maxAxis()];
|
||||
return btPoint3(sup.getX(),sup.getY(),sup.getZ());
|
||||
break;
|
||||
}
|
||||
|
||||
case CYLINDER_SHAPE_PROXYTYPE:
|
||||
{
|
||||
btCylinderShape* cylShape = (btCylinderShape*)shape;
|
||||
|
||||
//mapping of halfextents/dimension onto radius/height depends on how cylinder local orientation is (upAxis)
|
||||
|
||||
btVector3 halfExtents = cylShape->getImplicitShapeDimensions();
|
||||
btVector3 v(localDir.getX(),localDir.getY(),localDir.getZ());
|
||||
|
||||
int cylinderUpAxis = cylShape->getUpAxis();
|
||||
int XX(1),YY(0),ZZ(2);
|
||||
|
||||
switch (cylinderUpAxis)
|
||||
{
|
||||
case 0:
|
||||
{
|
||||
XX = 1;
|
||||
YY = 0;
|
||||
ZZ = 2;
|
||||
break;
|
||||
}
|
||||
case 1:
|
||||
{
|
||||
XX = 0;
|
||||
YY = 1;
|
||||
ZZ = 2;
|
||||
break;
|
||||
}
|
||||
case 2:
|
||||
{
|
||||
XX = 0;
|
||||
YY = 2;
|
||||
ZZ = 1;
|
||||
break;
|
||||
}
|
||||
default:
|
||||
btAssert(0);
|
||||
//printf("SPU:localGetSupportingVertexWithoutMargin unknown Cylinder up-axis\n");
|
||||
};
|
||||
|
||||
btScalar radius = halfExtents[XX];
|
||||
btScalar halfHeight = halfExtents[cylinderUpAxis];
|
||||
|
||||
btVector3 tmp;
|
||||
btScalar d ;
|
||||
|
||||
btScalar s = btSqrt(v[XX] * v[XX] + v[ZZ] * v[ZZ]);
|
||||
if (s != btScalar(0.0))
|
||||
{
|
||||
d = radius / s;
|
||||
tmp[XX] = v[XX] * d;
|
||||
tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight;
|
||||
tmp[ZZ] = v[ZZ] * d;
|
||||
return btPoint3(tmp.getX(),tmp.getY(),tmp.getZ());
|
||||
}
|
||||
else
|
||||
{
|
||||
tmp[XX] = radius;
|
||||
tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight;
|
||||
tmp[ZZ] = btScalar(0.0);
|
||||
return btPoint3(tmp.getX(),tmp.getY(),tmp.getZ());
|
||||
}
|
||||
}
|
||||
|
||||
case CAPSULE_SHAPE_PROXYTYPE:
|
||||
{
|
||||
//spu_printf("SPU: todo: getSupport CAPSULE_SHAPE_PROXYTYPE\n");
|
||||
btVector3 vec0(localDir.getX(),localDir.getY(),localDir.getZ());
|
||||
|
||||
btConvexInternalShape* cnvxShape = (btConvexInternalShape*)shape;
|
||||
btVector3 halfExtents = cnvxShape->getImplicitShapeDimensions();
|
||||
btScalar halfHeight = halfExtents.getY();
|
||||
btScalar radius = halfExtents.getX();
|
||||
btVector3 supVec(0,0,0);
|
||||
|
||||
btScalar maxDot(btScalar(-1e30));
|
||||
|
||||
btVector3 vec = vec0;
|
||||
btScalar lenSqr = vec.length2();
|
||||
if (lenSqr < btScalar(0.0001))
|
||||
{
|
||||
vec.setValue(1,0,0);
|
||||
} else
|
||||
{
|
||||
btScalar rlen = btScalar(1.) / btSqrt(lenSqr );
|
||||
vec *= rlen;
|
||||
}
|
||||
btVector3 vtx;
|
||||
btScalar newDot;
|
||||
{
|
||||
btVector3 pos(0,halfHeight,0);
|
||||
vtx = pos +vec*(radius);
|
||||
newDot = vec.dot(vtx);
|
||||
if (newDot > maxDot)
|
||||
{
|
||||
maxDot = newDot;
|
||||
supVec = vtx;
|
||||
}
|
||||
}
|
||||
{
|
||||
btVector3 pos(0,-halfHeight,0);
|
||||
vtx = pos +vec*(radius);
|
||||
newDot = vec.dot(vtx);
|
||||
if (newDot > maxDot)
|
||||
{
|
||||
maxDot = newDot;
|
||||
supVec = vtx;
|
||||
}
|
||||
}
|
||||
return btPoint3(supVec.getX(),supVec.getY(),supVec.getZ());
|
||||
break;
|
||||
};
|
||||
|
||||
case CONVEX_HULL_SHAPE_PROXYTYPE:
|
||||
{
|
||||
//spu_printf("SPU: todo: getSupport CONVEX_HULL_SHAPE_PROXYTYPE\n");
|
||||
|
||||
|
||||
|
||||
btPoint3* points = 0;
|
||||
int numPoints = 0;
|
||||
if (shape==convexVertexData->gSpuConvexShapePtr0)
|
||||
{
|
||||
points = convexVertexData->gConvexPoints0;
|
||||
numPoints = convexVertexData->gNumConvexPoints0;
|
||||
}
|
||||
if (shape == convexVertexData->gSpuConvexShapePtr1)
|
||||
{
|
||||
points = convexVertexData->gConvexPoints1;
|
||||
numPoints = convexVertexData->gNumConvexPoints1;
|
||||
}
|
||||
|
||||
// spu_printf("numPoints = %d\n",numPoints);
|
||||
|
||||
btVector3 supVec(btScalar(0.),btScalar(0.),btScalar(0.));
|
||||
btScalar newDot,maxDot = btScalar(-1e30);
|
||||
|
||||
btVector3 vec0(localDir.getX(),localDir.getY(),localDir.getZ());
|
||||
btVector3 vec = vec0;
|
||||
btScalar lenSqr = vec.length2();
|
||||
if (lenSqr < btScalar(0.0001))
|
||||
{
|
||||
vec.setValue(1,0,0);
|
||||
} else
|
||||
{
|
||||
btScalar rlen = btScalar(1.) / btSqrt(lenSqr );
|
||||
vec *= rlen;
|
||||
}
|
||||
|
||||
|
||||
for (int i=0;i<numPoints;i++)
|
||||
{
|
||||
btPoint3 vtx = points[i];// * m_localScaling;
|
||||
|
||||
newDot = vec.dot(vtx);
|
||||
if (newDot > maxDot)
|
||||
{
|
||||
maxDot = newDot;
|
||||
supVec = vtx;
|
||||
}
|
||||
}
|
||||
return btPoint3(supVec.getX(),supVec.getY(),supVec.getZ());
|
||||
|
||||
break;
|
||||
};
|
||||
|
||||
default:
|
||||
|
||||
//spu_printf("SPU:(type %i) missing support function\n",shapeType);
|
||||
|
||||
|
||||
#if __ASSERT
|
||||
spu_printf("localGetSupportingVertexWithoutMargin() - Unsupported bound type: %d.\n", shapeType);
|
||||
#endif // __ASSERT
|
||||
return btPoint3(0.f, 0.f, 0.f);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
@@ -1,342 +1,342 @@
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
|
||||
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.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
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.
|
||||
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.
|
||||
*/
|
||||
|
||||
#include "SpuMinkowskiPenetrationDepthSolver.h"
|
||||
#include "SpuVoronoiSimplexSolver.h"
|
||||
#include "SpuGjkPairDetector.h"
|
||||
#include "SpuContactResult.h"
|
||||
#include "SpuPreferredPenetrationDirections.h"
|
||||
|
||||
|
||||
#include "SpuLocalSupport.h"
|
||||
|
||||
#define NUM_UNITSPHERE_POINTS 42
|
||||
static btVector3 sPenetrationDirections[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2] =
|
||||
{
|
||||
btVector3(btScalar(0.000000) , btScalar(-0.000000),btScalar(-1.000000)),
|
||||
btVector3(btScalar(0.723608) , btScalar(-0.525725),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.276388) , btScalar(0.850649),btScalar(-0.447220)),
|
||||
btVector3(btScalar(0.723608) , btScalar(0.525725),btScalar(-0.447219)),
|
||||
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.447219)),
|
||||
btVector3(btScalar(0.894426) , btScalar(0.000000),btScalar(0.447216)),
|
||||
btVector3(btScalar(-0.000000) , btScalar(0.000000),btScalar(1.000000)),
|
||||
btVector3(btScalar(0.425323) , btScalar(-0.309011),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.425323) , btScalar(0.309011),btScalar(-0.850654)),
|
||||
btVector3(btScalar(0.850648) , btScalar(-0.000000),btScalar(-0.525736)),
|
||||
btVector3(btScalar(-0.525730) , btScalar(-0.000000),btScalar(-0.850652)),
|
||||
btVector3(btScalar(-0.688190) , btScalar(-0.499997),btScalar(-0.525736)),
|
||||
btVector3(btScalar(-0.162456) , btScalar(0.499995),btScalar(-0.850654)),
|
||||
btVector3(btScalar(-0.688190) , btScalar(0.499997),btScalar(-0.525736)),
|
||||
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.587786) , btScalar(-0.809017),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.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.000000) , btScalar(1.000000),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.262869) , btScalar(-0.809012),btScalar(0.525738)),
|
||||
btVector3(btScalar(-0.850648) , btScalar(0.000000),btScalar(0.525736)),
|
||||
btVector3(btScalar(-0.262869) , btScalar(0.809012),btScalar(0.525738)),
|
||||
btVector3(btScalar(0.688190) , btScalar(0.499997),btScalar(0.525736)),
|
||||
btVector3(btScalar(0.525730) , btScalar(0.000000),btScalar(0.850652)),
|
||||
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.162456) , btScalar(0.499995),btScalar(0.850654))
|
||||
};
|
||||
|
||||
bool SpuMinkowskiPenetrationDepthSolver::calcPenDepth( SpuVoronoiSimplexSolver& simplexSolver,
|
||||
void* convexA,void* convexB,int shapeTypeA, int shapeTypeB, float marginA, float marginB,
|
||||
btTransform& transA,const btTransform& transB,
|
||||
btVector3& v, btPoint3& pa, btPoint3& pb,
|
||||
class btIDebugDraw* debugDraw,btStackAlloc* stackAlloc,
|
||||
struct SpuConvexPolyhedronVertexData* convexVertexData
|
||||
) const
|
||||
{
|
||||
|
||||
(void)stackAlloc;
|
||||
(void)v;
|
||||
|
||||
|
||||
struct btIntermediateResult : public SpuContactResult
|
||||
{
|
||||
|
||||
btIntermediateResult():m_hasResult(false)
|
||||
{
|
||||
}
|
||||
|
||||
btVector3 m_normalOnBInWorld;
|
||||
btVector3 m_pointInWorld;
|
||||
btScalar m_depth;
|
||||
bool m_hasResult;
|
||||
|
||||
virtual void setShapeIdentifiers(int partId0,int index0, int partId1,int index1)
|
||||
{
|
||||
(void)partId0;
|
||||
(void)index0;
|
||||
(void)partId1;
|
||||
(void)index1;
|
||||
}
|
||||
void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth)
|
||||
{
|
||||
m_normalOnBInWorld = normalOnBInWorld;
|
||||
m_pointInWorld = pointInWorld;
|
||||
m_depth = depth;
|
||||
m_hasResult = true;
|
||||
}
|
||||
};
|
||||
|
||||
//just take fixed number of orientation, and sample the penetration depth in that direction
|
||||
btScalar minProj = btScalar(1e30);
|
||||
btVector3 minNorm;
|
||||
btVector3 minVertex;
|
||||
btVector3 minA,minB;
|
||||
btVector3 seperatingAxisInA,seperatingAxisInB;
|
||||
btVector3 pInA,qInB,pWorld,qWorld,w;
|
||||
|
||||
//#define USE_BATCHED_SUPPORT 1
|
||||
#ifdef USE_BATCHED_SUPPORT
|
||||
|
||||
btVector3 supportVerticesABatch[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 seperatingAxisInBBatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2];
|
||||
int i;
|
||||
|
||||
int numSampleDirections = NUM_UNITSPHERE_POINTS;
|
||||
|
||||
for (i=0;i<numSampleDirections;i++)
|
||||
{
|
||||
const btVector3& norm = sPenetrationDirections[i];
|
||||
seperatingAxisInABatch[i] = (-norm) * transA.getBasis() ;
|
||||
seperatingAxisInBBatch[i] = norm * transB.getBasis() ;
|
||||
}
|
||||
|
||||
{
|
||||
int numPDA = convexA->getNumPreferredPenetrationDirections();
|
||||
if (numPDA)
|
||||
{
|
||||
for (int i=0;i<numPDA;i++)
|
||||
{
|
||||
btVector3 norm;
|
||||
convexA->getPreferredPenetrationDirection(i,norm);
|
||||
norm = transA.getBasis() * norm;
|
||||
sPenetrationDirections[numSampleDirections] = norm;
|
||||
seperatingAxisInABatch[numSampleDirections] = (-norm) * transA.getBasis();
|
||||
seperatingAxisInBBatch[numSampleDirections] = norm * transB.getBasis();
|
||||
numSampleDirections++;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
{
|
||||
int numPDB = convexB->getNumPreferredPenetrationDirections();
|
||||
if (numPDB)
|
||||
{
|
||||
for (int i=0;i<numPDB;i++)
|
||||
{
|
||||
btVector3 norm;
|
||||
convexB->getPreferredPenetrationDirection(i,norm);
|
||||
norm = transB.getBasis() * norm;
|
||||
sPenetrationDirections[numSampleDirections] = norm;
|
||||
seperatingAxisInABatch[numSampleDirections] = (-norm) * transA.getBasis();
|
||||
seperatingAxisInBBatch[numSampleDirections] = norm * transB.getBasis();
|
||||
numSampleDirections++;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
convexA->batchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInABatch,supportVerticesABatch,numSampleDirections);
|
||||
convexB->batchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInBBatch,supportVerticesBBatch,numSampleDirections);
|
||||
|
||||
for (i=0;i<numSampleDirections;i++)
|
||||
{
|
||||
const btVector3& norm = sPenetrationDirections[i];
|
||||
seperatingAxisInA = seperatingAxisInABatch[i];
|
||||
seperatingAxisInB = seperatingAxisInBBatch[i];
|
||||
|
||||
pInA = supportVerticesABatch[i];
|
||||
qInB = supportVerticesBBatch[i];
|
||||
|
||||
pWorld = transA(pInA);
|
||||
qWorld = transB(qInB);
|
||||
w = qWorld - pWorld;
|
||||
btScalar delta = norm.dot(w);
|
||||
//find smallest delta
|
||||
if (delta < minProj)
|
||||
{
|
||||
minProj = delta;
|
||||
minNorm = norm;
|
||||
minA = pWorld;
|
||||
minB = qWorld;
|
||||
}
|
||||
}
|
||||
#else
|
||||
|
||||
int numSampleDirections = NUM_UNITSPHERE_POINTS;
|
||||
|
||||
///this is necessary, otherwise the normal is not correct, and sphere will rotate forever on a sloped triangle mesh
|
||||
#define DO_PREFERRED_DIRECTIONS 1
|
||||
#ifdef DO_PREFERRED_DIRECTIONS
|
||||
{
|
||||
int numPDA = spuGetNumPreferredPenetrationDirections(shapeTypeA,convexA);
|
||||
if (numPDA)
|
||||
{
|
||||
for (int i=0;i<numPDA;i++)
|
||||
{
|
||||
btVector3 norm;
|
||||
spuGetPreferredPenetrationDirection(shapeTypeA,convexA,i,norm);
|
||||
norm = transA.getBasis() * norm;
|
||||
sPenetrationDirections[numSampleDirections] = norm;
|
||||
numSampleDirections++;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
{
|
||||
int numPDB = spuGetNumPreferredPenetrationDirections(shapeTypeB,convexB);
|
||||
if (numPDB)
|
||||
{
|
||||
for (int i=0;i<numPDB;i++)
|
||||
{
|
||||
btVector3 norm;
|
||||
spuGetPreferredPenetrationDirection(shapeTypeB,convexB,i,norm);
|
||||
norm = transB.getBasis() * norm;
|
||||
sPenetrationDirections[numSampleDirections] = norm;
|
||||
numSampleDirections++;
|
||||
}
|
||||
}
|
||||
}
|
||||
#endif //DO_PREFERRED_DIRECTIONS
|
||||
|
||||
for (int i=0;i<numSampleDirections;i++)
|
||||
{
|
||||
const btVector3& norm = sPenetrationDirections[i];
|
||||
seperatingAxisInA = (-norm)* transA.getBasis();
|
||||
seperatingAxisInB = norm* transB.getBasis();
|
||||
|
||||
pInA = localGetSupportingVertexWithoutMargin(shapeTypeA, convexA, seperatingAxisInA,convexVertexData);//, NULL);
|
||||
qInB = localGetSupportingVertexWithoutMargin(shapeTypeB, convexB, seperatingAxisInB,convexVertexData);//, NULL);
|
||||
|
||||
// pInA = convexA->localGetSupportingVertexWithoutMargin(seperatingAxisInA);
|
||||
// qInB = convexB->localGetSupportingVertexWithoutMargin(seperatingAxisInB);
|
||||
|
||||
pWorld = transA(pInA);
|
||||
qWorld = transB(qInB);
|
||||
w = qWorld - pWorld;
|
||||
btScalar delta = norm.dot(w);
|
||||
//find smallest delta
|
||||
if (delta < minProj)
|
||||
{
|
||||
minProj = delta;
|
||||
minNorm = norm;
|
||||
minA = pWorld;
|
||||
minB = qWorld;
|
||||
}
|
||||
}
|
||||
#endif //USE_BATCHED_SUPPORT
|
||||
|
||||
//add the margins
|
||||
|
||||
minA += minNorm*marginA;
|
||||
minB -= minNorm*marginB;
|
||||
//no penetration
|
||||
if (minProj < btScalar(0.))
|
||||
return false;
|
||||
|
||||
minProj += (marginA + marginB);
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
//#define DEBUG_DRAW 1
|
||||
#ifdef DEBUG_DRAW
|
||||
if (debugDraw)
|
||||
{
|
||||
btVector3 color(0,1,0);
|
||||
debugDraw->drawLine(minA,minB,color);
|
||||
color = btVector3 (1,1,1);
|
||||
btVector3 vec = minB-minA;
|
||||
btScalar prj2 = minNorm.dot(vec);
|
||||
debugDraw->drawLine(minA,minA+(minNorm*minProj),color);
|
||||
|
||||
}
|
||||
#endif //DEBUG_DRAW
|
||||
|
||||
|
||||
|
||||
SpuGjkPairDetector gjkdet(convexA,convexB,shapeTypeA,shapeTypeB,marginA,marginB,&simplexSolver,0);
|
||||
|
||||
btScalar offsetDist = minProj;
|
||||
btVector3 offset = minNorm * offsetDist;
|
||||
|
||||
|
||||
SpuClosestPointInput input;
|
||||
input.m_convexVertexData = convexVertexData;
|
||||
btVector3 newOrg = transA.getOrigin() + offset;
|
||||
|
||||
btTransform displacedTrans = transA;
|
||||
displacedTrans.setOrigin(newOrg);
|
||||
|
||||
input.m_transformA = displacedTrans;
|
||||
input.m_transformB = transB;
|
||||
input.m_maximumDistanceSquared = btScalar(1e30);//minProj;
|
||||
|
||||
btIntermediateResult res;
|
||||
gjkdet.getClosestPoints(input,res);
|
||||
|
||||
btScalar correctedMinNorm = minProj - res.m_depth;
|
||||
|
||||
|
||||
//the penetration depth is over-estimated, relax it
|
||||
btScalar penetration_relaxation= btScalar(1.);
|
||||
minNorm*=penetration_relaxation;
|
||||
|
||||
if (res.m_hasResult)
|
||||
{
|
||||
|
||||
pa = res.m_pointInWorld - minNorm * correctedMinNorm;
|
||||
pb = res.m_pointInWorld;
|
||||
|
||||
#ifdef DEBUG_DRAW
|
||||
if (debugDraw)
|
||||
{
|
||||
btVector3 color(1,0,0);
|
||||
debugDraw->drawLine(pa,pb,color);
|
||||
}
|
||||
#endif//DEBUG_DRAW
|
||||
|
||||
|
||||
}
|
||||
return res.m_hasResult;
|
||||
}
|
||||
|
||||
|
||||
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
|
||||
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.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
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.
|
||||
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.
|
||||
*/
|
||||
|
||||
#include "SpuMinkowskiPenetrationDepthSolver.h"
|
||||
#include "SpuVoronoiSimplexSolver.h"
|
||||
#include "SpuGjkPairDetector.h"
|
||||
#include "SpuContactResult.h"
|
||||
#include "SpuPreferredPenetrationDirections.h"
|
||||
|
||||
|
||||
#include "SpuLocalSupport.h"
|
||||
|
||||
#define NUM_UNITSPHERE_POINTS 42
|
||||
static btVector3 sPenetrationDirections[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2] =
|
||||
{
|
||||
btVector3(btScalar(0.000000) , btScalar(-0.000000),btScalar(-1.000000)),
|
||||
btVector3(btScalar(0.723608) , btScalar(-0.525725),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.276388) , btScalar(0.850649),btScalar(-0.447220)),
|
||||
btVector3(btScalar(0.723608) , btScalar(0.525725),btScalar(-0.447219)),
|
||||
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.447219)),
|
||||
btVector3(btScalar(0.894426) , btScalar(0.000000),btScalar(0.447216)),
|
||||
btVector3(btScalar(-0.000000) , btScalar(0.000000),btScalar(1.000000)),
|
||||
btVector3(btScalar(0.425323) , btScalar(-0.309011),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.425323) , btScalar(0.309011),btScalar(-0.850654)),
|
||||
btVector3(btScalar(0.850648) , btScalar(-0.000000),btScalar(-0.525736)),
|
||||
btVector3(btScalar(-0.525730) , btScalar(-0.000000),btScalar(-0.850652)),
|
||||
btVector3(btScalar(-0.688190) , btScalar(-0.499997),btScalar(-0.525736)),
|
||||
btVector3(btScalar(-0.162456) , btScalar(0.499995),btScalar(-0.850654)),
|
||||
btVector3(btScalar(-0.688190) , btScalar(0.499997),btScalar(-0.525736)),
|
||||
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.587786) , btScalar(-0.809017),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.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.000000) , btScalar(1.000000),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.262869) , btScalar(-0.809012),btScalar(0.525738)),
|
||||
btVector3(btScalar(-0.850648) , btScalar(0.000000),btScalar(0.525736)),
|
||||
btVector3(btScalar(-0.262869) , btScalar(0.809012),btScalar(0.525738)),
|
||||
btVector3(btScalar(0.688190) , btScalar(0.499997),btScalar(0.525736)),
|
||||
btVector3(btScalar(0.525730) , btScalar(0.000000),btScalar(0.850652)),
|
||||
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.162456) , btScalar(0.499995),btScalar(0.850654))
|
||||
};
|
||||
|
||||
bool SpuMinkowskiPenetrationDepthSolver::calcPenDepth( SpuVoronoiSimplexSolver& simplexSolver,
|
||||
void* convexA,void* convexB,int shapeTypeA, int shapeTypeB, float marginA, float marginB,
|
||||
btTransform& transA,const btTransform& transB,
|
||||
btVector3& v, btPoint3& pa, btPoint3& pb,
|
||||
class btIDebugDraw* debugDraw,btStackAlloc* stackAlloc,
|
||||
struct SpuConvexPolyhedronVertexData* convexVertexData
|
||||
) const
|
||||
{
|
||||
|
||||
(void)stackAlloc;
|
||||
(void)v;
|
||||
|
||||
|
||||
struct btIntermediateResult : public SpuContactResult
|
||||
{
|
||||
|
||||
btIntermediateResult():m_hasResult(false)
|
||||
{
|
||||
}
|
||||
|
||||
btVector3 m_normalOnBInWorld;
|
||||
btVector3 m_pointInWorld;
|
||||
btScalar m_depth;
|
||||
bool m_hasResult;
|
||||
|
||||
virtual void setShapeIdentifiers(int partId0,int index0, int partId1,int index1)
|
||||
{
|
||||
(void)partId0;
|
||||
(void)index0;
|
||||
(void)partId1;
|
||||
(void)index1;
|
||||
}
|
||||
void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth)
|
||||
{
|
||||
m_normalOnBInWorld = normalOnBInWorld;
|
||||
m_pointInWorld = pointInWorld;
|
||||
m_depth = depth;
|
||||
m_hasResult = true;
|
||||
}
|
||||
};
|
||||
|
||||
//just take fixed number of orientation, and sample the penetration depth in that direction
|
||||
btScalar minProj = btScalar(1e30);
|
||||
btVector3 minNorm;
|
||||
btVector3 minVertex;
|
||||
btVector3 minA,minB;
|
||||
btVector3 seperatingAxisInA,seperatingAxisInB;
|
||||
btVector3 pInA,qInB,pWorld,qWorld,w;
|
||||
|
||||
//#define USE_BATCHED_SUPPORT 1
|
||||
#ifdef USE_BATCHED_SUPPORT
|
||||
|
||||
btVector3 supportVerticesABatch[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 seperatingAxisInBBatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2];
|
||||
int i;
|
||||
|
||||
int numSampleDirections = NUM_UNITSPHERE_POINTS;
|
||||
|
||||
for (i=0;i<numSampleDirections;i++)
|
||||
{
|
||||
const btVector3& norm = sPenetrationDirections[i];
|
||||
seperatingAxisInABatch[i] = (-norm) * transA.getBasis() ;
|
||||
seperatingAxisInBBatch[i] = norm * transB.getBasis() ;
|
||||
}
|
||||
|
||||
{
|
||||
int numPDA = convexA->getNumPreferredPenetrationDirections();
|
||||
if (numPDA)
|
||||
{
|
||||
for (int i=0;i<numPDA;i++)
|
||||
{
|
||||
btVector3 norm;
|
||||
convexA->getPreferredPenetrationDirection(i,norm);
|
||||
norm = transA.getBasis() * norm;
|
||||
sPenetrationDirections[numSampleDirections] = norm;
|
||||
seperatingAxisInABatch[numSampleDirections] = (-norm) * transA.getBasis();
|
||||
seperatingAxisInBBatch[numSampleDirections] = norm * transB.getBasis();
|
||||
numSampleDirections++;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
{
|
||||
int numPDB = convexB->getNumPreferredPenetrationDirections();
|
||||
if (numPDB)
|
||||
{
|
||||
for (int i=0;i<numPDB;i++)
|
||||
{
|
||||
btVector3 norm;
|
||||
convexB->getPreferredPenetrationDirection(i,norm);
|
||||
norm = transB.getBasis() * norm;
|
||||
sPenetrationDirections[numSampleDirections] = norm;
|
||||
seperatingAxisInABatch[numSampleDirections] = (-norm) * transA.getBasis();
|
||||
seperatingAxisInBBatch[numSampleDirections] = norm * transB.getBasis();
|
||||
numSampleDirections++;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
convexA->batchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInABatch,supportVerticesABatch,numSampleDirections);
|
||||
convexB->batchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInBBatch,supportVerticesBBatch,numSampleDirections);
|
||||
|
||||
for (i=0;i<numSampleDirections;i++)
|
||||
{
|
||||
const btVector3& norm = sPenetrationDirections[i];
|
||||
seperatingAxisInA = seperatingAxisInABatch[i];
|
||||
seperatingAxisInB = seperatingAxisInBBatch[i];
|
||||
|
||||
pInA = supportVerticesABatch[i];
|
||||
qInB = supportVerticesBBatch[i];
|
||||
|
||||
pWorld = transA(pInA);
|
||||
qWorld = transB(qInB);
|
||||
w = qWorld - pWorld;
|
||||
btScalar delta = norm.dot(w);
|
||||
//find smallest delta
|
||||
if (delta < minProj)
|
||||
{
|
||||
minProj = delta;
|
||||
minNorm = norm;
|
||||
minA = pWorld;
|
||||
minB = qWorld;
|
||||
}
|
||||
}
|
||||
#else
|
||||
|
||||
int numSampleDirections = NUM_UNITSPHERE_POINTS;
|
||||
|
||||
///this is necessary, otherwise the normal is not correct, and sphere will rotate forever on a sloped triangle mesh
|
||||
#define DO_PREFERRED_DIRECTIONS 1
|
||||
#ifdef DO_PREFERRED_DIRECTIONS
|
||||
{
|
||||
int numPDA = spuGetNumPreferredPenetrationDirections(shapeTypeA,convexA);
|
||||
if (numPDA)
|
||||
{
|
||||
for (int i=0;i<numPDA;i++)
|
||||
{
|
||||
btVector3 norm;
|
||||
spuGetPreferredPenetrationDirection(shapeTypeA,convexA,i,norm);
|
||||
norm = transA.getBasis() * norm;
|
||||
sPenetrationDirections[numSampleDirections] = norm;
|
||||
numSampleDirections++;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
{
|
||||
int numPDB = spuGetNumPreferredPenetrationDirections(shapeTypeB,convexB);
|
||||
if (numPDB)
|
||||
{
|
||||
for (int i=0;i<numPDB;i++)
|
||||
{
|
||||
btVector3 norm;
|
||||
spuGetPreferredPenetrationDirection(shapeTypeB,convexB,i,norm);
|
||||
norm = transB.getBasis() * norm;
|
||||
sPenetrationDirections[numSampleDirections] = norm;
|
||||
numSampleDirections++;
|
||||
}
|
||||
}
|
||||
}
|
||||
#endif //DO_PREFERRED_DIRECTIONS
|
||||
|
||||
for (int i=0;i<numSampleDirections;i++)
|
||||
{
|
||||
const btVector3& norm = sPenetrationDirections[i];
|
||||
seperatingAxisInA = (-norm)* transA.getBasis();
|
||||
seperatingAxisInB = norm* transB.getBasis();
|
||||
|
||||
pInA = localGetSupportingVertexWithoutMargin(shapeTypeA, convexA, seperatingAxisInA,convexVertexData);//, NULL);
|
||||
qInB = localGetSupportingVertexWithoutMargin(shapeTypeB, convexB, seperatingAxisInB,convexVertexData);//, NULL);
|
||||
|
||||
// pInA = convexA->localGetSupportingVertexWithoutMargin(seperatingAxisInA);
|
||||
// qInB = convexB->localGetSupportingVertexWithoutMargin(seperatingAxisInB);
|
||||
|
||||
pWorld = transA(pInA);
|
||||
qWorld = transB(qInB);
|
||||
w = qWorld - pWorld;
|
||||
btScalar delta = norm.dot(w);
|
||||
//find smallest delta
|
||||
if (delta < minProj)
|
||||
{
|
||||
minProj = delta;
|
||||
minNorm = norm;
|
||||
minA = pWorld;
|
||||
minB = qWorld;
|
||||
}
|
||||
}
|
||||
#endif //USE_BATCHED_SUPPORT
|
||||
|
||||
//add the margins
|
||||
|
||||
minA += minNorm*marginA;
|
||||
minB -= minNorm*marginB;
|
||||
//no penetration
|
||||
if (minProj < btScalar(0.))
|
||||
return false;
|
||||
|
||||
minProj += (marginA + marginB);
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
//#define DEBUG_DRAW 1
|
||||
#ifdef DEBUG_DRAW
|
||||
if (debugDraw)
|
||||
{
|
||||
btVector3 color(0,1,0);
|
||||
debugDraw->drawLine(minA,minB,color);
|
||||
color = btVector3 (1,1,1);
|
||||
btVector3 vec = minB-minA;
|
||||
btScalar prj2 = minNorm.dot(vec);
|
||||
debugDraw->drawLine(minA,minA+(minNorm*minProj),color);
|
||||
|
||||
}
|
||||
#endif //DEBUG_DRAW
|
||||
|
||||
|
||||
|
||||
SpuGjkPairDetector gjkdet(convexA,convexB,shapeTypeA,shapeTypeB,marginA,marginB,&simplexSolver,0);
|
||||
|
||||
btScalar offsetDist = minProj;
|
||||
btVector3 offset = minNorm * offsetDist;
|
||||
|
||||
|
||||
SpuClosestPointInput input;
|
||||
input.m_convexVertexData = convexVertexData;
|
||||
btVector3 newOrg = transA.getOrigin() + offset;
|
||||
|
||||
btTransform displacedTrans = transA;
|
||||
displacedTrans.setOrigin(newOrg);
|
||||
|
||||
input.m_transformA = displacedTrans;
|
||||
input.m_transformB = transB;
|
||||
input.m_maximumDistanceSquared = btScalar(1e30);//minProj;
|
||||
|
||||
btIntermediateResult res;
|
||||
gjkdet.getClosestPoints(input,res);
|
||||
|
||||
btScalar correctedMinNorm = minProj - res.m_depth;
|
||||
|
||||
|
||||
//the penetration depth is over-estimated, relax it
|
||||
btScalar penetration_relaxation= btScalar(1.);
|
||||
minNorm*=penetration_relaxation;
|
||||
|
||||
if (res.m_hasResult)
|
||||
{
|
||||
|
||||
pa = res.m_pointInWorld - minNorm * correctedMinNorm;
|
||||
pb = res.m_pointInWorld;
|
||||
|
||||
#ifdef DEBUG_DRAW
|
||||
if (debugDraw)
|
||||
{
|
||||
btVector3 color(1,0,0);
|
||||
debugDraw->drawLine(pa,pb,color);
|
||||
}
|
||||
#endif//DEBUG_DRAW
|
||||
|
||||
|
||||
}
|
||||
return res.m_hasResult;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -1,46 +1,46 @@
|
||||
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
|
||||
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.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
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.
|
||||
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.
|
||||
*/
|
||||
|
||||
#ifndef MINKOWSKI_PENETRATION_DEPTH_SOLVER_H
|
||||
#define MINKOWSKI_PENETRATION_DEPTH_SOLVER_H
|
||||
|
||||
|
||||
#include "SpuConvexPenetrationDepthSolver.h"
|
||||
|
||||
class btStackAlloc;
|
||||
class btIDebugDraw;
|
||||
class SpuVoronoiSimplexSolver;
|
||||
|
||||
///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.
|
||||
class SpuMinkowskiPenetrationDepthSolver : public SpuConvexPenetrationDepthSolver
|
||||
{
|
||||
public:
|
||||
|
||||
virtual bool calcPenDepth( SpuVoronoiSimplexSolver& simplexSolver,
|
||||
void* convexA,void* convexB,int shapeTypeA, int shapeTypeB, float marginA, float marginB,
|
||||
btTransform& transA,const btTransform& transB,
|
||||
btVector3& v, btPoint3& pa, btPoint3& pb,
|
||||
class btIDebugDraw* debugDraw,btStackAlloc* stackAlloc,
|
||||
struct SpuConvexPolyhedronVertexData* convexVertexData
|
||||
) const;
|
||||
|
||||
|
||||
};
|
||||
|
||||
|
||||
#endif //MINKOWSKI_PENETRATION_DEPTH_SOLVER_H
|
||||
|
||||
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
|
||||
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.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
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.
|
||||
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.
|
||||
*/
|
||||
|
||||
#ifndef MINKOWSKI_PENETRATION_DEPTH_SOLVER_H
|
||||
#define MINKOWSKI_PENETRATION_DEPTH_SOLVER_H
|
||||
|
||||
|
||||
#include "SpuConvexPenetrationDepthSolver.h"
|
||||
|
||||
class btStackAlloc;
|
||||
class btIDebugDraw;
|
||||
class SpuVoronoiSimplexSolver;
|
||||
|
||||
///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.
|
||||
class SpuMinkowskiPenetrationDepthSolver : public SpuConvexPenetrationDepthSolver
|
||||
{
|
||||
public:
|
||||
|
||||
virtual bool calcPenDepth( SpuVoronoiSimplexSolver& simplexSolver,
|
||||
void* convexA,void* convexB,int shapeTypeA, int shapeTypeB, float marginA, float marginB,
|
||||
btTransform& transA,const btTransform& transB,
|
||||
btVector3& v, btPoint3& pa, btPoint3& pb,
|
||||
class btIDebugDraw* debugDraw,btStackAlloc* stackAlloc,
|
||||
struct SpuConvexPolyhedronVertexData* convexVertexData
|
||||
) const;
|
||||
|
||||
|
||||
};
|
||||
|
||||
|
||||
#endif //MINKOWSKI_PENETRATION_DEPTH_SOLVER_H
|
||||
|
||||
|
||||
@@ -1,70 +1,70 @@
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
|
||||
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.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
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.
|
||||
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.
|
||||
*/
|
||||
|
||||
#ifndef _SPU_PREFERRED_PENETRATION_DIRECTIONS_H
|
||||
#define _SPU_PREFERRED_PENETRATION_DIRECTIONS_H
|
||||
|
||||
|
||||
#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
|
||||
|
||||
int spuGetNumPreferredPenetrationDirections(int shapeType, void* shape)
|
||||
{
|
||||
switch (shapeType)
|
||||
{
|
||||
case TRIANGLE_SHAPE_PROXYTYPE:
|
||||
{
|
||||
return 2;
|
||||
//spu_printf("2\n");
|
||||
break;
|
||||
}
|
||||
default:
|
||||
{
|
||||
#if __ASSERT
|
||||
spu_printf("spuGetNumPreferredPenetrationDirections() - Unsupported bound type: %d.\n", shapeType);
|
||||
#endif // __ASSERT
|
||||
}
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
void spuGetPreferredPenetrationDirection(int shapeType, void* shape, int index, btVector3& penetrationVector)
|
||||
{
|
||||
|
||||
|
||||
switch (shapeType)
|
||||
{
|
||||
case TRIANGLE_SHAPE_PROXYTYPE:
|
||||
{
|
||||
btVector3* vertices = (btVector3*)shape;
|
||||
///calcNormal
|
||||
penetrationVector = (vertices[1]-vertices[0]).cross(vertices[2]-vertices[0]);
|
||||
penetrationVector.normalize();
|
||||
if (index)
|
||||
penetrationVector *= btScalar(-1.);
|
||||
break;
|
||||
}
|
||||
default:
|
||||
{
|
||||
|
||||
#if __ASSERT
|
||||
spu_printf("spuGetNumPreferredPenetrationDirections() - Unsupported bound type: %d.\n", shapeType);
|
||||
#endif // __ASSERT
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
#endif //_SPU_PREFERRED_PENETRATION_DIRECTIONS_H
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
|
||||
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.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
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.
|
||||
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.
|
||||
*/
|
||||
|
||||
#ifndef _SPU_PREFERRED_PENETRATION_DIRECTIONS_H
|
||||
#define _SPU_PREFERRED_PENETRATION_DIRECTIONS_H
|
||||
|
||||
|
||||
#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
|
||||
|
||||
int spuGetNumPreferredPenetrationDirections(int shapeType, void* shape)
|
||||
{
|
||||
switch (shapeType)
|
||||
{
|
||||
case TRIANGLE_SHAPE_PROXYTYPE:
|
||||
{
|
||||
return 2;
|
||||
//spu_printf("2\n");
|
||||
break;
|
||||
}
|
||||
default:
|
||||
{
|
||||
#if __ASSERT
|
||||
spu_printf("spuGetNumPreferredPenetrationDirections() - Unsupported bound type: %d.\n", shapeType);
|
||||
#endif // __ASSERT
|
||||
}
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
void spuGetPreferredPenetrationDirection(int shapeType, void* shape, int index, btVector3& penetrationVector)
|
||||
{
|
||||
|
||||
|
||||
switch (shapeType)
|
||||
{
|
||||
case TRIANGLE_SHAPE_PROXYTYPE:
|
||||
{
|
||||
btVector3* vertices = (btVector3*)shape;
|
||||
///calcNormal
|
||||
penetrationVector = (vertices[1]-vertices[0]).cross(vertices[2]-vertices[0]);
|
||||
penetrationVector.normalize();
|
||||
if (index)
|
||||
penetrationVector *= btScalar(-1.);
|
||||
break;
|
||||
}
|
||||
default:
|
||||
{
|
||||
|
||||
#if __ASSERT
|
||||
spu_printf("spuGetNumPreferredPenetrationDirections() - Unsupported bound type: %d.\n", shapeType);
|
||||
#endif // __ASSERT
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
#endif //_SPU_PREFERRED_PENETRATION_DIRECTIONS_H
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -1,156 +1,156 @@
|
||||
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
|
||||
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.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
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.
|
||||
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.
|
||||
*/
|
||||
|
||||
|
||||
|
||||
#ifndef SPUVoronoiSimplexSolver_H
|
||||
#define SPUVoronoiSimplexSolver_H
|
||||
|
||||
#include <LinearMath/btTransform.h>
|
||||
#include <LinearMath/btPoint3.h>
|
||||
|
||||
#define VORONOI_SIMPLEX_MAX_VERTS 5
|
||||
|
||||
struct SpuUsageBitfield{
|
||||
SpuUsageBitfield()
|
||||
{
|
||||
reset();
|
||||
}
|
||||
|
||||
void reset()
|
||||
{
|
||||
usedVertexA = false;
|
||||
usedVertexB = false;
|
||||
usedVertexC = false;
|
||||
usedVertexD = false;
|
||||
}
|
||||
unsigned short usedVertexA : 1;
|
||||
unsigned short usedVertexB : 1;
|
||||
unsigned short usedVertexC : 1;
|
||||
unsigned short usedVertexD : 1;
|
||||
unsigned short unused1 : 1;
|
||||
unsigned short unused2 : 1;
|
||||
unsigned short unused3 : 1;
|
||||
unsigned short unused4 : 1;
|
||||
};
|
||||
|
||||
|
||||
struct SpuSubSimplexClosestResult
|
||||
{
|
||||
btVector3 m_closestPointOnSimplex;
|
||||
//MASK for m_usedVertices
|
||||
//stores the simplex vertex-usage, using the MASK,
|
||||
// if m_usedVertices & MASK then the related vertex is used
|
||||
SpuUsageBitfield m_usedVertices;
|
||||
float m_barycentricCoords[4];
|
||||
bool m_degenerate;
|
||||
|
||||
void reset()
|
||||
{
|
||||
m_degenerate = false;
|
||||
setBarycentricCoordinates();
|
||||
m_usedVertices.reset();
|
||||
}
|
||||
bool isValid()
|
||||
{
|
||||
bool valid = (m_barycentricCoords[0] >= float(0.)) &&
|
||||
(m_barycentricCoords[1] >= float(0.)) &&
|
||||
(m_barycentricCoords[2] >= float(0.)) &&
|
||||
(m_barycentricCoords[3] >= float(0.));
|
||||
|
||||
|
||||
return valid;
|
||||
}
|
||||
void setBarycentricCoordinates(float a=float(0.),float b=float(0.),float c=float(0.),float d=float(0.))
|
||||
{
|
||||
m_barycentricCoords[0] = a;
|
||||
m_barycentricCoords[1] = b;
|
||||
m_barycentricCoords[2] = c;
|
||||
m_barycentricCoords[3] = d;
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
/// 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.
|
||||
class SpuVoronoiSimplexSolver
|
||||
{
|
||||
public:
|
||||
|
||||
int m_numVertices;
|
||||
|
||||
btVector3 m_simplexVectorW[VORONOI_SIMPLEX_MAX_VERTS];
|
||||
btVector3 m_simplexPointsP[VORONOI_SIMPLEX_MAX_VERTS];
|
||||
btVector3 m_simplexPointsQ[VORONOI_SIMPLEX_MAX_VERTS];
|
||||
|
||||
int m_VertexIndexA[VORONOI_SIMPLEX_MAX_VERTS];
|
||||
int m_VertexIndexB[VORONOI_SIMPLEX_MAX_VERTS];
|
||||
|
||||
btVector3 m_cachedP1;
|
||||
btVector3 m_cachedP2;
|
||||
btVector3 m_cachedV;
|
||||
btVector3 m_lastW;
|
||||
bool m_cachedValidClosest;
|
||||
|
||||
SpuSubSimplexClosestResult m_cachedBC;
|
||||
|
||||
bool m_needsUpdate;
|
||||
|
||||
void removeVertex(int index);
|
||||
void reduceVertices (const SpuUsageBitfield& usedVerts);
|
||||
bool updateClosestVectorAndPoints();
|
||||
|
||||
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);
|
||||
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;
|
||||
|
||||
public:
|
||||
|
||||
void reset();
|
||||
|
||||
void addVertex(const btVector3& w, const btPoint3& p, const btPoint3& q);
|
||||
|
||||
|
||||
bool closest(btVector3& v);
|
||||
|
||||
btScalar maxVertex();
|
||||
|
||||
bool fullSimplex() const
|
||||
{
|
||||
return (m_numVertices == 4);
|
||||
}
|
||||
|
||||
int getSimplex(btVector3 *pBuf, btVector3 *qBuf, btVector3 *yBuf) const;
|
||||
|
||||
bool inSimplex(const btVector3& w);
|
||||
|
||||
void backup_closest(btVector3& v) ;
|
||||
|
||||
bool emptySimplex() const ;
|
||||
|
||||
void compute_points(btVector3& p1, btVector3& p2) ;
|
||||
|
||||
int numVertices() const
|
||||
{
|
||||
return m_numVertices;
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
|
||||
#endif //SpuVoronoiSimplexSolver
|
||||
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
|
||||
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.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
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.
|
||||
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.
|
||||
*/
|
||||
|
||||
|
||||
|
||||
#ifndef SPUVoronoiSimplexSolver_H
|
||||
#define SPUVoronoiSimplexSolver_H
|
||||
|
||||
#include <LinearMath/btTransform.h>
|
||||
#include <LinearMath/btPoint3.h>
|
||||
|
||||
#define VORONOI_SIMPLEX_MAX_VERTS 5
|
||||
|
||||
struct SpuUsageBitfield{
|
||||
SpuUsageBitfield()
|
||||
{
|
||||
reset();
|
||||
}
|
||||
|
||||
void reset()
|
||||
{
|
||||
usedVertexA = false;
|
||||
usedVertexB = false;
|
||||
usedVertexC = false;
|
||||
usedVertexD = false;
|
||||
}
|
||||
unsigned short usedVertexA : 1;
|
||||
unsigned short usedVertexB : 1;
|
||||
unsigned short usedVertexC : 1;
|
||||
unsigned short usedVertexD : 1;
|
||||
unsigned short unused1 : 1;
|
||||
unsigned short unused2 : 1;
|
||||
unsigned short unused3 : 1;
|
||||
unsigned short unused4 : 1;
|
||||
};
|
||||
|
||||
|
||||
struct SpuSubSimplexClosestResult
|
||||
{
|
||||
btVector3 m_closestPointOnSimplex;
|
||||
//MASK for m_usedVertices
|
||||
//stores the simplex vertex-usage, using the MASK,
|
||||
// if m_usedVertices & MASK then the related vertex is used
|
||||
SpuUsageBitfield m_usedVertices;
|
||||
float m_barycentricCoords[4];
|
||||
bool m_degenerate;
|
||||
|
||||
void reset()
|
||||
{
|
||||
m_degenerate = false;
|
||||
setBarycentricCoordinates();
|
||||
m_usedVertices.reset();
|
||||
}
|
||||
bool isValid()
|
||||
{
|
||||
bool valid = (m_barycentricCoords[0] >= float(0.)) &&
|
||||
(m_barycentricCoords[1] >= float(0.)) &&
|
||||
(m_barycentricCoords[2] >= float(0.)) &&
|
||||
(m_barycentricCoords[3] >= float(0.));
|
||||
|
||||
|
||||
return valid;
|
||||
}
|
||||
void setBarycentricCoordinates(float a=float(0.),float b=float(0.),float c=float(0.),float d=float(0.))
|
||||
{
|
||||
m_barycentricCoords[0] = a;
|
||||
m_barycentricCoords[1] = b;
|
||||
m_barycentricCoords[2] = c;
|
||||
m_barycentricCoords[3] = d;
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
/// 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.
|
||||
class SpuVoronoiSimplexSolver
|
||||
{
|
||||
public:
|
||||
|
||||
int m_numVertices;
|
||||
|
||||
btVector3 m_simplexVectorW[VORONOI_SIMPLEX_MAX_VERTS];
|
||||
btVector3 m_simplexPointsP[VORONOI_SIMPLEX_MAX_VERTS];
|
||||
btVector3 m_simplexPointsQ[VORONOI_SIMPLEX_MAX_VERTS];
|
||||
|
||||
int m_VertexIndexA[VORONOI_SIMPLEX_MAX_VERTS];
|
||||
int m_VertexIndexB[VORONOI_SIMPLEX_MAX_VERTS];
|
||||
|
||||
btVector3 m_cachedP1;
|
||||
btVector3 m_cachedP2;
|
||||
btVector3 m_cachedV;
|
||||
btVector3 m_lastW;
|
||||
bool m_cachedValidClosest;
|
||||
|
||||
SpuSubSimplexClosestResult m_cachedBC;
|
||||
|
||||
bool m_needsUpdate;
|
||||
|
||||
void removeVertex(int index);
|
||||
void reduceVertices (const SpuUsageBitfield& usedVerts);
|
||||
bool updateClosestVectorAndPoints();
|
||||
|
||||
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);
|
||||
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;
|
||||
|
||||
public:
|
||||
|
||||
void reset();
|
||||
|
||||
void addVertex(const btVector3& w, const btPoint3& p, const btPoint3& q);
|
||||
|
||||
|
||||
bool closest(btVector3& v);
|
||||
|
||||
btScalar maxVertex();
|
||||
|
||||
bool fullSimplex() const
|
||||
{
|
||||
return (m_numVertices == 4);
|
||||
}
|
||||
|
||||
int getSimplex(btVector3 *pBuf, btVector3 *qBuf, btVector3 *yBuf) const;
|
||||
|
||||
bool inSimplex(const btVector3& w);
|
||||
|
||||
void backup_closest(btVector3& v) ;
|
||||
|
||||
bool emptySimplex() const ;
|
||||
|
||||
void compute_points(btVector3& p1, btVector3& p2) ;
|
||||
|
||||
int numVertices() const
|
||||
{
|
||||
return m_numVertices;
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
|
||||
#endif //SpuVoronoiSimplexSolver
|
||||
|
||||
@@ -1,196 +1,205 @@
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
|
||||
|
||||
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.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
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.
|
||||
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.
|
||||
*/
|
||||
|
||||
//#define __CELLOS_LV2__ 1
|
||||
|
||||
#define USE_SAMPLE_PROCESS 1
|
||||
#ifdef USE_SAMPLE_PROCESS
|
||||
|
||||
|
||||
#include "btThreadSupportInterface.h"
|
||||
|
||||
//#include "SPUAssert.h"
|
||||
#include <string.h>
|
||||
|
||||
|
||||
#include "SpuSampleTaskProcess.h"
|
||||
|
||||
|
||||
#include <stdio.h>
|
||||
|
||||
|
||||
void SampleThreadFunc(void* userPtr,void* lsMemory)
|
||||
{
|
||||
//do nothing
|
||||
printf("hello world\n");
|
||||
}
|
||||
|
||||
void* SamplelsMemoryFunc()
|
||||
{
|
||||
//don't create local store memory, just return 0
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
|
||||
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);
|
||||
m_spuSampleTaskDesc.resize(m_maxNumOutstandingTasks);
|
||||
|
||||
for (int i = 0; i < m_maxNumOutstandingTasks; i++)
|
||||
{
|
||||
m_taskBusy[i] = false;
|
||||
}
|
||||
m_numBusyTasks = 0;
|
||||
m_currentTask = 0;
|
||||
|
||||
m_initialized = false;
|
||||
|
||||
m_threadInterface->startSPU();
|
||||
|
||||
|
||||
}
|
||||
|
||||
SpuSampleTaskProcess::~SpuSampleTaskProcess()
|
||||
{
|
||||
m_threadInterface->stopSPU();
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
void SpuSampleTaskProcess::initialize()
|
||||
{
|
||||
#ifdef DEBUG_SPU_TASK_SCHEDULING
|
||||
printf("SpuSampleTaskProcess::initialize()\n");
|
||||
#endif //DEBUG_SPU_TASK_SCHEDULING
|
||||
|
||||
for (int i = 0; i < m_maxNumOutstandingTasks; i++)
|
||||
{
|
||||
m_taskBusy[i] = false;
|
||||
}
|
||||
m_numBusyTasks = 0;
|
||||
m_currentTask = 0;
|
||||
m_initialized = true;
|
||||
|
||||
}
|
||||
|
||||
|
||||
void SpuSampleTaskProcess::issueTask(void* sampleMainMemPtr,int sampleValue)
|
||||
{
|
||||
|
||||
#ifdef DEBUG_SPU_TASK_SCHEDULING
|
||||
printf("SpuSampleTaskProcess::issueTask (m_currentTask= %d\)n", m_currentTask);
|
||||
#endif //DEBUG_SPU_TASK_SCHEDULING
|
||||
|
||||
m_taskBusy[m_currentTask] = true;
|
||||
m_numBusyTasks++;
|
||||
|
||||
SpuSampleTaskDesc& taskDesc = m_spuSampleTaskDesc[m_currentTask];
|
||||
{
|
||||
// send task description in event message
|
||||
// no error checking here...
|
||||
// but, currently, event queue can be no larger than NUM_WORKUNIT_TASKS.
|
||||
|
||||
taskDesc.m_mainMemoryPtr = reinterpret_cast<uint64_t>(sampleMainMemPtr);
|
||||
taskDesc.m_sampleValue = sampleValue;
|
||||
|
||||
//some bookkeeping to recognize finished tasks
|
||||
taskDesc.m_taskId = m_currentTask;
|
||||
}
|
||||
|
||||
|
||||
m_threadInterface->sendRequest(CMD_SAMPLE_TASK_COMMAND, (uint32_t) &taskDesc, m_currentTask);
|
||||
|
||||
// if all tasks busy, wait for spu event to clear the task.
|
||||
|
||||
if (m_numBusyTasks >= m_maxNumOutstandingTasks)
|
||||
{
|
||||
unsigned int taskId;
|
||||
unsigned int outputSize;
|
||||
|
||||
m_threadInterface->waitForResponse(&taskId, &outputSize);
|
||||
|
||||
//printf("PPU: after issue, received event: %u %d\n", taskId, outputSize);
|
||||
|
||||
postProcess(taskId, outputSize);
|
||||
|
||||
m_taskBusy[taskId] = false;
|
||||
|
||||
m_numBusyTasks--;
|
||||
}
|
||||
|
||||
// find new task buffer
|
||||
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)
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
|
||||
void SpuSampleTaskProcess::flush()
|
||||
{
|
||||
#ifdef DEBUG_SPU_TASK_SCHEDULING
|
||||
printf("\nSpuCollisionTaskProcess::flush()\n");
|
||||
#endif //DEBUG_SPU_TASK_SCHEDULING
|
||||
|
||||
|
||||
// all tasks are issued, wait for all tasks to be complete
|
||||
while(m_numBusyTasks > 0)
|
||||
{
|
||||
// Consolidating SPU code
|
||||
unsigned int taskId;
|
||||
unsigned int outputSize;
|
||||
|
||||
{
|
||||
|
||||
m_threadInterface->waitForResponse(&taskId, &outputSize);
|
||||
}
|
||||
|
||||
//printf("PPU: flushing, received event: %u %d\n", taskId, outputSize);
|
||||
|
||||
postProcess(taskId, outputSize);
|
||||
|
||||
m_taskBusy[taskId] = false;
|
||||
|
||||
m_numBusyTasks--;
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
#endif //USE_SAMPLE_PROCESS
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
|
||||
|
||||
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.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
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.
|
||||
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.
|
||||
*/
|
||||
|
||||
//#define __CELLOS_LV2__ 1
|
||||
|
||||
#define USE_SAMPLE_PROCESS 1
|
||||
#ifdef USE_SAMPLE_PROCESS
|
||||
|
||||
|
||||
#include "SpuSampleTaskProcess.h"
|
||||
#include <stdio.h>
|
||||
|
||||
#ifdef __SPU__
|
||||
|
||||
|
||||
|
||||
void SampleThreadFunc(void* userPtr,void* lsMemory)
|
||||
{
|
||||
//do nothing
|
||||
printf("hello world\n");
|
||||
}
|
||||
|
||||
|
||||
void* SamplelsMemoryFunc()
|
||||
{
|
||||
//don't create local store memory, just return 0
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
#else
|
||||
|
||||
|
||||
#include "btThreadSupportInterface.h"
|
||||
|
||||
//# include "SPUAssert.h"
|
||||
#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);
|
||||
m_spuSampleTaskDesc.resize(m_maxNumOutstandingTasks);
|
||||
|
||||
for (int i = 0; i < m_maxNumOutstandingTasks; i++)
|
||||
{
|
||||
m_taskBusy[i] = false;
|
||||
}
|
||||
m_numBusyTasks = 0;
|
||||
m_currentTask = 0;
|
||||
|
||||
m_initialized = false;
|
||||
|
||||
m_threadInterface->startSPU();
|
||||
|
||||
|
||||
}
|
||||
|
||||
SpuSampleTaskProcess::~SpuSampleTaskProcess()
|
||||
{
|
||||
m_threadInterface->stopSPU();
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
void SpuSampleTaskProcess::initialize()
|
||||
{
|
||||
#ifdef DEBUG_SPU_TASK_SCHEDULING
|
||||
printf("SpuSampleTaskProcess::initialize()\n");
|
||||
#endif //DEBUG_SPU_TASK_SCHEDULING
|
||||
|
||||
for (int i = 0; i < m_maxNumOutstandingTasks; i++)
|
||||
{
|
||||
m_taskBusy[i] = false;
|
||||
}
|
||||
m_numBusyTasks = 0;
|
||||
m_currentTask = 0;
|
||||
m_initialized = true;
|
||||
|
||||
}
|
||||
|
||||
|
||||
void SpuSampleTaskProcess::issueTask(void* sampleMainMemPtr,int sampleValue)
|
||||
{
|
||||
|
||||
#ifdef DEBUG_SPU_TASK_SCHEDULING
|
||||
printf("SpuSampleTaskProcess::issueTask (m_currentTask= %d\)n", m_currentTask);
|
||||
#endif //DEBUG_SPU_TASK_SCHEDULING
|
||||
|
||||
m_taskBusy[m_currentTask] = true;
|
||||
m_numBusyTasks++;
|
||||
|
||||
SpuSampleTaskDesc& taskDesc = m_spuSampleTaskDesc[m_currentTask];
|
||||
{
|
||||
// send task description in event message
|
||||
// no error checking here...
|
||||
// but, currently, event queue can be no larger than NUM_WORKUNIT_TASKS.
|
||||
|
||||
taskDesc.m_mainMemoryPtr = reinterpret_cast<uint64_t>(sampleMainMemPtr);
|
||||
taskDesc.m_sampleValue = sampleValue;
|
||||
|
||||
//some bookkeeping to recognize finished tasks
|
||||
taskDesc.m_taskId = m_currentTask;
|
||||
}
|
||||
|
||||
|
||||
m_threadInterface->sendRequest(CMD_SAMPLE_TASK_COMMAND, (uint32_t) &taskDesc, m_currentTask);
|
||||
|
||||
// if all tasks busy, wait for spu event to clear the task.
|
||||
|
||||
if (m_numBusyTasks >= m_maxNumOutstandingTasks)
|
||||
{
|
||||
unsigned int taskId;
|
||||
unsigned int outputSize;
|
||||
|
||||
m_threadInterface->waitForResponse(&taskId, &outputSize);
|
||||
|
||||
//printf("PPU: after issue, received event: %u %d\n", taskId, outputSize);
|
||||
|
||||
postProcess(taskId, outputSize);
|
||||
|
||||
m_taskBusy[taskId] = false;
|
||||
|
||||
m_numBusyTasks--;
|
||||
}
|
||||
|
||||
// find new task buffer
|
||||
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)
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
|
||||
void SpuSampleTaskProcess::flush()
|
||||
{
|
||||
#ifdef DEBUG_SPU_TASK_SCHEDULING
|
||||
printf("\nSpuCollisionTaskProcess::flush()\n");
|
||||
#endif //DEBUG_SPU_TASK_SCHEDULING
|
||||
|
||||
|
||||
// all tasks are issued, wait for all tasks to be complete
|
||||
while(m_numBusyTasks > 0)
|
||||
{
|
||||
// Consolidating SPU code
|
||||
unsigned int taskId;
|
||||
unsigned int outputSize;
|
||||
|
||||
{
|
||||
|
||||
m_threadInterface->waitForResponse(&taskId, &outputSize);
|
||||
}
|
||||
|
||||
//printf("PPU: flushing, received event: %u %d\n", taskId, outputSize);
|
||||
|
||||
postProcess(taskId, outputSize);
|
||||
|
||||
m_taskBusy[taskId] = false;
|
||||
|
||||
m_numBusyTasks--;
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
|
||||
#endif //USE_SAMPLE_PROCESS
|
||||
|
||||
@@ -1,87 +1,159 @@
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
|
||||
|
||||
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.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
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.
|
||||
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.
|
||||
*/
|
||||
|
||||
#ifndef SPU_SAMPLE_TASK_PROCESS_H
|
||||
#define SPU_SAMPLE_TASK_PROCESS_H
|
||||
|
||||
#include <assert.h>
|
||||
|
||||
|
||||
#include "PlatformDefinitions.h"
|
||||
|
||||
#include <stdlib.h>
|
||||
|
||||
#include "LinearMath/btAlignedObjectArray.h"
|
||||
|
||||
|
||||
///SpuSampleTaskDesc
|
||||
struct SpuSampleTaskDesc
|
||||
{
|
||||
uint64_t m_mainMemoryPtr;
|
||||
int m_sampleValue;
|
||||
|
||||
uint16_t m_taskId;
|
||||
}
|
||||
#ifdef __CELLOS_LV2__
|
||||
__attribute__ ((aligned (16)))
|
||||
#endif
|
||||
;
|
||||
|
||||
//just add your commands here, try to keep them globally unique for debugging purposes
|
||||
#define CMD_SAMPLE_TASK_COMMAND 10
|
||||
|
||||
|
||||
|
||||
/// SpuSampleTaskProcess handles SPU processing of collision pairs.
|
||||
/// When PPU issues a task, it will look for completed task buffers
|
||||
/// PPU will do postprocessing, dependent on workunit output (not likely)
|
||||
class SpuSampleTaskProcess
|
||||
{
|
||||
// track task buffers that are being used, and total busy tasks
|
||||
btAlignedObjectArray<bool> m_taskBusy;
|
||||
btAlignedObjectArray<SpuSampleTaskDesc>m_spuSampleTaskDesc;
|
||||
|
||||
unsigned int m_numBusyTasks;
|
||||
|
||||
// the current task and the current entry to insert a new work unit
|
||||
unsigned int m_currentTask;
|
||||
|
||||
bool m_initialized;
|
||||
|
||||
void postProcess(int taskId, int outputSize);
|
||||
|
||||
class btThreadSupportInterface* m_threadInterface;
|
||||
|
||||
unsigned int m_maxNumOutstandingTasks;
|
||||
|
||||
|
||||
|
||||
public:
|
||||
SpuSampleTaskProcess(btThreadSupportInterface* threadInterface, unsigned int maxNumOutstandingTasks);
|
||||
|
||||
~SpuSampleTaskProcess();
|
||||
|
||||
///call initialize in the beginning of the frame, before addCollisionPairToTask
|
||||
void initialize();
|
||||
|
||||
void issueTask(void* sampleMainMemPtr,int sampleValue);
|
||||
|
||||
///call flush to submit potential outstanding work to SPUs and wait for all involved SPUs to be finished
|
||||
void flush();
|
||||
};
|
||||
|
||||
|
||||
#endif // SPU_SAMPLE_TASK_PROCESS_H
|
||||
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
|
||||
|
||||
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.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
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.
|
||||
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.
|
||||
*/
|
||||
|
||||
#ifndef SPU_SAMPLE_TASK_PROCESS_H
|
||||
#define SPU_SAMPLE_TASK_PROCESS_H
|
||||
|
||||
#include <assert.h>
|
||||
|
||||
|
||||
#include "PlatformDefinitions.h"
|
||||
|
||||
#include <stdlib.h>
|
||||
|
||||
#include "LinearMath/btAlignedObjectArray.h"
|
||||
|
||||
|
||||
///SpuSampleTaskDesc
|
||||
struct SpuSampleTaskDesc
|
||||
{
|
||||
uint64_t m_mainMemoryPtr;
|
||||
int m_sampleValue;
|
||||
|
||||
uint16_t m_taskId;
|
||||
}
|
||||
#if defined(__CELLOS_LV2__) || defined(USE_LIBSPE2)
|
||||
__attribute__ ((aligned (16)))
|
||||
#endif
|
||||
;
|
||||
|
||||
//just add your commands here, try to keep them globally unique for debugging purposes
|
||||
#define CMD_SAMPLE_TASK_COMMAND 10
|
||||
|
||||
|
||||
|
||||
/// SpuSampleTaskProcess handles SPU processing of collision pairs.
|
||||
/// When PPU issues a task, it will look for completed task buffers
|
||||
/// PPU will do postprocessing, dependent on workunit output (not likely)
|
||||
class SpuSampleTaskProcess
|
||||
{
|
||||
// track task buffers that are being used, and total busy tasks
|
||||
btAlignedObjectArray<bool> m_taskBusy;
|
||||
btAlignedObjectArray<SpuSampleTaskDesc>m_spuSampleTaskDesc;
|
||||
|
||||
unsigned int m_numBusyTasks;
|
||||
|
||||
// the current task and the current entry to insert a new work unit
|
||||
unsigned int m_currentTask;
|
||||
|
||||
bool m_initialized;
|
||||
|
||||
void postProcess(int taskId, int outputSize);
|
||||
|
||||
class btThreadSupportInterface* m_threadInterface;
|
||||
|
||||
unsigned int m_maxNumOutstandingTasks;
|
||||
|
||||
|
||||
|
||||
public:
|
||||
SpuSampleTaskProcess(btThreadSupportInterface* threadInterface, unsigned int maxNumOutstandingTasks);
|
||||
|
||||
~SpuSampleTaskProcess();
|
||||
|
||||
///call initialize in the beginning of the frame, before addCollisionPairToTask
|
||||
void initialize();
|
||||
|
||||
void issueTask(void* sampleMainMemPtr,int sampleValue);
|
||||
|
||||
///call flush to submit potential outstanding work to SPUs and wait for all involved SPUs to be finished
|
||||
void flush();
|
||||
};
|
||||
|
||||
|
||||
#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
|
||||
|
||||
|
||||
@@ -25,16 +25,21 @@ Written by: Marten Svanfeldt
|
||||
#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
|
||||
#include "../SpuSync.h"
|
||||
#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
|
||||
#include "LinearMath/btAlignedAllocator.h"
|
||||
|
||||
|
||||
ATTRIBUTE_ALIGNED16(struct) ManifoldCellHolder
|
||||
{
|
||||
BT_DECLARE_ALIGNED_ALLOCATOR();
|
||||
|
||||
uint32_t m_hashCellIndex;
|
||||
class btPersistentManifold* m_manifold;
|
||||
};
|
||||
|
||||
ATTRIBUTE_ALIGNED16(struct) ConstraintCellHolder
|
||||
{
|
||||
BT_DECLARE_ALIGNED_ALLOCATOR();
|
||||
|
||||
uint32_t m_hashCellIndex;
|
||||
uint32_t m_constraintType;
|
||||
class btTypedConstraint* m_constraint;
|
||||
@@ -103,6 +108,8 @@ inline unsigned int spuGetHashCellIndex(int x, int y, int z)
|
||||
|
||||
ATTRIBUTE_ALIGNED16(struct) SpuSolverBody
|
||||
{
|
||||
BT_DECLARE_ALIGNED_ALLOCATOR();
|
||||
|
||||
btVector3 m_linearVelocity;
|
||||
btVector3 m_angularVelocity;
|
||||
|
||||
@@ -113,6 +120,8 @@ ATTRIBUTE_ALIGNED16(struct) SpuSolverBody
|
||||
|
||||
ATTRIBUTE_ALIGNED16(struct) SpuSolverInternalConstraint
|
||||
{
|
||||
BT_DECLARE_ALIGNED_ALLOCATOR();
|
||||
|
||||
uint32_t m_localOffsetBodyA;
|
||||
uint32_t m_localOffsetBodyB;
|
||||
|
||||
@@ -135,6 +144,8 @@ ATTRIBUTE_ALIGNED16(struct) SpuSolverInternalConstraint
|
||||
|
||||
ATTRIBUTE_ALIGNED16(struct) SpuSolverConstraint
|
||||
{
|
||||
BT_DECLARE_ALIGNED_ALLOCATOR();
|
||||
|
||||
uint16_t m_localOffsetBodyA;
|
||||
uint16_t m_localOffsetBodyB;
|
||||
|
||||
@@ -204,6 +215,8 @@ ATTRIBUTE_ALIGNED16(struct) SpuSolverConstraint
|
||||
|
||||
ATTRIBUTE_ALIGNED16(struct) SpuSolverDataDesc
|
||||
{
|
||||
BT_DECLARE_ALIGNED_ALLOCATOR();
|
||||
|
||||
SpuSolverHash* m_solverHash;
|
||||
SpuSolverBody* m_solverBodyList;
|
||||
SpuSolverInternalConstraint* m_solverInternalConstraintList;
|
||||
@@ -214,6 +227,8 @@ ATTRIBUTE_ALIGNED16(struct) SpuSolverDataDesc
|
||||
|
||||
ATTRIBUTE_ALIGNED16(struct) SpuSolverTaskDesc
|
||||
{
|
||||
BT_DECLARE_ALIGNED_ALLOCATOR();
|
||||
|
||||
uint32_t m_solverCommand;
|
||||
uint32_t m_taskId;
|
||||
SpuSolverDataDesc m_solverData;
|
||||
|
||||
Reference in New Issue
Block a user