mirror of
https://github.com/bulletphysics/bullet3
synced 2024-12-05 01:50:05 +00:00
add support for compound hull against plane
This commit is contained in:
erwin coumans
parent
ce5652c26a
commit
e1a4400037
@ -66,6 +66,10 @@ int selectedDemo = 0;
|
||||
GpuDemo::CreateFunc* allDemos[]=
|
||||
{
|
||||
GpuConvexPlaneScene::MyCreateFunc,
|
||||
|
||||
GpuCompoundScene::MyCreateFunc,
|
||||
GpuCompoundPlaneScene::MyCreateFunc,
|
||||
|
||||
GpuConvexScene::MyCreateFunc,
|
||||
ConcaveCompoundScene::MyCreateFunc,
|
||||
|
||||
@ -78,7 +82,7 @@ GpuDemo::CreateFunc* allDemos[]=
|
||||
|
||||
|
||||
|
||||
GpuCompoundScene::MyCreateFunc,
|
||||
|
||||
PairBench::MyCreateFunc,
|
||||
|
||||
|
||||
|
||||
@ -24,19 +24,135 @@
|
||||
|
||||
void GpuCompoundScene::setupScene(const ConstructionInfo& ci)
|
||||
{
|
||||
int strideInBytes = 9*sizeof(float);
|
||||
|
||||
createStaticEnvironment(ci);
|
||||
|
||||
int strideInBytes = 9*sizeof(float);
|
||||
int numVertices = sizeof(cube_vertices)/strideInBytes;
|
||||
int numIndices = sizeof(cube_indices)/sizeof(int);
|
||||
float scaling[4] = {1,1,1,1};
|
||||
|
||||
GLInstanceVertex* cubeVerts = (GLInstanceVertex*)&cube_vertices[0];
|
||||
int stride2 = sizeof(GLInstanceVertex);
|
||||
btAssert(stride2 == strideInBytes);
|
||||
int index=0;
|
||||
int colIndex = -1;
|
||||
btAlignedObjectArray<GLInstanceVertex> vertexArray;
|
||||
btAlignedObjectArray<int> indexArray;
|
||||
{
|
||||
int childColIndex = m_data->m_np->registerConvexHullShape(&cube_vertices[0],strideInBytes,numVertices, scaling);
|
||||
|
||||
|
||||
btVector3 childPositions[3] = {
|
||||
btVector3(0,-2,0),
|
||||
btVector3(0,0,0),
|
||||
btVector3(0,2,0)
|
||||
};
|
||||
|
||||
|
||||
btAlignedObjectArray<btGpuChildShape> childShapes;
|
||||
int numChildShapes = 3;
|
||||
for (int i=0;i<numChildShapes;i++)
|
||||
{
|
||||
//for now, only support polyhedral child shapes
|
||||
btGpuChildShape child;
|
||||
child.m_shapeIndex = childColIndex;
|
||||
btVector3 pos = childPositions[i];
|
||||
btQuaternion orn(0,0,0,1);
|
||||
for (int v=0;v<4;v++)
|
||||
{
|
||||
child.m_childPosition[v] = pos[v];
|
||||
child.m_childOrientation[v] = orn[v];
|
||||
}
|
||||
childShapes.push_back(child);
|
||||
btTransform tr;
|
||||
tr.setIdentity();
|
||||
tr.setOrigin(pos);
|
||||
tr.setRotation(orn);
|
||||
|
||||
int baseIndex = vertexArray.size();
|
||||
for (int j=0;j<numIndices;j++)
|
||||
indexArray.push_back(cube_indices[j]+baseIndex);
|
||||
|
||||
//add transformed graphics vertices and indices
|
||||
for (int v=0;v<numVertices;v++)
|
||||
{
|
||||
GLInstanceVertex vert = cubeVerts[v];
|
||||
btVector3 vertPos(vert.xyzw[0],vert.xyzw[1],vert.xyzw[2]);
|
||||
btVector3 newPos = tr*vertPos;
|
||||
vert.xyzw[0] = newPos[0];
|
||||
vert.xyzw[1] = newPos[1];
|
||||
vert.xyzw[2] = newPos[2];
|
||||
vert.xyzw[3] = 0.f;
|
||||
vertexArray.push_back(vert);
|
||||
}
|
||||
|
||||
}
|
||||
colIndex= m_data->m_np->registerCompoundShape(&childShapes);
|
||||
|
||||
}
|
||||
|
||||
|
||||
//int shapeId = ci.m_instancingRenderer->registerShape(&cube_vertices[0],numVertices,cube_indices,numIndices);
|
||||
int shapeId = ci.m_instancingRenderer->registerShape(&vertexArray[0].xyzw[0],vertexArray.size(),&indexArray[0],indexArray.size());
|
||||
|
||||
btVector4 colors[4] =
|
||||
{
|
||||
btVector4(1,0,0,1),
|
||||
btVector4(0,1,0,1),
|
||||
btVector4(0,0,1,1),
|
||||
btVector4(0,1,1,1),
|
||||
};
|
||||
|
||||
int curColor = 0;
|
||||
for (int i=0;i<ci.arraySizeX;i++)
|
||||
{
|
||||
for (int j=0;j<ci.arraySizeY;j++)
|
||||
{
|
||||
for (int k=0;k<ci.arraySizeZ;k++)
|
||||
{
|
||||
float mass = 1;//j==0? 0.f : 1.f;
|
||||
|
||||
btVector3 position(i*ci.gapX,10+j*ci.gapY,k*ci.gapZ);
|
||||
//btQuaternion orn(0,0,0,1);
|
||||
btQuaternion orn(btVector3(1,0,0),0.7);
|
||||
|
||||
btVector4 color = colors[curColor];
|
||||
curColor++;
|
||||
curColor&=3;
|
||||
btVector4 scaling(1,1,1,1);
|
||||
int id = ci.m_instancingRenderer->registerGraphicsInstance(shapeId,position,orn,color,scaling);
|
||||
int pid = m_data->m_rigidBodyPipeline->registerPhysicsInstance(mass,position,orn,colIndex,index);
|
||||
|
||||
index++;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
float camPos[4]={0,0,0};//65.5,4.5,65.5,0};
|
||||
//float camPos[4]={1,12.5,1.5,0};
|
||||
m_instancingRenderer->setCameraTargetPosition(camPos);
|
||||
m_instancingRenderer->setCameraDistance(20);
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
void GpuCompoundScene::createStaticEnvironment(const ConstructionInfo& ci)
|
||||
{
|
||||
|
||||
int strideInBytes = 9*sizeof(float);
|
||||
|
||||
|
||||
|
||||
|
||||
//int shapeId = ci.m_instancingRenderer->registerShape(&cube_vertices[0],numVertices,cube_indices,numIndices);
|
||||
int group=1;
|
||||
int mask=1;
|
||||
int index=0;
|
||||
float scaling[4] = {1,1,1,1};
|
||||
int colIndex = 0;
|
||||
|
||||
{
|
||||
@ -110,106 +226,26 @@ void GpuCompoundScene::setupScene(const ConstructionInfo& ci)
|
||||
|
||||
}
|
||||
}
|
||||
GLInstanceVertex* cubeVerts = (GLInstanceVertex*)&cube_vertices[0];
|
||||
int stride2 = sizeof(GLInstanceVertex);
|
||||
btAssert(stride2 == strideInBytes);
|
||||
|
||||
{
|
||||
int childColIndex = m_data->m_np->registerConvexHullShape(&cube_vertices[0],strideInBytes,numVertices, scaling);
|
||||
|
||||
|
||||
btVector3 childPositions[3] = {
|
||||
btVector3(0,-2,0),
|
||||
btVector3(0,0,0),
|
||||
btVector3(0,2,0)
|
||||
};
|
||||
|
||||
|
||||
btAlignedObjectArray<btGpuChildShape> childShapes;
|
||||
int numChildShapes = 3;
|
||||
for (int i=0;i<numChildShapes;i++)
|
||||
{
|
||||
//for now, only support polyhedral child shapes
|
||||
btGpuChildShape child;
|
||||
child.m_shapeIndex = childColIndex;
|
||||
btVector3 pos = childPositions[i];
|
||||
btQuaternion orn(0,0,0,1);
|
||||
for (int v=0;v<4;v++)
|
||||
{
|
||||
child.m_childPosition[v] = pos[v];
|
||||
child.m_childOrientation[v] = orn[v];
|
||||
}
|
||||
childShapes.push_back(child);
|
||||
btTransform tr;
|
||||
tr.setIdentity();
|
||||
tr.setOrigin(pos);
|
||||
tr.setRotation(orn);
|
||||
|
||||
int baseIndex = vertexArray.size();
|
||||
for (int j=0;j<numIndices;j++)
|
||||
indexArray.push_back(cube_indices[j]+baseIndex);
|
||||
|
||||
//add transformed graphics vertices and indices
|
||||
for (int v=0;v<numVertices;v++)
|
||||
{
|
||||
GLInstanceVertex vert = cubeVerts[v];
|
||||
btVector3 vertPos(vert.xyzw[0],vert.xyzw[1],vert.xyzw[2]);
|
||||
btVector3 newPos = tr*vertPos;
|
||||
vert.xyzw[0] = newPos[0];
|
||||
vert.xyzw[1] = newPos[1];
|
||||
vert.xyzw[2] = newPos[2];
|
||||
vert.xyzw[3] = 0.f;
|
||||
vertexArray.push_back(vert);
|
||||
}
|
||||
|
||||
}
|
||||
colIndex= m_data->m_np->registerCompoundShape(&childShapes);
|
||||
|
||||
}
|
||||
|
||||
//int shapeId = ci.m_instancingRenderer->registerShape(&cube_vertices[0],numVertices,cube_indices,numIndices);
|
||||
int shapeId = ci.m_instancingRenderer->registerShape(&vertexArray[0].xyzw[0],vertexArray.size(),&indexArray[0],indexArray.size());
|
||||
|
||||
btVector4 colors[4] =
|
||||
{
|
||||
btVector4(1,0,0,1),
|
||||
btVector4(0,1,0,1),
|
||||
btVector4(0,0,1,1),
|
||||
btVector4(0,1,1,1),
|
||||
};
|
||||
|
||||
int curColor = 0;
|
||||
for (int i=0;i<ci.arraySizeX;i++)
|
||||
{
|
||||
for (int j=0;j<ci.arraySizeY;j++)
|
||||
{
|
||||
for (int k=0;k<ci.arraySizeZ;k++)
|
||||
{
|
||||
float mass = 1;//j==0? 0.f : 1.f;
|
||||
|
||||
btVector3 position(i*ci.gapX,10+j*ci.gapY,k*ci.gapZ);
|
||||
//btQuaternion orn(0,0,0,1);
|
||||
btQuaternion orn(btVector3(1,0,0),0.7);
|
||||
|
||||
btVector4 color = colors[curColor];
|
||||
curColor++;
|
||||
curColor&=3;
|
||||
btVector4 scaling(1,1,1,1);
|
||||
int id = ci.m_instancingRenderer->registerGraphicsInstance(shapeId,position,orn,color,scaling);
|
||||
int pid = m_data->m_rigidBodyPipeline->registerPhysicsInstance(mass,position,orn,colIndex,index);
|
||||
|
||||
index++;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
float camPos[4]={0,0,0};//65.5,4.5,65.5,0};
|
||||
//float camPos[4]={1,12.5,1.5,0};
|
||||
m_instancingRenderer->setCameraTargetPosition(camPos);
|
||||
m_instancingRenderer->setCameraDistance(20);
|
||||
|
||||
}
|
||||
|
||||
void GpuCompoundPlaneScene::createStaticEnvironment(const ConstructionInfo& ci)
|
||||
{
|
||||
|
||||
|
||||
|
||||
int index=0;
|
||||
btVector3 normal(0,1,0);
|
||||
float constant=0.f;
|
||||
int colIndex = m_data->m_np->registerPlaneShape(normal,constant);//>registerConvexHullShape(&cube_vertices[0],strideInBytes,numVertices, scaling);
|
||||
btVector3 position(0,0,0);
|
||||
btQuaternion orn(0,0,0,1);
|
||||
// btQuaternion orn(btVector3(1,0,0),0.3);
|
||||
btVector4 color(0,0,1,1);
|
||||
btVector4 scaling(100,0.01,100,1);
|
||||
int strideInBytes = 9*sizeof(float);
|
||||
int numVertices = sizeof(cube_vertices)/strideInBytes;
|
||||
int numIndices = sizeof(cube_indices)/sizeof(int);
|
||||
int shapeId = ci.m_instancingRenderer->registerShape(&cube_vertices[0],numVertices,cube_indices,numIndices);
|
||||
|
||||
|
||||
int id = ci.m_instancingRenderer->registerGraphicsInstance(shapeId,position,orn,color,scaling);
|
||||
int pid = m_data->m_rigidBodyPipeline->registerPhysicsInstance(0.f,position,orn,colIndex,index);
|
||||
}
|
||||
@ -22,6 +22,29 @@ public:
|
||||
|
||||
virtual void setupScene(const ConstructionInfo& ci);
|
||||
|
||||
virtual void createStaticEnvironment(const ConstructionInfo& ci);
|
||||
|
||||
};
|
||||
|
||||
|
||||
class GpuCompoundPlaneScene : public GpuCompoundScene
|
||||
{
|
||||
public:
|
||||
|
||||
GpuCompoundPlaneScene(){}
|
||||
virtual ~GpuCompoundPlaneScene(){}
|
||||
virtual const char* getName()
|
||||
{
|
||||
return "GpuCompoundPlane";
|
||||
}
|
||||
|
||||
static GpuDemo* MyCreateFunc()
|
||||
{
|
||||
GpuDemo* demo = new GpuCompoundPlaneScene;
|
||||
return demo;
|
||||
}
|
||||
|
||||
virtual void createStaticEnvironment(const ConstructionInfo& ci);
|
||||
|
||||
};
|
||||
#endif //GPU_COMPOUND_SCENE_H
|
||||
|
||||
@ -387,6 +387,14 @@ int extractManifoldSequentialGlobal( const float4* p, int nPoints, const float4&
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
void computeContactPlaneConvex(int pairIndex,
|
||||
int bodyIndexA, int bodyIndexB,
|
||||
int collidableIndexA, int collidableIndexB,
|
||||
@ -517,6 +525,148 @@ void computeContactPlaneConvex(int pairIndex,
|
||||
// printf("computeContactPlaneConvex\n");
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
void computeContactPlaneCompound(int pairIndex,
|
||||
int bodyIndexA, int bodyIndexB,
|
||||
int collidableIndexA, int collidableIndexB,
|
||||
const btRigidBodyCL* rigidBodies,
|
||||
const btCollidable* collidables,
|
||||
const btConvexPolyhedronCL* convexShapes,
|
||||
const btVector3* convexVertices,
|
||||
const int* convexIndices,
|
||||
const btGpuFace* faces,
|
||||
btContact4* globalContactsOut,
|
||||
int& nGlobalContactsOut,
|
||||
int maxContactCapacity)
|
||||
{
|
||||
|
||||
int shapeTypeB = collidables[collidableIndexB].m_shapeType;
|
||||
btAssert(shapeTypeB == SHAPE_COMPOUND_OF_CONVEX_HULLS);
|
||||
|
||||
|
||||
|
||||
int shapeIndex = collidables[collidableIndexB].m_shapeIndex;
|
||||
const btConvexPolyhedronCL* hullB = &convexShapes[shapeIndex];
|
||||
|
||||
btVector3 posB = rigidBodies[bodyIndexB].m_pos;
|
||||
btQuaternion ornB = rigidBodies[bodyIndexB].m_quat;
|
||||
btVector3 posA = rigidBodies[bodyIndexA].m_pos;
|
||||
btQuaternion ornA = rigidBodies[bodyIndexA].m_quat;
|
||||
|
||||
int numContactsOut = 0;
|
||||
int numWorldVertsB1= 0;
|
||||
|
||||
btVector3 planeEq = faces[collidables[collidableIndexA].m_shapeIndex].m_plane;
|
||||
btVector3 planeNormal(planeEq.x,planeEq.y,planeEq.z);
|
||||
btVector3 planeNormalWorld = quatRotate(ornA,planeNormal);
|
||||
float planeConstant = planeEq.w;
|
||||
btTransform convexWorldTransform;
|
||||
convexWorldTransform.setIdentity();
|
||||
convexWorldTransform.setOrigin(posB);
|
||||
convexWorldTransform.setRotation(ornB);
|
||||
btTransform planeTransform;
|
||||
planeTransform.setIdentity();
|
||||
planeTransform.setOrigin(posA);
|
||||
planeTransform.setRotation(ornA);
|
||||
|
||||
btTransform planeInConvex;
|
||||
planeInConvex= convexWorldTransform.inverse() * planeTransform;
|
||||
btTransform convexInPlane;
|
||||
convexInPlane = planeTransform.inverse() * convexWorldTransform;
|
||||
|
||||
btVector3 planeNormalInConvex = planeInConvex.getBasis()*-planeNormal;
|
||||
float maxDot = -1e30;
|
||||
int hitVertex=-1;
|
||||
btVector3 hitVtx;
|
||||
|
||||
#define MAX_PLANE_CONVEX_POINTS 64
|
||||
|
||||
btVector3 contactPoints[MAX_PLANE_CONVEX_POINTS];
|
||||
int numPoints = 0;
|
||||
|
||||
btInt4 contactIdx;
|
||||
contactIdx.s[0] = 0;
|
||||
contactIdx.s[1] = 1;
|
||||
contactIdx.s[2] = 2;
|
||||
contactIdx.s[3] = 3;
|
||||
|
||||
for (int i=0;i<hullB->m_numVertices;i++)
|
||||
{
|
||||
btVector3 vtx = convexVertices[hullB->m_vertexOffset+i];
|
||||
float curDot = vtx.dot(planeNormalInConvex);
|
||||
|
||||
|
||||
if (curDot>maxDot)
|
||||
{
|
||||
hitVertex=i;
|
||||
maxDot=curDot;
|
||||
hitVtx = vtx;
|
||||
//make sure the deepest points is always included
|
||||
if (numPoints==MAX_PLANE_CONVEX_POINTS)
|
||||
numPoints--;
|
||||
}
|
||||
|
||||
if (numPoints<MAX_PLANE_CONVEX_POINTS)
|
||||
{
|
||||
btVector3 vtxWorld = convexWorldTransform*vtx;
|
||||
btVector3 vtxInPlane = planeTransform.inverse()*vtxWorld;
|
||||
float dist = planeNormal.dot(vtxInPlane)-planeConstant;
|
||||
if (dist<0.f)
|
||||
{
|
||||
vtxWorld.w = dist;
|
||||
contactPoints[numPoints] = vtxWorld;
|
||||
numPoints++;
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
int numReducedPoints = 0;
|
||||
|
||||
numReducedPoints = numPoints;
|
||||
|
||||
if (numPoints>4)
|
||||
{
|
||||
numReducedPoints = extractManifoldSequentialGlobal( contactPoints, numPoints, planeNormalInConvex, &contactIdx);
|
||||
}
|
||||
int dstIdx;
|
||||
// dstIdx = nGlobalContactsOut++;//AppendInc( nGlobalContactsOut, dstIdx );
|
||||
|
||||
if (numReducedPoints>0)
|
||||
{
|
||||
if (nGlobalContactsOut < maxContactCapacity)
|
||||
{
|
||||
dstIdx=nGlobalContactsOut;
|
||||
nGlobalContactsOut++;
|
||||
|
||||
btContact4* c = &globalContactsOut[dstIdx];
|
||||
c->m_worldNormal = planeNormalWorld;
|
||||
c->setFrictionCoeff(0.7);
|
||||
c->setRestituitionCoeff(0.f);
|
||||
|
||||
c->m_batchIdx = pairIndex;
|
||||
c->m_bodyAPtrAndSignBit = rigidBodies[bodyIndexA].m_invMass==0?-bodyIndexA:bodyIndexA;
|
||||
c->m_bodyBPtrAndSignBit = rigidBodies[bodyIndexB].m_invMass==0?-bodyIndexB:bodyIndexB;
|
||||
for (int i=0;i<numReducedPoints;i++)
|
||||
{
|
||||
btVector3 pOnB1 = contactPoints[contactIdx.s[i]];
|
||||
c->m_worldPos[i] = pOnB1;
|
||||
}
|
||||
c->m_worldNormal[3] = numReducedPoints;
|
||||
}//if (dstIdx < numPairs)
|
||||
}
|
||||
|
||||
|
||||
|
||||
// printf("computeContactPlaneConvex\n");
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
void computeContactSphereConvex(int pairIndex,
|
||||
int bodyIndexA, int bodyIndexB,
|
||||
int collidableIndexA, int collidableIndexB,
|
||||
@ -785,7 +935,26 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( const btOpenCLArray<btI
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
if (hostCollidables[collidableIndexA].m_shapeType == SHAPE_COMPOUND_OF_CONVEX_HULLS &&
|
||||
hostCollidables[collidableIndexB].m_shapeType == SHAPE_PLANE)
|
||||
{
|
||||
computeContactPlaneCompound(i,bodyIndexB,bodyIndexA,collidableIndexB,collidableIndexA,&hostBodyBuf[0],
|
||||
&hostCollidables[0],&hostConvexData[0],&hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,nPairs);
|
||||
// printf("convex-plane\n");
|
||||
|
||||
}
|
||||
|
||||
if (hostCollidables[collidableIndexA].m_shapeType == SHAPE_PLANE &&
|
||||
hostCollidables[collidableIndexB].m_shapeType == SHAPE_COMPOUND_OF_CONVEX_HULLS)
|
||||
{
|
||||
computeContactPlaneCompound(i,bodyIndexA,bodyIndexB,collidableIndexA,collidableIndexB,&hostBodyBuf[0],
|
||||
&hostCollidables[0],&hostConvexData[0],&hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,nPairs);
|
||||
// printf("plane-convex\n");
|
||||
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
if (nContacts)
|
||||
|
||||
@ -507,18 +507,17 @@ void computeContactPlaneConvex(int pairIndex,
|
||||
__global const btGpuFace* faces,
|
||||
__global Contact4* restrict globalContactsOut,
|
||||
counter32_t nGlobalContactsOut,
|
||||
int maxContactCapacity)
|
||||
int maxContactCapacity,
|
||||
float4 posB,
|
||||
Quaternion ornB
|
||||
)
|
||||
{
|
||||
|
||||
int shapeIndex = collidables[collidableIndexB].m_shapeIndex;
|
||||
__global const ConvexPolyhedronCL* hullB = &convexShapes[shapeIndex];
|
||||
|
||||
float4 posB;
|
||||
posB = rigidBodies[bodyIndexB].m_pos;
|
||||
Quaternion ornB;
|
||||
ornB = rigidBodies[bodyIndexB].m_quat;
|
||||
float4 posA;
|
||||
posA = rigidBodies[bodyIndexA].m_pos;
|
||||
posA = rigidBodies[bodyIndexA].m_pos;
|
||||
Quaternion ornA;
|
||||
ornA = rigidBodies[bodyIndexA].m_quat;
|
||||
|
||||
@ -736,9 +735,14 @@ __kernel void primitiveContactsKernel( __global const int2* pairs,
|
||||
if (collidables[collidableIndexA].m_shapeType == SHAPE_PLANE &&
|
||||
collidables[collidableIndexB].m_shapeType == SHAPE_CONVEX_HULL)
|
||||
{
|
||||
|
||||
float4 posB;
|
||||
posB = rigidBodies[bodyIndexB].m_pos;
|
||||
Quaternion ornB;
|
||||
ornB = rigidBodies[bodyIndexB].m_quat;
|
||||
computeContactPlaneConvex(pairIndex, bodyIndexA, bodyIndexB, collidableIndexA, collidableIndexB,
|
||||
rigidBodies,collidables,convexShapes,vertices,indices,
|
||||
faces, globalContactsOut, nGlobalContactsOut,maxContactCapacity);
|
||||
faces, globalContactsOut, nGlobalContactsOut,maxContactCapacity, posB,ornB);
|
||||
return;
|
||||
}
|
||||
|
||||
@ -747,10 +751,15 @@ __kernel void primitiveContactsKernel( __global const int2* pairs,
|
||||
collidables[collidableIndexB].m_shapeType == SHAPE_PLANE)
|
||||
{
|
||||
|
||||
float4 posA;
|
||||
posA = rigidBodies[bodyIndexA].m_pos;
|
||||
Quaternion ornA;
|
||||
ornA = rigidBodies[bodyIndexA].m_quat;
|
||||
|
||||
|
||||
computeContactPlaneConvex( pairIndex, bodyIndexB,bodyIndexA, collidableIndexB,collidableIndexA,
|
||||
rigidBodies,collidables,convexShapes,vertices,indices,
|
||||
faces, globalContactsOut, nGlobalContactsOut,maxContactCapacity);
|
||||
faces, globalContactsOut, nGlobalContactsOut,maxContactCapacity,posA,ornA);
|
||||
|
||||
return;
|
||||
}
|
||||
@ -936,6 +945,24 @@ __kernel void processCompoundPairsPrimitivesKernel( __global const int4* gpuCo
|
||||
int shapeTypeB = collidables[collidableIndexB].m_shapeType;
|
||||
|
||||
int pairIndex = i;
|
||||
if ((shapeTypeA == SHAPE_PLANE) && (shapeTypeB==SHAPE_CONVEX_HULL))
|
||||
{
|
||||
|
||||
computeContactPlaneConvex( pairIndex, bodyIndexA,bodyIndexB, collidableIndexA,collidableIndexB,
|
||||
rigidBodies,collidables,convexShapes,vertices,indices,
|
||||
faces, globalContactsOut, nGlobalContactsOut,maxContactCapacity,posB,ornB);
|
||||
return;
|
||||
}
|
||||
|
||||
if ((shapeTypeA == SHAPE_CONVEX_HULL) && (shapeTypeB==SHAPE_PLANE))
|
||||
{
|
||||
|
||||
computeContactPlaneConvex( pairIndex, bodyIndexB,bodyIndexA, collidableIndexB,collidableIndexA,
|
||||
rigidBodies,collidables,convexShapes,vertices,indices,
|
||||
faces, globalContactsOut, nGlobalContactsOut,maxContactCapacity,posA,ornA);
|
||||
return;
|
||||
}
|
||||
|
||||
if ((shapeTypeA == SHAPE_CONVEX_HULL) && (shapeTypeB == SHAPE_SPHERE))
|
||||
{
|
||||
float4 spherePos = rigidBodies[bodyIndexB].m_pos;
|
||||
|
||||
@ -496,8 +496,148 @@ static const char* primitiveContactsKernelsCL= \
|
||||
"\n"
|
||||
"\n"
|
||||
"\n"
|
||||
" \n"
|
||||
"void computeContactPlaneConvex(int pairIndex,\n"
|
||||
"#define MAX_PLANE_CONVEX_POINTS 64\n"
|
||||
"\n"
|
||||
"void computeContactPlaneConvex(int pairIndex,\n"
|
||||
" int bodyIndexA, int bodyIndexB, \n"
|
||||
" int collidableIndexA, int collidableIndexB, \n"
|
||||
" __global const BodyData* rigidBodies, \n"
|
||||
" __global const btCollidableGpu*collidables,\n"
|
||||
" __global const ConvexPolyhedronCL* convexShapes,\n"
|
||||
" __global const float4* convexVertices,\n"
|
||||
" __global const int* convexIndices,\n"
|
||||
" __global const btGpuFace* faces,\n"
|
||||
" __global Contact4* restrict globalContactsOut,\n"
|
||||
" counter32_t nGlobalContactsOut,\n"
|
||||
" int maxContactCapacity,\n"
|
||||
" float4 posB,\n"
|
||||
" Quaternion ornB\n"
|
||||
" )\n"
|
||||
"{\n"
|
||||
"\n"
|
||||
" int shapeIndex = collidables[collidableIndexB].m_shapeIndex;\n"
|
||||
" __global const ConvexPolyhedronCL* hullB = &convexShapes[shapeIndex];\n"
|
||||
" \n"
|
||||
" float4 posA;\n"
|
||||
" posA = rigidBodies[bodyIndexA].m_pos;\n"
|
||||
" Quaternion ornA;\n"
|
||||
" ornA = rigidBodies[bodyIndexA].m_quat;\n"
|
||||
"\n"
|
||||
" int numContactsOut = 0;\n"
|
||||
" int numWorldVertsB1= 0;\n"
|
||||
"\n"
|
||||
" float4 planeEq;\n"
|
||||
" planeEq = faces[collidables[collidableIndexA].m_shapeIndex].m_plane;\n"
|
||||
" float4 planeNormal = make_float4(planeEq.x,planeEq.y,planeEq.z,0.f);\n"
|
||||
" float4 planeNormalWorld;\n"
|
||||
" planeNormalWorld = qtRotate(ornA,planeNormal);\n"
|
||||
" float planeConstant = planeEq.w;\n"
|
||||
" \n"
|
||||
" float4 invPosA;Quaternion invOrnA;\n"
|
||||
" float4 convexInPlaneTransPos1; Quaternion convexInPlaneTransOrn1;\n"
|
||||
" {\n"
|
||||
" \n"
|
||||
" trInverse(posA,ornA,&invPosA,&invOrnA);\n"
|
||||
" trMul(invPosA,invOrnA,posB,ornB,&convexInPlaneTransPos1,&convexInPlaneTransOrn1);\n"
|
||||
" }\n"
|
||||
" float4 invPosB;Quaternion invOrnB;\n"
|
||||
" float4 planeInConvexPos1; Quaternion planeInConvexOrn1;\n"
|
||||
" {\n"
|
||||
" \n"
|
||||
" trInverse(posB,ornB,&invPosB,&invOrnB);\n"
|
||||
" trMul(invPosB,invOrnB,posA,ornA,&planeInConvexPos1,&planeInConvexOrn1); \n"
|
||||
" }\n"
|
||||
"\n"
|
||||
" \n"
|
||||
" float4 planeNormalInConvex = qtRotate(planeInConvexOrn1,-planeNormal);\n"
|
||||
" float maxDot = -1e30;\n"
|
||||
" int hitVertex=-1;\n"
|
||||
" float4 hitVtx;\n"
|
||||
"\n"
|
||||
"\n"
|
||||
"\n"
|
||||
" float4 contactPoints[MAX_PLANE_CONVEX_POINTS];\n"
|
||||
" int numPoints = 0;\n"
|
||||
"\n"
|
||||
" int4 contactIdx;\n"
|
||||
" contactIdx=make_int4(0,1,2,3);\n"
|
||||
" \n"
|
||||
" \n"
|
||||
" for (int i=0;i<hullB->m_numVertices;i++)\n"
|
||||
" {\n"
|
||||
" float4 vtx = convexVertices[hullB->m_vertexOffset+i];\n"
|
||||
" float curDot = dot(vtx,planeNormalInConvex);\n"
|
||||
"\n"
|
||||
"\n"
|
||||
" if (curDot>maxDot)\n"
|
||||
" {\n"
|
||||
" hitVertex=i;\n"
|
||||
" maxDot=curDot;\n"
|
||||
" hitVtx = vtx;\n"
|
||||
" //make sure the deepest points is always included\n"
|
||||
" if (numPoints==MAX_PLANE_CONVEX_POINTS)\n"
|
||||
" numPoints--;\n"
|
||||
" }\n"
|
||||
"\n"
|
||||
" if (numPoints<MAX_PLANE_CONVEX_POINTS)\n"
|
||||
" {\n"
|
||||
" float4 vtxWorld = transform(&vtx, &posB, &ornB);\n"
|
||||
" float4 vtxInPlane = transform(&vtxWorld, &invPosA, &invOrnA);//oplaneTransform.inverse()*vtxWorld;\n"
|
||||
" float dist = dot(planeNormal,vtxInPlane)-planeConstant;\n"
|
||||
" if (dist<0.f)\n"
|
||||
" {\n"
|
||||
" vtxWorld.w = dist;\n"
|
||||
" contactPoints[numPoints] = vtxWorld;\n"
|
||||
" numPoints++;\n"
|
||||
" }\n"
|
||||
" }\n"
|
||||
"\n"
|
||||
" }\n"
|
||||
"\n"
|
||||
" int numReducedPoints = numPoints;\n"
|
||||
" //if (numPoints>4)\n"
|
||||
" //{\n"
|
||||
"// numReducedPoints = extractManifoldSequentialGlobal( contactPoints, numPoints, planeNormalInConvex, &contactIdx);\n"
|
||||
" //}\n"
|
||||
"\n"
|
||||
" if (numReducedPoints>0)\n"
|
||||
" {\n"
|
||||
" int dstIdx;\n"
|
||||
" AppendInc( nGlobalContactsOut, dstIdx );\n"
|
||||
"\n"
|
||||
" if (dstIdx < maxContactCapacity)\n"
|
||||
" {\n"
|
||||
" __global Contact4* c = &globalContactsOut[dstIdx];\n"
|
||||
" c->m_worldNormal = planeNormalWorld;\n"
|
||||
" //c->setFrictionCoeff(0.7);\n"
|
||||
" //c->setRestituitionCoeff(0.f);\n"
|
||||
" c->m_coeffs = (u32)(0.f*0xffff) | ((u32)(0.7f*0xffff)<<16);\n"
|
||||
" c->m_batchIdx = pairIndex;\n"
|
||||
" c->m_bodyAPtrAndSignBit = rigidBodies[bodyIndexA].m_invMass==0?-bodyIndexA:bodyIndexA;\n"
|
||||
" c->m_bodyBPtrAndSignBit = rigidBodies[bodyIndexB].m_invMass==0?-bodyIndexB:bodyIndexB;\n"
|
||||
"\n"
|
||||
" switch (numReducedPoints)\n"
|
||||
" {\n"
|
||||
" case 4:\n"
|
||||
" c->m_worldPos[3] = contactPoints[contactIdx.w];\n"
|
||||
" case 3:\n"
|
||||
" c->m_worldPos[2] = contactPoints[contactIdx.z];\n"
|
||||
" case 2:\n"
|
||||
" c->m_worldPos[1] = contactPoints[contactIdx.y];\n"
|
||||
" case 1:\n"
|
||||
" c->m_worldPos[0] = contactPoints[contactIdx.x];\n"
|
||||
" default:\n"
|
||||
" {\n"
|
||||
" }\n"
|
||||
" };\n"
|
||||
" \n"
|
||||
" GET_NPOINTS(*c) = numReducedPoints;\n"
|
||||
" }//if (dstIdx < numPairs)\n"
|
||||
" } \n"
|
||||
"}\n"
|
||||
"\n"
|
||||
"\n"
|
||||
"void computeContactPlaneSphere(int pairIndex,\n"
|
||||
" int bodyIndexA, int bodyIndexB, \n"
|
||||
" int collidableIndexA, int collidableIndexB, \n"
|
||||
" __global const BodyData* rigidBodies, \n"
|
||||
@ -559,8 +699,6 @@ static const char* primitiveContactsKernelsCL= \
|
||||
"}\n"
|
||||
"\n"
|
||||
"\n"
|
||||
"\n"
|
||||
"\n"
|
||||
"__kernel void primitiveContactsKernel( __global const int2* pairs, \n"
|
||||
" __global const BodyData* rigidBodies, \n"
|
||||
" __global const btCollidableGpu* collidables,\n"
|
||||
@ -596,27 +734,61 @@ static const char* primitiveContactsKernelsCL= \
|
||||
" int collidableIndexA = rigidBodies[bodyIndexA].m_collidableIdx;\n"
|
||||
" int collidableIndexB = rigidBodies[bodyIndexB].m_collidableIdx;\n"
|
||||
" \n"
|
||||
" if (collidables[collidableIndexA].m_shapeType == SHAPE_PLANE &&\n"
|
||||
" collidables[collidableIndexB].m_shapeType == SHAPE_CONVEX_HULL)\n"
|
||||
" {\n"
|
||||
"\n"
|
||||
" if (collidables[collidableIndexA].m_shapeType == SHAPE_SPHERE &&\n"
|
||||
" float4 posB;\n"
|
||||
" posB = rigidBodies[bodyIndexB].m_pos;\n"
|
||||
" Quaternion ornB;\n"
|
||||
" ornB = rigidBodies[bodyIndexB].m_quat;\n"
|
||||
" computeContactPlaneConvex(pairIndex, bodyIndexA, bodyIndexB, collidableIndexA, collidableIndexB, \n"
|
||||
" rigidBodies,collidables,convexShapes,vertices,indices,\n"
|
||||
" faces, globalContactsOut, nGlobalContactsOut,maxContactCapacity, posB,ornB);\n"
|
||||
" return;\n"
|
||||
" }\n"
|
||||
"\n"
|
||||
"\n"
|
||||
" if (collidables[collidableIndexA].m_shapeType == SHAPE_CONVEX_HULL &&\n"
|
||||
" collidables[collidableIndexB].m_shapeType == SHAPE_PLANE)\n"
|
||||
" {\n"
|
||||
"\n"
|
||||
" float4 posA;\n"
|
||||
" posA = rigidBodies[bodyIndexA].m_pos;\n"
|
||||
" Quaternion ornA;\n"
|
||||
" ornA = rigidBodies[bodyIndexA].m_quat;\n"
|
||||
"\n"
|
||||
"\n"
|
||||
" computeContactPlaneConvex( pairIndex, bodyIndexB,bodyIndexA, collidableIndexB,collidableIndexA, \n"
|
||||
" rigidBodies,collidables,faces, globalContactsOut, nGlobalContactsOut,maxContactCapacity);\n"
|
||||
" rigidBodies,collidables,convexShapes,vertices,indices,\n"
|
||||
" faces, globalContactsOut, nGlobalContactsOut,maxContactCapacity,posA,ornA);\n"
|
||||
"\n"
|
||||
" return;\n"
|
||||
" }\n"
|
||||
"\n"
|
||||
" if (collidables[collidableIndexA].m_shapeType == SHAPE_PLANE &&\n"
|
||||
" collidables[collidableIndexB].m_shapeType == SHAPE_SPHERE)\n"
|
||||
" {\n"
|
||||
"\n"
|
||||
"\n"
|
||||
" computeContactPlaneConvex(pairIndex, bodyIndexA, bodyIndexB, collidableIndexA, collidableIndexB, \n"
|
||||
" rigidBodies,collidables,faces, globalContactsOut, nGlobalContactsOut,maxContactCapacity);\n"
|
||||
" computeContactPlaneSphere(pairIndex, bodyIndexA, bodyIndexB, collidableIndexA, collidableIndexB, \n"
|
||||
" rigidBodies,collidables,faces, globalContactsOut, nGlobalContactsOut,maxContactCapacity);\n"
|
||||
" return;\n"
|
||||
" \n"
|
||||
" }\n"
|
||||
"\n"
|
||||
"\n"
|
||||
" if (collidables[collidableIndexA].m_shapeType == SHAPE_SPHERE &&\n"
|
||||
" collidables[collidableIndexB].m_shapeType == SHAPE_PLANE)\n"
|
||||
" {\n"
|
||||
"\n"
|
||||
"\n"
|
||||
" computeContactPlaneSphere( pairIndex, bodyIndexB,bodyIndexA, collidableIndexB,collidableIndexA, \n"
|
||||
" rigidBodies,collidables,\n"
|
||||
" faces, globalContactsOut, nGlobalContactsOut,maxContactCapacity);\n"
|
||||
"\n"
|
||||
" return;\n"
|
||||
" }\n"
|
||||
"\n"
|
||||
" \n"
|
||||
"\n"
|
||||
" \n"
|
||||
" if (collidables[collidableIndexA].m_shapeType == SHAPE_SPHERE &&\n"
|
||||
" collidables[collidableIndexB].m_shapeType == SHAPE_CONVEX_HULL)\n"
|
||||
@ -651,6 +823,7 @@ static const char* primitiveContactsKernelsCL= \
|
||||
" \n"
|
||||
" \n"
|
||||
" \n"
|
||||
" \n"
|
||||
" \n"
|
||||
" \n"
|
||||
" if (collidables[collidableIndexA].m_shapeType == SHAPE_SPHERE &&\n"
|
||||
@ -774,6 +947,24 @@ static const char* primitiveContactsKernelsCL= \
|
||||
" int shapeTypeB = collidables[collidableIndexB].m_shapeType;\n"
|
||||
"\n"
|
||||
" int pairIndex = i;\n"
|
||||
" if ((shapeTypeA == SHAPE_PLANE) && (shapeTypeB==SHAPE_CONVEX_HULL))\n"
|
||||
" {\n"
|
||||
"\n"
|
||||
" computeContactPlaneConvex( pairIndex, bodyIndexA,bodyIndexB, collidableIndexA,collidableIndexB, \n"
|
||||
" rigidBodies,collidables,convexShapes,vertices,indices,\n"
|
||||
" faces, globalContactsOut, nGlobalContactsOut,maxContactCapacity,posB,ornB);\n"
|
||||
" return;\n"
|
||||
" }\n"
|
||||
"\n"
|
||||
" if ((shapeTypeA == SHAPE_CONVEX_HULL) && (shapeTypeB==SHAPE_PLANE))\n"
|
||||
" {\n"
|
||||
"\n"
|
||||
" computeContactPlaneConvex( pairIndex, bodyIndexB,bodyIndexA, collidableIndexB,collidableIndexA, \n"
|
||||
" rigidBodies,collidables,convexShapes,vertices,indices,\n"
|
||||
" faces, globalContactsOut, nGlobalContactsOut,maxContactCapacity,posA,ornA);\n"
|
||||
" return;\n"
|
||||
" }\n"
|
||||
"\n"
|
||||
" if ((shapeTypeA == SHAPE_CONVEX_HULL) && (shapeTypeB == SHAPE_SPHERE))\n"
|
||||
" {\n"
|
||||
" float4 spherePos = rigidBodies[bodyIndexB].m_pos;\n"
|
||||
|
||||
Loading…
Reference in New Issue
Block a user