summaryrefslogtreecommitdiff
path: root/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3ContactConvexConvexSAT.h
diff options
context:
space:
mode:
Diffstat (limited to 'thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3ContactConvexConvexSAT.h')
-rw-r--r--thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3ContactConvexConvexSAT.h478
1 files changed, 222 insertions, 256 deletions
diff --git a/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3ContactConvexConvexSAT.h b/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3ContactConvexConvexSAT.h
index f295f01a6c..ca68f4bc4e 100644
--- a/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3ContactConvexConvexSAT.h
+++ b/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3ContactConvexConvexSAT.h
@@ -2,48 +2,43 @@
#ifndef B3_CONTACT_CONVEX_CONVEX_SAT_H
#define B3_CONTACT_CONVEX_CONVEX_SAT_H
-
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Contact4Data.h"
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3FindSeparatingAxis.h"
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3ReduceContacts.h"
#define B3_MAX_VERTS 1024
-
-
-inline b3Float4 b3Lerp3(const b3Float4& a,const b3Float4& b, float t)
+inline b3Float4 b3Lerp3(const b3Float4& a, const b3Float4& b, float 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);
+ 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);
}
-
// Clips a face to the back of a plane, return the number of vertices out, stored in ppVtxOut
-inline int b3ClipFace(const b3Float4* pVtxIn, int numVertsIn, b3Float4& planeNormalWS,float planeEqWS, b3Float4* ppVtxOut)
+inline int b3ClipFace(const b3Float4* pVtxIn, int numVertsIn, b3Float4& planeNormalWS, float planeEqWS, b3Float4* ppVtxOut)
{
-
int ve;
float ds, de;
int numVertsOut = 0;
if (numVertsIn < 2)
return 0;
- b3Float4 firstVertex=pVtxIn[numVertsIn-1];
+ b3Float4 firstVertex = pVtxIn[numVertsIn - 1];
b3Float4 endVertex = pVtxIn[0];
-
- ds = b3Dot3F4(planeNormalWS,firstVertex)+planeEqWS;
+
+ ds = b3Dot3F4(planeNormalWS, firstVertex) + planeEqWS;
for (ve = 0; ve < numVertsIn; ve++)
{
- endVertex=pVtxIn[ve];
+ endVertex = pVtxIn[ve];
- de = b3Dot3F4(planeNormalWS,endVertex)+planeEqWS;
+ de = b3Dot3F4(planeNormalWS, endVertex) + planeEqWS;
- if (ds<0)
+ if (ds < 0)
{
- if (de<0)
+ if (de < 0)
{
// Start < 0, end < 0, so output endVertex
ppVtxOut[numVertsOut++] = endVertex;
@@ -51,15 +46,15 @@ inline int b3ClipFace(const b3Float4* pVtxIn, int numVertsIn, b3Float4& planeNor
else
{
// Start < 0, end >= 0, so output intersection
- ppVtxOut[numVertsOut++] = b3Lerp3(firstVertex, endVertex,(ds * 1.f/(ds - de)) );
+ ppVtxOut[numVertsOut++] = b3Lerp3(firstVertex, endVertex, (ds * 1.f / (ds - de)));
}
}
else
{
- if (de<0)
+ if (de < 0)
{
// Start >= 0, end < 0 so output intersection and end
- ppVtxOut[numVertsOut++] = b3Lerp3(firstVertex, endVertex,(ds * 1.f/(ds - de)) );
+ ppVtxOut[numVertsOut++] = b3Lerp3(firstVertex, endVertex, (ds * 1.f / (ds - de)));
ppVtxOut[numVertsOut++] = endVertex;
}
}
@@ -69,36 +64,35 @@ inline int b3ClipFace(const b3Float4* pVtxIn, int numVertsIn, b3Float4& planeNor
return numVertsOut;
}
-
-inline int b3ClipFaceAgainstHull(const b3Float4& separatingNormal, const b3ConvexPolyhedronData* hullA,
- const b3Float4& posA, const b3Quaternion& ornA, b3Float4* worldVertsB1, int numWorldVertsB1,
- b3Float4* worldVertsB2, int capacityWorldVertsB2,
- const float minDist, float maxDist,
- const b3AlignedObjectArray<b3Float4>& verticesA, const b3AlignedObjectArray<b3GpuFace>& facesA, const b3AlignedObjectArray<int>& indicesA,
- //const b3Float4* verticesB, const b3GpuFace* facesB, const int* indicesB,
- b3Float4* contactsOut,
- int contactCapacity)
+inline int b3ClipFaceAgainstHull(const b3Float4& separatingNormal, const b3ConvexPolyhedronData* hullA,
+ const b3Float4& posA, const b3Quaternion& ornA, b3Float4* worldVertsB1, int numWorldVertsB1,
+ b3Float4* worldVertsB2, int capacityWorldVertsB2,
+ const float minDist, float maxDist,
+ const b3AlignedObjectArray<b3Float4>& verticesA, const b3AlignedObjectArray<b3GpuFace>& facesA, const b3AlignedObjectArray<int>& indicesA,
+ //const b3Float4* verticesB, const b3GpuFace* facesB, const int* indicesB,
+ b3Float4* contactsOut,
+ int contactCapacity)
{
int numContactsOut = 0;
b3Float4* pVtxIn = worldVertsB1;
b3Float4* pVtxOut = worldVertsB2;
-
+
int numVertsIn = numWorldVertsB1;
int numVertsOut = 0;
- int closestFaceA=-1;
+ int closestFaceA = -1;
{
float dmin = FLT_MAX;
- for(int face=0;face<hullA->m_numFaces;face++)
+ for (int face = 0; face < hullA->m_numFaces; face++)
{
const b3Float4 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);
- const b3Float4 faceANormalWS = b3QuatRotate(ornA,Normal);
-
- float d = b3Dot3F4(faceANormalWS,separatingNormal);
+ 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);
+ const b3Float4 faceANormalWS = b3QuatRotate(ornA, Normal);
+
+ float d = b3Dot3F4(faceANormalWS, separatingNormal);
if (d < dmin)
{
dmin = d;
@@ -106,33 +100,33 @@ inline int b3ClipFaceAgainstHull(const b3Float4& separatingNormal, const b3Conve
}
}
}
- if (closestFaceA<0)
+ if (closestFaceA < 0)
return numContactsOut;
- b3GpuFace polyA = facesA[hullA->m_faceOffset+closestFaceA];
+ b3GpuFace polyA = facesA[hullA->m_faceOffset + closestFaceA];
// clip polygon to back of planes of all faces of hull A that are adjacent to witness face
//int numContacts = numWorldVertsB1;
int numVerticesA = polyA.m_numIndices;
- for(int e0=0;e0<numVerticesA;e0++)
+ for (int e0 = 0; e0 < numVerticesA; e0++)
{
- const b3Float4 a = verticesA[hullA->m_vertexOffset+indicesA[polyA.m_indexOffset+e0]];
- const b3Float4 b = verticesA[hullA->m_vertexOffset+indicesA[polyA.m_indexOffset+((e0+1)%numVerticesA)]];
+ const b3Float4 a = verticesA[hullA->m_vertexOffset + indicesA[polyA.m_indexOffset + e0]];
+ const b3Float4 b = verticesA[hullA->m_vertexOffset + indicesA[polyA.m_indexOffset + ((e0 + 1) % numVerticesA)]];
const b3Float4 edge0 = a - b;
- const b3Float4 WorldEdge0 = b3QuatRotate(ornA,edge0);
- b3Float4 planeNormalA = b3MakeFloat4(polyA.m_plane.x,polyA.m_plane.y,polyA.m_plane.z,0.f);
- b3Float4 worldPlaneAnormal1 = b3QuatRotate(ornA,planeNormalA);
-
- b3Float4 planeNormalWS1 = -b3Cross3(WorldEdge0,worldPlaneAnormal1);
- b3Float4 worldA1 = b3TransformPoint(a,posA,ornA);
- float planeEqWS1 = -b3Dot3F4(worldA1,planeNormalWS1);
-
+ const b3Float4 WorldEdge0 = b3QuatRotate(ornA, edge0);
+ b3Float4 planeNormalA = b3MakeFloat4(polyA.m_plane.x, polyA.m_plane.y, polyA.m_plane.z, 0.f);
+ b3Float4 worldPlaneAnormal1 = b3QuatRotate(ornA, planeNormalA);
+
+ b3Float4 planeNormalWS1 = -b3Cross3(WorldEdge0, worldPlaneAnormal1);
+ b3Float4 worldA1 = b3TransformPoint(a, posA, ornA);
+ float planeEqWS1 = -b3Dot3F4(worldA1, planeNormalWS1);
+
b3Float4 planeNormalWS = planeNormalWS1;
- float planeEqWS=planeEqWS1;
-
+ float planeEqWS = planeEqWS1;
+
//clip face
//clipFace(*pVtxIn, *pVtxOut,planeNormalWS,planeEqWS);
- numVertsOut = b3ClipFace(pVtxIn, numVertsIn, planeNormalWS,planeEqWS, pVtxOut);
+ numVertsOut = b3ClipFace(pVtxIn, numVertsIn, planeNormalWS, planeEqWS, pVtxOut);
//btSwap(pVtxIn,pVtxOut);
b3Float4* tmp = pVtxOut;
@@ -142,32 +136,32 @@ inline int b3ClipFaceAgainstHull(const b3Float4& separatingNormal, const b3Conve
numVertsOut = 0;
}
-
// only keep points that are behind the witness face
{
- b3Float4 localPlaneNormal = b3MakeFloat4(polyA.m_plane.x,polyA.m_plane.y,polyA.m_plane.z,0.f);
+ b3Float4 localPlaneNormal = b3MakeFloat4(polyA.m_plane.x, polyA.m_plane.y, polyA.m_plane.z, 0.f);
float localPlaneEq = polyA.m_plane.w;
- b3Float4 planeNormalWS = b3QuatRotate(ornA,localPlaneNormal);
- float planeEqWS=localPlaneEq-b3Dot3F4(planeNormalWS,posA);
- for (int i=0;i<numVertsIn;i++)
+ b3Float4 planeNormalWS = b3QuatRotate(ornA, localPlaneNormal);
+ float planeEqWS = localPlaneEq - b3Dot3F4(planeNormalWS, posA);
+ for (int i = 0; i < numVertsIn; i++)
{
- float depth = b3Dot3F4(planeNormalWS,pVtxIn[i])+planeEqWS;
- if (depth <=minDist)
+ float depth = b3Dot3F4(planeNormalWS, pVtxIn[i]) + planeEqWS;
+ if (depth <= minDist)
{
depth = minDist;
}
- if (numContactsOut<contactCapacity)
+ if (numContactsOut < contactCapacity)
{
- if (depth <=maxDist)
+ if (depth <= maxDist)
{
b3Float4 pointInWorld = pVtxIn[i];
//resultOut.addContactPoint(separatingNormal,point,depth);
- contactsOut[numContactsOut++] = b3MakeVector3(pointInWorld.x,pointInWorld.y,pointInWorld.z,depth);
+ contactsOut[numContactsOut++] = b3MakeVector3(pointInWorld.x, pointInWorld.y, pointInWorld.z, depth);
//printf("depth=%f\n",depth);
}
- } else
+ }
+ else
{
- b3Error("exceeding contact capacity (%d,%df)\n", numContactsOut,contactCapacity);
+ b3Error("exceeding contact capacity (%d,%df)\n", numContactsOut, contactCapacity);
}
}
}
@@ -175,62 +169,60 @@ inline int b3ClipFaceAgainstHull(const b3Float4& separatingNormal, const b3Conve
return numContactsOut;
}
+inline int b3ClipHullAgainstHull(const b3Float4& separatingNormal,
+ const b3ConvexPolyhedronData& hullA, const b3ConvexPolyhedronData& hullB,
+ const b3Float4& posA, const b3Quaternion& ornA, const b3Float4& posB, const b3Quaternion& ornB,
+ b3Float4* worldVertsB1, b3Float4* worldVertsB2, int capacityWorldVerts,
+ const float minDist, float maxDist,
+ const b3AlignedObjectArray<b3Float4>& verticesA, const b3AlignedObjectArray<b3GpuFace>& facesA, const b3AlignedObjectArray<int>& indicesA,
+ const b3AlignedObjectArray<b3Float4>& verticesB, const b3AlignedObjectArray<b3GpuFace>& facesB, const b3AlignedObjectArray<int>& indicesB,
-
-inline int b3ClipHullAgainstHull(const b3Float4& separatingNormal,
- const b3ConvexPolyhedronData& hullA, const b3ConvexPolyhedronData& hullB,
- const b3Float4& posA, const b3Quaternion& ornA,const b3Float4& posB, const b3Quaternion& ornB,
- b3Float4* worldVertsB1, b3Float4* worldVertsB2, int capacityWorldVerts,
- const float minDist, float maxDist,
- const b3AlignedObjectArray<b3Float4>& verticesA, const b3AlignedObjectArray<b3GpuFace>& facesA, const b3AlignedObjectArray<int>& indicesA,
- const b3AlignedObjectArray<b3Float4>& verticesB, const b3AlignedObjectArray<b3GpuFace>& facesB, const b3AlignedObjectArray<int>& indicesB,
-
- b3Float4* contactsOut,
- int contactCapacity)
+ b3Float4* contactsOut,
+ int contactCapacity)
{
int numContactsOut = 0;
- int numWorldVertsB1= 0;
-
+ int numWorldVertsB1 = 0;
+
B3_PROFILE("clipHullAgainstHull");
//float curMaxDist=maxDist;
- int closestFaceB=-1;
+ int closestFaceB = -1;
float dmax = -FLT_MAX;
{
//B3_PROFILE("closestFaceB");
- if (hullB.m_numFaces!=1)
+ if (hullB.m_numFaces != 1)
{
//printf("wtf\n");
}
static bool once = true;
//printf("separatingNormal=%f,%f,%f\n",separatingNormal.x,separatingNormal.y,separatingNormal.z);
-
- for(int face=0;face<hullB.m_numFaces;face++)
+
+ for (int face = 0; face < hullB.m_numFaces; face++)
{
#ifdef BT_DEBUG_SAT_FACE
if (once)
- printf("face %d\n",face);
- const b3GpuFace* faceB = &facesB[hullB.m_faceOffset+face];
+ printf("face %d\n", face);
+ const b3GpuFace* faceB = &facesB[hullB.m_faceOffset + face];
if (once)
{
- for (int i=0;i<faceB->m_numIndices;i++)
+ for (int i = 0; i < faceB->m_numIndices; i++)
{
- b3Float4 vert = verticesB[hullB.m_vertexOffset+indicesB[faceB->m_indexOffset+i]];
- printf("vert[%d] = %f,%f,%f\n",i,vert.x,vert.y,vert.z);
+ b3Float4 vert = verticesB[hullB.m_vertexOffset + indicesB[faceB->m_indexOffset + i]];
+ printf("vert[%d] = %f,%f,%f\n", i, vert.x, vert.y, vert.z);
}
}
-#endif //BT_DEBUG_SAT_FACE
- //if (facesB[hullB.m_faceOffset+face].m_numIndices>2)
+#endif //BT_DEBUG_SAT_FACE \
+ //if (facesB[hullB.m_faceOffset+face].m_numIndices>2)
{
- const b3Float4 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 b3Float4 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 b3Float4 WorldNormal = b3QuatRotate(ornB, Normal);
#ifdef BT_DEBUG_SAT_FACE
if (once)
- printf("faceNormal = %f,%f,%f\n",Normal.x,Normal.y,Normal.z);
+ printf("faceNormal = %f,%f,%f\n", Normal.x, Normal.y, Normal.z);
#endif
- float d = b3Dot3F4(WorldNormal,separatingNormal);
+ float d = b3Dot3F4(WorldNormal, separatingNormal);
if (d > dmax)
{
dmax = d;
@@ -241,79 +233,73 @@ inline int b3ClipHullAgainstHull(const b3Float4& separatingNormal,
once = false;
}
-
- b3Assert(closestFaceB>=0);
+ b3Assert(closestFaceB >= 0);
{
//B3_PROFILE("worldVertsB1");
- const b3GpuFace& polyB = facesB[hullB.m_faceOffset+closestFaceB];
+ const b3GpuFace& polyB = facesB[hullB.m_faceOffset + closestFaceB];
const int numVertices = polyB.m_numIndices;
- for(int e0=0;e0<numVertices;e0++)
+ for (int e0 = 0; e0 < numVertices; e0++)
{
- const b3Float4& b = verticesB[hullB.m_vertexOffset+indicesB[polyB.m_indexOffset+e0]];
- worldVertsB1[numWorldVertsB1++] = b3TransformPoint(b,posB,ornB);
+ const b3Float4& b = verticesB[hullB.m_vertexOffset + indicesB[polyB.m_indexOffset + e0]];
+ worldVertsB1[numWorldVertsB1++] = b3TransformPoint(b, posB, ornB);
}
}
- if (closestFaceB>=0)
+ if (closestFaceB >= 0)
{
//B3_PROFILE("clipFaceAgainstHull");
- numContactsOut = b3ClipFaceAgainstHull((b3Float4&)separatingNormal, &hullA,
- posA,ornA,
- worldVertsB1,numWorldVertsB1,worldVertsB2,capacityWorldVerts, minDist, maxDist,
- verticesA, facesA, indicesA,
- contactsOut,contactCapacity);
+ numContactsOut = b3ClipFaceAgainstHull((b3Float4&)separatingNormal, &hullA,
+ posA, ornA,
+ worldVertsB1, numWorldVertsB1, worldVertsB2, capacityWorldVerts, minDist, maxDist,
+ verticesA, facesA, indicesA,
+ contactsOut, contactCapacity);
}
return numContactsOut;
}
-
-
-
inline int b3ClipHullHullSingle(
- int bodyIndexA, int bodyIndexB,
- const b3Float4& posA,
- const b3Quaternion& ornA,
- const b3Float4& posB,
- const b3Quaternion& ornB,
-
- int collidableIndexA, int collidableIndexB,
-
- const b3AlignedObjectArray<b3RigidBodyData>* bodyBuf,
- b3AlignedObjectArray<b3Contact4Data>* globalContactOut,
- int& nContacts,
-
- const b3AlignedObjectArray<b3ConvexPolyhedronData>& hostConvexDataA,
- const b3AlignedObjectArray<b3ConvexPolyhedronData>& hostConvexDataB,
-
- const b3AlignedObjectArray<b3Vector3>& verticesA,
- const b3AlignedObjectArray<b3Vector3>& uniqueEdgesA,
- const b3AlignedObjectArray<b3GpuFace>& facesA,
- const b3AlignedObjectArray<int>& indicesA,
-
- const b3AlignedObjectArray<b3Vector3>& verticesB,
- const b3AlignedObjectArray<b3Vector3>& uniqueEdgesB,
- const b3AlignedObjectArray<b3GpuFace>& facesB,
- const b3AlignedObjectArray<int>& indicesB,
-
- const b3AlignedObjectArray<b3Collidable>& hostCollidablesA,
- const b3AlignedObjectArray<b3Collidable>& hostCollidablesB,
- const b3Vector3& sepNormalWorldSpace,
- int maxContactCapacity )
+ int bodyIndexA, int bodyIndexB,
+ const b3Float4& posA,
+ const b3Quaternion& ornA,
+ const b3Float4& posB,
+ const b3Quaternion& ornB,
+
+ int collidableIndexA, int collidableIndexB,
+
+ const b3AlignedObjectArray<b3RigidBodyData>* bodyBuf,
+ b3AlignedObjectArray<b3Contact4Data>* globalContactOut,
+ int& nContacts,
+
+ const b3AlignedObjectArray<b3ConvexPolyhedronData>& hostConvexDataA,
+ const b3AlignedObjectArray<b3ConvexPolyhedronData>& hostConvexDataB,
+
+ const b3AlignedObjectArray<b3Vector3>& verticesA,
+ const b3AlignedObjectArray<b3Vector3>& uniqueEdgesA,
+ const b3AlignedObjectArray<b3GpuFace>& facesA,
+ const b3AlignedObjectArray<int>& indicesA,
+
+ const b3AlignedObjectArray<b3Vector3>& verticesB,
+ const b3AlignedObjectArray<b3Vector3>& uniqueEdgesB,
+ const b3AlignedObjectArray<b3GpuFace>& facesB,
+ const b3AlignedObjectArray<int>& indicesB,
+
+ const b3AlignedObjectArray<b3Collidable>& hostCollidablesA,
+ const b3AlignedObjectArray<b3Collidable>& hostCollidablesB,
+ const b3Vector3& sepNormalWorldSpace,
+ int maxContactCapacity)
{
int contactIndex = -1;
b3ConvexPolyhedronData hullA, hullB;
-
- b3Collidable colA = hostCollidablesA[collidableIndexA];
- hullA = hostConvexDataA[colA.m_shapeIndex];
- //printf("numvertsA = %d\n",hullA.m_numVertices);
-
-
- b3Collidable colB = hostCollidablesB[collidableIndexB];
- hullB = hostConvexDataB[colB.m_shapeIndex];
- //printf("numvertsB = %d\n",hullB.m_numVertices);
-
-
+
+ b3Collidable colA = hostCollidablesA[collidableIndexA];
+ hullA = hostConvexDataA[colA.m_shapeIndex];
+ //printf("numvertsA = %d\n",hullA.m_numVertices);
+
+ b3Collidable colB = hostCollidablesB[collidableIndexB];
+ hullB = hostConvexDataB[colB.m_shapeIndex];
+ //printf("numvertsB = %d\n",hullB.m_numVertices);
+
b3Float4 contactsOut[B3_MAX_VERTS];
int localContactCapacity = B3_MAX_VERTS;
@@ -321,187 +307,168 @@ inline int b3ClipHullHullSingle(
b3Assert(_finite(bodyBuf->at(bodyIndexA).m_pos.x));
b3Assert(_finite(bodyBuf->at(bodyIndexB).m_pos.x));
#endif
-
-
+
{
-
b3Float4 worldVertsB1[B3_MAX_VERTS];
b3Float4 worldVertsB2[B3_MAX_VERTS];
int capacityWorldVerts = B3_MAX_VERTS;
- b3Float4 hostNormal = b3MakeFloat4(sepNormalWorldSpace.x,sepNormalWorldSpace.y,sepNormalWorldSpace.z,0.f);
+ b3Float4 hostNormal = b3MakeFloat4(sepNormalWorldSpace.x, sepNormalWorldSpace.y, sepNormalWorldSpace.z, 0.f);
int shapeA = hostCollidablesA[collidableIndexA].m_shapeIndex;
int shapeB = hostCollidablesB[collidableIndexB].m_shapeIndex;
b3Scalar minDist = -1;
b3Scalar maxDist = 0.;
-
-
- b3Transform trA,trB;
+ b3Transform trA, trB;
{
- //B3_PROFILE("b3TransformPoint computation");
- //trA.setIdentity();
- trA.setOrigin(b3MakeVector3(posA.x,posA.y,posA.z));
- trA.setRotation(b3Quaternion(ornA.x,ornA.y,ornA.z,ornA.w));
-
- //trB.setIdentity();
- trB.setOrigin(b3MakeVector3(posB.x,posB.y,posB.z));
- trB.setRotation(b3Quaternion(ornB.x,ornB.y,ornB.z,ornB.w));
+ //B3_PROFILE("b3TransformPoint computation");
+ //trA.setIdentity();
+ trA.setOrigin(b3MakeVector3(posA.x, posA.y, posA.z));
+ trA.setRotation(b3Quaternion(ornA.x, ornA.y, ornA.z, ornA.w));
+
+ //trB.setIdentity();
+ trB.setOrigin(b3MakeVector3(posB.x, posB.y, posB.z));
+ trB.setRotation(b3Quaternion(ornB.x, ornB.y, ornB.z, ornB.w));
}
b3Quaternion trAorn = trA.getRotation();
- b3Quaternion trBorn = trB.getRotation();
-
- int numContactsOut = b3ClipHullAgainstHull(hostNormal,
- hostConvexDataA.at(shapeA),
- hostConvexDataB.at(shapeB),
- (b3Float4&)trA.getOrigin(), (b3Quaternion&)trAorn,
- (b3Float4&)trB.getOrigin(), (b3Quaternion&)trBorn,
- worldVertsB1,worldVertsB2,capacityWorldVerts,
- minDist, maxDist,
- verticesA, facesA,indicesA,
- verticesB, facesB,indicesB,
-
- contactsOut,localContactCapacity);
-
- if (numContactsOut>0)
+ b3Quaternion trBorn = trB.getRotation();
+
+ int numContactsOut = b3ClipHullAgainstHull(hostNormal,
+ hostConvexDataA.at(shapeA),
+ hostConvexDataB.at(shapeB),
+ (b3Float4&)trA.getOrigin(), (b3Quaternion&)trAorn,
+ (b3Float4&)trB.getOrigin(), (b3Quaternion&)trBorn,
+ worldVertsB1, worldVertsB2, capacityWorldVerts,
+ minDist, maxDist,
+ verticesA, facesA, indicesA,
+ verticesB, facesB, indicesB,
+
+ contactsOut, localContactCapacity);
+
+ if (numContactsOut > 0)
{
B3_PROFILE("overlap");
b3Float4 normalOnSurfaceB = (b3Float4&)hostNormal;
-// b3Float4 centerOut;
-
+ // b3Float4 centerOut;
+
b3Int4 contactIdx;
contactIdx.x = 0;
contactIdx.y = 1;
contactIdx.z = 2;
contactIdx.w = 3;
-
+
int numPoints = 0;
-
+
{
B3_PROFILE("extractManifold");
- numPoints = b3ReduceContacts(contactsOut, numContactsOut, normalOnSurfaceB, &contactIdx);
+ numPoints = b3ReduceContacts(contactsOut, numContactsOut, normalOnSurfaceB, &contactIdx);
}
-
+
b3Assert(numPoints);
-
- if (nContacts<maxContactCapacity)
+
+ if (nContacts < maxContactCapacity)
{
contactIndex = nContacts;
globalContactOut->expand();
b3Contact4Data& contact = globalContactOut->at(nContacts);
- contact.m_batchIdx = 0;//i;
- contact.m_bodyAPtrAndSignBit = (bodyBuf->at(bodyIndexA).m_invMass==0)? -bodyIndexA:bodyIndexA;
- contact.m_bodyBPtrAndSignBit = (bodyBuf->at(bodyIndexB).m_invMass==0)? -bodyIndexB:bodyIndexB;
+ contact.m_batchIdx = 0; //i;
+ contact.m_bodyAPtrAndSignBit = (bodyBuf->at(bodyIndexA).m_invMass == 0) ? -bodyIndexA : bodyIndexA;
+ contact.m_bodyBPtrAndSignBit = (bodyBuf->at(bodyIndexB).m_invMass == 0) ? -bodyIndexB : bodyIndexB;
contact.m_frictionCoeffCmp = 45874;
contact.m_restituitionCoeffCmp = 0;
-
- // float distance = 0.f;
- for (int p=0;p<numPoints;p++)
+
+ // float distance = 0.f;
+ for (int p = 0; p < numPoints; p++)
{
- contact.m_worldPosB[p] = contactsOut[contactIdx.s[p]];//check if it is actually on B
- contact.m_worldNormalOnB = normalOnSurfaceB;
+ contact.m_worldPosB[p] = contactsOut[contactIdx.s[p]]; //check if it is actually on B
+ contact.m_worldNormalOnB = normalOnSurfaceB;
}
//printf("bodyIndexA %d,bodyIndexB %d,normal=%f,%f,%f numPoints %d\n",bodyIndexA,bodyIndexB,normalOnSurfaceB.x,normalOnSurfaceB.y,normalOnSurfaceB.z,numPoints);
contact.m_worldNormalOnB.w = (b3Scalar)numPoints;
nContacts++;
- } else
+ }
+ else
{
- b3Error("Error: exceeding contact capacity (%d/%d)\n", nContacts,maxContactCapacity);
+ b3Error("Error: exceeding contact capacity (%d/%d)\n", nContacts, maxContactCapacity);
}
}
}
return contactIndex;
}
-
-
-
-
inline int b3ContactConvexConvexSAT(
- int pairIndex,
- int bodyIndexA, int bodyIndexB,
- int collidableIndexA, int collidableIndexB,
- const b3AlignedObjectArray<b3RigidBodyData>& rigidBodies,
- const b3AlignedObjectArray<b3Collidable>& collidables,
- const b3AlignedObjectArray<b3ConvexPolyhedronData>& convexShapes,
- const b3AlignedObjectArray<b3Float4>& convexVertices,
- const b3AlignedObjectArray<b3Float4>& uniqueEdges,
- const b3AlignedObjectArray<int>& convexIndices,
- const b3AlignedObjectArray<b3GpuFace>& faces,
- b3AlignedObjectArray<b3Contact4Data>& globalContactsOut,
- int& nGlobalContactsOut,
- int maxContactCapacity)
+ int pairIndex,
+ int bodyIndexA, int bodyIndexB,
+ int collidableIndexA, int collidableIndexB,
+ const b3AlignedObjectArray<b3RigidBodyData>& rigidBodies,
+ const b3AlignedObjectArray<b3Collidable>& collidables,
+ const b3AlignedObjectArray<b3ConvexPolyhedronData>& convexShapes,
+ const b3AlignedObjectArray<b3Float4>& convexVertices,
+ const b3AlignedObjectArray<b3Float4>& uniqueEdges,
+ const b3AlignedObjectArray<int>& convexIndices,
+ const b3AlignedObjectArray<b3GpuFace>& faces,
+ b3AlignedObjectArray<b3Contact4Data>& globalContactsOut,
+ int& nGlobalContactsOut,
+ int maxContactCapacity)
{
int contactIndex = -1;
-
b3Float4 posA = rigidBodies[bodyIndexA].m_pos;
b3Quaternion ornA = rigidBodies[bodyIndexA].m_quat;
b3Float4 posB = rigidBodies[bodyIndexB].m_pos;
b3Quaternion ornB = rigidBodies[bodyIndexB].m_quat;
-
b3ConvexPolyhedronData hullA, hullB;
-
- b3Float4 sepNormalWorldSpace;
-
+ b3Float4 sepNormalWorldSpace;
- b3Collidable colA = collidables[collidableIndexA];
- hullA = convexShapes[colA.m_shapeIndex];
- //printf("numvertsA = %d\n",hullA.m_numVertices);
-
-
- b3Collidable colB = collidables[collidableIndexB];
- hullB = convexShapes[colB.m_shapeIndex];
- //printf("numvertsB = %d\n",hullB.m_numVertices);
-
-
+ b3Collidable colA = collidables[collidableIndexA];
+ hullA = convexShapes[colA.m_shapeIndex];
+ //printf("numvertsA = %d\n",hullA.m_numVertices);
+ b3Collidable colB = collidables[collidableIndexB];
+ hullB = convexShapes[colB.m_shapeIndex];
+ //printf("numvertsB = %d\n",hullB.m_numVertices);
#ifdef _WIN32
b3Assert(_finite(rigidBodies[bodyIndexA].m_pos.x));
b3Assert(_finite(rigidBodies[bodyIndexB].m_pos.x));
#endif
-
- bool foundSepAxis = b3FindSeparatingAxis(hullA,hullB,
- posA,
- ornA,
- posB,
- ornB,
-
- convexVertices,uniqueEdges,faces,convexIndices,
- convexVertices,uniqueEdges,faces,convexIndices,
-
- sepNormalWorldSpace
- );
-
-
+
+ bool foundSepAxis = b3FindSeparatingAxis(hullA, hullB,
+ posA,
+ ornA,
+ posB,
+ ornB,
+
+ convexVertices, uniqueEdges, faces, convexIndices,
+ convexVertices, uniqueEdges, faces, convexIndices,
+
+ sepNormalWorldSpace);
+
if (foundSepAxis)
{
-
-
contactIndex = b3ClipHullHullSingle(
bodyIndexA, bodyIndexB,
- posA,ornA,
- posB,ornB,
+ posA, ornA,
+ posB, ornB,
collidableIndexA, collidableIndexB,
- &rigidBodies,
+ &rigidBodies,
&globalContactsOut,
nGlobalContactsOut,
-
+
convexShapes,
convexShapes,
-
- convexVertices,
- uniqueEdges,
+
+ convexVertices,
+ uniqueEdges,
faces,
convexIndices,
-
+
convexVertices,
uniqueEdges,
faces,
@@ -511,10 +478,9 @@ inline int b3ContactConvexConvexSAT(
collidables,
sepNormalWorldSpace,
maxContactCapacity);
-
}
return contactIndex;
}
-#endif //B3_CONTACT_CONVEX_CONVEX_SAT_H
+#endif //B3_CONTACT_CONVEX_CONVEX_SAT_H
generated by cgit v1.2.3 (git 2.25.1) at 2024-12-30 16:39:09 +0000