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

support compound versus compound collision shape acceleration on GPU, using aabb tree versus aabb tree.

Browse Source 
Remove constructor from b3Vector3,  to make it a POD type, so it can go into a union (and more compatible with OpenCL float4)
Use b3MakeVector3 instead of constructor
Share some code between C++ and GPU in a shared file: see b3TransformAabb2 in src/Bullet3Collision/BroadPhaseCollision/shared/b3Aabb.h
Improve PairBench a bit, show timings and #overlapping pairs.
Increase shadowmap default size to 8192x8192 (hope the GPU supports it)
This commit is contained in:
erwincoumans
2013-08-20 03:19:59 -07:00
parent 41ba48b10d
commit 677722bba3
62 changed files with 1827 additions and 564 deletions
Download Patch File Download Diff File Expand all files Collapse all files

239
src/Bullet3OpenCL/NarrowphaseCollision/b3ConvexHullContact.cpp
View File

@@ -252,7 +252,7 @@ struct MyTriangleCallback : public b3NodeOverlapCallback
#define float4 b3Vector3
#define make_float4(x,y,z,w) b3Vector4(x,y,z,w)
#define make_float4(x,y,z,w) b3MakeVector3(x,y,z,w)
float signedDistanceFromPointToPlane(const float4& point, const float4& planeEqn, float4* closestPointOnFace)
{
@@ -289,7 +289,7 @@ inline bool IsPointInPolygon(const float4& p,
float4 ap;
float4 v;
float4 plane = make_float4(face->m_plane.x,face->m_plane.y,face->m_plane.z,0.f);
float4 plane = b3MakeVector3(face->m_plane.x,face->m_plane.y,face->m_plane.z,0.f);
if (face->m_numIndices<2)
return false;
@@ -347,7 +347,7 @@ int extractManifoldSequentialGlobal( const float4* p, int nPoints, const float4&
if (nPoints >64)
nPoints = 64;
float4 center = make_float4(0,0,0,0);
float4 center = b3MakeVector3(0,0,0,0);
{
for (int i=0;i<nPoints;i++)
@@ -724,7 +724,7 @@ bool findSeparatingAxisEdgeEdge( __global const b3ConvexPolyhedronData* hullA, _
__inline float4 lerp3(const float4& a,const float4& b, float t)
{
return make_float4( a.x + (b.x - a.x) * t,
return b3MakeVector3( a.x + (b.x - a.x) * t,
a.y + (b.y - a.y) * t,
a.z + (b.z - a.z) * t,
0.f);
@@ -803,7 +803,7 @@ int clipFaceAgainstHull(const float4& separatingNormal, const b3ConvexPolyhedron
float dmin = FLT_MAX;
for(int face=0;face<hullA->m_numFaces;face++)
{
const float4 Normal = make_float4(
const float4 Normal = b3MakeVector3(
facesA[hullA->m_faceOffset+face].m_plane.x,
facesA[hullA->m_faceOffset+face].m_plane.y,
facesA[hullA->m_faceOffset+face].m_plane.z,0.f);
@@ -873,7 +873,7 @@ int clipFaceAgainstHull(const float4& separatingNormal, const b3ConvexPolyhedron
{
float4 pointInWorld = pVtxIn[i];
//resultOut.addContactPoint(separatingNormal,point,depth);
contactsOut[numContactsOut++] = make_float4(pointInWorld.x,pointInWorld.y,pointInWorld.z,depth);
contactsOut[numContactsOut++] = b3MakeVector3(pointInWorld.x,pointInWorld.y,pointInWorld.z,depth);
//printf("depth=%f\n",depth);
}
} else
@@ -934,7 +934,7 @@ static int clipHullAgainstHull(const float4& separatingNormal,
#endif //BT_DEBUG_SAT_FACE
//if (facesB[hullB.m_faceOffset+face].m_numIndices>2)
{
const float4 Normal = make_float4(facesB[hullB.m_faceOffset+face].m_plane.x,
const float4 Normal = b3MakeVector3(facesB[hullB.m_faceOffset+face].m_plane.x,
facesB[hullB.m_faceOffset+face].m_plane.y, facesB[hullB.m_faceOffset+face].m_plane.z,0.f);
const float4 WorldNormal = b3QuatRotate(ornB, Normal);
#ifdef BT_DEBUG_SAT_FACE
@@ -1145,7 +1145,7 @@ int clipHullHullSingle(
float4 worldVertsB2[MAX_VERTS];
int capacityWorldVerts = MAX_VERTS;
float4 hostNormal = make_float4(sepNormalWorldSpace.getX(),sepNormalWorldSpace.getY(),sepNormalWorldSpace.getZ(),0.f);
float4 hostNormal = make_float4(sepNormalWorldSpace.x,sepNormalWorldSpace.y,sepNormalWorldSpace.z,0.f);
int shapeA = hostCollidablesA[collidableIndexA].m_shapeIndex;
int shapeB = hostCollidablesB[collidableIndexB].m_shapeIndex;
@@ -1158,11 +1158,11 @@ int clipHullHullSingle(
{
//B3_PROFILE("transform computation");
//trA.setIdentity();
trA.setOrigin(b3Vector3(posA.x,posA.y,posA.z));
trA.setOrigin(b3MakeVector3(posA.x,posA.y,posA.z));
trA.setRotation(b3Quaternion(ornA.x,ornA.y,ornA.z,ornA.w));
//trB.setIdentity();
trB.setOrigin(b3Vector3(posB.x,posB.y,posB.z));
trB.setOrigin(b3MakeVector3(posB.x,posB.y,posB.z));
trB.setRotation(b3Quaternion(ornB.x,ornB.y,ornB.z,ornB.w));
}
@@ -1263,7 +1263,7 @@ void computeContactPlaneConvex(int pairIndex,
int numWorldVertsB1= 0;
b3Vector3 planeEq = faces[collidables[collidableIndexA].m_shapeIndex].m_plane;
b3Vector3 planeNormal(planeEq.x,planeEq.y,planeEq.z);
b3Vector3 planeNormal=b3MakeVector3(planeEq.x,planeEq.y,planeEq.z);
b3Vector3 planeNormalWorld = b3QuatRotate(ornA,planeNormal);
float planeConstant = planeEq.w;
b3Transform convexWorldTransform;
@@ -1358,7 +1358,7 @@ void computeContactPlaneConvex(int pairIndex,
b3Vector3 pOnB1 = contactPoints[contactIdx.s[i]];
c->m_worldPosB[i] = pOnB1;
}
c->m_worldNormalOnB[3] = (b3Scalar)numReducedPoints;
c->m_worldNormalOnB.w = (b3Scalar)numReducedPoints;
}//if (dstIdx < numPairs)
}
@@ -1373,9 +1373,9 @@ B3_FORCE_INLINE b3Vector3 MyUnQuantize(const unsigned short* vecIn, const b3Vect
{
b3Vector3 vecOut;
vecOut.setValue(
(b3Scalar)(vecIn[0]) / (quantization.getX()),
(b3Scalar)(vecIn[1]) / (quantization.getY()),
(b3Scalar)(vecIn[2]) / (quantization.getZ()));
(b3Scalar)(vecIn[0]) / (quantization.x),
(b3Scalar)(vecIn[1]) / (quantization.y),
(b3Scalar)(vecIn[2]) / (quantization.z));
vecOut += bvhAabbMin;
return vecOut;
}
@@ -1385,6 +1385,11 @@ void traverseTreeTree()
}
#include "Bullet3Common/shared/b3Mat3x3.h"
int numAabbChecks = 0;
int maxNumAabbChecks = 0;
int maxDepth = 0;
// work-in-progress
__kernel void findCompoundPairsKernel(
@@ -1408,8 +1413,8 @@ __kernel void findCompoundPairsKernel(
b3AlignedObjectArray<b3BvhInfo>& bvhInfoCPU
)
{
numAabbChecks=0;
maxNumAabbChecks=0;
int i = pairIndex;
{
@@ -1462,7 +1467,7 @@ __kernel void findCompoundPairsKernel(
b3Vector3 aabbAMinOut,aabbAMaxOut;
float margin=0.f;
b3TransformAabb(treeAminLocal,treeAmaxLocal, margin,transA,aabbAMinOut,aabbAMaxOut);
b3TransformAabb2(treeAminLocal,treeAmaxLocal, margin,transA.getOrigin(),transA.getRotation(),&aabbAMinOut,&aabbAMaxOut);
for (int q=0;q<numSubTreesB;q++)
{
@@ -1473,17 +1478,19 @@ __kernel void findCompoundPairsKernel(
b3Vector3 aabbBMinOut,aabbBMaxOut;
float margin=0.f;
b3TransformAabb(treeBminLocal,treeBmaxLocal, margin,transB,aabbBMinOut,aabbBMaxOut);
b3TransformAabb2(treeBminLocal,treeBmaxLocal, margin,transB.getOrigin(),transB.getRotation(),&aabbBMinOut,&aabbBMaxOut);
bool aabbOverlap = b3TestAabbAgainstAabb2(aabbAMinOut,aabbAMaxOut,aabbBMinOut,aabbBMaxOut);
numAabbChecks=0;
bool aabbOverlap = b3TestAabbAgainstAabb(aabbAMinOut,aabbAMaxOut,aabbBMinOut,aabbBMaxOut);
if (aabbOverlap)
{
int startNodeIndexA = subtreeA.m_rootNodeIndex;
int endNodeIndexA = subtreeA.m_rootNodeIndex+subtreeA.m_subtreeSize;
int startNodeIndexA = subtreeA.m_rootNodeIndex+bvhInfoCPU[bvhA].m_nodeOffset;
int endNodeIndexA = startNodeIndexA+subtreeA.m_subtreeSize;
int startNodeIndexB = subtreeB.m_rootNodeIndex;
int endNodeIndexB = subtreeB.m_rootNodeIndex+subtreeB.m_subtreeSize;
int startNodeIndexB = subtreeB.m_rootNodeIndex+bvhInfoCPU[bvhB].m_nodeOffset;
int endNodeIndexB = startNodeIndexB+subtreeB.m_subtreeSize;
b3AlignedObjectArray<b3Int2> nodeStack;
b3Int2 node0;
@@ -1497,8 +1504,13 @@ __kernel void findCompoundPairsKernel(
do
{
if (depth > maxDepth)
{
maxDepth=depth;
printf("maxDepth=%d\n",maxDepth);
}
b3Int2 node = nodeStack[--depth];
b3Vector3 aMinLocal = MyUnQuantize(treeNodesCPU[node.x].m_quantizedAabbMin,bvhInfoCPU[bvhA].m_quantization,bvhInfoCPU[bvhA].m_aabbMin);
b3Vector3 aMaxLocal = MyUnQuantize(treeNodesCPU[node.x].m_quantizedAabbMax,bvhInfoCPU[bvhA].m_quantization,bvhInfoCPU[bvhA].m_aabbMin);
@@ -1507,12 +1519,13 @@ __kernel void findCompoundPairsKernel(
float margin=0.f;
b3Vector3 aabbAMinOut,aabbAMaxOut;
b3TransformAabb(aMinLocal,aMaxLocal, margin,transA,aabbAMinOut,aabbAMaxOut);
b3TransformAabb2(aMinLocal,aMaxLocal, margin,transA.getOrigin(),transA.getRotation(),&aabbAMinOut,&aabbAMaxOut);
b3Vector3 aabbBMinOut,aabbBMaxOut;
b3TransformAabb(bMinLocal,bMaxLocal, margin,transB,aabbBMinOut,aabbBMaxOut);
b3TransformAabb2(bMinLocal,bMaxLocal, margin,transB.getOrigin(),transB.getRotation(),&aabbBMinOut,&aabbBMaxOut);
bool nodeOverlap = b3TestAabbAgainstAabb2(aabbAMinOut,aabbAMaxOut,aabbBMinOut,aabbBMaxOut);
numAabbChecks++;
bool nodeOverlap = b3TestAabbAgainstAabb(aabbAMinOut,aabbAMaxOut,aabbBMinOut,aabbBMaxOut);
if (nodeOverlap)
{
bool isLeafA = treeNodesCPU[node.x].isLeafNode();
@@ -1573,34 +1586,11 @@ __kernel void findCompoundPairsKernel(
}
}
} while (depth);
maxNumAabbChecks = b3Max(numAabbChecks,maxNumAabbChecks);
}
/*
for (i=0;i<this->m_SubtreeHeaders.size();i++)
{
const b3BvhSubtreeInfo& subtree = m_SubtreeHeaders[i];
//PCK: unsigned instead of bool
unsigned overlap = b3TestQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin,quantizedQueryAabbMax,subtree.m_quantizedAabbMin,subtree.m_quantizedAabbMax);
if (overlap != 0)
{
walkStacklessQuantizedTree(nodeCallback,quantizedQueryAabbMin,quantizedQueryAabbMax,
subtree.m_rootNodeIndex,
subtree.m_rootNodeIndex+subtree.m_subtreeSize);
}
}
*/
/*bvhInfoCPU[bvhA].m_numNodes;
bvhInfoCPU[bvhA].m_nodeOffset
b3AlignedObjectArray<b3Int2> nodeStack;
b3Int2 n;n.x =
nodeStack.push_back(
*/
}
}
return;
}
@@ -1624,8 +1614,8 @@ __kernel void findCompoundPairsKernel(
b3Quat newOrnA = b3QuatMul(ornA,childOrnA);
int shapeIndexA = collidables[childColIndexA].m_shapeIndex;
b3Aabb aabbA = aabbsLocalSpace[shapeIndexA];
b3Aabb aabbA = aabbsLocalSpace[childColIndexA];
b3Transform transA;
@@ -1636,7 +1626,7 @@ __kernel void findCompoundPairsKernel(
b3Vector3 aabbAMinOut,aabbAMaxOut;
b3TransformAabb((const b3Float4&)aabbA.m_min,(const b3Float4&)aabbA.m_max, margin,transA,aabbAMinOut,aabbAMaxOut);
b3TransformAabb2((const b3Float4&)aabbA.m_min,(const b3Float4&)aabbA.m_max, margin,transA.getOrigin(),transA.getRotation(),&aabbAMinOut,&aabbAMaxOut);
if (collidables[collidableIndexB].m_shapeType==SHAPE_COMPOUND_OF_CONVEX_HULLS)
{
@@ -1654,10 +1644,7 @@ __kernel void findCompoundPairsKernel(
int shapeIndexB = collidables[childColIndexB].m_shapeIndex;
b3Aabb aabbB = aabbsLocalSpace[shapeIndexB];
b3Aabb aabbB = aabbsLocalSpace[childColIndexB];
b3Transform transB;
transB.setIdentity();
@@ -1665,9 +1652,10 @@ __kernel void findCompoundPairsKernel(
transB.setRotation(newOrnB);
b3Vector3 aabbBMinOut,aabbBMaxOut;
b3TransformAabb((const b3Float4&)aabbB.m_min,(const b3Float4&)aabbB.m_max, margin,transB,aabbBMinOut,aabbBMaxOut);
b3TransformAabb2((const b3Float4&)aabbB.m_min,(const b3Float4&)aabbB.m_max, margin,transB.getOrigin(),transB.getRotation(),&aabbBMinOut,&aabbBMaxOut);
bool aabbOverlap = b3TestAabbAgainstAabb2(aabbAMinOut,aabbAMaxOut,aabbBMinOut,aabbBMaxOut);
numAabbChecks++;
bool aabbOverlap = b3TestAabbAgainstAabb(aabbAMinOut,aabbAMaxOut,aabbBMinOut,aabbBMaxOut);
if (aabbOverlap)
{
/*
@@ -2083,6 +2071,7 @@ void computeContactCompoundCompound(int pairIndex,
bvhInfoCPU
);
printf("maxNumAabbChecks=%d\n",maxNumAabbChecks);
if (numCompoundPairsOut>maxNumCompoundPairsCapacity)
{
b3Error("numCompoundPairsOut exceeded maxNumCompoundPairsCapacity (%d)\n",maxNumCompoundPairsCapacity);
@@ -2238,7 +2227,7 @@ void computeContactPlaneCompound(int pairIndex,
int numWorldVertsB1= 0;
b3Vector3 planeEq = faces[collidables[collidableIndexA].m_shapeIndex].m_plane;
b3Vector3 planeNormal(planeEq.x,planeEq.y,planeEq.z);
b3Vector3 planeNormal=b3MakeVector3(planeEq.x,planeEq.y,planeEq.z);
b3Vector3 planeNormalWorld = b3QuatRotate(ornA,planeNormal);
float planeConstant = planeEq.w;
b3Transform convexWorldTransform;
@@ -2333,7 +2322,7 @@ void computeContactPlaneCompound(int pairIndex,
b3Vector3 pOnB1 = contactPoints[contactIdx.s[i]];
c->m_worldPosB[i] = pOnB1;
}
c->m_worldNormalOnB[3] = (b3Scalar)numReducedPoints;
c->m_worldNormalOnB.w = (b3Scalar)numReducedPoints;
}//if (dstIdx < numPairs)
}
@@ -2382,8 +2371,8 @@ void computeContactSphereConvex(int pairIndex,
int collidableIndex = rigidBodies[bodyIndexB].m_collidableIdx;
int shapeIndex = collidables[collidableIndex].m_shapeIndex;
int numFaces = convexShapes[shapeIndex].m_numFaces;
float4 closestPnt = make_float4(0, 0, 0, 0);
float4 hitNormalWorld = make_float4(0, 0, 0, 0);
float4 closestPnt = b3MakeVector3(0, 0, 0, 0);
float4 hitNormalWorld = b3MakeVector3(0, 0, 0, 0);
float minDist = -1000000.f; // TODO: What is the largest/smallest float?
bool bCollide = true;
int region = -1;
@@ -2392,10 +2381,10 @@ void computeContactSphereConvex(int pairIndex,
{
b3GpuFace face = faces[convexShapes[shapeIndex].m_faceOffset+f];
float4 planeEqn;
float4 localPlaneNormal = make_float4(face.m_plane.getX(),face.m_plane.getY(),face.m_plane.getZ(),0.f);
float4 localPlaneNormal = b3MakeVector3(face.m_plane.x,face.m_plane.y,face.m_plane.z,0.f);
float4 n1 = localPlaneNormal;//quatRotate(quat,localPlaneNormal);
planeEqn = n1;
planeEqn[3] = face.m_plane[3];
planeEqn[3] = face.m_plane.w;
float4 pntReturn;
float dist = signedDistanceFromPointToPlane(spherePos, planeEqn, &pntReturn);
@@ -2471,7 +2460,7 @@ void computeContactSphereConvex(int pairIndex,
if (actualDepth<0)
{
//printf("actualDepth = ,%f,", actualDepth);
//printf("normalOnSurfaceB1 = ,%f,%f,%f,", normalOnSurfaceB1.getX(),normalOnSurfaceB1.getY(),normalOnSurfaceB1.getZ());
//printf("normalOnSurfaceB1 = ,%f,%f,%f,", normalOnSurfaceB1.x,normalOnSurfaceB1.y,normalOnSurfaceB1.z);
//printf("region=,%d,\n", region);
pOnB1[3] = actualDepth;
@@ -2493,7 +2482,7 @@ void computeContactSphereConvex(int pairIndex,
c->m_bodyBPtrAndSignBit = rigidBodies[bodyIndexB].m_invMass==0?-bodyIndexB:bodyIndexB;
c->m_worldPosB[0] = pOnB1;
int numPoints = 1;
c->m_worldNormalOnB[3] = (b3Scalar)numPoints;
c->m_worldNormalOnB.w = (b3Scalar)numPoints;
}//if (dstIdx < numPairs)
}
}//if (hasCollision)
@@ -2539,7 +2528,7 @@ int computeContactConvexConvex( b3AlignedObjectArray<b3Int4>& pairs,
float maximumDistanceSquared = 1e30f;
b3Vector3 resultPointOnBWorld;
b3Vector3 sepAxis2(0,1,0);
b3Vector3 sepAxis2=b3MakeVector3(0,1,0);
b3Scalar distance2 = 1e30f;
int shapeIndexA = collidables[collidableIndexA].m_shapeIndex;
@@ -2749,7 +2738,7 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( b3OpenCLArray<b3Int4>*
const b3OpenCLArray<b3GpuChildShape>& gpuChildShapes,
const b3OpenCLArray<b3Aabb>& clAabbsWorldSpace,
const b3OpenCLArray<b3Aabb>& clAabbslocalSpace,
const b3OpenCLArray<b3Aabb>& clAabbsLocalSpace,
b3OpenCLArray<b3Vector3>& worldVertsB1GPU,
b3OpenCLArray<b3Int4>& clippingFacesOutGPU,
@@ -2788,7 +2777,7 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( b3OpenCLArray<b3Int4>*
clAabbsWorldSpace.copyToHost(hostAabbsWorldSpace);
b3AlignedObjectArray<b3Aabb> hostAabbsLocalSpace;
clAabbslocalSpace.copyToHost(hostAabbsLocalSpace);
clAabbsLocalSpace.copyToHost(hostAabbsLocalSpace);
b3AlignedObjectArray<b3Int4> hostPairs;
pairs->copyToHost(hostPairs);
@@ -3115,8 +3104,8 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( b3OpenCLArray<b3Int4>*
}
numCompoundPairs = m_numCompoundPairsOut.at(0);
if (1)
bool useGpuFindCompoundPairs=true;
if (useGpuFindCompoundPairs)
{
B3_PROFILE("findCompoundPairsKernel");
b3BufferInfoCL bInfo[] =
@@ -3129,10 +3118,13 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( b3OpenCLArray<b3Int4>*
b3BufferInfoCL( gpuUniqueEdges.getBufferCL(),true),
b3BufferInfoCL( gpuFaces.getBufferCL(),true),
b3BufferInfoCL( gpuIndices.getBufferCL(),true),
b3BufferInfoCL( clAabbsWorldSpace.getBufferCL(),true),
b3BufferInfoCL( clAabbsLocalSpace.getBufferCL(),true),
b3BufferInfoCL( gpuChildShapes.getBufferCL(),true),
b3BufferInfoCL( m_gpuCompoundPairs.getBufferCL()),
b3BufferInfoCL( m_numCompoundPairsOut.getBufferCL())
b3BufferInfoCL( m_numCompoundPairsOut.getBufferCL()),
b3BufferInfoCL(subTreesGPU->getBufferCL()),
b3BufferInfoCL(treeNodesGPU->getBufferCL()),
b3BufferInfoCL(bvhInfo->getBufferCL())
};
b3LauncherCL launcher(m_queue, m_findCompoundPairsKernel);
@@ -3143,17 +3135,104 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( b3OpenCLArray<b3Int4>*
int num = nPairs;
launcher.launch1D( num);
clFinish(m_queue);
numCompoundPairs = m_numCompoundPairsOut.at(0);
//printf("numCompoundPairs =%d\n",numCompoundPairs );
if (numCompoundPairs)
{
//printf("numCompoundPairs=%d\n",numCompoundPairs);
}
} else
{
b3AlignedObjectArray<b3QuantizedBvhNode> treeNodesCPU;
treeNodesGPU->copyToHost(treeNodesCPU);
b3AlignedObjectArray<b3BvhSubtreeInfo> subTreesCPU;
subTreesGPU->copyToHost(subTreesCPU);
b3AlignedObjectArray<b3BvhInfo> bvhInfoCPU;
bvhInfo->copyToHost(bvhInfoCPU);
b3AlignedObjectArray<b3Aabb> hostAabbsWorldSpace;
clAabbsWorldSpace.copyToHost(hostAabbsWorldSpace);
b3AlignedObjectArray<b3Aabb> hostAabbsLocalSpace;
clAabbsLocalSpace.copyToHost(hostAabbsLocalSpace);
b3AlignedObjectArray<b3Int4> hostPairs;
pairs->copyToHost(hostPairs);
b3AlignedObjectArray<b3RigidBodyCL> hostBodyBuf;
bodyBuf->copyToHost(hostBodyBuf);
int numCompoundPairsOut=0;
b3AlignedObjectArray<b3Int4> cpuCompoundPairsOut;
cpuCompoundPairsOut.resize(compoundPairCapacity);
b3AlignedObjectArray<b3Collidable> hostCollidables;
gpuCollidables.copyToHost(hostCollidables);
b3AlignedObjectArray<b3GpuChildShape> cpuChildShapes;
gpuChildShapes.copyToHost(cpuChildShapes);
b3AlignedObjectArray<b3ConvexPolyhedronCL> hostConvexData;
convexData.copyToHost(hostConvexData);
b3AlignedObjectArray<b3Vector3> hostVertices;
gpuVertices.copyToHost(hostVertices);
for (int pairIndex=0;pairIndex<nPairs;pairIndex++)
{
int bodyIndexA = hostPairs[pairIndex].x;
int bodyIndexB = hostPairs[pairIndex].y;
int collidableIndexA = hostBodyBuf[bodyIndexA].m_collidableIdx;
int collidableIndexB = hostBodyBuf[bodyIndexB].m_collidableIdx;
findCompoundPairsKernel(
pairIndex,
bodyIndexA,
bodyIndexB,
collidableIndexA,
collidableIndexB,
&hostBodyBuf[0],
&hostCollidables[0],
&hostConvexData[0],
hostVertices,
hostAabbsWorldSpace,
hostAabbsLocalSpace,
&cpuChildShapes[0],
&cpuCompoundPairsOut[0],
&numCompoundPairsOut,
compoundPairCapacity,
treeNodesCPU,
subTreesCPU,
bvhInfoCPU
);
}
if (numCompoundPairsOut)
{
printf("numCompoundPairsOut=%d\n",numCompoundPairsOut);
}
}
numCompoundPairs = m_numCompoundPairsOut.at(0);
//printf("numCompoundPairs =%d\n",numCompoundPairs );
if (numCompoundPairs > compoundPairCapacity)
{
b3Error("Exceeded compound pair capacity (%d/%d)\n", numCompoundPairs, compoundPairCapacity);
numCompoundPairs = compoundPairCapacity;
}
m_gpuCompoundPairs.resize(numCompoundPairs);
m_gpuHasCompoundSepNormals.resize(numCompoundPairs);
m_gpuCompoundSepNormals.resize(numCompoundPairs);

8
src/Bullet3OpenCL/NarrowphaseCollision/b3ConvexUtility.cpp
View File

@@ -127,12 +127,12 @@ bool b3ConvexUtility::initializePolyhedralFeatures(const b3Vector3* orgVertices,
b3MyFace& faceA = tmpFaces[refFace];
todoFaces.pop_back();
b3Vector3 faceNormalA(faceA.m_plane[0],faceA.m_plane[1],faceA.m_plane[2]);
b3Vector3 faceNormalA = b3MakeVector3(faceA.m_plane[0],faceA.m_plane[1],faceA.m_plane[2]);
for (int j=todoFaces.size()-1;j>=0;j--)
{
int i = todoFaces[j];
b3MyFace& faceB = tmpFaces[i];
b3Vector3 faceNormalB(faceB.m_plane[0],faceB.m_plane[1],faceB.m_plane[2]);
b3Vector3 faceNormalB = b3MakeVector3(faceB.m_plane[0],faceB.m_plane[1],faceB.m_plane[2]);
if (faceNormalA.dot(faceNormalB)>faceWeldThreshold)
{
coplanarFaceGroup.push_back(i);
@@ -147,14 +147,14 @@ bool b3ConvexUtility::initializePolyhedralFeatures(const b3Vector3* orgVertices,
//do the merge: use Graham Scan 2d convex hull
b3AlignedObjectArray<b3GrahamVector3> orgpoints;
b3Vector3 averageFaceNormal(0,0,0);
b3Vector3 averageFaceNormal = b3MakeVector3(0,0,0);
for (int i=0;i<coplanarFaceGroup.size();i++)
{
// m_polyhedron->m_faces.push_back(tmpFaces[coplanarFaceGroup[i]]);
b3MyFace& face = tmpFaces[coplanarFaceGroup[i]];
b3Vector3 faceNormal(face.m_plane[0],face.m_plane[1],face.m_plane[2]);
b3Vector3 faceNormal = b3MakeVector3(face.m_plane[0],face.m_plane[1],face.m_plane[2]);
averageFaceNormal+=faceNormal;
for (int f=0;f<face.m_indices.size();f++)
{

22
src/Bullet3OpenCL/NarrowphaseCollision/b3GjkEpa.cpp
View File

@@ -148,7 +148,7 @@ namespace gjkepa2_impl
}
void Initialize()
{
m_ray = b3Vector3(0,0,0);
m_ray = b3MakeVector3(0,0,0);
m_nfree = 0;
m_status = eStatus::Failed;
m_current = 0;
@@ -175,7 +175,7 @@ namespace gjkepa2_impl
m_simplices[0].rank = 0;
m_ray = guess;
const b3Scalar sqrl= m_ray.length2();
appendvertice(m_simplices[0],sqrl>0?-m_ray:b3Vector3(1,0,0));
appendvertice(m_simplices[0],sqrl>0?-m_ray:b3MakeVector3(1,0,0));
m_simplices[0].p[0] = 1;
m_ray = m_simplices[0].c[0]->w;
sqdist = sqrl;
@@ -242,7 +242,7 @@ namespace gjkepa2_impl
if(sqdist>=0)
{/* Valid */
ns.rank = 0;
m_ray = b3Vector3(0,0,0);
m_ray = b3MakeVector3(0,0,0);
m_current = next;
for(unsigned int i=0,ni=cs.rank;i<ni;++i)
{
@@ -285,7 +285,7 @@ namespace gjkepa2_impl
{
for(unsigned int i=0;i<3;++i)
{
b3Vector3 axis=b3Vector3(0,0,0);
b3Vector3 axis=b3MakeVector3(0,0,0);
axis[i]=1;
appendvertice(*m_simplex, axis);
if(EncloseOrigin()) return(true);
@@ -301,7 +301,7 @@ namespace gjkepa2_impl
const b3Vector3 d=m_simplex->c[1]->w-m_simplex->c[0]->w;
for(unsigned int i=0;i<3;++i)
{
b3Vector3 axis=b3Vector3(0,0,0);
b3Vector3 axis=b3MakeVector3(0,0,0);
axis[i]=1;
const b3Vector3 p=b3Cross(d,axis);
if(p.length2()>0)
@@ -557,7 +557,7 @@ namespace gjkepa2_impl
void Initialize()
{
m_status = eStatus::Failed;
m_normal = b3Vector3(0,0,0);
m_normal = b3MakeVector3(0,0,0);
m_depth = 0;
m_nextsv = 0;
for(unsigned int i=0;i<EPA_MAX_FACES;++i)
@@ -662,7 +662,7 @@ namespace gjkepa2_impl
if(nl>0)
m_normal = m_normal/nl;
else
m_normal = b3Vector3(1,0,0);
m_normal = b3MakeVector3(1,0,0);
m_depth = 0;
m_result.rank=1;
m_result.c[0]=simplex.c[0];
@@ -813,7 +813,7 @@ namespace gjkepa2_impl
{
/* Results */
results.witnesses[0] =
results.witnesses[1] = b3Vector3(0,0,0);
results.witnesses[1] = b3MakeVector3(0,0,0);
results.status = b3GjkEpaSolver2::sResults::Separated;
/* Shape */
shape.m_shapes[0] = hullA;
@@ -851,8 +851,8 @@ bool b3GjkEpaSolver2::Distance( const b3Transform& transA, const b3Transform& t
GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,guess);
if(gjk_status==GJK::eStatus::Valid)
{
b3Vector3 w0=b3Vector3(0,0,0);
b3Vector3 w1=b3Vector3(0,0,0);
b3Vector3 w0=b3MakeVector3(0,0,0);
b3Vector3 w1=b3MakeVector3(0,0,0);
for(unsigned int i=0;i<gjk.m_simplex->rank;++i)
{
const b3Scalar p=gjk.m_simplex->p[i];
@@ -897,7 +897,7 @@ bool b3GjkEpaSolver2::Penetration( const b3Transform& transA, const b3Transform&
EPA::eStatus::_ epa_status=epa.Evaluate(gjk,-guess);
if(epa_status!=EPA::eStatus::Failed)
{
b3Vector3 w0=b3Vector3(0,0,0);
b3Vector3 w0=b3MakeVector3(0,0,0);
for(unsigned int i=0;i<epa.m_result.rank;++i)
{
w0+=shape.Support(epa.m_result.c[i]->d,0,verticesA,verticesB)*epa.m_result.p[i];

8
src/Bullet3OpenCL/NarrowphaseCollision/b3GjkPairDetector.cpp
View File

@@ -33,7 +33,7 @@ int gEpaSeparatingAxis=0;
b3GjkPairDetector::b3GjkPairDetector(b3VoronoiSimplexSolver* simplexSolver,b3GjkEpaSolver2* penetrationDepthSolver)
:m_cachedSeparatingAxis(b3Scalar(0.),b3Scalar(-1.),b3Scalar(0.)),
:m_cachedSeparatingAxis(b3MakeVector3(b3Scalar(0.),b3Scalar(-1.),b3Scalar(0.))),
m_penetrationDepthSolver(penetrationDepthSolver),
m_simplexSolver(simplexSolver),
m_ignoreMargin(false),
@@ -159,13 +159,13 @@ bool getClosestPoints(b3GjkPairDetector* gjkDetector, const b3Transform& transA,
gjkDetector->m_cachedSeparatingDistance = 0.f;
b3Scalar distance=b3Scalar(0.);
b3Vector3 normalInB(b3Scalar(0.),b3Scalar(0.),b3Scalar(0.));
b3Vector3 normalInB= b3MakeVector3(b3Scalar(0.),b3Scalar(0.),b3Scalar(0.));
b3Vector3 pointOnA,pointOnB;
b3Transform localTransA = transA;
b3Transform localTransB = transB;
b3Vector3 positionOffset(0,0,0);// = (localTransA.getOrigin() + localTransB.getOrigin()) * b3Scalar(0.5);
b3Vector3 positionOffset = b3MakeVector3(0,0,0);// = (localTransA.getOrigin() + localTransB.getOrigin()) * b3Scalar(0.5);
localTransA.getOrigin() -= positionOffset;
localTransB.getOrigin() -= positionOffset;
@@ -202,7 +202,7 @@ bool getClosestPoints(b3GjkPairDetector* gjkDetector, const b3Transform& transA,
b3Scalar margin = marginA + marginB;
b3Scalar bestDeltaN = -1e30f;
b3Vector3 bestSepAxis(0,0,0);
b3Vector3 bestSepAxis= b3MakeVector3(0,0,0);
b3Vector3 bestPointOnA;
b3Vector3 bestPointOnB;

8
src/Bullet3OpenCL/NarrowphaseCollision/b3OptimizedBvh.cpp
View File

@@ -167,8 +167,8 @@ void b3OptimizedBvh::build(b3StridingMeshInterface* triangles, bool useQuantized
{
NodeTriangleCallback callback(m_leafNodes);
b3Vector3 aabbMin(b3Scalar(-B3_LARGE_FLOAT),b3Scalar(-B3_LARGE_FLOAT),b3Scalar(-B3_LARGE_FLOAT));
b3Vector3 aabbMax(b3Scalar(B3_LARGE_FLOAT),b3Scalar(B3_LARGE_FLOAT),b3Scalar(B3_LARGE_FLOAT));
b3Vector3 aabbMin=b3MakeVector3(b3Scalar(-B3_LARGE_FLOAT),b3Scalar(-B3_LARGE_FLOAT),b3Scalar(-B3_LARGE_FLOAT));
b3Vector3 aabbMax=b3MakeVector3(b3Scalar(B3_LARGE_FLOAT),b3Scalar(B3_LARGE_FLOAT),b3Scalar(B3_LARGE_FLOAT));
triangles->InternalProcessAllTriangles(&callback,aabbMin,aabbMax);
@@ -322,7 +322,7 @@ void b3OptimizedBvh::updateBvhNodes(b3StridingMeshInterface* meshInterface,int f
if (type == PHY_FLOAT)
{
float* graphicsbase = (float*)(vertexbase+graphicsindex*stride);
triangleVerts[j] = b3Vector3(
triangleVerts[j] = b3MakeVector3(
graphicsbase[0]*meshScaling.getX(),
graphicsbase[1]*meshScaling.getY(),
graphicsbase[2]*meshScaling.getZ());
@@ -330,7 +330,7 @@ void b3OptimizedBvh::updateBvhNodes(b3StridingMeshInterface* meshInterface,int f
else
{
double* graphicsbase = (double*)(vertexbase+graphicsindex*stride);
triangleVerts[j] = b3Vector3( b3Scalar(graphicsbase[0]*meshScaling.getX()), b3Scalar(graphicsbase[1]*meshScaling.getY()), b3Scalar(graphicsbase[2]*meshScaling.getZ()));
triangleVerts[j] = b3MakeVector3( b3Scalar(graphicsbase[0]*meshScaling.getX()), b3Scalar(graphicsbase[1]*meshScaling.getY()), b3Scalar(graphicsbase[2]*meshScaling.getZ()));
}
}

12
src/Bullet3OpenCL/NarrowphaseCollision/b3QuantizedBvh.cpp
View File

@@ -90,11 +90,11 @@ b3Vector3 color[4]=
void b3QuantizedBvh::setQuantizationValues(const b3Vector3& bvhAabbMin,const b3Vector3& bvhAabbMax,b3Scalar quantizationMargin)
{
//enlarge the AABB to avoid division by zero when initializing the quantization values
b3Vector3 clampValue(quantizationMargin,quantizationMargin,quantizationMargin);
b3Vector3 clampValue =b3MakeVector3(quantizationMargin,quantizationMargin,quantizationMargin);
m_bvhAabbMin = bvhAabbMin - clampValue;
m_bvhAabbMax = bvhAabbMax + clampValue;
b3Vector3 aabbSize = m_bvhAabbMax - m_bvhAabbMin;
m_bvhQuantization = b3Vector3(b3Scalar(65533.0),b3Scalar(65533.0),b3Scalar(65533.0)) / aabbSize;
m_bvhQuantization = b3MakeVector3(b3Scalar(65533.0),b3Scalar(65533.0),b3Scalar(65533.0)) / aabbSize;
m_useQuantization = true;
}
@@ -233,7 +233,7 @@ int b3QuantizedBvh::sortAndCalcSplittingIndex(int startIndex,int endIndex,int sp
int numIndices = endIndex - startIndex;
b3Scalar splitValue;
b3Vector3 means(b3Scalar(0.),b3Scalar(0.),b3Scalar(0.));
b3Vector3 means=b3MakeVector3(b3Scalar(0.),b3Scalar(0.),b3Scalar(0.));
for (i=startIndex;i<endIndex;i++)
{
b3Vector3 center = b3Scalar(0.5)*(getAabbMax(i)+getAabbMin(i));
@@ -284,8 +284,8 @@ int b3QuantizedBvh::calcSplittingAxis(int startIndex,int endIndex)
{
int i;
b3Vector3 means(b3Scalar(0.),b3Scalar(0.),b3Scalar(0.));
b3Vector3 variance(b3Scalar(0.),b3Scalar(0.),b3Scalar(0.));
b3Vector3 means=b3MakeVector3(b3Scalar(0.),b3Scalar(0.),b3Scalar(0.));
b3Vector3 variance=b3MakeVector3(b3Scalar(0.),b3Scalar(0.),b3Scalar(0.));
int numIndices = endIndex-startIndex;
for (i=startIndex;i<endIndex;i++)
@@ -754,7 +754,7 @@ void b3QuantizedBvh::walkStacklessQuantizedTreeCacheFriendly(b3NodeOverlapCallba
void b3QuantizedBvh::reportRayOverlappingNodex (b3NodeOverlapCallback* nodeCallback, const b3Vector3& raySource, const b3Vector3& rayTarget) const
{
reportBoxCastOverlappingNodex(nodeCallback,raySource,rayTarget,b3Vector3(0,0,0),b3Vector3(0,0,0));
reportBoxCastOverlappingNodex(nodeCallback,raySource,rayTarget,b3MakeVector3(0,0,0),b3MakeVector3(0,0,0));
}

2
src/Bullet3OpenCL/NarrowphaseCollision/b3StridingMeshInterface.h
View File

@@ -39,7 +39,7 @@ B3_ATTRIBUTE_ALIGNED16(class ) b3StridingMeshInterface
public:
B3_DECLARE_ALIGNED_ALLOCATOR();
b3StridingMeshInterface() :m_scaling(b3Scalar(1.),b3Scalar(1.),b3Scalar(1.))
b3StridingMeshInterface() :m_scaling(b3MakeVector3(b3Scalar(1.),b3Scalar(1.),b3Scalar(1.)))
{
}

2
src/Bullet3OpenCL/NarrowphaseCollision/b3SupportMappings.h
View File

@@ -12,7 +12,7 @@ struct b3GjkPairDetector;
inline b3Vector3 localGetSupportVertexWithMargin(const float4& supportVec,const struct b3ConvexPolyhedronCL* hull,
const b3AlignedObjectArray<b3Vector3>& verticesA, b3Scalar margin)
{
b3Vector3 supVec(b3Scalar(0.),b3Scalar(0.),b3Scalar(0.));
b3Vector3 supVec = b3MakeVector3(b3Scalar(0.),b3Scalar(0.),b3Scalar(0.));
b3Scalar maxDot = b3Scalar(-B3_LARGE_FLOAT);
// Here we take advantage of dot(a, b*c) = dot(a*b, c). Note: This is true mathematically, but not numerically.

2
src/Bullet3OpenCL/NarrowphaseCollision/b3TriangleCallback.cpp
View File

@@ -17,7 +17,7 @@ subject to the following restrictions:
b3TriangleCallback::~b3TriangleCallback()
{
}

8
src/Bullet3OpenCL/NarrowphaseCollision/b3VoronoiSimplexSolver.cpp
View File

@@ -68,7 +68,7 @@ void b3VoronoiSimplexSolver::reset()
m_cachedValidClosest = false;
m_numVertices = 0;
m_needsUpdate = true;
m_lastW = b3Vector3(b3Scalar(B3_LARGE_FLOAT),b3Scalar(B3_LARGE_FLOAT),b3Scalar(B3_LARGE_FLOAT));
m_lastW = b3MakeVector3(b3Scalar(B3_LARGE_FLOAT),b3Scalar(B3_LARGE_FLOAT),b3Scalar(B3_LARGE_FLOAT));
m_cachedBC.reset();
}
@@ -118,7 +118,7 @@ bool b3VoronoiSimplexSolver::updateClosestVectorAndPoints()
const b3Vector3& to = m_simplexVectorW[1];
b3Vector3 nearest;
b3Vector3 p (b3Scalar(0.),b3Scalar(0.),b3Scalar(0.));
b3Vector3 p =b3MakeVector3(b3Scalar(0.),b3Scalar(0.),b3Scalar(0.));
b3Vector3 diff = p - from;
b3Vector3 v = to - from;
b3Scalar t = v.dot(diff);
@@ -157,7 +157,7 @@ bool b3VoronoiSimplexSolver::updateClosestVectorAndPoints()
case 3:
{
//closest point origin from triangle
b3Vector3 p (b3Scalar(0.),b3Scalar(0.),b3Scalar(0.));
b3Vector3 p =b3MakeVector3(b3Scalar(0.),b3Scalar(0.),b3Scalar(0.));
const b3Vector3& a = m_simplexVectorW[0];
const b3Vector3& b = m_simplexVectorW[1];
@@ -183,7 +183,7 @@ bool b3VoronoiSimplexSolver::updateClosestVectorAndPoints()
{
b3Vector3 p (b3Scalar(0.),b3Scalar(0.),b3Scalar(0.));
b3Vector3 p =b3MakeVector3(b3Scalar(0.),b3Scalar(0.),b3Scalar(0.));
const b3Vector3& a = m_simplexVectorW[0];
const b3Vector3& b = m_simplexVectorW[1];

14
src/Bullet3OpenCL/NarrowphaseCollision/kernels/primitiveContacts.h
View File

@@ -18,6 +18,20 @@ static const char* primitiveContactsKernelsCL= \
"#ifdef __cplusplus\n"
"#else\n"
" typedef float4 b3Float4;\n"
" #define b3Float4ConstArg const b3Float4\n"
" #define b3MakeFloat4 (float4)\n"
" float b3Dot3F4(b3Float4ConstArg v0,b3Float4ConstArg v1)\n"
" {\n"
" float4 a1 = b3MakeFloat4(v0.xyz,0.f);\n"
" float4 b1 = b3MakeFloat4(v1.xyz,0.f);\n"
" return dot(a1, b1);\n"
" }\n"
" b3Float4 b3Cross3(b3Float4ConstArg v0,b3Float4ConstArg v1)\n"
" {\n"
" float4 a1 = b3MakeFloat4(v0.xyz,0.f);\n"
" float4 b1 = b3MakeFloat4(v1.xyz,0.f);\n"
" return cross(a1, b1);\n"
" }\n"
"#endif \n"
"#endif //B3_FLOAT4_H\n"
"typedef struct b3Contact4Data b3Contact4Data_t;\n"

314
src/Bullet3OpenCL/NarrowphaseCollision/kernels/sat.cl
View File

@@ -7,6 +7,7 @@
#define TRIANGLE_NUM_CONVEX_FACES 5
#define SHAPE_COMPOUND_OF_CONVEX_HULLS 6
#define B3_MAX_STACK_DEPTH 256
typedef unsigned int u32;
@@ -14,13 +15,104 @@ typedef unsigned int u32;
///keep this in sync with btCollidable.h
typedef struct
{
int m_numChildShapes;
int blaat2;
union {
int m_numChildShapes;
int m_bvhIndex;
};
union
{
float m_radius;
int m_compoundBvhIndex;
};
int m_shapeType;
int m_shapeIndex;
} btCollidableGpu;
#define MAX_NUM_PARTS_IN_BITS 10
///b3QuantizedBvhNode is a compressed aabb node, 16 bytes.
///Node can be used for leafnode or internal node. Leafnodes can point to 32-bit triangle index (non-negative range).
typedef struct
{
//12 bytes
unsigned short int m_quantizedAabbMin[3];
unsigned short int m_quantizedAabbMax[3];
//4 bytes
int m_escapeIndexOrTriangleIndex;
} b3QuantizedBvhNode;
typedef struct
{
float4 m_aabbMin;
float4 m_aabbMax;
float4 m_quantization;
int m_numNodes;
int m_numSubTrees;
int m_nodeOffset;
int m_subTreeOffset;
} b3BvhInfo;
int getTriangleIndex(const b3QuantizedBvhNode* rootNode)
{
unsigned int x=0;
unsigned int y = (~(x&0))<<(31-MAX_NUM_PARTS_IN_BITS);
// Get only the lower bits where the triangle index is stored
return (rootNode->m_escapeIndexOrTriangleIndex&~(y));
}
int getTriangleIndexGlobal(__global const b3QuantizedBvhNode* rootNode)
{
unsigned int x=0;
unsigned int y = (~(x&0))<<(31-MAX_NUM_PARTS_IN_BITS);
// Get only the lower bits where the triangle index is stored
return (rootNode->m_escapeIndexOrTriangleIndex&~(y));
}
int isLeafNode(const b3QuantizedBvhNode* rootNode)
{
//skipindex is negative (internal node), triangleindex >=0 (leafnode)
return (rootNode->m_escapeIndexOrTriangleIndex >= 0)? 1 : 0;
}
int isLeafNodeGlobal(__global const b3QuantizedBvhNode* rootNode)
{
//skipindex is negative (internal node), triangleindex >=0 (leafnode)
return (rootNode->m_escapeIndexOrTriangleIndex >= 0)? 1 : 0;
}
int getEscapeIndex(const b3QuantizedBvhNode* rootNode)
{
return -rootNode->m_escapeIndexOrTriangleIndex;
}
int getEscapeIndexGlobal(__global const b3QuantizedBvhNode* rootNode)
{
return -rootNode->m_escapeIndexOrTriangleIndex;
}
typedef struct
{
//12 bytes
unsigned short int m_quantizedAabbMin[3];
unsigned short int m_quantizedAabbMax[3];
//4 bytes, points to the root of the subtree
int m_rootNodeIndex;
//4 bytes
int m_subtreeSize;
int m_padding[3];
} b3BvhSubtreeInfo;
typedef struct
{
float4 m_childPosition;
@@ -80,6 +172,11 @@ typedef struct
};
} btAabbCL;
#include "Bullet3Collision/BroadPhaseCollision/shared/b3Aabb.h"
#include "Bullet3Common/shared/b3Int2.h"
typedef struct
{
float4 m_plane;
@@ -755,6 +852,34 @@ __kernel void processCompoundPairsKernel( __global const int4* gpuCompoundPair
}
inline b3Float4 MyUnQuantize(const unsigned short* vecIn, b3Float4 quantization, b3Float4 bvhAabbMin)
{
b3Float4 vecOut;
vecOut = b3MakeFloat4(
(float)(vecIn[0]) / (quantization.x),
(float)(vecIn[1]) / (quantization.y),
(float)(vecIn[2]) / (quantization.z),
0.f);
vecOut += bvhAabbMin;
return vecOut;
}
inline b3Float4 MyUnQuantizeGlobal(__global const unsigned short* vecIn, b3Float4 quantization, b3Float4 bvhAabbMin)
{
b3Float4 vecOut;
vecOut = b3MakeFloat4(
(float)(vecIn[0]) / (quantization.x),
(float)(vecIn[1]) / (quantization.y),
(float)(vecIn[2]) / (quantization.z),
0.f);
vecOut += bvhAabbMin;
return vecOut;
}
// work-in-progress
__kernel void findCompoundPairsKernel( __global const int4* pairs,
__global const BodyData* rigidBodies,
@@ -764,10 +889,13 @@ __kernel void findCompoundPairsKernel( __global const int4* pairs,
__global const float4* uniqueEdges,
__global const btGpuFace* faces,
__global const int* indices,
__global btAabbCL* aabbs,
__global b3Aabb_t* aabbLocalSpace,
__global const btGpuChildShape* gpuChildShapes,
__global volatile int4* gpuCompoundPairsOut,
__global volatile int* numCompoundPairsOut,
__global const b3BvhSubtreeInfo* subtrees,
__global const b3QuantizedBvhNode* quantizedNodes,
__global const b3BvhInfo* bvhInfos,
int numPairs,
int maxNumCompoundPairsCapacity
)
@@ -793,6 +921,157 @@ __kernel void findCompoundPairsKernel( __global const int4* pairs,
return;
}
if ((collidables[collidableIndexA].m_shapeType==SHAPE_COMPOUND_OF_CONVEX_HULLS) &&(collidables[collidableIndexB].m_shapeType==SHAPE_COMPOUND_OF_CONVEX_HULLS))
{
int bvhA = collidables[collidableIndexA].m_compoundBvhIndex;
int bvhB = collidables[collidableIndexB].m_compoundBvhIndex;
int numSubTreesA = bvhInfos[bvhA].m_numSubTrees;
int subTreesOffsetA = bvhInfos[bvhA].m_subTreeOffset;
int subTreesOffsetB = bvhInfos[bvhB].m_subTreeOffset;
int numSubTreesB = bvhInfos[bvhB].m_numSubTrees;
float4 posA = rigidBodies[bodyIndexA].m_pos;
b3Quat ornA = rigidBodies[bodyIndexA].m_quat;
b3Quat ornB = rigidBodies[bodyIndexB].m_quat;
float4 posB = rigidBodies[bodyIndexB].m_pos;
for (int p=0;p<numSubTreesA;p++)
{
b3BvhSubtreeInfo subtreeA = subtrees[subTreesOffsetA+p];
//bvhInfos[bvhA].m_quantization
b3Float4 treeAminLocal = MyUnQuantize(subtreeA.m_quantizedAabbMin,bvhInfos[bvhA].m_quantization,bvhInfos[bvhA].m_aabbMin);
b3Float4 treeAmaxLocal = MyUnQuantize(subtreeA.m_quantizedAabbMax,bvhInfos[bvhA].m_quantization,bvhInfos[bvhA].m_aabbMin);
b3Float4 aabbAMinOut,aabbAMaxOut;
float margin=0.f;
b3TransformAabb2(treeAminLocal,treeAmaxLocal, margin,posA,ornA,&aabbAMinOut,&aabbAMaxOut);
for (int q=0;q<numSubTreesB;q++)
{
b3BvhSubtreeInfo subtreeB = subtrees[subTreesOffsetB+q];
b3Float4 treeBminLocal = MyUnQuantize(subtreeB.m_quantizedAabbMin,bvhInfos[bvhB].m_quantization,bvhInfos[bvhB].m_aabbMin);
b3Float4 treeBmaxLocal = MyUnQuantize(subtreeB.m_quantizedAabbMax,bvhInfos[bvhB].m_quantization,bvhInfos[bvhB].m_aabbMin);
b3Float4 aabbBMinOut,aabbBMaxOut;
float margin=0.f;
b3TransformAabb2(treeBminLocal,treeBmaxLocal, margin,posB,ornB,&aabbBMinOut,&aabbBMaxOut);
bool aabbOverlap = b3TestAabbAgainstAabb(aabbAMinOut,aabbAMaxOut,aabbBMinOut,aabbBMaxOut);
if (aabbOverlap)
{
int startNodeIndexA = subtreeA.m_rootNodeIndex+bvhInfos[bvhA].m_nodeOffset;
int endNodeIndexA = startNodeIndexA+subtreeA.m_subtreeSize;
int startNodeIndexB = subtreeB.m_rootNodeIndex+bvhInfos[bvhB].m_nodeOffset;
int endNodeIndexB = startNodeIndexB+subtreeB.m_subtreeSize;
b3Int2 nodeStack[B3_MAX_STACK_DEPTH];
b3Int2 node0;
node0.x = startNodeIndexA;
node0.y = startNodeIndexB;
int maxStackDepth = B3_MAX_STACK_DEPTH;
int depth=0;
nodeStack[depth++]=node0;
do
{
b3Int2 node = nodeStack[--depth];
b3Float4 aMinLocal = MyUnQuantizeGlobal(quantizedNodes[node.x].m_quantizedAabbMin,bvhInfos[bvhA].m_quantization,bvhInfos[bvhA].m_aabbMin);
b3Float4 aMaxLocal = MyUnQuantizeGlobal(quantizedNodes[node.x].m_quantizedAabbMax,bvhInfos[bvhA].m_quantization,bvhInfos[bvhA].m_aabbMin);
b3Float4 bMinLocal = MyUnQuantizeGlobal(quantizedNodes[node.y].m_quantizedAabbMin,bvhInfos[bvhB].m_quantization,bvhInfos[bvhB].m_aabbMin);
b3Float4 bMaxLocal = MyUnQuantizeGlobal(quantizedNodes[node.y].m_quantizedAabbMax,bvhInfos[bvhB].m_quantization,bvhInfos[bvhB].m_aabbMin);
float margin=0.f;
b3Float4 aabbAMinOut,aabbAMaxOut;
b3TransformAabb2(aMinLocal,aMaxLocal, margin,posA,ornA,&aabbAMinOut,&aabbAMaxOut);
b3Float4 aabbBMinOut,aabbBMaxOut;
b3TransformAabb2(bMinLocal,bMaxLocal, margin,posB,ornB,&aabbBMinOut,&aabbBMaxOut);
bool nodeOverlap = b3TestAabbAgainstAabb(aabbAMinOut,aabbAMaxOut,aabbBMinOut,aabbBMaxOut);
if (nodeOverlap)
{
bool isLeafA = isLeafNodeGlobal(&quantizedNodes[node.x]);
bool isLeafB = isLeafNodeGlobal(&quantizedNodes[node.y]);
bool isInternalA = !isLeafA;
bool isInternalB = !isLeafB;
//fail, even though it might hit two leaf nodes
if (depth+4>maxStackDepth && !(isLeafA && isLeafB))
{
//printf("Error: traversal exceeded maxStackDepth");
continue;
}
if(isInternalA)
{
int nodeAleftChild = node.x+1;
bool isNodeALeftChildLeaf = isLeafNodeGlobal(&quantizedNodes[node.x+1]);
int nodeArightChild = isNodeALeftChildLeaf? node.x+2 : node.x+1 + getEscapeIndexGlobal(&quantizedNodes[node.x+1]);
if(isInternalB)
{
int nodeBleftChild = node.y+1;
bool isNodeBLeftChildLeaf = isLeafNodeGlobal(&quantizedNodes[node.y+1]);
int nodeBrightChild = isNodeBLeftChildLeaf? node.y+2 : node.y+1 + getEscapeIndexGlobal(&quantizedNodes[node.y+1]);
nodeStack[depth++] = b3MakeInt2(nodeAleftChild, nodeBleftChild);
nodeStack[depth++] = b3MakeInt2(nodeArightChild, nodeBleftChild);
nodeStack[depth++] = b3MakeInt2(nodeAleftChild, nodeBrightChild);
nodeStack[depth++] = b3MakeInt2(nodeArightChild, nodeBrightChild);
}
else
{
nodeStack[depth++] = b3MakeInt2(nodeAleftChild,node.y);
nodeStack[depth++] = b3MakeInt2(nodeArightChild,node.y);
}
}
else
{
if(isInternalB)
{
int nodeBleftChild = node.y+1;
bool isNodeBLeftChildLeaf = isLeafNodeGlobal(&quantizedNodes[node.y+1]);
int nodeBrightChild = isNodeBLeftChildLeaf? node.y+2 : node.y+1 + getEscapeIndexGlobal(&quantizedNodes[node.y+1]);
nodeStack[depth++] = b3MakeInt2(node.x,nodeBleftChild);
nodeStack[depth++] = b3MakeInt2(node.x,nodeBrightChild);
}
else
{
int compoundPairIdx = atomic_inc(numCompoundPairsOut);
if (compoundPairIdx<maxNumCompoundPairsCapacity)
{
int childShapeIndexA = getTriangleIndexGlobal(&quantizedNodes[node.x]);
int childShapeIndexB = getTriangleIndexGlobal(&quantizedNodes[node.y]);
gpuCompoundPairsOut[compoundPairIdx] = (int4)(bodyIndexA,bodyIndexB,childShapeIndexA,childShapeIndexB);
}
}
}
}
} while (depth);
}
}
}
return;
}
if ((collidables[collidableIndexA].m_shapeType==SHAPE_COMPOUND_OF_CONVEX_HULLS) ||(collidables[collidableIndexB].m_shapeType==SHAPE_COMPOUND_OF_CONVEX_HULLS))
{
@@ -813,7 +1092,18 @@ __kernel void findCompoundPairsKernel( __global const int4* pairs,
float4 newOrnA = qtMul(ornA,childOrnA);
int shapeIndexA = collidables[childColIndexA].m_shapeIndex;
b3Aabb_t aabbAlocal = aabbLocalSpace[shapeIndexA];
float margin = 0.f;
b3Float4 aabbAMinWS;
b3Float4 aabbAMaxWS;
b3TransformAabb2(aabbAlocal.m_minVec,aabbAlocal.m_maxVec,margin,
newPosA,
newOrnA,
&aabbAMinWS,&aabbAMaxWS);
if (collidables[collidableIndexB].m_shapeType==SHAPE_COMPOUND_OF_CONVEX_HULLS)
{
int numChildrenB = collidables[collidableIndexB].m_numChildShapes;
@@ -829,8 +1119,20 @@ __kernel void findCompoundPairsKernel( __global const int4* pairs,
float4 newOrnB = qtMul(ornB,childOrnB);
int shapeIndexB = collidables[childColIndexB].m_shapeIndex;
if (1)
b3Aabb_t aabbBlocal = aabbLocalSpace[shapeIndexB];
b3Float4 aabbBMinWS;
b3Float4 aabbBMaxWS;
b3TransformAabb2(aabbBlocal.m_minVec,aabbBlocal.m_maxVec,margin,
newPosB,
newOrnB,
&aabbBMinWS,&aabbBMaxWS);
bool aabbOverlap = b3TestAabbAgainstAabb(aabbAMinWS,aabbAMaxWS,aabbBMinWS,aabbBMaxWS);
if (aabbOverlap)
{
int numFacesA = convexShapes[shapeIndexA].m_numFaces;
float dmin = FLT_MAX;

14
src/Bullet3OpenCL/NarrowphaseCollision/kernels/satClipHullContacts.h
View File

@@ -50,6 +50,20 @@ static const char* satClipKernelsCL= \
"#ifdef __cplusplus\n"
"#else\n"
" typedef float4 b3Float4;\n"
" #define b3Float4ConstArg const b3Float4\n"
" #define b3MakeFloat4 (float4)\n"
" float b3Dot3F4(b3Float4ConstArg v0,b3Float4ConstArg v1)\n"
" {\n"
" float4 a1 = b3MakeFloat4(v0.xyz,0.f);\n"
" float4 b1 = b3MakeFloat4(v1.xyz,0.f);\n"
" return dot(a1, b1);\n"
" }\n"
" b3Float4 b3Cross3(b3Float4ConstArg v0,b3Float4ConstArg v1)\n"
" {\n"
" float4 a1 = b3MakeFloat4(v0.xyz,0.f);\n"
" float4 b1 = b3MakeFloat4(v1.xyz,0.f);\n"
" return cross(a1, b1);\n"
" }\n"
"#endif \n"
"#endif //B3_FLOAT4_H\n"
"typedef struct b3Contact4Data b3Contact4Data_t;\n"

461
src/Bullet3OpenCL/NarrowphaseCollision/kernels/satKernels.h
View File

@@ -6,16 +6,90 @@ static const char* satKernelsCL= \
"#define SHAPE_CONCAVE_TRIMESH 5\n"
"#define TRIANGLE_NUM_CONVEX_FACES 5\n"
"#define SHAPE_COMPOUND_OF_CONVEX_HULLS 6\n"
"#define B3_MAX_STACK_DEPTH 256\n"
"typedef unsigned int u32;\n"
"///keep this in sync with btCollidable.h\n"
"typedef struct\n"
"{\n"
" int m_numChildShapes;\n"
" int blaat2;\n"
" union {\n"
" int m_numChildShapes;\n"
" int m_bvhIndex;\n"
" };\n"
" union\n"
" {\n"
" float m_radius;\n"
" int m_compoundBvhIndex;\n"
" };\n"
" \n"
" int m_shapeType;\n"
" int m_shapeIndex;\n"
" \n"
"} btCollidableGpu;\n"
"#define MAX_NUM_PARTS_IN_BITS 10\n"
"///b3QuantizedBvhNode is a compressed aabb node, 16 bytes.\n"
"///Node can be used for leafnode or internal node. Leafnodes can point to 32-bit triangle index (non-negative range).\n"
"typedef struct\n"
"{\n"
" //12 bytes\n"
" unsigned short int m_quantizedAabbMin[3];\n"
" unsigned short int m_quantizedAabbMax[3];\n"
" //4 bytes\n"
" int m_escapeIndexOrTriangleIndex;\n"
"} b3QuantizedBvhNode;\n"
"typedef struct\n"
"{\n"
" float4 m_aabbMin;\n"
" float4 m_aabbMax;\n"
" float4 m_quantization;\n"
" int m_numNodes;\n"
" int m_numSubTrees;\n"
" int m_nodeOffset;\n"
" int m_subTreeOffset;\n"
"} b3BvhInfo;\n"
"int getTriangleIndex(const b3QuantizedBvhNode* rootNode)\n"
"{\n"
" unsigned int x=0;\n"
" unsigned int y = (~(x&0))<<(31-MAX_NUM_PARTS_IN_BITS);\n"
" // Get only the lower bits where the triangle index is stored\n"
" return (rootNode->m_escapeIndexOrTriangleIndex&~(y));\n"
"}\n"
"int getTriangleIndexGlobal(__global const b3QuantizedBvhNode* rootNode)\n"
"{\n"
" unsigned int x=0;\n"
" unsigned int y = (~(x&0))<<(31-MAX_NUM_PARTS_IN_BITS);\n"
" // Get only the lower bits where the triangle index is stored\n"
" return (rootNode->m_escapeIndexOrTriangleIndex&~(y));\n"
"}\n"
"int isLeafNode(const b3QuantizedBvhNode* rootNode)\n"
"{\n"
" //skipindex is negative (internal node), triangleindex >=0 (leafnode)\n"
" return (rootNode->m_escapeIndexOrTriangleIndex >= 0)? 1 : 0;\n"
"}\n"
"int isLeafNodeGlobal(__global const b3QuantizedBvhNode* rootNode)\n"
"{\n"
" //skipindex is negative (internal node), triangleindex >=0 (leafnode)\n"
" return (rootNode->m_escapeIndexOrTriangleIndex >= 0)? 1 : 0;\n"
"}\n"
" \n"
"int getEscapeIndex(const b3QuantizedBvhNode* rootNode)\n"
"{\n"
" return -rootNode->m_escapeIndexOrTriangleIndex;\n"
"}\n"
"int getEscapeIndexGlobal(__global const b3QuantizedBvhNode* rootNode)\n"
"{\n"
" return -rootNode->m_escapeIndexOrTriangleIndex;\n"
"}\n"
"typedef struct\n"
"{\n"
" //12 bytes\n"
" unsigned short int m_quantizedAabbMin[3];\n"
" unsigned short int m_quantizedAabbMax[3];\n"
" //4 bytes, points to the root of the subtree\n"
" int m_rootNodeIndex;\n"
" //4 bytes\n"
" int m_subtreeSize;\n"
" int m_padding[3];\n"
"} b3BvhSubtreeInfo;\n"
"typedef struct\n"
"{\n"
" float4 m_childPosition;\n"
@@ -67,6 +141,210 @@ static const char* satKernelsCL= \
" int m_maxIndices[4];\n"
" };\n"
"} btAabbCL;\n"
"#ifndef B3_AABB_H\n"
"#define B3_AABB_H\n"
"#ifndef B3_FLOAT4_H\n"
"#define B3_FLOAT4_H\n"
"#ifndef B3_PLATFORM_DEFINITIONS_H\n"
"#define B3_PLATFORM_DEFINITIONS_H\n"
"struct MyTest\n"
"{\n"
" int bla;\n"
"};\n"
"#ifdef __cplusplus\n"
"#else\n"
"#define b3AtomicInc atomic_inc\n"
"#endif\n"
"#endif\n"
"#ifdef __cplusplus\n"
"#else\n"
" typedef float4 b3Float4;\n"
" #define b3Float4ConstArg const b3Float4\n"
" #define b3MakeFloat4 (float4)\n"
" float b3Dot3F4(b3Float4ConstArg v0,b3Float4ConstArg v1)\n"
" {\n"
" float4 a1 = b3MakeFloat4(v0.xyz,0.f);\n"
" float4 b1 = b3MakeFloat4(v1.xyz,0.f);\n"
" return dot(a1, b1);\n"
" }\n"
" b3Float4 b3Cross3(b3Float4ConstArg v0,b3Float4ConstArg v1)\n"
" {\n"
" float4 a1 = b3MakeFloat4(v0.xyz,0.f);\n"
" float4 b1 = b3MakeFloat4(v1.xyz,0.f);\n"
" return cross(a1, b1);\n"
" }\n"
"#endif \n"
"#endif //B3_FLOAT4_H\n"
"#ifndef B3_MAT3x3_H\n"
"#define B3_MAT3x3_H\n"
"#ifndef B3_QUAT_H\n"
"#define B3_QUAT_H\n"
"#ifndef B3_PLATFORM_DEFINITIONS_H\n"
"#ifdef __cplusplus\n"
"#else\n"
"#endif\n"
"#endif\n"
"#ifndef B3_FLOAT4_H\n"
"#ifdef __cplusplus\n"
"#else\n"
"#endif \n"
"#endif //B3_FLOAT4_H\n"
"#ifdef __cplusplus\n"
"#else\n"
" typedef float4 b3Quat;\n"
" #define b3QuatConstArg const b3Quat\n"
" \n"
" \n"
"inline float4 b3FastNormalize4(float4 v)\n"
"{\n"
" v = (float4)(v.xyz,0.f);\n"
" return fast_normalize(v);\n"
"}\n"
" \n"
"inline b3Quat b3QuatMul(b3Quat a, b3Quat b);\n"
"inline b3Quat b3QuatNormalize(b3QuatConstArg in);\n"
"inline b3Quat b3QuatRotate(b3QuatConstArg q, b3QuatConstArg vec);\n"
"inline b3Quat b3QuatInvert(b3QuatConstArg q);\n"
"inline b3Quat b3QuatMul(b3QuatConstArg a, b3QuatConstArg b)\n"
"{\n"
" b3Quat ans;\n"
" ans = b3Cross3( a, b );\n"
" ans += a.w*b+b.w*a;\n"
"// ans.w = a.w*b.w - (a.x*b.x+a.y*b.y+a.z*b.z);\n"
" ans.w = a.w*b.w - b3Dot3F4(a, b);\n"
" return ans;\n"
"}\n"
"inline b3Quat b3QuatNormalize(b3QuatConstArg in)\n"
"{\n"
" return b3FastNormalize4(in);\n"
"}\n"
"inline float4 b3QuatRotate(b3QuatConstArg q, b3QuatConstArg vec)\n"
"{\n"
" b3Quat qInv = b3QuatInvert( q );\n"
" float4 vcpy = vec;\n"
" vcpy.w = 0.f;\n"
" float4 out = b3QuatMul(b3QuatMul(q,vcpy),qInv);\n"
" return out;\n"
"}\n"
"inline b3Quat b3QuatInvert(b3QuatConstArg q)\n"
"{\n"
" return (b3Quat)(-q.xyz, q.w);\n"
"}\n"
"inline float4 b3QuatInvRotate(b3QuatConstArg q, b3QuatConstArg vec)\n"
"{\n"
" return b3QuatRotate( b3QuatInvert( q ), vec );\n"
"}\n"
"inline b3Float4 b3TransformPoint(b3Float4ConstArg point, b3Float4ConstArg translation, b3QuatConstArg orientation)\n"
"{\n"
" return b3QuatRotate( orientation, point ) + (translation);\n"
"}\n"
" \n"
"#endif \n"
"#endif //B3_QUAT_H\n"
"#ifdef __cplusplus\n"
"#else\n"
"typedef struct\n"
"{\n"
" float4 m_row[3];\n"
"}b3Mat3x3;\n"
"#define b3Mat3x3ConstArg const b3Mat3x3\n"
"#define b3GetRow(m,row) (m.m_row[row])\n"
"inline b3Mat3x3 b3QuatGetRotationMatrix(b3Quat quat)\n"
"{\n"
" float4 quat2 = (float4)(quat.x*quat.x, quat.y*quat.y, quat.z*quat.z, 0.f);\n"
" b3Mat3x3 out;\n"
" out.m_row[0].x=1-2*quat2.y-2*quat2.z;\n"
" out.m_row[0].y=2*quat.x*quat.y-2*quat.w*quat.z;\n"
" out.m_row[0].z=2*quat.x*quat.z+2*quat.w*quat.y;\n"
" out.m_row[0].w = 0.f;\n"
" out.m_row[1].x=2*quat.x*quat.y+2*quat.w*quat.z;\n"
" out.m_row[1].y=1-2*quat2.x-2*quat2.z;\n"
" out.m_row[1].z=2*quat.y*quat.z-2*quat.w*quat.x;\n"
" out.m_row[1].w = 0.f;\n"
" out.m_row[2].x=2*quat.x*quat.z-2*quat.w*quat.y;\n"
" out.m_row[2].y=2*quat.y*quat.z+2*quat.w*quat.x;\n"
" out.m_row[2].z=1-2*quat2.x-2*quat2.y;\n"
" out.m_row[2].w = 0.f;\n"
" return out;\n"
"}\n"
"inline b3Mat3x3 b3AbsoluteMat3x3(b3Mat3x3ConstArg matIn)\n"
"{\n"
" b3Mat3x3 out;\n"
" out.m_row[0] = fabs(matIn.m_row[0]);\n"
" out.m_row[1] = fabs(matIn.m_row[1]);\n"
" out.m_row[2] = fabs(matIn.m_row[2]);\n"
" return out;\n"
"}\n"
"#endif\n"
"#endif //B3_MAT3x3_H\n"
"typedef struct b3Aabb b3Aabb_t;\n"
"struct b3Aabb\n"
"{\n"
" union\n"
" {\n"
" float m_min[4];\n"
" b3Float4 m_minVec;\n"
" int m_minIndices[4];\n"
" };\n"
" union\n"
" {\n"
" float m_max[4];\n"
" b3Float4 m_maxVec;\n"
" int m_signedMaxIndices[4];\n"
" };\n"
"};\n"
"inline void b3TransformAabb2(b3Float4ConstArg localAabbMin,b3Float4ConstArg localAabbMax, float margin,\n"
" b3Float4ConstArg pos,\n"
" b3QuatConstArg orn,\n"
" b3Float4* aabbMinOut,b3Float4* aabbMaxOut)\n"
"{\n"
" b3Float4 localHalfExtents = 0.5f*(localAabbMax-localAabbMin);\n"
" localHalfExtents+=b3MakeFloat4(margin,margin,margin,0.f);\n"
" b3Float4 localCenter = 0.5f*(localAabbMax+localAabbMin);\n"
" b3Mat3x3 m;\n"
" m = b3QuatGetRotationMatrix(orn);\n"
" b3Mat3x3 abs_b = b3AbsoluteMat3x3(m);\n"
" b3Float4 center = b3TransformPoint(localCenter,pos,orn);\n"
" \n"
" b3Float4 extent = b3MakeFloat4(b3Dot3F4(localHalfExtents,b3GetRow(abs_b,0)),\n"
" b3Dot3F4(localHalfExtents,b3GetRow(abs_b,1)),\n"
" b3Dot3F4(localHalfExtents,b3GetRow(abs_b,2)),\n"
" 0.f);\n"
" *aabbMinOut = center-extent;\n"
" *aabbMaxOut = center+extent;\n"
"}\n"
"/// conservative test for overlap between two aabbs\n"
"inline bool b3TestAabbAgainstAabb(b3Float4ConstArg aabbMin1,b3Float4ConstArg aabbMax1,\n"
" b3Float4ConstArg aabbMin2, b3Float4ConstArg aabbMax2)\n"
"{\n"
" bool overlap = true;\n"
" overlap = (aabbMin1.x > aabbMax2.x || aabbMax1.x < aabbMin2.x) ? false : overlap;\n"
" overlap = (aabbMin1.z > aabbMax2.z || aabbMax1.z < aabbMin2.z) ? false : overlap;\n"
" overlap = (aabbMin1.y > aabbMax2.y || aabbMax1.y < aabbMin2.y) ? false : overlap;\n"
" return overlap;\n"
"}\n"
"#endif //B3_AABB_H\n"
"/*\n"
"Bullet Continuous Collision Detection and Physics Library\n"
"Copyright (c) 2003-2013 Erwin Coumans http://bulletphysics.org\n"
"This software is provided 'as-is', without any express or implied warranty.\n"
"In no event will the authors be held liable for any damages arising from the use of this software.\n"
"Permission is granted to anyone to use this software for any purpose,\n"
"including commercial applications, and to alter it and redistribute it freely,\n"
"subject to the following restrictions:\n"
"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.\n"
"2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.\n"
"3. This notice may not be removed or altered from any source distribution.\n"
"*/\n"
"#ifndef B3_INT2_H\n"
"#define B3_INT2_H\n"
"#ifdef __cplusplus\n"
"#else\n"
"#define b3UnsignedInt2 uint2\n"
"#define b3Int2 int2\n"
"#define b3MakeInt2 (int2)\n"
"#endif //__cplusplus\n"
"#endif\n"
"typedef struct\n"
"{\n"
" float4 m_plane;\n"
@@ -650,6 +928,28 @@ static const char* satKernelsCL= \
" }\n"
" \n"
"}\n"
"inline b3Float4 MyUnQuantize(const unsigned short* vecIn, b3Float4 quantization, b3Float4 bvhAabbMin)\n"
"{\n"
" b3Float4 vecOut;\n"
" vecOut = b3MakeFloat4(\n"
" (float)(vecIn[0]) / (quantization.x),\n"
" (float)(vecIn[1]) / (quantization.y),\n"
" (float)(vecIn[2]) / (quantization.z),\n"
" 0.f);\n"
" vecOut += bvhAabbMin;\n"
" return vecOut;\n"
"}\n"
"inline b3Float4 MyUnQuantizeGlobal(__global const unsigned short* vecIn, b3Float4 quantization, b3Float4 bvhAabbMin)\n"
"{\n"
" b3Float4 vecOut;\n"
" vecOut = b3MakeFloat4(\n"
" (float)(vecIn[0]) / (quantization.x),\n"
" (float)(vecIn[1]) / (quantization.y),\n"
" (float)(vecIn[2]) / (quantization.z),\n"
" 0.f);\n"
" vecOut += bvhAabbMin;\n"
" return vecOut;\n"
"}\n"
"// work-in-progress\n"
"__kernel void findCompoundPairsKernel( __global const int4* pairs, \n"
" __global const BodyData* rigidBodies, \n"
@@ -659,10 +959,13 @@ static const char* satKernelsCL= \
" __global const float4* uniqueEdges,\n"
" __global const btGpuFace* faces,\n"
" __global const int* indices,\n"
" __global btAabbCL* aabbs,\n"
" __global b3Aabb_t* aabbLocalSpace,\n"
" __global const btGpuChildShape* gpuChildShapes,\n"
" __global volatile int4* gpuCompoundPairsOut,\n"
" __global volatile int* numCompoundPairsOut,\n"
" __global const b3BvhSubtreeInfo* subtrees,\n"
" __global const b3QuantizedBvhNode* quantizedNodes,\n"
" __global const b3BvhInfo* bvhInfos,\n"
" int numPairs,\n"
" int maxNumCompoundPairsCapacity\n"
" )\n"
@@ -681,6 +984,131 @@ static const char* satKernelsCL= \
" {\n"
" return;\n"
" }\n"
" if ((collidables[collidableIndexA].m_shapeType==SHAPE_COMPOUND_OF_CONVEX_HULLS) &&(collidables[collidableIndexB].m_shapeType==SHAPE_COMPOUND_OF_CONVEX_HULLS))\n"
" {\n"
" int bvhA = collidables[collidableIndexA].m_compoundBvhIndex;\n"
" int bvhB = collidables[collidableIndexB].m_compoundBvhIndex;\n"
" int numSubTreesA = bvhInfos[bvhA].m_numSubTrees;\n"
" int subTreesOffsetA = bvhInfos[bvhA].m_subTreeOffset;\n"
" int subTreesOffsetB = bvhInfos[bvhB].m_subTreeOffset;\n"
" int numSubTreesB = bvhInfos[bvhB].m_numSubTrees;\n"
" \n"
" float4 posA = rigidBodies[bodyIndexA].m_pos;\n"
" b3Quat ornA = rigidBodies[bodyIndexA].m_quat;\n"
" b3Quat ornB = rigidBodies[bodyIndexB].m_quat;\n"
" float4 posB = rigidBodies[bodyIndexB].m_pos;\n"
" \n"
" for (int p=0;p<numSubTreesA;p++)\n"
" {\n"
" b3BvhSubtreeInfo subtreeA = subtrees[subTreesOffsetA+p];\n"
" //bvhInfos[bvhA].m_quantization\n"
" b3Float4 treeAminLocal = MyUnQuantize(subtreeA.m_quantizedAabbMin,bvhInfos[bvhA].m_quantization,bvhInfos[bvhA].m_aabbMin);\n"
" b3Float4 treeAmaxLocal = MyUnQuantize(subtreeA.m_quantizedAabbMax,bvhInfos[bvhA].m_quantization,bvhInfos[bvhA].m_aabbMin);\n"
" b3Float4 aabbAMinOut,aabbAMaxOut;\n"
" float margin=0.f;\n"
" b3TransformAabb2(treeAminLocal,treeAmaxLocal, margin,posA,ornA,&aabbAMinOut,&aabbAMaxOut);\n"
" \n"
" for (int q=0;q<numSubTreesB;q++)\n"
" {\n"
" b3BvhSubtreeInfo subtreeB = subtrees[subTreesOffsetB+q];\n"
" b3Float4 treeBminLocal = MyUnQuantize(subtreeB.m_quantizedAabbMin,bvhInfos[bvhB].m_quantization,bvhInfos[bvhB].m_aabbMin);\n"
" b3Float4 treeBmaxLocal = MyUnQuantize(subtreeB.m_quantizedAabbMax,bvhInfos[bvhB].m_quantization,bvhInfos[bvhB].m_aabbMin);\n"
" b3Float4 aabbBMinOut,aabbBMaxOut;\n"
" float margin=0.f;\n"
" b3TransformAabb2(treeBminLocal,treeBmaxLocal, margin,posB,ornB,&aabbBMinOut,&aabbBMaxOut);\n"
" \n"
" \n"
" bool aabbOverlap = b3TestAabbAgainstAabb(aabbAMinOut,aabbAMaxOut,aabbBMinOut,aabbBMaxOut);\n"
" if (aabbOverlap)\n"
" {\n"
" \n"
" int startNodeIndexA = subtreeA.m_rootNodeIndex+bvhInfos[bvhA].m_nodeOffset;\n"
" int endNodeIndexA = startNodeIndexA+subtreeA.m_subtreeSize;\n"
" int startNodeIndexB = subtreeB.m_rootNodeIndex+bvhInfos[bvhB].m_nodeOffset;\n"
" int endNodeIndexB = startNodeIndexB+subtreeB.m_subtreeSize;\n"
" b3Int2 nodeStack[B3_MAX_STACK_DEPTH];\n"
" b3Int2 node0;\n"
" node0.x = startNodeIndexA;\n"
" node0.y = startNodeIndexB;\n"
" int maxStackDepth = B3_MAX_STACK_DEPTH;\n"
" int depth=0;\n"
" nodeStack[depth++]=node0;\n"
" do\n"
" {\n"
" b3Int2 node = nodeStack[--depth];\n"
" b3Float4 aMinLocal = MyUnQuantizeGlobal(quantizedNodes[node.x].m_quantizedAabbMin,bvhInfos[bvhA].m_quantization,bvhInfos[bvhA].m_aabbMin);\n"
" b3Float4 aMaxLocal = MyUnQuantizeGlobal(quantizedNodes[node.x].m_quantizedAabbMax,bvhInfos[bvhA].m_quantization,bvhInfos[bvhA].m_aabbMin);\n"
" b3Float4 bMinLocal = MyUnQuantizeGlobal(quantizedNodes[node.y].m_quantizedAabbMin,bvhInfos[bvhB].m_quantization,bvhInfos[bvhB].m_aabbMin);\n"
" b3Float4 bMaxLocal = MyUnQuantizeGlobal(quantizedNodes[node.y].m_quantizedAabbMax,bvhInfos[bvhB].m_quantization,bvhInfos[bvhB].m_aabbMin);\n"
" float margin=0.f;\n"
" b3Float4 aabbAMinOut,aabbAMaxOut;\n"
" b3TransformAabb2(aMinLocal,aMaxLocal, margin,posA,ornA,&aabbAMinOut,&aabbAMaxOut);\n"
" b3Float4 aabbBMinOut,aabbBMaxOut;\n"
" b3TransformAabb2(bMinLocal,bMaxLocal, margin,posB,ornB,&aabbBMinOut,&aabbBMaxOut);\n"
" \n"
" bool nodeOverlap = b3TestAabbAgainstAabb(aabbAMinOut,aabbAMaxOut,aabbBMinOut,aabbBMaxOut);\n"
" if (nodeOverlap)\n"
" {\n"
" bool isLeafA = isLeafNodeGlobal(&quantizedNodes[node.x]);\n"
" bool isLeafB = isLeafNodeGlobal(&quantizedNodes[node.y]);\n"
" bool isInternalA = !isLeafA;\n"
" bool isInternalB = !isLeafB;\n"
" //fail, even though it might hit two leaf nodes\n"
" if (depth+4>maxStackDepth && !(isLeafA && isLeafB))\n"
" {\n"
" //printf(\"Error: traversal exceeded maxStackDepth\");\n"
" continue;\n"
" }\n"
" if(isInternalA)\n"
" {\n"
" int nodeAleftChild = node.x+1;\n"
" bool isNodeALeftChildLeaf = isLeafNodeGlobal(&quantizedNodes[node.x+1]);\n"
" int nodeArightChild = isNodeALeftChildLeaf? node.x+2 : node.x+1 + getEscapeIndexGlobal(&quantizedNodes[node.x+1]);\n"
" if(isInternalB)\n"
" { \n"
" int nodeBleftChild = node.y+1;\n"
" bool isNodeBLeftChildLeaf = isLeafNodeGlobal(&quantizedNodes[node.y+1]);\n"
" int nodeBrightChild = isNodeBLeftChildLeaf? node.y+2 : node.y+1 + getEscapeIndexGlobal(&quantizedNodes[node.y+1]);\n"
" nodeStack[depth++] = b3MakeInt2(nodeAleftChild, nodeBleftChild);\n"
" nodeStack[depth++] = b3MakeInt2(nodeArightChild, nodeBleftChild);\n"
" nodeStack[depth++] = b3MakeInt2(nodeAleftChild, nodeBrightChild);\n"
" nodeStack[depth++] = b3MakeInt2(nodeArightChild, nodeBrightChild);\n"
" }\n"
" else\n"
" {\n"
" nodeStack[depth++] = b3MakeInt2(nodeAleftChild,node.y);\n"
" nodeStack[depth++] = b3MakeInt2(nodeArightChild,node.y);\n"
" }\n"
" }\n"
" else\n"
" {\n"
" if(isInternalB)\n"
" {\n"
" int nodeBleftChild = node.y+1;\n"
" bool isNodeBLeftChildLeaf = isLeafNodeGlobal(&quantizedNodes[node.y+1]);\n"
" int nodeBrightChild = isNodeBLeftChildLeaf? node.y+2 : node.y+1 + getEscapeIndexGlobal(&quantizedNodes[node.y+1]);\n"
" nodeStack[depth++] = b3MakeInt2(node.x,nodeBleftChild);\n"
" nodeStack[depth++] = b3MakeInt2(node.x,nodeBrightChild);\n"
" }\n"
" else\n"
" {\n"
" int compoundPairIdx = atomic_inc(numCompoundPairsOut);\n"
" if (compoundPairIdx<maxNumCompoundPairsCapacity)\n"
" {\n"
" int childShapeIndexA = getTriangleIndexGlobal(&quantizedNodes[node.x]);\n"
" int childShapeIndexB = getTriangleIndexGlobal(&quantizedNodes[node.y]);\n"
" gpuCompoundPairsOut[compoundPairIdx] = (int4)(bodyIndexA,bodyIndexB,childShapeIndexA,childShapeIndexB);\n"
" }\n"
" }\n"
" }\n"
" }\n"
" } while (depth);\n"
" }\n"
" }\n"
" }\n"
" \n"
" return;\n"
" }\n"
" if ((collidables[collidableIndexA].m_shapeType==SHAPE_COMPOUND_OF_CONVEX_HULLS) ||(collidables[collidableIndexB].m_shapeType==SHAPE_COMPOUND_OF_CONVEX_HULLS))\n"
" {\n"
" if (collidables[collidableIndexA].m_shapeType==SHAPE_COMPOUND_OF_CONVEX_HULLS) \n"
@@ -697,6 +1125,18 @@ static const char* satKernelsCL= \
" float4 newPosA = qtRotate(ornA,childPosA)+posA;\n"
" float4 newOrnA = qtMul(ornA,childOrnA);\n"
" int shapeIndexA = collidables[childColIndexA].m_shapeIndex;\n"
" b3Aabb_t aabbAlocal = aabbLocalSpace[shapeIndexA];\n"
" float margin = 0.f;\n"
" \n"
" b3Float4 aabbAMinWS;\n"
" b3Float4 aabbAMaxWS;\n"
" \n"
" b3TransformAabb2(aabbAlocal.m_minVec,aabbAlocal.m_maxVec,margin,\n"
" newPosA,\n"
" newOrnA,\n"
" &aabbAMinWS,&aabbAMaxWS);\n"
" \n"
" \n"
" if (collidables[collidableIndexB].m_shapeType==SHAPE_COMPOUND_OF_CONVEX_HULLS)\n"
" {\n"
" int numChildrenB = collidables[collidableIndexB].m_numChildShapes;\n"
@@ -711,7 +1151,20 @@ static const char* satKernelsCL= \
" float4 newPosB = transform(&childPosB,&posB,&ornB);\n"
" float4 newOrnB = qtMul(ornB,childOrnB);\n"
" int shapeIndexB = collidables[childColIndexB].m_shapeIndex;\n"
" if (1)\n"
" b3Aabb_t aabbBlocal = aabbLocalSpace[shapeIndexB];\n"
" \n"
" b3Float4 aabbBMinWS;\n"
" b3Float4 aabbBMaxWS;\n"
" \n"
" b3TransformAabb2(aabbBlocal.m_minVec,aabbBlocal.m_maxVec,margin,\n"
" newPosB,\n"
" newOrnB,\n"
" &aabbBMinWS,&aabbBMaxWS);\n"
" \n"
" \n"
" \n"
" bool aabbOverlap = b3TestAabbAgainstAabb(aabbAMinWS,aabbAMaxWS,aabbBMinWS,aabbBMaxWS);\n"
" if (aabbOverlap)\n"
" {\n"
" int numFacesA = convexShapes[shapeIndexA].m_numFaces;\n"
" float dmin = FLT_MAX;\n"