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 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 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 452 /// Changes a btManifoldPoint collision normal to the normal from the mesh. 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
