btInternalEdgeUtility.cpp

Go to the documentation of this file.
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 //#define DEBUG_INTERNAL_EDGE
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                 //skip self-collisions
00063                 if ((m_partIdA == partId) && (m_triangleIndexA == triangleIndex))
00064                         return;
00065 
00066                 //skip duplicates (disabled for now)
00067                 //if ((m_partIdA <= partId) && (m_triangleIndexA <= triangleIndex))
00068                 //      return;
00069 
00070                 //search for shared vertices and edges
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                                 //shared vertex
00126                                 break;
00127                         }
00128                 case 2:
00129                         {
00130                                 //shared edge
00131                                 //we need to make sure the edge is in the order V2V0 and not V0V2 so that the signs are correct
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                                 //btTriangleShape tB(triangle[0],triangle[1],triangle[2]);
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                                 //alternatively use 
00227                                 //btVector3 calculatedNormalB2 = quatRotate(orn,normalA);
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                                 //                              printf("warning: duplicate triangle\n");
00307                         }
00308 
00309                 }
00310         }
00311 };
00314 
00315 void btGenerateInternalEdgeInfo (btBvhTriangleMeshShape*trimeshShape, btTriangleInfoMap* triangleInfoMap)
00316 {
00317         //the user pointer shouldn't already be used for other purposes, we intend to store connectivity info there!
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                 //PHY_ScalarType indexType=0;
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 // Given a point and a line segment (defined by two points), compute the closest point
00390 // in the line.  Cap the point at the endpoints of the line segment.
00391 void btNearestPointInLineSegment(const btVector3 &point, const btVector3& line0, const btVector3& line1, btVector3& nearestPoint)
00392 {
00393         btVector3 lineDelta     = line1 - line0;
00394 
00395         // Handle degenerate lines
00396         if ( lineDelta.fuzzyZero())
00397         {
00398                 nearestPoint = line0;
00399         }
00400         else
00401         {
00402                 btScalar delta = (point-line0).dot(lineDelta) / (lineDelta).dot(lineDelta);
00403 
00404                 // Clamp the point to conform to the segment's endpoints
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         //we only have a local triangle normal, not a local contact normal -> only normal in world space...
00421         //either compute the current angle all in local space, or all in world space
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         //btAssert(colObj0->getCollisionShape()->getShapeType() == TRIANGLE_SHAPE_PROXYTYPE);
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         //btVector3 center = (v0+v1+v2)*btScalar(1./3.);
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         //btScalar dot = tri_normal.dot(cp.m_normalWorldOnB);
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();//is this necessary?
00510         
00511         // Get closest edge
00512         int      bestedge=-1;
00513         btScalar    disttobestedge=BT_LARGE_FLOAT;
00514         //
00515         // Edge 0 -> 1
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         // Edge 1 -> 2
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         // Edge 2 -> 0
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                                                         //                                      cp.m_distance1 = cp.m_distance1 * newNormal.dot(cp.m_normalWorldOnB);
00618                                                         cp.m_normalWorldOnB = newNormal;
00619                                                         // Reproject collision point along normal. (what about cp.m_distance1?)
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                                                         //                                      cp.m_distance1 = cp.m_distance1 * newNormal.dot(cp.m_normalWorldOnB);
00706                                                         cp.m_normalWorldOnB = newNormal;
00707                                                         // Reproject collision point along normal.
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                                         //                              printf("hitting convex edge\n");
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                                                         //                                      cp.m_distance1 = cp.m_distance1 * newNormal.dot(cp.m_normalWorldOnB);
00791                                                         cp.m_normalWorldOnB = newNormal;
00792                                                         // Reproject collision point along normal.
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                                 //fix tri_normal so it pointing the same direction as the current local contact normal
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                                 //if the tri_normal is pointing opposite direction as the current local contact normal, skip it
00828                                 btScalar d = newNormal.dot(localContactNormalOnB) ;
00829                                 if (d< 0)
00830                                 {
00831                                         return;
00832                                 }
00833                                 //modify the normal to be the triangle normal (or backfacing normal)
00834                                 cp.m_normalWorldOnB = colObj0Wrap->getWorldTransform().getBasis() *newNormal;
00835                         }
00836                                                 
00837                         // Reproject collision point along normal.
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 }