1 #include "btInternalEdgeUtility.h"
2
3 #include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h"
4 #include "BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h"
5
6 #include "BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.h"
7 #include "BulletCollision/CollisionShapes/btTriangleShape.h"
8 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
9 #include "BulletCollision/NarrowPhaseCollision/btManifoldPoint.h"
10 #include "LinearMath/btIDebugDraw.h"
11 #include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h"
12
13 //#define DEBUG_INTERNAL_EDGE
14
15 #ifdef DEBUG_INTERNAL_EDGE
16 #include <stdio.h>
17 #endif //DEBUG_INTERNAL_EDGE
18
19 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
20 static btIDebugDraw* gDebugDrawer = 0;
21
btSetDebugDrawer(btIDebugDraw * debugDrawer)22 void btSetDebugDrawer(btIDebugDraw* debugDrawer)
23 {
24 gDebugDrawer = debugDrawer;
25 }
26
btDebugDrawLine(const btVector3 & from,const btVector3 & to,const btVector3 & color)27 static void btDebugDrawLine(const btVector3& from, const btVector3& to, const btVector3& color)
28 {
29 if (gDebugDrawer)
30 gDebugDrawer->drawLine(from, to, color);
31 }
32 #endif //BT_INTERNAL_EDGE_DEBUG_DRAW
33
btGetHash(int partId,int triangleIndex)34 static int btGetHash(int partId, int triangleIndex)
35 {
36 int hash = (partId << (31 - MAX_NUM_PARTS_IN_BITS)) | triangleIndex;
37 return hash;
38 }
39
btGetAngle(const btVector3 & edgeA,const btVector3 & normalA,const btVector3 & normalB)40 static btScalar btGetAngle(const btVector3& edgeA, const btVector3& normalA, const btVector3& normalB)
41 {
42 const btVector3 refAxis0 = edgeA;
43 const btVector3 refAxis1 = normalA;
44 const btVector3 swingAxis = normalB;
45 btScalar angle = btAtan2(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1));
46 return angle;
47 }
48
49 struct btConnectivityProcessor : public btTriangleCallback
50 {
51 int m_partIdA;
52 int m_triangleIndexA;
53 btVector3* m_triangleVerticesA;
54 btTriangleInfoMap* m_triangleInfoMap;
55
processTrianglebtConnectivityProcessor56 virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex)
57 {
58 //skip self-collisions
59 if ((m_partIdA == partId) && (m_triangleIndexA == triangleIndex))
60 return;
61
62 //skip duplicates (disabled for now)
63 //if ((m_partIdA <= partId) && (m_triangleIndexA <= triangleIndex))
64 // return;
65
66 //search for shared vertices and edges
67 int numshared = 0;
68 int sharedVertsA[3] = {-1, -1, -1};
69 int sharedVertsB[3] = {-1, -1, -1};
70
71 ///skip degenerate triangles
72 btScalar crossBSqr = ((triangle[1] - triangle[0]).cross(triangle[2] - triangle[0])).length2();
73 if (crossBSqr < m_triangleInfoMap->m_equalVertexThreshold)
74 return;
75
76 btScalar crossASqr = ((m_triangleVerticesA[1] - m_triangleVerticesA[0]).cross(m_triangleVerticesA[2] - m_triangleVerticesA[0])).length2();
77 ///skip degenerate triangles
78 if (crossASqr < m_triangleInfoMap->m_equalVertexThreshold)
79 return;
80
81 #if 0
82 printf("triangle A[0] = (%f,%f,%f)\ntriangle A[1] = (%f,%f,%f)\ntriangle A[2] = (%f,%f,%f)\n",
83 m_triangleVerticesA[0].getX(),m_triangleVerticesA[0].getY(),m_triangleVerticesA[0].getZ(),
84 m_triangleVerticesA[1].getX(),m_triangleVerticesA[1].getY(),m_triangleVerticesA[1].getZ(),
85 m_triangleVerticesA[2].getX(),m_triangleVerticesA[2].getY(),m_triangleVerticesA[2].getZ());
86
87 printf("partId=%d, triangleIndex=%d\n",partId,triangleIndex);
88 printf("triangle B[0] = (%f,%f,%f)\ntriangle B[1] = (%f,%f,%f)\ntriangle B[2] = (%f,%f,%f)\n",
89 triangle[0].getX(),triangle[0].getY(),triangle[0].getZ(),
90 triangle[1].getX(),triangle[1].getY(),triangle[1].getZ(),
91 triangle[2].getX(),triangle[2].getY(),triangle[2].getZ());
92 #endif
93
94 for (int i = 0; i < 3; i++)
95 {
96 for (int j = 0; j < 3; j++)
97 {
98 if ((m_triangleVerticesA[i] - triangle[j]).length2() < m_triangleInfoMap->m_equalVertexThreshold)
99 {
100 sharedVertsA[numshared] = i;
101 sharedVertsB[numshared] = j;
102 numshared++;
103 ///degenerate case
104 if (numshared >= 3)
105 return;
106 }
107 }
108 ///degenerate case
109 if (numshared >= 3)
110 return;
111 }
112 switch (numshared)
113 {
114 case 0:
115 {
116 break;
117 }
118 case 1:
119 {
120 //shared vertex
121 break;
122 }
123 case 2:
124 {
125 //shared edge
126 //we need to make sure the edge is in the order V2V0 and not V0V2 so that the signs are correct
127 if (sharedVertsA[0] == 0 && sharedVertsA[1] == 2)
128 {
129 sharedVertsA[0] = 2;
130 sharedVertsA[1] = 0;
131 int tmp = sharedVertsB[1];
132 sharedVertsB[1] = sharedVertsB[0];
133 sharedVertsB[0] = tmp;
134 }
135
136 int hash = btGetHash(m_partIdA, m_triangleIndexA);
137
138 btTriangleInfo* info = m_triangleInfoMap->find(hash);
139 if (!info)
140 {
141 btTriangleInfo tmp;
142 m_triangleInfoMap->insert(hash, tmp);
143 info = m_triangleInfoMap->find(hash);
144 }
145
146 int sumvertsA = sharedVertsA[0] + sharedVertsA[1];
147 int otherIndexA = 3 - sumvertsA;
148
149 btVector3 edge(m_triangleVerticesA[sharedVertsA[1]] - m_triangleVerticesA[sharedVertsA[0]]);
150
151 btTriangleShape tA(m_triangleVerticesA[0], m_triangleVerticesA[1], m_triangleVerticesA[2]);
152 int otherIndexB = 3 - (sharedVertsB[0] + sharedVertsB[1]);
153
154 btTriangleShape tB(triangle[sharedVertsB[1]], triangle[sharedVertsB[0]], triangle[otherIndexB]);
155 //btTriangleShape tB(triangle[0],triangle[1],triangle[2]);
156
157 btVector3 normalA;
158 btVector3 normalB;
159 tA.calcNormal(normalA);
160 tB.calcNormal(normalB);
161 edge.normalize();
162 btVector3 edgeCrossA = edge.cross(normalA).normalize();
163
164 {
165 btVector3 tmp = m_triangleVerticesA[otherIndexA] - m_triangleVerticesA[sharedVertsA[0]];
166 if (edgeCrossA.dot(tmp) < 0)
167 {
168 edgeCrossA *= -1;
169 }
170 }
171
172 btVector3 edgeCrossB = edge.cross(normalB).normalize();
173
174 {
175 btVector3 tmp = triangle[otherIndexB] - triangle[sharedVertsB[0]];
176 if (edgeCrossB.dot(tmp) < 0)
177 {
178 edgeCrossB *= -1;
179 }
180 }
181
182 btScalar angle2 = 0;
183 btScalar ang4 = 0.f;
184
185 btVector3 calculatedEdge = edgeCrossA.cross(edgeCrossB);
186 btScalar len2 = calculatedEdge.length2();
187
188 btScalar correctedAngle(0);
189 //btVector3 calculatedNormalB = normalA;
190 bool isConvex = false;
191
192 if (len2 < m_triangleInfoMap->m_planarEpsilon)
193 {
194 angle2 = 0.f;
195 ang4 = 0.f;
196 }
197 else
198 {
199 calculatedEdge.normalize();
200 btVector3 calculatedNormalA = calculatedEdge.cross(edgeCrossA);
201 calculatedNormalA.normalize();
202 angle2 = btGetAngle(calculatedNormalA, edgeCrossA, edgeCrossB);
203 ang4 = SIMD_PI - angle2;
204 btScalar dotA = normalA.dot(edgeCrossB);
205 ///@todo: check if we need some epsilon, due to floating point imprecision
206 isConvex = (dotA < 0.);
207
208 correctedAngle = isConvex ? ang4 : -ang4;
209 }
210
211 //alternatively use
212 //btVector3 calculatedNormalB2 = quatRotate(orn,normalA);
213
214 switch (sumvertsA)
215 {
216 case 1:
217 {
218 btVector3 edge = m_triangleVerticesA[0] - m_triangleVerticesA[1];
219 btQuaternion orn(edge, -correctedAngle);
220 btVector3 computedNormalB = quatRotate(orn, normalA);
221 btScalar bla = computedNormalB.dot(normalB);
222 if (bla < 0)
223 {
224 computedNormalB *= -1;
225 info->m_flags |= TRI_INFO_V0V1_SWAP_NORMALB;
226 }
227 #ifdef DEBUG_INTERNAL_EDGE
228 if ((computedNormalB - normalB).length() > 0.0001)
229 {
230 printf("warning: normals not identical\n");
231 }
232 #endif //DEBUG_INTERNAL_EDGE
233
234 info->m_edgeV0V1Angle = -correctedAngle;
235
236 if (isConvex)
237 info->m_flags |= TRI_INFO_V0V1_CONVEX;
238 break;
239 }
240 case 2:
241 {
242 btVector3 edge = m_triangleVerticesA[2] - m_triangleVerticesA[0];
243 btQuaternion orn(edge, -correctedAngle);
244 btVector3 computedNormalB = quatRotate(orn, normalA);
245 if (computedNormalB.dot(normalB) < 0)
246 {
247 computedNormalB *= -1;
248 info->m_flags |= TRI_INFO_V2V0_SWAP_NORMALB;
249 }
250
251 #ifdef DEBUG_INTERNAL_EDGE
252 if ((computedNormalB - normalB).length() > 0.0001)
253 {
254 printf("warning: normals not identical\n");
255 }
256 #endif //DEBUG_INTERNAL_EDGE
257 info->m_edgeV2V0Angle = -correctedAngle;
258 if (isConvex)
259 info->m_flags |= TRI_INFO_V2V0_CONVEX;
260 break;
261 }
262 case 3:
263 {
264 btVector3 edge = m_triangleVerticesA[1] - m_triangleVerticesA[2];
265 btQuaternion orn(edge, -correctedAngle);
266 btVector3 computedNormalB = quatRotate(orn, normalA);
267 if (computedNormalB.dot(normalB) < 0)
268 {
269 info->m_flags |= TRI_INFO_V1V2_SWAP_NORMALB;
270 computedNormalB *= -1;
271 }
272 #ifdef DEBUG_INTERNAL_EDGE
273 if ((computedNormalB - normalB).length() > 0.0001)
274 {
275 printf("warning: normals not identical\n");
276 }
277 #endif //DEBUG_INTERNAL_EDGE
278 info->m_edgeV1V2Angle = -correctedAngle;
279
280 if (isConvex)
281 info->m_flags |= TRI_INFO_V1V2_CONVEX;
282 break;
283 }
284 }
285
286 break;
287 }
288 default:
289 {
290 // printf("warning: duplicate triangle\n");
291 }
292 }
293 }
294 };
295
296
297 struct b3ProcessAllTrianglesHeightfield: public btTriangleCallback
298 {
299 btHeightfieldTerrainShape* m_heightfieldShape;
300 btTriangleInfoMap* m_triangleInfoMap;
301
302
b3ProcessAllTrianglesHeightfieldb3ProcessAllTrianglesHeightfield303 b3ProcessAllTrianglesHeightfield(btHeightfieldTerrainShape* heightFieldShape, btTriangleInfoMap* triangleInfoMap)
304 :m_heightfieldShape(heightFieldShape),
305 m_triangleInfoMap(triangleInfoMap)
306 {
307 }
processTriangleb3ProcessAllTrianglesHeightfield308 virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex)
309 {
310 btConnectivityProcessor connectivityProcessor;
311 connectivityProcessor.m_partIdA = partId;
312 connectivityProcessor.m_triangleIndexA = triangleIndex;
313 connectivityProcessor.m_triangleVerticesA = triangle;
314 connectivityProcessor.m_triangleInfoMap = m_triangleInfoMap;
315 btVector3 aabbMin, aabbMax;
316 aabbMin.setValue(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT));
317 aabbMax.setValue(btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT));
318 aabbMin.setMin(triangle[0]);
319 aabbMax.setMax(triangle[0]);
320 aabbMin.setMin(triangle[1]);
321 aabbMax.setMax(triangle[1]);
322 aabbMin.setMin(triangle[2]);
323 aabbMax.setMax(triangle[2]);
324
325 m_heightfieldShape->processAllTriangles(&connectivityProcessor, aabbMin, aabbMax);
326 }
327 };
328 /////////////////////////////////////////////////////////
329 /////////////////////////////////////////////////////////
330
btGenerateInternalEdgeInfo(btBvhTriangleMeshShape * trimeshShape,btTriangleInfoMap * triangleInfoMap)331 void btGenerateInternalEdgeInfo(btBvhTriangleMeshShape* trimeshShape, btTriangleInfoMap* triangleInfoMap)
332 {
333 //the user pointer shouldn't already be used for other purposes, we intend to store connectivity info there!
334 if (trimeshShape->getTriangleInfoMap())
335 return;
336
337 trimeshShape->setTriangleInfoMap(triangleInfoMap);
338
339 btStridingMeshInterface* meshInterface = trimeshShape->getMeshInterface();
340 const btVector3& meshScaling = meshInterface->getScaling();
341
342 for (int partId = 0; partId < meshInterface->getNumSubParts(); partId++)
343 {
344 const unsigned char* vertexbase = 0;
345 int numverts = 0;
346 PHY_ScalarType type = PHY_INTEGER;
347 int stride = 0;
348 const unsigned char* indexbase = 0;
349 int indexstride = 0;
350 int numfaces = 0;
351 PHY_ScalarType indicestype = PHY_INTEGER;
352 //PHY_ScalarType indexType=0;
353
354 btVector3 triangleVerts[3];
355 meshInterface->getLockedReadOnlyVertexIndexBase(&vertexbase, numverts, type, stride, &indexbase, indexstride, numfaces, indicestype, partId);
356 btVector3 aabbMin, aabbMax;
357
358 for (int triangleIndex = 0; triangleIndex < numfaces; triangleIndex++)
359 {
360 unsigned int* gfxbase = (unsigned int*)(indexbase + triangleIndex * indexstride);
361
362 for (int j = 2; j >= 0; j--)
363 {
364 int graphicsindex = indicestype == PHY_SHORT ? ((unsigned short*)gfxbase)[j] : gfxbase[j];
365 if (type == PHY_FLOAT)
366 {
367 float* graphicsbase = (float*)(vertexbase + graphicsindex * stride);
368 triangleVerts[j] = btVector3(
369 graphicsbase[0] * meshScaling.getX(),
370 graphicsbase[1] * meshScaling.getY(),
371 graphicsbase[2] * meshScaling.getZ());
372 }
373 else
374 {
375 double* graphicsbase = (double*)(vertexbase + graphicsindex * stride);
376 triangleVerts[j] = btVector3(btScalar(graphicsbase[0] * meshScaling.getX()), btScalar(graphicsbase[1] * meshScaling.getY()), btScalar(graphicsbase[2] * meshScaling.getZ()));
377 }
378 }
379 aabbMin.setValue(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT));
380 aabbMax.setValue(btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT));
381 aabbMin.setMin(triangleVerts[0]);
382 aabbMax.setMax(triangleVerts[0]);
383 aabbMin.setMin(triangleVerts[1]);
384 aabbMax.setMax(triangleVerts[1]);
385 aabbMin.setMin(triangleVerts[2]);
386 aabbMax.setMax(triangleVerts[2]);
387
388 btConnectivityProcessor connectivityProcessor;
389 connectivityProcessor.m_partIdA = partId;
390 connectivityProcessor.m_triangleIndexA = triangleIndex;
391 connectivityProcessor.m_triangleVerticesA = &triangleVerts[0];
392 connectivityProcessor.m_triangleInfoMap = triangleInfoMap;
393
394 trimeshShape->processAllTriangles(&connectivityProcessor, aabbMin, aabbMax);
395 }
396 }
397 }
398
399
btGenerateInternalEdgeInfo(btHeightfieldTerrainShape * heightfieldShape,btTriangleInfoMap * triangleInfoMap)400 void btGenerateInternalEdgeInfo(btHeightfieldTerrainShape* heightfieldShape, btTriangleInfoMap* triangleInfoMap)
401 {
402
403 //the user pointer shouldn't already be used for other purposes, we intend to store connectivity info there!
404 if (heightfieldShape->getTriangleInfoMap())
405 return;
406
407 heightfieldShape->setTriangleInfoMap(triangleInfoMap);
408
409 //get all the triangles of the heightfield
410
411 btVector3 aabbMin, aabbMax;
412
413 aabbMax.setValue(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT));
414 aabbMin.setValue(btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT));
415
416 b3ProcessAllTrianglesHeightfield processHeightfield(heightfieldShape, triangleInfoMap);
417 heightfieldShape->processAllTriangles(&processHeightfield, aabbMin, aabbMax);
418
419 }
420
421 // Given a point and a line segment (defined by two points), compute the closest point
422 // in the line. Cap the point at the endpoints of the line segment.
btNearestPointInLineSegment(const btVector3 & point,const btVector3 & line0,const btVector3 & line1,btVector3 & nearestPoint)423 void btNearestPointInLineSegment(const btVector3& point, const btVector3& line0, const btVector3& line1, btVector3& nearestPoint)
424 {
425 btVector3 lineDelta = line1 - line0;
426
427 // Handle degenerate lines
428 if (lineDelta.fuzzyZero())
429 {
430 nearestPoint = line0;
431 }
432 else
433 {
434 btScalar delta = (point - line0).dot(lineDelta) / (lineDelta).dot(lineDelta);
435
436 // Clamp the point to conform to the segment's endpoints
437 if (delta < 0)
438 delta = 0;
439 else if (delta > 1)
440 delta = 1;
441
442 nearestPoint = line0 + lineDelta * delta;
443 }
444 }
445
btClampNormal(const btVector3 & edge,const btVector3 & tri_normal_org,const btVector3 & localContactNormalOnB,btScalar correctedEdgeAngle,btVector3 & clampedLocalNormal)446 bool btClampNormal(const btVector3& edge, const btVector3& tri_normal_org, const btVector3& localContactNormalOnB, btScalar correctedEdgeAngle, btVector3& clampedLocalNormal)
447 {
448 btVector3 tri_normal = tri_normal_org;
449 //we only have a local triangle normal, not a local contact normal -> only normal in world space...
450 //either compute the current angle all in local space, or all in world space
451
452 btVector3 edgeCross = edge.cross(tri_normal).normalize();
453 btScalar curAngle = btGetAngle(edgeCross, tri_normal, localContactNormalOnB);
454
455 if (correctedEdgeAngle < 0)
456 {
457 if (curAngle < correctedEdgeAngle)
458 {
459 btScalar diffAngle = correctedEdgeAngle - curAngle;
460 btQuaternion rotation(edge, diffAngle);
461 clampedLocalNormal = btMatrix3x3(rotation) * localContactNormalOnB;
462 return true;
463 }
464 }
465
466 if (correctedEdgeAngle >= 0)
467 {
468 if (curAngle > correctedEdgeAngle)
469 {
470 btScalar diffAngle = correctedEdgeAngle - curAngle;
471 btQuaternion rotation(edge, diffAngle);
472 clampedLocalNormal = btMatrix3x3(rotation) * localContactNormalOnB;
473 return true;
474 }
475 }
476 return false;
477 }
478
479 /// Changes a btManifoldPoint collision normal to the normal from the mesh.
btAdjustInternalEdgeContacts(btManifoldPoint & cp,const btCollisionObjectWrapper * colObj0Wrap,const btCollisionObjectWrapper * colObj1Wrap,int partId0,int index0,int normalAdjustFlags)480 void btAdjustInternalEdgeContacts(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap, const btCollisionObjectWrapper* colObj1Wrap, int partId0, int index0, int normalAdjustFlags)
481 {
482 //btAssert(colObj0->getCollisionShape()->getShapeType() == TRIANGLE_SHAPE_PROXYTYPE);
483 if (colObj0Wrap->getCollisionShape()->getShapeType() != TRIANGLE_SHAPE_PROXYTYPE)
484 return;
485
486
487 btTriangleInfoMap* triangleInfoMapPtr = 0;
488
489 if (colObj0Wrap->getCollisionObject()->getCollisionShape()->getShapeType() == TERRAIN_SHAPE_PROXYTYPE)
490 {
491 btHeightfieldTerrainShape* heightfield = (btHeightfieldTerrainShape*)colObj0Wrap->getCollisionObject()->getCollisionShape();
492 triangleInfoMapPtr = heightfield->getTriangleInfoMap();
493
494 //#define USE_HEIGHTFIELD_TRIANGLES
495 #ifdef USE_HEIGHTFIELD_TRIANGLES
496 btVector3 newNormal = btVector3(0, 0, 1);
497
498 const btTriangleShape* tri_shape = static_cast<const btTriangleShape*>(colObj0Wrap->getCollisionShape());
499 btVector3 tri_normal;
500 tri_shape->calcNormal(tri_normal);
501 newNormal = tri_normal;
502 // cp.m_distance1 = cp.m_distance1 * newNormal.dot(cp.m_normalWorldOnB);
503 cp.m_normalWorldOnB = newNormal;
504 // Reproject collision point along normal. (what about cp.m_distance1?)
505 cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1;
506 cp.m_localPointB = colObj0Wrap->getWorldTransform().invXform(cp.m_positionWorldOnB);
507 return;
508 #endif
509 }
510
511
512 btBvhTriangleMeshShape* trimesh = 0;
513
514 if (colObj0Wrap->getCollisionObject()->getCollisionShape()->getShapeType() == SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE)
515 {
516 trimesh = ((btScaledBvhTriangleMeshShape*)colObj0Wrap->getCollisionObject()->getCollisionShape())->getChildShape();
517 }
518 else
519 {
520 if (colObj0Wrap->getCollisionObject()->getCollisionShape()->getShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE)
521 {
522 trimesh = (btBvhTriangleMeshShape*)colObj0Wrap->getCollisionObject()->getCollisionShape();
523 }
524 }
525 if (trimesh)
526 {
527 triangleInfoMapPtr = (btTriangleInfoMap*)trimesh->getTriangleInfoMap();
528 }
529
530
531 if (!triangleInfoMapPtr)
532 return;
533
534 int hash = btGetHash(partId0, index0);
535
536 btTriangleInfo* info = triangleInfoMapPtr->find(hash);
537 if (!info)
538 return;
539
540 btScalar frontFacing = (normalAdjustFlags & BT_TRIANGLE_CONVEX_BACKFACE_MODE) == 0 ? 1.f : -1.f;
541
542 const btTriangleShape* tri_shape = static_cast<const btTriangleShape*>(colObj0Wrap->getCollisionShape());
543 btVector3 v0, v1, v2;
544 tri_shape->getVertex(0, v0);
545 tri_shape->getVertex(1, v1);
546 tri_shape->getVertex(2, v2);
547
548 //btVector3 center = (v0+v1+v2)*btScalar(1./3.);
549
550 btVector3 red(1, 0, 0), green(0, 1, 0), blue(0, 0, 1), white(1, 1, 1), black(0, 0, 0);
551 btVector3 tri_normal;
552 tri_shape->calcNormal(tri_normal);
553
554 //btScalar dot = tri_normal.dot(cp.m_normalWorldOnB);
555 btVector3 nearest;
556 btNearestPointInLineSegment(cp.m_localPointB, v0, v1, nearest);
557
558 btVector3 contact = cp.m_localPointB;
559 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
560 const btTransform& tr = colObj0->getWorldTransform();
561 btDebugDrawLine(tr * nearest, tr * cp.m_localPointB, red);
562 #endif //BT_INTERNAL_EDGE_DEBUG_DRAW
563
564 bool isNearEdge = false;
565
566 int numConcaveEdgeHits = 0;
567 int numConvexEdgeHits = 0;
568
569 btVector3 localContactNormalOnB = colObj0Wrap->getWorldTransform().getBasis().transpose() * cp.m_normalWorldOnB;
570 localContactNormalOnB.normalize(); //is this necessary?
571
572 // Get closest edge
573 int bestedge = -1;
574 btScalar disttobestedge = BT_LARGE_FLOAT;
575 //
576 // Edge 0 -> 1
577 if (btFabs(info->m_edgeV0V1Angle) < triangleInfoMapPtr->m_maxEdgeAngleThreshold)
578 {
579 btVector3 nearest;
580 btNearestPointInLineSegment(cp.m_localPointB, v0, v1, nearest);
581 btScalar len = (contact - nearest).length();
582 //
583 if (len < disttobestedge)
584 {
585 bestedge = 0;
586 disttobestedge = len;
587 }
588 }
589 // Edge 1 -> 2
590 if (btFabs(info->m_edgeV1V2Angle) < triangleInfoMapPtr->m_maxEdgeAngleThreshold)
591 {
592 btVector3 nearest;
593 btNearestPointInLineSegment(cp.m_localPointB, v1, v2, nearest);
594 btScalar len = (contact - nearest).length();
595 //
596 if (len < disttobestedge)
597 {
598 bestedge = 1;
599 disttobestedge = len;
600 }
601 }
602 // Edge 2 -> 0
603 if (btFabs(info->m_edgeV2V0Angle) < triangleInfoMapPtr->m_maxEdgeAngleThreshold)
604 {
605 btVector3 nearest;
606 btNearestPointInLineSegment(cp.m_localPointB, v2, v0, nearest);
607 btScalar len = (contact - nearest).length();
608 //
609 if (len < disttobestedge)
610 {
611 bestedge = 2;
612 disttobestedge = len;
613 }
614 }
615
616 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
617 btVector3 upfix = tri_normal * btVector3(0.1f, 0.1f, 0.1f);
618 btDebugDrawLine(tr * v0 + upfix, tr * v1 + upfix, red);
619 #endif
620 if (btFabs(info->m_edgeV0V1Angle) < triangleInfoMapPtr->m_maxEdgeAngleThreshold)
621 {
622 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
623 btDebugDrawLine(tr * contact, tr * (contact + cp.m_normalWorldOnB * 10), black);
624 #endif
625 btScalar len = (contact - nearest).length();
626 if (len < triangleInfoMapPtr->m_edgeDistanceThreshold)
627 if (bestedge == 0)
628 {
629 btVector3 edge(v0 - v1);
630 isNearEdge = true;
631
632 if (info->m_edgeV0V1Angle == btScalar(0))
633 {
634 numConcaveEdgeHits++;
635 }
636 else
637 {
638 bool isEdgeConvex = (info->m_flags & TRI_INFO_V0V1_CONVEX);
639 btScalar swapFactor = isEdgeConvex ? btScalar(1) : btScalar(-1);
640 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
641 btDebugDrawLine(tr * nearest, tr * (nearest + swapFactor * tri_normal * 10), white);
642 #endif //BT_INTERNAL_EDGE_DEBUG_DRAW
643
644 btVector3 nA = swapFactor * tri_normal;
645
646 btQuaternion orn(edge, info->m_edgeV0V1Angle);
647 btVector3 computedNormalB = quatRotate(orn, tri_normal);
648 if (info->m_flags & TRI_INFO_V0V1_SWAP_NORMALB)
649 computedNormalB *= -1;
650 btVector3 nB = swapFactor * computedNormalB;
651
652 btScalar NdotA = localContactNormalOnB.dot(nA);
653 btScalar NdotB = localContactNormalOnB.dot(nB);
654 bool backFacingNormal = (NdotA < triangleInfoMapPtr->m_convexEpsilon) && (NdotB < triangleInfoMapPtr->m_convexEpsilon);
655
656 #ifdef DEBUG_INTERNAL_EDGE
657 {
658 btDebugDrawLine(cp.getPositionWorldOnB(), cp.getPositionWorldOnB() + tr.getBasis() * (nB * 20), red);
659 }
660 #endif //DEBUG_INTERNAL_EDGE
661
662 if (backFacingNormal)
663 {
664 numConcaveEdgeHits++;
665 }
666 else
667 {
668 numConvexEdgeHits++;
669 btVector3 clampedLocalNormal;
670 bool isClamped = btClampNormal(edge, swapFactor * tri_normal, localContactNormalOnB, info->m_edgeV0V1Angle, clampedLocalNormal);
671 if (isClamped)
672 {
673 if (((normalAdjustFlags & BT_TRIANGLE_CONVEX_DOUBLE_SIDED) != 0) || (clampedLocalNormal.dot(frontFacing * tri_normal) > 0))
674 {
675 btVector3 newNormal = colObj0Wrap->getWorldTransform().getBasis() * clampedLocalNormal;
676 // cp.m_distance1 = cp.m_distance1 * newNormal.dot(cp.m_normalWorldOnB);
677 cp.m_normalWorldOnB = newNormal;
678 // Reproject collision point along normal. (what about cp.m_distance1?)
679 cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1;
680 cp.m_localPointB = colObj0Wrap->getWorldTransform().invXform(cp.m_positionWorldOnB);
681 }
682 }
683 }
684 }
685 }
686 }
687
688 btNearestPointInLineSegment(contact, v1, v2, nearest);
689 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
690 btDebugDrawLine(tr * nearest, tr * cp.m_localPointB, green);
691 #endif //BT_INTERNAL_EDGE_DEBUG_DRAW
692
693 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
694 btDebugDrawLine(tr * v1 + upfix, tr * v2 + upfix, green);
695 #endif
696
697 if (btFabs(info->m_edgeV1V2Angle) < triangleInfoMapPtr->m_maxEdgeAngleThreshold)
698 {
699 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
700 btDebugDrawLine(tr * contact, tr * (contact + cp.m_normalWorldOnB * 10), black);
701 #endif //BT_INTERNAL_EDGE_DEBUG_DRAW
702
703 btScalar len = (contact - nearest).length();
704 if (len < triangleInfoMapPtr->m_edgeDistanceThreshold)
705 if (bestedge == 1)
706 {
707 isNearEdge = true;
708 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
709 btDebugDrawLine(tr * nearest, tr * (nearest + tri_normal * 10), white);
710 #endif //BT_INTERNAL_EDGE_DEBUG_DRAW
711
712 btVector3 edge(v1 - v2);
713
714 isNearEdge = true;
715
716 if (info->m_edgeV1V2Angle == btScalar(0))
717 {
718 numConcaveEdgeHits++;
719 }
720 else
721 {
722 bool isEdgeConvex = (info->m_flags & TRI_INFO_V1V2_CONVEX) != 0;
723 btScalar swapFactor = isEdgeConvex ? btScalar(1) : btScalar(-1);
724 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
725 btDebugDrawLine(tr * nearest, tr * (nearest + swapFactor * tri_normal * 10), white);
726 #endif //BT_INTERNAL_EDGE_DEBUG_DRAW
727
728 btVector3 nA = swapFactor * tri_normal;
729
730 btQuaternion orn(edge, info->m_edgeV1V2Angle);
731 btVector3 computedNormalB = quatRotate(orn, tri_normal);
732 if (info->m_flags & TRI_INFO_V1V2_SWAP_NORMALB)
733 computedNormalB *= -1;
734 btVector3 nB = swapFactor * computedNormalB;
735
736 #ifdef DEBUG_INTERNAL_EDGE
737 {
738 btDebugDrawLine(cp.getPositionWorldOnB(), cp.getPositionWorldOnB() + tr.getBasis() * (nB * 20), red);
739 }
740 #endif //DEBUG_INTERNAL_EDGE
741
742 btScalar NdotA = localContactNormalOnB.dot(nA);
743 btScalar NdotB = localContactNormalOnB.dot(nB);
744 bool backFacingNormal = (NdotA < triangleInfoMapPtr->m_convexEpsilon) && (NdotB < triangleInfoMapPtr->m_convexEpsilon);
745
746 if (backFacingNormal)
747 {
748 numConcaveEdgeHits++;
749 }
750 else
751 {
752 numConvexEdgeHits++;
753 btVector3 localContactNormalOnB = colObj0Wrap->getWorldTransform().getBasis().transpose() * cp.m_normalWorldOnB;
754 btVector3 clampedLocalNormal;
755 bool isClamped = btClampNormal(edge, swapFactor * tri_normal, localContactNormalOnB, info->m_edgeV1V2Angle, clampedLocalNormal);
756 if (isClamped)
757 {
758 if (((normalAdjustFlags & BT_TRIANGLE_CONVEX_DOUBLE_SIDED) != 0) || (clampedLocalNormal.dot(frontFacing * tri_normal) > 0))
759 {
760 btVector3 newNormal = colObj0Wrap->getWorldTransform().getBasis() * clampedLocalNormal;
761 // cp.m_distance1 = cp.m_distance1 * newNormal.dot(cp.m_normalWorldOnB);
762 cp.m_normalWorldOnB = newNormal;
763 // Reproject collision point along normal.
764 cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1;
765 cp.m_localPointB = colObj0Wrap->getWorldTransform().invXform(cp.m_positionWorldOnB);
766 }
767 }
768 }
769 }
770 }
771 }
772
773 btNearestPointInLineSegment(contact, v2, v0, nearest);
774 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
775 btDebugDrawLine(tr * nearest, tr * cp.m_localPointB, blue);
776 #endif //BT_INTERNAL_EDGE_DEBUG_DRAW
777 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
778 btDebugDrawLine(tr * v2 + upfix, tr * v0 + upfix, blue);
779 #endif
780
781 if (btFabs(info->m_edgeV2V0Angle) < triangleInfoMapPtr->m_maxEdgeAngleThreshold)
782 {
783 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
784 btDebugDrawLine(tr * contact, tr * (contact + cp.m_normalWorldOnB * 10), black);
785 #endif //BT_INTERNAL_EDGE_DEBUG_DRAW
786
787 btScalar len = (contact - nearest).length();
788 if (len < triangleInfoMapPtr->m_edgeDistanceThreshold)
789 if (bestedge == 2)
790 {
791 isNearEdge = true;
792 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
793 btDebugDrawLine(tr * nearest, tr * (nearest + tri_normal * 10), white);
794 #endif //BT_INTERNAL_EDGE_DEBUG_DRAW
795
796 btVector3 edge(v2 - v0);
797
798 if (info->m_edgeV2V0Angle == btScalar(0))
799 {
800 numConcaveEdgeHits++;
801 }
802 else
803 {
804 bool isEdgeConvex = (info->m_flags & TRI_INFO_V2V0_CONVEX) != 0;
805 btScalar swapFactor = isEdgeConvex ? btScalar(1) : btScalar(-1);
806 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
807 btDebugDrawLine(tr * nearest, tr * (nearest + swapFactor * tri_normal * 10), white);
808 #endif //BT_INTERNAL_EDGE_DEBUG_DRAW
809
810 btVector3 nA = swapFactor * tri_normal;
811 btQuaternion orn(edge, info->m_edgeV2V0Angle);
812 btVector3 computedNormalB = quatRotate(orn, tri_normal);
813 if (info->m_flags & TRI_INFO_V2V0_SWAP_NORMALB)
814 computedNormalB *= -1;
815 btVector3 nB = swapFactor * computedNormalB;
816
817 #ifdef DEBUG_INTERNAL_EDGE
818 {
819 btDebugDrawLine(cp.getPositionWorldOnB(), cp.getPositionWorldOnB() + tr.getBasis() * (nB * 20), red);
820 }
821 #endif //DEBUG_INTERNAL_EDGE
822
823 btScalar NdotA = localContactNormalOnB.dot(nA);
824 btScalar NdotB = localContactNormalOnB.dot(nB);
825 bool backFacingNormal = (NdotA < triangleInfoMapPtr->m_convexEpsilon) && (NdotB < triangleInfoMapPtr->m_convexEpsilon);
826
827 if (backFacingNormal)
828 {
829 numConcaveEdgeHits++;
830 }
831 else
832 {
833 numConvexEdgeHits++;
834 // printf("hitting convex edge\n");
835
836 btVector3 localContactNormalOnB = colObj0Wrap->getWorldTransform().getBasis().transpose() * cp.m_normalWorldOnB;
837 btVector3 clampedLocalNormal;
838 bool isClamped = btClampNormal(edge, swapFactor * tri_normal, localContactNormalOnB, info->m_edgeV2V0Angle, clampedLocalNormal);
839 if (isClamped)
840 {
841 if (((normalAdjustFlags & BT_TRIANGLE_CONVEX_DOUBLE_SIDED) != 0) || (clampedLocalNormal.dot(frontFacing * tri_normal) > 0))
842 {
843 btVector3 newNormal = colObj0Wrap->getWorldTransform().getBasis() * clampedLocalNormal;
844 // cp.m_distance1 = cp.m_distance1 * newNormal.dot(cp.m_normalWorldOnB);
845 cp.m_normalWorldOnB = newNormal;
846 // Reproject collision point along normal.
847 cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1;
848 cp.m_localPointB = colObj0Wrap->getWorldTransform().invXform(cp.m_positionWorldOnB);
849 }
850 }
851 }
852 }
853 }
854 }
855
856 #ifdef DEBUG_INTERNAL_EDGE
857 {
858 btVector3 color(0, 1, 1);
859 btDebugDrawLine(cp.getPositionWorldOnB(), cp.getPositionWorldOnB() + cp.m_normalWorldOnB * 10, color);
860 }
861 #endif //DEBUG_INTERNAL_EDGE
862
863 if (isNearEdge)
864 {
865 if (numConcaveEdgeHits > 0)
866 {
867 if ((normalAdjustFlags & BT_TRIANGLE_CONCAVE_DOUBLE_SIDED) != 0)
868 {
869 //fix tri_normal so it pointing the same direction as the current local contact normal
870 if (tri_normal.dot(localContactNormalOnB) < 0)
871 {
872 tri_normal *= -1;
873 }
874 cp.m_normalWorldOnB = colObj0Wrap->getWorldTransform().getBasis() * tri_normal;
875 }
876 else
877 {
878 btVector3 newNormal = tri_normal * frontFacing;
879 //if the tri_normal is pointing opposite direction as the current local contact normal, skip it
880 btScalar d = newNormal.dot(localContactNormalOnB);
881 if (d < 0)
882 {
883 return;
884 }
885 //modify the normal to be the triangle normal (or backfacing normal)
886 cp.m_normalWorldOnB = colObj0Wrap->getWorldTransform().getBasis() * newNormal;
887 }
888
889 // Reproject collision point along normal.
890 cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1;
891 cp.m_localPointB = colObj0Wrap->getWorldTransform().invXform(cp.m_positionWorldOnB);
892 }
893 }
894 }
895