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
19static btIDebugDraw* gDebugDrawer = 0;
20
21void btSetDebugDrawer(btIDebugDraw* debugDrawer)
22{
23 gDebugDrawer = debugDrawer;
24}
25
26static 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
34static 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
42static 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
52struct 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 }
217
218
219
220
221
222 //alternatively use
223 //btVector3 calculatedNormalB2 = quatRotate(orn,normalA);
224
225
226 switch (sumvertsA)
227 {
228 case 1:
229 {
230 btVector3 edge = m_triangleVerticesA[0]-m_triangleVerticesA[1];
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;
237 info->m_flags |= TRI_INFO_V0V1_SWAP_NORMALB;
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 {
254 btVector3 edge = m_triangleVerticesA[2]-m_triangleVerticesA[0];
255 btQuaternion orn(edge,-correctedAngle);
256 btVector3 computedNormalB = quatRotate(orn,normalA);
257 if (computedNormalB.dot(normalB)<0)
258 {
259 computedNormalB*=-1;
260 info->m_flags |= TRI_INFO_V2V0_SWAP_NORMALB;
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 {
276 btVector3 edge = m_triangleVerticesA[1]-m_triangleVerticesA[2];
277 btQuaternion orn(edge,-correctedAngle);
278 btVector3 computedNormalB = quatRotate(orn,normalA);
279 if (computedNormalB.dot(normalB)<0)
280 {
281 info->m_flags |= TRI_INFO_V1V2_SWAP_NORMALB;
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
311void btGenerateInternalEdgeInfo (btBvhTriangleMeshShape*trimeshShape, btTriangleInfoMap* triangleInfoMap)
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;
326 PHY_ScalarType type = PHY_INTEGER;
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 }
360 aabbMin.setValue(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
361 aabbMax.setValue(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT));
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.
387void 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
413bool 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
449void btAdjustInternalEdgeContacts(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap,const btCollisionObjectWrapper* colObj1Wrap, int partId0, int index0, int normalAdjustFlags)
450{
451 //btAssert(colObj0->getCollisionShape()->getShapeType() == TRIANGLE_SHAPE_PROXYTYPE);
452 if (colObj0Wrap->getCollisionShape()->getShapeType() != TRIANGLE_SHAPE_PROXYTYPE)
453 return;
454
455 btBvhTriangleMeshShape* trimesh = 0;
456
457 if( colObj0Wrap->getCollisionObject()->getCollisionShape()->getShapeType() == SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE )
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);
583 if (info->m_flags & TRI_INFO_V0V1_SWAP_NORMALB)
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?)
616 cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1;
617 cp.m_localPointB = colObj0Wrap->getWorldTransform().invXform(cp.m_positionWorldOnB);
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);
671 if (info->m_flags & TRI_INFO_V1V2_SWAP_NORMALB)
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.
704 cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1;
705 cp.m_localPointB = colObj0Wrap->getWorldTransform().invXform(cp.m_positionWorldOnB);
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);
754 if (info->m_flags & TRI_INFO_V2V0_SWAP_NORMALB)
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.
789 cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1;
790 cp.m_localPointB = colObj0Wrap->getWorldTransform().invXform(cp.m_positionWorldOnB);
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.
834 cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1;
835 cp.m_localPointB = colObj0Wrap->getWorldTransform().invXform(cp.m_positionWorldOnB);
836 }
837 }
838}
SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE
@ SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE
Definition btBroadphaseProxy.h:56
TRIANGLE_SHAPE_PROXYTYPE
@ TRIANGLE_SHAPE_PROXYTYPE
Definition btBroadphaseProxy.h:32
PHY_ScalarType
PHY_ScalarType
PHY_ScalarType enumerates possible scalar types.
Definition btConcaveShape.h:25
PHY_FLOAT
@ PHY_FLOAT
Definition btConcaveShape.h:26
PHY_SHORT
@ PHY_SHORT
Definition btConcaveShape.h:29
PHY_INTEGER
@ PHY_INTEGER
Definition btConcaveShape.h:28
btGetAngle
static btScalar btGetAngle(const btVector3 &edgeA, const btVector3 &normalA, const btVector3 &normalB)
Definition btInternalEdgeUtility.cpp:42
btClampNormal
bool btClampNormal(const btVector3 &edge, const btVector3 &tri_normal_org, const btVector3 &localContactNormalOnB, btScalar correctedEdgeAngle, btVector3 &clampedLocalNormal)
Definition btInternalEdgeUtility.cpp:413
btAdjustInternalEdgeContacts
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.
Definition btInternalEdgeUtility.cpp:449
btGetHash
static int btGetHash(int partId, int triangleIndex)
Definition btInternalEdgeUtility.cpp:34
btGenerateInternalEdgeInfo
void btGenerateInternalEdgeInfo(btBvhTriangleMeshShape *trimeshShape, btTriangleInfoMap *triangleInfoMap)
Call btGenerateInternalEdgeInfo to create triangle info, store in the shape 'userInfo'.
Definition btInternalEdgeUtility.cpp:311
btNearestPointInLineSegment
void btNearestPointInLineSegment(const btVector3 &point, const btVector3 &line0, const btVector3 &line1, btVector3 &nearestPoint)
Definition btInternalEdgeUtility.cpp:387
BT_TRIANGLE_CONVEX_DOUBLE_SIDED
@ BT_TRIANGLE_CONVEX_DOUBLE_SIDED
Definition btInternalEdgeUtility.h:25
BT_TRIANGLE_CONVEX_BACKFACE_MODE
@ BT_TRIANGLE_CONVEX_BACKFACE_MODE
Definition btInternalEdgeUtility.h:23
BT_TRIANGLE_CONCAVE_DOUBLE_SIDED
@ BT_TRIANGLE_CONCAVE_DOUBLE_SIDED
Definition btInternalEdgeUtility.h:24
btMax
const T & btMax(const T &a, const T &b)
Definition btMinMax.h:29
MAX_NUM_PARTS_IN_BITS
#define MAX_NUM_PARTS_IN_BITS
Definition btQuantizedBvh.h:54
dot
btScalar dot(const btQuaternion &q1, const btQuaternion &q2)
Calculate the dot product between two quaternions.
Definition btQuaternion.h:878
quatRotate
btVector3 quatRotate(const btQuaternion &rotation, const btVector3 &v)
Definition btQuaternion.h:917
length
btScalar length(const btQuaternion &q)
Return the length of a quaternion.
Definition btQuaternion.h:886
SIMD_PI
#define SIMD_PI
Definition btScalar.h:504
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition btScalar.h:292
BT_LARGE_FLOAT
#define BT_LARGE_FLOAT
Definition btScalar.h:294
btAtan2
btScalar btAtan2(btScalar x, btScalar y)
Definition btScalar.h:496
btFabs
btScalar btFabs(btScalar x)
Definition btScalar.h:475
TRI_INFO_V1V2_CONVEX
#define TRI_INFO_V1V2_CONVEX
Definition btTriangleInfoMap.h:26
TRI_INFO_V0V1_CONVEX
#define TRI_INFO_V0V1_CONVEX
for btTriangleInfo m_flags
Definition btTriangleInfoMap.h:25
TRI_INFO_V2V0_CONVEX
#define TRI_INFO_V2V0_CONVEX
Definition btTriangleInfoMap.h:27
TRI_INFO_V2V0_SWAP_NORMALB
#define TRI_INFO_V2V0_SWAP_NORMALB
Definition btTriangleInfoMap.h:31
TRI_INFO_V1V2_SWAP_NORMALB
#define TRI_INFO_V1V2_SWAP_NORMALB
Definition btTriangleInfoMap.h:30
TRI_INFO_V0V1_SWAP_NORMALB
#define TRI_INFO_V0V1_SWAP_NORMALB
Definition btTriangleInfoMap.h:29
btBvhTriangleMeshShape
The btBvhTriangleMeshShape is a static-triangle mesh shape, it can only be used for fixed/non-moving ...
Definition btBvhTriangleMeshShape.h:35
btHashMap::insert
void insert(const Key &key, const Value &value)
Definition btHashMap.h:274
btHashMap::find
const Value * find(const Key &key) const
Definition btHashMap.h:434
btIDebugDraw
The btIDebugDraw interface class allows hooking up a debug renderer to visually debug simulations.
Definition btIDebugDraw.h:30
btIDebugDraw::drawLine
virtual void drawLine(const btVector3 &from, const btVector3 &to, const btVector3 &color)=0
btManifoldPoint
ManifoldContactPoint collects and maintains persistent contactpoints.
Definition btManifoldPoint.h:50
btMatrix3x3
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
Definition btMatrix3x3.h:48
btQuaternion
The btQuaternion implements quaternion to perform linear algebra rotations in combination with btMatr...
Definition btQuaternion.h:55
btScaledBvhTriangleMeshShape
The btScaledBvhTriangleMeshShape allows to instance a scaled version of an existing btBvhTriangleMesh...
Definition btScaledBvhTriangleMeshShape.h:25
btStridingMeshInterface
The btStridingMeshInterface is the interface class for high performance generic access to triangle me...
Definition btStridingMeshInterface.h:31
btTransform
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition btTransform.h:34
btTriangleCallback
The btTriangleCallback provides a callback for each overlapping triangle when calling processAllTrian...
Definition btTriangleCallback.h:25
btVector3
btVector3 can be used to represent 3D points and vectors.
Definition btVector3.h:84
btVector3::cross
btVector3 cross(const btVector3 &v) const
Return the cross product between this and another vector.
Definition btVector3.h:389
btVector3::dot
btScalar dot(const btVector3 &v) const
Return the dot product.
Definition btVector3.h:235
btVector3::length2
btScalar length2() const
Return the length of the vector squared.
Definition btVector3.h:257
btVector3::normalize
btVector3 & normalize()
Normalize this vector x^2 + y^2 + z^2 = 1.
Definition btVector3.h:309
btConnectivityProcessor::m_triangleInfoMap
btTriangleInfoMap * m_triangleInfoMap
Definition btInternalEdgeUtility.cpp:57
btConnectivityProcessor::processTriangle
virtual void processTriangle(btVector3 *triangle, int partId, int triangleIndex)
Definition btInternalEdgeUtility.cpp:60
btTriangleInfoMap
The btTriangleInfoMap stores edge angle information for some triangles. You can compute this informat...
Definition btTriangleInfoMap.h:59
btTriangleInfoMap::m_equalVertexThreshold
btScalar m_equalVertexThreshold
used to determine if a triangle edge is planar with zero angle
Definition btTriangleInfoMap.h:62
btTriangleInfo
The btTriangleInfo structure stores information to adjust collision normals to avoid collisions again...
Definition btTriangleInfoMap.h:37
Generated by doxygen 1.10.0