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

Ported Minkowski Portal Refinement mpr.c from libccd to OpenCL, for bettwe edge-edge performance (and additional contact point for degenerate/high detailed convex shapes)

Browse Source 
Removed b3RigidBodyCL, replace by b3RigidBodyData and b3RigidBodyData_t shared between C++ host and OpenCL,
Same for b3InertiaCL -> b3InertiaData
This commit is contained in:
erwincoumans
2014-01-04 20:54:27 -08:00
parent 999c5ff766
commit 271f458837
52 changed files with 3368 additions and 727 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -16,6 +16,7 @@ subject to the following restrictions:
bool findSeparatingAxisOnGpu = true;
bool splitSearchSepAxisConcave = false;
bool splitSearchSepAxisConvex = true;
bool useMprGpu = false;//use mpr for edge-edge (+contact point) or sat. Needs testing on main OpenCL platforms, before enabling...
bool bvhTraversalKernelGPU = true;
bool findConcaveSeparatingAxisKernelGPU = true;
bool clipConcaveFacesAndFindContactsCPU = false;//false;//true;
@@ -23,6 +24,11 @@ bool clipConvexFacesAndFindContactsCPU = false;//false;//true;
bool reduceConcaveContactsOnGPU = true;//false;
bool reduceConvexContactsOnGPU = true;//false;
bool findConvexClippingFacesGPU = true;
bool useGjk = true;///option for CPU/host testing, when findSeparatingAxisOnGpu = false
bool useGjkContacts = true;//////option for CPU/host testing when findSeparatingAxisOnGpu = false
static int myframecount=0;///for testing
///This file was written by Erwin Coumans
///Separating axis rest based on work from Pierre Terdiman, see
@@ -40,7 +46,9 @@ int b3g_actualSATPairTests=0;
#include "b3ConvexHullContact.h"
#include <string.h>//memcpy
#include "b3ConvexPolyhedronCL.h"
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h"
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3MprPenetration.h"
#include "Bullet3OpenCL/NarrowphaseCollision/b3ContactCache.h"
#include "Bullet3Geometry/b3AabbUtil.h"
@@ -53,6 +61,8 @@ typedef b3AlignedObjectArray<b3Vector3> b3VertexArray;
//#include "AdlQuaternion.h"
#include "kernels/satKernels.h"
#include "kernels/mprKernels.h"
#include "kernels/satConcaveKernels.h"
#include "kernels/satClipHullContacts.h"
@@ -65,6 +75,7 @@ typedef b3AlignedObjectArray<b3Vector3> b3VertexArray;
#define BT_NARROWPHASE_SAT_PATH "src/Bullet3OpenCL/NarrowphaseCollision/kernels/sat.cl"
#define BT_NARROWPHASE_SAT_CONCAVE_PATH "src/Bullet3OpenCL/NarrowphaseCollision/kernels/satConcave.cl"
#define BT_NARROWPHASE_MPR_PATH "src/Bullet3OpenCL/NarrowphaseCollision/kernels/mpr.cl"
#define BT_NARROWPHASE_CLIPHULL_PATH "src/Bullet3OpenCL/NarrowphaseCollision/kernels/satClipHullContacts.cl"
@@ -115,12 +126,25 @@ m_dmins(m_context,m_queue)
if (1)
{
const char* mprSrc = mprKernelsCL;
const char* src = satKernelsCL;
const char* srcConcave = satConcaveKernelsCL;
char flags[1024]={0};
//#ifdef CL_PLATFORM_INTEL
// sprintf(flags,"-g -s \"%s\"","C:/develop/bullet3_experiments2/opencl/gpu_narrowphase/kernels/sat.cl");
//#endif
m_mprPenetrationKernel = 0;
if (useMprGpu)
{
cl_program mprProg = b3OpenCLUtils::compileCLProgramFromString(m_context,m_device,mprSrc,&errNum,flags,BT_NARROWPHASE_MPR_PATH);
b3Assert(errNum==CL_SUCCESS);
m_mprPenetrationKernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device,mprSrc, "mprPenetrationKernel",&errNum,mprProg );
b3Assert(m_mprPenetrationKernel);
b3Assert(errNum==CL_SUCCESS);
}
cl_program satProg = b3OpenCLUtils::compileCLProgramFromString(m_context,m_device,src,&errNum,flags,BT_NARROWPHASE_SAT_PATH);
b3Assert(errNum==CL_SUCCESS);
@@ -251,6 +275,9 @@ GpuSatCollision::~GpuSatCollision()
clReleaseKernel(m_findSeparatingAxisEdgeEdgeKernel);
if (m_mprPenetrationKernel)
clReleaseKernel(m_mprPenetrationKernel);
if (m_findSeparatingAxisKernel)
clReleaseKernel(m_findSeparatingAxisKernel);
@@ -1158,12 +1185,12 @@ int clipHullHullSingle(
int collidableIndexA, int collidableIndexB,
const b3AlignedObjectArray<b3RigidBodyCL>* bodyBuf,
const b3AlignedObjectArray<b3RigidBodyData>* bodyBuf,
b3AlignedObjectArray<b3Contact4>* globalContactOut,
int& nContacts,
const b3AlignedObjectArray<b3ConvexPolyhedronCL>& hostConvexDataA,
const b3AlignedObjectArray<b3ConvexPolyhedronCL>& hostConvexDataB,
const b3AlignedObjectArray<b3ConvexPolyhedronData>& hostConvexDataA,
const b3AlignedObjectArray<b3ConvexPolyhedronData>& hostConvexDataB,
const b3AlignedObjectArray<b3Vector3>& verticesA,
const b3AlignedObjectArray<b3Vector3>& uniqueEdgesA,
@@ -1181,7 +1208,7 @@ int clipHullHullSingle(
int maxContactCapacity )
{
int contactIndex = -1;
b3ConvexPolyhedronCL hullA, hullB;
b3ConvexPolyhedronData hullA, hullB;
b3Collidable colA = hostCollidablesA[collidableIndexA];
hullA = hostConvexDataA[colA.m_shapeIndex];
@@ -1303,9 +1330,9 @@ int clipHullHullSingle(
void computeContactPlaneConvex(int pairIndex,
int bodyIndexA, int bodyIndexB,
int collidableIndexA, int collidableIndexB,
const b3RigidBodyCL* rigidBodies,
const b3RigidBodyData* rigidBodies,
const b3Collidable* collidables,
const b3ConvexPolyhedronCL* convexShapes,
const b3ConvexPolyhedronData* convexShapes,
const b3Vector3* convexVertices,
const int* convexIndices,
const b3GpuFace* faces,
@@ -1315,7 +1342,7 @@ void computeContactPlaneConvex(int pairIndex,
{
int shapeIndex = collidables[collidableIndexB].m_shapeIndex;
const b3ConvexPolyhedronCL* hullB = &convexShapes[shapeIndex];
const b3ConvexPolyhedronData* hullB = &convexShapes[shapeIndex];
b3Vector3 posB = rigidBodies[bodyIndexB].m_pos;
b3Quaternion ornB = rigidBodies[bodyIndexB].m_quat;
@@ -2085,7 +2112,7 @@ __kernel void clipCompoundsHullHullKernel( __global const b3Int4* gpuCompoundP
void computeContactCompoundCompound(int pairIndex,
int bodyIndexA, int bodyIndexB,
int collidableIndexA, int collidableIndexB,
const b3RigidBodyCL* rigidBodies,
const b3RigidBodyData* rigidBodies,
const b3Collidable* collidables,
const b3ConvexPolyhedronData* convexShapes,
const b3GpuChildShape* cpuChildShapes,
@@ -2238,7 +2265,7 @@ void computeContactCompoundCompound(int pairIndex,
int shapeIndexB = collidables[childColIndexB].m_shapeIndex;
const b3ConvexPolyhedronCL* hullB = &convexShapes[shapeIndexB];
const b3ConvexPolyhedronData* hullB = &convexShapes[shapeIndexB];
}
*/
@@ -2248,9 +2275,9 @@ void computeContactCompoundCompound(int pairIndex,
void computeContactPlaneCompound(int pairIndex,
int bodyIndexA, int bodyIndexB,
int collidableIndexA, int collidableIndexB,
const b3RigidBodyCL* rigidBodies,
const b3RigidBodyData* rigidBodies,
const b3Collidable* collidables,
const b3ConvexPolyhedronCL* convexShapes,
const b3ConvexPolyhedronData* convexShapes,
const b3GpuChildShape* cpuChildShapes,
const b3Vector3* convexVertices,
const int* convexIndices,
@@ -2280,7 +2307,7 @@ void computeContactPlaneCompound(int pairIndex,
int shapeIndexB = collidables[childColIndexB].m_shapeIndex;
const b3ConvexPolyhedronCL* hullB = &convexShapes[shapeIndexB];
const b3ConvexPolyhedronData* hullB = &convexShapes[shapeIndexB];
b3Vector3 posA = rigidBodies[bodyIndexA].m_pos;
@@ -2401,9 +2428,9 @@ void computeContactPlaneCompound(int pairIndex,
void computeContactSphereConvex(int pairIndex,
int bodyIndexA, int bodyIndexB,
int collidableIndexA, int collidableIndexB,
const b3RigidBodyCL* rigidBodies,
const b3RigidBodyData* rigidBodies,
const b3Collidable* collidables,
const b3ConvexPolyhedronCL* convexShapes,
const b3ConvexPolyhedronData* convexShapes,
const b3Vector3* convexVertices,
const int* convexIndices,
const b3GpuFace* faces,
@@ -2562,9 +2589,9 @@ int computeContactConvexConvex( b3AlignedObjectArray<b3Int4>& pairs,
int pairIndex,
int bodyIndexA, int bodyIndexB,
int collidableIndexA, int collidableIndexB,
const b3AlignedObjectArray<b3RigidBodyCL>& rigidBodies,
const b3AlignedObjectArray<b3RigidBodyData>& rigidBodies,
const b3AlignedObjectArray<b3Collidable>& collidables,
const b3AlignedObjectArray<b3ConvexPolyhedronCL>& convexShapes,
const b3AlignedObjectArray<b3ConvexPolyhedronData>& convexShapes,
const b3AlignedObjectArray<b3Vector3>& convexVertices,
const b3AlignedObjectArray<b3Vector3>& uniqueEdges,
const b3AlignedObjectArray<int>& convexIndices,
@@ -2572,7 +2599,11 @@ int computeContactConvexConvex( b3AlignedObjectArray<b3Int4>& pairs,
b3AlignedObjectArray<b3Contact4>& globalContactsOut,
int& nGlobalContactsOut,
int maxContactCapacity,
const b3AlignedObjectArray<b3Contact4>& oldContacts
b3AlignedObjectArray<int>&hostHasSepAxis,
b3AlignedObjectArray<b3Vector3>&hostSepAxis
//,const b3AlignedObjectArray<b3Contact4>& oldContacts
)
{
int contactIndex = -1;
@@ -2629,10 +2660,10 @@ int computeContactConvexConvex( b3AlignedObjectArray<b3Int4>& pairs,
//printf("add existing points?\n");
//refresh
int numOldPoints = oldContacts[pairs[pairIndex].z].getNPoints();
int numOldPoints = 0;//oldContacts[pairs[pairIndex].z].getNPoints();
if (numOldPoints)
{
newContact = oldContacts[pairs[pairIndex].z];
//newContact = oldContacts[pairs[pairIndex].z];
#ifdef PERSISTENT_CONTACTS_HOST
b3ContactCache::refreshContactPoints(transA,transB,newContact);
#endif //PERSISTENT_CONTACTS_HOST
@@ -2670,7 +2701,12 @@ int computeContactConvexConvex( b3AlignedObjectArray<b3Int4>& pairs,
newContact.m_localPosA[p] = transA.inverse()*resultPointOnAWorld;
newContact.m_localPosB[p] = transB.inverse()*resultPointOnBWorld;
#endif
newContact.m_worldNormalOnB = sepAxis2;
newContact.m_worldNormalOnB = sepAxis2;
hostHasSepAxis[pairIndex] = 1;
hostSepAxis[pairIndex] =sepAxis2;
//printf("sepAxis[%d]=%f,%f,%f,%f\n",pairIndex,sepAxis2.x,sepAxis2.y,sepAxis2.z,sepAxis2.w);
}
//printf("bodyIndexA %d,bodyIndexB %d,normal=%f,%f,%f numPoints %d\n",bodyIndexA,bodyIndexB,normalOnSurfaceB.x,normalOnSurfaceB.y,normalOnSurfaceB.z,numPoints);
newContact.m_worldNormalOnB.w = (b3Scalar)numPoints;
@@ -2690,9 +2726,9 @@ int computeContactConvexConvex2(
int pairIndex,
int bodyIndexA, int bodyIndexB,
int collidableIndexA, int collidableIndexB,
const b3AlignedObjectArray<b3RigidBodyCL>& rigidBodies,
const b3AlignedObjectArray<b3RigidBodyData>& rigidBodies,
const b3AlignedObjectArray<b3Collidable>& collidables,
const b3AlignedObjectArray<b3ConvexPolyhedronCL>& convexShapes,
const b3AlignedObjectArray<b3ConvexPolyhedronData>& convexShapes,
const b3AlignedObjectArray<b3Vector3>& convexVertices,
const b3AlignedObjectArray<b3Vector3>& uniqueEdges,
const b3AlignedObjectArray<int>& convexIndices,
@@ -2710,7 +2746,7 @@ int computeContactConvexConvex2(
b3Quaternion ornB = rigidBodies[bodyIndexB].m_quat;
b3ConvexPolyhedronCL hullA, hullB;
b3ConvexPolyhedronData hullA, hullB;
b3Vector3 sepNormalWorldSpace;
@@ -2787,15 +2823,17 @@ int computeContactConvexConvex2(
void GpuSatCollision::computeConvexConvexContactsGPUSAT( b3OpenCLArray<b3Int4>* pairs, int nPairs,
const b3OpenCLArray<b3RigidBodyCL>* bodyBuf,
const b3OpenCLArray<b3RigidBodyData>* bodyBuf,
b3OpenCLArray<b3Contact4>* contactOut, int& nContacts,
const b3OpenCLArray<b3Contact4>* oldContacts,
int maxContactCapacity,
int compoundPairCapacity,
const b3OpenCLArray<b3ConvexPolyhedronCL>& convexData,
const b3OpenCLArray<b3ConvexPolyhedronData>& convexData,
const b3OpenCLArray<b3Vector3>& gpuVertices,
const b3OpenCLArray<b3Vector3>& gpuUniqueEdges,
const b3OpenCLArray<b3GpuFace>& gpuFaces,
@@ -2822,6 +2860,8 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( b3OpenCLArray<b3Int4>*
int& numTriConvexPairsOut
)
{
myframecount++;
if (!nPairs)
return;
@@ -2846,12 +2886,12 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( b3OpenCLArray<b3Int4>*
b3AlignedObjectArray<b3Int4> hostPairs;
pairs->copyToHost(hostPairs);
b3AlignedObjectArray<b3RigidBodyCL> hostBodyBuf;
b3AlignedObjectArray<b3RigidBodyData> hostBodyBuf;
bodyBuf->copyToHost(hostBodyBuf);
b3AlignedObjectArray<b3ConvexPolyhedronCL> hostConvexData;
b3AlignedObjectArray<b3ConvexPolyhedronData> hostConvexData;
convexData.copyToHost(hostConvexData);
b3AlignedObjectArray<b3Vector3> hostVertices;
@@ -2962,8 +3002,8 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( b3OpenCLArray<b3Int4>*
hostCollidables[collidableIndexB].m_shapeType == SHAPE_CONVEX_HULL)
{
//printf("hostPairs[i].z=%d\n",hostPairs[i].z);
int contactIndex = computeContactConvexConvex2(i,bodyIndexA,bodyIndexB,collidableIndexA,collidableIndexB,hostBodyBuf, hostCollidables,hostConvexData,hostVertices,hostUniqueEdges,hostIndices,hostFaces,hostContacts,nContacts,maxContactCapacity,oldHostContacts);
//int contactIndex = computeContactConvexConvex(hostPairs,i,bodyIndexA,bodyIndexB,collidableIndexA,collidableIndexB,hostBodyBuf,hostCollidables,hostConvexData,hostVertices,hostUniqueEdges,hostIndices,hostFaces,hostContacts,nContacts,maxContactCapacity,oldHostContacts);
//int contactIndex = computeContactConvexConvex2(i,bodyIndexA,bodyIndexB,collidableIndexA,collidableIndexB,hostBodyBuf, hostCollidables,hostConvexData,hostVertices,hostUniqueEdges,hostIndices,hostFaces,hostContacts,nContacts,maxContactCapacity,oldHostContacts);
int contactIndex = computeContactConvexConvex(hostPairs,i,bodyIndexA,bodyIndexB,collidableIndexA,collidableIndexB,hostBodyBuf,hostCollidables,hostConvexData,hostVertices,hostUniqueEdges,hostIndices,hostFaces,hostContacts,nContacts,maxContactCapacity,oldHostContacts);
if (contactIndex>=0)
@@ -3068,56 +3108,109 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( b3OpenCLArray<b3Int4>*
m_dmins.resize(nPairs);
if (splitSearchSepAxisConvex)
{
{
B3_PROFILE("findSeparatingAxisVertexFaceKernel");
b3BufferInfoCL bInfo[] = {
b3BufferInfoCL( pairs->getBufferCL(), true ),
b3BufferInfoCL( bodyBuf->getBufferCL(),true),
b3BufferInfoCL( gpuCollidables.getBufferCL(),true),
b3BufferInfoCL( convexData.getBufferCL(),true),
b3BufferInfoCL( gpuVertices.getBufferCL(),true),
b3BufferInfoCL( gpuUniqueEdges.getBufferCL(),true),
b3BufferInfoCL( gpuFaces.getBufferCL(),true),
b3BufferInfoCL( gpuIndices.getBufferCL(),true),
b3BufferInfoCL( clAabbsWorldSpace.getBufferCL(),true),
b3BufferInfoCL( m_sepNormals.getBufferCL()),
b3BufferInfoCL( m_hasSeparatingNormals.getBufferCL()),
b3BufferInfoCL( m_dmins.getBufferCL())
};
B3_PROFILE("findSeparatingAxisVertexFaceKernel");
b3BufferInfoCL bInfo[] = {
b3BufferInfoCL( pairs->getBufferCL(), true ),
b3BufferInfoCL( bodyBuf->getBufferCL(),true),
b3BufferInfoCL( gpuCollidables.getBufferCL(),true),
b3BufferInfoCL( convexData.getBufferCL(),true),
b3BufferInfoCL( gpuVertices.getBufferCL(),true),
b3BufferInfoCL( gpuUniqueEdges.getBufferCL(),true),
b3BufferInfoCL( gpuFaces.getBufferCL(),true),
b3BufferInfoCL( gpuIndices.getBufferCL(),true),
b3BufferInfoCL( clAabbsWorldSpace.getBufferCL(),true),
b3BufferInfoCL( m_sepNormals.getBufferCL()),
b3BufferInfoCL( m_hasSeparatingNormals.getBufferCL()),
b3BufferInfoCL( m_dmins.getBufferCL())
};
b3LauncherCL launcher(m_queue, m_findSeparatingAxisVertexFaceKernel,"findSeparatingAxisVertexFaceKernel");
launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(b3BufferInfoCL) );
launcher.setConst( nPairs );
b3LauncherCL launcher(m_queue, m_findSeparatingAxisVertexFaceKernel,"findSeparatingAxisVertexFaceKernel");
launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(b3BufferInfoCL) );
launcher.setConst( nPairs );
int num = nPairs;
launcher.launch1D( num);
clFinish(m_queue);
int num = nPairs;
launcher.launch1D( num);
clFinish(m_queue);
}
if (useMprGpu)
{
B3_PROFILE("findSeparatingAxisEdgeEdgeKernel");
b3BufferInfoCL bInfo[] = {
b3BufferInfoCL( pairs->getBufferCL(), true ),
b3BufferInfoCL( bodyBuf->getBufferCL(),true),
b3BufferInfoCL( gpuCollidables.getBufferCL(),true),
b3BufferInfoCL( convexData.getBufferCL(),true),
b3BufferInfoCL( gpuVertices.getBufferCL(),true),
b3BufferInfoCL( gpuUniqueEdges.getBufferCL(),true),
b3BufferInfoCL( gpuFaces.getBufferCL(),true),
b3BufferInfoCL( gpuIndices.getBufferCL(),true),
b3BufferInfoCL( clAabbsWorldSpace.getBufferCL(),true),
b3BufferInfoCL( m_sepNormals.getBufferCL()),
b3BufferInfoCL( m_hasSeparatingNormals.getBufferCL()),
b3BufferInfoCL( m_dmins.getBufferCL())
};
nContacts = m_totalContactsOut.at(0);
{
B3_PROFILE("mprPenetrationKernel");
b3BufferInfoCL bInfo[] = {
b3BufferInfoCL( pairs->getBufferCL(), true ),
b3BufferInfoCL( bodyBuf->getBufferCL(),true),
b3BufferInfoCL( gpuCollidables.getBufferCL(),true),
b3BufferInfoCL( convexData.getBufferCL(),true),
b3BufferInfoCL( gpuVertices.getBufferCL(),true),
b3BufferInfoCL( m_sepNormals.getBufferCL()),
b3BufferInfoCL( m_hasSeparatingNormals.getBufferCL()),
b3BufferInfoCL( contactOut->getBufferCL()),
b3BufferInfoCL( m_totalContactsOut.getBufferCL())
};
b3LauncherCL launcher(m_queue, m_findSeparatingAxisEdgeEdgeKernel,"findSeparatingAxisEdgeEdgeKernel");
launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(b3BufferInfoCL) );
launcher.setConst( nPairs );
b3LauncherCL launcher(m_queue, m_mprPenetrationKernel,"mprPenetrationKernel");
launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(b3BufferInfoCL) );
launcher.setConst(maxContactCapacity);
launcher.setConst( nPairs );
int num = nPairs;
launcher.launch1D( num);
clFinish(m_queue);
/*
b3AlignedObjectArray<int>hostHasSepAxis;
m_hasSeparatingNormals.copyToHost(hostHasSepAxis);
b3AlignedObjectArray<b3Vector3>hostSepAxis;
m_sepNormals.copyToHost(hostSepAxis);
*/
nContacts = m_totalContactsOut.at(0);
contactOut->resize(nContacts);
//printf("nContacts (after processCompoundPairsPrimitivesKernel) = %d\n",nContacts);
if (nContacts>maxContactCapacity)
{
b3Error("Error: contacts exceeds capacity (%d/%d)\n", nContacts, maxContactCapacity);
nContacts = maxContactCapacity;
}
}
} else
{
{
B3_PROFILE("findSeparatingAxisEdgeEdgeKernel");
b3BufferInfoCL bInfo[] = {
b3BufferInfoCL( pairs->getBufferCL(), true ),
b3BufferInfoCL( bodyBuf->getBufferCL(),true),
b3BufferInfoCL( gpuCollidables.getBufferCL(),true),
b3BufferInfoCL( convexData.getBufferCL(),true),
b3BufferInfoCL( gpuVertices.getBufferCL(),true),
b3BufferInfoCL( gpuUniqueEdges.getBufferCL(),true),
b3BufferInfoCL( gpuFaces.getBufferCL(),true),
b3BufferInfoCL( gpuIndices.getBufferCL(),true),
b3BufferInfoCL( clAabbsWorldSpace.getBufferCL(),true),
b3BufferInfoCL( m_sepNormals.getBufferCL()),
b3BufferInfoCL( m_hasSeparatingNormals.getBufferCL()),
b3BufferInfoCL( m_dmins.getBufferCL())
};
b3LauncherCL launcher(m_queue, m_findSeparatingAxisEdgeEdgeKernel,"findSeparatingAxisEdgeEdgeKernel");
launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(b3BufferInfoCL) );
launcher.setConst( nPairs );
int num = nPairs;
launcher.launch1D( num);
clFinish(m_queue);
int num = nPairs;
launcher.launch1D( num);
clFinish(m_queue);
}
}
} else
{
@@ -3150,11 +3243,12 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( b3OpenCLArray<b3Int4>*
else
{
B3_PROFILE("findSeparatingAxisKernel CPU");
b3AlignedObjectArray<b3Int4> hostPairs;
pairs->copyToHost(hostPairs);
b3AlignedObjectArray<b3RigidBodyCL> hostBodyBuf;
b3AlignedObjectArray<b3RigidBodyData> hostBodyBuf;
bodyBuf->copyToHost(hostBodyBuf);
b3AlignedObjectArray<b3Collidable> hostCollidables;
@@ -3163,7 +3257,7 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( b3OpenCLArray<b3Int4>*
b3AlignedObjectArray<b3GpuChildShape> cpuChildShapes;
gpuChildShapes.copyToHost(cpuChildShapes);
b3AlignedObjectArray<b3ConvexPolyhedronCL> hostConvexShapeData;
b3AlignedObjectArray<b3ConvexPolyhedronData> hostConvexShapeData;
convexData.copyToHost(hostConvexShapeData);
b3AlignedObjectArray<b3Vector3> hostVertices;
@@ -3181,6 +3275,15 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( b3OpenCLArray<b3Int4>*
b3AlignedObjectArray<int> hostIndices;
gpuIndices.copyToHost(hostIndices);
b3AlignedObjectArray<b3Contact4> hostContacts;
if (nContacts)
{
contactOut->copyToHost(hostContacts);
}
hostContacts.resize(maxContactCapacity);
int nGlobalContactsOut = nContacts;
for (int i=0;i<nPairs;i++)
{
@@ -3215,41 +3318,237 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( b3OpenCLArray<b3Int4>*
b3Vector3 posB = hostBodyBuf[bodyIndexB].m_pos;
b3Quaternion ornA =hostBodyBuf[bodyIndexA].m_quat;
b3Quaternion ornB =hostBodyBuf[bodyIndexB].m_quat;
if (useGjk)
{
//first approximate the separating axis, to 'fail-proof' GJK+EPA or MPR
{
b3Vector3 c0local = hostConvexShapeData[shapeIndexA].m_localCenter;
b3Vector3 c0 = b3TransformPoint(c0local, posA, ornA);
b3Vector3 c1local = hostConvexShapeData[shapeIndexB].m_localCenter;
b3Vector3 c1 = b3TransformPoint(c1local,posB,ornB);
b3Vector3 DeltaC2 = c0 - c1;
b3Vector3 c0local = hostConvexShapeData[shapeIndexA].m_localCenter;
b3Vector3 c0 = b3TransformPoint(c0local, posA, ornA);
b3Vector3 c1local = hostConvexShapeData[shapeIndexB].m_localCenter;
b3Vector3 c1 = b3TransformPoint(c1local,posB,ornB);
b3Vector3 DeltaC2 = c0 - c1;
b3Vector3 sepAxis;
b3Vector3 sepAxis;
bool hasSepAxisA = b3FindSeparatingAxis(convexShapeA, convexShapeB, posA, ornA, posB, ornB, DeltaC2,
&hostVertices.at(0), &hostUniqueEdges.at(0), &hostFaces.at(0), &hostIndices.at(0),
&hostVertices.at(0), &hostUniqueEdges.at(0), &hostFaces.at(0), &hostIndices.at(0),
&sepAxis, &dmin);
bool hasSepAxisA = b3FindSeparatingAxis(convexShapeA, convexShapeB, posA, ornA, posB, ornB, DeltaC2,
&hostVertices.at(0), &hostUniqueEdges.at(0), &hostFaces.at(0), &hostIndices.at(0),
&hostVertices.at(0), &hostUniqueEdges.at(0), &hostFaces.at(0), &hostIndices.at(0),
&sepAxis, &dmin);
if (hasSepAxisA)
{
bool hasSepAxisB = b3FindSeparatingAxis(convexShapeB, convexShapeA, posB, ornB, posA, ornA, DeltaC2,
&hostVertices.at(0), &hostUniqueEdges.at(0), &hostFaces.at(0), &hostIndices.at(0),
&hostVertices.at(0), &hostUniqueEdges.at(0), &hostFaces.at(0), &hostIndices.at(0),
&sepAxis, &dmin);
if (hasSepAxisB)
{
bool hasEdgeEdge =b3FindSeparatingAxisEdgeEdge(convexShapeA, convexShapeB, posA, ornA, posB, ornB, DeltaC2,
&hostVertices.at(0), &hostUniqueEdges.at(0), &hostFaces.at(0), &hostIndices.at(0),
&hostVertices.at(0), &hostUniqueEdges.at(0), &hostFaces.at(0), &hostIndices.at(0),
&sepAxis, &dmin,false);
if (hasEdgeEdge)
{
hostHasSepAxis[i] = 1;
hostSepAxis[i] = sepAxis;
hostSepAxis[i].w = dmin;
}
}
}
}
if (hostHasSepAxis[i])
{
int pairIndex = i;
bool useMpr = true;
if (useMpr)
{
int res=0;
float depth = 0.f;
b3Vector3 sepAxis2 = b3MakeVector3(1,0,0);
b3Vector3 resultPointOnBWorld = b3MakeVector3(0,0,0);
float depthOut;
b3Vector3 dirOut;
b3Vector3 posOut;
//res = b3MprPenetration(bodyIndexA,bodyIndexB,hostBodyBuf,hostConvexShapeData,hostCollidables,hostVertices,&mprConfig,&depthOut,&dirOut,&posOut);
res = b3MprPenetration(pairIndex,bodyIndexA,bodyIndexB,&hostBodyBuf[0],&hostConvexShapeData[0],&hostCollidables[0],&hostVertices[0],&hostSepAxis[0],&hostHasSepAxis[0],&depthOut,&dirOut,&posOut);
depth = depthOut;
sepAxis2 = b3MakeVector3(-dirOut.x,-dirOut.y,-dirOut.z);
resultPointOnBWorld = posOut;
//hostHasSepAxis[i] = 0;
if (res==0)
{
//add point?
//printf("depth = %f\n",depth);
//printf("normal = %f,%f,%f\n",dir.v[0],dir.v[1],dir.v[2]);
//qprintf("pos = %f,%f,%f\n",pos.v[0],pos.v[1],pos.v[2]);
float dist=0.f;
const b3ConvexPolyhedronData& hullA = hostConvexShapeData[hostCollidables[hostBodyBuf[bodyIndexA].m_collidableIdx].m_shapeIndex];
const b3ConvexPolyhedronData& hullB = hostConvexShapeData[hostCollidables[hostBodyBuf[bodyIndexB].m_collidableIdx].m_shapeIndex];
if(b3TestSepAxis( &hullA, &hullB, posA,ornA,posB,ornB,&sepAxis2, &hostVertices[0], &hostVertices[0],&dist))
{
if (depth > dist)
{
float diff = depth - dist;
static float maxdiff = 0.f;
if (maxdiff < diff)
{
maxdiff = diff;
printf("maxdiff = %20.10f\n",maxdiff);
}
}
}
if (depth > dmin)
{
b3Vector3 oldAxis = hostSepAxis[i];
depth = dmin;
sepAxis2 = oldAxis;
}
if(b3TestSepAxis( &hullA, &hullB, posA,ornA,posB,ornB,&sepAxis2, &hostVertices[0], &hostVertices[0],&dist))
{
if (depth > dist)
{
float diff = depth - dist;
//printf("?diff = %f\n",diff );
static float maxdiff = 0.f;
if (maxdiff < diff)
{
maxdiff = diff;
printf("maxdiff = %20.10f\n",maxdiff);
}
}
//this is used for SAT
//hostHasSepAxis[i] = 1;
//hostSepAxis[i] = sepAxis2;
//add contact point
int contactIndex = nGlobalContactsOut;
b3Contact4& newContact = hostContacts.at(nGlobalContactsOut);
nGlobalContactsOut++;
newContact.m_batchIdx = 0;//i;
newContact.m_bodyAPtrAndSignBit = (hostBodyBuf.at(bodyIndexA).m_invMass==0)? -bodyIndexA:bodyIndexA;
newContact.m_bodyBPtrAndSignBit = (hostBodyBuf.at(bodyIndexB).m_invMass==0)? -bodyIndexB:bodyIndexB;
newContact.m_frictionCoeffCmp = 45874;
newContact.m_restituitionCoeffCmp = 0;
static float maxDepth = 0.f;
if (depth > maxDepth)
{
maxDepth = depth;
printf("MPR maxdepth = %f\n",maxDepth );
}
resultPointOnBWorld.w = -depth;
newContact.m_worldPosB[0] = resultPointOnBWorld;
b3Vector3 resultPointOnAWorld = resultPointOnBWorld+depth*sepAxis2;
newContact.m_worldNormalOnB = sepAxis2;
newContact.m_worldNormalOnB.w = (b3Scalar)1;
} else
{
printf("rejected\n");
}
}
} else
{
int result = computeContactConvexConvex( hostPairs,
pairIndex,
bodyIndexA, bodyIndexB,
collidableIndexA, collidableIndexB,
hostBodyBuf,
hostCollidables,
hostConvexShapeData,
hostVertices,
hostUniqueEdges,
hostIndices,
hostFaces,
hostContacts,
nGlobalContactsOut,
maxContactCapacity,
hostHasSepAxis,
hostSepAxis
);
}//mpr
}//hostHasSepAxis[i] = 1;
} else
{
b3Vector3 c0local = hostConvexShapeData[shapeIndexA].m_localCenter;
b3Vector3 c0 = b3TransformPoint(c0local, posA, ornA);
b3Vector3 c1local = hostConvexShapeData[shapeIndexB].m_localCenter;
b3Vector3 c1 = b3TransformPoint(c1local,posB,ornB);
b3Vector3 DeltaC2 = c0 - c1;
if (hasSepAxisA)
{
bool hasSepAxisB = b3FindSeparatingAxis(convexShapeB, convexShapeA, posB, ornB, posA, ornA, DeltaC2,
&hostVertices.at(0), &hostUniqueEdges.at(0), &hostFaces.at(0), &hostIndices.at(0),
&hostVertices.at(0), &hostUniqueEdges.at(0), &hostFaces.at(0), &hostIndices.at(0),
&sepAxis, &dmin);
if (hasSepAxisB)
{
bool hasEdgeEdge = b3FindSeparatingAxisEdgeEdge(convexShapeA, convexShapeB, posA, ornA, posB, ornB, DeltaC2,
&hostVertices.at(0), &hostUniqueEdges.at(0), &hostFaces.at(0), &hostIndices.at(0),
&hostVertices.at(0), &hostUniqueEdges.at(0), &hostFaces.at(0), &hostIndices.at(0),
&sepAxis, &dmin);
if (hasEdgeEdge)
{
hostHasSepAxis[i] = 1;
hostSepAxis[i] = sepAxis;
}
}
}
b3Vector3 sepAxis;
bool hasSepAxisA = b3FindSeparatingAxis(convexShapeA, convexShapeB, posA, ornA, posB, ornB, DeltaC2,
&hostVertices.at(0), &hostUniqueEdges.at(0), &hostFaces.at(0), &hostIndices.at(0),
&hostVertices.at(0), &hostUniqueEdges.at(0), &hostFaces.at(0), &hostIndices.at(0),
&sepAxis, &dmin);
if (hasSepAxisA)
{
bool hasSepAxisB = b3FindSeparatingAxis(convexShapeB, convexShapeA, posB, ornB, posA, ornA, DeltaC2,
&hostVertices.at(0), &hostUniqueEdges.at(0), &hostFaces.at(0), &hostIndices.at(0),
&hostVertices.at(0), &hostUniqueEdges.at(0), &hostFaces.at(0), &hostIndices.at(0),
&sepAxis, &dmin);
if (hasSepAxisB)
{
bool hasEdgeEdge =b3FindSeparatingAxisEdgeEdge(convexShapeA, convexShapeB, posA, ornA, posB, ornB, DeltaC2,
&hostVertices.at(0), &hostUniqueEdges.at(0), &hostFaces.at(0), &hostIndices.at(0),
&hostVertices.at(0), &hostUniqueEdges.at(0), &hostFaces.at(0), &hostIndices.at(0),
&sepAxis, &dmin,true);
if (hasEdgeEdge)
{
hostHasSepAxis[i] = 1;
hostSepAxis[i] = sepAxis;
}
}
}
}
}
if (useGjkContacts)//nGlobalContactsOut>0)
{
//printf("nGlobalContactsOut=%d\n",nGlobalContactsOut);
nContacts = nGlobalContactsOut;
contactOut->copyFromHost(hostContacts);
m_totalContactsOut.copyFromHostPointer(&nContacts,1,0,true);
}
m_hasSeparatingNormals.copyFromHost(hostHasSepAxis);
m_sepNormals.copyFromHost(hostSepAxis);
@@ -3337,7 +3636,7 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( b3OpenCLArray<b3Int4>*
b3AlignedObjectArray<b3Int4> hostPairs;
pairs->copyToHost(hostPairs);
b3AlignedObjectArray<b3RigidBodyCL> hostBodyBuf;
b3AlignedObjectArray<b3RigidBodyData> hostBodyBuf;
bodyBuf->copyToHost(hostBodyBuf);
@@ -3350,7 +3649,7 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( b3OpenCLArray<b3Int4>*
b3AlignedObjectArray<b3GpuChildShape> cpuChildShapes;
gpuChildShapes.copyToHost(cpuChildShapes);
b3AlignedObjectArray<b3ConvexPolyhedronCL> hostConvexData;
b3AlignedObjectArray<b3ConvexPolyhedronData> hostConvexData;
convexData.copyToHost(hostConvexData);
b3AlignedObjectArray<b3Vector3> hostVertices;
@@ -3537,7 +3836,7 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( b3OpenCLArray<b3Int4>*
{
b3AlignedObjectArray<b3Int4> hostPairs;
pairs->copyToHost(hostPairs);
b3AlignedObjectArray<b3RigidBodyCL> hostBodyBuf;
b3AlignedObjectArray<b3RigidBodyData> hostBodyBuf;
bodyBuf->copyToHost(hostBodyBuf);
b3AlignedObjectArray<b3Collidable> hostCollidables;
gpuCollidables.copyToHost(hostCollidables);
@@ -3750,14 +4049,14 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( b3OpenCLArray<b3Int4>*
b3AlignedObjectArray<b3Int4> triangleConvexPairsOutHost;
triangleConvexPairsOut.copyToHost(triangleConvexPairsOutHost);
//triangleConvexPairsOutHost.resize(maxTriConvexPairCapacity);
b3AlignedObjectArray<b3RigidBodyCL> hostBodyBuf;
b3AlignedObjectArray<b3RigidBodyData> hostBodyBuf;
bodyBuf->copyToHost(hostBodyBuf);
b3AlignedObjectArray<b3Collidable> hostCollidables;
gpuCollidables.copyToHost(hostCollidables);
b3AlignedObjectArray<b3Aabb> hostAabbsWorldSpace;
clAabbsWorldSpace.copyToHost(hostAabbsWorldSpace);
b3AlignedObjectArray<b3ConvexPolyhedronCL> hostConvexData;
b3AlignedObjectArray<b3ConvexPolyhedronData> hostConvexData;
convexData.copyToHost(hostConvexData);
b3AlignedObjectArray<b3Vector3> hostVertices;
@@ -3887,7 +4186,7 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( b3OpenCLArray<b3Int4>*
//B3_PROFILE("clipHullHullKernel");
bool breakupConcaveConvexKernel = false;
bool breakupConcaveConvexKernel = true;
#ifdef __APPLE__
//actually, some Apple OpenCL platform/device combinations work fine...
@@ -4025,7 +4324,7 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( b3OpenCLArray<b3Int4>*
volatile int nGlobalContactsOut = nContacts;
b3AlignedObjectArray<b3Int4> triangleConvexPairsOutHost;
triangleConvexPairsOut.copyToHost(triangleConvexPairsOutHost);
b3AlignedObjectArray<b3RigidBodyCL> hostBodyBuf;
b3AlignedObjectArray<b3RigidBodyData> hostBodyBuf;
bodyBuf->copyToHost(hostBodyBuf);
b3AlignedObjectArray<int>concaveHasSeparatingNormalsCPU;
@@ -4123,7 +4422,7 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( b3OpenCLArray<b3Int4>*
//convex-convex contact clipping
B3_PROFILE("clipHullHullKernel");
bool breakupKernel = false;
bool breakupKernel = true;
#ifdef __APPLE__
breakupKernel = true;
@@ -4182,7 +4481,7 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( b3OpenCLArray<b3Int4>*
float minDist = -1e30f;
float maxDist = 0.02f;
b3AlignedObjectArray<b3ConvexPolyhedronCL> hostConvexData;
b3AlignedObjectArray<b3ConvexPolyhedronData> hostConvexData;
convexData.copyToHost(hostConvexData);
b3AlignedObjectArray<b3Collidable> hostCollidables;
gpuCollidables.copyToHost(hostCollidables);
@@ -4194,7 +4493,7 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( b3OpenCLArray<b3Int4>*
b3AlignedObjectArray<b3Int4> hostPairs;
pairs->copyToHost(hostPairs);
b3AlignedObjectArray<b3RigidBodyCL> hostBodyBuf;
b3AlignedObjectArray<b3RigidBodyData> hostBodyBuf;
bodyBuf->copyToHost(hostBodyBuf);
@@ -4267,6 +4566,9 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( b3OpenCLArray<b3Int4>*
if (clipConvexFacesAndFindContactsCPU)
{
//b3AlignedObjectArray<b3Int4> hostPairs;
//pairs->copyToHost(hostPairs);
b3AlignedObjectArray<b3Vector3> hostSepNormals;
m_sepNormals.copyToHost(hostSepNormals);
b3AlignedObjectArray<int> hostHasSepAxis;
@@ -4343,8 +4645,8 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( b3OpenCLArray<b3Int4>*
}
{
// nContacts = m_totalContactsOut.at(0);
// printf("nContacts = %d\n",nContacts);
nContacts = m_totalContactsOut.at(0);
//printf("nContacts = %d\n",nContacts);
int newContactCapacity = nContacts+nPairs;
contactOut->reserve(newContactCapacity);
@@ -4382,7 +4684,7 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( b3OpenCLArray<b3Int4>*
volatile int nGlobalContactsOut = nContacts;
b3AlignedObjectArray<b3Int4> hostPairs;
pairs->copyToHost(hostPairs);
b3AlignedObjectArray<b3RigidBodyCL> hostBodyBuf;
b3AlignedObjectArray<b3RigidBodyData> hostBodyBuf;
bodyBuf->copyToHost(hostBodyBuf);
b3AlignedObjectArray<b3Vector3> hostSepNormals;
m_sepNormals.copyToHost(hostSepNormals);

9
src/Bullet3OpenCL/NarrowphaseCollision/b3ConvexHullContact.h
View File

@@ -3,10 +3,10 @@
#define _CONVEX_HULL_CONTACT_H
#include "Bullet3OpenCL/ParallelPrimitives/b3OpenCLArray.h"
#include "Bullet3Collision/NarrowPhaseCollision/b3RigidBodyCL.h"
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
#include "Bullet3Common/b3AlignedObjectArray.h"
#include "b3ConvexPolyhedronCL.h"
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h"
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h"
#include "Bullet3Collision/NarrowPhaseCollision/b3Contact4.h"
#include "Bullet3Common/shared/b3Int2.h"
@@ -26,6 +26,7 @@ struct GpuSatCollision
cl_device_id m_device;
cl_command_queue m_queue;
cl_kernel m_findSeparatingAxisKernel;
cl_kernel m_mprPenetrationKernel;
cl_kernel m_findSeparatingAxisVertexFaceKernel;
cl_kernel m_findSeparatingAxisEdgeEdgeKernel;
@@ -77,12 +78,12 @@ struct GpuSatCollision
void computeConvexConvexContactsGPUSAT( b3OpenCLArray<b3Int4>* pairs, int nPairs,
const b3OpenCLArray<b3RigidBodyCL>* bodyBuf,
const b3OpenCLArray<b3RigidBodyData>* bodyBuf,
b3OpenCLArray<b3Contact4>* contactOut, int& nContacts,
const b3OpenCLArray<b3Contact4>* oldContacts,
int maxContactCapacity,
int compoundPairCapacity,
const b3OpenCLArray<b3ConvexPolyhedronCL>& hostConvexData,
const b3OpenCLArray<b3ConvexPolyhedronData>& hostConvexData,
const b3OpenCLArray<b3Vector3>& vertices,
const b3OpenCLArray<b3Vector3>& uniqueEdges,
const b3OpenCLArray<b3GpuFace>& faces,

35
src/Bullet3OpenCL/NarrowphaseCollision/b3ConvexPolyhedronCL.h
View File

@@ -6,39 +6,4 @@
B3_ATTRIBUTE_ALIGNED16(struct) b3ConvexPolyhedronCL : public b3ConvexPolyhedronData
{
inline void project(const b3Transform& trans, const b3Vector3& dir, const b3AlignedObjectArray<b3Vector3>& vertices, b3Scalar& min, b3Scalar& max) const
{
min = FLT_MAX;
max = -FLT_MAX;
int numVerts = m_numVertices;
const b3Vector3 localDir = trans.getBasis().transpose()*dir;
const b3Vector3 localDi2 = b3QuatRotate(trans.getRotation().inverse(),dir);
b3Scalar offset = trans.getOrigin().dot(dir);
for(int i=0;i<numVerts;i++)
{
//b3Vector3 pt = trans * vertices[m_vertexOffset+i];
//b3Scalar dp = pt.dot(dir);
b3Scalar dp = vertices[m_vertexOffset+i].dot(localDir);
//b3Assert(dp==dpL);
if(dp < min) min = dp;
if(dp > max) max = dp;
}
if(min>max)
{
b3Scalar tmp = min;
min = max;
max = tmp;
}
min += offset;
max += offset;
}
};
#endif //CONVEX_POLYHEDRON_CL

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

@@ -59,7 +59,7 @@ namespace gjkepa2_impl
{
const b3ConvexPolyhedronCL* m_shapes[2];
const b3ConvexPolyhedronData* m_shapes[2];
b3Matrix3x3 m_toshape1;
@@ -631,7 +631,10 @@ namespace gjkepa2_impl
append(m_stock,best);
best=findbest();
outer=*best;
} else { m_status=eStatus::InvalidHull;break; }
} else {
m_status=eStatus::Failed;
//m_status=eStatus::InvalidHull;
break; }
} else { m_status=eStatus::AccuraryReached;break; }
} else { m_status=eStatus::OutOfVertices;break; }
}
@@ -804,7 +807,7 @@ namespace gjkepa2_impl
//
static void Initialize(const b3Transform& transA, const b3Transform& transB,
const b3ConvexPolyhedronCL* hullA, const b3ConvexPolyhedronCL* hullB,
const b3ConvexPolyhedronData* hullA, const b3ConvexPolyhedronData* hullB,
const b3AlignedObjectArray<b3Vector3>& verticesA,
const b3AlignedObjectArray<b3Vector3>& verticesB,
b3GjkEpaSolver2::sResults& results,
@@ -839,7 +842,7 @@ int b3GjkEpaSolver2::StackSizeRequirement()
//
bool b3GjkEpaSolver2::Distance( const b3Transform& transA, const b3Transform& transB,
const b3ConvexPolyhedronCL* hullA, const b3ConvexPolyhedronCL* hullB,
const b3ConvexPolyhedronData* hullA, const b3ConvexPolyhedronData* hullB,
const b3AlignedObjectArray<b3Vector3>& verticesA,
const b3AlignedObjectArray<b3Vector3>& verticesB,
const b3Vector3& guess,
@@ -877,7 +880,7 @@ bool b3GjkEpaSolver2::Distance( const b3Transform& transA, const b3Transform& t
//
bool b3GjkEpaSolver2::Penetration( const b3Transform& transA, const b3Transform& transB,
const b3ConvexPolyhedronCL* hullA, const b3ConvexPolyhedronCL* hullB,
const b3ConvexPolyhedronData* hullA, const b3ConvexPolyhedronData* hullB,
const b3AlignedObjectArray<b3Vector3>& verticesA,
const b3AlignedObjectArray<b3Vector3>& verticesB,
const b3Vector3& guess,
@@ -927,7 +930,7 @@ bool b3GjkEpaSolver2::Penetration( const b3Transform& transA, const b3Transform&
b3Scalar b3GjkEpaSolver2::SignedDistance(const b3Vector3& position,
b3Scalar margin,
const b3Transform& transA,
const b3ConvexPolyhedronCL& hullA,
const b3ConvexPolyhedronData& hullA,
const b3AlignedObjectArray<b3Vector3>& verticesA,
sResults& results)
{

21
src/Bullet3OpenCL/NarrowphaseCollision/b3GjkEpa.h
View File

@@ -27,7 +27,8 @@ GJK-EPA collision solver by Nathanael Presson, 2008
#include "Bullet3Common/b3AlignedObjectArray.h"
#include "Bullet3Common/b3Transform.h"
#include "b3ConvexPolyhedronCL.h"
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h"
///btGjkEpaSolver contributed under zlib by Nathanael Presson
struct b3GjkEpaSolver2
@@ -49,14 +50,14 @@ struct sResults
static int StackSizeRequirement();
static bool Distance( const b3Transform& transA, const b3Transform& transB,
const b3ConvexPolyhedronCL* hullA, const b3ConvexPolyhedronCL* hullB,
const b3ConvexPolyhedronData* hullA, const b3ConvexPolyhedronData* hullB,
const b3AlignedObjectArray<b3Vector3>& verticesA,
const b3AlignedObjectArray<b3Vector3>& verticesB,
const b3Vector3& guess,
sResults& results);
static bool Penetration( const b3Transform& transA, const b3Transform& transB,
const b3ConvexPolyhedronCL* hullA, const b3ConvexPolyhedronCL* hullB,
const b3ConvexPolyhedronData* hullA, const b3ConvexPolyhedronData* hullB,
const b3AlignedObjectArray<b3Vector3>& verticesA,
const b3AlignedObjectArray<b3Vector3>& verticesB,
const b3Vector3& guess,
@@ -65,19 +66,15 @@ static bool Penetration( const b3Transform& transA, const b3Transform& transB,
#if 0
static b3Scalar SignedDistance( const b3Vector3& position,
b3Scalar margin,
const b3Transform& transA,
const b3ConvexPolyhedronCL& hullA,
const b3AlignedObjectArray<b3Vector3>& verticesA,
const btConvexShape* shape,
const btTransform& wtrs,
sResults& results);
static bool SignedDistance( const b3Transform& transA, const b3Transform& transB,
const b3ConvexPolyhedronCL& hullA, const b3ConvexPolyhedronCL& hullB,
const b3AlignedObjectArray<b3Vector3>& verticesA,
const b3AlignedObjectArray<b3Vector3>& verticesB,
static bool SignedDistance( const btConvexShape* shape0,const btTransform& wtrs0,
const btConvexShape* shape1,const btTransform& wtrs1,
const b3Vector3& guess,
sResults& results);
#endif //0
#endif
};

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

@@ -16,7 +16,7 @@ subject to the following restrictions:
#include "b3GjkPairDetector.h"
#include "Bullet3Common/b3Transform.h"
#include "b3VoronoiSimplexSolver.h"
#include "b3ConvexPolyhedronCL.h"
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h"
#include "b3VectorFloat4.h"
#include "b3GjkEpa.h"
#include "b3SupportMappings.h"
@@ -48,7 +48,7 @@ m_fixContactNormalDirection(1)
bool calcPenDepth( b3VoronoiSimplexSolver& simplexSolver,
const b3Transform& transformA, const b3Transform& transformB,
const b3ConvexPolyhedronCL& hullA, const b3ConvexPolyhedronCL& hullB,
const b3ConvexPolyhedronData& hullA, const b3ConvexPolyhedronData& hullB,
const b3AlignedObjectArray<b3Vector3>& verticesA,
const b3AlignedObjectArray<b3Vector3>& verticesB,
b3Vector3& v, b3Vector3& wWitnessOnA, b3Vector3& wWitnessOnB)
@@ -83,7 +83,7 @@ bool calcPenDepth( b3VoronoiSimplexSolver& simplexSolver,
}
#define dot3F4 b3Dot
inline void project(const b3ConvexPolyhedronCL& hull, const float4& pos, const b3Quaternion& orn, const float4& dir, const b3AlignedObjectArray<b3Vector3>& vertices, b3Scalar& min, b3Scalar& max)
inline void project(const b3ConvexPolyhedronData& hull, const float4& pos, const b3Quaternion& orn, const float4& dir, const b3AlignedObjectArray<b3Vector3>& vertices, b3Scalar& min, b3Scalar& max)
{
min = FLT_MAX;
max = -FLT_MAX;
@@ -114,7 +114,7 @@ inline void project(const b3ConvexPolyhedronCL& hull, const float4& pos, const
}
static bool TestSepAxis(const b3ConvexPolyhedronCL& hullA, const b3ConvexPolyhedronCL& hullB,
static bool TestSepAxis(const b3ConvexPolyhedronData& hullA, const b3ConvexPolyhedronData& hullB,
const float4& posA,const b3Quaternion& ornA,
const float4& posB,const b3Quaternion& ornB,
float4& sep_axis, const b3AlignedObjectArray<b3Vector3>& verticesA,const b3AlignedObjectArray<b3Vector3>& verticesB,b3Scalar& depth)
@@ -146,7 +146,7 @@ static bool TestSepAxis(const b3ConvexPolyhedronCL& hullA, const b3ConvexPolyhed
bool getClosestPoints(b3GjkPairDetector* gjkDetector, const b3Transform& transA, const b3Transform& transB,
const b3ConvexPolyhedronCL& hullA, const b3ConvexPolyhedronCL& hullB,
const b3ConvexPolyhedronData& hullA, const b3ConvexPolyhedronData& hullB,
const b3AlignedObjectArray<b3Vector3>& verticesA,
const b3AlignedObjectArray<b3Vector3>& verticesB,
b3Scalar maximumDistanceSquared,
@@ -203,9 +203,7 @@ bool getClosestPoints(b3GjkPairDetector* gjkDetector, const b3Transform& transA,
b3Scalar margin = marginA + marginB;
b3Scalar bestDeltaN = -1e30f;
b3Vector3 bestSepAxis= b3MakeVector3(0,0,0);
b3Vector3 bestPointOnA;
b3Vector3 bestPointOnB;
gjkDetector->m_simplexSolver->reset();
@@ -224,33 +222,10 @@ bool getClosestPoints(b3GjkPairDetector* gjkDetector, const b3Transform& transA,
b3Vector3 pWorld = localTransA(pInA);
b3Vector3 qWorld = localTransB(qInB);
{
b3Scalar l2 = gjkDetector->m_cachedSeparatingAxis.length2();
if (l2>B3_EPSILON*B3_EPSILON)
{
b3Vector3 testAxis = gjkDetector->m_cachedSeparatingAxis*(1.f/b3Sqrt(l2));
float computedDepth=1e30f;
if (!TestSepAxis(hullA,hullB,transA.getOrigin(),transA.getRotation(),
transB.getOrigin(),transB.getRotation(),testAxis,verticesA,verticesB,computedDepth))
{
return false;
}
if(computedDepth<resultSepDistance)
{
if (testAxis.length2()>B3_EPSILON*B3_EPSILON)
{
resultSepDistance = computedDepth;
resultSepNormal = testAxis;
}
}
}
}
#ifdef DEBUG_SPU_COLLISION_DETECTION
spu_printf("got local supporting vertices\n");
#endif
if (check2d)
{
@@ -260,26 +235,7 @@ bool getClosestPoints(b3GjkPairDetector* gjkDetector, const b3Transform& transA,
b3Vector3 w = pWorld - qWorld;
delta = gjkDetector->m_cachedSeparatingAxis.dot(w);
if (delta>0)
return false;
b3Scalar deltaN = gjkDetector->m_cachedSeparatingAxis.normalized().dot(w.normalized());
if (deltaN < bestDeltaN)
{
bestDeltaN = deltaN;
//printf("new solution?\n");
bestSepAxis = gjkDetector->m_cachedSeparatingAxis;
gjkDetector->m_simplexSolver->compute_points(bestPointOnA, bestPointOnB);
}
prevDelta = delta;
b3Scalar dist = 0;
if (delta<0)
dist = -b3Sqrt(b3Fabs(delta));
else
dist = b3Sqrt(delta);
//printf("gjkDetector->m_cachedSeparatingAxis = %f,%f,%f delta/dist = %f\n",gjkDetector->m_cachedSeparatingAxis.x,gjkDetector->m_cachedSeparatingAxis.y,gjkDetector->m_cachedSeparatingAxis.z,dist);
// potential exit, they don't overlap
if ((delta > b3Scalar(0.0)) && (delta * delta > squaredDistance * maximumDistanceSquared))
{
@@ -313,8 +269,14 @@ bool getClosestPoints(b3GjkPairDetector* gjkDetector, const b3Transform& transA,
break;
}
#ifdef DEBUG_SPU_COLLISION_DETECTION
spu_printf("addVertex 1\n");
#endif
//add current vertex to simplex
gjkDetector->m_simplexSolver->addVertex(w, pWorld, qWorld);
#ifdef DEBUG_SPU_COLLISION_DETECTION
spu_printf("addVertex 2\n");
#endif
b3Vector3 newCachedSeparatingAxis;
//calculate the closest point to the origin (update vector v)
@@ -325,12 +287,9 @@ bool getClosestPoints(b3GjkPairDetector* gjkDetector, const b3Transform& transA,
break;
}
if(0)//newCachedSeparatingAxis.length2()<REL_ERROR2)
if(newCachedSeparatingAxis.length2()<REL_ERROR2)
{
if (delta<bestDeltaN)
{
gjkDetector->m_cachedSeparatingAxis = newCachedSeparatingAxis;
}
gjkDetector->m_cachedSeparatingAxis = newCachedSeparatingAxis;
gjkDetector->m_degenerateSimplex = 6;
checkSimplex = true;
break;
@@ -338,17 +297,24 @@ bool getClosestPoints(b3GjkPairDetector* gjkDetector, const b3Transform& transA,
b3Scalar previousSquaredDistance = squaredDistance;
squaredDistance = newCachedSeparatingAxis.length2();
b3Vector3 sepAxis=newCachedSeparatingAxis.normalized();
#if 0
///warning: this termination condition leads to some problems in 2d test case see Bullet/Demos/Box2dDemo
if (squaredDistance>previousSquaredDistance)
{
gjkDetector->m_degenerateSimplex = 7;
squaredDistance = previousSquaredDistance;
checkSimplex = false;
break;
}
#endif //
//redundant m_simplexSolver->compute_points(pointOnA, pointOnB);
//redundant gjkDetector->m_simplexSolver->compute_points(pointOnA, pointOnB);
//are we getting any closer ?
if (previousSquaredDistance - squaredDistance <= B3_EPSILON * previousSquaredDistance)
{
// gjkDetector->m_simplexSolver->backup_closest(gjkDetector->m_cachedSeparatingAxis);
checkSimplex = true;
gjkDetector->m_degenerateSimplex = 12;
@@ -357,55 +323,32 @@ bool getClosestPoints(b3GjkPairDetector* gjkDetector, const b3Transform& transA,
gjkDetector->m_cachedSeparatingAxis = newCachedSeparatingAxis;
{
b3Scalar l2 = gjkDetector->m_cachedSeparatingAxis.length2();
if (l2>B3_EPSILON*B3_EPSILON)
{
//degeneracy, this is typically due to invalid/uninitialized worldtransforms for a btCollisionObject
if (gjkDetector->m_curIter++ > gGjkMaxIter)
{
#if defined(DEBUG) || defined (_DEBUG) || defined (DEBUG_SPU_COLLISION_DETECTION)
b3Vector3 testAxis = gjkDetector->m_cachedSeparatingAxis*(1.f/b3Sqrt(l2));
float computedDepth=1e30f;
if (!TestSepAxis(hullA,hullB,transA.getOrigin(),transA.getRotation(),
transB.getOrigin(),transB.getRotation(),testAxis,verticesA,verticesB,computedDepth))
{
return false;
}
printf("btGjkPairDetector maxIter exceeded:%i\n",gjkDetector->m_curIter);
printf("sepAxis=(%f,%f,%f), squaredDistance = %f\n",
gjkDetector->m_cachedSeparatingAxis.getX(),
gjkDetector->m_cachedSeparatingAxis.getY(),
gjkDetector->m_cachedSeparatingAxis.getZ(),
squaredDistance);
if(computedDepth<resultSepDistance)
{
if (testAxis.length2()>B3_EPSILON*B3_EPSILON)
{
resultSepDistance = computedDepth;
resultSepNormal = testAxis;
}
}
}
}
#endif
break;
//degeneracy, this is typically due to invalid/uninitialized worldtransforms for a btCollisionObject
if (gjkDetector->m_curIter++ > gGjkMaxIter)
{
break;
}
}
bool check = (!gjkDetector->m_simplexSolver->fullSimplex());
float projectedDepth = 0;
if (delta<0)
{
projectedDepth = -b3Sqrt(b3Fabs(delta));
} else
{
projectedDepth = b3Sqrt(delta);
}
//printf("dist2 = %f dist= %f projectedDepth = %f\n", squaredDistance,b3Sqrt(squaredDistance),projectedDepth);
//bool check = (!gjkDetector->m_simplexSolver->fullSimplex() && squaredDistance > B3_EPSILON * gjkDetector->m_simplexSolver->maxVertex());
if (!check)
{
//do we need this backup_closest here ?
// gjkDetector->m_simplexSolver->backup_closest(gjkDetector->m_cachedSeparatingAxis);
gjkDetector->m_degenerateSimplex = 13;
break;
}
@@ -413,16 +356,7 @@ bool getClosestPoints(b3GjkPairDetector* gjkDetector, const b3Transform& transA,
if (checkSimplex)
{
if (bestSepAxis.length2())
{
pointOnA = bestPointOnA;
pointOnB = bestPointOnB;
gjkDetector->m_cachedSeparatingAxis = bestSepAxis;
} else
{
gjkDetector->m_simplexSolver->compute_points(pointOnA, pointOnB);
}
gjkDetector->m_simplexSolver->compute_points(pointOnA, pointOnB);
normalInB = gjkDetector->m_cachedSeparatingAxis;
b3Scalar lenSqr =gjkDetector->m_cachedSeparatingAxis.length2();
@@ -450,7 +384,8 @@ bool getClosestPoints(b3GjkPairDetector* gjkDetector, const b3Transform& transA,
}
}
bool catchDegeneratePenetrationCase = (gjkDetector->m_catchDegeneracies && gjkDetector->m_penetrationDepthSolver && gjkDetector->m_degenerateSimplex && ((distance+margin) < 0.01));
bool catchDegeneratePenetrationCase =
(gjkDetector->m_catchDegeneracies && gjkDetector->m_penetrationDepthSolver && gjkDetector->m_degenerateSimplex && ((distance+margin) < 0.01));
//if (checkPenetration && !isValid)
if (checkPenetration && (!isValid || catchDegeneratePenetrationCase ))
@@ -543,10 +478,11 @@ bool getClosestPoints(b3GjkPairDetector* gjkDetector, const b3Transform& transA,
if (isValid && ((distance < 0) || (distance*distance < maximumDistanceSquared)))
if (isValid && (distance < 0))
//if (isValid && ((distance < 0) || (distance*distance < maximumDistanceSquared)))
{
if (gjkDetector->m_fixContactNormalDirection)
if (1)//m_fixContactNormalDirection)
{
///@workaround for sticky convex collisions
//in some degenerate cases (usually when the use uses very small margins)
@@ -556,104 +492,37 @@ bool getClosestPoints(b3GjkPairDetector* gjkDetector, const b3Transform& transA,
//We like to use a dot product of the normal against the difference of the centroids,
//once the centroid is available in the API
//until then we use the center of the aabb to approximate the centroid
b3Vector3 aabbMin,aabbMax;
//m_minkowskiA->getAabb(localTransA,aabbMin,aabbMax);
//b3Vector3 posA = (aabbMax+aabbMin)*b3Scalar(0.5);
//m_minkowskiB->getAabb(localTransB,aabbMin,aabbMax);
//b3Vector3 posB = (aabbMin+aabbMax)*b3Scalar(0.5);
b3Vector3 posA = localTransA*hullA.m_localCenter;
b3Vector3 posB = localTransB*hullB.m_localCenter;
b3Vector3 diff = transA.getOrigin()-transB.getOrigin();
b3Vector3 diff = posA-posB;
if (diff.dot(normalInB) < 0.f)
normalInB *= -1.f;
}
gjkDetector->m_cachedSeparatingAxis = normalInB;
gjkDetector->m_cachedSeparatingDistance = distance;
{
b3Scalar l2 = gjkDetector->m_cachedSeparatingAxis.length2();
if (l2>B3_EPSILON*B3_EPSILON)
{
b3Vector3 testAxis = gjkDetector->m_cachedSeparatingAxis*(1.f/b3Sqrt(l2));
float computedDepth=1e30f;
if (!TestSepAxis(hullA,hullB,transA.getOrigin(),transA.getRotation(),
transB.getOrigin(),transB.getRotation(),testAxis,verticesA,verticesB,computedDepth))
{
return false;
}
if(computedDepth<resultSepDistance)
{
if (testAxis.length2()>B3_EPSILON*B3_EPSILON)
{
resultSepDistance = computedDepth;
resultSepNormal = testAxis;
}
}
}
}
/*output.addContactPoint(
normalInB,
pointOnB+positionOffset,
distance);
*/
static float maxPenetrationDistance = 0.f;
if (distance<maxPenetrationDistance)
{
maxPenetrationDistance = distance;
printf("maxPenetrationDistance = %f\n",maxPenetrationDistance);
}
resultSepNormal = normalInB;
//printf("normalInB = %f,%f,%f, distance = %f\n",normalInB.x,normalInB.y,normalInB.z,distance);
resultSepDistance = distance;
b3Scalar lsqr = resultSepNormal.length2();
b3Assert(lsqr>B3_EPSILON*B3_EPSILON);
if (lsqr<B3_EPSILON*B3_EPSILON)
return false;
resultSepNormal *= 1.f/b3Sqrt(lsqr);
#if 0
float dot = resultSepNormal.dot(b3Vector3(0,-1,0));
//float dot = b3Vector3(-0.7,-0.6,-0.2).dot(b3Vector3(0,-1,0));
static float minDot = 1e30f;
if (dot<minDot)
{
//printf("minDot = %f\n", dot);
minDot=dot;
}
//printf("gNumGjkChecks = %d, gNumDeepPenetrationChecks = %d, minDot = %f\n", gNumGjkChecks,gNumDeepPenetrationChecks,minDot);
if (0)//dot<0.64)
{
{
b3Scalar l2 = resultSepNormal.length2();
if (l2>B3_EPSILON*B3_EPSILON)
{
b3Vector3 testAxis = gjkDetector->m_cachedSeparatingAxis*(1./b3Sqrt(l2));
float computedDepth=1e30f;
if (!TestSepAxis(hullA,hullB,transA.getOrigin(),transA.getRotation(),
transB.getOrigin(),transB.getRotation(),testAxis,verticesA,verticesB,computedDepth))
{
return false;
}
if(computedDepth<resultSepDistance)
{
if (testAxis.length2()>B3_EPSILON*B3_EPSILON)
{
resultSepDistance = computedDepth;
resultSepNormal = testAxis;
}
}
}
}
}
#endif
resultPointOnB = pointOnB+positionOffset;
return true;
}
return false;
}

4
src/Bullet3OpenCL/NarrowphaseCollision/b3GjkPairDetector.h
View File

@@ -12,7 +12,7 @@
class b3Transform;
struct b3GjkEpaSolver2;
class b3VoronoiSimplexSolver;
struct b3ConvexPolyhedronCL;
struct b3ConvexPolyhedronData;
B3_ATTRIBUTE_ALIGNED16(struct) b3GjkPairDetector
{
@@ -73,7 +73,7 @@ public:
bool getClosestPoints(b3GjkPairDetector* gjkDetector, const b3Transform& transA, const b3Transform& transB,
const b3ConvexPolyhedronCL& hullA, const b3ConvexPolyhedronCL& hullB,
const b3ConvexPolyhedronData& hullA, const b3ConvexPolyhedronData& hullB,
const b3AlignedObjectArray<b3Vector3>& verticesA,
const b3AlignedObjectArray<b3Vector3>& verticesB,
b3Scalar maximumDistanceSquared,

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

@@ -6,10 +6,12 @@
#include "Bullet3Common/b3AlignedObjectArray.h"
#include "b3VectorFloat4.h"
struct b3ConvexPolyhedronCL;
struct b3GjkPairDetector;
inline b3Vector3 localGetSupportVertexWithMargin(const float4& supportVec,const struct b3ConvexPolyhedronCL* hull,
inline b3Vector3 localGetSupportVertexWithMargin(const float4& supportVec,const struct b3ConvexPolyhedronData* hull,
const b3AlignedObjectArray<b3Vector3>& verticesA, b3Scalar margin)
{
b3Vector3 supVec = b3MakeVector3(b3Scalar(0.),b3Scalar(0.),b3Scalar(0.));
@@ -27,7 +29,7 @@ inline b3Vector3 localGetSupportVertexWithMargin(const float4& supportVec,const
}
inline b3Vector3 localGetSupportVertexWithoutMargin(const float4& supportVec,const struct b3ConvexPolyhedronCL* hull,
inline b3Vector3 localGetSupportVertexWithoutMargin(const float4& supportVec,const struct b3ConvexPolyhedronData* hull,
const b3AlignedObjectArray<b3Vector3>& verticesA)
{
return localGetSupportVertexWithMargin(supportVec,hull,verticesA,0.f);

13
src/Bullet3OpenCL/NarrowphaseCollision/b3VectorFloat4.h
View File

@@ -3,18 +3,9 @@
#include "Bullet3Common/b3Transform.h"
#define cross3(a,b) (a.cross(b))
//#define cross3(a,b) (a.cross(b))
#define float4 b3Vector3
#define make_float4(x,y,z,w) b3Vector4(x,y,z,w)
//#define make_float4(x,y,z,w) b3Vector4(x,y,z,w)
inline b3Vector3 transform(const b3Vector3* v, const b3Vector3* pos, const b3Quaternion* orn)
{
b3Transform tr;
tr.setIdentity();
tr.setOrigin(*pos);
tr.setRotation(*orn);
b3Vector3 res = tr(*v);
return res;
}
#endif //B3_VECTOR_FLOAT4_H

89
src/Bullet3OpenCL/NarrowphaseCollision/kernels/mpr.cl Normal file
View File

@@ -0,0 +1,89 @@
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3MprPenetration.h"
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Contact4Data.h"
#define AppendInc(x, out) out = atomic_inc(x)
#define GET_NPOINTS(x) (x).m_worldNormalOnB.w
#ifdef cl_ext_atomic_counters_32
#pragma OPENCL EXTENSION cl_ext_atomic_counters_32 : enable
#else
#define counter32_t volatile __global int*
#endif
__kernel void mprPenetrationKernel( __global int4* pairs,
__global const b3RigidBodyData_t* rigidBodies,
__global const b3Collidable_t* collidables,
__global const b3ConvexPolyhedronData_t* convexShapes,
__global const float4* vertices,
__global float4* separatingNormals,
__global int* hasSeparatingAxis,
__global struct b3Contact4Data* restrict globalContactsOut,
counter32_t nGlobalContactsOut,
int contactCapacity,
int numPairs)
{
int i = get_global_id(0);
int pairIndex = i;
if (i<numPairs)
{
int bodyIndexA = pairs[i].x;
int bodyIndexB = pairs[i].y;
int collidableIndexA = rigidBodies[bodyIndexA].m_collidableIdx;
int collidableIndexB = rigidBodies[bodyIndexB].m_collidableIdx;
int shapeIndexA = collidables[collidableIndexA].m_shapeIndex;
int shapeIndexB = collidables[collidableIndexB].m_shapeIndex;
//once the broadphase avoids static-static pairs, we can remove this test
if ((rigidBodies[bodyIndexA].m_invMass==0) &&(rigidBodies[bodyIndexB].m_invMass==0))
{
return;
}
if ((collidables[collidableIndexA].m_shapeType!=SHAPE_CONVEX_HULL) ||(collidables[collidableIndexB].m_shapeType!=SHAPE_CONVEX_HULL))
{
return;
}
float depthOut;
b3Float4 dirOut;
b3Float4 posOut;
int res = b3MprPenetration(pairIndex, bodyIndexA, bodyIndexB,rigidBodies,convexShapes,collidables,vertices,separatingNormals,hasSeparatingAxis,&depthOut, &dirOut, &posOut);
if (res==0)
{
//add a contact
int dstIdx;
AppendInc( nGlobalContactsOut, dstIdx );
if (dstIdx<contactCapacity)
{
__global struct b3Contact4Data* c = globalContactsOut + dstIdx;
c->m_worldNormalOnB = -dirOut;//normal;
c->m_restituitionCoeffCmp = (0.f*0xffff);c->m_frictionCoeffCmp = (0.7f*0xffff);
c->m_batchIdx = pairIndex;
int bodyA = pairs[pairIndex].x;
int bodyB = pairs[pairIndex].y;
c->m_bodyAPtrAndSignBit = rigidBodies[bodyA].m_invMass==0 ? -bodyA:bodyA;
c->m_bodyBPtrAndSignBit = rigidBodies[bodyB].m_invMass==0 ? -bodyB:bodyB;
c->m_childIndexA = -1;
c->m_childIndexB = -1;
//for (int i=0;i<nContacts;i++)
posOut.w = -depthOut;
c->m_worldPosB[0] = posOut;//localPoints[contactIdx[i]];
GET_NPOINTS(*c) = 1;//nContacts;
}
}
}
}

1244
src/Bullet3OpenCL/NarrowphaseCollision/kernels/mprKernels.h Normal file
View File

File diff suppressed because it is too large Load Diff