src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp

   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 }