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