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
btSetDebugDrawer(btIDebugDraw * debugDrawer)21 void btSetDebugDrawer(btIDebugDraw* debugDrawer)
22 {
23 gDebugDrawer = debugDrawer;
24 }
25
btDebugDrawLine(const btVector3 & from,const btVector3 & to,const btVector3 & color)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
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
40
41
btGetAngle(const btVector3 & edgeA,const btVector3 & normalA,const btVector3 & normalB)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
processTrianglebtConnectivityProcessor60 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
75 ///skip degenerate triangles
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();
82 ///skip degenerate triangles
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++;
108 ///degenerate case
109 if(numshared >= 3)
110 return;
111 }
112 }
113 ///degenerate case
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);
212 ///@todo: check if we need some epsilon, due to floating point imprecision
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 };
312 /////////////////////////////////////////////////////////
313 /////////////////////////////////////////////////////////
314
btGenerateInternalEdgeInfo(btBvhTriangleMeshShape * trimeshShape,btTriangleInfoMap * triangleInfoMap)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.
btNearestPointInLineSegment(const btVector3 & point,const btVector3 & line0,const btVector3 & line1,btVector3 & nearestPoint)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
btClampNormal(const btVector3 & edge,const btVector3 & tri_normal_org,const btVector3 & localContactNormalOnB,btScalar correctedEdgeAngle,btVector3 & clampedLocalNormal)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
452 /// 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)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 }
843