1 #ifndef B3_FIND_CONCAVE_SEPARATING_AXIS_H
2 #define B3_FIND_CONCAVE_SEPARATING_AXIS_H
4 #define B3_TRIANGLE_NUM_CONVEX_FACES 5
6 #include "Bullet3Common/shared/b3Int4.h"
7 #include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
8 #include "Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h"
9 #include "Bullet3Collision/BroadPhaseCollision/shared/b3Aabb.h"
10 #include "Bullet3Collision/NarrowPhaseCollision/shared/b3BvhSubtreeInfoData.h"
11 #include "Bullet3Collision/NarrowPhaseCollision/shared/b3QuantizedBvhNodeData.h"
12 #include "Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h"
14 inline void b3Project(__global const b3ConvexPolyhedronData* hull, b3Float4ConstArg pos, b3QuatConstArg orn,
15 const b3Float4* dir, __global const b3Float4* vertices, float* min, float* max)
19 int numVerts = hull->m_numVertices;
21 const b3Float4 localDir = b3QuatRotate(b3QuatInverse(orn), *dir);
22 float offset = b3Dot(pos, *dir);
23 for (int i = 0; i < numVerts; i++)
25 float dp = b3Dot(vertices[hull->m_vertexOffset + i], localDir);
41 inline bool b3TestSepAxis(const b3ConvexPolyhedronData* hullA, __global const b3ConvexPolyhedronData* hullB,
42 b3Float4ConstArg posA, b3QuatConstArg ornA,
43 b3Float4ConstArg posB, b3QuatConstArg ornB,
44 b3Float4* sep_axis, const b3Float4* verticesA, __global const b3Float4* verticesB, float* depth)
48 b3Project(hullA, posA, ornA, sep_axis, verticesA, &Min0, &Max0);
49 b3Project(hullB, posB, ornB, sep_axis, verticesB, &Min1, &Max1);
51 if (Max0 < Min1 || Max1 < Min0)
54 float d0 = Max0 - Min1;
55 float d1 = Max1 - Min0;
56 *depth = d0 < d1 ? d0 : d1;
60 bool b3FindSeparatingAxis(const b3ConvexPolyhedronData* hullA, __global const b3ConvexPolyhedronData* hullB,
61 b3Float4ConstArg posA1,
63 b3Float4ConstArg posB1,
65 b3Float4ConstArg DeltaC2,
67 const b3Float4* verticesA,
68 const b3Float4* uniqueEdgesA,
69 const b3GpuFace* facesA,
72 __global const b3Float4* verticesB,
73 __global const b3Float4* uniqueEdgesB,
74 __global const b3GpuFace* facesB,
75 __global const int* indicesB,
79 b3Float4 posA = posA1;
81 b3Float4 posB = posB1;
84 static int maxFaceVertex = 0;
86 int curFaceVertexAB = hullA->m_numFaces*hullB->m_numVertices;
87 curFaceVertexAB+= hullB->m_numFaces*hullA->m_numVertices;
89 if (curFaceVertexAB>maxFaceVertex)
91 maxFaceVertex = curFaceVertexAB;
92 printf("curFaceVertexAB = %d\n",curFaceVertexAB);
93 printf("hullA->m_numFaces = %d\n",hullA->m_numFaces);
94 printf("hullA->m_numVertices = %d\n",hullA->m_numVertices);
95 printf("hullB->m_numVertices = %d\n",hullB->m_numVertices);
99 int curPlaneTests = 0;
101 int numFacesA = hullA->m_numFaces;
102 // Test normals from hullA
103 for (int i = 0; i < numFacesA; i++)
105 const b3Float4 normal = facesA[hullA->m_faceOffset + i].m_plane;
106 b3Float4 faceANormalWS = b3QuatRotate(ornA, normal);
107 if (b3Dot(DeltaC2, faceANormalWS) < 0)
108 faceANormalWS *= -1.f;
111 if (!b3TestSepAxis(hullA, hullB, posA, ornA, posB, ornB, &faceANormalWS, verticesA, verticesB, &d))
116 *sep = faceANormalWS;
120 if ((b3Dot(-DeltaC2, *sep)) > 0.0f)
127 b3Vector3 unitSphere162[] =
129 b3MakeVector3(0.000000, -1.000000, 0.000000),
130 b3MakeVector3(0.203181, -0.967950, 0.147618),
131 b3MakeVector3(-0.077607, -0.967950, 0.238853),
132 b3MakeVector3(0.723607, -0.447220, 0.525725),
133 b3MakeVector3(0.609547, -0.657519, 0.442856),
134 b3MakeVector3(0.812729, -0.502301, 0.295238),
135 b3MakeVector3(-0.251147, -0.967949, 0.000000),
136 b3MakeVector3(-0.077607, -0.967950, -0.238853),
137 b3MakeVector3(0.203181, -0.967950, -0.147618),
138 b3MakeVector3(0.860698, -0.251151, 0.442858),
139 b3MakeVector3(-0.276388, -0.447220, 0.850649),
140 b3MakeVector3(-0.029639, -0.502302, 0.864184),
141 b3MakeVector3(-0.155215, -0.251152, 0.955422),
142 b3MakeVector3(-0.894426, -0.447216, 0.000000),
143 b3MakeVector3(-0.831051, -0.502299, 0.238853),
144 b3MakeVector3(-0.956626, -0.251149, 0.147618),
145 b3MakeVector3(-0.276388, -0.447220, -0.850649),
146 b3MakeVector3(-0.483971, -0.502302, -0.716565),
147 b3MakeVector3(-0.436007, -0.251152, -0.864188),
148 b3MakeVector3(0.723607, -0.447220, -0.525725),
149 b3MakeVector3(0.531941, -0.502302, -0.681712),
150 b3MakeVector3(0.687159, -0.251152, -0.681715),
151 b3MakeVector3(0.687159, -0.251152, 0.681715),
152 b3MakeVector3(-0.436007, -0.251152, 0.864188),
153 b3MakeVector3(-0.956626, -0.251149, -0.147618),
154 b3MakeVector3(-0.155215, -0.251152, -0.955422),
155 b3MakeVector3(0.860698, -0.251151, -0.442858),
156 b3MakeVector3(0.276388, 0.447220, 0.850649),
157 b3MakeVector3(0.483971, 0.502302, 0.716565),
158 b3MakeVector3(0.232822, 0.657519, 0.716563),
159 b3MakeVector3(-0.723607, 0.447220, 0.525725),
160 b3MakeVector3(-0.531941, 0.502302, 0.681712),
161 b3MakeVector3(-0.609547, 0.657519, 0.442856),
162 b3MakeVector3(-0.723607, 0.447220, -0.525725),
163 b3MakeVector3(-0.812729, 0.502301, -0.295238),
164 b3MakeVector3(-0.609547, 0.657519, -0.442856),
165 b3MakeVector3(0.276388, 0.447220, -0.850649),
166 b3MakeVector3(0.029639, 0.502302, -0.864184),
167 b3MakeVector3(0.232822, 0.657519, -0.716563),
168 b3MakeVector3(0.894426, 0.447216, 0.000000),
169 b3MakeVector3(0.831051, 0.502299, -0.238853),
170 b3MakeVector3(0.753442, 0.657515, 0.000000),
171 b3MakeVector3(-0.232822, -0.657519, 0.716563),
172 b3MakeVector3(-0.162456, -0.850654, 0.499995),
173 b3MakeVector3(0.052790, -0.723612, 0.688185),
174 b3MakeVector3(0.138199, -0.894429, 0.425321),
175 b3MakeVector3(0.262869, -0.525738, 0.809012),
176 b3MakeVector3(0.361805, -0.723611, 0.587779),
177 b3MakeVector3(0.531941, -0.502302, 0.681712),
178 b3MakeVector3(0.425323, -0.850654, 0.309011),
179 b3MakeVector3(0.812729, -0.502301, -0.295238),
180 b3MakeVector3(0.609547, -0.657519, -0.442856),
181 b3MakeVector3(0.850648, -0.525736, 0.000000),
182 b3MakeVector3(0.670817, -0.723611, -0.162457),
183 b3MakeVector3(0.670817, -0.723610, 0.162458),
184 b3MakeVector3(0.425323, -0.850654, -0.309011),
185 b3MakeVector3(0.447211, -0.894428, 0.000001),
186 b3MakeVector3(-0.753442, -0.657515, 0.000000),
187 b3MakeVector3(-0.525730, -0.850652, 0.000000),
188 b3MakeVector3(-0.638195, -0.723609, 0.262864),
189 b3MakeVector3(-0.361801, -0.894428, 0.262864),
190 b3MakeVector3(-0.688189, -0.525736, 0.499997),
191 b3MakeVector3(-0.447211, -0.723610, 0.525729),
192 b3MakeVector3(-0.483971, -0.502302, 0.716565),
193 b3MakeVector3(-0.232822, -0.657519, -0.716563),
194 b3MakeVector3(-0.162456, -0.850654, -0.499995),
195 b3MakeVector3(-0.447211, -0.723611, -0.525727),
196 b3MakeVector3(-0.361801, -0.894429, -0.262863),
197 b3MakeVector3(-0.688189, -0.525736, -0.499997),
198 b3MakeVector3(-0.638195, -0.723609, -0.262863),
199 b3MakeVector3(-0.831051, -0.502299, -0.238853),
200 b3MakeVector3(0.361804, -0.723612, -0.587779),
201 b3MakeVector3(0.138197, -0.894429, -0.425321),
202 b3MakeVector3(0.262869, -0.525738, -0.809012),
203 b3MakeVector3(0.052789, -0.723611, -0.688186),
204 b3MakeVector3(-0.029639, -0.502302, -0.864184),
205 b3MakeVector3(0.956626, 0.251149, 0.147618),
206 b3MakeVector3(0.956626, 0.251149, -0.147618),
207 b3MakeVector3(0.951058, -0.000000, 0.309013),
208 b3MakeVector3(1.000000, 0.000000, 0.000000),
209 b3MakeVector3(0.947213, -0.276396, 0.162458),
210 b3MakeVector3(0.951058, 0.000000, -0.309013),
211 b3MakeVector3(0.947213, -0.276396, -0.162458),
212 b3MakeVector3(0.155215, 0.251152, 0.955422),
213 b3MakeVector3(0.436007, 0.251152, 0.864188),
214 b3MakeVector3(-0.000000, -0.000000, 1.000000),
215 b3MakeVector3(0.309017, 0.000000, 0.951056),
216 b3MakeVector3(0.138199, -0.276398, 0.951055),
217 b3MakeVector3(0.587786, 0.000000, 0.809017),
218 b3MakeVector3(0.447216, -0.276398, 0.850648),
219 b3MakeVector3(-0.860698, 0.251151, 0.442858),
220 b3MakeVector3(-0.687159, 0.251152, 0.681715),
221 b3MakeVector3(-0.951058, -0.000000, 0.309013),
222 b3MakeVector3(-0.809018, 0.000000, 0.587783),
223 b3MakeVector3(-0.861803, -0.276396, 0.425324),
224 b3MakeVector3(-0.587786, 0.000000, 0.809017),
225 b3MakeVector3(-0.670819, -0.276397, 0.688191),
226 b3MakeVector3(-0.687159, 0.251152, -0.681715),
227 b3MakeVector3(-0.860698, 0.251151, -0.442858),
228 b3MakeVector3(-0.587786, -0.000000, -0.809017),
229 b3MakeVector3(-0.809018, -0.000000, -0.587783),
230 b3MakeVector3(-0.670819, -0.276397, -0.688191),
231 b3MakeVector3(-0.951058, 0.000000, -0.309013),
232 b3MakeVector3(-0.861803, -0.276396, -0.425324),
233 b3MakeVector3(0.436007, 0.251152, -0.864188),
234 b3MakeVector3(0.155215, 0.251152, -0.955422),
235 b3MakeVector3(0.587786, -0.000000, -0.809017),
236 b3MakeVector3(0.309017, -0.000000, -0.951056),
237 b3MakeVector3(0.447216, -0.276398, -0.850648),
238 b3MakeVector3(0.000000, 0.000000, -1.000000),
239 b3MakeVector3(0.138199, -0.276398, -0.951055),
240 b3MakeVector3(0.670820, 0.276396, 0.688190),
241 b3MakeVector3(0.809019, -0.000002, 0.587783),
242 b3MakeVector3(0.688189, 0.525736, 0.499997),
243 b3MakeVector3(0.861804, 0.276394, 0.425323),
244 b3MakeVector3(0.831051, 0.502299, 0.238853),
245 b3MakeVector3(-0.447216, 0.276397, 0.850649),
246 b3MakeVector3(-0.309017, -0.000001, 0.951056),
247 b3MakeVector3(-0.262869, 0.525738, 0.809012),
248 b3MakeVector3(-0.138199, 0.276397, 0.951055),
249 b3MakeVector3(0.029639, 0.502302, 0.864184),
250 b3MakeVector3(-0.947213, 0.276396, -0.162458),
251 b3MakeVector3(-1.000000, 0.000001, 0.000000),
252 b3MakeVector3(-0.850648, 0.525736, -0.000000),
253 b3MakeVector3(-0.947213, 0.276397, 0.162458),
254 b3MakeVector3(-0.812729, 0.502301, 0.295238),
255 b3MakeVector3(-0.138199, 0.276397, -0.951055),
256 b3MakeVector3(-0.309016, -0.000000, -0.951057),
257 b3MakeVector3(-0.262869, 0.525738, -0.809012),
258 b3MakeVector3(-0.447215, 0.276397, -0.850649),
259 b3MakeVector3(-0.531941, 0.502302, -0.681712),
260 b3MakeVector3(0.861804, 0.276396, -0.425322),
261 b3MakeVector3(0.809019, 0.000000, -0.587782),
262 b3MakeVector3(0.688189, 0.525736, -0.499997),
263 b3MakeVector3(0.670821, 0.276397, -0.688189),
264 b3MakeVector3(0.483971, 0.502302, -0.716565),
265 b3MakeVector3(0.077607, 0.967950, 0.238853),
266 b3MakeVector3(0.251147, 0.967949, 0.000000),
267 b3MakeVector3(0.000000, 1.000000, 0.000000),
268 b3MakeVector3(0.162456, 0.850654, 0.499995),
269 b3MakeVector3(0.361800, 0.894429, 0.262863),
270 b3MakeVector3(0.447209, 0.723612, 0.525728),
271 b3MakeVector3(0.525730, 0.850652, 0.000000),
272 b3MakeVector3(0.638194, 0.723610, 0.262864),
273 b3MakeVector3(-0.203181, 0.967950, 0.147618),
274 b3MakeVector3(-0.425323, 0.850654, 0.309011),
275 b3MakeVector3(-0.138197, 0.894430, 0.425320),
276 b3MakeVector3(-0.361804, 0.723612, 0.587778),
277 b3MakeVector3(-0.052790, 0.723612, 0.688185),
278 b3MakeVector3(-0.203181, 0.967950, -0.147618),
279 b3MakeVector3(-0.425323, 0.850654, -0.309011),
280 b3MakeVector3(-0.447210, 0.894429, 0.000000),
281 b3MakeVector3(-0.670817, 0.723611, -0.162457),
282 b3MakeVector3(-0.670817, 0.723611, 0.162457),
283 b3MakeVector3(0.077607, 0.967950, -0.238853),
284 b3MakeVector3(0.162456, 0.850654, -0.499995),
285 b3MakeVector3(-0.138197, 0.894430, -0.425320),
286 b3MakeVector3(-0.052790, 0.723612, -0.688185),
287 b3MakeVector3(-0.361804, 0.723612, -0.587778),
288 b3MakeVector3(0.361800, 0.894429, -0.262863),
289 b3MakeVector3(0.638194, 0.723610, -0.262864),
290 b3MakeVector3(0.447209, 0.723612, -0.525728)};
292 bool b3FindSeparatingAxisEdgeEdge(const b3ConvexPolyhedronData* hullA, __global const b3ConvexPolyhedronData* hullB,
293 b3Float4ConstArg posA1,
295 b3Float4ConstArg posB1,
297 b3Float4ConstArg DeltaC2,
298 const b3Float4* verticesA,
299 const b3Float4* uniqueEdgesA,
300 const b3GpuFace* facesA,
302 __global const b3Float4* verticesB,
303 __global const b3Float4* uniqueEdgesB,
304 __global const b3GpuFace* facesB,
305 __global const int* indicesB,
308 bool searchAllEdgeEdge)
310 b3Float4 posA = posA1;
312 b3Float4 posB = posB1;
315 // int curPlaneTests=0;
319 static int maxEdgeTests = 0;
320 int curEdgeTests = hullA->m_numUniqueEdges * hullB->m_numUniqueEdges;
321 if (curEdgeTests > maxEdgeTests)
323 maxEdgeTests = curEdgeTests;
324 printf("maxEdgeTests = %d\n", maxEdgeTests);
325 printf("hullA->m_numUniqueEdges = %d\n", hullA->m_numUniqueEdges);
326 printf("hullB->m_numUniqueEdges = %d\n", hullB->m_numUniqueEdges);
329 if (searchAllEdgeEdge)
331 for (int e0 = 0; e0 < hullA->m_numUniqueEdges; e0++)
333 const b3Float4 edge0 = uniqueEdgesA[hullA->m_uniqueEdgesOffset + e0];
334 b3Float4 edge0World = b3QuatRotate(ornA, edge0);
336 for (int e1 = 0; e1 < hullB->m_numUniqueEdges; e1++)
338 const b3Float4 edge1 = uniqueEdgesB[hullB->m_uniqueEdgesOffset + e1];
339 b3Float4 edge1World = b3QuatRotate(ornB, edge1);
341 b3Float4 crossje = b3Cross(edge0World, edge1World);
344 if (!b3IsAlmostZero(crossje))
346 crossje = b3Normalized(crossje);
347 if (b3Dot(DeltaC2, crossje) < 0)
355 b3Project(hullA, posA, ornA, &crossje, verticesA, &Min0, &Max0);
356 b3Project(hullB, posB, ornB, &crossje, verticesB, &Min1, &Max1);
358 if (Max0 < Min1 || Max1 < Min0)
361 float d0 = Max0 - Min1;
362 float d1 = Max1 - Min0;
363 dist = d0 < d1 ? d0 : d1;
378 int numDirections = sizeof(unitSphere162) / sizeof(b3Vector3);
379 //printf("numDirections =%d\n",numDirections );
381 for (int i = 0; i < numDirections; i++)
383 b3Float4 crossje = unitSphere162[i];
385 //if (b3Dot(DeltaC2,crossje)>0)
392 b3Project(hullA, posA, ornA, &crossje, verticesA, &Min0, &Max0);
393 b3Project(hullB, posB, ornB, &crossje, verticesB, &Min1, &Max1);
395 if (Max0 < Min1 || Max1 < Min0)
398 float d0 = Max0 - Min1;
399 float d1 = Max1 - Min0;
400 dist = d0 < d1 ? d0 : d1;
414 if ((b3Dot(-DeltaC2, *sep)) > 0.0f)
421 inline int b3FindClippingFaces(b3Float4ConstArg separatingNormal,
422 __global const b3ConvexPolyhedronData_t* hullA, __global const b3ConvexPolyhedronData_t* hullB,
423 b3Float4ConstArg posA, b3QuatConstArg ornA, b3Float4ConstArg posB, b3QuatConstArg ornB,
424 __global b3Float4* worldVertsA1,
425 __global b3Float4* worldNormalsA1,
426 __global b3Float4* worldVertsB1,
427 int capacityWorldVerts,
428 const float minDist, float maxDist,
429 __global const b3Float4* verticesA,
430 __global const b3GpuFace_t* facesA,
431 __global const int* indicesA,
432 __global const b3Float4* verticesB,
433 __global const b3GpuFace_t* facesB,
434 __global const int* indicesB,
436 __global b3Int4* clippingFaces, int pairIndex)
438 int numContactsOut = 0;
439 int numWorldVertsB1 = 0;
441 int closestFaceB = -1;
442 float dmax = -FLT_MAX;
445 for (int face = 0; face < hullB->m_numFaces; face++)
447 const b3Float4 Normal = b3MakeFloat4(facesB[hullB->m_faceOffset + face].m_plane.x,
448 facesB[hullB->m_faceOffset + face].m_plane.y, facesB[hullB->m_faceOffset + face].m_plane.z, 0.f);
449 const b3Float4 WorldNormal = b3QuatRotate(ornB, Normal);
450 float d = b3Dot(WorldNormal, separatingNormal);
460 const b3GpuFace_t polyB = facesB[hullB->m_faceOffset + closestFaceB];
461 const int numVertices = polyB.m_numIndices;
462 for (int e0 = 0; e0 < numVertices; e0++)
464 const b3Float4 b = verticesB[hullB->m_vertexOffset + indicesB[polyB.m_indexOffset + e0]];
465 worldVertsB1[pairIndex * capacityWorldVerts + numWorldVertsB1++] = b3TransformPoint(b, posB, ornB);
469 int closestFaceA = -1;
471 float dmin = FLT_MAX;
472 for (int face = 0; face < hullA->m_numFaces; face++)
474 const b3Float4 Normal = b3MakeFloat4(
475 facesA[hullA->m_faceOffset + face].m_plane.x,
476 facesA[hullA->m_faceOffset + face].m_plane.y,
477 facesA[hullA->m_faceOffset + face].m_plane.z,
479 const b3Float4 faceANormalWS = b3QuatRotate(ornA, Normal);
481 float d = b3Dot(faceANormalWS, separatingNormal);
486 worldNormalsA1[pairIndex] = faceANormalWS;
491 int numVerticesA = facesA[hullA->m_faceOffset + closestFaceA].m_numIndices;
492 for (int e0 = 0; e0 < numVerticesA; e0++)
494 const b3Float4 a = verticesA[hullA->m_vertexOffset + indicesA[facesA[hullA->m_faceOffset + closestFaceA].m_indexOffset + e0]];
495 worldVertsA1[pairIndex * capacityWorldVerts + e0] = b3TransformPoint(a, posA, ornA);
498 clippingFaces[pairIndex].x = closestFaceA;
499 clippingFaces[pairIndex].y = closestFaceB;
500 clippingFaces[pairIndex].z = numVerticesA;
501 clippingFaces[pairIndex].w = numWorldVertsB1;
503 return numContactsOut;
506 __kernel void b3FindConcaveSeparatingAxisKernel(__global b3Int4* concavePairs,
507 __global const b3RigidBodyData* rigidBodies,
508 __global const b3Collidable* collidables,
509 __global const b3ConvexPolyhedronData* convexShapes,
510 __global const b3Float4* vertices,
511 __global const b3Float4* uniqueEdges,
512 __global const b3GpuFace* faces,
513 __global const int* indices,
514 __global const b3GpuChildShape* gpuChildShapes,
515 __global b3Aabb* aabbs,
516 __global b3Float4* concaveSeparatingNormalsOut,
517 __global b3Int4* clippingFacesOut,
518 __global b3Vector3* worldVertsA1Out,
519 __global b3Vector3* worldNormalsA1Out,
520 __global b3Vector3* worldVertsB1Out,
521 __global int* hasSeparatingNormals,
522 int vertexFaceCapacity,
527 /* int i = get_global_id(0);
528 if (i>=numConcavePairs)
533 int bodyIndexA = concavePairs[i].x;
534 int bodyIndexB = concavePairs[i].y;
536 int collidableIndexA = rigidBodies[bodyIndexA].m_collidableIdx;
537 int collidableIndexB = rigidBodies[bodyIndexB].m_collidableIdx;
539 int shapeIndexA = collidables[collidableIndexA].m_shapeIndex;
540 int shapeIndexB = collidables[collidableIndexB].m_shapeIndex;
542 if (collidables[collidableIndexB].m_shapeType != SHAPE_CONVEX_HULL &&
543 collidables[collidableIndexB].m_shapeType != SHAPE_COMPOUND_OF_CONVEX_HULLS)
545 concavePairs[pairIdx].w = -1;
549 hasSeparatingNormals[i] = 0;
551 // int numFacesA = convexShapes[shapeIndexA].m_numFaces;
552 int numActualConcaveConvexTests = 0;
554 int f = concavePairs[i].z;
556 bool overlap = false;
558 b3ConvexPolyhedronData convexPolyhedronA;
560 //add 3 vertices of the triangle
561 convexPolyhedronA.m_numVertices = 3;
562 convexPolyhedronA.m_vertexOffset = 0;
563 b3Float4 localCenter = b3MakeFloat4(0.f, 0.f, 0.f, 0.f);
565 b3GpuFace face = faces[convexShapes[shapeIndexA].m_faceOffset + f];
567 triAabb.m_minVec = b3MakeFloat4(1e30f, 1e30f, 1e30f, 0.f);
568 triAabb.m_maxVec = b3MakeFloat4(-1e30f, -1e30f, -1e30f, 0.f);
570 b3Float4 verticesA[3];
571 for (int i = 0; i < 3; i++)
573 int index = indices[face.m_indexOffset + i];
574 b3Float4 vert = vertices[convexShapes[shapeIndexA].m_vertexOffset + index];
578 triAabb.m_minVec = b3MinFloat4(triAabb.m_minVec, vert);
579 triAabb.m_maxVec = b3MaxFloat4(triAabb.m_maxVec, vert);
583 overlap = (triAabb.m_minVec.x > aabbs[bodyIndexB].m_maxVec.x || triAabb.m_maxVec.x < aabbs[bodyIndexB].m_minVec.x) ? false : overlap;
584 overlap = (triAabb.m_minVec.z > aabbs[bodyIndexB].m_maxVec.z || triAabb.m_maxVec.z < aabbs[bodyIndexB].m_minVec.z) ? false : overlap;
585 overlap = (triAabb.m_minVec.y > aabbs[bodyIndexB].m_maxVec.y || triAabb.m_maxVec.y < aabbs[bodyIndexB].m_minVec.y) ? false : overlap;
589 float dmin = FLT_MAX;
590 int hasSeparatingAxis = 5;
591 b3Float4 sepAxis = b3MakeFloat4(1, 2, 3, 4);
594 numActualConcaveConvexTests++;
596 //a triangle has 3 unique edges
597 convexPolyhedronA.m_numUniqueEdges = 3;
598 convexPolyhedronA.m_uniqueEdgesOffset = 0;
599 b3Float4 uniqueEdgesA[3];
601 uniqueEdgesA[0] = (verticesA[1] - verticesA[0]);
602 uniqueEdgesA[1] = (verticesA[2] - verticesA[1]);
603 uniqueEdgesA[2] = (verticesA[0] - verticesA[2]);
605 convexPolyhedronA.m_faceOffset = 0;
607 b3Float4 normal = b3MakeFloat4(face.m_plane.x, face.m_plane.y, face.m_plane.z, 0.f);
609 b3GpuFace facesA[B3_TRIANGLE_NUM_CONVEX_FACES];
610 int indicesA[3 + 3 + 2 + 2 + 2];
611 int curUsedIndices = 0;
614 //front size of triangle
616 facesA[fidx].m_indexOffset = curUsedIndices;
621 float c = face.m_plane.w;
622 facesA[fidx].m_plane.x = normal.x;
623 facesA[fidx].m_plane.y = normal.y;
624 facesA[fidx].m_plane.z = normal.z;
625 facesA[fidx].m_plane.w = c;
626 facesA[fidx].m_numIndices = 3;
629 //back size of triangle
631 facesA[fidx].m_indexOffset = curUsedIndices;
636 float c = b3Dot(normal, verticesA[0]);
637 // float c1 = -face.m_plane.w;
638 facesA[fidx].m_plane.x = -normal.x;
639 facesA[fidx].m_plane.y = -normal.y;
640 facesA[fidx].m_plane.z = -normal.z;
641 facesA[fidx].m_plane.w = c;
642 facesA[fidx].m_numIndices = 3;
646 bool addEdgePlanes = true;
650 int prevVertex = numVertices - 1;
651 for (int i = 0; i < numVertices; i++)
653 b3Float4 v0 = verticesA[i];
654 b3Float4 v1 = verticesA[prevVertex];
656 b3Float4 edgeNormal = b3Normalized(b3Cross(normal, v1 - v0));
657 float c = -b3Dot(edgeNormal, v0);
659 facesA[fidx].m_numIndices = 2;
660 facesA[fidx].m_indexOffset = curUsedIndices;
661 indicesA[curUsedIndices++] = i;
662 indicesA[curUsedIndices++] = prevVertex;
664 facesA[fidx].m_plane.x = edgeNormal.x;
665 facesA[fidx].m_plane.y = edgeNormal.y;
666 facesA[fidx].m_plane.z = edgeNormal.z;
667 facesA[fidx].m_plane.w = c;
672 convexPolyhedronA.m_numFaces = B3_TRIANGLE_NUM_CONVEX_FACES;
673 convexPolyhedronA.m_localCenter = localCenter * (1.f / 3.f);
675 b3Float4 posA = rigidBodies[bodyIndexA].m_pos;
677 b3Float4 posB = rigidBodies[bodyIndexB].m_pos;
680 b3Quaternion ornA = rigidBodies[bodyIndexA].m_quat;
681 b3Quaternion ornB = rigidBodies[bodyIndexB].m_quat;
684 ///compound shape support
686 if (collidables[collidableIndexB].m_shapeType == SHAPE_COMPOUND_OF_CONVEX_HULLS)
688 int compoundChild = concavePairs[pairIdx].w;
689 int childShapeIndexB = compoundChild; //collidables[collidableIndexB].m_shapeIndex+compoundChild;
690 int childColIndexB = gpuChildShapes[childShapeIndexB].m_shapeIndex;
691 b3Float4 childPosB = gpuChildShapes[childShapeIndexB].m_childPosition;
692 b3Quaternion childOrnB = gpuChildShapes[childShapeIndexB].m_childOrientation;
693 b3Float4 newPosB = b3TransformPoint(childPosB, posB, ornB);
694 b3Quaternion newOrnB = b3QuatMul(ornB, childOrnB);
697 shapeIndexB = collidables[childColIndexB].m_shapeIndex;
701 b3Float4 c0local = convexPolyhedronA.m_localCenter;
702 b3Float4 c0 = b3TransformPoint(c0local, posA, ornA);
703 b3Float4 c1local = convexShapes[shapeIndexB].m_localCenter;
704 b3Float4 c1 = b3TransformPoint(c1local, posB, ornB);
705 const b3Float4 DeltaC2 = c0 - c1;
707 bool sepA = b3FindSeparatingAxis(&convexPolyhedronA, &convexShapes[shapeIndexB],
711 verticesA, uniqueEdgesA, facesA, indicesA,
712 vertices, uniqueEdges, faces, indices,
714 hasSeparatingAxis = 4;
717 hasSeparatingAxis = 0;
721 bool sepB = b3FindSeparatingAxis(&convexShapes[shapeIndexB], &convexPolyhedronA,
725 vertices, uniqueEdges, faces, indices,
726 verticesA, uniqueEdgesA, facesA, indicesA,
731 hasSeparatingAxis = 0;
735 bool sepEE = b3FindSeparatingAxisEdgeEdge(&convexPolyhedronA, &convexShapes[shapeIndexB],
739 verticesA, uniqueEdgesA, facesA, indicesA,
740 vertices, uniqueEdges, faces, indices,
741 &sepAxis, &dmin, true);
745 hasSeparatingAxis = 0;
749 hasSeparatingAxis = 1;
754 if (hasSeparatingAxis)
756 hasSeparatingNormals[i] = 1;
758 concaveSeparatingNormalsOut[pairIdx] = sepAxis;
760 //now compute clipping faces A and B, and world-space clipping vertices A and B...
762 float minDist = -1e30f;
763 float maxDist = 0.02f;
765 b3FindClippingFaces(sepAxis,
767 &convexShapes[shapeIndexB],
782 clippingFacesOut, pairIdx);
786 //mark this pair as in-active
787 concavePairs[pairIdx].w = -1;
792 //mark this pair as in-active
793 concavePairs[pairIdx].w = -1;
797 #endif //B3_FIND_CONCAVE_SEPARATING_AXIS_H