Share btGjkPairDetector, btGjkEpa2, btVoronoiSimplexSolver with SPU/Multithreaded implementation (remove duplicate code)
Make btTypedConstraint and btPersistentManifold both derive from btTypedObject to make SPU-side generic constraint solver easier. Note: all build systems need to be updated: remove SpuVoronoiSimplexSolver.cpp, SpuGjkPairDetector.cpp, SpuEpaPenetrationDepthSolver.cpp, SpuGjkEpa2.cpp
This commit is contained in:
erwin.coumans
@@ -28,253 +28,6 @@ static inline vec_float4 vec_dot3( vec_float4 vec0, vec_float4 vec1 )
|
||||
}
|
||||
#endif //__SPU__
|
||||
|
||||
btVector3 localGetSupportingVertexWithoutMargin(int shapeType, void* shape, const btVector3& localDir,struct SpuConvexPolyhedronVertexData* convexVertexData)//, int *featureIndex)
|
||||
{
|
||||
switch (shapeType)
|
||||
{
|
||||
case SPHERE_SHAPE_PROXYTYPE:
|
||||
{
|
||||
return btVector3(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 btVector3(
|
||||
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 btVector3(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 btVector3(tmp.getX(),tmp.getY(),tmp.getZ());
|
||||
}
|
||||
else
|
||||
{
|
||||
tmp[XX] = radius;
|
||||
tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight;
|
||||
tmp[ZZ] = btScalar(0.0);
|
||||
return btVector3(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(-BT_LARGE_FLOAT));
|
||||
|
||||
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 btVector3(supVec.getX(),supVec.getY(),supVec.getZ());
|
||||
break;
|
||||
};
|
||||
|
||||
case CONVEX_HULL_SHAPE_PROXYTYPE:
|
||||
{
|
||||
//spu_printf("SPU: todo: getSupport CONVEX_HULL_SHAPE_PROXYTYPE\n");
|
||||
|
||||
#if defined (__CELLOS_LV2__) && defined (__SPU__)
|
||||
vec_float4 v_distMax = {-FLT_MAX,0,0,0};
|
||||
vec_int4 v_idxMax = {-999,0,0,0};
|
||||
int v=0;
|
||||
int numverts = convexVertexData->gNumConvexPoints;
|
||||
btVector3* points = convexVertexData->gConvexPoints;
|
||||
|
||||
for(;v<(int)numverts-4;v+=4) {
|
||||
vec_float4 p0 = vec_dot3(points[v ].get128(),localDir.get128());
|
||||
vec_float4 p1 = vec_dot3(points[v+1].get128(),localDir.get128());
|
||||
vec_float4 p2 = vec_dot3(points[v+2].get128(),localDir.get128());
|
||||
vec_float4 p3 = vec_dot3(points[v+3].get128(),localDir.get128());
|
||||
const vec_int4 i0 = {v ,0,0,0};
|
||||
const vec_int4 i1 = {v+1,0,0,0};
|
||||
const vec_int4 i2 = {v+2,0,0,0};
|
||||
const vec_int4 i3 = {v+3,0,0,0};
|
||||
vec_uint4 retGt01 = spu_cmpgt(p0,p1);
|
||||
vec_float4 pmax01 = spu_sel(p1,p0,retGt01);
|
||||
vec_int4 imax01 = spu_sel(i1,i0,retGt01);
|
||||
vec_uint4 retGt23 = spu_cmpgt(p2,p3);
|
||||
vec_float4 pmax23 = spu_sel(p3,p2,retGt23);
|
||||
vec_int4 imax23 = spu_sel(i3,i2,retGt23);
|
||||
vec_uint4 retGt0123 = spu_cmpgt(pmax01,pmax23);
|
||||
vec_float4 pmax0123 = spu_sel(pmax23,pmax01,retGt0123);
|
||||
vec_int4 imax0123 = spu_sel(imax23,imax01,retGt0123);
|
||||
vec_uint4 retGtMax = spu_cmpgt(v_distMax,pmax0123);
|
||||
v_distMax = spu_sel(pmax0123,v_distMax,retGtMax);
|
||||
v_idxMax = spu_sel(imax0123,v_idxMax,retGtMax);
|
||||
}
|
||||
for(;v<(int)numverts;v++) {
|
||||
vec_float4 p = vec_dot3(points[v].get128(),localDir.get128());
|
||||
const vec_int4 i = {v,0,0,0};
|
||||
vec_uint4 retGtMax = spu_cmpgt(v_distMax,p);
|
||||
v_distMax = spu_sel(p,v_distMax,retGtMax);
|
||||
v_idxMax = spu_sel(i,v_idxMax,retGtMax);
|
||||
}
|
||||
int ptIndex = spu_extract(v_idxMax,0);
|
||||
const btVector3& supVec= points[ptIndex];
|
||||
#else
|
||||
|
||||
btVector3* points = 0;
|
||||
int numPoints = 0;
|
||||
points = convexVertexData->gConvexPoints;
|
||||
numPoints = convexVertexData->gNumConvexPoints;
|
||||
|
||||
// spu_printf("numPoints = %d\n",numPoints);
|
||||
|
||||
int ptIndex = 0;
|
||||
btScalar newDot,maxDot = btScalar(-BT_LARGE_FLOAT);
|
||||
|
||||
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++)
|
||||
{
|
||||
const btVector3& vtx = points[i];// * m_localScaling;
|
||||
|
||||
newDot = vec.dot(vtx);
|
||||
if (newDot > maxDot)
|
||||
{
|
||||
maxDot = newDot;
|
||||
ptIndex = i;
|
||||
}
|
||||
}
|
||||
const btVector3& supVec= points[ptIndex];
|
||||
|
||||
#endif
|
||||
return btVector3(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 btVector3(0.f, 0.f, 0.f);
|
||||
}
|
||||
}
|
||||
|
||||
void computeAabb (btVector3& aabbMin, btVector3& aabbMax, btConvexInternalShape* convexShape, ppu_address_t convexShapePtr, int shapeType, const btTransform& xform)
|
||||
{
|
||||
|
||||
Reference in New Issue
Block a user