Bullet Collision Detection & Physics Library
btInternalEdgeUtility.cpp
Go to the documentation of this file.
1 #include "btInternalEdgeUtility.h"
2 
3 #include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h"
4 #include "BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.h"
5 #include "BulletCollision/CollisionShapes/btTriangleShape.h"
6 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
7 #include "BulletCollision/NarrowPhaseCollision/btManifoldPoint.h"
8 #include "LinearMath/btIDebugDraw.h"
9 #include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h"
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 
52 struct btConnectivityProcessor : public btTriangleCallback
53 {
54  int m_partIdA;
55  int m_triangleIndexA;
56  btVector3* m_triangleVerticesA;
57  btTriangleInfoMap* m_triangleInfoMap;
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 
141  int hash = btGetHash(m_partIdA,m_triangleIndexA);
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 
157  btTriangleShape tA(m_triangleVerticesA[0],m_triangleVerticesA[1],m_triangleVerticesA[2]);
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  btQuaternion orn2(calculatedEdge,-correctedAngle);
217  calculatedNormalB = btMatrix3x3(orn2)*normalA;
218 
219 
220  }
221 
222 
223 
224 
225 
226  //alternatively use
227  //btVector3 calculatedNormalB2 = quatRotate(orn,normalA);
228 
229 
230  switch (sumvertsA)
231  {
232  case 1:
233  {
234  btVector3 edge = m_triangleVerticesA[0]-m_triangleVerticesA[1];
235  btQuaternion orn(edge,-correctedAngle);
236  btVector3 computedNormalB = quatRotate(orn,normalA);
237  btScalar bla = computedNormalB.dot(normalB);
238  if (bla<0)
239  {
240  computedNormalB*=-1;
241  info->m_flags |= TRI_INFO_V0V1_SWAP_NORMALB;
242  }
243 #ifdef DEBUG_INTERNAL_EDGE
244  if ((computedNormalB-normalB).length()>0.0001)
245  {
246  printf("warning: normals not identical\n");
247  }
248 #endif//DEBUG_INTERNAL_EDGE
249 
250  info->m_edgeV0V1Angle = -correctedAngle;
251 
252  if (isConvex)
253  info->m_flags |= TRI_INFO_V0V1_CONVEX;
254  break;
255  }
256  case 2:
257  {
258  btVector3 edge = m_triangleVerticesA[2]-m_triangleVerticesA[0];
259  btQuaternion orn(edge,-correctedAngle);
260  btVector3 computedNormalB = quatRotate(orn,normalA);
261  if (computedNormalB.dot(normalB)<0)
262  {
263  computedNormalB*=-1;
264  info->m_flags |= TRI_INFO_V2V0_SWAP_NORMALB;
265  }
266 
267 #ifdef DEBUG_INTERNAL_EDGE
268  if ((computedNormalB-normalB).length()>0.0001)
269  {
270  printf("warning: normals not identical\n");
271  }
272 #endif //DEBUG_INTERNAL_EDGE
273  info->m_edgeV2V0Angle = -correctedAngle;
274  if (isConvex)
275  info->m_flags |= TRI_INFO_V2V0_CONVEX;
276  break;
277  }
278  case 3:
279  {
280  btVector3 edge = m_triangleVerticesA[1]-m_triangleVerticesA[2];
281  btQuaternion orn(edge,-correctedAngle);
282  btVector3 computedNormalB = quatRotate(orn,normalA);
283  if (computedNormalB.dot(normalB)<0)
284  {
285  info->m_flags |= TRI_INFO_V1V2_SWAP_NORMALB;
286  computedNormalB*=-1;
287  }
288 #ifdef DEBUG_INTERNAL_EDGE
289  if ((computedNormalB-normalB).length()>0.0001)
290  {
291  printf("warning: normals not identical\n");
292  }
293 #endif //DEBUG_INTERNAL_EDGE
294  info->m_edgeV1V2Angle = -correctedAngle;
295 
296  if (isConvex)
297  info->m_flags |= TRI_INFO_V1V2_CONVEX;
298  break;
299  }
300  }
301 
302  break;
303  }
304  default:
305  {
306  // printf("warning: duplicate triangle\n");
307  }
308 
309  }
310  }
311 };
314 
315 void btGenerateInternalEdgeInfo (btBvhTriangleMeshShape*trimeshShape, btTriangleInfoMap* triangleInfoMap)
316 {
317  //the user pointer shouldn't already be used for other purposes, we intend to store connectivity info there!
318  if (trimeshShape->getTriangleInfoMap())
319  return;
320 
321  trimeshShape->setTriangleInfoMap(triangleInfoMap);
322 
323  btStridingMeshInterface* meshInterface = trimeshShape->getMeshInterface();
324  const btVector3& meshScaling = meshInterface->getScaling();
325 
326  for (int partId = 0; partId< meshInterface->getNumSubParts();partId++)
327  {
328  const unsigned char *vertexbase = 0;
329  int numverts = 0;
330  PHY_ScalarType type = PHY_INTEGER;
331  int stride = 0;
332  const unsigned char *indexbase = 0;
333  int indexstride = 0;
334  int numfaces = 0;
335  PHY_ScalarType indicestype = PHY_INTEGER;
336  //PHY_ScalarType indexType=0;
337 
338  btVector3 triangleVerts[3];
339  meshInterface->getLockedReadOnlyVertexIndexBase(&vertexbase,numverts, type,stride,&indexbase,indexstride,numfaces,indicestype,partId);
340  btVector3 aabbMin,aabbMax;
341 
342  for (int triangleIndex = 0 ; triangleIndex < numfaces;triangleIndex++)
343  {
344  unsigned int* gfxbase = (unsigned int*)(indexbase+triangleIndex*indexstride);
345 
346  for (int j=2;j>=0;j--)
347  {
348 
349  int graphicsindex = indicestype==PHY_SHORT?((unsigned short*)gfxbase)[j]:gfxbase[j];
350  if (type == PHY_FLOAT)
351  {
352  float* graphicsbase = (float*)(vertexbase+graphicsindex*stride);
353  triangleVerts[j] = btVector3(
354  graphicsbase[0]*meshScaling.getX(),
355  graphicsbase[1]*meshScaling.getY(),
356  graphicsbase[2]*meshScaling.getZ());
357  }
358  else
359  {
360  double* graphicsbase = (double*)(vertexbase+graphicsindex*stride);
361  triangleVerts[j] = btVector3( btScalar(graphicsbase[0]*meshScaling.getX()), btScalar(graphicsbase[1]*meshScaling.getY()), btScalar(graphicsbase[2]*meshScaling.getZ()));
362  }
363  }
364  aabbMin.setValue(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
365  aabbMax.setValue(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT));
366  aabbMin.setMin(triangleVerts[0]);
367  aabbMax.setMax(triangleVerts[0]);
368  aabbMin.setMin(triangleVerts[1]);
369  aabbMax.setMax(triangleVerts[1]);
370  aabbMin.setMin(triangleVerts[2]);
371  aabbMax.setMax(triangleVerts[2]);
372 
373  btConnectivityProcessor connectivityProcessor;
374  connectivityProcessor.m_partIdA = partId;
375  connectivityProcessor.m_triangleIndexA = triangleIndex;
376  connectivityProcessor.m_triangleVerticesA = &triangleVerts[0];
377  connectivityProcessor.m_triangleInfoMap = triangleInfoMap;
378 
379  trimeshShape->processAllTriangles(&connectivityProcessor,aabbMin,aabbMax);
380  }
381 
382  }
383 
384 }
385 
386 
387 
388 
389 // Given a point and a line segment (defined by two points), compute the closest point
390 // in the line. Cap the point at the endpoints of the line segment.
391 void btNearestPointInLineSegment(const btVector3 &point, const btVector3& line0, const btVector3& line1, btVector3& nearestPoint)
392 {
393  btVector3 lineDelta = line1 - line0;
394 
395  // Handle degenerate lines
396  if ( lineDelta.fuzzyZero())
397  {
398  nearestPoint = line0;
399  }
400  else
401  {
402  btScalar delta = (point-line0).dot(lineDelta) / (lineDelta).dot(lineDelta);
403 
404  // Clamp the point to conform to the segment's endpoints
405  if ( delta < 0 )
406  delta = 0;
407  else if ( delta > 1 )
408  delta = 1;
409 
410  nearestPoint = line0 + lineDelta*delta;
411  }
412 }
413 
414 
415 
416 
417 bool btClampNormal(const btVector3& edge,const btVector3& tri_normal_org,const btVector3& localContactNormalOnB, btScalar correctedEdgeAngle, btVector3 & clampedLocalNormal)
418 {
419  btVector3 tri_normal = tri_normal_org;
420  //we only have a local triangle normal, not a local contact normal -> only normal in world space...
421  //either compute the current angle all in local space, or all in world space
422 
423  btVector3 edgeCross = edge.cross(tri_normal).normalize();
424  btScalar curAngle = btGetAngle(edgeCross,tri_normal,localContactNormalOnB);
425 
426  if (correctedEdgeAngle<0)
427  {
428  if (curAngle < correctedEdgeAngle)
429  {
430  btScalar diffAngle = correctedEdgeAngle-curAngle;
431  btQuaternion rotation(edge,diffAngle );
432  clampedLocalNormal = btMatrix3x3(rotation)*localContactNormalOnB;
433  return true;
434  }
435  }
436 
437  if (correctedEdgeAngle>=0)
438  {
439  if (curAngle > correctedEdgeAngle)
440  {
441  btScalar diffAngle = correctedEdgeAngle-curAngle;
442  btQuaternion rotation(edge,diffAngle );
443  clampedLocalNormal = btMatrix3x3(rotation)*localContactNormalOnB;
444  return true;
445  }
446  }
447  return false;
448 }
449 
450 
451 
453 void btAdjustInternalEdgeContacts(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap,const btCollisionObjectWrapper* colObj1Wrap, int partId0, int index0, int normalAdjustFlags)
454 {
455  //btAssert(colObj0->getCollisionShape()->getShapeType() == TRIANGLE_SHAPE_PROXYTYPE);
456  if (colObj0Wrap->getCollisionShape()->getShapeType() != TRIANGLE_SHAPE_PROXYTYPE)
457  return;
458 
459  btBvhTriangleMeshShape* trimesh = 0;
460 
461  if( colObj0Wrap->getCollisionObject()->getCollisionShape()->getShapeType() == SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE )
462  trimesh = ((btScaledBvhTriangleMeshShape*)colObj0Wrap->getCollisionObject()->getCollisionShape())->getChildShape();
463  else
464  trimesh = (btBvhTriangleMeshShape*)colObj0Wrap->getCollisionObject()->getCollisionShape();
465 
466  btTriangleInfoMap* triangleInfoMapPtr = (btTriangleInfoMap*) trimesh->getTriangleInfoMap();
467  if (!triangleInfoMapPtr)
468  return;
469 
470  int hash = btGetHash(partId0,index0);
471 
472 
473  btTriangleInfo* info = triangleInfoMapPtr->find(hash);
474  if (!info)
475  return;
476 
477  btScalar frontFacing = (normalAdjustFlags & BT_TRIANGLE_CONVEX_BACKFACE_MODE)==0? 1.f : -1.f;
478 
479  const btTriangleShape* tri_shape = static_cast<const btTriangleShape*>(colObj0Wrap->getCollisionShape());
480  btVector3 v0,v1,v2;
481  tri_shape->getVertex(0,v0);
482  tri_shape->getVertex(1,v1);
483  tri_shape->getVertex(2,v2);
484 
485  //btVector3 center = (v0+v1+v2)*btScalar(1./3.);
486 
487  btVector3 red(1,0,0), green(0,1,0),blue(0,0,1),white(1,1,1),black(0,0,0);
488  btVector3 tri_normal;
489  tri_shape->calcNormal(tri_normal);
490 
491  //btScalar dot = tri_normal.dot(cp.m_normalWorldOnB);
492  btVector3 nearest;
493  btNearestPointInLineSegment(cp.m_localPointB,v0,v1,nearest);
494 
495  btVector3 contact = cp.m_localPointB;
496 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
497  const btTransform& tr = colObj0->getWorldTransform();
498  btDebugDrawLine(tr*nearest,tr*cp.m_localPointB,red);
499 #endif //BT_INTERNAL_EDGE_DEBUG_DRAW
500 
501 
502 
503  bool isNearEdge = false;
504 
505  int numConcaveEdgeHits = 0;
506  int numConvexEdgeHits = 0;
507 
508  btVector3 localContactNormalOnB = colObj0Wrap->getWorldTransform().getBasis().transpose() * cp.m_normalWorldOnB;
509  localContactNormalOnB.normalize();//is this necessary?
510 
511  // Get closest edge
512  int bestedge=-1;
513  btScalar disttobestedge=BT_LARGE_FLOAT;
514  //
515  // Edge 0 -> 1
516  if (btFabs(info->m_edgeV0V1Angle)< triangleInfoMapPtr->m_maxEdgeAngleThreshold)
517  {
518  btVector3 nearest;
519  btNearestPointInLineSegment( cp.m_localPointB, v0, v1, nearest );
520  btScalar len=(contact-nearest).length();
521  //
522  if( len < disttobestedge )
523  {
524  bestedge=0;
525  disttobestedge=len;
526  }
527  }
528  // Edge 1 -> 2
529  if (btFabs(info->m_edgeV1V2Angle)< triangleInfoMapPtr->m_maxEdgeAngleThreshold)
530  {
531  btVector3 nearest;
532  btNearestPointInLineSegment( cp.m_localPointB, v1, v2, nearest );
533  btScalar len=(contact-nearest).length();
534  //
535  if( len < disttobestedge )
536  {
537  bestedge=1;
538  disttobestedge=len;
539  }
540  }
541  // Edge 2 -> 0
542  if (btFabs(info->m_edgeV2V0Angle)< triangleInfoMapPtr->m_maxEdgeAngleThreshold)
543  {
544  btVector3 nearest;
545  btNearestPointInLineSegment( cp.m_localPointB, v2, v0, nearest );
546  btScalar len=(contact-nearest).length();
547  //
548  if( len < disttobestedge )
549  {
550  bestedge=2;
551  disttobestedge=len;
552  }
553  }
554 
555 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
556  btVector3 upfix=tri_normal * btVector3(0.1f,0.1f,0.1f);
557  btDebugDrawLine(tr * v0 + upfix, tr * v1 + upfix, red );
558 #endif
559  if (btFabs(info->m_edgeV0V1Angle)< triangleInfoMapPtr->m_maxEdgeAngleThreshold)
560  {
561 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
562  btDebugDrawLine(tr*contact,tr*(contact+cp.m_normalWorldOnB*10),black);
563 #endif
564  btScalar len = (contact-nearest).length();
565  if(len<triangleInfoMapPtr->m_edgeDistanceThreshold)
566  if( bestedge==0 )
567  {
568  btVector3 edge(v0-v1);
569  isNearEdge = true;
570 
571  if (info->m_edgeV0V1Angle==btScalar(0))
572  {
573  numConcaveEdgeHits++;
574  } else
575  {
576 
577  bool isEdgeConvex = (info->m_flags & TRI_INFO_V0V1_CONVEX);
578  btScalar swapFactor = isEdgeConvex ? btScalar(1) : btScalar(-1);
579  #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
580  btDebugDrawLine(tr*nearest,tr*(nearest+swapFactor*tri_normal*10),white);
581  #endif //BT_INTERNAL_EDGE_DEBUG_DRAW
582 
583  btVector3 nA = swapFactor * tri_normal;
584 
585  btQuaternion orn(edge,info->m_edgeV0V1Angle);
586  btVector3 computedNormalB = quatRotate(orn,tri_normal);
587  if (info->m_flags & TRI_INFO_V0V1_SWAP_NORMALB)
588  computedNormalB*=-1;
589  btVector3 nB = swapFactor*computedNormalB;
590 
591  btScalar NdotA = localContactNormalOnB.dot(nA);
592  btScalar NdotB = localContactNormalOnB.dot(nB);
593  bool backFacingNormal = (NdotA< triangleInfoMapPtr->m_convexEpsilon) && (NdotB<triangleInfoMapPtr->m_convexEpsilon);
594 
595 #ifdef DEBUG_INTERNAL_EDGE
596  {
597 
598  btDebugDrawLine(cp.getPositionWorldOnB(),cp.getPositionWorldOnB()+tr.getBasis()*(nB*20),red);
599  }
600 #endif //DEBUG_INTERNAL_EDGE
601 
602 
603  if (backFacingNormal)
604  {
605  numConcaveEdgeHits++;
606  }
607  else
608  {
609  numConvexEdgeHits++;
610  btVector3 clampedLocalNormal;
611  bool isClamped = btClampNormal(edge,swapFactor*tri_normal,localContactNormalOnB, info->m_edgeV0V1Angle,clampedLocalNormal);
612  if (isClamped)
613  {
614  if (((normalAdjustFlags & BT_TRIANGLE_CONVEX_DOUBLE_SIDED)!=0) || (clampedLocalNormal.dot(frontFacing*tri_normal)>0))
615  {
616  btVector3 newNormal = colObj0Wrap->getWorldTransform().getBasis() * clampedLocalNormal;
617  // cp.m_distance1 = cp.m_distance1 * newNormal.dot(cp.m_normalWorldOnB);
618  cp.m_normalWorldOnB = newNormal;
619  // Reproject collision point along normal. (what about cp.m_distance1?)
620  cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1;
621  cp.m_localPointB = colObj0Wrap->getWorldTransform().invXform(cp.m_positionWorldOnB);
622 
623  }
624  }
625  }
626  }
627  }
628  }
629 
630  btNearestPointInLineSegment(contact,v1,v2,nearest);
631 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
632  btDebugDrawLine(tr*nearest,tr*cp.m_localPointB,green);
633 #endif //BT_INTERNAL_EDGE_DEBUG_DRAW
634 
635 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
636  btDebugDrawLine(tr * v1 + upfix, tr * v2 + upfix , green );
637 #endif
638 
639  if (btFabs(info->m_edgeV1V2Angle)< triangleInfoMapPtr->m_maxEdgeAngleThreshold)
640  {
641 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
642  btDebugDrawLine(tr*contact,tr*(contact+cp.m_normalWorldOnB*10),black);
643 #endif //BT_INTERNAL_EDGE_DEBUG_DRAW
644 
645 
646 
647  btScalar len = (contact-nearest).length();
648  if(len<triangleInfoMapPtr->m_edgeDistanceThreshold)
649  if( bestedge==1 )
650  {
651  isNearEdge = true;
652 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
653  btDebugDrawLine(tr*nearest,tr*(nearest+tri_normal*10),white);
654 #endif //BT_INTERNAL_EDGE_DEBUG_DRAW
655 
656  btVector3 edge(v1-v2);
657 
658  isNearEdge = true;
659 
660  if (info->m_edgeV1V2Angle == btScalar(0))
661  {
662  numConcaveEdgeHits++;
663  } else
664  {
665  bool isEdgeConvex = (info->m_flags & TRI_INFO_V1V2_CONVEX)!=0;
666  btScalar swapFactor = isEdgeConvex ? btScalar(1) : btScalar(-1);
667  #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
668  btDebugDrawLine(tr*nearest,tr*(nearest+swapFactor*tri_normal*10),white);
669  #endif //BT_INTERNAL_EDGE_DEBUG_DRAW
670 
671  btVector3 nA = swapFactor * tri_normal;
672 
673  btQuaternion orn(edge,info->m_edgeV1V2Angle);
674  btVector3 computedNormalB = quatRotate(orn,tri_normal);
675  if (info->m_flags & TRI_INFO_V1V2_SWAP_NORMALB)
676  computedNormalB*=-1;
677  btVector3 nB = swapFactor*computedNormalB;
678 
679 #ifdef DEBUG_INTERNAL_EDGE
680  {
681  btDebugDrawLine(cp.getPositionWorldOnB(),cp.getPositionWorldOnB()+tr.getBasis()*(nB*20),red);
682  }
683 #endif //DEBUG_INTERNAL_EDGE
684 
685 
686  btScalar NdotA = localContactNormalOnB.dot(nA);
687  btScalar NdotB = localContactNormalOnB.dot(nB);
688  bool backFacingNormal = (NdotA< triangleInfoMapPtr->m_convexEpsilon) && (NdotB<triangleInfoMapPtr->m_convexEpsilon);
689 
690  if (backFacingNormal)
691  {
692  numConcaveEdgeHits++;
693  }
694  else
695  {
696  numConvexEdgeHits++;
697  btVector3 localContactNormalOnB = colObj0Wrap->getWorldTransform().getBasis().transpose() * cp.m_normalWorldOnB;
698  btVector3 clampedLocalNormal;
699  bool isClamped = btClampNormal(edge,swapFactor*tri_normal,localContactNormalOnB, info->m_edgeV1V2Angle,clampedLocalNormal);
700  if (isClamped)
701  {
702  if (((normalAdjustFlags & BT_TRIANGLE_CONVEX_DOUBLE_SIDED)!=0) || (clampedLocalNormal.dot(frontFacing*tri_normal)>0))
703  {
704  btVector3 newNormal = colObj0Wrap->getWorldTransform().getBasis() * clampedLocalNormal;
705  // cp.m_distance1 = cp.m_distance1 * newNormal.dot(cp.m_normalWorldOnB);
706  cp.m_normalWorldOnB = newNormal;
707  // Reproject collision point along normal.
708  cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1;
709  cp.m_localPointB = colObj0Wrap->getWorldTransform().invXform(cp.m_positionWorldOnB);
710  }
711  }
712  }
713  }
714  }
715  }
716 
717  btNearestPointInLineSegment(contact,v2,v0,nearest);
718 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
719  btDebugDrawLine(tr*nearest,tr*cp.m_localPointB,blue);
720 #endif //BT_INTERNAL_EDGE_DEBUG_DRAW
721 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
722  btDebugDrawLine(tr * v2 + upfix, tr * v0 + upfix , blue );
723 #endif
724 
725  if (btFabs(info->m_edgeV2V0Angle)< triangleInfoMapPtr->m_maxEdgeAngleThreshold)
726  {
727 
728 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
729  btDebugDrawLine(tr*contact,tr*(contact+cp.m_normalWorldOnB*10),black);
730 #endif //BT_INTERNAL_EDGE_DEBUG_DRAW
731 
732  btScalar len = (contact-nearest).length();
733  if(len<triangleInfoMapPtr->m_edgeDistanceThreshold)
734  if( bestedge==2 )
735  {
736  isNearEdge = true;
737 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
738  btDebugDrawLine(tr*nearest,tr*(nearest+tri_normal*10),white);
739 #endif //BT_INTERNAL_EDGE_DEBUG_DRAW
740 
741  btVector3 edge(v2-v0);
742 
743  if (info->m_edgeV2V0Angle==btScalar(0))
744  {
745  numConcaveEdgeHits++;
746  } else
747  {
748 
749  bool isEdgeConvex = (info->m_flags & TRI_INFO_V2V0_CONVEX)!=0;
750  btScalar swapFactor = isEdgeConvex ? btScalar(1) : btScalar(-1);
751  #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
752  btDebugDrawLine(tr*nearest,tr*(nearest+swapFactor*tri_normal*10),white);
753  #endif //BT_INTERNAL_EDGE_DEBUG_DRAW
754 
755  btVector3 nA = swapFactor * tri_normal;
756  btQuaternion orn(edge,info->m_edgeV2V0Angle);
757  btVector3 computedNormalB = quatRotate(orn,tri_normal);
758  if (info->m_flags & TRI_INFO_V2V0_SWAP_NORMALB)
759  computedNormalB*=-1;
760  btVector3 nB = swapFactor*computedNormalB;
761 
762 #ifdef DEBUG_INTERNAL_EDGE
763  {
764  btDebugDrawLine(cp.getPositionWorldOnB(),cp.getPositionWorldOnB()+tr.getBasis()*(nB*20),red);
765  }
766 #endif //DEBUG_INTERNAL_EDGE
767 
768  btScalar NdotA = localContactNormalOnB.dot(nA);
769  btScalar NdotB = localContactNormalOnB.dot(nB);
770  bool backFacingNormal = (NdotA< triangleInfoMapPtr->m_convexEpsilon) && (NdotB<triangleInfoMapPtr->m_convexEpsilon);
771 
772  if (backFacingNormal)
773  {
774  numConcaveEdgeHits++;
775  }
776  else
777  {
778  numConvexEdgeHits++;
779  // printf("hitting convex edge\n");
780 
781 
782  btVector3 localContactNormalOnB = colObj0Wrap->getWorldTransform().getBasis().transpose() * cp.m_normalWorldOnB;
783  btVector3 clampedLocalNormal;
784  bool isClamped = btClampNormal(edge,swapFactor*tri_normal,localContactNormalOnB,info->m_edgeV2V0Angle,clampedLocalNormal);
785  if (isClamped)
786  {
787  if (((normalAdjustFlags & BT_TRIANGLE_CONVEX_DOUBLE_SIDED)!=0) || (clampedLocalNormal.dot(frontFacing*tri_normal)>0))
788  {
789  btVector3 newNormal = colObj0Wrap->getWorldTransform().getBasis() * clampedLocalNormal;
790  // cp.m_distance1 = cp.m_distance1 * newNormal.dot(cp.m_normalWorldOnB);
791  cp.m_normalWorldOnB = newNormal;
792  // Reproject collision point along normal.
793  cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1;
794  cp.m_localPointB = colObj0Wrap->getWorldTransform().invXform(cp.m_positionWorldOnB);
795  }
796  }
797  }
798  }
799 
800 
801  }
802  }
803 
804 #ifdef DEBUG_INTERNAL_EDGE
805  {
806  btVector3 color(0,1,1);
807  btDebugDrawLine(cp.getPositionWorldOnB(),cp.getPositionWorldOnB()+cp.m_normalWorldOnB*10,color);
808  }
809 #endif //DEBUG_INTERNAL_EDGE
810 
811  if (isNearEdge)
812  {
813 
814  if (numConcaveEdgeHits>0)
815  {
816  if ((normalAdjustFlags & BT_TRIANGLE_CONCAVE_DOUBLE_SIDED)!=0)
817  {
818  //fix tri_normal so it pointing the same direction as the current local contact normal
819  if (tri_normal.dot(localContactNormalOnB) < 0)
820  {
821  tri_normal *= -1;
822  }
823  cp.m_normalWorldOnB = colObj0Wrap->getWorldTransform().getBasis()*tri_normal;
824  } else
825  {
826  btVector3 newNormal = tri_normal *frontFacing;
827  //if the tri_normal is pointing opposite direction as the current local contact normal, skip it
828  btScalar d = newNormal.dot(localContactNormalOnB) ;
829  if (d< 0)
830  {
831  return;
832  }
833  //modify the normal to be the triangle normal (or backfacing normal)
834  cp.m_normalWorldOnB = colObj0Wrap->getWorldTransform().getBasis() *newNormal;
835  }
836 
837  // Reproject collision point along normal.
838  cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1;
839  cp.m_localPointB = colObj0Wrap->getWorldTransform().invXform(cp.m_positionWorldOnB);
840  }
841  }
842 }
Generated on Thu Oct 3 2013 18:25:06 for Bullet Collision Detection & Physics Library by   doxygen 1.8.4