Bullet Collision Detection & Physics Library
btInternalEdgeUtility.cpp
Go to the documentation of this file.
2 
10 
11 //#define DEBUG_INTERNAL_EDGE
12 
13 #ifdef DEBUG_INTERNAL_EDGE
14 #include <stdio.h>
15 #endif //DEBUG_INTERNAL_EDGE
16 
17 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
18 static btIDebugDraw* gDebugDrawer = 0;
19 
20 void btSetDebugDrawer(btIDebugDraw* debugDrawer)
21 {
22  gDebugDrawer = debugDrawer;
23 }
24 
25 static void btDebugDrawLine(const btVector3& from, const btVector3& to, const btVector3& color)
26 {
27  if (gDebugDrawer)
28  gDebugDrawer->drawLine(from, to, color);
29 }
30 #endif //BT_INTERNAL_EDGE_DEBUG_DRAW
31 
32 static int btGetHash(int partId, int triangleIndex)
33 {
34  int hash = (partId << (31 - MAX_NUM_PARTS_IN_BITS)) | triangleIndex;
35  return hash;
36 }
37 
38 static btScalar btGetAngle(const btVector3& edgeA, const btVector3& normalA, const btVector3& normalB)
39 {
40  const btVector3 refAxis0 = edgeA;
41  const btVector3 refAxis1 = normalA;
42  const btVector3 swingAxis = normalB;
43  btScalar angle = btAtan2(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1));
44  return angle;
45 }
46 
48 {
49  int m_partIdA;
53 
54  virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex)
55  {
56  //skip self-collisions
57  if ((m_partIdA == partId) && (m_triangleIndexA == triangleIndex))
58  return;
59 
60  //skip duplicates (disabled for now)
61  //if ((m_partIdA <= partId) && (m_triangleIndexA <= triangleIndex))
62  // return;
63 
64  //search for shared vertices and edges
65  int numshared = 0;
66  int sharedVertsA[3] = {-1, -1, -1};
67  int sharedVertsB[3] = {-1, -1, -1};
68 
70  btScalar crossBSqr = ((triangle[1] - triangle[0]).cross(triangle[2] - triangle[0])).length2();
71  if (crossBSqr < m_triangleInfoMap->m_equalVertexThreshold)
72  return;
73 
74  btScalar crossASqr = ((m_triangleVerticesA[1] - m_triangleVerticesA[0]).cross(m_triangleVerticesA[2] - m_triangleVerticesA[0])).length2();
76  if (crossASqr < m_triangleInfoMap->m_equalVertexThreshold)
77  return;
78 
79 #if 0
80  printf("triangle A[0] = (%f,%f,%f)\ntriangle A[1] = (%f,%f,%f)\ntriangle A[2] = (%f,%f,%f)\n",
81  m_triangleVerticesA[0].getX(),m_triangleVerticesA[0].getY(),m_triangleVerticesA[0].getZ(),
82  m_triangleVerticesA[1].getX(),m_triangleVerticesA[1].getY(),m_triangleVerticesA[1].getZ(),
83  m_triangleVerticesA[2].getX(),m_triangleVerticesA[2].getY(),m_triangleVerticesA[2].getZ());
84 
85  printf("partId=%d, triangleIndex=%d\n",partId,triangleIndex);
86  printf("triangle B[0] = (%f,%f,%f)\ntriangle B[1] = (%f,%f,%f)\ntriangle B[2] = (%f,%f,%f)\n",
87  triangle[0].getX(),triangle[0].getY(),triangle[0].getZ(),
88  triangle[1].getX(),triangle[1].getY(),triangle[1].getZ(),
89  triangle[2].getX(),triangle[2].getY(),triangle[2].getZ());
90 #endif
91 
92  for (int i = 0; i < 3; i++)
93  {
94  for (int j = 0; j < 3; j++)
95  {
96  if ((m_triangleVerticesA[i] - triangle[j]).length2() < m_triangleInfoMap->m_equalVertexThreshold)
97  {
98  sharedVertsA[numshared] = i;
99  sharedVertsB[numshared] = j;
100  numshared++;
102  if (numshared >= 3)
103  return;
104  }
105  }
107  if (numshared >= 3)
108  return;
109  }
110  switch (numshared)
111  {
112  case 0:
113  {
114  break;
115  }
116  case 1:
117  {
118  //shared vertex
119  break;
120  }
121  case 2:
122  {
123  //shared edge
124  //we need to make sure the edge is in the order V2V0 and not V0V2 so that the signs are correct
125  if (sharedVertsA[0] == 0 && sharedVertsA[1] == 2)
126  {
127  sharedVertsA[0] = 2;
128  sharedVertsA[1] = 0;
129  int tmp = sharedVertsB[1];
130  sharedVertsB[1] = sharedVertsB[0];
131  sharedVertsB[0] = tmp;
132  }
133 
134  int hash = btGetHash(m_partIdA, m_triangleIndexA);
135 
136  btTriangleInfo* info = m_triangleInfoMap->find(hash);
137  if (!info)
138  {
139  btTriangleInfo tmp;
140  m_triangleInfoMap->insert(hash, tmp);
141  info = m_triangleInfoMap->find(hash);
142  }
143 
144  int sumvertsA = sharedVertsA[0] + sharedVertsA[1];
145  int otherIndexA = 3 - sumvertsA;
146 
147  btVector3 edge(m_triangleVerticesA[sharedVertsA[1]] - m_triangleVerticesA[sharedVertsA[0]]);
148 
150  int otherIndexB = 3 - (sharedVertsB[0] + sharedVertsB[1]);
151 
152  btTriangleShape tB(triangle[sharedVertsB[1]], triangle[sharedVertsB[0]], triangle[otherIndexB]);
153  //btTriangleShape tB(triangle[0],triangle[1],triangle[2]);
154 
155  btVector3 normalA;
156  btVector3 normalB;
157  tA.calcNormal(normalA);
158  tB.calcNormal(normalB);
159  edge.normalize();
160  btVector3 edgeCrossA = edge.cross(normalA).normalize();
161 
162  {
163  btVector3 tmp = m_triangleVerticesA[otherIndexA] - m_triangleVerticesA[sharedVertsA[0]];
164  if (edgeCrossA.dot(tmp) < 0)
165  {
166  edgeCrossA *= -1;
167  }
168  }
169 
170  btVector3 edgeCrossB = edge.cross(normalB).normalize();
171 
172  {
173  btVector3 tmp = triangle[otherIndexB] - triangle[sharedVertsB[0]];
174  if (edgeCrossB.dot(tmp) < 0)
175  {
176  edgeCrossB *= -1;
177  }
178  }
179 
180  btScalar angle2 = 0;
181  btScalar ang4 = 0.f;
182 
183  btVector3 calculatedEdge = edgeCrossA.cross(edgeCrossB);
184  btScalar len2 = calculatedEdge.length2();
185 
186  btScalar correctedAngle(0);
187  //btVector3 calculatedNormalB = normalA;
188  bool isConvex = false;
189 
190  if (len2 < m_triangleInfoMap->m_planarEpsilon)
191  {
192  angle2 = 0.f;
193  ang4 = 0.f;
194  }
195  else
196  {
197  calculatedEdge.normalize();
198  btVector3 calculatedNormalA = calculatedEdge.cross(edgeCrossA);
199  calculatedNormalA.normalize();
200  angle2 = btGetAngle(calculatedNormalA, edgeCrossA, edgeCrossB);
201  ang4 = SIMD_PI - angle2;
202  btScalar dotA = normalA.dot(edgeCrossB);
204  isConvex = (dotA < 0.);
205 
206  correctedAngle = isConvex ? ang4 : -ang4;
207  }
208 
209  //alternatively use
210  //btVector3 calculatedNormalB2 = quatRotate(orn,normalA);
211 
212  switch (sumvertsA)
213  {
214  case 1:
215  {
217  btQuaternion orn(edge, -correctedAngle);
218  btVector3 computedNormalB = quatRotate(orn, normalA);
219  btScalar bla = computedNormalB.dot(normalB);
220  if (bla < 0)
221  {
222  computedNormalB *= -1;
224  }
225 #ifdef DEBUG_INTERNAL_EDGE
226  if ((computedNormalB - normalB).length() > 0.0001)
227  {
228  printf("warning: normals not identical\n");
229  }
230 #endif //DEBUG_INTERNAL_EDGE
231 
232  info->m_edgeV0V1Angle = -correctedAngle;
233 
234  if (isConvex)
235  info->m_flags |= TRI_INFO_V0V1_CONVEX;
236  break;
237  }
238  case 2:
239  {
241  btQuaternion orn(edge, -correctedAngle);
242  btVector3 computedNormalB = quatRotate(orn, normalA);
243  if (computedNormalB.dot(normalB) < 0)
244  {
245  computedNormalB *= -1;
247  }
248 
249 #ifdef DEBUG_INTERNAL_EDGE
250  if ((computedNormalB - normalB).length() > 0.0001)
251  {
252  printf("warning: normals not identical\n");
253  }
254 #endif //DEBUG_INTERNAL_EDGE
255  info->m_edgeV2V0Angle = -correctedAngle;
256  if (isConvex)
257  info->m_flags |= TRI_INFO_V2V0_CONVEX;
258  break;
259  }
260  case 3:
261  {
263  btQuaternion orn(edge, -correctedAngle);
264  btVector3 computedNormalB = quatRotate(orn, normalA);
265  if (computedNormalB.dot(normalB) < 0)
266  {
268  computedNormalB *= -1;
269  }
270 #ifdef DEBUG_INTERNAL_EDGE
271  if ((computedNormalB - normalB).length() > 0.0001)
272  {
273  printf("warning: normals not identical\n");
274  }
275 #endif //DEBUG_INTERNAL_EDGE
276  info->m_edgeV1V2Angle = -correctedAngle;
277 
278  if (isConvex)
279  info->m_flags |= TRI_INFO_V1V2_CONVEX;
280  break;
281  }
282  }
283 
284  break;
285  }
286  default:
287  {
288  // printf("warning: duplicate triangle\n");
289  }
290  }
291  }
292 };
295 
297 {
298  //the user pointer shouldn't already be used for other purposes, we intend to store connectivity info there!
299  if (trimeshShape->getTriangleInfoMap())
300  return;
301 
302  trimeshShape->setTriangleInfoMap(triangleInfoMap);
303 
304  btStridingMeshInterface* meshInterface = trimeshShape->getMeshInterface();
305  const btVector3& meshScaling = meshInterface->getScaling();
306 
307  for (int partId = 0; partId < meshInterface->getNumSubParts(); partId++)
308  {
309  const unsigned char* vertexbase = 0;
310  int numverts = 0;
312  int stride = 0;
313  const unsigned char* indexbase = 0;
314  int indexstride = 0;
315  int numfaces = 0;
316  PHY_ScalarType indicestype = PHY_INTEGER;
317  //PHY_ScalarType indexType=0;
318 
319  btVector3 triangleVerts[3];
320  meshInterface->getLockedReadOnlyVertexIndexBase(&vertexbase, numverts, type, stride, &indexbase, indexstride, numfaces, indicestype, partId);
321  btVector3 aabbMin, aabbMax;
322 
323  for (int triangleIndex = 0; triangleIndex < numfaces; triangleIndex++)
324  {
325  unsigned int* gfxbase = (unsigned int*)(indexbase + triangleIndex * indexstride);
326 
327  for (int j = 2; j >= 0; j--)
328  {
329  int graphicsindex = indicestype == PHY_SHORT ? ((unsigned short*)gfxbase)[j] : gfxbase[j];
330  if (type == PHY_FLOAT)
331  {
332  float* graphicsbase = (float*)(vertexbase + graphicsindex * stride);
333  triangleVerts[j] = btVector3(
334  graphicsbase[0] * meshScaling.getX(),
335  graphicsbase[1] * meshScaling.getY(),
336  graphicsbase[2] * meshScaling.getZ());
337  }
338  else
339  {
340  double* graphicsbase = (double*)(vertexbase + graphicsindex * stride);
341  triangleVerts[j] = btVector3(btScalar(graphicsbase[0] * meshScaling.getX()), btScalar(graphicsbase[1] * meshScaling.getY()), btScalar(graphicsbase[2] * meshScaling.getZ()));
342  }
343  }
346  aabbMin.setMin(triangleVerts[0]);
347  aabbMax.setMax(triangleVerts[0]);
348  aabbMin.setMin(triangleVerts[1]);
349  aabbMax.setMax(triangleVerts[1]);
350  aabbMin.setMin(triangleVerts[2]);
351  aabbMax.setMax(triangleVerts[2]);
352 
353  btConnectivityProcessor connectivityProcessor;
354  connectivityProcessor.m_partIdA = partId;
355  connectivityProcessor.m_triangleIndexA = triangleIndex;
356  connectivityProcessor.m_triangleVerticesA = &triangleVerts[0];
357  connectivityProcessor.m_triangleInfoMap = triangleInfoMap;
358 
359  trimeshShape->processAllTriangles(&connectivityProcessor, aabbMin, aabbMax);
360  }
361  }
362 }
363 
364 // Given a point and a line segment (defined by two points), compute the closest point
365 // in the line. Cap the point at the endpoints of the line segment.
366 void btNearestPointInLineSegment(const btVector3& point, const btVector3& line0, const btVector3& line1, btVector3& nearestPoint)
367 {
368  btVector3 lineDelta = line1 - line0;
369 
370  // Handle degenerate lines
371  if (lineDelta.fuzzyZero())
372  {
373  nearestPoint = line0;
374  }
375  else
376  {
377  btScalar delta = (point - line0).dot(lineDelta) / (lineDelta).dot(lineDelta);
378 
379  // Clamp the point to conform to the segment's endpoints
380  if (delta < 0)
381  delta = 0;
382  else if (delta > 1)
383  delta = 1;
384 
385  nearestPoint = line0 + lineDelta * delta;
386  }
387 }
388 
389 bool btClampNormal(const btVector3& edge, const btVector3& tri_normal_org, const btVector3& localContactNormalOnB, btScalar correctedEdgeAngle, btVector3& clampedLocalNormal)
390 {
391  btVector3 tri_normal = tri_normal_org;
392  //we only have a local triangle normal, not a local contact normal -> only normal in world space...
393  //either compute the current angle all in local space, or all in world space
394 
395  btVector3 edgeCross = edge.cross(tri_normal).normalize();
396  btScalar curAngle = btGetAngle(edgeCross, tri_normal, localContactNormalOnB);
397 
398  if (correctedEdgeAngle < 0)
399  {
400  if (curAngle < correctedEdgeAngle)
401  {
402  btScalar diffAngle = correctedEdgeAngle - curAngle;
403  btQuaternion rotation(edge, diffAngle);
404  clampedLocalNormal = btMatrix3x3(rotation) * localContactNormalOnB;
405  return true;
406  }
407  }
408 
409  if (correctedEdgeAngle >= 0)
410  {
411  if (curAngle > correctedEdgeAngle)
412  {
413  btScalar diffAngle = correctedEdgeAngle - curAngle;
414  btQuaternion rotation(edge, diffAngle);
415  clampedLocalNormal = btMatrix3x3(rotation) * localContactNormalOnB;
416  return true;
417  }
418  }
419  return false;
420 }
421 
423 void btAdjustInternalEdgeContacts(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap, const btCollisionObjectWrapper* colObj1Wrap, int partId0, int index0, int normalAdjustFlags)
424 {
425  //btAssert(colObj0->getCollisionShape()->getShapeType() == TRIANGLE_SHAPE_PROXYTYPE);
427  return;
428 
429  btBvhTriangleMeshShape* trimesh = 0;
430 
432  {
433  trimesh = ((btScaledBvhTriangleMeshShape*)colObj0Wrap->getCollisionObject()->getCollisionShape())->getChildShape();
434  }
435  else
436  {
438  {
439  trimesh = (btBvhTriangleMeshShape*)colObj0Wrap->getCollisionObject()->getCollisionShape();
440  }
441  }
442  if (trimesh == 0)
443  return;
444 
445  btTriangleInfoMap* triangleInfoMapPtr = (btTriangleInfoMap*)trimesh->getTriangleInfoMap();
446  if (!triangleInfoMapPtr)
447  return;
448 
449  int hash = btGetHash(partId0, index0);
450 
451  btTriangleInfo* info = triangleInfoMapPtr->find(hash);
452  if (!info)
453  return;
454 
455  btScalar frontFacing = (normalAdjustFlags & BT_TRIANGLE_CONVEX_BACKFACE_MODE) == 0 ? 1.f : -1.f;
456 
457  const btTriangleShape* tri_shape = static_cast<const btTriangleShape*>(colObj0Wrap->getCollisionShape());
458  btVector3 v0, v1, v2;
459  tri_shape->getVertex(0, v0);
460  tri_shape->getVertex(1, v1);
461  tri_shape->getVertex(2, v2);
462 
463  //btVector3 center = (v0+v1+v2)*btScalar(1./3.);
464 
465  btVector3 red(1, 0, 0), green(0, 1, 0), blue(0, 0, 1), white(1, 1, 1), black(0, 0, 0);
466  btVector3 tri_normal;
467  tri_shape->calcNormal(tri_normal);
468 
469  //btScalar dot = tri_normal.dot(cp.m_normalWorldOnB);
470  btVector3 nearest;
471  btNearestPointInLineSegment(cp.m_localPointB, v0, v1, nearest);
472 
473  btVector3 contact = cp.m_localPointB;
474 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
475  const btTransform& tr = colObj0->getWorldTransform();
476  btDebugDrawLine(tr * nearest, tr * cp.m_localPointB, red);
477 #endif //BT_INTERNAL_EDGE_DEBUG_DRAW
478 
479  bool isNearEdge = false;
480 
481  int numConcaveEdgeHits = 0;
482  int numConvexEdgeHits = 0;
483 
484  btVector3 localContactNormalOnB = colObj0Wrap->getWorldTransform().getBasis().transpose() * cp.m_normalWorldOnB;
485  localContactNormalOnB.normalize(); //is this necessary?
486 
487  // Get closest edge
488  int bestedge = -1;
489  btScalar disttobestedge = BT_LARGE_FLOAT;
490  //
491  // Edge 0 -> 1
492  if (btFabs(info->m_edgeV0V1Angle) < triangleInfoMapPtr->m_maxEdgeAngleThreshold)
493  {
494  btVector3 nearest;
495  btNearestPointInLineSegment(cp.m_localPointB, v0, v1, nearest);
496  btScalar len = (contact - nearest).length();
497  //
498  if (len < disttobestedge)
499  {
500  bestedge = 0;
501  disttobestedge = len;
502  }
503  }
504  // Edge 1 -> 2
505  if (btFabs(info->m_edgeV1V2Angle) < triangleInfoMapPtr->m_maxEdgeAngleThreshold)
506  {
507  btVector3 nearest;
508  btNearestPointInLineSegment(cp.m_localPointB, v1, v2, nearest);
509  btScalar len = (contact - nearest).length();
510  //
511  if (len < disttobestedge)
512  {
513  bestedge = 1;
514  disttobestedge = len;
515  }
516  }
517  // Edge 2 -> 0
518  if (btFabs(info->m_edgeV2V0Angle) < triangleInfoMapPtr->m_maxEdgeAngleThreshold)
519  {
520  btVector3 nearest;
521  btNearestPointInLineSegment(cp.m_localPointB, v2, v0, nearest);
522  btScalar len = (contact - nearest).length();
523  //
524  if (len < disttobestedge)
525  {
526  bestedge = 2;
527  disttobestedge = len;
528  }
529  }
530 
531 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
532  btVector3 upfix = tri_normal * btVector3(0.1f, 0.1f, 0.1f);
533  btDebugDrawLine(tr * v0 + upfix, tr * v1 + upfix, red);
534 #endif
535  if (btFabs(info->m_edgeV0V1Angle) < triangleInfoMapPtr->m_maxEdgeAngleThreshold)
536  {
537 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
538  btDebugDrawLine(tr * contact, tr * (contact + cp.m_normalWorldOnB * 10), black);
539 #endif
540  btScalar len = (contact - nearest).length();
541  if (len < triangleInfoMapPtr->m_edgeDistanceThreshold)
542  if (bestedge == 0)
543  {
544  btVector3 edge(v0 - v1);
545  isNearEdge = true;
546 
547  if (info->m_edgeV0V1Angle == btScalar(0))
548  {
549  numConcaveEdgeHits++;
550  }
551  else
552  {
553  bool isEdgeConvex = (info->m_flags & TRI_INFO_V0V1_CONVEX);
554  btScalar swapFactor = isEdgeConvex ? btScalar(1) : btScalar(-1);
555 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
556  btDebugDrawLine(tr * nearest, tr * (nearest + swapFactor * tri_normal * 10), white);
557 #endif //BT_INTERNAL_EDGE_DEBUG_DRAW
558 
559  btVector3 nA = swapFactor * tri_normal;
560 
561  btQuaternion orn(edge, info->m_edgeV0V1Angle);
562  btVector3 computedNormalB = quatRotate(orn, tri_normal);
564  computedNormalB *= -1;
565  btVector3 nB = swapFactor * computedNormalB;
566 
567  btScalar NdotA = localContactNormalOnB.dot(nA);
568  btScalar NdotB = localContactNormalOnB.dot(nB);
569  bool backFacingNormal = (NdotA < triangleInfoMapPtr->m_convexEpsilon) && (NdotB < triangleInfoMapPtr->m_convexEpsilon);
570 
571 #ifdef DEBUG_INTERNAL_EDGE
572  {
573  btDebugDrawLine(cp.getPositionWorldOnB(), cp.getPositionWorldOnB() + tr.getBasis() * (nB * 20), red);
574  }
575 #endif //DEBUG_INTERNAL_EDGE
576 
577  if (backFacingNormal)
578  {
579  numConcaveEdgeHits++;
580  }
581  else
582  {
583  numConvexEdgeHits++;
584  btVector3 clampedLocalNormal;
585  bool isClamped = btClampNormal(edge, swapFactor * tri_normal, localContactNormalOnB, info->m_edgeV0V1Angle, clampedLocalNormal);
586  if (isClamped)
587  {
588  if (((normalAdjustFlags & BT_TRIANGLE_CONVEX_DOUBLE_SIDED) != 0) || (clampedLocalNormal.dot(frontFacing * tri_normal) > 0))
589  {
590  btVector3 newNormal = colObj0Wrap->getWorldTransform().getBasis() * clampedLocalNormal;
591  // cp.m_distance1 = cp.m_distance1 * newNormal.dot(cp.m_normalWorldOnB);
592  cp.m_normalWorldOnB = newNormal;
593  // Reproject collision point along normal. (what about cp.m_distance1?)
596  }
597  }
598  }
599  }
600  }
601  }
602 
603  btNearestPointInLineSegment(contact, v1, v2, nearest);
604 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
605  btDebugDrawLine(tr * nearest, tr * cp.m_localPointB, green);
606 #endif //BT_INTERNAL_EDGE_DEBUG_DRAW
607 
608 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
609  btDebugDrawLine(tr * v1 + upfix, tr * v2 + upfix, green);
610 #endif
611 
612  if (btFabs(info->m_edgeV1V2Angle) < triangleInfoMapPtr->m_maxEdgeAngleThreshold)
613  {
614 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
615  btDebugDrawLine(tr * contact, tr * (contact + cp.m_normalWorldOnB * 10), black);
616 #endif //BT_INTERNAL_EDGE_DEBUG_DRAW
617 
618  btScalar len = (contact - nearest).length();
619  if (len < triangleInfoMapPtr->m_edgeDistanceThreshold)
620  if (bestedge == 1)
621  {
622  isNearEdge = true;
623 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
624  btDebugDrawLine(tr * nearest, tr * (nearest + tri_normal * 10), white);
625 #endif //BT_INTERNAL_EDGE_DEBUG_DRAW
626 
627  btVector3 edge(v1 - v2);
628 
629  isNearEdge = true;
630 
631  if (info->m_edgeV1V2Angle == btScalar(0))
632  {
633  numConcaveEdgeHits++;
634  }
635  else
636  {
637  bool isEdgeConvex = (info->m_flags & TRI_INFO_V1V2_CONVEX) != 0;
638  btScalar swapFactor = isEdgeConvex ? btScalar(1) : btScalar(-1);
639 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
640  btDebugDrawLine(tr * nearest, tr * (nearest + swapFactor * tri_normal * 10), white);
641 #endif //BT_INTERNAL_EDGE_DEBUG_DRAW
642 
643  btVector3 nA = swapFactor * tri_normal;
644 
645  btQuaternion orn(edge, info->m_edgeV1V2Angle);
646  btVector3 computedNormalB = quatRotate(orn, tri_normal);
648  computedNormalB *= -1;
649  btVector3 nB = swapFactor * computedNormalB;
650 
651 #ifdef DEBUG_INTERNAL_EDGE
652  {
653  btDebugDrawLine(cp.getPositionWorldOnB(), cp.getPositionWorldOnB() + tr.getBasis() * (nB * 20), red);
654  }
655 #endif //DEBUG_INTERNAL_EDGE
656 
657  btScalar NdotA = localContactNormalOnB.dot(nA);
658  btScalar NdotB = localContactNormalOnB.dot(nB);
659  bool backFacingNormal = (NdotA < triangleInfoMapPtr->m_convexEpsilon) && (NdotB < triangleInfoMapPtr->m_convexEpsilon);
660 
661  if (backFacingNormal)
662  {
663  numConcaveEdgeHits++;
664  }
665  else
666  {
667  numConvexEdgeHits++;
668  btVector3 localContactNormalOnB = colObj0Wrap->getWorldTransform().getBasis().transpose() * cp.m_normalWorldOnB;
669  btVector3 clampedLocalNormal;
670  bool isClamped = btClampNormal(edge, swapFactor * tri_normal, localContactNormalOnB, info->m_edgeV1V2Angle, 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.
681  }
682  }
683  }
684  }
685  }
686  }
687 
688  btNearestPointInLineSegment(contact, v2, v0, nearest);
689 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
690  btDebugDrawLine(tr * nearest, tr * cp.m_localPointB, blue);
691 #endif //BT_INTERNAL_EDGE_DEBUG_DRAW
692 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
693  btDebugDrawLine(tr * v2 + upfix, tr * v0 + upfix, blue);
694 #endif
695 
696  if (btFabs(info->m_edgeV2V0Angle) < triangleInfoMapPtr->m_maxEdgeAngleThreshold)
697  {
698 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
699  btDebugDrawLine(tr * contact, tr * (contact + cp.m_normalWorldOnB * 10), black);
700 #endif //BT_INTERNAL_EDGE_DEBUG_DRAW
701 
702  btScalar len = (contact - nearest).length();
703  if (len < triangleInfoMapPtr->m_edgeDistanceThreshold)
704  if (bestedge == 2)
705  {
706  isNearEdge = true;
707 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
708  btDebugDrawLine(tr * nearest, tr * (nearest + tri_normal * 10), white);
709 #endif //BT_INTERNAL_EDGE_DEBUG_DRAW
710 
711  btVector3 edge(v2 - v0);
712 
713  if (info->m_edgeV2V0Angle == btScalar(0))
714  {
715  numConcaveEdgeHits++;
716  }
717  else
718  {
719  bool isEdgeConvex = (info->m_flags & TRI_INFO_V2V0_CONVEX) != 0;
720  btScalar swapFactor = isEdgeConvex ? btScalar(1) : btScalar(-1);
721 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
722  btDebugDrawLine(tr * nearest, tr * (nearest + swapFactor * tri_normal * 10), white);
723 #endif //BT_INTERNAL_EDGE_DEBUG_DRAW
724 
725  btVector3 nA = swapFactor * tri_normal;
726  btQuaternion orn(edge, info->m_edgeV2V0Angle);
727  btVector3 computedNormalB = quatRotate(orn, tri_normal);
729  computedNormalB *= -1;
730  btVector3 nB = swapFactor * computedNormalB;
731 
732 #ifdef DEBUG_INTERNAL_EDGE
733  {
734  btDebugDrawLine(cp.getPositionWorldOnB(), cp.getPositionWorldOnB() + tr.getBasis() * (nB * 20), red);
735  }
736 #endif //DEBUG_INTERNAL_EDGE
737 
738  btScalar NdotA = localContactNormalOnB.dot(nA);
739  btScalar NdotB = localContactNormalOnB.dot(nB);
740  bool backFacingNormal = (NdotA < triangleInfoMapPtr->m_convexEpsilon) && (NdotB < triangleInfoMapPtr->m_convexEpsilon);
741 
742  if (backFacingNormal)
743  {
744  numConcaveEdgeHits++;
745  }
746  else
747  {
748  numConvexEdgeHits++;
749  // printf("hitting convex edge\n");
750 
751  btVector3 localContactNormalOnB = colObj0Wrap->getWorldTransform().getBasis().transpose() * cp.m_normalWorldOnB;
752  btVector3 clampedLocalNormal;
753  bool isClamped = btClampNormal(edge, swapFactor * tri_normal, localContactNormalOnB, info->m_edgeV2V0Angle, clampedLocalNormal);
754  if (isClamped)
755  {
756  if (((normalAdjustFlags & BT_TRIANGLE_CONVEX_DOUBLE_SIDED) != 0) || (clampedLocalNormal.dot(frontFacing * tri_normal) > 0))
757  {
758  btVector3 newNormal = colObj0Wrap->getWorldTransform().getBasis() * clampedLocalNormal;
759  // cp.m_distance1 = cp.m_distance1 * newNormal.dot(cp.m_normalWorldOnB);
760  cp.m_normalWorldOnB = newNormal;
761  // Reproject collision point along normal.
764  }
765  }
766  }
767  }
768  }
769  }
770 
771 #ifdef DEBUG_INTERNAL_EDGE
772  {
773  btVector3 color(0, 1, 1);
774  btDebugDrawLine(cp.getPositionWorldOnB(), cp.getPositionWorldOnB() + cp.m_normalWorldOnB * 10, color);
775  }
776 #endif //DEBUG_INTERNAL_EDGE
777 
778  if (isNearEdge)
779  {
780  if (numConcaveEdgeHits > 0)
781  {
782  if ((normalAdjustFlags & BT_TRIANGLE_CONCAVE_DOUBLE_SIDED) != 0)
783  {
784  //fix tri_normal so it pointing the same direction as the current local contact normal
785  if (tri_normal.dot(localContactNormalOnB) < 0)
786  {
787  tri_normal *= -1;
788  }
789  cp.m_normalWorldOnB = colObj0Wrap->getWorldTransform().getBasis() * tri_normal;
790  }
791  else
792  {
793  btVector3 newNormal = tri_normal * frontFacing;
794  //if the tri_normal is pointing opposite direction as the current local contact normal, skip it
795  btScalar d = newNormal.dot(localContactNormalOnB);
796  if (d < 0)
797  {
798  return;
799  }
800  //modify the normal to be the triangle normal (or backfacing normal)
801  cp.m_normalWorldOnB = colObj0Wrap->getWorldTransform().getBasis() * newNormal;
802  }
803 
804  // Reproject collision point along normal.
807  }
808  }
809 }
const btTriangleInfoMap * getTriangleInfoMap() const
void btNearestPointInLineSegment(const btVector3 &point, const btVector3 &line0, const btVector3 &line1, btVector3 &nearestPoint)
btScalar length(const btQuaternion &q)
Return the length of a quaternion.
Definition: btQuaternion.h:895
#define BT_LARGE_FLOAT
Definition: btScalar.h:296
#define TRI_INFO_V0V1_SWAP_NORMALB
void setValue(const btScalar &_x, const btScalar &_y, const btScalar &_z)
Definition: btVector3.h:640
btScalar m_edgeV1V2Angle
#define TRI_INFO_V1V2_CONVEX
const Value * find(const Key &key) const
Definition: btHashMap.h:424
btScalar length2() const
Return the length of the vector squared.
Definition: btVector3.h:251
virtual void drawLine(const btVector3 &from, const btVector3 &to, const btVector3 &color)=0
btScalar m_maxEdgeAngleThreshold
used to determine edge contacts: if the closest distance between a contact point and an edge is small...
const btScalar & getY() const
Return the y value.
Definition: btVector3.h:563
static int btGetHash(int partId, int triangleIndex)
ManifoldContactPoint collects and maintains persistent contactpoints.
const btCollisionShape * getCollisionShape() const
btScalar m_edgeV0V1Angle
btScalar m_equalVertexThreshold
used to determine if a triangle edge is planar with zero angle
#define TRI_INFO_V1V2_SWAP_NORMALB
btVector3 & normalize()
Normalize this vector x^2 + y^2 + z^2 = 1.
Definition: btVector3.h:303
bool fuzzyZero() const
Definition: btVector3.h:688
btVector3 quatRotate(const btQuaternion &rotation, const btVector3 &v)
Definition: btQuaternion.h:926
btMatrix3x3 transpose() const
Return the transpose of the matrix.
Definition: btMatrix3x3.h:1026
const btScalar & getZ() const
Return the z value.
Definition: btVector3.h:565
The btBvhTriangleMeshShape is a static-triangle mesh shape, it can only be used for fixed/non-moving ...
#define MAX_NUM_PARTS_IN_BITS
void btAdjustInternalEdgeContacts(btManifoldPoint &cp, const btCollisionObjectWrapper *colObj0Wrap, const btCollisionObjectWrapper *colObj1Wrap, int partId0, int index0, int normalAdjustFlags)
Changes a btManifoldPoint collision normal to the normal from the mesh.
#define SIMD_PI
Definition: btScalar.h:506
btVector3 m_normalWorldOnB
btVector3 m_positionWorldOnB
const btVector3 & getPositionWorldOnB() const
btTriangleInfoMap * m_triangleInfoMap
The btTriangleCallback provides a callback for each overlapping triangle when calling processAllTrian...
btVector3 cross(const btVector3 &v) const
Return the cross product between this and another vector.
Definition: btVector3.h:380
btScalar dot(const btVector3 &v) const
Return the dot product.
Definition: btVector3.h:229
const btCollisionObject * getCollisionObject() const
btScalar btAtan2(btScalar x, btScalar y)
Definition: btScalar.h:498
void btGenerateInternalEdgeInfo(btBvhTriangleMeshShape *trimeshShape, btTriangleInfoMap *triangleInfoMap)
Call btGenerateInternalEdgeInfo to create triangle info, store in the shape &#39;userInfo&#39;.
btMatrix3x3 & getBasis()
Return the basis matrix for the rotation.
Definition: btTransform.h:108
The btTriangleInfo structure stores information to adjust collision normals to avoid collisions again...
void insert(const Key &key, const Value &value)
Definition: btHashMap.h:264
The btIDebugDraw interface class allows hooking up a debug renderer to visually debug simulations...
Definition: btIDebugDraw.h:26
btVector3 m_positionWorldOnA
m_positionWorldOnA is redundant information, see getPositionWorldOnA(), but for clarity ...
btVector3 invXform(const btVector3 &inVec) const
Definition: btTransform.h:215
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:80
virtual int getNumSubParts() const =0
getNumSubParts returns the number of seperate subparts each subpart has a continuous array of vertice...
#define TRI_INFO_V0V1_CONVEX
for btTriangleInfo m_flags
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:28
#define TRI_INFO_V2V0_SWAP_NORMALB
The btStridingMeshInterface is the interface class for high performance generic access to triangle me...
btVector3 m_localPointB
virtual void processAllTriangles(btTriangleCallback *callback, const btVector3 &aabbMin, const btVector3 &aabbMax) const
const btVector3 & getScaling() const
btScalar m_edgeV2V0Angle
const btTransform & getWorldTransform() const
static btScalar btGetAngle(const btVector3 &edgeA, const btVector3 &normalA, const btVector3 &normalB)
#define TRI_INFO_V2V0_CONVEX
virtual void getLockedReadOnlyVertexIndexBase(const unsigned char **vertexbase, int &numverts, PHY_ScalarType &type, int &stride, const unsigned char **indexbase, int &indexstride, int &numfaces, PHY_ScalarType &indicestype, int subpart=0) const =0
bool btClampNormal(const btVector3 &edge, const btVector3 &tri_normal_org, const btVector3 &localContactNormalOnB, btScalar correctedEdgeAngle, btVector3 &clampedLocalNormal)
The btScaledBvhTriangleMeshShape allows to instance a scaled version of an existing btBvhTriangleMesh...
void setTriangleInfoMap(btTriangleInfoMap *triangleInfoMap)
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
Definition: btMatrix3x3.h:46
btScalar dot(const btQuaternion &q1, const btQuaternion &q2)
Calculate the dot product between two quaternions.
Definition: btQuaternion.h:888
int getShapeType() const
The btTriangleInfoMap stores edge angle information for some triangles. You can compute this informat...
virtual void getVertex(int index, btVector3 &vert) const
The btQuaternion implements quaternion to perform linear algebra rotations in combination with btMatr...
Definition: btQuaternion.h:49
btStridingMeshInterface * getMeshInterface()
void setMax(const btVector3 &other)
Set each element to the max of the current values and the values of another btVector3.
Definition: btVector3.h:609
void calcNormal(btVector3 &normal) const
const btScalar & getX() const
Return the x value.
Definition: btVector3.h:561
virtual void processTriangle(btVector3 *triangle, int partId, int triangleIndex)
void setMin(const btVector3 &other)
Set each element to the min of the current values and the values of another btVector3.
Definition: btVector3.h:626
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:294
const btCollisionShape * getCollisionShape() const
PHY_ScalarType
PHY_ScalarType enumerates possible scalar types.
btScalar btFabs(btScalar x)
Definition: btScalar.h:477