1
2 #ifndef B3_CONTACT_CONVEX_CONVEX_SAT_H
3 #define B3_CONTACT_CONVEX_CONVEX_SAT_H
4
5 #include "Bullet3Collision/NarrowPhaseCollision/shared/b3Contact4Data.h"
6 #include "Bullet3Collision/NarrowPhaseCollision/shared/b3FindSeparatingAxis.h"
7 #include "Bullet3Collision/NarrowPhaseCollision/shared/b3ReduceContacts.h"
8
9 #define B3_MAX_VERTS 1024
10
b3Lerp3(const b3Float4 & a,const b3Float4 & b,float t)11 inline b3Float4 b3Lerp3(const b3Float4& a, const b3Float4& b, float t)
12 {
13 return b3MakeVector3(a.x + (b.x - a.x) * t,
14 a.y + (b.y - a.y) * t,
15 a.z + (b.z - a.z) * t,
16 0.f);
17 }
18
19 // Clips a face to the back of a plane, return the number of vertices out, stored in ppVtxOut
b3ClipFace(const b3Float4 * pVtxIn,int numVertsIn,b3Float4 & planeNormalWS,float planeEqWS,b3Float4 * ppVtxOut)20 inline int b3ClipFace(const b3Float4* pVtxIn, int numVertsIn, b3Float4& planeNormalWS, float planeEqWS, b3Float4* ppVtxOut)
21 {
22 int ve;
23 float ds, de;
24 int numVertsOut = 0;
25 if (numVertsIn < 2)
26 return 0;
27
28 b3Float4 firstVertex = pVtxIn[numVertsIn - 1];
29 b3Float4 endVertex = pVtxIn[0];
30
31 ds = b3Dot3F4(planeNormalWS, firstVertex) + planeEqWS;
32
33 for (ve = 0; ve < numVertsIn; ve++)
34 {
35 endVertex = pVtxIn[ve];
36
37 de = b3Dot3F4(planeNormalWS, endVertex) + planeEqWS;
38
39 if (ds < 0)
40 {
41 if (de < 0)
42 {
43 // Start < 0, end < 0, so output endVertex
44 ppVtxOut[numVertsOut++] = endVertex;
45 }
46 else
47 {
48 // Start < 0, end >= 0, so output intersection
49 ppVtxOut[numVertsOut++] = b3Lerp3(firstVertex, endVertex, (ds * 1.f / (ds - de)));
50 }
51 }
52 else
53 {
54 if (de < 0)
55 {
56 // Start >= 0, end < 0 so output intersection and end
57 ppVtxOut[numVertsOut++] = b3Lerp3(firstVertex, endVertex, (ds * 1.f / (ds - de)));
58 ppVtxOut[numVertsOut++] = endVertex;
59 }
60 }
61 firstVertex = endVertex;
62 ds = de;
63 }
64 return numVertsOut;
65 }
66
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,b3Float4 * contactsOut,int contactCapacity)67 inline int b3ClipFaceAgainstHull(const b3Float4& separatingNormal, const b3ConvexPolyhedronData* hullA,
68 const b3Float4& posA, const b3Quaternion& ornA, b3Float4* worldVertsB1, int numWorldVertsB1,
69 b3Float4* worldVertsB2, int capacityWorldVertsB2,
70 const float minDist, float maxDist,
71 const b3AlignedObjectArray<b3Float4>& verticesA, const b3AlignedObjectArray<b3GpuFace>& facesA, const b3AlignedObjectArray<int>& indicesA,
72 //const b3Float4* verticesB, const b3GpuFace* facesB, const int* indicesB,
73 b3Float4* contactsOut,
74 int contactCapacity)
75 {
76 int numContactsOut = 0;
77
78 b3Float4* pVtxIn = worldVertsB1;
79 b3Float4* pVtxOut = worldVertsB2;
80
81 int numVertsIn = numWorldVertsB1;
82 int numVertsOut = 0;
83
84 int closestFaceA = -1;
85 {
86 float dmin = FLT_MAX;
87 for (int face = 0; face < hullA->m_numFaces; face++)
88 {
89 const b3Float4 Normal = b3MakeVector3(
90 facesA[hullA->m_faceOffset + face].m_plane.x,
91 facesA[hullA->m_faceOffset + face].m_plane.y,
92 facesA[hullA->m_faceOffset + face].m_plane.z, 0.f);
93 const b3Float4 faceANormalWS = b3QuatRotate(ornA, Normal);
94
95 float d = b3Dot3F4(faceANormalWS, separatingNormal);
96 if (d < dmin)
97 {
98 dmin = d;
99 closestFaceA = face;
100 }
101 }
102 }
103 if (closestFaceA < 0)
104 return numContactsOut;
105
106 b3GpuFace polyA = facesA[hullA->m_faceOffset + closestFaceA];
107
108 // clip polygon to back of planes of all faces of hull A that are adjacent to witness face
109 //int numContacts = numWorldVertsB1;
110 int numVerticesA = polyA.m_numIndices;
111 for (int e0 = 0; e0 < numVerticesA; e0++)
112 {
113 const b3Float4 a = verticesA[hullA->m_vertexOffset + indicesA[polyA.m_indexOffset + e0]];
114 const b3Float4 b = verticesA[hullA->m_vertexOffset + indicesA[polyA.m_indexOffset + ((e0 + 1) % numVerticesA)]];
115 const b3Float4 edge0 = a - b;
116 const b3Float4 WorldEdge0 = b3QuatRotate(ornA, edge0);
117 b3Float4 planeNormalA = b3MakeFloat4(polyA.m_plane.x, polyA.m_plane.y, polyA.m_plane.z, 0.f);
118 b3Float4 worldPlaneAnormal1 = b3QuatRotate(ornA, planeNormalA);
119
120 b3Float4 planeNormalWS1 = -b3Cross3(WorldEdge0, worldPlaneAnormal1);
121 b3Float4 worldA1 = b3TransformPoint(a, posA, ornA);
122 float planeEqWS1 = -b3Dot3F4(worldA1, planeNormalWS1);
123
124 b3Float4 planeNormalWS = planeNormalWS1;
125 float planeEqWS = planeEqWS1;
126
127 //clip face
128 //clipFace(*pVtxIn, *pVtxOut,planeNormalWS,planeEqWS);
129 numVertsOut = b3ClipFace(pVtxIn, numVertsIn, planeNormalWS, planeEqWS, pVtxOut);
130
131 //btSwap(pVtxIn,pVtxOut);
132 b3Float4* tmp = pVtxOut;
133 pVtxOut = pVtxIn;
134 pVtxIn = tmp;
135 numVertsIn = numVertsOut;
136 numVertsOut = 0;
137 }
138
139 // only keep points that are behind the witness face
140 {
141 b3Float4 localPlaneNormal = b3MakeFloat4(polyA.m_plane.x, polyA.m_plane.y, polyA.m_plane.z, 0.f);
142 float localPlaneEq = polyA.m_plane.w;
143 b3Float4 planeNormalWS = b3QuatRotate(ornA, localPlaneNormal);
144 float planeEqWS = localPlaneEq - b3Dot3F4(planeNormalWS, posA);
145 for (int i = 0; i < numVertsIn; i++)
146 {
147 float depth = b3Dot3F4(planeNormalWS, pVtxIn[i]) + planeEqWS;
148 if (depth <= minDist)
149 {
150 depth = minDist;
151 }
152 if (numContactsOut < contactCapacity)
153 {
154 if (depth <= maxDist)
155 {
156 b3Float4 pointInWorld = pVtxIn[i];
157 //resultOut.addContactPoint(separatingNormal,point,depth);
158 contactsOut[numContactsOut++] = b3MakeVector3(pointInWorld.x, pointInWorld.y, pointInWorld.z, depth);
159 //printf("depth=%f\n",depth);
160 }
161 }
162 else
163 {
164 b3Error("exceeding contact capacity (%d,%df)\n", numContactsOut, contactCapacity);
165 }
166 }
167 }
168
169 return numContactsOut;
170 }
171
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)172 inline int b3ClipHullAgainstHull(const b3Float4& separatingNormal,
173 const b3ConvexPolyhedronData& hullA, const b3ConvexPolyhedronData& hullB,
174 const b3Float4& posA, const b3Quaternion& ornA, const b3Float4& posB, const b3Quaternion& ornB,
175 b3Float4* worldVertsB1, b3Float4* worldVertsB2, int capacityWorldVerts,
176 const float minDist, float maxDist,
177 const b3AlignedObjectArray<b3Float4>& verticesA, const b3AlignedObjectArray<b3GpuFace>& facesA, const b3AlignedObjectArray<int>& indicesA,
178 const b3AlignedObjectArray<b3Float4>& verticesB, const b3AlignedObjectArray<b3GpuFace>& facesB, const b3AlignedObjectArray<int>& indicesB,
179
180 b3Float4* contactsOut,
181 int contactCapacity)
182 {
183 int numContactsOut = 0;
184 int numWorldVertsB1 = 0;
185
186 B3_PROFILE("clipHullAgainstHull");
187
188 //float curMaxDist=maxDist;
189 int closestFaceB = -1;
190 float dmax = -FLT_MAX;
191
192 {
193 //B3_PROFILE("closestFaceB");
194 if (hullB.m_numFaces != 1)
195 {
196 //printf("wtf\n");
197 }
198 static bool once = true;
199 //printf("separatingNormal=%f,%f,%f\n",separatingNormal.x,separatingNormal.y,separatingNormal.z);
200
201 for (int face = 0; face < hullB.m_numFaces; face++)
202 {
203 #ifdef BT_DEBUG_SAT_FACE
204 if (once)
205 printf("face %d\n", face);
206 const b3GpuFace* faceB = &facesB[hullB.m_faceOffset + face];
207 if (once)
208 {
209 for (int i = 0; i < faceB->m_numIndices; i++)
210 {
211 b3Float4 vert = verticesB[hullB.m_vertexOffset + indicesB[faceB->m_indexOffset + i]];
212 printf("vert[%d] = %f,%f,%f\n", i, vert.x, vert.y, vert.z);
213 }
214 }
215 #endif //BT_DEBUG_SAT_FACE \
216 //if (facesB[hullB.m_faceOffset+face].m_numIndices>2)
217 {
218 const b3Float4 Normal = b3MakeVector3(facesB[hullB.m_faceOffset + face].m_plane.x,
219 facesB[hullB.m_faceOffset + face].m_plane.y, facesB[hullB.m_faceOffset + face].m_plane.z, 0.f);
220 const b3Float4 WorldNormal = b3QuatRotate(ornB, Normal);
221 #ifdef BT_DEBUG_SAT_FACE
222 if (once)
223 printf("faceNormal = %f,%f,%f\n", Normal.x, Normal.y, Normal.z);
224 #endif
225 float d = b3Dot3F4(WorldNormal, separatingNormal);
226 if (d > dmax)
227 {
228 dmax = d;
229 closestFaceB = face;
230 }
231 }
232 }
233 once = false;
234 }
235
236 b3Assert(closestFaceB >= 0);
237 {
238 //B3_PROFILE("worldVertsB1");
239 const b3GpuFace& polyB = facesB[hullB.m_faceOffset + closestFaceB];
240 const int numVertices = polyB.m_numIndices;
241 for (int e0 = 0; e0 < numVertices; e0++)
242 {
243 const b3Float4& b = verticesB[hullB.m_vertexOffset + indicesB[polyB.m_indexOffset + e0]];
244 worldVertsB1[numWorldVertsB1++] = b3TransformPoint(b, posB, ornB);
245 }
246 }
247
248 if (closestFaceB >= 0)
249 {
250 //B3_PROFILE("clipFaceAgainstHull");
251 numContactsOut = b3ClipFaceAgainstHull((b3Float4&)separatingNormal, &hullA,
252 posA, ornA,
253 worldVertsB1, numWorldVertsB1, worldVertsB2, capacityWorldVerts, minDist, maxDist,
254 verticesA, facesA, indicesA,
255 contactsOut, contactCapacity);
256 }
257
258 return numContactsOut;
259 }
260
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)261 inline int b3ClipHullHullSingle(
262 int bodyIndexA, int bodyIndexB,
263 const b3Float4& posA,
264 const b3Quaternion& ornA,
265 const b3Float4& posB,
266 const b3Quaternion& ornB,
267
268 int collidableIndexA, int collidableIndexB,
269
270 const b3AlignedObjectArray<b3RigidBodyData>* bodyBuf,
271 b3AlignedObjectArray<b3Contact4Data>* globalContactOut,
272 int& nContacts,
273
274 const b3AlignedObjectArray<b3ConvexPolyhedronData>& hostConvexDataA,
275 const b3AlignedObjectArray<b3ConvexPolyhedronData>& hostConvexDataB,
276
277 const b3AlignedObjectArray<b3Vector3>& verticesA,
278 const b3AlignedObjectArray<b3Vector3>& uniqueEdgesA,
279 const b3AlignedObjectArray<b3GpuFace>& facesA,
280 const b3AlignedObjectArray<int>& indicesA,
281
282 const b3AlignedObjectArray<b3Vector3>& verticesB,
283 const b3AlignedObjectArray<b3Vector3>& uniqueEdgesB,
284 const b3AlignedObjectArray<b3GpuFace>& facesB,
285 const b3AlignedObjectArray<int>& indicesB,
286
287 const b3AlignedObjectArray<b3Collidable>& hostCollidablesA,
288 const b3AlignedObjectArray<b3Collidable>& hostCollidablesB,
289 const b3Vector3& sepNormalWorldSpace,
290 int maxContactCapacity)
291 {
292 int contactIndex = -1;
293 b3ConvexPolyhedronData hullA, hullB;
294
295 b3Collidable colA = hostCollidablesA[collidableIndexA];
296 hullA = hostConvexDataA[colA.m_shapeIndex];
297 //printf("numvertsA = %d\n",hullA.m_numVertices);
298
299 b3Collidable colB = hostCollidablesB[collidableIndexB];
300 hullB = hostConvexDataB[colB.m_shapeIndex];
301 //printf("numvertsB = %d\n",hullB.m_numVertices);
302
303 b3Float4 contactsOut[B3_MAX_VERTS];
304 int localContactCapacity = B3_MAX_VERTS;
305
306 #ifdef _WIN32
307 b3Assert(_finite(bodyBuf->at(bodyIndexA).m_pos.x));
308 b3Assert(_finite(bodyBuf->at(bodyIndexB).m_pos.x));
309 #endif
310
311 {
312 b3Float4 worldVertsB1[B3_MAX_VERTS];
313 b3Float4 worldVertsB2[B3_MAX_VERTS];
314 int capacityWorldVerts = B3_MAX_VERTS;
315
316 b3Float4 hostNormal = b3MakeFloat4(sepNormalWorldSpace.x, sepNormalWorldSpace.y, sepNormalWorldSpace.z, 0.f);
317 int shapeA = hostCollidablesA[collidableIndexA].m_shapeIndex;
318 int shapeB = hostCollidablesB[collidableIndexB].m_shapeIndex;
319
320 b3Scalar minDist = -1;
321 b3Scalar maxDist = 0.;
322
323 b3Transform trA, trB;
324 {
325 //B3_PROFILE("b3TransformPoint computation");
326 //trA.setIdentity();
327 trA.setOrigin(b3MakeVector3(posA.x, posA.y, posA.z));
328 trA.setRotation(b3Quaternion(ornA.x, ornA.y, ornA.z, ornA.w));
329
330 //trB.setIdentity();
331 trB.setOrigin(b3MakeVector3(posB.x, posB.y, posB.z));
332 trB.setRotation(b3Quaternion(ornB.x, ornB.y, ornB.z, ornB.w));
333 }
334
335 b3Quaternion trAorn = trA.getRotation();
336 b3Quaternion trBorn = trB.getRotation();
337
338 int numContactsOut = b3ClipHullAgainstHull(hostNormal,
339 hostConvexDataA.at(shapeA),
340 hostConvexDataB.at(shapeB),
341 (b3Float4&)trA.getOrigin(), (b3Quaternion&)trAorn,
342 (b3Float4&)trB.getOrigin(), (b3Quaternion&)trBorn,
343 worldVertsB1, worldVertsB2, capacityWorldVerts,
344 minDist, maxDist,
345 verticesA, facesA, indicesA,
346 verticesB, facesB, indicesB,
347
348 contactsOut, localContactCapacity);
349
350 if (numContactsOut > 0)
351 {
352 B3_PROFILE("overlap");
353
354 b3Float4 normalOnSurfaceB = (b3Float4&)hostNormal;
355 // b3Float4 centerOut;
356
357 b3Int4 contactIdx;
358 contactIdx.x = 0;
359 contactIdx.y = 1;
360 contactIdx.z = 2;
361 contactIdx.w = 3;
362
363 int numPoints = 0;
364
365 {
366 B3_PROFILE("extractManifold");
367 numPoints = b3ReduceContacts(contactsOut, numContactsOut, normalOnSurfaceB, &contactIdx);
368 }
369
370 b3Assert(numPoints);
371
372 if (nContacts < maxContactCapacity)
373 {
374 contactIndex = nContacts;
375 globalContactOut->expand();
376 b3Contact4Data& contact = globalContactOut->at(nContacts);
377 contact.m_batchIdx = 0; //i;
378 contact.m_bodyAPtrAndSignBit = (bodyBuf->at(bodyIndexA).m_invMass == 0) ? -bodyIndexA : bodyIndexA;
379 contact.m_bodyBPtrAndSignBit = (bodyBuf->at(bodyIndexB).m_invMass == 0) ? -bodyIndexB : bodyIndexB;
380
381 contact.m_frictionCoeffCmp = 45874;
382 contact.m_restituitionCoeffCmp = 0;
383
384 // float distance = 0.f;
385 for (int p = 0; p < numPoints; p++)
386 {
387 contact.m_worldPosB[p] = contactsOut[contactIdx.s[p]]; //check if it is actually on B
388 contact.m_worldNormalOnB = normalOnSurfaceB;
389 }
390 //printf("bodyIndexA %d,bodyIndexB %d,normal=%f,%f,%f numPoints %d\n",bodyIndexA,bodyIndexB,normalOnSurfaceB.x,normalOnSurfaceB.y,normalOnSurfaceB.z,numPoints);
391 contact.m_worldNormalOnB.w = (b3Scalar)numPoints;
392 nContacts++;
393 }
394 else
395 {
396 b3Error("Error: exceeding contact capacity (%d/%d)\n", nContacts, maxContactCapacity);
397 }
398 }
399 }
400 return contactIndex;
401 }
402
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)403 inline int b3ContactConvexConvexSAT(
404 int pairIndex,
405 int bodyIndexA, int bodyIndexB,
406 int collidableIndexA, int collidableIndexB,
407 const b3AlignedObjectArray<b3RigidBodyData>& rigidBodies,
408 const b3AlignedObjectArray<b3Collidable>& collidables,
409 const b3AlignedObjectArray<b3ConvexPolyhedronData>& convexShapes,
410 const b3AlignedObjectArray<b3Float4>& convexVertices,
411 const b3AlignedObjectArray<b3Float4>& uniqueEdges,
412 const b3AlignedObjectArray<int>& convexIndices,
413 const b3AlignedObjectArray<b3GpuFace>& faces,
414 b3AlignedObjectArray<b3Contact4Data>& globalContactsOut,
415 int& nGlobalContactsOut,
416 int maxContactCapacity)
417 {
418 int contactIndex = -1;
419
420 b3Float4 posA = rigidBodies[bodyIndexA].m_pos;
421 b3Quaternion ornA = rigidBodies[bodyIndexA].m_quat;
422 b3Float4 posB = rigidBodies[bodyIndexB].m_pos;
423 b3Quaternion ornB = rigidBodies[bodyIndexB].m_quat;
424
425 b3ConvexPolyhedronData hullA, hullB;
426
427 b3Float4 sepNormalWorldSpace;
428
429 b3Collidable colA = collidables[collidableIndexA];
430 hullA = convexShapes[colA.m_shapeIndex];
431 //printf("numvertsA = %d\n",hullA.m_numVertices);
432
433 b3Collidable colB = collidables[collidableIndexB];
434 hullB = convexShapes[colB.m_shapeIndex];
435 //printf("numvertsB = %d\n",hullB.m_numVertices);
436
437 #ifdef _WIN32
438 b3Assert(_finite(rigidBodies[bodyIndexA].m_pos.x));
439 b3Assert(_finite(rigidBodies[bodyIndexB].m_pos.x));
440 #endif
441
442 bool foundSepAxis = b3FindSeparatingAxis(hullA, hullB,
443 posA,
444 ornA,
445 posB,
446 ornB,
447
448 convexVertices, uniqueEdges, faces, convexIndices,
449 convexVertices, uniqueEdges, faces, convexIndices,
450
451 sepNormalWorldSpace);
452
453 if (foundSepAxis)
454 {
455 contactIndex = b3ClipHullHullSingle(
456 bodyIndexA, bodyIndexB,
457 posA, ornA,
458 posB, ornB,
459 collidableIndexA, collidableIndexB,
460 &rigidBodies,
461 &globalContactsOut,
462 nGlobalContactsOut,
463
464 convexShapes,
465 convexShapes,
466
467 convexVertices,
468 uniqueEdges,
469 faces,
470 convexIndices,
471
472 convexVertices,
473 uniqueEdges,
474 faces,
475 convexIndices,
476
477 collidables,
478 collidables,
479 sepNormalWorldSpace,
480 maxContactCapacity);
481 }
482
483 return contactIndex;
484 }
485
486 #endif //B3_CONTACT_CONVEX_CONVEX_SAT_H
487