00001 #include "btInternalEdgeUtility.h"
00002
00003 #include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h"
00004 #include "BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.h"
00005 #include "BulletCollision/CollisionShapes/btTriangleShape.h"
00006 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
00007 #include "BulletCollision/NarrowPhaseCollision/btManifoldPoint.h"
00008 #include "LinearMath/btIDebugDraw.h"
00009 #include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h"
00010
00011
00012
00013 #ifdef DEBUG_INTERNAL_EDGE
00014 #include <stdio.h>
00015 #endif //DEBUG_INTERNAL_EDGE
00016
00017
00018 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
00019 static btIDebugDraw* gDebugDrawer = 0;
00020
00021 void btSetDebugDrawer(btIDebugDraw* debugDrawer)
00022 {
00023 gDebugDrawer = debugDrawer;
00024 }
00025
00026 static void btDebugDrawLine(const btVector3& from,const btVector3& to, const btVector3& color)
00027 {
00028 if (gDebugDrawer)
00029 gDebugDrawer->drawLine(from,to,color);
00030 }
00031 #endif //BT_INTERNAL_EDGE_DEBUG_DRAW
00032
00033
00034 static int btGetHash(int partId, int triangleIndex)
00035 {
00036 int hash = (partId<<(31-MAX_NUM_PARTS_IN_BITS)) | triangleIndex;
00037 return hash;
00038 }
00039
00040
00041
00042 static btScalar btGetAngle(const btVector3& edgeA, const btVector3& normalA,const btVector3& normalB)
00043 {
00044 const btVector3 refAxis0 = edgeA;
00045 const btVector3 refAxis1 = normalA;
00046 const btVector3 swingAxis = normalB;
00047 btScalar angle = btAtan2(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1));
00048 return angle;
00049 }
00050
00051
00052 struct btConnectivityProcessor : public btTriangleCallback
00053 {
00054 int m_partIdA;
00055 int m_triangleIndexA;
00056 btVector3* m_triangleVerticesA;
00057 btTriangleInfoMap* m_triangleInfoMap;
00058
00059
00060 virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex)
00061 {
00062
00063 if ((m_partIdA == partId) && (m_triangleIndexA == triangleIndex))
00064 return;
00065
00066
00067
00068
00069
00070
00071 int numshared = 0;
00072 int sharedVertsA[3]={-1,-1,-1};
00073 int sharedVertsB[3]={-1,-1,-1};
00074
00076 btScalar crossBSqr = ((triangle[1]-triangle[0]).cross(triangle[2]-triangle[0])).length2();
00077 if (crossBSqr < m_triangleInfoMap->m_equalVertexThreshold)
00078 return;
00079
00080
00081 btScalar crossASqr = ((m_triangleVerticesA[1]-m_triangleVerticesA[0]).cross(m_triangleVerticesA[2]-m_triangleVerticesA[0])).length2();
00083 if (crossASqr< m_triangleInfoMap->m_equalVertexThreshold)
00084 return;
00085
00086 #if 0
00087 printf("triangle A[0] = (%f,%f,%f)\ntriangle A[1] = (%f,%f,%f)\ntriangle A[2] = (%f,%f,%f)\n",
00088 m_triangleVerticesA[0].getX(),m_triangleVerticesA[0].getY(),m_triangleVerticesA[0].getZ(),
00089 m_triangleVerticesA[1].getX(),m_triangleVerticesA[1].getY(),m_triangleVerticesA[1].getZ(),
00090 m_triangleVerticesA[2].getX(),m_triangleVerticesA[2].getY(),m_triangleVerticesA[2].getZ());
00091
00092 printf("partId=%d, triangleIndex=%d\n",partId,triangleIndex);
00093 printf("triangle B[0] = (%f,%f,%f)\ntriangle B[1] = (%f,%f,%f)\ntriangle B[2] = (%f,%f,%f)\n",
00094 triangle[0].getX(),triangle[0].getY(),triangle[0].getZ(),
00095 triangle[1].getX(),triangle[1].getY(),triangle[1].getZ(),
00096 triangle[2].getX(),triangle[2].getY(),triangle[2].getZ());
00097 #endif
00098
00099 for (int i=0;i<3;i++)
00100 {
00101 for (int j=0;j<3;j++)
00102 {
00103 if ( (m_triangleVerticesA[i]-triangle[j]).length2() < m_triangleInfoMap->m_equalVertexThreshold)
00104 {
00105 sharedVertsA[numshared] = i;
00106 sharedVertsB[numshared] = j;
00107 numshared++;
00109 if(numshared >= 3)
00110 return;
00111 }
00112 }
00114 if(numshared >= 3)
00115 return;
00116 }
00117 switch (numshared)
00118 {
00119 case 0:
00120 {
00121 break;
00122 }
00123 case 1:
00124 {
00125
00126 break;
00127 }
00128 case 2:
00129 {
00130
00131
00132 if (sharedVertsA[0] == 0 && sharedVertsA[1] == 2)
00133 {
00134 sharedVertsA[0] = 2;
00135 sharedVertsA[1] = 0;
00136 int tmp = sharedVertsB[1];
00137 sharedVertsB[1] = sharedVertsB[0];
00138 sharedVertsB[0] = tmp;
00139 }
00140
00141 int hash = btGetHash(m_partIdA,m_triangleIndexA);
00142
00143 btTriangleInfo* info = m_triangleInfoMap->find(hash);
00144 if (!info)
00145 {
00146 btTriangleInfo tmp;
00147 m_triangleInfoMap->insert(hash,tmp);
00148 info = m_triangleInfoMap->find(hash);
00149 }
00150
00151 int sumvertsA = sharedVertsA[0]+sharedVertsA[1];
00152 int otherIndexA = 3-sumvertsA;
00153
00154
00155 btVector3 edge(m_triangleVerticesA[sharedVertsA[1]]-m_triangleVerticesA[sharedVertsA[0]]);
00156
00157 btTriangleShape tA(m_triangleVerticesA[0],m_triangleVerticesA[1],m_triangleVerticesA[2]);
00158 int otherIndexB = 3-(sharedVertsB[0]+sharedVertsB[1]);
00159
00160 btTriangleShape tB(triangle[sharedVertsB[1]],triangle[sharedVertsB[0]],triangle[otherIndexB]);
00161
00162
00163 btVector3 normalA;
00164 btVector3 normalB;
00165 tA.calcNormal(normalA);
00166 tB.calcNormal(normalB);
00167 edge.normalize();
00168 btVector3 edgeCrossA = edge.cross(normalA).normalize();
00169
00170 {
00171 btVector3 tmp = m_triangleVerticesA[otherIndexA]-m_triangleVerticesA[sharedVertsA[0]];
00172 if (edgeCrossA.dot(tmp) < 0)
00173 {
00174 edgeCrossA*=-1;
00175 }
00176 }
00177
00178 btVector3 edgeCrossB = edge.cross(normalB).normalize();
00179
00180 {
00181 btVector3 tmp = triangle[otherIndexB]-triangle[sharedVertsB[0]];
00182 if (edgeCrossB.dot(tmp) < 0)
00183 {
00184 edgeCrossB*=-1;
00185 }
00186 }
00187
00188 btScalar angle2 = 0;
00189 btScalar ang4 = 0.f;
00190
00191
00192 btVector3 calculatedEdge = edgeCrossA.cross(edgeCrossB);
00193 btScalar len2 = calculatedEdge.length2();
00194
00195 btScalar correctedAngle(0);
00196 btVector3 calculatedNormalB = normalA;
00197 bool isConvex = false;
00198
00199 if (len2<m_triangleInfoMap->m_planarEpsilon)
00200 {
00201 angle2 = 0.f;
00202 ang4 = 0.f;
00203 } else
00204 {
00205
00206 calculatedEdge.normalize();
00207 btVector3 calculatedNormalA = calculatedEdge.cross(edgeCrossA);
00208 calculatedNormalA.normalize();
00209 angle2 = btGetAngle(calculatedNormalA,edgeCrossA,edgeCrossB);
00210 ang4 = SIMD_PI-angle2;
00211 btScalar dotA = normalA.dot(edgeCrossB);
00213 isConvex = (dotA<0.);
00214
00215 correctedAngle = isConvex ? ang4 : -ang4;
00216 btQuaternion orn2(calculatedEdge,-correctedAngle);
00217 calculatedNormalB = btMatrix3x3(orn2)*normalA;
00218
00219
00220 }
00221
00222
00223
00224
00225
00226
00227
00228
00229
00230 switch (sumvertsA)
00231 {
00232 case 1:
00233 {
00234 btVector3 edge = m_triangleVerticesA[0]-m_triangleVerticesA[1];
00235 btQuaternion orn(edge,-correctedAngle);
00236 btVector3 computedNormalB = quatRotate(orn,normalA);
00237 btScalar bla = computedNormalB.dot(normalB);
00238 if (bla<0)
00239 {
00240 computedNormalB*=-1;
00241 info->m_flags |= TRI_INFO_V0V1_SWAP_NORMALB;
00242 }
00243 #ifdef DEBUG_INTERNAL_EDGE
00244 if ((computedNormalB-normalB).length()>0.0001)
00245 {
00246 printf("warning: normals not identical\n");
00247 }
00248 #endif//DEBUG_INTERNAL_EDGE
00249
00250 info->m_edgeV0V1Angle = -correctedAngle;
00251
00252 if (isConvex)
00253 info->m_flags |= TRI_INFO_V0V1_CONVEX;
00254 break;
00255 }
00256 case 2:
00257 {
00258 btVector3 edge = m_triangleVerticesA[2]-m_triangleVerticesA[0];
00259 btQuaternion orn(edge,-correctedAngle);
00260 btVector3 computedNormalB = quatRotate(orn,normalA);
00261 if (computedNormalB.dot(normalB)<0)
00262 {
00263 computedNormalB*=-1;
00264 info->m_flags |= TRI_INFO_V2V0_SWAP_NORMALB;
00265 }
00266
00267 #ifdef DEBUG_INTERNAL_EDGE
00268 if ((computedNormalB-normalB).length()>0.0001)
00269 {
00270 printf("warning: normals not identical\n");
00271 }
00272 #endif //DEBUG_INTERNAL_EDGE
00273 info->m_edgeV2V0Angle = -correctedAngle;
00274 if (isConvex)
00275 info->m_flags |= TRI_INFO_V2V0_CONVEX;
00276 break;
00277 }
00278 case 3:
00279 {
00280 btVector3 edge = m_triangleVerticesA[1]-m_triangleVerticesA[2];
00281 btQuaternion orn(edge,-correctedAngle);
00282 btVector3 computedNormalB = quatRotate(orn,normalA);
00283 if (computedNormalB.dot(normalB)<0)
00284 {
00285 info->m_flags |= TRI_INFO_V1V2_SWAP_NORMALB;
00286 computedNormalB*=-1;
00287 }
00288 #ifdef DEBUG_INTERNAL_EDGE
00289 if ((computedNormalB-normalB).length()>0.0001)
00290 {
00291 printf("warning: normals not identical\n");
00292 }
00293 #endif //DEBUG_INTERNAL_EDGE
00294 info->m_edgeV1V2Angle = -correctedAngle;
00295
00296 if (isConvex)
00297 info->m_flags |= TRI_INFO_V1V2_CONVEX;
00298 break;
00299 }
00300 }
00301
00302 break;
00303 }
00304 default:
00305 {
00306
00307 }
00308
00309 }
00310 }
00311 };
00314
00315 void btGenerateInternalEdgeInfo (btBvhTriangleMeshShape*trimeshShape, btTriangleInfoMap* triangleInfoMap)
00316 {
00317
00318 if (trimeshShape->getTriangleInfoMap())
00319 return;
00320
00321 trimeshShape->setTriangleInfoMap(triangleInfoMap);
00322
00323 btStridingMeshInterface* meshInterface = trimeshShape->getMeshInterface();
00324 const btVector3& meshScaling = meshInterface->getScaling();
00325
00326 for (int partId = 0; partId< meshInterface->getNumSubParts();partId++)
00327 {
00328 const unsigned char *vertexbase = 0;
00329 int numverts = 0;
00330 PHY_ScalarType type = PHY_INTEGER;
00331 int stride = 0;
00332 const unsigned char *indexbase = 0;
00333 int indexstride = 0;
00334 int numfaces = 0;
00335 PHY_ScalarType indicestype = PHY_INTEGER;
00336
00337
00338 btVector3 triangleVerts[3];
00339 meshInterface->getLockedReadOnlyVertexIndexBase(&vertexbase,numverts, type,stride,&indexbase,indexstride,numfaces,indicestype,partId);
00340 btVector3 aabbMin,aabbMax;
00341
00342 for (int triangleIndex = 0 ; triangleIndex < numfaces;triangleIndex++)
00343 {
00344 unsigned int* gfxbase = (unsigned int*)(indexbase+triangleIndex*indexstride);
00345
00346 for (int j=2;j>=0;j--)
00347 {
00348
00349 int graphicsindex = indicestype==PHY_SHORT?((unsigned short*)gfxbase)[j]:gfxbase[j];
00350 if (type == PHY_FLOAT)
00351 {
00352 float* graphicsbase = (float*)(vertexbase+graphicsindex*stride);
00353 triangleVerts[j] = btVector3(
00354 graphicsbase[0]*meshScaling.getX(),
00355 graphicsbase[1]*meshScaling.getY(),
00356 graphicsbase[2]*meshScaling.getZ());
00357 }
00358 else
00359 {
00360 double* graphicsbase = (double*)(vertexbase+graphicsindex*stride);
00361 triangleVerts[j] = btVector3( btScalar(graphicsbase[0]*meshScaling.getX()), btScalar(graphicsbase[1]*meshScaling.getY()), btScalar(graphicsbase[2]*meshScaling.getZ()));
00362 }
00363 }
00364 aabbMin.setValue(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
00365 aabbMax.setValue(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT));
00366 aabbMin.setMin(triangleVerts[0]);
00367 aabbMax.setMax(triangleVerts[0]);
00368 aabbMin.setMin(triangleVerts[1]);
00369 aabbMax.setMax(triangleVerts[1]);
00370 aabbMin.setMin(triangleVerts[2]);
00371 aabbMax.setMax(triangleVerts[2]);
00372
00373 btConnectivityProcessor connectivityProcessor;
00374 connectivityProcessor.m_partIdA = partId;
00375 connectivityProcessor.m_triangleIndexA = triangleIndex;
00376 connectivityProcessor.m_triangleVerticesA = &triangleVerts[0];
00377 connectivityProcessor.m_triangleInfoMap = triangleInfoMap;
00378
00379 trimeshShape->processAllTriangles(&connectivityProcessor,aabbMin,aabbMax);
00380 }
00381
00382 }
00383
00384 }
00385
00386
00387
00388
00389
00390
00391 void btNearestPointInLineSegment(const btVector3 &point, const btVector3& line0, const btVector3& line1, btVector3& nearestPoint)
00392 {
00393 btVector3 lineDelta = line1 - line0;
00394
00395
00396 if ( lineDelta.fuzzyZero())
00397 {
00398 nearestPoint = line0;
00399 }
00400 else
00401 {
00402 btScalar delta = (point-line0).dot(lineDelta) / (lineDelta).dot(lineDelta);
00403
00404
00405 if ( delta < 0 )
00406 delta = 0;
00407 else if ( delta > 1 )
00408 delta = 1;
00409
00410 nearestPoint = line0 + lineDelta*delta;
00411 }
00412 }
00413
00414
00415
00416
00417 bool btClampNormal(const btVector3& edge,const btVector3& tri_normal_org,const btVector3& localContactNormalOnB, btScalar correctedEdgeAngle, btVector3 & clampedLocalNormal)
00418 {
00419 btVector3 tri_normal = tri_normal_org;
00420
00421
00422
00423 btVector3 edgeCross = edge.cross(tri_normal).normalize();
00424 btScalar curAngle = btGetAngle(edgeCross,tri_normal,localContactNormalOnB);
00425
00426 if (correctedEdgeAngle<0)
00427 {
00428 if (curAngle < correctedEdgeAngle)
00429 {
00430 btScalar diffAngle = correctedEdgeAngle-curAngle;
00431 btQuaternion rotation(edge,diffAngle );
00432 clampedLocalNormal = btMatrix3x3(rotation)*localContactNormalOnB;
00433 return true;
00434 }
00435 }
00436
00437 if (correctedEdgeAngle>=0)
00438 {
00439 if (curAngle > correctedEdgeAngle)
00440 {
00441 btScalar diffAngle = correctedEdgeAngle-curAngle;
00442 btQuaternion rotation(edge,diffAngle );
00443 clampedLocalNormal = btMatrix3x3(rotation)*localContactNormalOnB;
00444 return true;
00445 }
00446 }
00447 return false;
00448 }
00449
00450
00451
00453 void btAdjustInternalEdgeContacts(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap,const btCollisionObjectWrapper* colObj1Wrap, int partId0, int index0, int normalAdjustFlags)
00454 {
00455
00456 if (colObj0Wrap->getCollisionShape()->getShapeType() != TRIANGLE_SHAPE_PROXYTYPE)
00457 return;
00458
00459 btBvhTriangleMeshShape* trimesh = 0;
00460
00461 if( colObj0Wrap->getCollisionObject()->getCollisionShape()->getShapeType() == SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE )
00462 trimesh = ((btScaledBvhTriangleMeshShape*)colObj0Wrap->getCollisionObject()->getCollisionShape())->getChildShape();
00463 else
00464 trimesh = (btBvhTriangleMeshShape*)colObj0Wrap->getCollisionObject()->getCollisionShape();
00465
00466 btTriangleInfoMap* triangleInfoMapPtr = (btTriangleInfoMap*) trimesh->getTriangleInfoMap();
00467 if (!triangleInfoMapPtr)
00468 return;
00469
00470 int hash = btGetHash(partId0,index0);
00471
00472
00473 btTriangleInfo* info = triangleInfoMapPtr->find(hash);
00474 if (!info)
00475 return;
00476
00477 btScalar frontFacing = (normalAdjustFlags & BT_TRIANGLE_CONVEX_BACKFACE_MODE)==0? 1.f : -1.f;
00478
00479 const btTriangleShape* tri_shape = static_cast<const btTriangleShape*>(colObj0Wrap->getCollisionShape());
00480 btVector3 v0,v1,v2;
00481 tri_shape->getVertex(0,v0);
00482 tri_shape->getVertex(1,v1);
00483 tri_shape->getVertex(2,v2);
00484
00485
00486
00487 btVector3 red(1,0,0), green(0,1,0),blue(0,0,1),white(1,1,1),black(0,0,0);
00488 btVector3 tri_normal;
00489 tri_shape->calcNormal(tri_normal);
00490
00491
00492 btVector3 nearest;
00493 btNearestPointInLineSegment(cp.m_localPointB,v0,v1,nearest);
00494
00495 btVector3 contact = cp.m_localPointB;
00496 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
00497 const btTransform& tr = colObj0->getWorldTransform();
00498 btDebugDrawLine(tr*nearest,tr*cp.m_localPointB,red);
00499 #endif //BT_INTERNAL_EDGE_DEBUG_DRAW
00500
00501
00502
00503 bool isNearEdge = false;
00504
00505 int numConcaveEdgeHits = 0;
00506 int numConvexEdgeHits = 0;
00507
00508 btVector3 localContactNormalOnB = colObj0Wrap->getWorldTransform().getBasis().transpose() * cp.m_normalWorldOnB;
00509 localContactNormalOnB.normalize();
00510
00511
00512 int bestedge=-1;
00513 btScalar disttobestedge=BT_LARGE_FLOAT;
00514
00515
00516 if (btFabs(info->m_edgeV0V1Angle)< triangleInfoMapPtr->m_maxEdgeAngleThreshold)
00517 {
00518 btVector3 nearest;
00519 btNearestPointInLineSegment( cp.m_localPointB, v0, v1, nearest );
00520 btScalar len=(contact-nearest).length();
00521
00522 if( len < disttobestedge )
00523 {
00524 bestedge=0;
00525 disttobestedge=len;
00526 }
00527 }
00528
00529 if (btFabs(info->m_edgeV1V2Angle)< triangleInfoMapPtr->m_maxEdgeAngleThreshold)
00530 {
00531 btVector3 nearest;
00532 btNearestPointInLineSegment( cp.m_localPointB, v1, v2, nearest );
00533 btScalar len=(contact-nearest).length();
00534
00535 if( len < disttobestedge )
00536 {
00537 bestedge=1;
00538 disttobestedge=len;
00539 }
00540 }
00541
00542 if (btFabs(info->m_edgeV2V0Angle)< triangleInfoMapPtr->m_maxEdgeAngleThreshold)
00543 {
00544 btVector3 nearest;
00545 btNearestPointInLineSegment( cp.m_localPointB, v2, v0, nearest );
00546 btScalar len=(contact-nearest).length();
00547
00548 if( len < disttobestedge )
00549 {
00550 bestedge=2;
00551 disttobestedge=len;
00552 }
00553 }
00554
00555 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
00556 btVector3 upfix=tri_normal * btVector3(0.1f,0.1f,0.1f);
00557 btDebugDrawLine(tr * v0 + upfix, tr * v1 + upfix, red );
00558 #endif
00559 if (btFabs(info->m_edgeV0V1Angle)< triangleInfoMapPtr->m_maxEdgeAngleThreshold)
00560 {
00561 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
00562 btDebugDrawLine(tr*contact,tr*(contact+cp.m_normalWorldOnB*10),black);
00563 #endif
00564 btScalar len = (contact-nearest).length();
00565 if(len<triangleInfoMapPtr->m_edgeDistanceThreshold)
00566 if( bestedge==0 )
00567 {
00568 btVector3 edge(v0-v1);
00569 isNearEdge = true;
00570
00571 if (info->m_edgeV0V1Angle==btScalar(0))
00572 {
00573 numConcaveEdgeHits++;
00574 } else
00575 {
00576
00577 bool isEdgeConvex = (info->m_flags & TRI_INFO_V0V1_CONVEX);
00578 btScalar swapFactor = isEdgeConvex ? btScalar(1) : btScalar(-1);
00579 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
00580 btDebugDrawLine(tr*nearest,tr*(nearest+swapFactor*tri_normal*10),white);
00581 #endif //BT_INTERNAL_EDGE_DEBUG_DRAW
00582
00583 btVector3 nA = swapFactor * tri_normal;
00584
00585 btQuaternion orn(edge,info->m_edgeV0V1Angle);
00586 btVector3 computedNormalB = quatRotate(orn,tri_normal);
00587 if (info->m_flags & TRI_INFO_V0V1_SWAP_NORMALB)
00588 computedNormalB*=-1;
00589 btVector3 nB = swapFactor*computedNormalB;
00590
00591 btScalar NdotA = localContactNormalOnB.dot(nA);
00592 btScalar NdotB = localContactNormalOnB.dot(nB);
00593 bool backFacingNormal = (NdotA< triangleInfoMapPtr->m_convexEpsilon) && (NdotB<triangleInfoMapPtr->m_convexEpsilon);
00594
00595 #ifdef DEBUG_INTERNAL_EDGE
00596 {
00597
00598 btDebugDrawLine(cp.getPositionWorldOnB(),cp.getPositionWorldOnB()+tr.getBasis()*(nB*20),red);
00599 }
00600 #endif //DEBUG_INTERNAL_EDGE
00601
00602
00603 if (backFacingNormal)
00604 {
00605 numConcaveEdgeHits++;
00606 }
00607 else
00608 {
00609 numConvexEdgeHits++;
00610 btVector3 clampedLocalNormal;
00611 bool isClamped = btClampNormal(edge,swapFactor*tri_normal,localContactNormalOnB, info->m_edgeV0V1Angle,clampedLocalNormal);
00612 if (isClamped)
00613 {
00614 if (((normalAdjustFlags & BT_TRIANGLE_CONVEX_DOUBLE_SIDED)!=0) || (clampedLocalNormal.dot(frontFacing*tri_normal)>0))
00615 {
00616 btVector3 newNormal = colObj0Wrap->getWorldTransform().getBasis() * clampedLocalNormal;
00617
00618 cp.m_normalWorldOnB = newNormal;
00619
00620 cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1;
00621 cp.m_localPointB = colObj0Wrap->getWorldTransform().invXform(cp.m_positionWorldOnB);
00622
00623 }
00624 }
00625 }
00626 }
00627 }
00628 }
00629
00630 btNearestPointInLineSegment(contact,v1,v2,nearest);
00631 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
00632 btDebugDrawLine(tr*nearest,tr*cp.m_localPointB,green);
00633 #endif //BT_INTERNAL_EDGE_DEBUG_DRAW
00634
00635 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
00636 btDebugDrawLine(tr * v1 + upfix, tr * v2 + upfix , green );
00637 #endif
00638
00639 if (btFabs(info->m_edgeV1V2Angle)< triangleInfoMapPtr->m_maxEdgeAngleThreshold)
00640 {
00641 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
00642 btDebugDrawLine(tr*contact,tr*(contact+cp.m_normalWorldOnB*10),black);
00643 #endif //BT_INTERNAL_EDGE_DEBUG_DRAW
00644
00645
00646
00647 btScalar len = (contact-nearest).length();
00648 if(len<triangleInfoMapPtr->m_edgeDistanceThreshold)
00649 if( bestedge==1 )
00650 {
00651 isNearEdge = true;
00652 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
00653 btDebugDrawLine(tr*nearest,tr*(nearest+tri_normal*10),white);
00654 #endif //BT_INTERNAL_EDGE_DEBUG_DRAW
00655
00656 btVector3 edge(v1-v2);
00657
00658 isNearEdge = true;
00659
00660 if (info->m_edgeV1V2Angle == btScalar(0))
00661 {
00662 numConcaveEdgeHits++;
00663 } else
00664 {
00665 bool isEdgeConvex = (info->m_flags & TRI_INFO_V1V2_CONVEX)!=0;
00666 btScalar swapFactor = isEdgeConvex ? btScalar(1) : btScalar(-1);
00667 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
00668 btDebugDrawLine(tr*nearest,tr*(nearest+swapFactor*tri_normal*10),white);
00669 #endif //BT_INTERNAL_EDGE_DEBUG_DRAW
00670
00671 btVector3 nA = swapFactor * tri_normal;
00672
00673 btQuaternion orn(edge,info->m_edgeV1V2Angle);
00674 btVector3 computedNormalB = quatRotate(orn,tri_normal);
00675 if (info->m_flags & TRI_INFO_V1V2_SWAP_NORMALB)
00676 computedNormalB*=-1;
00677 btVector3 nB = swapFactor*computedNormalB;
00678
00679 #ifdef DEBUG_INTERNAL_EDGE
00680 {
00681 btDebugDrawLine(cp.getPositionWorldOnB(),cp.getPositionWorldOnB()+tr.getBasis()*(nB*20),red);
00682 }
00683 #endif //DEBUG_INTERNAL_EDGE
00684
00685
00686 btScalar NdotA = localContactNormalOnB.dot(nA);
00687 btScalar NdotB = localContactNormalOnB.dot(nB);
00688 bool backFacingNormal = (NdotA< triangleInfoMapPtr->m_convexEpsilon) && (NdotB<triangleInfoMapPtr->m_convexEpsilon);
00689
00690 if (backFacingNormal)
00691 {
00692 numConcaveEdgeHits++;
00693 }
00694 else
00695 {
00696 numConvexEdgeHits++;
00697 btVector3 localContactNormalOnB = colObj0Wrap->getWorldTransform().getBasis().transpose() * cp.m_normalWorldOnB;
00698 btVector3 clampedLocalNormal;
00699 bool isClamped = btClampNormal(edge,swapFactor*tri_normal,localContactNormalOnB, info->m_edgeV1V2Angle,clampedLocalNormal);
00700 if (isClamped)
00701 {
00702 if (((normalAdjustFlags & BT_TRIANGLE_CONVEX_DOUBLE_SIDED)!=0) || (clampedLocalNormal.dot(frontFacing*tri_normal)>0))
00703 {
00704 btVector3 newNormal = colObj0Wrap->getWorldTransform().getBasis() * clampedLocalNormal;
00705
00706 cp.m_normalWorldOnB = newNormal;
00707
00708 cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1;
00709 cp.m_localPointB = colObj0Wrap->getWorldTransform().invXform(cp.m_positionWorldOnB);
00710 }
00711 }
00712 }
00713 }
00714 }
00715 }
00716
00717 btNearestPointInLineSegment(contact,v2,v0,nearest);
00718 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
00719 btDebugDrawLine(tr*nearest,tr*cp.m_localPointB,blue);
00720 #endif //BT_INTERNAL_EDGE_DEBUG_DRAW
00721 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
00722 btDebugDrawLine(tr * v2 + upfix, tr * v0 + upfix , blue );
00723 #endif
00724
00725 if (btFabs(info->m_edgeV2V0Angle)< triangleInfoMapPtr->m_maxEdgeAngleThreshold)
00726 {
00727
00728 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
00729 btDebugDrawLine(tr*contact,tr*(contact+cp.m_normalWorldOnB*10),black);
00730 #endif //BT_INTERNAL_EDGE_DEBUG_DRAW
00731
00732 btScalar len = (contact-nearest).length();
00733 if(len<triangleInfoMapPtr->m_edgeDistanceThreshold)
00734 if( bestedge==2 )
00735 {
00736 isNearEdge = true;
00737 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
00738 btDebugDrawLine(tr*nearest,tr*(nearest+tri_normal*10),white);
00739 #endif //BT_INTERNAL_EDGE_DEBUG_DRAW
00740
00741 btVector3 edge(v2-v0);
00742
00743 if (info->m_edgeV2V0Angle==btScalar(0))
00744 {
00745 numConcaveEdgeHits++;
00746 } else
00747 {
00748
00749 bool isEdgeConvex = (info->m_flags & TRI_INFO_V2V0_CONVEX)!=0;
00750 btScalar swapFactor = isEdgeConvex ? btScalar(1) : btScalar(-1);
00751 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
00752 btDebugDrawLine(tr*nearest,tr*(nearest+swapFactor*tri_normal*10),white);
00753 #endif //BT_INTERNAL_EDGE_DEBUG_DRAW
00754
00755 btVector3 nA = swapFactor * tri_normal;
00756 btQuaternion orn(edge,info->m_edgeV2V0Angle);
00757 btVector3 computedNormalB = quatRotate(orn,tri_normal);
00758 if (info->m_flags & TRI_INFO_V2V0_SWAP_NORMALB)
00759 computedNormalB*=-1;
00760 btVector3 nB = swapFactor*computedNormalB;
00761
00762 #ifdef DEBUG_INTERNAL_EDGE
00763 {
00764 btDebugDrawLine(cp.getPositionWorldOnB(),cp.getPositionWorldOnB()+tr.getBasis()*(nB*20),red);
00765 }
00766 #endif //DEBUG_INTERNAL_EDGE
00767
00768 btScalar NdotA = localContactNormalOnB.dot(nA);
00769 btScalar NdotB = localContactNormalOnB.dot(nB);
00770 bool backFacingNormal = (NdotA< triangleInfoMapPtr->m_convexEpsilon) && (NdotB<triangleInfoMapPtr->m_convexEpsilon);
00771
00772 if (backFacingNormal)
00773 {
00774 numConcaveEdgeHits++;
00775 }
00776 else
00777 {
00778 numConvexEdgeHits++;
00779
00780
00781
00782 btVector3 localContactNormalOnB = colObj0Wrap->getWorldTransform().getBasis().transpose() * cp.m_normalWorldOnB;
00783 btVector3 clampedLocalNormal;
00784 bool isClamped = btClampNormal(edge,swapFactor*tri_normal,localContactNormalOnB,info->m_edgeV2V0Angle,clampedLocalNormal);
00785 if (isClamped)
00786 {
00787 if (((normalAdjustFlags & BT_TRIANGLE_CONVEX_DOUBLE_SIDED)!=0) || (clampedLocalNormal.dot(frontFacing*tri_normal)>0))
00788 {
00789 btVector3 newNormal = colObj0Wrap->getWorldTransform().getBasis() * clampedLocalNormal;
00790
00791 cp.m_normalWorldOnB = newNormal;
00792
00793 cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1;
00794 cp.m_localPointB = colObj0Wrap->getWorldTransform().invXform(cp.m_positionWorldOnB);
00795 }
00796 }
00797 }
00798 }
00799
00800
00801 }
00802 }
00803
00804 #ifdef DEBUG_INTERNAL_EDGE
00805 {
00806 btVector3 color(0,1,1);
00807 btDebugDrawLine(cp.getPositionWorldOnB(),cp.getPositionWorldOnB()+cp.m_normalWorldOnB*10,color);
00808 }
00809 #endif //DEBUG_INTERNAL_EDGE
00810
00811 if (isNearEdge)
00812 {
00813
00814 if (numConcaveEdgeHits>0)
00815 {
00816 if ((normalAdjustFlags & BT_TRIANGLE_CONCAVE_DOUBLE_SIDED)!=0)
00817 {
00818
00819 if (tri_normal.dot(localContactNormalOnB) < 0)
00820 {
00821 tri_normal *= -1;
00822 }
00823 cp.m_normalWorldOnB = colObj0Wrap->getWorldTransform().getBasis()*tri_normal;
00824 } else
00825 {
00826 btVector3 newNormal = tri_normal *frontFacing;
00827
00828 btScalar d = newNormal.dot(localContactNormalOnB) ;
00829 if (d< 0)
00830 {
00831 return;
00832 }
00833
00834 cp.m_normalWorldOnB = colObj0Wrap->getWorldTransform().getBasis() *newNormal;
00835 }
00836
00837
00838 cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1;
00839 cp.m_localPointB = colObj0Wrap->getWorldTransform().invXform(cp.m_positionWorldOnB);
00840 }
00841 }
00842 }