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

x() -> x or getX() or [0]

Browse Source 
y() -> y or getY() or [1]
z() -> z or getZ() or [2]
w() -> w or getW() or [3]

make sphere-convex and sphere-compound collision work (still issues remaining)
This commit is contained in:
erwin coumans
2013-04-03 18:27:36 -07:00
parent 8cee2e9b23
commit 4a93c2e704
23 changed files with 692 additions and 315 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
opencl/basic_initialize/btOpenCLUtils.cpp
View File

@@ -525,6 +525,11 @@ cl_program btOpenCLUtils_compileCLProgramFromString(cl_context clContext, cl_dev
{
const char* additionalMacros = additionalMacrosArg?additionalMacrosArg:"";
if (disableBinaryCaching)
{
kernelSourceOrg = 0;
}
cl_program m_cpProgram=0;
cl_int status;

8
opencl/gpu_rigidbody/host/Solver.cpp
View File

@@ -344,8 +344,8 @@ void solveContact(btGpuConstraint4& cs,
btVector3 angImp0 = (invInertiaA* angular0)*rambdaDt;
btVector3 angImp1 = (invInertiaB* angular1)*rambdaDt;
#ifdef _WIN32
btAssert(_finite(linImp0.x()));
btAssert(_finite(linImp1.x()));
btAssert(_finite(linImp0.getX()));
btAssert(_finite(linImp1.getX()));
#endif
if( JACOBI )
{
@@ -427,8 +427,8 @@ void solveContact(btGpuConstraint4& cs,
btVector3 angImp0 = (invInertiaA* angular0)*rambdaDt;
btVector3 angImp1 = (invInertiaB* angular1)*rambdaDt;
#ifdef _WIN32
btAssert(_finite(linImp0.x()));
btAssert(_finite(linImp1.x()));
btAssert(_finite(linImp0.getX()));
btAssert(_finite(linImp1.getX()));
#endif
linVelA += linImp0;
angVelA += angImp0;

4
opencl/gpu_rigidbody/host/btConfig.h
View File

@@ -6,6 +6,7 @@ struct btConfig
int m_maxConvexBodies;
int m_maxConvexShapes;
int m_maxBroadphasePairs;
int m_maxContactCapacity;
int m_maxVerticesPerFace;
int m_maxFacesPerShape;
@@ -18,7 +19,7 @@ struct btConfig
int m_maxTriConvexPairCapacity;
btConfig()
:m_maxConvexBodies(32*1024),
:m_maxConvexBodies(128*1024),
m_maxConvexShapes(8192),
m_maxVerticesPerFace(64),
m_maxFacesPerShape(64),
@@ -29,6 +30,7 @@ struct btConfig
m_maxTriConvexPairCapacity(512*1024)
{
m_maxBroadphasePairs = 16*m_maxConvexBodies;
m_maxContactCapacity = m_maxBroadphasePairs;
}
};

8
opencl/gpu_rigidbody/host/btGpuJacobiSolver.cpp
View File

@@ -182,8 +182,8 @@ static __inline void solveContact(btGpuConstraint4& cs,
btVector3 angImp0 = (invInertiaA* angular0)*rambdaDt;
btVector3 angImp1 = (invInertiaB* angular1)*rambdaDt;
#ifdef _WIN32
btAssert(_finite(linImp0.x()));
btAssert(_finite(linImp1.x()));
btAssert(_finite(linImp0.getX()));
btAssert(_finite(linImp1.getX()));
#endif
if (invMassA)
@@ -304,8 +304,8 @@ static inline void solveFriction(btGpuConstraint4& cs,
btVector3 angImp0 = (invInertiaA* angular0)*rambdaDt;
btVector3 angImp1 = (invInertiaB* angular1)*rambdaDt;
#ifdef _WIN32
btAssert(_finite(linImp0.x()));
btAssert(_finite(linImp1.x()));
btAssert(_finite(linImp0.getX()));
btAssert(_finite(linImp1.getX()));
#endif
if (invMassA)
{

12
opencl/gpu_rigidbody/host/btGpuNarrowPhase.cpp
View File

@@ -103,9 +103,8 @@ m_queue(queue)
m_data->m_inertiaBufferCPU = new btAlignedObjectArray<btInertiaCL>();
m_data->m_inertiaBufferCPU->resize(config.m_maxConvexBodies);
m_data->m_pBufContactOutGPU = new btOpenCLArray<btContact4>(ctx,queue, config.m_maxBroadphasePairs,true);
btContact4 test = m_data->m_pBufContactOutGPU->forcedAt(0);
m_data->m_pBufContactOutGPU = new btOpenCLArray<btContact4>(ctx,queue, config.m_maxContactCapacity,true);
m_data->m_inertiaBufferGPU = new btOpenCLArray<btInertiaCL>(ctx,queue,config.m_maxConvexBodies,false);
m_data->m_collidablesGPU = new btOpenCLArray<btCollidable>(ctx,queue,config.m_maxConvexShapes);
@@ -601,9 +600,9 @@ int btGpuNarrowPhase::registerConcaveMeshShape(btAlignedObjectArray<btVector3>*
btVector3 normal = ((vert1-vert0).cross(vert2-vert0)).normalize();
btScalar c = -(normal.dot(vert0));
m_data->m_convexFaces[convex.m_faceOffset+i].m_plane[0] = normal.x();
m_data->m_convexFaces[convex.m_faceOffset+i].m_plane[1] = normal.y();
m_data->m_convexFaces[convex.m_faceOffset+i].m_plane[2] = normal.z();
m_data->m_convexFaces[convex.m_faceOffset+i].m_plane[0] = normal.getX();
m_data->m_convexFaces[convex.m_faceOffset+i].m_plane[1] = normal.getY();
m_data->m_convexFaces[convex.m_faceOffset+i].m_plane[2] = normal.getZ();
m_data->m_convexFaces[convex.m_faceOffset+i].m_plane[3] = c;
int indexOffset = m_data->m_convexIndices.size();
int numIndices = 3;
@@ -718,6 +717,7 @@ void btGpuNarrowPhase::computeContacts(cl_mem broadphasePairs, int numBroadphase
m_data->m_bodyBufferGPU,
m_data->m_pBufContactOutGPU,
nContactOut,
m_data->m_config.m_maxContactCapacity,
*m_data->m_convexPolyhedraGPU,
*m_data->m_convexVerticesGPU,
*m_data->m_uniqueEdgesGPU,

158
opencl/gpu_sat/host/ConvexHullContact.cpp
View File

@@ -53,6 +53,7 @@ m_totalContactsOut(m_context, m_queue)
m_totalContactsOut.push_back(0);
cl_int errNum=0;
bool disableKernelCaching = true;
if (1)
{
@@ -125,11 +126,15 @@ m_totalContactsOut(m_context, m_queue)
{
const char* primitiveContactsSrc = primitiveContactsKernelsCL;
cl_program primitiveContactsProg = btOpenCLUtils::compileCLProgramFromString(m_context,m_device,primitiveContactsSrc,&errNum,"","opencl/gpu_sat/kernels/primitiveContacts.cl");
cl_program primitiveContactsProg = btOpenCLUtils::compileCLProgramFromString(m_context,m_device,primitiveContactsSrc,&errNum,"","opencl/gpu_sat/kernels/primitiveContacts.cl",disableKernelCaching);
btAssert(errNum==CL_SUCCESS);
m_primitiveContactsKernel = btOpenCLUtils::compileCLKernelFromString(m_context, m_device,primitiveContactsSrc, "primitiveContactsKernel",&errNum,primitiveContactsProg,"");
btAssert(errNum==CL_SUCCESS);
m_processCompoundPairsPrimitivesKernel = btOpenCLUtils::compileCLKernelFromString(m_context, m_device,primitiveContactsSrc, "processCompoundPairsPrimitivesKernel",&errNum,primitiveContactsProg,"");
btAssert(errNum==CL_SUCCESS);
btAssert(m_processCompoundPairsPrimitivesKernel);
}
@@ -161,6 +166,9 @@ GpuSatCollision::~GpuSatCollision()
if (m_primitiveContactsKernel)
clReleaseKernel(m_primitiveContactsKernel);
if (m_processCompoundPairsPrimitivesKernel)
clReleaseKernel(m_processCompoundPairsPrimitivesKernel);
if (m_clipHullHullKernel)
clReleaseKernel(m_clipHullHullKernel);
if (m_clipCompoundsHullHullKernel)
@@ -203,59 +211,67 @@ float signedDistanceFromPointToPlane(const float4& point, const float4& planeEqn
inline bool IsPointInPolygon(const btVector3& p,
const btVector3& posConvex,
const btQuaternion& ornConvex,
const btGpuFace* face,
const btVector3* baseVertex,
const int* convexIndices,
btVector3* out)
#define cross3(a,b) (a.cross(b))
btVector3 transform(btVector3* v, const btVector3* pos, const btVector3* orn)
{
btVector3 a;
btVector3 b;
btVector3 ab;
btVector3 ap;
btVector3 v;
btTransform tr;
tr.setIdentity();
tr.setOrigin(*pos);
btQuaternion* o = (btQuaternion*) orn;
tr.setRotation(*o);
btVector3 res = tr(*v);
return res;
}
btVector3 plane (face->m_plane[0],face->m_plane[1],face->m_plane[2]);
inline bool IsPointInPolygon(const float4& p,
const btGpuFace* face,
const float4* baseVertex,
const int* convexIndices,
float4* out)
{
float4 a;
float4 b;
float4 ab;
float4 ap;
float4 v;
float4 plane = make_float4(face->m_plane.x,face->m_plane.y,face->m_plane.z,0.f);
if (face->m_numIndices<2)
return false;
btTransform tr;
tr.setIdentity();
tr.setOrigin(posConvex);
tr.setRotation(ornConvex);
float4 v0 = baseVertex[convexIndices[face->m_indexOffset + face->m_numIndices-1]];
btVector3 worldV0 = tr(v0);
b = worldV0;
b = v0;
for(unsigned i=0; i != face->m_numIndices; ++i)
{
a = b;
float4 vi = baseVertex[convexIndices[face->m_indexOffset + i]];
btVector3 worldVi = tr(vi);
b = worldVi;
b = vi;
ab = b-a;
ap = p-a;
v = ab.cross(plane);
v = cross3(ab,plane);
if (btDot(ap, v) > 0.f)
{
btScalar ab_m2 = btDot(ab, ab);
btScalar s = ab_m2 != btScalar(0.0) ? btDot(ab, ap) / ab_m2 : btScalar(0.0);
if (s <= btScalar(0.0))
float ab_m2 = btDot(ab, ab);
float rt = ab_m2 != 0.f ? btDot(ab, ap) / ab_m2 : 0.f;
if (rt <= 0.f)
{
*out = a;
}
else if (s >= btScalar(1.0))
else if (rt >= 1.f)
{
*out = b;
}
else
{
out->setInterpolate3(a,b,s);
float s = 1.f - rt;
out[0].x = s * a.x + rt * b.x;
out[0].y = s * a.y + rt * b.y;
out[0].z = s * a.z + rt * b.z;
}
return false;
}
@@ -264,7 +280,6 @@ inline bool IsPointInPolygon(const btVector3& p,
}
void computeContactSphereConvex(int pairIndex,
int bodyIndexA, int bodyIndexB,
int collidableIndexA, int collidableIndexB,
@@ -278,7 +293,7 @@ void computeContactSphereConvex(int pairIndex,
int& nGlobalContactsOut,
int maxContactCapacity)
{
float radius = collidables[collidableIndexA].m_radius;
float4 spherePos1 = rigidBodies[bodyIndexA].m_pos;
btQuaternion sphereOrn = rigidBodies[bodyIndexA].m_quat;
@@ -286,9 +301,18 @@ void computeContactSphereConvex(int pairIndex,
float4 pos = rigidBodies[bodyIndexB].m_pos;
float4 spherePos = spherePos1-pos;
btQuaternion quat = rigidBodies[bodyIndexB].m_quat;
btTransform tr;
tr.setIdentity();
tr.setOrigin(pos);
tr.setRotation(quat);
btTransform trInv = tr.inverse();
float4 spherePos = trInv(spherePos1);
int collidableIndex = rigidBodies[bodyIndexB].m_collidableIdx;
int shapeIndex = collidables[collidableIndex].m_shapeIndex;
int numFaces = convexShapes[shapeIndex].m_numFaces;
@@ -297,12 +321,13 @@ void computeContactSphereConvex(int pairIndex,
float minDist = -1000000.f; // TODO: What is the largest/smallest float?
bool bCollide = true;
int region = -1;
float4 localHitNormal;
for ( int f = 0; f < numFaces; f++ )
{
btGpuFace face = faces[convexShapes[shapeIndex].m_faceOffset+f];
float4 planeEqn;
float4 localPlaneNormal = make_float4(face.m_plane.x(),face.m_plane.y(),face.m_plane.z(),0.f);
float4 n1 = quatRotate(quat,localPlaneNormal);
float4 localPlaneNormal = make_float4(face.m_plane.getX(),face.m_plane.getY(),face.m_plane.getZ(),0.f);
float4 n1 = localPlaneNormal;//quatRotate(quat,localPlaneNormal);
planeEqn = n1;
planeEqn[3] = face.m_plane[3];
@@ -319,9 +344,8 @@ void computeContactSphereConvex(int pairIndex,
{
//might hit an edge or vertex
btVector3 out;
bool isInPoly = IsPointInPolygon(spherePos,
pos,
quat,
&face,
&convexVertices[convexShapes[shapeIndex].m_vertexOffset],
convexIndices,
@@ -332,7 +356,7 @@ void computeContactSphereConvex(int pairIndex,
{
minDist = dist;
closestPnt = pntReturn;
hitNormalWorld = planeEqn;
localHitNormal = planeEqn;
region=1;
}
} else
@@ -346,7 +370,7 @@ void computeContactSphereConvex(int pairIndex,
{
minDist = dist;
closestPnt = out;
hitNormalWorld = tmp/dist;
localHitNormal = tmp/dist;
region=2;
}
@@ -363,19 +387,23 @@ void computeContactSphereConvex(int pairIndex,
{
minDist = dist;
closestPnt = pntReturn;
hitNormalWorld = planeEqn;
localHitNormal = planeEqn;
region=3;
}
}
}
if (bCollide && minDist > -100)
static int numChecks = 0;
numChecks++;
if (bCollide && minDist > -10000)
{
float4 normalOnSurfaceB1 = -hitNormalWorld;
float4 pOnB1 = closestPnt+pos;
float4 normalOnSurfaceB1 = tr.getBasis()*-localHitNormal;//-hitNormalWorld;
float4 pOnB1 = tr(closestPnt);
//printf("dist ,%f,",minDist);
float actualDepth = minDist-radius;
if (actualDepth<0)
{
//printf("actualDepth = ,%f,", actualDepth);
//printf("normalOnSurfaceB1 = ,%f,%f,%f,", normalOnSurfaceB1.getX(),normalOnSurfaceB1.getY(),normalOnSurfaceB1.getZ());
//printf("region=,%d,\n", region);
@@ -401,6 +429,7 @@ void computeContactSphereConvex(int pairIndex,
int numPoints = 1;
c->m_worldNormal[3] = numPoints;
}//if (dstIdx < numPairs)
}
}//if (hasCollision)
}
@@ -409,7 +438,7 @@ void computeContactSphereConvex(int pairIndex,
void GpuSatCollision::computeConvexConvexContactsGPUSAT( const btOpenCLArray<btInt2>* pairs, int nPairs,
const btOpenCLArray<btRigidBodyCL>* bodyBuf,
btOpenCLArray<btContact4>* contactOut, int& nContacts,
int maxContactCapacity,
const btOpenCLArray<btConvexPolyhedronCL>& convexData,
const btOpenCLArray<btVector3>& gpuVertices,
const btOpenCLArray<btVector3>& gpuUniqueEdges,
@@ -488,7 +517,8 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( const btOpenCLArray<btI
if (hostCollidables[collidableIndexA].m_shapeType == SHAPE_SPHERE &&
hostCollidables[collidableIndexB].m_shapeType == SHAPE_CONVEX_HULL)
{
printf("sphere-convex\n");
computeContactSphereConvex(i,bodyIndexA,bodyIndexB,collidableIndexA,collidableIndexB,&hostBodyBuf[0],
&hostCollidables[0],&hostConvexData[0],&hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,nPairs);
}
if (hostCollidables[collidableIndexA].m_shapeType == SHAPE_CONVEX_HULL &&
@@ -534,6 +564,7 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( const btOpenCLArray<btI
btLauncherCL launcher(m_queue, m_primitiveContactsKernel);
launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(btBufferInfoCL) );
launcher.setConst( nPairs );
launcher.setConst(maxContactCapacity);
int num = nPairs;
launcher.launch1D( num);
clFinish(m_queue);
@@ -542,7 +573,9 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( const btOpenCLArray<btI
contactOut->resize(nContacts);
}
}
#endif//CHECK_ON_HOST
BT_PROFILE("computeConvexConvexContactsGPUSAT");
// printf("nContacts = %d\n",nContacts);
@@ -720,6 +753,39 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( const btOpenCLArray<btI
gpuCompoundSepNormals.resize(numCompoundPairs);
if (numCompoundPairs)
{
#ifndef CHECK_ON_HOST
BT_PROFILE("processCompoundPairsPrimitivesKernel");
btBufferInfoCL bInfo[] =
{
btBufferInfoCL( gpuCompoundPairs.getBufferCL(), true ),
btBufferInfoCL( bodyBuf->getBufferCL(),true),
btBufferInfoCL( gpuCollidables.getBufferCL(),true),
btBufferInfoCL( convexData.getBufferCL(),true),
btBufferInfoCL( gpuVertices.getBufferCL(),true),
btBufferInfoCL( gpuUniqueEdges.getBufferCL(),true),
btBufferInfoCL( gpuFaces.getBufferCL(),true),
btBufferInfoCL( gpuIndices.getBufferCL(),true),
btBufferInfoCL( clAabbsWS.getBufferCL(),true),
btBufferInfoCL( gpuChildShapes.getBufferCL(),true),
btBufferInfoCL( contactOut->getBufferCL()),
btBufferInfoCL( m_totalContactsOut.getBufferCL())
};
btLauncherCL launcher(m_queue, m_processCompoundPairsPrimitivesKernel);
launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(btBufferInfoCL) );
launcher.setConst( numCompoundPairs );
launcher.setConst(maxContactCapacity);
int num = numCompoundPairs;
launcher.launch1D( num);
clFinish(m_queue);
nContacts = m_totalContactsOut.at(0);
#endif
}
if (numCompoundPairs)
{

2
opencl/gpu_sat/host/ConvexHullContact.h
View File

@@ -53,6 +53,7 @@ struct GpuSatCollision
cl_kernel m_bvhTraversalKernel;
cl_kernel m_primitiveContactsKernel;
cl_kernel m_processCompoundPairsPrimitivesKernel;
btOpenCLArray<int> m_totalContactsOut;
@@ -64,6 +65,7 @@ struct GpuSatCollision
void computeConvexConvexContactsGPUSAT( const btOpenCLArray<btInt2>* pairs, int nPairs,
const btOpenCLArray<btRigidBodyCL>* bodyBuf,
btOpenCLArray<btContact4>* contactOut, int& nContacts,
int maxContactCapacity,
const btOpenCLArray<btConvexPolyhedronCL>& hostConvexData,
const btOpenCLArray<btVector3>& vertices,
const btOpenCLArray<btVector3>& uniqueEdges,

14
opencl/gpu_sat/host/btConvexUtility.cpp
View File

@@ -271,7 +271,7 @@ bool btConvexUtility::initializePolyhedralFeatures(const btVector3* orgVertices,
inline bool IsAlmostZero(const btVector3& v)
{
if(fabsf(v.x())>1e-6 || fabsf(v.y())>1e-6 || fabsf(v.z())>1e-6) return false;
if(fabsf(v.getX())>1e-6 || fabsf(v.getY())>1e-6 || fabsf(v.getZ())>1e-6) return false;
return true;
}
@@ -453,12 +453,12 @@ void btConvexUtility::initialize()
for(int i=0; i<m_vertices.size(); i++)
{
const btVector3& pt = m_vertices[i];
if(pt.x()<MinX) MinX = pt.x();
if(pt.x()>MaxX) MaxX = pt.x();
if(pt.y()<MinY) MinY = pt.y();
if(pt.y()>MaxY) MaxY = pt.y();
if(pt.z()<MinZ) MinZ = pt.z();
if(pt.z()>MaxZ) MaxZ = pt.z();
if(pt.getX()<MinX) MinX = pt.getX();
if(pt.getX()>MaxX) MaxX = pt.getX();
if(pt.getY()<MinY) MinY = pt.getY();
if(pt.getY()>MaxY) MaxY = pt.getY();
if(pt.getZ()<MinZ) MinZ = pt.getZ();
if(pt.getZ()>MaxZ) MaxZ = pt.getZ();
}
mC.setValue(MaxX+MinX, MaxY+MinY, MaxZ+MinZ);
mE.setValue(MaxX-MinX, MaxY-MinY, MaxZ-MinZ);

18
opencl/gpu_sat/host/btOptimizedBvh.cpp
View File

@@ -116,20 +116,20 @@ void btOptimizedBvh::build(btStridingMeshInterface* triangles, bool useQuantized
//PCK: add these checks for zero dimensions of aabb
const btScalar MIN_AABB_DIMENSION = btScalar(0.002);
const btScalar MIN_AABB_HALF_DIMENSION = btScalar(0.001);
if (aabbMax.x() - aabbMin.x() < MIN_AABB_DIMENSION)
if (aabbMax.getX() - aabbMin.getX() < MIN_AABB_DIMENSION)
{
aabbMax.setX(aabbMax.x() + MIN_AABB_HALF_DIMENSION);
aabbMin.setX(aabbMin.x() - MIN_AABB_HALF_DIMENSION);
aabbMax.setX(aabbMax.getX() + MIN_AABB_HALF_DIMENSION);
aabbMin.setX(aabbMin.getX() - MIN_AABB_HALF_DIMENSION);
}
if (aabbMax.y() - aabbMin.y() < MIN_AABB_DIMENSION)
if (aabbMax.getY() - aabbMin.getY() < MIN_AABB_DIMENSION)
{
aabbMax.setY(aabbMax.y() + MIN_AABB_HALF_DIMENSION);
aabbMin.setY(aabbMin.y() - MIN_AABB_HALF_DIMENSION);
aabbMax.setY(aabbMax.getY() + MIN_AABB_HALF_DIMENSION);
aabbMin.setY(aabbMin.getY() - MIN_AABB_HALF_DIMENSION);
}
if (aabbMax.z() - aabbMin.z() < MIN_AABB_DIMENSION)
if (aabbMax.getZ() - aabbMin.getZ() < MIN_AABB_DIMENSION)
{
aabbMax.setZ(aabbMax.z() + MIN_AABB_HALF_DIMENSION);
aabbMin.setZ(aabbMin.z() - MIN_AABB_HALF_DIMENSION);
aabbMax.setZ(aabbMax.getZ() + MIN_AABB_HALF_DIMENSION);
aabbMin.setZ(aabbMin.getZ() - MIN_AABB_HALF_DIMENSION);
}
m_optimizedTree->quantize(&node.m_quantizedAabbMin[0],aabbMin,0);

237
opencl/gpu_sat/kernels/primitiveContacts.cl
View File

@@ -51,6 +51,21 @@ typedef struct
int m_bodyBPtrAndSignBit;
} Contact4;
typedef struct
{
union
{
float4 m_min;
float m_minElems[4];
int m_minIndices[4];
};
union
{
float4 m_max;
float m_maxElems[4];
int m_maxIndices[4];
};
} btAabbCL;
///keep this in sync with btCollidable.h
typedef struct
@@ -273,8 +288,6 @@ float signedDistanceFromPointToPlane(float4 point, float4 planeEqn, float4* clos
inline bool IsPointInPolygon(float4 p,
float4 posConvex,
float4 ornConvex,
const btGpuFace* face,
__global const float4* baseVertex,
__global const int* convexIndices,
@@ -293,16 +306,14 @@ inline bool IsPointInPolygon(float4 p,
float4 v0 = baseVertex[convexIndices[face->m_indexOffset + face->m_numIndices-1]];
float4 worldV0 = transform(&v0, &posConvex, &ornConvex);
b = worldV0;
b = v0;
for(unsigned i=0; i != face->m_numIndices; ++i)
{
a = b;
float4 vi = baseVertex[convexIndices[face->m_indexOffset + i]];
float4 worldVi = transform(&vi, &posConvex, &ornConvex);
b = worldVi;
b = vi;
ab = b-a;
ap = p-a;
v = cross3(ab,plane);
@@ -322,9 +333,9 @@ inline bool IsPointInPolygon(float4 p,
else
{
float s = 1.f - rt;
out[0].x = s * a.x + rt * b.x;
out[0].y = s * a.y + rt * b.y;
out[0].z = s * a.z + rt * b.z;
out[0].x = s * a.x + rt * b.x;
out[0].y = s * a.y + rt * b.y;
out[0].z = s * a.z + rt * b.z;
}
return false;
}
@@ -346,22 +357,22 @@ void computeContactSphereConvex(int pairIndex,
__global const btGpuFace* faces,
__global Contact4* restrict globalContactsOut,
counter32_t nGlobalContactsOut,
int numPairs)
int maxContactCapacity,
float4 spherePos2,
float radius,
float4 pos,
float4 quat
)
{
float radius = collidables[collidableIndexA].m_radius;
float4 spherePos1 = rigidBodies[bodyIndexA].m_pos;
float4 sphereOrn = rigidBodies[bodyIndexA].m_quat;
float4 invPos;
float4 invOrn;
trInverse(pos,quat, &invPos,&invOrn);
float4 spherePos = transform(&spherePos2,&invPos,&invOrn);
float4 pos = rigidBodies[bodyIndexB].m_pos;
float4 quat = rigidBodies[bodyIndexB].m_quat;
float4 spherePos = spherePos1 - pos;
int collidableIndex = rigidBodies[bodyIndexB].m_collidableIdx;
int shapeIndex = collidables[collidableIndex].m_shapeIndex;
int shapeIndex = collidables[collidableIndexB].m_shapeIndex;
int numFaces = convexShapes[shapeIndex].m_numFaces;
float4 closestPnt = (float4)(0, 0, 0, 0);
float4 hitNormalWorld = (float4)(0, 0, 0, 0);
@@ -374,7 +385,8 @@ void computeContactSphereConvex(int pairIndex,
// set up a plane equation
float4 planeEqn;
float4 n1 = qtRotate(quat, (float4)(face.m_plane.xyz, 0));
float4 n1 = face.m_plane;
n1.w = 0.f;
planeEqn = n1;
planeEqn.w = face.m_plane.w;
@@ -395,9 +407,9 @@ void computeContactSphereConvex(int pairIndex,
{
//might hit an edge or vertex
float4 out;
float4 zeroPos = make_float4(0,0,0,0);
bool isInPoly = IsPointInPolygon(spherePos,
pos,
quat,
&face,
&convexVertices[convexShapes[shapeIndex].m_vertexOffset],
convexIndices,
@@ -446,27 +458,36 @@ void computeContactSphereConvex(int pairIndex,
if (bCollide)
if (bCollide && minDist > -10000)
{
float4 normalOnSurfaceB1 = -hitNormalWorld;
float4 pOnB1 = closestPnt+pos;
float4 normalOnSurfaceB1 = qtRotate(quat,-hitNormalWorld);
float4 pOnB1 = transform(&closestPnt,&pos,&quat);
//printf("pOnB1=%f,%f,%f\n", pOnB1.x,pOnB1.y,pOnB1.z);
float actualDepth = minDist-radius;
pOnB1.w = actualDepth;
int dstIdx;
AppendInc( nGlobalContactsOut, dstIdx );
if (dstIdx < numPairs)
if (actualDepth<=0.f)
{
__global Contact4* c = &globalContactsOut[dstIdx];
c->m_worldNormal = normalOnSurfaceB1;
c->m_coeffs = (u32)(0.f*0xffff) | ((u32)(0.7f*0xffff)<<16);
c->m_batchIdx = pairIndex;
c->m_bodyAPtrAndSignBit = rigidBodies[bodyIndexA].m_invMass==0?-bodyIndexA:bodyIndexA;
c->m_bodyBPtrAndSignBit = rigidBodies[bodyIndexB].m_invMass==0?-bodyIndexB:bodyIndexB;
c->m_worldPos[0] = pOnB1;
GET_NPOINTS(*c) = 1;
}//if (dstIdx < numPairs)
//printf("actualDepth = %f\n", actualDepth );
//printf("normalOnSurfaceB1 = ,%f,%f,%f,", normalOnSurfaceB1.x,normalOnSurfaceB1.y,normalOnSurfaceB1.z);
pOnB1.w = actualDepth;
int dstIdx;
AppendInc( nGlobalContactsOut, dstIdx );
//printf("maxContactCapacity=%d\n", maxContactCapacity);
if (1)//dstIdx < maxContactCapacity)
{
__global Contact4* c = &globalContactsOut[dstIdx];
c->m_worldNormal = normalOnSurfaceB1;
c->m_coeffs = (u32)(0.f*0xffff) | ((u32)(0.7f*0xffff)<<16);
c->m_batchIdx = pairIndex;
c->m_bodyAPtrAndSignBit = rigidBodies[bodyIndexA].m_invMass==0?-bodyIndexA:bodyIndexA;
c->m_bodyBPtrAndSignBit = rigidBodies[bodyIndexB].m_invMass==0?-bodyIndexB:bodyIndexB;
c->m_worldPos[0] = pOnB1;
GET_NPOINTS(*c) = 1;
}
}
}//if (hasCollision)
}
@@ -481,7 +502,7 @@ void computeContactPlaneConvex(int pairIndex,
__global const btGpuFace* faces,
__global Contact4* restrict globalContactsOut,
counter32_t nGlobalContactsOut,
int numPairs)
int maxContactCapacity)
{
float4 planeEq = faces[collidables[collidableIndexA].m_shapeIndex].m_plane;
float radius = collidables[collidableIndexB].m_radius;
@@ -520,7 +541,7 @@ void computeContactPlaneConvex(int pairIndex,
int dstIdx;
AppendInc( nGlobalContactsOut, dstIdx );
if (dstIdx < numPairs)
if (dstIdx < maxContactCapacity)
{
__global Contact4* c = &globalContactsOut[dstIdx];
c->m_worldNormal = normalOnSurfaceB1;
@@ -547,7 +568,7 @@ __kernel void primitiveContactsKernel( __global const int2* pairs,
__global const int* indices,
__global Contact4* restrict globalContactsOut,
counter32_t nGlobalContactsOut,
int numPairs)
int numPairs, int maxContactCapacity)
{
int i = get_global_id(0);
@@ -579,7 +600,7 @@ __kernel void primitiveContactsKernel( __global const int2* pairs,
computeContactPlaneConvex( pairIndex, bodyIndexB,bodyIndexA, collidableIndexB,collidableIndexA,
rigidBodies,collidables,faces, globalContactsOut, nGlobalContactsOut,numPairs);
rigidBodies,collidables,faces, globalContactsOut, nGlobalContactsOut,maxContactCapacity);
return;
}
@@ -589,7 +610,7 @@ __kernel void primitiveContactsKernel( __global const int2* pairs,
computeContactPlaneConvex(pairIndex, bodyIndexA, bodyIndexB, collidableIndexA, collidableIndexB,
rigidBodies,collidables,faces, globalContactsOut, nGlobalContactsOut,numPairs);
rigidBodies,collidables,faces, globalContactsOut, nGlobalContactsOut,maxContactCapacity);
return;
}
@@ -598,8 +619,15 @@ __kernel void primitiveContactsKernel( __global const int2* pairs,
collidables[collidableIndexB].m_shapeType == SHAPE_CONVEX_HULL)
{
float4 spherePos = rigidBodies[bodyIndexA].m_pos;
float sphereRadius = collidables[collidableIndexA].m_radius;
float4 convexPos = rigidBodies[bodyIndexB].m_pos;
float4 convexOrn = rigidBodies[bodyIndexB].m_quat;
computeContactSphereConvex(pairIndex, bodyIndexA, bodyIndexB, collidableIndexA, collidableIndexB,
rigidBodies,collidables,convexShapes,vertices,indices,faces, globalContactsOut, nGlobalContactsOut,numPairs);
rigidBodies,collidables,convexShapes,vertices,indices,faces, globalContactsOut, nGlobalContactsOut,maxContactCapacity,
spherePos,sphereRadius,convexPos,convexOrn);
return;
}
@@ -607,8 +635,14 @@ __kernel void primitiveContactsKernel( __global const int2* pairs,
collidables[collidableIndexB].m_shapeType == SHAPE_SPHERE)
{
float4 spherePos = rigidBodies[bodyIndexB].m_pos;
float sphereRadius = collidables[collidableIndexB].m_radius;
float4 convexPos = rigidBodies[bodyIndexA].m_pos;
float4 convexOrn = rigidBodies[bodyIndexA].m_quat;
computeContactSphereConvex(pairIndex, bodyIndexB, bodyIndexA, collidableIndexB, collidableIndexA,
rigidBodies,collidables,convexShapes,vertices,indices,faces, globalContactsOut, nGlobalContactsOut,numPairs);
rigidBodies,collidables,convexShapes,vertices,indices,faces, globalContactsOut, nGlobalContactsOut,maxContactCapacity,
spherePos,sphereRadius,convexPos,convexOrn);
return;
}
@@ -642,9 +676,9 @@ __kernel void primitiveContactsKernel( __global const int2* pairs,
contactPosB.w = dist;
int dstIdx;
AppendInc( nGlobalContactsOut, dstIdx );
AppendInc( nGlobalContactsOut, dstIdx );
if (dstIdx < numPairs)
if (dstIdx < maxContactCapacity)
{
__global Contact4* c = &globalContactsOut[dstIdx];
c->m_worldNormal = -normalOnSurfaceB;
@@ -665,3 +699,106 @@ __kernel void primitiveContactsKernel( __global const int2* pairs,
}// if (i<numPairs)
}
// work-in-progress
__kernel void processCompoundPairsPrimitivesKernel( __global const int4* gpuCompoundPairs,
__global const BodyData* rigidBodies,
__global const btCollidableGpu* collidables,
__global const ConvexPolyhedronCL* convexShapes,
__global const float4* vertices,
__global const float4* uniqueEdges,
__global const btGpuFace* faces,
__global const int* indices,
__global btAabbCL* aabbs,
__global const btGpuChildShape* gpuChildShapes,
__global Contact4* restrict globalContactsOut,
counter32_t nGlobalContactsOut,
int numCompoundPairs, int maxContactCapacity
)
{
int i = get_global_id(0);
if (i<numCompoundPairs)
{
int bodyIndexA = gpuCompoundPairs[i].x;
int bodyIndexB = gpuCompoundPairs[i].y;
int childShapeIndexA = gpuCompoundPairs[i].z;
int childShapeIndexB = gpuCompoundPairs[i].w;
int collidableIndexA = -1;
int collidableIndexB = -1;
float4 ornA = rigidBodies[bodyIndexA].m_quat;
float4 posA = rigidBodies[bodyIndexA].m_pos;
float4 ornB = rigidBodies[bodyIndexB].m_quat;
float4 posB = rigidBodies[bodyIndexB].m_pos;
if (childShapeIndexA >= 0)
{
collidableIndexA = gpuChildShapes[childShapeIndexA].m_shapeIndex;
float4 childPosA = gpuChildShapes[childShapeIndexA].m_childPosition;
float4 childOrnA = gpuChildShapes[childShapeIndexA].m_childOrientation;
float4 newPosA = qtRotate(ornA,childPosA)+posA;
float4 newOrnA = qtMul(ornA,childOrnA);
posA = newPosA;
ornA = newOrnA;
} else
{
collidableIndexA = rigidBodies[bodyIndexA].m_collidableIdx;
}
if (childShapeIndexB>=0)
{
collidableIndexB = gpuChildShapes[childShapeIndexB].m_shapeIndex;
float4 childPosB = gpuChildShapes[childShapeIndexB].m_childPosition;
float4 childOrnB = gpuChildShapes[childShapeIndexB].m_childOrientation;
float4 newPosB = transform(&childPosB,&posB,&ornB);
float4 newOrnB = qtMul(ornB,childOrnB);
posB = newPosB;
ornB = newOrnB;
} else
{
collidableIndexB = rigidBodies[bodyIndexB].m_collidableIdx;
}
int shapeIndexA = collidables[collidableIndexA].m_shapeIndex;
int shapeIndexB = collidables[collidableIndexB].m_shapeIndex;
int shapeTypeA = collidables[collidableIndexA].m_shapeType;
int shapeTypeB = collidables[collidableIndexB].m_shapeType;
int pairIndex = i;
if ((shapeTypeA == SHAPE_CONVEX_HULL) && (shapeTypeB == SHAPE_SPHERE))
{
float4 spherePos = rigidBodies[bodyIndexB].m_pos;
float sphereRadius = collidables[collidableIndexB].m_radius;
float4 convexPos = posA;
float4 convexOrn = ornA;
//printf("convex-sphere with radius %f\n",sphereRadius);
computeContactSphereConvex(pairIndex, bodyIndexB, bodyIndexA , collidableIndexB,collidableIndexA,
rigidBodies,collidables,convexShapes,vertices,indices,faces, globalContactsOut, nGlobalContactsOut,maxContactCapacity,
spherePos,sphereRadius,convexPos,convexOrn);
return;
}
if ((shapeTypeA == SHAPE_SPHERE) && (shapeTypeB == SHAPE_CONVEX_HULL))
{
float4 spherePos = rigidBodies[bodyIndexA].m_pos;
float sphereRadius = collidables[collidableIndexA].m_radius;
float4 convexPos = posB;
float4 convexOrn = ornB;
//printf("sphere-convex with radius %f\n", sphereRadius);
computeContactSphereConvex(pairIndex, bodyIndexA, bodyIndexB, collidableIndexA, collidableIndexB,
rigidBodies,collidables,convexShapes,vertices,indices,faces, globalContactsOut, nGlobalContactsOut,maxContactCapacity,
spherePos,sphereRadius,convexPos,convexOrn);
return;
}
}// if (i<numCompoundPairs)
}

9
opencl/gpu_sat/kernels/sat.cl
View File

@@ -706,6 +706,15 @@ __kernel void processCompoundPairsKernel( __global const int4* gpuCompoundPair
int shapeIndexA = collidables[collidableIndexA].m_shapeIndex;
int shapeIndexB = collidables[collidableIndexB].m_shapeIndex;
int shapeTypeA = collidables[collidableIndexA].m_shapeType;
int shapeTypeB = collidables[collidableIndexB].m_shapeType;
if ((shapeTypeA != SHAPE_CONVEX_HULL) || (shapeTypeB != SHAPE_CONVEX_HULL))
{
return;
}
int hasSeparatingAxis = 5;
int numFacesA = convexShapes[shapeIndexA].m_numFaces;