moved Extras/Gimpact to src/BulletCollision/Gimpact
moved Extras/BulletMultiThreaded to src/BulletMultiThreaded (build systems will be updated soon)
This commit is contained in:
erwin.coumans
@@ -0,0 +1,487 @@
|
||||
/*
|
||||
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 "SpuCollisionShapes.h"
|
||||
|
||||
btPoint3 localGetSupportingVertexWithoutMargin(int shapeType, void* shape, const 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;
|
||||
const btVector3& halfExtents = convexShape->getImplicitShapeDimensions();
|
||||
|
||||
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());
|
||||
|
||||
btCapsuleShape* capsuleShape = (btCapsuleShape*)shape;
|
||||
btVector3 halfExtents = capsuleShape->getImplicitShapeDimensions();
|
||||
btScalar halfHeight = capsuleShape->getHalfHeight();
|
||||
int capsuleUpAxis = capsuleShape->getUpAxis();
|
||||
|
||||
btScalar radius = capsuleShape->getRadius();
|
||||
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,0,0);
|
||||
pos[capsuleUpAxis] = halfHeight;
|
||||
|
||||
vtx = pos +vec*(radius);
|
||||
newDot = vec.dot(vtx);
|
||||
if (newDot > maxDot)
|
||||
{
|
||||
maxDot = newDot;
|
||||
supVec = vtx;
|
||||
}
|
||||
}
|
||||
{
|
||||
btVector3 pos(0,0,0);
|
||||
pos[capsuleUpAxis] = -halfHeight;
|
||||
|
||||
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;
|
||||
points = convexVertexData->gConvexPoints;
|
||||
numPoints = convexVertexData->gNumConvexPoints;
|
||||
|
||||
// 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);
|
||||
}
|
||||
}
|
||||
|
||||
void computeAabb (btVector3& aabbMin, btVector3& aabbMax, btConvexInternalShape* convexShape, ppu_address_t convexShapePtr, int shapeType, btTransform xform)
|
||||
{
|
||||
//calculate the aabb, given the types...
|
||||
switch (shapeType)
|
||||
{
|
||||
case CYLINDER_SHAPE_PROXYTYPE:
|
||||
/* fall through */
|
||||
case BOX_SHAPE_PROXYTYPE:
|
||||
{
|
||||
float margin=convexShape->getMarginNV();
|
||||
btVector3 halfExtents = convexShape->getImplicitShapeDimensions();
|
||||
halfExtents += btVector3(margin,margin,margin);
|
||||
btTransform& t = xform;
|
||||
btMatrix3x3 abs_b = t.getBasis().absolute();
|
||||
btPoint3 center = t.getOrigin();
|
||||
btVector3 extent = btVector3(abs_b[0].dot(halfExtents),abs_b[1].dot(halfExtents),abs_b[2].dot(halfExtents));
|
||||
|
||||
aabbMin = center - extent;
|
||||
aabbMax = center + extent;
|
||||
break;
|
||||
}
|
||||
case CAPSULE_SHAPE_PROXYTYPE:
|
||||
{
|
||||
float margin=convexShape->getMarginNV();
|
||||
btVector3 halfExtents = convexShape->getImplicitShapeDimensions();
|
||||
//add the radius to y-axis to get full height
|
||||
btScalar radius = halfExtents[0];
|
||||
halfExtents[1] += radius;
|
||||
halfExtents += btVector3(margin,margin,margin);
|
||||
#if 0
|
||||
int capsuleUpAxis = convexShape->getUpAxis();
|
||||
btScalar halfHeight = convexShape->getHalfHeight();
|
||||
btScalar radius = convexShape->getRadius();
|
||||
halfExtents[capsuleUpAxis] = radius + halfHeight;
|
||||
#endif
|
||||
btTransform& t = xform;
|
||||
btMatrix3x3 abs_b = t.getBasis().absolute();
|
||||
btPoint3 center = t.getOrigin();
|
||||
btVector3 extent = btVector3(abs_b[0].dot(halfExtents),abs_b[1].dot(halfExtents),abs_b[2].dot(halfExtents));
|
||||
|
||||
aabbMin = center - extent;
|
||||
aabbMax = center + extent;
|
||||
break;
|
||||
}
|
||||
case SPHERE_SHAPE_PROXYTYPE:
|
||||
{
|
||||
float radius = convexShape->getImplicitShapeDimensions().getX();// * convexShape->getLocalScaling().getX();
|
||||
float margin = radius + convexShape->getMarginNV();
|
||||
btTransform& t = xform;
|
||||
const btVector3& center = t.getOrigin();
|
||||
btVector3 extent(margin,margin,margin);
|
||||
aabbMin = center - extent;
|
||||
aabbMax = center + extent;
|
||||
break;
|
||||
}
|
||||
case CONVEX_HULL_SHAPE_PROXYTYPE:
|
||||
{
|
||||
ATTRIBUTE_ALIGNED16(char convexHullShape0[sizeof(btConvexHullShape)]);
|
||||
cellDmaGet(&convexHullShape0, convexShapePtr , sizeof(btConvexHullShape), DMA_TAG(1), 0, 0);
|
||||
cellDmaWaitTagStatusAll(DMA_MASK(1));
|
||||
btConvexHullShape* localPtr = (btConvexHullShape*)&convexHullShape0;
|
||||
btTransform& t = xform;
|
||||
btScalar margin = convexShape->getMarginNV();
|
||||
localPtr->getNonvirtualAabb(t,aabbMin,aabbMax,margin);
|
||||
//spu_printf("SPU convex aabbMin=%f,%f,%f=\n",aabbMin.getX(),aabbMin.getY(),aabbMin.getZ());
|
||||
//spu_printf("SPU convex aabbMax=%f,%f,%f=\n",aabbMax.getX(),aabbMax.getY(),aabbMax.getZ());
|
||||
break;
|
||||
}
|
||||
default:
|
||||
{
|
||||
// spu_printf("SPU: unsupported shapetype %d in AABB calculation\n");
|
||||
}
|
||||
};
|
||||
}
|
||||
|
||||
void dmaBvhShapeData (bvhMeshShape_LocalStoreMemory* bvhMeshShape, btBvhTriangleMeshShape* triMeshShape)
|
||||
{
|
||||
register int dmaSize;
|
||||
register ppu_address_t dmaPpuAddress2;
|
||||
|
||||
dmaSize = sizeof(btTriangleIndexVertexArray);
|
||||
dmaPpuAddress2 = reinterpret_cast<ppu_address_t>(triMeshShape->getMeshInterface());
|
||||
// spu_printf("trimeshShape->getMeshInterface() == %llx\n",dmaPpuAddress2);
|
||||
#ifdef __SPU__
|
||||
cellDmaGet(&bvhMeshShape->gTriangleMeshInterfaceStorage, dmaPpuAddress2 , dmaSize, DMA_TAG(1), 0, 0);
|
||||
bvhMeshShape->gTriangleMeshInterfacePtr = &bvhMeshShape->gTriangleMeshInterfaceStorage;
|
||||
#else
|
||||
bvhMeshShape->gTriangleMeshInterfacePtr = (btTriangleIndexVertexArray*)cellDmaGetReadOnly(&bvhMeshShape->gTriangleMeshInterfaceStorage, dmaPpuAddress2 , dmaSize, DMA_TAG(1), 0, 0);
|
||||
#endif
|
||||
|
||||
//cellDmaWaitTagStatusAll(DMA_MASK(1));
|
||||
|
||||
///now DMA over the BVH
|
||||
|
||||
dmaSize = sizeof(btOptimizedBvh);
|
||||
dmaPpuAddress2 = reinterpret_cast<ppu_address_t>(triMeshShape->getOptimizedBvh());
|
||||
//spu_printf("trimeshShape->getOptimizedBvh() == %llx\n",dmaPpuAddress2);
|
||||
cellDmaGet(&bvhMeshShape->gOptimizedBvh, dmaPpuAddress2 , dmaSize, DMA_TAG(2), 0, 0);
|
||||
//cellDmaWaitTagStatusAll(DMA_MASK(2));
|
||||
cellDmaWaitTagStatusAll(DMA_MASK(1) | DMA_MASK(2));
|
||||
}
|
||||
|
||||
void dmaBvhIndexedMesh (btIndexedMesh* IndexMesh, IndexedMeshArray& indexArray, int index, uint32_t dmaTag)
|
||||
{
|
||||
cellDmaGet(IndexMesh, (ppu_address_t)&indexArray[index] , sizeof(btIndexedMesh), DMA_TAG(dmaTag), 0, 0);
|
||||
|
||||
}
|
||||
|
||||
void dmaBvhSubTreeHeaders (btBvhSubtreeInfo* subTreeHeaders, ppu_address_t subTreePtr, int batchSize, uint32_t dmaTag)
|
||||
{
|
||||
cellDmaGet(subTreeHeaders, subTreePtr, batchSize * sizeof(btBvhSubtreeInfo), DMA_TAG(dmaTag), 0, 0);
|
||||
}
|
||||
|
||||
void dmaBvhSubTreeNodes (btQuantizedBvhNode* nodes, const btBvhSubtreeInfo& subtree, QuantizedNodeArray& nodeArray, int dmaTag)
|
||||
{
|
||||
cellDmaGet(nodes, reinterpret_cast<ppu_address_t>(&nodeArray[subtree.m_rootNodeIndex]) , subtree.m_subtreeSize* sizeof(btQuantizedBvhNode), DMA_TAG(2), 0, 0);
|
||||
}
|
||||
|
||||
///getShapeTypeSize could easily be optimized, but it is not likely a bottleneck
|
||||
int getShapeTypeSize(int shapeType)
|
||||
{
|
||||
|
||||
|
||||
switch (shapeType)
|
||||
{
|
||||
case CYLINDER_SHAPE_PROXYTYPE:
|
||||
{
|
||||
int shapeSize = sizeof(btCylinderShape);
|
||||
btAssert(shapeSize < MAX_SHAPE_SIZE);
|
||||
return shapeSize;
|
||||
}
|
||||
case BOX_SHAPE_PROXYTYPE:
|
||||
{
|
||||
int shapeSize = sizeof(btBoxShape);
|
||||
btAssert(shapeSize < MAX_SHAPE_SIZE);
|
||||
return shapeSize;
|
||||
}
|
||||
case SPHERE_SHAPE_PROXYTYPE:
|
||||
{
|
||||
int shapeSize = sizeof(btSphereShape);
|
||||
btAssert(shapeSize < MAX_SHAPE_SIZE);
|
||||
return shapeSize;
|
||||
}
|
||||
case TRIANGLE_MESH_SHAPE_PROXYTYPE:
|
||||
{
|
||||
int shapeSize = sizeof(btBvhTriangleMeshShape);
|
||||
btAssert(shapeSize < MAX_SHAPE_SIZE);
|
||||
return shapeSize;
|
||||
}
|
||||
case CAPSULE_SHAPE_PROXYTYPE:
|
||||
{
|
||||
int shapeSize = sizeof(btCapsuleShape);
|
||||
btAssert(shapeSize < MAX_SHAPE_SIZE);
|
||||
return shapeSize;
|
||||
}
|
||||
|
||||
case CONVEX_HULL_SHAPE_PROXYTYPE:
|
||||
{
|
||||
int shapeSize = sizeof(btConvexHullShape);
|
||||
btAssert(shapeSize < MAX_SHAPE_SIZE);
|
||||
return shapeSize;
|
||||
}
|
||||
|
||||
case COMPOUND_SHAPE_PROXYTYPE:
|
||||
{
|
||||
int shapeSize = sizeof(btCompoundShape);
|
||||
btAssert(shapeSize < MAX_SHAPE_SIZE);
|
||||
return shapeSize;
|
||||
}
|
||||
|
||||
default:
|
||||
btAssert(0);
|
||||
//unsupported shapetype, please add here
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
|
||||
void dmaConvexVertexData (SpuConvexPolyhedronVertexData* convexVertexData, btConvexHullShape* convexShapeSPU)
|
||||
{
|
||||
convexVertexData->gNumConvexPoints = convexShapeSPU->getNumPoints();
|
||||
if (convexVertexData->gNumConvexPoints>MAX_NUM_SPU_CONVEX_POINTS)
|
||||
{
|
||||
btAssert(0);
|
||||
// spu_printf("SPU: Error: MAX_NUM_SPU_CONVEX_POINTS(%d) exceeded: %d\n",MAX_NUM_SPU_CONVEX_POINTS,convexVertexData->gNumConvexPoints);
|
||||
return;
|
||||
}
|
||||
|
||||
register int dmaSize = convexVertexData->gNumConvexPoints*sizeof(btPoint3);
|
||||
ppu_address_t pointsPPU = (ppu_address_t) convexShapeSPU->getPoints();
|
||||
cellDmaGet(&convexVertexData->g_convexPointBuffer[0], pointsPPU , dmaSize, DMA_TAG(2), 0, 0);
|
||||
}
|
||||
|
||||
void dmaCollisionShape (void* collisionShapeLocation, ppu_address_t collisionShapePtr, uint32_t dmaTag, int shapeType)
|
||||
{
|
||||
register int dmaSize = getShapeTypeSize(shapeType);
|
||||
cellDmaGet(collisionShapeLocation, collisionShapePtr , dmaSize, DMA_TAG(dmaTag), 0, 0);
|
||||
//cellDmaWaitTagStatusAll(DMA_MASK(dmaTag));
|
||||
}
|
||||
|
||||
void dmaCompoundShapeInfo (CompoundShape_LocalStoreMemory* compoundShapeLocation, btCompoundShape* spuCompoundShape, uint32_t dmaTag)
|
||||
{
|
||||
register int dmaSize;
|
||||
register ppu_address_t dmaPpuAddress2;
|
||||
int childShapeCount = spuCompoundShape->getNumChildShapes();
|
||||
dmaSize = childShapeCount * sizeof(btCompoundShapeChild);
|
||||
dmaPpuAddress2 = (ppu_address_t)spuCompoundShape->getChildList();
|
||||
cellDmaGet(&compoundShapeLocation->gSubshapes[0], dmaPpuAddress2, dmaSize, DMA_TAG(dmaTag), 0, 0);
|
||||
}
|
||||
|
||||
void dmaCompoundSubShapes (CompoundShape_LocalStoreMemory* compoundShapeLocation, btCompoundShape* spuCompoundShape, uint32_t dmaTag)
|
||||
{
|
||||
int childShapeCount = spuCompoundShape->getNumChildShapes();
|
||||
int i;
|
||||
// DMA all the subshapes
|
||||
for ( i = 0; i < childShapeCount; ++i)
|
||||
{
|
||||
btCompoundShapeChild& childShape = compoundShapeLocation->gSubshapes[i];
|
||||
dmaCollisionShape (&compoundShapeLocation->gSubshapeShape[i],(ppu_address_t)childShape.m_childShape, dmaTag, childShape.m_childShapeType);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void spuWalkStacklessQuantizedTree(btNodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax,const btQuantizedBvhNode* rootNode,int startNodeIndex,int endNodeIndex)
|
||||
{
|
||||
|
||||
int curIndex = startNodeIndex;
|
||||
int walkIterations = 0;
|
||||
int subTreeSize = endNodeIndex - startNodeIndex;
|
||||
|
||||
int escapeIndex;
|
||||
|
||||
unsigned int aabbOverlap, isLeafNode;
|
||||
|
||||
while (curIndex < endNodeIndex)
|
||||
{
|
||||
//catch bugs in tree data
|
||||
assert (walkIterations < subTreeSize);
|
||||
|
||||
walkIterations++;
|
||||
aabbOverlap = spuTestQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin,quantizedQueryAabbMax,rootNode->m_quantizedAabbMin,rootNode->m_quantizedAabbMax);
|
||||
isLeafNode = rootNode->isLeafNode();
|
||||
|
||||
if (isLeafNode && aabbOverlap)
|
||||
{
|
||||
//printf("overlap with node %d\n",rootNode->getTriangleIndex());
|
||||
nodeCallback->processNode(0,rootNode->getTriangleIndex());
|
||||
// spu_printf("SPU: overlap detected with triangleIndex:%d\n",rootNode->getTriangleIndex());
|
||||
}
|
||||
|
||||
if (aabbOverlap || isLeafNode)
|
||||
{
|
||||
rootNode++;
|
||||
curIndex++;
|
||||
} else
|
||||
{
|
||||
escapeIndex = rootNode->getEscapeIndex();
|
||||
rootNode += escapeIndex;
|
||||
curIndex += escapeIndex;
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
@@ -0,0 +1,102 @@
|
||||
#ifndef __SPU_COLLISION_SHAPES_H
|
||||
#define __SPU_COLLISION_SHAPES_H
|
||||
|
||||
#include "../SpuDoubleBuffer.h"
|
||||
|
||||
#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
|
||||
#include "BulletCollision/CollisionShapes/btConvexInternalShape.h"
|
||||
#include "BulletCollision/CollisionShapes/btCylinderShape.h"
|
||||
|
||||
#include "BulletCollision/CollisionShapes/btOptimizedBvh.h"
|
||||
#include "BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h"
|
||||
#include "BulletCollision/CollisionShapes/btSphereShape.h"
|
||||
|
||||
#include "BulletCollision/CollisionShapes/btCapsuleShape.h"
|
||||
|
||||
#include "BulletCollision/CollisionShapes/btConvexShape.h"
|
||||
#include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h"
|
||||
#include "BulletCollision/CollisionShapes/btConvexHullShape.h"
|
||||
#include "BulletCollision/CollisionShapes/btCompoundShape.h"
|
||||
|
||||
#define MAX_NUM_SPU_CONVEX_POINTS 128
|
||||
|
||||
struct SpuConvexPolyhedronVertexData
|
||||
{
|
||||
void* gSpuConvexShapePtr;
|
||||
btPoint3* gConvexPoints;
|
||||
int gNumConvexPoints;
|
||||
ATTRIBUTE_ALIGNED16(btPoint3 g_convexPointBuffer[MAX_NUM_SPU_CONVEX_POINTS]);
|
||||
};
|
||||
|
||||
#define MAX_SHAPE_SIZE 256
|
||||
|
||||
struct CollisionShape_LocalStoreMemory
|
||||
{
|
||||
ATTRIBUTE_ALIGNED16(char collisionShape[MAX_SHAPE_SIZE]);
|
||||
};
|
||||
|
||||
struct CompoundShape_LocalStoreMemory
|
||||
{
|
||||
// Compound data
|
||||
#define MAX_SPU_COMPOUND_SUBSHAPES 16
|
||||
ATTRIBUTE_ALIGNED16(btCompoundShapeChild gSubshapes[MAX_SPU_COMPOUND_SUBSHAPES]);
|
||||
ATTRIBUTE_ALIGNED16(char gSubshapeShape[MAX_SPU_COMPOUND_SUBSHAPES][MAX_SHAPE_SIZE]);
|
||||
};
|
||||
|
||||
struct bvhMeshShape_LocalStoreMemory
|
||||
{
|
||||
//ATTRIBUTE_ALIGNED16(btOptimizedBvh gOptimizedBvh);
|
||||
ATTRIBUTE_ALIGNED16(char gOptimizedBvh[sizeof(btOptimizedBvh)+16]);
|
||||
btOptimizedBvh* getOptimizedBvh()
|
||||
{
|
||||
return (btOptimizedBvh*) gOptimizedBvh;
|
||||
}
|
||||
|
||||
ATTRIBUTE_ALIGNED16(btTriangleIndexVertexArray gTriangleMeshInterfaceStorage);
|
||||
btTriangleIndexVertexArray* gTriangleMeshInterfacePtr;
|
||||
///only a single mesh part for now, we can add support for multiple parts, but quantized trees don't support this at the moment
|
||||
ATTRIBUTE_ALIGNED16(btIndexedMesh gIndexMesh);
|
||||
#define MAX_SPU_SUBTREE_HEADERS 32
|
||||
//1024
|
||||
ATTRIBUTE_ALIGNED16(btBvhSubtreeInfo gSubtreeHeaders[MAX_SPU_SUBTREE_HEADERS]);
|
||||
ATTRIBUTE_ALIGNED16(btQuantizedBvhNode gSubtreeNodes[MAX_SUBTREE_SIZE_IN_BYTES/sizeof(btQuantizedBvhNode)]);
|
||||
};
|
||||
|
||||
|
||||
btPoint3 localGetSupportingVertexWithoutMargin(int shapeType, void* shape, const btVector3& localDir,struct SpuConvexPolyhedronVertexData* convexVertexData);//, int *featureIndex)
|
||||
void computeAabb (btVector3& aabbMin, btVector3& aabbMax, btConvexInternalShape* convexShape, ppu_address_t convexShapePtr, int shapeType, btTransform xform);
|
||||
void dmaBvhShapeData (bvhMeshShape_LocalStoreMemory* bvhMeshShape, btBvhTriangleMeshShape* triMeshShape);
|
||||
void dmaBvhIndexedMesh (btIndexedMesh* IndexMesh, IndexedMeshArray& indexArray, int index, uint32_t dmaTag);
|
||||
void dmaBvhSubTreeHeaders (btBvhSubtreeInfo* subTreeHeaders, ppu_address_t subTreePtr, int batchSize, uint32_t dmaTag);
|
||||
void dmaBvhSubTreeNodes (btQuantizedBvhNode* nodes, const btBvhSubtreeInfo& subtree, QuantizedNodeArray& nodeArray, int dmaTag);
|
||||
|
||||
int getShapeTypeSize(int shapeType);
|
||||
void dmaConvexVertexData (SpuConvexPolyhedronVertexData* convexVertexData, btConvexHullShape* convexShapeSPU);
|
||||
void dmaCollisionShape (void* collisionShapeLocation, ppu_address_t collisionShapePtr, uint32_t dmaTag, int shapeType);
|
||||
void dmaCompoundShapeInfo (CompoundShape_LocalStoreMemory* compoundShapeLocation, btCompoundShape* spuCompoundShape, uint32_t dmaTag);
|
||||
void dmaCompoundSubShapes (CompoundShape_LocalStoreMemory* compoundShapeLocation, btCompoundShape* spuCompoundShape, uint32_t dmaTag);
|
||||
|
||||
#define USE_BRANCHFREE_TEST 1
|
||||
#ifdef USE_BRANCHFREE_TEST
|
||||
SIMD_FORCE_INLINE unsigned int spuTestQuantizedAabbAgainstQuantizedAabb(unsigned short int* aabbMin1,unsigned short int* aabbMax1,const unsigned short int* aabbMin2,const unsigned short int* aabbMax2)
|
||||
{
|
||||
return btSelect((unsigned)((aabbMin1[0] <= aabbMax2[0]) & (aabbMax1[0] >= aabbMin2[0])
|
||||
& (aabbMin1[2] <= aabbMax2[2]) & (aabbMax1[2] >= aabbMin2[2])
|
||||
& (aabbMin1[1] <= aabbMax2[1]) & (aabbMax1[1] >= aabbMin2[1])),
|
||||
1, 0);
|
||||
}
|
||||
#else
|
||||
|
||||
unsigned int spuTestQuantizedAabbAgainstQuantizedAabb(const unsigned short int* aabbMin1,const unsigned short int* aabbMax1,const unsigned short int* aabbMin2,const unsigned short int* aabbMax2)
|
||||
{
|
||||
unsigned int overlap = 1;
|
||||
overlap = (aabbMin1[0] > aabbMax2[0] || aabbMax1[0] < aabbMin2[0]) ? 0 : overlap;
|
||||
overlap = (aabbMin1[2] > aabbMax2[2] || aabbMax1[2] < aabbMin2[2]) ? 0 : overlap;
|
||||
overlap = (aabbMin1[1] > aabbMax2[1] || aabbMax1[1] < aabbMin2[1]) ? 0 : overlap;
|
||||
return overlap;
|
||||
}
|
||||
#endif
|
||||
|
||||
void spuWalkStacklessQuantizedTree(btNodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax,const btQuantizedBvhNode* rootNode,int startNodeIndex,int endNodeIndex);
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,227 @@
|
||||
/*
|
||||
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 "SpuContactResult.h"
|
||||
|
||||
//#define DEBUG_SPU_COLLISION_DETECTION 1
|
||||
|
||||
|
||||
SpuContactResult::SpuContactResult()
|
||||
{
|
||||
m_manifoldAddress = 0;
|
||||
m_spuManifold = NULL;
|
||||
m_RequiresWriteBack = false;
|
||||
}
|
||||
|
||||
SpuContactResult::~SpuContactResult()
|
||||
{
|
||||
g_manifoldDmaExport.swapBuffers();
|
||||
}
|
||||
|
||||
///User can override this material combiner by implementing gContactAddedCallback and setting body0->m_collisionFlags |= btCollisionObject::customMaterialCallback;
|
||||
inline btScalar calculateCombinedFriction(btScalar friction0,btScalar friction1)
|
||||
{
|
||||
btScalar friction = friction0*friction1;
|
||||
|
||||
const btScalar MAX_FRICTION = btScalar(10.);
|
||||
|
||||
if (friction < -MAX_FRICTION)
|
||||
friction = -MAX_FRICTION;
|
||||
if (friction > MAX_FRICTION)
|
||||
friction = MAX_FRICTION;
|
||||
return friction;
|
||||
|
||||
}
|
||||
|
||||
inline btScalar calculateCombinedRestitution(btScalar restitution0,btScalar restitution1)
|
||||
{
|
||||
return restitution0*restitution1;
|
||||
}
|
||||
|
||||
|
||||
|
||||
void SpuContactResult::setContactInfo(btPersistentManifold* spuManifold, ppu_address_t manifoldAddress,const btTransform& worldTrans0,const btTransform& worldTrans1, btScalar restitution0,btScalar restitution1, btScalar friction0,btScalar friction1, bool isSwapped)
|
||||
{
|
||||
//spu_printf("SpuContactResult::setContactInfo ManifoldAddress: %lu\n", manifoldAddress);
|
||||
m_rootWorldTransform0 = worldTrans0;
|
||||
m_rootWorldTransform1 = worldTrans1;
|
||||
m_manifoldAddress = manifoldAddress;
|
||||
m_spuManifold = spuManifold;
|
||||
|
||||
m_combinedFriction = calculateCombinedFriction(friction0,friction1);
|
||||
m_combinedRestitution = calculateCombinedRestitution(restitution0,restitution1);
|
||||
m_isSwapped = isSwapped;
|
||||
}
|
||||
|
||||
void SpuContactResult::setShapeIdentifiers(int partId0,int index0, int partId1,int index1)
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
///return true if it requires a dma transfer back
|
||||
bool ManifoldResultAddContactPoint(const btVector3& normalOnBInWorld,
|
||||
const btVector3& pointInWorld,
|
||||
float depth,
|
||||
btPersistentManifold* manifoldPtr,
|
||||
btTransform& transA,
|
||||
btTransform& transB,
|
||||
btScalar combinedFriction,
|
||||
btScalar combinedRestitution,
|
||||
bool isSwapped)
|
||||
{
|
||||
|
||||
float contactTreshold = manifoldPtr->getContactBreakingThreshold();
|
||||
|
||||
//spu_printf("SPU: add contactpoint, depth:%f, contactTreshold %f, manifoldPtr %llx\n",depth,contactTreshold,manifoldPtr);
|
||||
|
||||
#ifdef DEBUG_SPU_COLLISION_DETECTION
|
||||
spu_printf("SPU: contactTreshold %f\n",contactTreshold);
|
||||
#endif //DEBUG_SPU_COLLISION_DETECTION
|
||||
if (depth > manifoldPtr->getContactBreakingThreshold())
|
||||
return false;
|
||||
|
||||
//provide inverses or just calculate?
|
||||
btTransform transAInv = transA.inverse();//m_body0->m_cachedInvertedWorldTransform;
|
||||
btTransform transBInv= transB.inverse();//m_body1->m_cachedInvertedWorldTransform;
|
||||
|
||||
btVector3 pointA;
|
||||
btVector3 localA;
|
||||
btVector3 localB;
|
||||
btVector3 normal;
|
||||
|
||||
if (isSwapped)
|
||||
{
|
||||
normal = normalOnBInWorld * -1;
|
||||
pointA = pointInWorld + normal * depth;
|
||||
localA = transAInv(pointA );
|
||||
localB = transBInv(pointInWorld);
|
||||
/*localA = transBInv(pointA );
|
||||
localB = transAInv(pointInWorld);*/
|
||||
}
|
||||
else
|
||||
{
|
||||
normal = normalOnBInWorld;
|
||||
pointA = pointInWorld + normal * depth;
|
||||
localA = transAInv(pointA );
|
||||
localB = transBInv(pointInWorld);
|
||||
}
|
||||
|
||||
btManifoldPoint newPt(localA,localB,normal,depth);
|
||||
|
||||
int insertIndex = manifoldPtr->getCacheEntry(newPt);
|
||||
if (insertIndex >= 0)
|
||||
{
|
||||
// manifoldPtr->replaceContactPoint(newPt,insertIndex);
|
||||
// return true;
|
||||
|
||||
#ifdef DEBUG_SPU_COLLISION_DETECTION
|
||||
spu_printf("SPU: same contact detected, nothing done\n");
|
||||
#endif //DEBUG_SPU_COLLISION_DETECTION
|
||||
// This is not needed, just use the old info! saves a DMA transfer as well
|
||||
} else
|
||||
{
|
||||
|
||||
newPt.m_combinedFriction = combinedFriction;
|
||||
newPt.m_combinedRestitution = combinedRestitution;
|
||||
|
||||
/*
|
||||
//potential TODO: SPU callbacks, either immediate (local on the SPU), or deferred
|
||||
//User can override friction and/or restitution
|
||||
if (gContactAddedCallback &&
|
||||
//and if either of the two bodies requires custom material
|
||||
((m_body0->m_collisionFlags & btCollisionObject::customMaterialCallback) ||
|
||||
(m_body1->m_collisionFlags & btCollisionObject::customMaterialCallback)))
|
||||
{
|
||||
//experimental feature info, for per-triangle material etc.
|
||||
(*gContactAddedCallback)(newPt,m_body0,m_partId0,m_index0,m_body1,m_partId1,m_index1);
|
||||
}
|
||||
*/
|
||||
manifoldPtr->addManifoldPoint(newPt);
|
||||
return true;
|
||||
|
||||
}
|
||||
return false;
|
||||
|
||||
}
|
||||
|
||||
|
||||
void SpuContactResult::writeDoubleBufferedManifold(btPersistentManifold* lsManifold, btPersistentManifold* mmManifold)
|
||||
{
|
||||
memcpy(g_manifoldDmaExport.getFront(),lsManifold,sizeof(btPersistentManifold));
|
||||
|
||||
g_manifoldDmaExport.swapBuffers();
|
||||
uint64_t mmAddr = (uint32_t)mmManifold;
|
||||
g_manifoldDmaExport.backBufferDmaPut(mmAddr, sizeof(btPersistentManifold), DMA_TAG(9));
|
||||
// Should there be any kind of wait here? What if somebody tries to use this tag again? What if we call this function again really soon?
|
||||
//no, the swapBuffers does the wait
|
||||
}
|
||||
|
||||
void SpuContactResult::addContactPoint(const btVector3& normalOnBInWorld,const btPoint3& pointInWorld,float depth)
|
||||
{
|
||||
//spu_printf("*** SpuContactResult::addContactPoint: depth = %f\n",depth);
|
||||
|
||||
#ifdef DEBUG_SPU_COLLISION_DETECTION
|
||||
// int sman = sizeof(rage::phManifold);
|
||||
// spu_printf("sizeof_manifold = %i\n",sman);
|
||||
#endif //DEBUG_SPU_COLLISION_DETECTION
|
||||
|
||||
btPersistentManifold* localManifold = m_spuManifold;
|
||||
|
||||
btVector3 normalB(normalOnBInWorld.getX(),normalOnBInWorld.getY(),normalOnBInWorld.getZ());
|
||||
btVector3 pointWrld(pointInWorld.getX(),pointInWorld.getY(),pointInWorld.getZ());
|
||||
|
||||
//process the contact point
|
||||
const bool retVal = ManifoldResultAddContactPoint(normalB,
|
||||
pointWrld,
|
||||
depth,
|
||||
localManifold,
|
||||
m_rootWorldTransform0,
|
||||
m_rootWorldTransform1,
|
||||
m_combinedFriction,
|
||||
m_combinedRestitution,
|
||||
m_isSwapped);
|
||||
m_RequiresWriteBack = m_RequiresWriteBack || retVal;
|
||||
}
|
||||
|
||||
void SpuContactResult::flush()
|
||||
{
|
||||
|
||||
if (m_spuManifold && m_spuManifold->getNumContacts())
|
||||
{
|
||||
m_spuManifold->refreshContactPoints(m_rootWorldTransform0,m_rootWorldTransform1);
|
||||
m_RequiresWriteBack = true;
|
||||
}
|
||||
|
||||
|
||||
if (m_RequiresWriteBack)
|
||||
{
|
||||
#ifdef DEBUG_SPU_COLLISION_DETECTION
|
||||
spu_printf("SPU: Start SpuContactResult::flush (Put) DMA\n");
|
||||
spu_printf("Num contacts:%d\n", m_spuManifold->getNumContacts());
|
||||
spu_printf("Manifold address: %llu\n", m_manifoldAddress);
|
||||
#endif //DEBUG_SPU_COLLISION_DETECTION
|
||||
// spu_printf("writeDoubleBufferedManifold\n");
|
||||
writeDoubleBufferedManifold(m_spuManifold, (btPersistentManifold*)m_manifoldAddress);
|
||||
#ifdef DEBUG_SPU_COLLISION_DETECTION
|
||||
spu_printf("SPU: Finished (Put) DMA\n");
|
||||
#endif //DEBUG_SPU_COLLISION_DETECTION
|
||||
}
|
||||
m_spuManifold = NULL;
|
||||
m_RequiresWriteBack = false;
|
||||
}
|
||||
|
||||
|
||||
@@ -0,0 +1,113 @@
|
||||
/*
|
||||
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_CONTACT_RESULT2_H
|
||||
#define SPU_CONTACT_RESULT2_H
|
||||
|
||||
|
||||
#ifndef WIN32
|
||||
#include <stdint.h>
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
#include "../SpuDoubleBuffer.h"
|
||||
|
||||
|
||||
#include "LinearMath/btTransform.h"
|
||||
#include "LinearMath/btPoint3.h"
|
||||
|
||||
|
||||
#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
|
||||
|
||||
|
||||
struct SpuCollisionPairInput
|
||||
{
|
||||
ppu_address_t m_collisionShapes[2];
|
||||
void* m_spuCollisionShapes[2];
|
||||
|
||||
ppu_address_t m_persistentManifoldPtr;
|
||||
btVector3 m_primitiveDimensions0;
|
||||
btVector3 m_primitiveDimensions1;
|
||||
int m_shapeType0;
|
||||
int m_shapeType1;
|
||||
float m_collisionMargin0;
|
||||
float m_collisionMargin1;
|
||||
|
||||
btTransform m_worldTransform0;
|
||||
btTransform m_worldTransform1;
|
||||
|
||||
bool m_isSwapped;
|
||||
bool m_useEpa;
|
||||
};
|
||||
|
||||
|
||||
struct SpuClosestPointInput
|
||||
{
|
||||
SpuClosestPointInput()
|
||||
:m_maximumDistanceSquared(float(1e30)),
|
||||
m_stackAlloc(0)
|
||||
{
|
||||
}
|
||||
|
||||
btTransform m_transformA;
|
||||
btTransform m_transformB;
|
||||
float m_maximumDistanceSquared;
|
||||
class btStackAlloc* m_stackAlloc;
|
||||
struct SpuConvexPolyhedronVertexData* m_convexVertexData[2];
|
||||
};
|
||||
|
||||
///SpuContactResult exports the contact points using double-buffered DMA transfers, only when needed
|
||||
///So when an existing contact point is duplicated, no transfer/refresh is performed.
|
||||
class SpuContactResult
|
||||
{
|
||||
btTransform m_rootWorldTransform0;
|
||||
btTransform m_rootWorldTransform1;
|
||||
ppu_address_t m_manifoldAddress;
|
||||
|
||||
btPersistentManifold* m_spuManifold;
|
||||
bool m_RequiresWriteBack;
|
||||
btScalar m_combinedFriction;
|
||||
btScalar m_combinedRestitution;
|
||||
|
||||
bool m_isSwapped;
|
||||
|
||||
DoubleBuffer<btPersistentManifold, 1> g_manifoldDmaExport;
|
||||
|
||||
public:
|
||||
SpuContactResult();
|
||||
virtual ~SpuContactResult();
|
||||
|
||||
btPersistentManifold* GetSpuManifold() const
|
||||
{
|
||||
return m_spuManifold;
|
||||
}
|
||||
|
||||
virtual void setShapeIdentifiers(int partId0,int index0, int partId1,int index1);
|
||||
|
||||
void setContactInfo(btPersistentManifold* spuManifold, ppu_address_t manifoldAddress,const btTransform& worldTrans0,const btTransform& worldTrans1, btScalar restitution0,btScalar restitution1, btScalar friction0,btScalar friction01, bool isSwapped);
|
||||
|
||||
|
||||
void writeDoubleBufferedManifold(btPersistentManifold* lsManifold, btPersistentManifold* mmManifold);
|
||||
|
||||
virtual void addContactPoint(const btVector3& normalOnBInWorld,const btPoint3& pointInWorld,float depth);
|
||||
|
||||
void flush();
|
||||
};
|
||||
|
||||
|
||||
|
||||
#endif //SPU_CONTACT_RESULT2_H
|
||||
|
||||
@@ -0,0 +1,52 @@
|
||||
|
||||
/*
|
||||
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* convexVertexDataA,
|
||||
struct SpuConvexPolyhedronVertexData* convexVertexDataB
|
||||
) const = 0;
|
||||
|
||||
|
||||
};
|
||||
|
||||
|
||||
|
||||
#endif //CONVEX_PENETRATION_DEPTH_H
|
||||
|
||||
@@ -0,0 +1,37 @@
|
||||
/*
|
||||
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 "SpuEpaPenetrationDepthSolver.h"
|
||||
#include "SpuVoronoiSimplexSolver.h"
|
||||
#include "SpuGjkPairDetector.h"
|
||||
#include "SpuContactResult.h"
|
||||
#include "SpuGjkEpa2.h"
|
||||
|
||||
bool SpuEpaPenetrationDepthSolver::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* convexVertexDataA,
|
||||
struct SpuConvexPolyhedronVertexData* convexVertexDataB
|
||||
) const
|
||||
{
|
||||
bool r;
|
||||
SpuGjkEpaSolver2::sResults results;
|
||||
r = SpuGjkEpaSolver2::Penetration (convexA, convexVertexDataA, shapeTypeA, marginA, transA, convexB, convexVertexDataB, shapeTypeB, marginB, transB, btVector3(1.0f, 0.0f, 0.0f), results);
|
||||
pa = results.witnesses[0];
|
||||
pb = results.witnesses[1];
|
||||
return r;
|
||||
}
|
||||
@@ -0,0 +1,47 @@
|
||||
|
||||
/*
|
||||
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_EPA_PENETRATION_DEPTH_SOLVER_H
|
||||
#define SPU_EPA_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 SpuEpaPenetrationDepthSolver : 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* convexVertexDataA,
|
||||
struct SpuConvexPolyhedronVertexData* convexVertexDataB
|
||||
) const;
|
||||
|
||||
|
||||
};
|
||||
|
||||
|
||||
#endif //SPU_EPA_PENETRATION_DEPTH_SOLVER_H
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,140 @@
|
||||
/*
|
||||
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
|
||||
{
|
||||
ppu_address_t inPtr;//m_pairArrayPtr;
|
||||
//mutex variable
|
||||
uint32_t m_someMutexVariableInMainMemory;
|
||||
|
||||
ppu_address_t m_dispatcher;
|
||||
|
||||
uint32_t numOnLastPage;
|
||||
|
||||
uint16_t numPages;
|
||||
uint16_t taskId;
|
||||
bool m_useEpa;
|
||||
|
||||
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>
|
||||
|
||||
//#define DEBUG_LIBSPE2_SPU_TASK
|
||||
|
||||
|
||||
|
||||
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 ))
|
||||
{
|
||||
#ifdef DEBUG_LIBSPE2_SPU_TASK
|
||||
printf("SPU: received Spu_Mailbox_Event_Task\n");
|
||||
#endif //DEBUG_LIBSPE2_SPU_TASK
|
||||
|
||||
// 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));
|
||||
#ifdef DEBUG_LIBSPE2_SPU_TASK
|
||||
printf("SPU:processCollisionTask\n");
|
||||
#endif //DEBUG_LIBSPE2_SPU_TASK
|
||||
processCollisionTask((void*)&taskDesc, taskDesc.m_lsMemory);
|
||||
|
||||
#ifdef DEBUG_LIBSPE2_SPU_TASK
|
||||
printf("SPU:finished processCollisionTask\n");
|
||||
#endif //DEBUG_LIBSPE2_SPU_TASK
|
||||
}
|
||||
else
|
||||
{
|
||||
#ifdef DEBUG_LIBSPE2_SPU_TASK
|
||||
printf("SPU: received ShutDown\n");
|
||||
#endif //DEBUG_LIBSPE2_SPU_TASK
|
||||
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
|
||||
|
||||
|
||||
@@ -0,0 +1,850 @@
|
||||
#include "BulletCollision/CollisionShapes/btConvexInternalShape.h"
|
||||
#include "BulletCollision/CollisionShapes/btSphereShape.h"
|
||||
#include "SpuCollisionShapes.h"
|
||||
#include "SpuGjkEpa2.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
|
||||
|
||||
namespace gjkepa2_spu_impl
|
||||
{
|
||||
|
||||
// Config
|
||||
|
||||
/* GJK */
|
||||
#define GJK_MAX_ITERATIONS 128
|
||||
#define GJK_ACCURARY ((btScalar)0.0001)
|
||||
#define GJK_MIN_DISTANCE ((btScalar)0.0001)
|
||||
#define GJK_DUPLICATED_EPS ((btScalar)0.0001)
|
||||
#define GJK_SIMPLEX2_EPS ((btScalar)0.0)
|
||||
#define GJK_SIMPLEX3_EPS ((btScalar)0.0)
|
||||
#define GJK_SIMPLEX4_EPS ((btScalar)0.0)
|
||||
|
||||
/* EPA */
|
||||
#define EPA_MAX_VERTICES 64
|
||||
#define EPA_MAX_FACES (EPA_MAX_VERTICES*2)
|
||||
#define EPA_MAX_ITERATIONS 255
|
||||
#define EPA_ACCURACY ((btScalar)0.0001)
|
||||
#define EPA_FALLBACK (10*EPA_ACCURACY)
|
||||
#define EPA_PLANE_EPS ((btScalar)0.00001)
|
||||
#define EPA_INSIDE_EPS ((btScalar)0.01)
|
||||
|
||||
|
||||
// Shorthands
|
||||
typedef unsigned int U;
|
||||
typedef unsigned char U1;
|
||||
|
||||
struct convexShape
|
||||
{
|
||||
void* shape;
|
||||
SpuConvexPolyhedronVertexData* convexData;
|
||||
int shapeType;
|
||||
float margin;
|
||||
};
|
||||
|
||||
// MinkowskiDiff
|
||||
struct MinkowskiDiff
|
||||
{
|
||||
convexShape m_shapes[2];
|
||||
btMatrix3x3 m_toshape1;
|
||||
btTransform m_toshape0;
|
||||
btVector3 (btConvexShape::*Ls)(const btVector3&) const;
|
||||
void EnableMargin(bool enable)
|
||||
{
|
||||
#if 0
|
||||
if(enable)
|
||||
Ls=&btConvexShape::localGetSupportingVertex;
|
||||
else
|
||||
Ls=&btConvexShape::localGetSupportingVertexWithoutMargin;
|
||||
#endif
|
||||
}
|
||||
inline btVector3 Support0(const btVector3& d) const
|
||||
{
|
||||
btVector3 sp = localGetSupportingVertexWithoutMargin (m_shapes[0].shapeType, m_shapes[0].shape, d, m_shapes[0].convexData);
|
||||
btVector3 ud = d;
|
||||
ud.normalize();
|
||||
sp += ud * m_shapes[0].margin;
|
||||
return sp;
|
||||
// return(((m_shapes[0])->*(Ls))(d));
|
||||
}
|
||||
inline btVector3 Support1(const btVector3& d) const
|
||||
{
|
||||
btVector3 nd = m_toshape1*d;
|
||||
btVector3 ud = nd;
|
||||
ud.normalize ();
|
||||
btVector3 sp = localGetSupportingVertexWithoutMargin (m_shapes[1].shapeType, m_shapes[1].shape, nd, m_shapes[1].convexData);
|
||||
sp += ud * m_shapes[1].margin;
|
||||
return m_toshape0 * sp;
|
||||
// return(m_toshape0*((m_shapes[1])->*(Ls))(m_toshape1*d));
|
||||
}
|
||||
inline btVector3 Support(const btVector3& d) const
|
||||
{
|
||||
return(Support0(d)-Support1(-d));
|
||||
}
|
||||
btVector3 Support(const btVector3& d,U index) const
|
||||
{
|
||||
|
||||
if(index)
|
||||
return(Support1(d));
|
||||
else
|
||||
return(Support0(d));
|
||||
|
||||
}
|
||||
};
|
||||
|
||||
typedef MinkowskiDiff tShape;
|
||||
|
||||
// GJK
|
||||
struct GJK
|
||||
{
|
||||
/* Types */
|
||||
struct sSV
|
||||
{
|
||||
btVector3 d,w;
|
||||
};
|
||||
struct sSimplex
|
||||
{
|
||||
sSV* c[4];
|
||||
btScalar p[4];
|
||||
U rank;
|
||||
};
|
||||
struct eStatus { enum _ {
|
||||
Valid,
|
||||
Inside,
|
||||
Failed };};
|
||||
/* Fields */
|
||||
tShape m_shape;
|
||||
btVector3 m_ray;
|
||||
btScalar m_distance;
|
||||
sSimplex m_simplices[2];
|
||||
sSV m_store[4];
|
||||
sSV* m_free[4];
|
||||
U m_nfree;
|
||||
U m_current;
|
||||
sSimplex* m_simplex;
|
||||
eStatus::_ m_status;
|
||||
/* Methods */
|
||||
GJK()
|
||||
{
|
||||
Initialize();
|
||||
}
|
||||
void Initialize()
|
||||
{
|
||||
m_ray = btVector3(0,0,0);
|
||||
m_nfree = 0;
|
||||
m_status = eStatus::Failed;
|
||||
m_current = 0;
|
||||
m_distance = 0;
|
||||
}
|
||||
eStatus::_ Evaluate(const tShape& shapearg,const btVector3& guess)
|
||||
{
|
||||
U iterations=0;
|
||||
btScalar sqdist=0;
|
||||
btScalar alpha=0;
|
||||
btVector3 lastw[4];
|
||||
U clastw=0;
|
||||
/* Initialize solver */
|
||||
m_free[0] = &m_store[0];
|
||||
m_free[1] = &m_store[1];
|
||||
m_free[2] = &m_store[2];
|
||||
m_free[3] = &m_store[3];
|
||||
m_nfree = 4;
|
||||
m_current = 0;
|
||||
m_status = eStatus::Valid;
|
||||
m_shape = shapearg;
|
||||
m_distance = 0;
|
||||
/* Initialize simplex */
|
||||
m_simplices[0].rank = 0;
|
||||
m_ray = guess;
|
||||
const btScalar sqrl= m_ray.length2();
|
||||
appendvertice(m_simplices[0],sqrl>0?-m_ray:btVector3(1,0,0));
|
||||
m_simplices[0].p[0] = 1;
|
||||
m_ray = m_simplices[0].c[0]->w;
|
||||
sqdist = sqrl;
|
||||
lastw[0] =
|
||||
lastw[1] =
|
||||
lastw[2] =
|
||||
lastw[3] = m_ray;
|
||||
/* Loop */
|
||||
do {
|
||||
const U next=1-m_current;
|
||||
sSimplex& cs=m_simplices[m_current];
|
||||
sSimplex& ns=m_simplices[next];
|
||||
/* Check zero */
|
||||
const btScalar rl=m_ray.length();
|
||||
if(rl<GJK_MIN_DISTANCE)
|
||||
{/* Touching or inside */
|
||||
m_status=eStatus::Inside;
|
||||
break;
|
||||
}
|
||||
/* Append new vertice in -'v' direction */
|
||||
appendvertice(cs,-m_ray);
|
||||
const btVector3& w=cs.c[cs.rank-1]->w;
|
||||
bool found=false;
|
||||
for(U i=0;i<4;++i)
|
||||
{
|
||||
if((w-lastw[i]).length2()<GJK_DUPLICATED_EPS)
|
||||
{ found=true;break; }
|
||||
}
|
||||
if(found)
|
||||
{/* Return old simplex */
|
||||
removevertice(m_simplices[m_current]);
|
||||
break;
|
||||
}
|
||||
else
|
||||
{/* Update lastw */
|
||||
lastw[clastw=(clastw+1)&3]=w;
|
||||
}
|
||||
/* Check for termination */
|
||||
const btScalar omega=dot(m_ray,w)/rl;
|
||||
alpha=btMax(omega,alpha);
|
||||
if(((rl-alpha)-(GJK_ACCURARY*rl))<=0)
|
||||
{/* Return old simplex */
|
||||
removevertice(m_simplices[m_current]);
|
||||
break;
|
||||
}
|
||||
/* Reduce simplex */
|
||||
btScalar weights[4];
|
||||
U mask=0;
|
||||
switch(cs.rank)
|
||||
{
|
||||
case 2: sqdist=projectorigin( cs.c[0]->w,
|
||||
cs.c[1]->w,
|
||||
weights,mask);break;
|
||||
case 3: sqdist=projectorigin( cs.c[0]->w,
|
||||
cs.c[1]->w,
|
||||
cs.c[2]->w,
|
||||
weights,mask);break;
|
||||
case 4: sqdist=projectorigin( cs.c[0]->w,
|
||||
cs.c[1]->w,
|
||||
cs.c[2]->w,
|
||||
cs.c[3]->w,
|
||||
weights,mask);break;
|
||||
}
|
||||
if(sqdist>=0)
|
||||
{/* Valid */
|
||||
ns.rank = 0;
|
||||
m_ray = btVector3(0,0,0);
|
||||
m_current = next;
|
||||
for(U i=0,ni=cs.rank;i<ni;++i)
|
||||
{
|
||||
if(mask&(1<<i))
|
||||
{
|
||||
ns.c[ns.rank] = cs.c[i];
|
||||
ns.p[ns.rank++] = weights[i];
|
||||
m_ray += cs.c[i]->w*weights[i];
|
||||
}
|
||||
else
|
||||
{
|
||||
m_free[m_nfree++] = cs.c[i];
|
||||
}
|
||||
}
|
||||
if(mask==15) m_status=eStatus::Inside;
|
||||
}
|
||||
else
|
||||
{/* Return old simplex */
|
||||
removevertice(m_simplices[m_current]);
|
||||
break;
|
||||
}
|
||||
m_status=((++iterations)<GJK_MAX_ITERATIONS)?m_status:eStatus::Failed;
|
||||
} while(m_status==eStatus::Valid);
|
||||
m_simplex=&m_simplices[m_current];
|
||||
switch(m_status)
|
||||
{
|
||||
case eStatus::Valid: m_distance=m_ray.length();break;
|
||||
case eStatus::Inside: m_distance=0;break;
|
||||
}
|
||||
return(m_status);
|
||||
}
|
||||
bool EncloseOrigin()
|
||||
{
|
||||
switch(m_simplex->rank)
|
||||
{
|
||||
case 1:
|
||||
{
|
||||
for(U i=0;i<3;++i)
|
||||
{
|
||||
btVector3 axis=btVector3(0,0,0);
|
||||
axis[i]=1;
|
||||
appendvertice(*m_simplex, axis);
|
||||
if(EncloseOrigin()) return(true);
|
||||
removevertice(*m_simplex);
|
||||
appendvertice(*m_simplex,-axis);
|
||||
if(EncloseOrigin()) return(true);
|
||||
removevertice(*m_simplex);
|
||||
}
|
||||
}
|
||||
break;
|
||||
case 2:
|
||||
{
|
||||
const btVector3 d=m_simplex->c[1]->w-m_simplex->c[0]->w;
|
||||
for(U i=0;i<3;++i)
|
||||
{
|
||||
btVector3 axis=btVector3(0,0,0);
|
||||
axis[i]=1;
|
||||
if(btFabs(dot(axis,d))>0)
|
||||
{
|
||||
const btVector3 p=cross(d,axis);
|
||||
appendvertice(*m_simplex, p);
|
||||
if(EncloseOrigin()) return(true);
|
||||
removevertice(*m_simplex);
|
||||
appendvertice(*m_simplex,-p);
|
||||
if(EncloseOrigin()) return(true);
|
||||
removevertice(*m_simplex);
|
||||
}
|
||||
}
|
||||
}
|
||||
break;
|
||||
case 3:
|
||||
{
|
||||
const btVector3 n=cross(m_simplex->c[1]->w-m_simplex->c[0]->w,
|
||||
m_simplex->c[2]->w-m_simplex->c[0]->w);
|
||||
const btScalar l=n.length();
|
||||
if(l>0)
|
||||
{
|
||||
appendvertice(*m_simplex,n);
|
||||
if(EncloseOrigin()) return(true);
|
||||
removevertice(*m_simplex);
|
||||
appendvertice(*m_simplex,-n);
|
||||
if(EncloseOrigin()) return(true);
|
||||
removevertice(*m_simplex);
|
||||
}
|
||||
}
|
||||
break;
|
||||
case 4:
|
||||
{
|
||||
if(btFabs(det( m_simplex->c[0]->w-m_simplex->c[3]->w,
|
||||
m_simplex->c[1]->w-m_simplex->c[3]->w,
|
||||
m_simplex->c[2]->w-m_simplex->c[3]->w))>0)
|
||||
return(true);
|
||||
}
|
||||
break;
|
||||
}
|
||||
return(false);
|
||||
}
|
||||
/* Internals */
|
||||
void getsupport(const btVector3& d,sSV& sv) const
|
||||
{
|
||||
sv.d = d/d.length();
|
||||
sv.w = m_shape.Support(sv.d);
|
||||
}
|
||||
void removevertice(sSimplex& simplex)
|
||||
{
|
||||
m_free[m_nfree++]=simplex.c[--simplex.rank];
|
||||
}
|
||||
void appendvertice(sSimplex& simplex,const btVector3& v)
|
||||
{
|
||||
simplex.p[simplex.rank]=0;
|
||||
simplex.c[simplex.rank]=m_free[--m_nfree];
|
||||
getsupport(v,*simplex.c[simplex.rank++]);
|
||||
}
|
||||
static btScalar det(const btVector3& a,const btVector3& b,const btVector3& c)
|
||||
{
|
||||
return( a.y()*b.z()*c.x()+a.z()*b.x()*c.y()-
|
||||
a.x()*b.z()*c.y()-a.y()*b.x()*c.z()+
|
||||
a.x()*b.y()*c.z()-a.z()*b.y()*c.x());
|
||||
}
|
||||
static btScalar projectorigin( const btVector3& a,
|
||||
const btVector3& b,
|
||||
btScalar* w,U& m)
|
||||
{
|
||||
const btVector3 d=b-a;
|
||||
const btScalar l=d.length2();
|
||||
if(l>GJK_SIMPLEX2_EPS)
|
||||
{
|
||||
const btScalar t(l>0?-dot(a,d)/l:0);
|
||||
if(t>=1) { w[0]=0;w[1]=1;m=2;return(b.length2()); }
|
||||
else if(t<=0) { w[0]=1;w[1]=0;m=1;return(a.length2()); }
|
||||
else { w[0]=1-(w[1]=t);m=3;return((a+d*t).length2()); }
|
||||
}
|
||||
return(-1);
|
||||
}
|
||||
static btScalar projectorigin( const btVector3& a,
|
||||
const btVector3& b,
|
||||
const btVector3& c,
|
||||
btScalar* w,U& m)
|
||||
{
|
||||
static const U imd3[]={1,2,0};
|
||||
const btVector3* vt[]={&a,&b,&c};
|
||||
const btVector3 dl[]={a-b,b-c,c-a};
|
||||
const btVector3 n=cross(dl[0],dl[1]);
|
||||
const btScalar l=n.length2();
|
||||
if(l>GJK_SIMPLEX3_EPS)
|
||||
{
|
||||
btScalar mindist=-1;
|
||||
btScalar subw[2] = { btScalar(0.0f), btScalar(0.0f) };
|
||||
U subm;
|
||||
for(U i=0;i<3;++i)
|
||||
{
|
||||
if(dot(*vt[i],cross(dl[i],n))>0)
|
||||
{
|
||||
const U j=imd3[i];
|
||||
const btScalar subd(projectorigin(*vt[i],*vt[j],subw,subm));
|
||||
if((mindist<0)||(subd<mindist))
|
||||
{
|
||||
mindist = subd;
|
||||
m = ((subm&1)?1<<i:0)+((subm&2)?1<<j:0);
|
||||
w[i] = subw[0];
|
||||
w[j] = subw[1];
|
||||
w[imd3[j]] = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
if(mindist<0)
|
||||
{
|
||||
const btScalar d=dot(a,n);
|
||||
const btScalar s=btSqrt(l);
|
||||
const btVector3 p=n*(d/l);
|
||||
mindist = p.length2();
|
||||
m = 7;
|
||||
w[0] = (cross(dl[1],b-p)).length()/s;
|
||||
w[1] = (cross(dl[2],c-p)).length()/s;
|
||||
w[2] = 1-(w[0]+w[1]);
|
||||
}
|
||||
return(mindist);
|
||||
}
|
||||
return(-1);
|
||||
}
|
||||
static btScalar projectorigin( const btVector3& a,
|
||||
const btVector3& b,
|
||||
const btVector3& c,
|
||||
const btVector3& d,
|
||||
btScalar* w,U& m)
|
||||
{
|
||||
static const U imd3[]={1,2,0};
|
||||
const btVector3* vt[]={&a,&b,&c,&d};
|
||||
const btVector3 dl[]={a-d,b-d,c-d};
|
||||
const btScalar vl=det(dl[0],dl[1],dl[2]);
|
||||
const bool ng=(vl*dot(a,cross(b-c,a-b)))<=0;
|
||||
if(ng&&(btFabs(vl)>GJK_SIMPLEX4_EPS))
|
||||
{
|
||||
btScalar mindist=-1;
|
||||
btScalar subw[3];
|
||||
U subm;
|
||||
for(U i=0;i<3;++i)
|
||||
{
|
||||
const U j=imd3[i];
|
||||
const btScalar s=vl*dot(d,cross(dl[i],dl[j]));
|
||||
if(s>0)
|
||||
{
|
||||
const btScalar subd=projectorigin(*vt[i],*vt[j],d,subw,subm);
|
||||
if((mindist<0)||(subd<mindist))
|
||||
{
|
||||
mindist = subd;
|
||||
m = (subm&1?1<<i:0)+
|
||||
(subm&2?1<<j:0)+
|
||||
(subm&4?8:0);
|
||||
w[i] = subw[0];
|
||||
w[j] = subw[1];
|
||||
w[imd3[j]] = 0;
|
||||
w[3] = subw[2];
|
||||
}
|
||||
}
|
||||
}
|
||||
if(mindist<0)
|
||||
{
|
||||
mindist = 0;
|
||||
m = 15;
|
||||
w[0] = det(c,b,d)/vl;
|
||||
w[1] = det(a,c,d)/vl;
|
||||
w[2] = det(b,a,d)/vl;
|
||||
w[3] = 1-(w[0]+w[1]+w[2]);
|
||||
}
|
||||
return(mindist);
|
||||
}
|
||||
return(-1);
|
||||
}
|
||||
};
|
||||
|
||||
// EPA
|
||||
struct EPA
|
||||
{
|
||||
/* Types */
|
||||
typedef GJK::sSV sSV;
|
||||
struct sFace
|
||||
{
|
||||
btVector3 n;
|
||||
btScalar d;
|
||||
btScalar p;
|
||||
sSV* c[3];
|
||||
sFace* f[3];
|
||||
sFace* l[2];
|
||||
U1 e[3];
|
||||
U1 pass;
|
||||
};
|
||||
struct sList
|
||||
{
|
||||
sFace* root;
|
||||
U count;
|
||||
sList() : root(0),count(0) {}
|
||||
};
|
||||
struct sHorizon
|
||||
{
|
||||
sFace* cf;
|
||||
sFace* ff;
|
||||
U nf;
|
||||
sHorizon() : cf(0),ff(0),nf(0) {}
|
||||
};
|
||||
struct eStatus { enum _ {
|
||||
Valid,
|
||||
Touching,
|
||||
Degenerated,
|
||||
NonConvex,
|
||||
InvalidHull,
|
||||
OutOfFaces,
|
||||
OutOfVertices,
|
||||
AccuraryReached,
|
||||
FallBack,
|
||||
Failed, };};
|
||||
/* Fields */
|
||||
eStatus::_ m_status;
|
||||
GJK::sSimplex m_result;
|
||||
btVector3 m_normal;
|
||||
btScalar m_depth;
|
||||
sSV m_sv_store[EPA_MAX_VERTICES];
|
||||
sFace m_fc_store[EPA_MAX_FACES];
|
||||
U m_nextsv;
|
||||
sList m_hull;
|
||||
sList m_stock;
|
||||
/* Methods */
|
||||
EPA()
|
||||
{
|
||||
Initialize();
|
||||
}
|
||||
void Initialize()
|
||||
{
|
||||
m_status = eStatus::Failed;
|
||||
m_normal = btVector3(0,0,0);
|
||||
m_depth = 0;
|
||||
m_nextsv = 0;
|
||||
for(U i=0;i<EPA_MAX_FACES;++i)
|
||||
{
|
||||
append(m_stock,&m_fc_store[EPA_MAX_FACES-i-1]);
|
||||
}
|
||||
}
|
||||
eStatus::_ Evaluate(GJK& gjk,const btVector3& guess)
|
||||
{
|
||||
GJK::sSimplex& simplex=*gjk.m_simplex;
|
||||
if((simplex.rank>1)&&gjk.EncloseOrigin())
|
||||
{
|
||||
/* Clean up */
|
||||
while(m_hull.root)
|
||||
{
|
||||
sFace* f(m_hull.root);
|
||||
remove(m_hull,f);
|
||||
append(m_stock,f);
|
||||
}
|
||||
m_status = eStatus::Valid;
|
||||
m_nextsv = 0;
|
||||
/* Orient simplex */
|
||||
if(gjk.det( simplex.c[0]->w-simplex.c[3]->w,
|
||||
simplex.c[1]->w-simplex.c[3]->w,
|
||||
simplex.c[2]->w-simplex.c[3]->w)<0)
|
||||
{
|
||||
btSwap(simplex.c[0],simplex.c[1]);
|
||||
btSwap(simplex.p[0],simplex.p[1]);
|
||||
}
|
||||
/* Build initial hull */
|
||||
sFace* tetra[]={newface(simplex.c[0],simplex.c[1],simplex.c[2],true),
|
||||
newface(simplex.c[1],simplex.c[0],simplex.c[3],true),
|
||||
newface(simplex.c[2],simplex.c[1],simplex.c[3],true),
|
||||
newface(simplex.c[0],simplex.c[2],simplex.c[3],true)};
|
||||
if(m_hull.count==4)
|
||||
{
|
||||
sFace* best=findbest();
|
||||
sFace outer=*best;
|
||||
U pass=0;
|
||||
U iterations=0;
|
||||
bind(tetra[0],0,tetra[1],0);
|
||||
bind(tetra[0],1,tetra[2],0);
|
||||
bind(tetra[0],2,tetra[3],0);
|
||||
bind(tetra[1],1,tetra[3],2);
|
||||
bind(tetra[1],2,tetra[2],1);
|
||||
bind(tetra[2],2,tetra[3],1);
|
||||
m_status=eStatus::Valid;
|
||||
for(;iterations<EPA_MAX_ITERATIONS;++iterations)
|
||||
{
|
||||
if(m_nextsv<EPA_MAX_VERTICES)
|
||||
{
|
||||
sHorizon horizon;
|
||||
sSV* w=&m_sv_store[m_nextsv++];
|
||||
bool valid=true;
|
||||
best->pass = (U1)(++pass);
|
||||
gjk.getsupport(best->n,*w);
|
||||
const btScalar wdist=dot(best->n,w->w)-best->d;
|
||||
if(wdist>EPA_ACCURACY)
|
||||
{
|
||||
for(U j=0;(j<3)&&valid;++j)
|
||||
{
|
||||
valid&=expand( pass,w,
|
||||
best->f[j],best->e[j],
|
||||
horizon);
|
||||
}
|
||||
if(valid&&(horizon.nf>=3))
|
||||
{
|
||||
bind(horizon.cf,1,horizon.ff,2);
|
||||
remove(m_hull,best);
|
||||
append(m_stock,best);
|
||||
best=findbest();
|
||||
if(best->p>=outer.p) outer=*best;
|
||||
} else { m_status=eStatus::InvalidHull;break; }
|
||||
} else { m_status=eStatus::AccuraryReached;break; }
|
||||
} else { m_status=eStatus::OutOfVertices;break; }
|
||||
}
|
||||
const btVector3 projection=outer.n*outer.d;
|
||||
m_normal = outer.n;
|
||||
m_depth = outer.d;
|
||||
m_result.rank = 3;
|
||||
m_result.c[0] = outer.c[0];
|
||||
m_result.c[1] = outer.c[1];
|
||||
m_result.c[2] = outer.c[2];
|
||||
m_result.p[0] = cross( outer.c[1]->w-projection,
|
||||
outer.c[2]->w-projection).length();
|
||||
m_result.p[1] = cross( outer.c[2]->w-projection,
|
||||
outer.c[0]->w-projection).length();
|
||||
m_result.p[2] = cross( outer.c[0]->w-projection,
|
||||
outer.c[1]->w-projection).length();
|
||||
const btScalar sum=m_result.p[0]+m_result.p[1]+m_result.p[2];
|
||||
m_result.p[0] /= sum;
|
||||
m_result.p[1] /= sum;
|
||||
m_result.p[2] /= sum;
|
||||
return(m_status);
|
||||
}
|
||||
}
|
||||
/* Fallback */
|
||||
m_status = eStatus::FallBack;
|
||||
m_normal = -guess;
|
||||
const btScalar nl=m_normal.length();
|
||||
if(nl>0)
|
||||
m_normal = m_normal/nl;
|
||||
else
|
||||
m_normal = btVector3(1,0,0);
|
||||
m_depth = 0;
|
||||
m_result.rank=1;
|
||||
m_result.c[0]=simplex.c[0];
|
||||
m_result.p[0]=1;
|
||||
return(m_status);
|
||||
}
|
||||
sFace* newface(sSV* a,sSV* b,sSV* c,bool forced)
|
||||
{
|
||||
if(m_stock.root)
|
||||
{
|
||||
sFace* face=m_stock.root;
|
||||
remove(m_stock,face);
|
||||
append(m_hull,face);
|
||||
face->pass = 0;
|
||||
face->c[0] = a;
|
||||
face->c[1] = b;
|
||||
face->c[2] = c;
|
||||
face->n = cross(b->w-a->w,c->w-a->w);
|
||||
const btScalar l=face->n.length();
|
||||
const bool v=l>EPA_ACCURACY;
|
||||
face->p = btMin(btMin(
|
||||
dot(a->w,cross(face->n,a->w-b->w)),
|
||||
dot(b->w,cross(face->n,b->w-c->w))),
|
||||
dot(c->w,cross(face->n,c->w-a->w))) /
|
||||
(v?l:1);
|
||||
face->p = face->p>=-EPA_INSIDE_EPS?0:face->p;
|
||||
if(v)
|
||||
{
|
||||
face->d = dot(a->w,face->n)/l;
|
||||
face->n /= l;
|
||||
if(forced||(face->d>=-EPA_PLANE_EPS))
|
||||
{
|
||||
return(face);
|
||||
} else m_status=eStatus::NonConvex;
|
||||
} else m_status=eStatus::Degenerated;
|
||||
remove(m_hull,face);
|
||||
append(m_stock,face);
|
||||
return(0);
|
||||
}
|
||||
m_status=m_stock.root?eStatus::OutOfVertices:eStatus::OutOfFaces;
|
||||
return(0);
|
||||
}
|
||||
sFace* findbest()
|
||||
{
|
||||
sFace* minf=m_hull.root;
|
||||
btScalar mind=minf->d*minf->d;
|
||||
btScalar maxp=minf->p;
|
||||
for(sFace* f=minf->l[1];f;f=f->l[1])
|
||||
{
|
||||
const btScalar sqd=f->d*f->d;
|
||||
if((f->p>=maxp)&&(sqd<mind))
|
||||
{
|
||||
minf=f;
|
||||
mind=sqd;
|
||||
maxp=f->p;
|
||||
}
|
||||
}
|
||||
return(minf);
|
||||
}
|
||||
bool expand(U pass,sSV* w,sFace* f,U e,sHorizon& horizon)
|
||||
{
|
||||
static const U i1m3[]={1,2,0};
|
||||
static const U i2m3[]={2,0,1};
|
||||
if(f->pass!=pass)
|
||||
{
|
||||
const U e1=i1m3[e];
|
||||
if((dot(f->n,w->w)-f->d)<-EPA_PLANE_EPS)
|
||||
{
|
||||
sFace* nf=newface(f->c[e1],f->c[e],w,false);
|
||||
if(nf)
|
||||
{
|
||||
bind(nf,0,f,e);
|
||||
if(horizon.cf) bind(horizon.cf,1,nf,2); else horizon.ff=nf;
|
||||
horizon.cf=nf;
|
||||
++horizon.nf;
|
||||
return(true);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
const U e2=i2m3[e];
|
||||
f->pass = (U1)pass;
|
||||
if( expand(pass,w,f->f[e1],f->e[e1],horizon)&&
|
||||
expand(pass,w,f->f[e2],f->e[e2],horizon))
|
||||
{
|
||||
remove(m_hull,f);
|
||||
append(m_stock,f);
|
||||
return(true);
|
||||
}
|
||||
}
|
||||
}
|
||||
return(false);
|
||||
}
|
||||
static inline void bind(sFace* fa,U ea,sFace* fb,U eb)
|
||||
{
|
||||
fa->e[ea]=(U1)eb;fa->f[ea]=fb;
|
||||
fb->e[eb]=(U1)ea;fb->f[eb]=fa;
|
||||
}
|
||||
static inline void append(sList& list,sFace* face)
|
||||
{
|
||||
face->l[0] = 0;
|
||||
face->l[1] = list.root;
|
||||
if(list.root) list.root->l[0]=face;
|
||||
list.root = face;
|
||||
++list.count;
|
||||
}
|
||||
static inline void remove(sList& list,sFace* face)
|
||||
{
|
||||
if(face->l[1]) face->l[1]->l[0]=face->l[0];
|
||||
if(face->l[0]) face->l[0]->l[1]=face->l[1];
|
||||
if(face==list.root) list.root=face->l[1];
|
||||
--list.count;
|
||||
}
|
||||
};
|
||||
|
||||
//
|
||||
static void Initialize(void* shapeA,
|
||||
SpuConvexPolyhedronVertexData* convexDataA,
|
||||
int shapeTypeA,
|
||||
float marginA,
|
||||
const btTransform& wtrs0,
|
||||
void* shapeB,
|
||||
SpuConvexPolyhedronVertexData* convexDataB,
|
||||
int shapeTypeB,
|
||||
float marginB,
|
||||
const btTransform& wtrs1,
|
||||
SpuGjkEpaSolver2::sResults& results,
|
||||
tShape& shape,
|
||||
bool withmargins)
|
||||
{
|
||||
/* Results */
|
||||
results.witnesses[0] =
|
||||
results.witnesses[1] = btVector3(0,0,0);
|
||||
results.status = SpuGjkEpaSolver2::sResults::Separated;
|
||||
/* Shape */
|
||||
shape.m_shapes[0].margin = marginA;
|
||||
shape.m_shapes[0].shape = shapeA;
|
||||
shape.m_shapes[0].shapeType = shapeTypeA;
|
||||
shape.m_shapes[0].convexData = convexDataA;
|
||||
shape.m_shapes[1].margin = marginB;
|
||||
shape.m_shapes[1].shape = shapeB;
|
||||
shape.m_shapes[1].shapeType = shapeTypeB;
|
||||
shape.m_shapes[1].convexData = convexDataB;
|
||||
shape.m_toshape1 = wtrs1.getBasis().transposeTimes(wtrs0.getBasis());
|
||||
shape.m_toshape0 = wtrs0.inverseTimes(wtrs1);
|
||||
shape.EnableMargin(withmargins);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
//
|
||||
// Api
|
||||
//
|
||||
|
||||
using namespace gjkepa2_spu_impl;
|
||||
|
||||
//
|
||||
int SpuGjkEpaSolver2::StackSizeRequirement()
|
||||
{
|
||||
return(sizeof(GJK)+sizeof(EPA));
|
||||
}
|
||||
|
||||
//
|
||||
bool SpuGjkEpaSolver2::Penetration(void* shapeA,
|
||||
SpuConvexPolyhedronVertexData* convexDataA,
|
||||
int shapeTypeA,
|
||||
float marginA,
|
||||
const btTransform& wtrs0,
|
||||
void* shapeB,
|
||||
SpuConvexPolyhedronVertexData* convexDataB,
|
||||
int shapeTypeB,
|
||||
float marginB,
|
||||
const btTransform& wtrs1,
|
||||
const btVector3& guess,
|
||||
sResults& results)
|
||||
{
|
||||
tShape shape;
|
||||
Initialize(shapeA, convexDataA, shapeTypeA, marginA, wtrs0, shapeB, convexDataB, shapeTypeB, marginB, wtrs1, results,shape,true);
|
||||
GJK gjk;
|
||||
GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,-guess);
|
||||
switch(gjk_status)
|
||||
{
|
||||
case GJK::eStatus::Inside:
|
||||
{
|
||||
EPA epa;
|
||||
EPA::eStatus::_ epa_status=epa.Evaluate(gjk,-guess);
|
||||
if(epa_status!=EPA::eStatus::Failed)
|
||||
{
|
||||
btVector3 w0=btVector3(0,0,0);
|
||||
for(U i=0;i<epa.m_result.rank;++i)
|
||||
{
|
||||
w0+=shape.Support(epa.m_result.c[i]->d,0)*epa.m_result.p[i];
|
||||
}
|
||||
results.status = sResults::Penetrating;
|
||||
results.witnesses[0] = wtrs0*w0;
|
||||
results.witnesses[1] = wtrs0*(w0-epa.m_normal*epa.m_depth);
|
||||
return(true);
|
||||
} else results.status=sResults::EPA_Failed;
|
||||
}
|
||||
break;
|
||||
case GJK::eStatus::Failed:
|
||||
results.status=sResults::GJK_Failed;
|
||||
break;
|
||||
}
|
||||
return(false);
|
||||
}
|
||||
|
||||
/* Symbols cleanup */
|
||||
|
||||
#undef GJK_MAX_ITERATIONS
|
||||
#undef GJK_ACCURARY
|
||||
#undef GJK_MIN_DISTANCE
|
||||
#undef GJK_DUPLICATED_EPS
|
||||
#undef GJK_SIMPLEX2_EPS
|
||||
#undef GJK_SIMPLEX3_EPS
|
||||
#undef GJK_SIMPLEX4_EPS
|
||||
|
||||
#undef EPA_MAX_VERTICES
|
||||
#undef EPA_MAX_FACES
|
||||
#undef EPA_MAX_ITERATIONS
|
||||
#undef EPA_ACCURACY
|
||||
#undef EPA_FALLBACK
|
||||
#undef EPA_PLANE_EPS
|
||||
#undef EPA_INSIDE_EPS
|
||||
@@ -0,0 +1,38 @@
|
||||
#ifndef _68DA1F85_90B7_4bb0_A705_83B4040A75C6_
|
||||
#define _68DA1F85_90B7_4bb0_A705_83B4040A75C6_
|
||||
#include "BulletCollision/CollisionShapes/btConvexShape.h"
|
||||
|
||||
///btGjkEpaSolver contributed under zlib by Nathanael Presson
|
||||
struct SpuGjkEpaSolver2
|
||||
{
|
||||
struct sResults
|
||||
{
|
||||
enum eStatus
|
||||
{
|
||||
Separated, /* Shapes doesnt penetrate */
|
||||
Penetrating, /* Shapes are penetrating */
|
||||
GJK_Failed, /* GJK phase fail, no big issue, shapes are probably just 'touching' */
|
||||
EPA_Failed, /* EPA phase fail, bigger problem, need to save parameters, and debug */
|
||||
} status;
|
||||
btVector3 witnesses[2];
|
||||
btVector3 normal;
|
||||
};
|
||||
|
||||
static int StackSizeRequirement();
|
||||
|
||||
|
||||
static bool Penetration(void* shapeA,
|
||||
SpuConvexPolyhedronVertexData* convexDataA,
|
||||
int shapeTypeA,
|
||||
float marginA,
|
||||
const btTransform& xformA,
|
||||
void* shapeB,
|
||||
SpuConvexPolyhedronVertexData* convexDataB,
|
||||
int shapeTypeB,
|
||||
float marginB,
|
||||
const btTransform& xformB,
|
||||
const btVector3& guess,
|
||||
sResults& results);
|
||||
};
|
||||
|
||||
#endif
|
||||
@@ -0,0 +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 "SpuCollisionShapes.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[0]);//, &featureIndexA);
|
||||
btVector3 qInB = localGetSupportingVertexWithoutMargin(m_shapeTypeB, m_minkowskiB, seperatingAxisInB,input.m_convexVertexData[1]);//, &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[0], input.m_convexVertexData[1]
|
||||
);
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -0,0 +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
|
||||
@@ -0,0 +1,19 @@
|
||||
/*
|
||||
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.
|
||||
*/
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -0,0 +1,347 @@
|
||||
/*
|
||||
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 "SpuCollisionShapes.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* convexVertexDataA,
|
||||
struct SpuConvexPolyhedronVertexData* convexVertexDataB
|
||||
) 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,convexVertexDataA);//, NULL);
|
||||
qInB = localGetSupportingVertexWithoutMargin(shapeTypeB, convexB, seperatingAxisInB,convexVertexDataB);//, 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) + btScalar(1.00);
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
//#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[0] = convexVertexDataA;
|
||||
input.m_convexVertexData[1] = convexVertexDataB;
|
||||
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
|
||||
|
||||
|
||||
} else {
|
||||
// could not seperate shapes
|
||||
btAssert (false);
|
||||
}
|
||||
return res.m_hasResult;
|
||||
}
|
||||
|
||||
|
||||
|
||||
@@ -0,0 +1,47 @@
|
||||
|
||||
/*
|
||||
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* convexVertexDataA,
|
||||
struct SpuConvexPolyhedronVertexData* convexVertexDataB
|
||||
) const;
|
||||
|
||||
|
||||
};
|
||||
|
||||
|
||||
#endif //MINKOWSKI_PENETRATION_DEPTH_SOLVER_H
|
||||
|
||||
@@ -0,0 +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
|
||||
@@ -0,0 +1,606 @@
|
||||
|
||||
/*
|
||||
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.
|
||||
|
||||
Elsevier CDROM license agreements grants nonexclusive license to use the software
|
||||
for any purpose, commercial or non-commercial as long as the following credit is included
|
||||
identifying the original source of the software:
|
||||
|
||||
Parts of the source are "from the book Real-Time Collision Detection by
|
||||
Christer Ericson, published by Morgan Kaufmann Publishers,
|
||||
(c) 2005 Elsevier Inc."
|
||||
|
||||
*/
|
||||
|
||||
|
||||
#include "SpuVoronoiSimplexSolver.h"
|
||||
#include <assert.h>
|
||||
#include <stdio.h>
|
||||
|
||||
#define VERTA 0
|
||||
#define VERTB 1
|
||||
#define VERTC 2
|
||||
#define VERTD 3
|
||||
|
||||
#define CATCH_DEGENERATE_TETRAHEDRON 1
|
||||
void SpuVoronoiSimplexSolver::removeVertex(int index)
|
||||
{
|
||||
|
||||
assert(m_numVertices>0);
|
||||
m_numVertices--;
|
||||
m_simplexVectorW[index] = m_simplexVectorW[m_numVertices];
|
||||
m_simplexPointsP[index] = m_simplexPointsP[m_numVertices];
|
||||
m_simplexPointsQ[index] = m_simplexPointsQ[m_numVertices];
|
||||
}
|
||||
|
||||
void SpuVoronoiSimplexSolver::reduceVertices (const SpuUsageBitfield& usedVerts)
|
||||
{
|
||||
if ((numVertices() >= 4) && (!usedVerts.usedVertexD))
|
||||
removeVertex(3);
|
||||
|
||||
if ((numVertices() >= 3) && (!usedVerts.usedVertexC))
|
||||
removeVertex(2);
|
||||
|
||||
if ((numVertices() >= 2) && (!usedVerts.usedVertexB))
|
||||
removeVertex(1);
|
||||
|
||||
if ((numVertices() >= 1) && (!usedVerts.usedVertexA))
|
||||
removeVertex(0);
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
//clear the simplex, remove all the vertices
|
||||
void SpuVoronoiSimplexSolver::reset()
|
||||
{
|
||||
m_cachedValidClosest = false;
|
||||
m_numVertices = 0;
|
||||
m_needsUpdate = true;
|
||||
m_lastW = btVector3(btScalar(1e30),btScalar(1e30),btScalar(1e30));
|
||||
m_cachedBC.reset();
|
||||
}
|
||||
|
||||
|
||||
|
||||
//add a vertex
|
||||
void SpuVoronoiSimplexSolver::addVertex(const btVector3& w, const btPoint3& p, const btPoint3& q)
|
||||
{
|
||||
m_lastW = w;
|
||||
m_needsUpdate = true;
|
||||
|
||||
m_simplexVectorW[m_numVertices] = w;
|
||||
m_simplexPointsP[m_numVertices] = p;
|
||||
m_simplexPointsQ[m_numVertices] = q;
|
||||
|
||||
m_numVertices++;
|
||||
}
|
||||
|
||||
bool SpuVoronoiSimplexSolver::updateClosestVectorAndPoints()
|
||||
{
|
||||
|
||||
if (m_needsUpdate)
|
||||
{
|
||||
m_cachedBC.reset();
|
||||
|
||||
m_needsUpdate = false;
|
||||
|
||||
switch (numVertices())
|
||||
{
|
||||
case 0:
|
||||
m_cachedValidClosest = false;
|
||||
break;
|
||||
case 1:
|
||||
{
|
||||
m_cachedP1 = m_simplexPointsP[0];
|
||||
m_cachedP2 = m_simplexPointsQ[0];
|
||||
m_cachedV = m_cachedP1-m_cachedP2; //== m_simplexVectorW[0]
|
||||
m_cachedBC.reset();
|
||||
m_cachedBC.setBarycentricCoordinates(btScalar(1.),btScalar(0.),btScalar(0.),btScalar(0.));
|
||||
m_cachedValidClosest = m_cachedBC.isValid();
|
||||
break;
|
||||
};
|
||||
case 2:
|
||||
{
|
||||
//closest point origin from line segment
|
||||
const btVector3& from = m_simplexVectorW[0];
|
||||
const btVector3& to = m_simplexVectorW[1];
|
||||
btVector3 nearest;
|
||||
|
||||
btVector3 p (btScalar(0.),btScalar(0.),btScalar(0.));
|
||||
btVector3 diff = p - from;
|
||||
btVector3 v = to - from;
|
||||
btScalar t = v.dot(diff);
|
||||
|
||||
if (t > 0) {
|
||||
btScalar dotVV = v.dot(v);
|
||||
if (t < dotVV) {
|
||||
t /= dotVV;
|
||||
diff -= t*v;
|
||||
m_cachedBC.m_usedVertices.usedVertexA = true;
|
||||
m_cachedBC.m_usedVertices.usedVertexB = true;
|
||||
} else {
|
||||
t = 1;
|
||||
diff -= v;
|
||||
//reduce to 1 point
|
||||
m_cachedBC.m_usedVertices.usedVertexB = true;
|
||||
}
|
||||
} else
|
||||
{
|
||||
t = 0;
|
||||
//reduce to 1 point
|
||||
m_cachedBC.m_usedVertices.usedVertexA = true;
|
||||
}
|
||||
m_cachedBC.setBarycentricCoordinates(1-t,t);
|
||||
nearest = from + t*v;
|
||||
|
||||
m_cachedP1 = m_simplexPointsP[0] + t * (m_simplexPointsP[1] - m_simplexPointsP[0]);
|
||||
m_cachedP2 = m_simplexPointsQ[0] + t * (m_simplexPointsQ[1] - m_simplexPointsQ[0]);
|
||||
m_cachedV = m_cachedP1 - m_cachedP2;
|
||||
|
||||
reduceVertices(m_cachedBC.m_usedVertices);
|
||||
|
||||
m_cachedValidClosest = m_cachedBC.isValid();
|
||||
break;
|
||||
}
|
||||
case 3:
|
||||
{
|
||||
//closest point origin from triangle
|
||||
btVector3 p (btScalar(0.),btScalar(0.),btScalar(0.));
|
||||
|
||||
const btVector3& a = m_simplexVectorW[0];
|
||||
const btVector3& b = m_simplexVectorW[1];
|
||||
const btVector3& c = m_simplexVectorW[2];
|
||||
|
||||
closestPtPointTriangle(p,a,b,c,m_cachedBC);
|
||||
m_cachedP1 = m_simplexPointsP[0] * m_cachedBC.m_barycentricCoords[0] +
|
||||
m_simplexPointsP[1] * m_cachedBC.m_barycentricCoords[1] +
|
||||
m_simplexPointsP[2] * m_cachedBC.m_barycentricCoords[2] +
|
||||
m_simplexPointsP[3] * m_cachedBC.m_barycentricCoords[3];
|
||||
|
||||
m_cachedP2 = m_simplexPointsQ[0] * m_cachedBC.m_barycentricCoords[0] +
|
||||
m_simplexPointsQ[1] * m_cachedBC.m_barycentricCoords[1] +
|
||||
m_simplexPointsQ[2] * m_cachedBC.m_barycentricCoords[2] +
|
||||
m_simplexPointsQ[3] * m_cachedBC.m_barycentricCoords[3];
|
||||
|
||||
m_cachedV = m_cachedP1-m_cachedP2;
|
||||
|
||||
reduceVertices (m_cachedBC.m_usedVertices);
|
||||
m_cachedValidClosest = m_cachedBC.isValid();
|
||||
|
||||
break;
|
||||
}
|
||||
case 4:
|
||||
{
|
||||
|
||||
|
||||
btVector3 p (btScalar(0.),btScalar(0.),btScalar(0.));
|
||||
|
||||
const btVector3& a = m_simplexVectorW[0];
|
||||
const btVector3& b = m_simplexVectorW[1];
|
||||
const btVector3& c = m_simplexVectorW[2];
|
||||
const btVector3& d = m_simplexVectorW[3];
|
||||
|
||||
bool hasSeperation = closestPtPointTetrahedron(p,a,b,c,d,m_cachedBC);
|
||||
|
||||
if (hasSeperation)
|
||||
{
|
||||
|
||||
m_cachedP1 = m_simplexPointsP[0] * m_cachedBC.m_barycentricCoords[0] +
|
||||
m_simplexPointsP[1] * m_cachedBC.m_barycentricCoords[1] +
|
||||
m_simplexPointsP[2] * m_cachedBC.m_barycentricCoords[2] +
|
||||
m_simplexPointsP[3] * m_cachedBC.m_barycentricCoords[3];
|
||||
|
||||
m_cachedP2 = m_simplexPointsQ[0] * m_cachedBC.m_barycentricCoords[0] +
|
||||
m_simplexPointsQ[1] * m_cachedBC.m_barycentricCoords[1] +
|
||||
m_simplexPointsQ[2] * m_cachedBC.m_barycentricCoords[2] +
|
||||
m_simplexPointsQ[3] * m_cachedBC.m_barycentricCoords[3];
|
||||
|
||||
m_cachedV = m_cachedP1-m_cachedP2;
|
||||
reduceVertices (m_cachedBC.m_usedVertices);
|
||||
} else
|
||||
{
|
||||
// printf("sub distance got penetration\n");
|
||||
|
||||
if (m_cachedBC.m_degenerate)
|
||||
{
|
||||
m_cachedValidClosest = false;
|
||||
} else
|
||||
{
|
||||
m_cachedValidClosest = true;
|
||||
//degenerate case == false, penetration = true + zero
|
||||
m_cachedV.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
m_cachedValidClosest = m_cachedBC.isValid();
|
||||
|
||||
//closest point origin from tetrahedron
|
||||
break;
|
||||
}
|
||||
default:
|
||||
{
|
||||
m_cachedValidClosest = false;
|
||||
}
|
||||
};
|
||||
}
|
||||
|
||||
return m_cachedValidClosest;
|
||||
|
||||
}
|
||||
|
||||
//return/calculate the closest vertex
|
||||
bool SpuVoronoiSimplexSolver::closest(btVector3& v)
|
||||
{
|
||||
bool succes = updateClosestVectorAndPoints();
|
||||
v = m_cachedV;
|
||||
return succes;
|
||||
}
|
||||
|
||||
|
||||
|
||||
btScalar SpuVoronoiSimplexSolver::maxVertex()
|
||||
{
|
||||
int i, numverts = numVertices();
|
||||
btScalar maxV = btScalar(0.);
|
||||
for (i=0;i<numverts;i++)
|
||||
{
|
||||
btScalar curLen2 = m_simplexVectorW[i].length2();
|
||||
if (maxV < curLen2)
|
||||
maxV = curLen2;
|
||||
}
|
||||
return maxV;
|
||||
}
|
||||
|
||||
|
||||
|
||||
//return the current simplex
|
||||
int SpuVoronoiSimplexSolver::getSimplex(btPoint3 *pBuf, btPoint3 *qBuf, btVector3 *yBuf) const
|
||||
{
|
||||
int i;
|
||||
for (i=0;i<numVertices();i++)
|
||||
{
|
||||
yBuf[i] = m_simplexVectorW[i];
|
||||
pBuf[i] = m_simplexPointsP[i];
|
||||
qBuf[i] = m_simplexPointsQ[i];
|
||||
}
|
||||
return numVertices();
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
bool SpuVoronoiSimplexSolver::inSimplex(const btVector3& w)
|
||||
{
|
||||
bool found = false;
|
||||
int i, numverts = numVertices();
|
||||
//btScalar maxV = btScalar(0.);
|
||||
|
||||
//w is in the current (reduced) simplex
|
||||
for (i=0;i<numverts;i++)
|
||||
{
|
||||
if (m_simplexVectorW[i] == w)
|
||||
found = true;
|
||||
}
|
||||
|
||||
//check in case lastW is already removed
|
||||
if (w == m_lastW)
|
||||
return true;
|
||||
|
||||
return found;
|
||||
}
|
||||
|
||||
void SpuVoronoiSimplexSolver::backup_closest(btVector3& v)
|
||||
{
|
||||
v = m_cachedV;
|
||||
}
|
||||
|
||||
|
||||
bool SpuVoronoiSimplexSolver::emptySimplex() const
|
||||
{
|
||||
return (numVertices() == 0);
|
||||
|
||||
}
|
||||
|
||||
void SpuVoronoiSimplexSolver::compute_points(btPoint3& p1, btPoint3& p2)
|
||||
{
|
||||
updateClosestVectorAndPoints();
|
||||
p1 = m_cachedP1;
|
||||
p2 = m_cachedP2;
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
bool SpuVoronoiSimplexSolver::closestPtPointTriangle(const btPoint3& p, const btPoint3& a, const btPoint3& b, const btPoint3& c,SpuSubSimplexClosestResult& result)
|
||||
{
|
||||
result.m_usedVertices.reset();
|
||||
|
||||
// Check if P in vertex region outside A
|
||||
btVector3 ab = b - a;
|
||||
btVector3 ac = c - a;
|
||||
btVector3 ap = p - a;
|
||||
btScalar d1 = ab.dot(ap);
|
||||
btScalar d2 = ac.dot(ap);
|
||||
if (d1 <= btScalar(0.0) && d2 <= btScalar(0.0))
|
||||
{
|
||||
result.m_closestPointOnSimplex = a;
|
||||
result.m_usedVertices.usedVertexA = true;
|
||||
result.setBarycentricCoordinates(1,0,0);
|
||||
return true;// a; // barycentric coordinates (1,0,0)
|
||||
}
|
||||
|
||||
// Check if P in vertex region outside B
|
||||
btVector3 bp = p - b;
|
||||
btScalar d3 = ab.dot(bp);
|
||||
btScalar d4 = ac.dot(bp);
|
||||
if (d3 >= btScalar(0.0) && d4 <= d3)
|
||||
{
|
||||
result.m_closestPointOnSimplex = b;
|
||||
result.m_usedVertices.usedVertexB = true;
|
||||
result.setBarycentricCoordinates(0,1,0);
|
||||
|
||||
return true; // b; // barycentric coordinates (0,1,0)
|
||||
}
|
||||
// Check if P in edge region of AB, if so return projection of P onto AB
|
||||
btScalar vc = d1*d4 - d3*d2;
|
||||
if (vc <= btScalar(0.0) && d1 >= btScalar(0.0) && d3 <= btScalar(0.0)) {
|
||||
btScalar v = d1 / (d1 - d3);
|
||||
result.m_closestPointOnSimplex = a + v * ab;
|
||||
result.m_usedVertices.usedVertexA = true;
|
||||
result.m_usedVertices.usedVertexB = true;
|
||||
result.setBarycentricCoordinates(1-v,v,0);
|
||||
return true;
|
||||
//return a + v * ab; // barycentric coordinates (1-v,v,0)
|
||||
}
|
||||
|
||||
// Check if P in vertex region outside C
|
||||
btVector3 cp = p - c;
|
||||
btScalar d5 = ab.dot(cp);
|
||||
btScalar d6 = ac.dot(cp);
|
||||
if (d6 >= btScalar(0.0) && d5 <= d6)
|
||||
{
|
||||
result.m_closestPointOnSimplex = c;
|
||||
result.m_usedVertices.usedVertexC = true;
|
||||
result.setBarycentricCoordinates(0,0,1);
|
||||
return true;//c; // barycentric coordinates (0,0,1)
|
||||
}
|
||||
|
||||
// Check if P in edge region of AC, if so return projection of P onto AC
|
||||
btScalar vb = d5*d2 - d1*d6;
|
||||
if (vb <= btScalar(0.0) && d2 >= btScalar(0.0) && d6 <= btScalar(0.0)) {
|
||||
btScalar w = d2 / (d2 - d6);
|
||||
result.m_closestPointOnSimplex = a + w * ac;
|
||||
result.m_usedVertices.usedVertexA = true;
|
||||
result.m_usedVertices.usedVertexC = true;
|
||||
result.setBarycentricCoordinates(1-w,0,w);
|
||||
return true;
|
||||
//return a + w * ac; // barycentric coordinates (1-w,0,w)
|
||||
}
|
||||
|
||||
// Check if P in edge region of BC, if so return projection of P onto BC
|
||||
btScalar va = d3*d6 - d5*d4;
|
||||
if (va <= btScalar(0.0) && (d4 - d3) >= btScalar(0.0) && (d5 - d6) >= btScalar(0.0)) {
|
||||
btScalar w = (d4 - d3) / ((d4 - d3) + (d5 - d6));
|
||||
|
||||
result.m_closestPointOnSimplex = b + w * (c - b);
|
||||
result.m_usedVertices.usedVertexB = true;
|
||||
result.m_usedVertices.usedVertexC = true;
|
||||
result.setBarycentricCoordinates(0,1-w,w);
|
||||
return true;
|
||||
// return b + w * (c - b); // barycentric coordinates (0,1-w,w)
|
||||
}
|
||||
|
||||
// P inside face region. Compute Q through its barycentric coordinates (u,v,w)
|
||||
btScalar denom = btScalar(1.0) / (va + vb + vc);
|
||||
btScalar v = vb * denom;
|
||||
btScalar w = vc * denom;
|
||||
|
||||
result.m_closestPointOnSimplex = a + ab * v + ac * w;
|
||||
result.m_usedVertices.usedVertexA = true;
|
||||
result.m_usedVertices.usedVertexB = true;
|
||||
result.m_usedVertices.usedVertexC = true;
|
||||
result.setBarycentricCoordinates(1-v-w,v,w);
|
||||
|
||||
return true;
|
||||
// return a + ab * v + ac * w; // = u*a + v*b + w*c, u = va * denom = btScalar(1.0) - v - w
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
/// Test if point p and d lie on opposite sides of plane through abc
|
||||
int SpuVoronoiSimplexSolver::pointOutsideOfPlane(const btPoint3& p, const btPoint3& a, const btPoint3& b, const btPoint3& c, const btPoint3& d)
|
||||
{
|
||||
btVector3 normal = (b-a).cross(c-a);
|
||||
|
||||
btScalar signp = (p - a).dot(normal); // [AP AB AC]
|
||||
btScalar signd = (d - a).dot( normal); // [AD AB AC]
|
||||
|
||||
#ifdef CATCH_DEGENERATE_TETRAHEDRON
|
||||
#ifdef BT_USE_DOUBLE_PRECISION
|
||||
if (signd * signd < (btScalar(1e-8) * btScalar(1e-8)))
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
#else
|
||||
if (signd * signd < (btScalar(1e-4) * btScalar(1e-4)))
|
||||
{
|
||||
// printf("affine dependent/degenerate\n");//
|
||||
return -1;
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif
|
||||
// Points on opposite sides if expression signs are opposite
|
||||
return signp * signd < btScalar(0.);
|
||||
}
|
||||
|
||||
|
||||
bool SpuVoronoiSimplexSolver::closestPtPointTetrahedron(const btPoint3& p, const btPoint3& a, const btPoint3& b, const btPoint3& c, const btPoint3& d, SpuSubSimplexClosestResult& finalResult)
|
||||
{
|
||||
SpuSubSimplexClosestResult tempResult;
|
||||
|
||||
// Start out assuming point inside all halfspaces, so closest to itself
|
||||
finalResult.m_closestPointOnSimplex = p;
|
||||
finalResult.m_usedVertices.reset();
|
||||
finalResult.m_usedVertices.usedVertexA = true;
|
||||
finalResult.m_usedVertices.usedVertexB = true;
|
||||
finalResult.m_usedVertices.usedVertexC = true;
|
||||
finalResult.m_usedVertices.usedVertexD = true;
|
||||
|
||||
int pointOutsideABC = pointOutsideOfPlane(p, a, b, c, d);
|
||||
int pointOutsideACD = pointOutsideOfPlane(p, a, c, d, b);
|
||||
int pointOutsideADB = pointOutsideOfPlane(p, a, d, b, c);
|
||||
int pointOutsideBDC = pointOutsideOfPlane(p, b, d, c, a);
|
||||
|
||||
if (pointOutsideABC < 0 || pointOutsideACD < 0 || pointOutsideADB < 0 || pointOutsideBDC < 0)
|
||||
{
|
||||
finalResult.m_degenerate = true;
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!pointOutsideABC && !pointOutsideACD && !pointOutsideADB && !pointOutsideBDC)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
|
||||
btScalar bestSqDist = FLT_MAX;
|
||||
// If point outside face abc then compute closest point on abc
|
||||
if (pointOutsideABC)
|
||||
{
|
||||
closestPtPointTriangle(p, a, b, c,tempResult);
|
||||
btPoint3 q = tempResult.m_closestPointOnSimplex;
|
||||
|
||||
btScalar sqDist = (q - p).dot( q - p);
|
||||
// Update best closest point if (squared) distance is less than current best
|
||||
if (sqDist < bestSqDist) {
|
||||
bestSqDist = sqDist;
|
||||
finalResult.m_closestPointOnSimplex = q;
|
||||
//convert result bitmask!
|
||||
finalResult.m_usedVertices.reset();
|
||||
finalResult.m_usedVertices.usedVertexA = tempResult.m_usedVertices.usedVertexA;
|
||||
finalResult.m_usedVertices.usedVertexB = tempResult.m_usedVertices.usedVertexB;
|
||||
finalResult.m_usedVertices.usedVertexC = tempResult.m_usedVertices.usedVertexC;
|
||||
finalResult.setBarycentricCoordinates(
|
||||
tempResult.m_barycentricCoords[VERTA],
|
||||
tempResult.m_barycentricCoords[VERTB],
|
||||
tempResult.m_barycentricCoords[VERTC],
|
||||
0
|
||||
);
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// Repeat test for face acd
|
||||
if (pointOutsideACD)
|
||||
{
|
||||
closestPtPointTriangle(p, a, c, d,tempResult);
|
||||
btPoint3 q = tempResult.m_closestPointOnSimplex;
|
||||
//convert result bitmask!
|
||||
|
||||
btScalar sqDist = (q - p).dot( q - p);
|
||||
if (sqDist < bestSqDist)
|
||||
{
|
||||
bestSqDist = sqDist;
|
||||
finalResult.m_closestPointOnSimplex = q;
|
||||
finalResult.m_usedVertices.reset();
|
||||
finalResult.m_usedVertices.usedVertexA = tempResult.m_usedVertices.usedVertexA;
|
||||
finalResult.m_usedVertices.usedVertexC = tempResult.m_usedVertices.usedVertexB;
|
||||
finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexC;
|
||||
finalResult.setBarycentricCoordinates(
|
||||
tempResult.m_barycentricCoords[VERTA],
|
||||
0,
|
||||
tempResult.m_barycentricCoords[VERTB],
|
||||
tempResult.m_barycentricCoords[VERTC]
|
||||
);
|
||||
|
||||
}
|
||||
}
|
||||
// Repeat test for face adb
|
||||
|
||||
|
||||
if (pointOutsideADB)
|
||||
{
|
||||
closestPtPointTriangle(p, a, d, b,tempResult);
|
||||
btPoint3 q = tempResult.m_closestPointOnSimplex;
|
||||
//convert result bitmask!
|
||||
|
||||
btScalar sqDist = (q - p).dot( q - p);
|
||||
if (sqDist < bestSqDist)
|
||||
{
|
||||
bestSqDist = sqDist;
|
||||
finalResult.m_closestPointOnSimplex = q;
|
||||
finalResult.m_usedVertices.reset();
|
||||
finalResult.m_usedVertices.usedVertexA = tempResult.m_usedVertices.usedVertexA;
|
||||
finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexB;
|
||||
finalResult.m_usedVertices.usedVertexB = tempResult.m_usedVertices.usedVertexC;
|
||||
finalResult.setBarycentricCoordinates(
|
||||
tempResult.m_barycentricCoords[VERTA],
|
||||
tempResult.m_barycentricCoords[VERTC],
|
||||
0,
|
||||
tempResult.m_barycentricCoords[VERTB]
|
||||
);
|
||||
|
||||
}
|
||||
}
|
||||
// Repeat test for face bdc
|
||||
|
||||
|
||||
if (pointOutsideBDC)
|
||||
{
|
||||
closestPtPointTriangle(p, b, d, c,tempResult);
|
||||
btPoint3 q = tempResult.m_closestPointOnSimplex;
|
||||
//convert result bitmask!
|
||||
btScalar sqDist = (q - p).dot( q - p);
|
||||
if (sqDist < bestSqDist)
|
||||
{
|
||||
bestSqDist = sqDist;
|
||||
finalResult.m_closestPointOnSimplex = q;
|
||||
finalResult.m_usedVertices.reset();
|
||||
finalResult.m_usedVertices.usedVertexB = tempResult.m_usedVertices.usedVertexA;
|
||||
finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexB;
|
||||
finalResult.m_usedVertices.usedVertexC = tempResult.m_usedVertices.usedVertexC;
|
||||
|
||||
finalResult.setBarycentricCoordinates(
|
||||
0,
|
||||
tempResult.m_barycentricCoords[VERTA],
|
||||
tempResult.m_barycentricCoords[VERTC],
|
||||
tempResult.m_barycentricCoords[VERTB]
|
||||
);
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
//help! we ended up full !
|
||||
|
||||
if (finalResult.m_usedVertices.usedVertexA &&
|
||||
finalResult.m_usedVertices.usedVertexB &&
|
||||
finalResult.m_usedVertices.usedVertexC &&
|
||||
finalResult.m_usedVertices.usedVertexD)
|
||||
{
|
||||
return true;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
@@ -0,0 +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
|
||||
@@ -0,0 +1 @@
|
||||
Empty placeholder for future Libspe2 SPU task
|
||||
Reference in New Issue
Block a user