dali-physics/third-party/bullet3/src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp

   1 #include "btInternalEdgeUtility.h"
   2
   3 #include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h"
   4 #include "BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h"
   5
   6 #include "BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.h"
   7 #include "BulletCollision/CollisionShapes/btTriangleShape.h"
   8 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
   9 #include "BulletCollision/NarrowPhaseCollision/btManifoldPoint.h"
  10 #include "LinearMath/btIDebugDraw.h"
  11 #include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h"
  12
  13 //#define DEBUG_INTERNAL_EDGE
  14
  15 #ifdef DEBUG_INTERNAL_EDGE
  16 #include <stdio.h>
  17 #endif  //DEBUG_INTERNAL_EDGE
  18
  19 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
  20 static btIDebugDraw* gDebugDrawer = 0;
  21
  22 void btSetDebugDrawer(btIDebugDraw* debugDrawer)
  23 {
  24         gDebugDrawer = debugDrawer;
  25 }
  26
  27 static void btDebugDrawLine(const btVector3& from, const btVector3& to, const btVector3& color)
  28 {
  29         if (gDebugDrawer)
  30                 gDebugDrawer->drawLine(from, to, color);
  31 }
  32 #endif  //BT_INTERNAL_EDGE_DEBUG_DRAW
  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 static btScalar btGetAngle(const btVector3& edgeA, const btVector3& normalA, const btVector3& normalB)
  41 {
  42         const btVector3 refAxis0 = edgeA;
  43         const btVector3 refAxis1 = normalA;
  44         const btVector3 swingAxis = normalB;
  45         btScalar angle = btAtan2(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1));
  46         return angle;
  47 }
  48
  49 struct btConnectivityProcessor : public btTriangleCallback
  50 {
  51         int m_partIdA;
  52         int m_triangleIndexA;
  53         btVector3* m_triangleVerticesA;
  54         btTriangleInfoMap* m_triangleInfoMap;
  55
  56         virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex)
  57         {
  58                 //skip self-collisions
  59                 if ((m_partIdA == partId) && (m_triangleIndexA == triangleIndex))
  60                         return;
  61
  62                 //skip duplicates (disabled for now)
  63                 //if ((m_partIdA <= partId) && (m_triangleIndexA <= triangleIndex))
  64                 //      return;
  65
  66                 //search for shared vertices and edges
  67                 int numshared = 0;
  68                 int sharedVertsA[3] = {-1, -1, -1};
  69                 int sharedVertsB[3] = {-1, -1, -1};
  70
  71                 ///skip degenerate triangles
  72                 btScalar crossBSqr = ((triangle[1] - triangle[0]).cross(triangle[2] - triangle[0])).length2();
  73                 if (crossBSqr < m_triangleInfoMap->m_equalVertexThreshold)
  74                         return;
  75
  76                 btScalar crossASqr = ((m_triangleVerticesA[1] - m_triangleVerticesA[0]).cross(m_triangleVerticesA[2] - m_triangleVerticesA[0])).length2();
  77                 ///skip degenerate triangles
  78                 if (crossASqr < m_triangleInfoMap->m_equalVertexThreshold)
  79                         return;
  80
  81 #if 0
  82                 printf("triangle A[0]   =       (%f,%f,%f)\ntriangle A[1]       =       (%f,%f,%f)\ntriangle A[2]       =       (%f,%f,%f)\n",
  83                         m_triangleVerticesA[0].getX(),m_triangleVerticesA[0].getY(),m_triangleVerticesA[0].getZ(),
  84                         m_triangleVerticesA[1].getX(),m_triangleVerticesA[1].getY(),m_triangleVerticesA[1].getZ(),
  85                         m_triangleVerticesA[2].getX(),m_triangleVerticesA[2].getY(),m_triangleVerticesA[2].getZ());
  86
  87                 printf("partId=%d, triangleIndex=%d\n",partId,triangleIndex);
  88                 printf("triangle B[0]   =       (%f,%f,%f)\ntriangle B[1]       =       (%f,%f,%f)\ntriangle B[2]       =       (%f,%f,%f)\n",
  89                         triangle[0].getX(),triangle[0].getY(),triangle[0].getZ(),
  90                         triangle[1].getX(),triangle[1].getY(),triangle[1].getZ(),
  91                         triangle[2].getX(),triangle[2].getY(),triangle[2].getZ());
  92 #endif
  93
  94                 for (int i = 0; i < 3; i++)
  95                 {
  96                         for (int j = 0; j < 3; j++)
  97                         {
  98                                 if ((m_triangleVerticesA[i] - triangle[j]).length2() < m_triangleInfoMap->m_equalVertexThreshold)
  99                                 {
 100                                         sharedVertsA[numshared] = i;
 101                                         sharedVertsB[numshared] = j;
 102                                         numshared++;
 103                                         ///degenerate case
 104                                         if (numshared >= 3)
 105                                                 return;
 106                                 }
 107                         }
 108                         ///degenerate case
 109                         if (numshared >= 3)
 110                                 return;
 111                 }
 112                 switch (numshared)
 113                 {
 114                         case 0:
 115                         {
 116                                 break;
 117                         }
 118                         case 1:
 119                         {
 120                                 //shared vertex
 121                                 break;
 122                         }
 123                         case 2:
 124                         {
 125                                 //shared edge
 126                                 //we need to make sure the edge is in the order V2V0 and not V0V2 so that the signs are correct
 127                                 if (sharedVertsA[0] == 0 && sharedVertsA[1] == 2)
 128                                 {
 129                                         sharedVertsA[0] = 2;
 130                                         sharedVertsA[1] = 0;
 131                                         int tmp = sharedVertsB[1];
 132                                         sharedVertsB[1] = sharedVertsB[0];
 133                                         sharedVertsB[0] = tmp;
 134                                 }
 135
 136                                 int hash = btGetHash(m_partIdA, m_triangleIndexA);
 137
 138                                 btTriangleInfo* info = m_triangleInfoMap->find(hash);
 139                                 if (!info)
 140                                 {
 141                                         btTriangleInfo tmp;
 142                                         m_triangleInfoMap->insert(hash, tmp);
 143                                         info = m_triangleInfoMap->find(hash);
 144                                 }
 145
 146                                 int sumvertsA = sharedVertsA[0] + sharedVertsA[1];
 147                                 int otherIndexA = 3 - sumvertsA;
 148
 149                                 btVector3 edge(m_triangleVerticesA[sharedVertsA[1]] - m_triangleVerticesA[sharedVertsA[0]]);
 150
 151                                 btTriangleShape tA(m_triangleVerticesA[0], m_triangleVerticesA[1], m_triangleVerticesA[2]);
 152                                 int otherIndexB = 3 - (sharedVertsB[0] + sharedVertsB[1]);
 153
 154                                 btTriangleShape tB(triangle[sharedVertsB[1]], triangle[sharedVertsB[0]], triangle[otherIndexB]);
 155                                 //btTriangleShape tB(triangle[0],triangle[1],triangle[2]);
 156
 157                                 btVector3 normalA;
 158                                 btVector3 normalB;
 159                                 tA.calcNormal(normalA);
 160                                 tB.calcNormal(normalB);
 161                                 edge.normalize();
 162                                 btVector3 edgeCrossA = edge.cross(normalA).normalize();
 163
 164                                 {
 165                                         btVector3 tmp = m_triangleVerticesA[otherIndexA] - m_triangleVerticesA[sharedVertsA[0]];
 166                                         if (edgeCrossA.dot(tmp) < 0)
 167                                         {
 168                                                 edgeCrossA *= -1;
 169                                         }
 170                                 }
 171
 172                                 btVector3 edgeCrossB = edge.cross(normalB).normalize();
 173
 174                                 {
 175                                         btVector3 tmp = triangle[otherIndexB] - triangle[sharedVertsB[0]];
 176                                         if (edgeCrossB.dot(tmp) < 0)
 177                                         {
 178                                                 edgeCrossB *= -1;
 179                                         }
 180                                 }
 181
 182                                 btScalar angle2 = 0;
 183                                 btScalar ang4 = 0.f;
 184
 185                                 btVector3 calculatedEdge = edgeCrossA.cross(edgeCrossB);
 186                                 btScalar len2 = calculatedEdge.length2();
 187
 188                                 btScalar correctedAngle(0);
 189                                 //btVector3 calculatedNormalB = normalA;
 190                                 bool isConvex = false;
 191
 192                                 if (len2 < m_triangleInfoMap->m_planarEpsilon)
 193                                 {
 194                                         angle2 = 0.f;
 195                                         ang4 = 0.f;
 196                                 }
 197                                 else
 198                                 {
 199                                         calculatedEdge.normalize();
 200                                         btVector3 calculatedNormalA = calculatedEdge.cross(edgeCrossA);
 201                                         calculatedNormalA.normalize();
 202                                         angle2 = btGetAngle(calculatedNormalA, edgeCrossA, edgeCrossB);
 203                                         ang4 = SIMD_PI - angle2;
 204                                         btScalar dotA = normalA.dot(edgeCrossB);
 205                                         ///@todo: check if we need some epsilon, due to floating point imprecision
 206                                         isConvex = (dotA < 0.);
 207
 208                                         correctedAngle = isConvex ? ang4 : -ang4;
 209                                 }
 210
 211                                 //alternatively use
 212                                 //btVector3 calculatedNormalB2 = quatRotate(orn,normalA);
 213
 214                                 switch (sumvertsA)
 215                                 {
 216                                         case 1:
 217                                         {
 218                                                 btVector3 edge = m_triangleVerticesA[0] - m_triangleVerticesA[1];
 219                                                 btQuaternion orn(edge, -correctedAngle);
 220                                                 btVector3 computedNormalB = quatRotate(orn, normalA);
 221                                                 btScalar bla = computedNormalB.dot(normalB);
 222                                                 if (bla < 0)
 223                                                 {
 224                                                         computedNormalB *= -1;
 225                                                         info->m_flags |= TRI_INFO_V0V1_SWAP_NORMALB;
 226                                                 }
 227 #ifdef DEBUG_INTERNAL_EDGE
 228                                                 if ((computedNormalB - normalB).length() > 0.0001)
 229                                                 {
 230                                                         printf("warning: normals not identical\n");
 231                                                 }
 232 #endif  //DEBUG_INTERNAL_EDGE
 233
 234                                                 info->m_edgeV0V1Angle = -correctedAngle;
 235
 236                                                 if (isConvex)
 237                                                         info->m_flags |= TRI_INFO_V0V1_CONVEX;
 238                                                 break;
 239                                         }
 240                                         case 2:
 241                                         {
 242                                                 btVector3 edge = m_triangleVerticesA[2] - m_triangleVerticesA[0];
 243                                                 btQuaternion orn(edge, -correctedAngle);
 244                                                 btVector3 computedNormalB = quatRotate(orn, normalA);
 245                                                 if (computedNormalB.dot(normalB) < 0)
 246                                                 {
 247                                                         computedNormalB *= -1;
 248                                                         info->m_flags |= TRI_INFO_V2V0_SWAP_NORMALB;
 249                                                 }
 250
 251 #ifdef DEBUG_INTERNAL_EDGE
 252                                                 if ((computedNormalB - normalB).length() > 0.0001)
 253                                                 {
 254                                                         printf("warning: normals not identical\n");
 255                                                 }
 256 #endif  //DEBUG_INTERNAL_EDGE
 257                                                 info->m_edgeV2V0Angle = -correctedAngle;
 258                                                 if (isConvex)
 259                                                         info->m_flags |= TRI_INFO_V2V0_CONVEX;
 260                                                 break;
 261                                         }
 262                                         case 3:
 263                                         {
 264                                                 btVector3 edge = m_triangleVerticesA[1] - m_triangleVerticesA[2];
 265                                                 btQuaternion orn(edge, -correctedAngle);
 266                                                 btVector3 computedNormalB = quatRotate(orn, normalA);
 267                                                 if (computedNormalB.dot(normalB) < 0)
 268                                                 {
 269                                                         info->m_flags |= TRI_INFO_V1V2_SWAP_NORMALB;
 270                                                         computedNormalB *= -1;
 271                                                 }
 272 #ifdef DEBUG_INTERNAL_EDGE
 273                                                 if ((computedNormalB - normalB).length() > 0.0001)
 274                                                 {
 275                                                         printf("warning: normals not identical\n");
 276                                                 }
 277 #endif  //DEBUG_INTERNAL_EDGE
 278                                                 info->m_edgeV1V2Angle = -correctedAngle;
 279
 280                                                 if (isConvex)
 281                                                         info->m_flags |= TRI_INFO_V1V2_CONVEX;
 282                                                 break;
 283                                         }
 284                                 }
 285
 286                                 break;
 287                         }
 288                         default:
 289                         {
 290                                 //                              printf("warning: duplicate triangle\n");
 291                         }
 292                 }
 293         }
 294 };
 295
 296
 297 struct b3ProcessAllTrianglesHeightfield: public btTriangleCallback
 298 {
 299         btHeightfieldTerrainShape* m_heightfieldShape;
 300         btTriangleInfoMap* m_triangleInfoMap;
 301
 302
 303         b3ProcessAllTrianglesHeightfield(btHeightfieldTerrainShape* heightFieldShape, btTriangleInfoMap* triangleInfoMap)
 304                 :m_heightfieldShape(heightFieldShape),
 305                 m_triangleInfoMap(triangleInfoMap)
 306         {
 307         }
 308         virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex)
 309         {
 310                 btConnectivityProcessor connectivityProcessor;
 311                 connectivityProcessor.m_partIdA = partId;
 312                 connectivityProcessor.m_triangleIndexA = triangleIndex;
 313                 connectivityProcessor.m_triangleVerticesA = triangle;
 314                 connectivityProcessor.m_triangleInfoMap = m_triangleInfoMap;
 315                 btVector3 aabbMin, aabbMax;
 316                 aabbMin.setValue(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT));
 317                 aabbMax.setValue(btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT));
 318                 aabbMin.setMin(triangle[0]);
 319                 aabbMax.setMax(triangle[0]);
 320                 aabbMin.setMin(triangle[1]);
 321                 aabbMax.setMax(triangle[1]);
 322                 aabbMin.setMin(triangle[2]);
 323                 aabbMax.setMax(triangle[2]);
 324
 325                 m_heightfieldShape->processAllTriangles(&connectivityProcessor, aabbMin, aabbMax);
 326         }
 327 };
 328 /////////////////////////////////////////////////////////
 329 /////////////////////////////////////////////////////////
 330
 331 void btGenerateInternalEdgeInfo(btBvhTriangleMeshShape* trimeshShape, btTriangleInfoMap* triangleInfoMap)
 332 {
 333         //the user pointer shouldn't already be used for other purposes, we intend to store connectivity info there!
 334         if (trimeshShape->getTriangleInfoMap())
 335                 return;
 336
 337         trimeshShape->setTriangleInfoMap(triangleInfoMap);
 338
 339         btStridingMeshInterface* meshInterface = trimeshShape->getMeshInterface();
 340         const btVector3& meshScaling = meshInterface->getScaling();
 341
 342         for (int partId = 0; partId < meshInterface->getNumSubParts(); partId++)
 343         {
 344                 const unsigned char* vertexbase = 0;
 345                 int numverts = 0;
 346                 PHY_ScalarType type = PHY_INTEGER;
 347                 int stride = 0;
 348                 const unsigned char* indexbase = 0;
 349                 int indexstride = 0;
 350                 int numfaces = 0;
 351                 PHY_ScalarType indicestype = PHY_INTEGER;
 352                 //PHY_ScalarType indexType=0;
 353
 354                 btVector3 triangleVerts[3];
 355                 meshInterface->getLockedReadOnlyVertexIndexBase(&vertexbase, numverts, type, stride, &indexbase, indexstride, numfaces, indicestype, partId);
 356                 btVector3 aabbMin, aabbMax;
 357
 358                 for (int triangleIndex = 0; triangleIndex < numfaces; triangleIndex++)
 359                 {
 360                         unsigned int* gfxbase = (unsigned int*)(indexbase + triangleIndex * indexstride);
 361
 362                         for (int j = 2; j >= 0; j--)
 363                         {
 364                                 int graphicsindex;
 365                                 switch (indicestype) {
 366                                         case PHY_INTEGER: graphicsindex = gfxbase[j]; break;
 367                                         case PHY_SHORT: graphicsindex = ((unsigned short*)gfxbase)[j]; break;
 368                                         case PHY_UCHAR: graphicsindex = ((unsigned char*)gfxbase)[j]; break;
 369                                         default: btAssert(0);
 370                                 }
 371                                 if (type == PHY_FLOAT)
 372                                 {
 373                                         float* graphicsbase = (float*)(vertexbase + graphicsindex * stride);
 374                                         triangleVerts[j] = btVector3(
 375                                                 graphicsbase[0] * meshScaling.getX(),
 376                                                 graphicsbase[1] * meshScaling.getY(),
 377                                                 graphicsbase[2] * meshScaling.getZ());
 378                                 }
 379                                 else
 380                                 {
 381                                         double* graphicsbase = (double*)(vertexbase + graphicsindex * stride);
 382                                         triangleVerts[j] = btVector3(btScalar(graphicsbase[0] * meshScaling.getX()), btScalar(graphicsbase[1] * meshScaling.getY()), btScalar(graphicsbase[2] * meshScaling.getZ()));
 383                                 }
 384                         }
 385                         aabbMin.setValue(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT));
 386                         aabbMax.setValue(btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT));
 387                         aabbMin.setMin(triangleVerts[0]);
 388                         aabbMax.setMax(triangleVerts[0]);
 389                         aabbMin.setMin(triangleVerts[1]);
 390                         aabbMax.setMax(triangleVerts[1]);
 391                         aabbMin.setMin(triangleVerts[2]);
 392                         aabbMax.setMax(triangleVerts[2]);
 393
 394                         btConnectivityProcessor connectivityProcessor;
 395                         connectivityProcessor.m_partIdA = partId;
 396                         connectivityProcessor.m_triangleIndexA = triangleIndex;
 397                         connectivityProcessor.m_triangleVerticesA = &triangleVerts[0];
 398                         connectivityProcessor.m_triangleInfoMap = triangleInfoMap;
 399
 400                         trimeshShape->processAllTriangles(&connectivityProcessor, aabbMin, aabbMax);
 401                 }
 402         }
 403 }
 404
 405
 406 void btGenerateInternalEdgeInfo(btHeightfieldTerrainShape* heightfieldShape, btTriangleInfoMap* triangleInfoMap)
 407 {
 408
 409         //the user pointer shouldn't already be used for other purposes, we intend to store connectivity info there!
 410         if (heightfieldShape->getTriangleInfoMap())
 411                 return;
 412
 413         heightfieldShape->setTriangleInfoMap(triangleInfoMap);
 414
 415         //get all the triangles of the heightfield
 416
 417         btVector3 aabbMin, aabbMax;
 418
 419         aabbMax.setValue(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT));
 420         aabbMin.setValue(btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT));
 421
 422         b3ProcessAllTrianglesHeightfield processHeightfield(heightfieldShape, triangleInfoMap);
 423         heightfieldShape->processAllTriangles(&processHeightfield, aabbMin, aabbMax);
 424
 425 }
 426
 427 // Given a point and a line segment (defined by two points), compute the closest point
 428 // in the line.  Cap the point at the endpoints of the line segment.
 429 void btNearestPointInLineSegment(const btVector3& point, const btVector3& line0, const btVector3& line1, btVector3& nearestPoint)
 430 {
 431         btVector3 lineDelta = line1 - line0;
 432
 433         // Handle degenerate lines
 434         if (lineDelta.fuzzyZero())
 435         {
 436                 nearestPoint = line0;
 437         }
 438         else
 439         {
 440                 btScalar delta = (point - line0).dot(lineDelta) / (lineDelta).dot(lineDelta);
 441
 442                 // Clamp the point to conform to the segment's endpoints
 443                 if (delta < 0)
 444                         delta = 0;
 445                 else if (delta > 1)
 446                         delta = 1;
 447
 448                 nearestPoint = line0 + lineDelta * delta;
 449         }
 450 }
 451
 452 bool btClampNormal(const btVector3& edge, const btVector3& tri_normal_org, const btVector3& localContactNormalOnB, btScalar correctedEdgeAngle, btVector3& clampedLocalNormal)
 453 {
 454         btVector3 tri_normal = tri_normal_org;
 455         //we only have a local triangle normal, not a local contact normal -> only normal in world space...
 456         //either compute the current angle all in local space, or all in world space
 457
 458         btVector3 edgeCross = edge.cross(tri_normal).normalize();
 459         btScalar curAngle = btGetAngle(edgeCross, tri_normal, localContactNormalOnB);
 460
 461         if (correctedEdgeAngle < 0)
 462         {
 463                 if (curAngle < correctedEdgeAngle)
 464                 {
 465                         btScalar diffAngle = correctedEdgeAngle - curAngle;
 466                         btQuaternion rotation(edge, diffAngle);
 467                         clampedLocalNormal = btMatrix3x3(rotation) * localContactNormalOnB;
 468                         return true;
 469                 }
 470         }
 471
 472         if (correctedEdgeAngle >= 0)
 473         {
 474                 if (curAngle > correctedEdgeAngle)
 475                 {
 476                         btScalar diffAngle = correctedEdgeAngle - curAngle;
 477                         btQuaternion rotation(edge, diffAngle);
 478                         clampedLocalNormal = btMatrix3x3(rotation) * localContactNormalOnB;
 479                         return true;
 480                 }
 481         }
 482         return false;
 483 }
 484
 485 /// Changes a btManifoldPoint collision normal to the normal from the mesh.
 486 void btAdjustInternalEdgeContacts(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap, const btCollisionObjectWrapper* colObj1Wrap, int partId0, int index0, int normalAdjustFlags)
 487 {
 488         //btAssert(colObj0->getCollisionShape()->getShapeType() == TRIANGLE_SHAPE_PROXYTYPE);
 489         if (colObj0Wrap->getCollisionShape()->getShapeType() != TRIANGLE_SHAPE_PROXYTYPE)
 490                 return;
 491
 492
 493         btTriangleInfoMap* triangleInfoMapPtr = 0;
 494
 495         if (colObj0Wrap->getCollisionObject()->getCollisionShape()->getShapeType() == TERRAIN_SHAPE_PROXYTYPE)
 496         {
 497                 btHeightfieldTerrainShape* heightfield = (btHeightfieldTerrainShape*)colObj0Wrap->getCollisionObject()->getCollisionShape();
 498                 triangleInfoMapPtr = heightfield->getTriangleInfoMap();
 499
 500 //#define USE_HEIGHTFIELD_TRIANGLES
 501 #ifdef USE_HEIGHTFIELD_TRIANGLES
 502                 btVector3 newNormal = btVector3(0, 0, 1);
 503
 504                 const btTriangleShape* tri_shape = static_cast<const btTriangleShape*>(colObj0Wrap->getCollisionShape());
 505                 btVector3 tri_normal;
 506                 tri_shape->calcNormal(tri_normal);
 507                 newNormal = tri_normal;
 508                 //                                      cp.m_distance1 = cp.m_distance1 * newNormal.dot(cp.m_normalWorldOnB);
 509                 cp.m_normalWorldOnB = newNormal;
 510                 // Reproject collision point along normal. (what about cp.m_distance1?)
 511                 cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1;
 512                 cp.m_localPointB = colObj0Wrap->getWorldTransform().invXform(cp.m_positionWorldOnB);
 513                 return;
 514 #endif
 515         }
 516
 517
 518         btBvhTriangleMeshShape* trimesh = 0;
 519
 520         if (colObj0Wrap->getCollisionObject()->getCollisionShape()->getShapeType() == SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE)
 521         {
 522                 trimesh = ((btScaledBvhTriangleMeshShape*)colObj0Wrap->getCollisionObject()->getCollisionShape())->getChildShape();
 523         }
 524         else
 525         {
 526                 if (colObj0Wrap->getCollisionObject()->getCollisionShape()->getShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE)
 527                 {
 528                         trimesh = (btBvhTriangleMeshShape*)colObj0Wrap->getCollisionObject()->getCollisionShape();
 529                 }
 530         }
 531         if (trimesh)
 532         {
 533                 triangleInfoMapPtr = (btTriangleInfoMap*)trimesh->getTriangleInfoMap();
 534         }
 535
 536
 537         if (!triangleInfoMapPtr)
 538                 return;
 539
 540         int hash = btGetHash(partId0, index0);
 541
 542         btTriangleInfo* info = triangleInfoMapPtr->find(hash);
 543         if (!info)
 544                 return;
 545
 546         btScalar frontFacing = (normalAdjustFlags & BT_TRIANGLE_CONVEX_BACKFACE_MODE) == 0 ? 1.f : -1.f;
 547
 548         const btTriangleShape* tri_shape = static_cast<const btTriangleShape*>(colObj0Wrap->getCollisionShape());
 549         btVector3 v0, v1, v2;
 550         tri_shape->getVertex(0, v0);
 551         tri_shape->getVertex(1, v1);
 552         tri_shape->getVertex(2, v2);
 553
 554         //btVector3 center = (v0+v1+v2)*btScalar(1./3.);
 555
 556         btVector3 red(1, 0, 0), green(0, 1, 0), blue(0, 0, 1), white(1, 1, 1), black(0, 0, 0);
 557         btVector3 tri_normal;
 558         tri_shape->calcNormal(tri_normal);
 559
 560         //btScalar dot = tri_normal.dot(cp.m_normalWorldOnB);
 561         btVector3 nearest;
 562         btNearestPointInLineSegment(cp.m_localPointB, v0, v1, nearest);
 563
 564         btVector3 contact = cp.m_localPointB;
 565 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
 566         const btTransform& tr = colObj0->getWorldTransform();
 567         btDebugDrawLine(tr * nearest, tr * cp.m_localPointB, red);
 568 #endif  //BT_INTERNAL_EDGE_DEBUG_DRAW
 569
 570         bool isNearEdge = false;
 571
 572         int numConcaveEdgeHits = 0;
 573         int numConvexEdgeHits = 0;
 574
 575         btVector3 localContactNormalOnB = colObj0Wrap->getWorldTransform().getBasis().transpose() * cp.m_normalWorldOnB;
 576         localContactNormalOnB.normalize();  //is this necessary?
 577
 578         // Get closest edge
 579         int bestedge = -1;
 580         btScalar disttobestedge = BT_LARGE_FLOAT;
 581         //
 582         // Edge 0 -> 1
 583         if (btFabs(info->m_edgeV0V1Angle) < triangleInfoMapPtr->m_maxEdgeAngleThreshold)
 584         {
 585                 btVector3 nearest;
 586                 btNearestPointInLineSegment(cp.m_localPointB, v0, v1, nearest);
 587                 btScalar len = (contact - nearest).length();
 588                 //
 589                 if (len < disttobestedge)
 590                 {
 591                         bestedge = 0;
 592                         disttobestedge = len;
 593                 }
 594         }
 595         // Edge 1 -> 2
 596         if (btFabs(info->m_edgeV1V2Angle) < triangleInfoMapPtr->m_maxEdgeAngleThreshold)
 597         {
 598                 btVector3 nearest;
 599                 btNearestPointInLineSegment(cp.m_localPointB, v1, v2, nearest);
 600                 btScalar len = (contact - nearest).length();
 601                 //
 602                 if (len < disttobestedge)
 603                 {
 604                         bestedge = 1;
 605                         disttobestedge = len;
 606                 }
 607         }
 608         // Edge 2 -> 0
 609         if (btFabs(info->m_edgeV2V0Angle) < triangleInfoMapPtr->m_maxEdgeAngleThreshold)
 610         {
 611                 btVector3 nearest;
 612                 btNearestPointInLineSegment(cp.m_localPointB, v2, v0, nearest);
 613                 btScalar len = (contact - nearest).length();
 614                 //
 615                 if (len < disttobestedge)
 616                 {
 617                         bestedge = 2;
 618                         disttobestedge = len;
 619                 }
 620         }
 621
 622 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
 623         btVector3 upfix = tri_normal * btVector3(0.1f, 0.1f, 0.1f);
 624         btDebugDrawLine(tr * v0 + upfix, tr * v1 + upfix, red);
 625 #endif
 626         if (btFabs(info->m_edgeV0V1Angle) < triangleInfoMapPtr->m_maxEdgeAngleThreshold)
 627         {
 628 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
 629                 btDebugDrawLine(tr * contact, tr * (contact + cp.m_normalWorldOnB * 10), black);
 630 #endif
 631                 btScalar len = (contact - nearest).length();
 632                 if (len < triangleInfoMapPtr->m_edgeDistanceThreshold)
 633                         if (bestedge == 0)
 634                         {
 635                                 btVector3 edge(v0 - v1);
 636                                 isNearEdge = true;
 637
 638                                 if (info->m_edgeV0V1Angle == btScalar(0))
 639                                 {
 640                                         numConcaveEdgeHits++;
 641                                 }
 642                                 else
 643                                 {
 644                                         bool isEdgeConvex = (info->m_flags & TRI_INFO_V0V1_CONVEX);
 645                                         btScalar swapFactor = isEdgeConvex ? btScalar(1) : btScalar(-1);
 646 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
 647                                         btDebugDrawLine(tr * nearest, tr * (nearest + swapFactor * tri_normal * 10), white);
 648 #endif  //BT_INTERNAL_EDGE_DEBUG_DRAW
 649
 650                                         btVector3 nA = swapFactor * tri_normal;
 651
 652                                         btQuaternion orn(edge, info->m_edgeV0V1Angle);
 653                                         btVector3 computedNormalB = quatRotate(orn, tri_normal);
 654                                         if (info->m_flags & TRI_INFO_V0V1_SWAP_NORMALB)
 655                                                 computedNormalB *= -1;
 656                                         btVector3 nB = swapFactor * computedNormalB;
 657
 658                                         btScalar NdotA = localContactNormalOnB.dot(nA);
 659                                         btScalar NdotB = localContactNormalOnB.dot(nB);
 660                                         bool backFacingNormal = (NdotA < triangleInfoMapPtr->m_convexEpsilon) && (NdotB < triangleInfoMapPtr->m_convexEpsilon);
 661
 662 #ifdef DEBUG_INTERNAL_EDGE
 663                                         {
 664                                                 btDebugDrawLine(cp.getPositionWorldOnB(), cp.getPositionWorldOnB() + tr.getBasis() * (nB * 20), red);
 665                                         }
 666 #endif  //DEBUG_INTERNAL_EDGE
 667
 668                                         if (backFacingNormal)
 669                                         {
 670                                                 numConcaveEdgeHits++;
 671                                         }
 672                                         else
 673                                         {
 674                                                 numConvexEdgeHits++;
 675                                                 btVector3 clampedLocalNormal;
 676                                                 bool isClamped = btClampNormal(edge, swapFactor * tri_normal, localContactNormalOnB, info->m_edgeV0V1Angle, clampedLocalNormal);
 677                                                 if (isClamped)
 678                                                 {
 679                                                         if (((normalAdjustFlags & BT_TRIANGLE_CONVEX_DOUBLE_SIDED) != 0) || (clampedLocalNormal.dot(frontFacing * tri_normal) > 0))
 680                                                         {
 681                                                                 btVector3 newNormal = colObj0Wrap->getWorldTransform().getBasis() * clampedLocalNormal;
 682                                                                 //                                      cp.m_distance1 = cp.m_distance1 * newNormal.dot(cp.m_normalWorldOnB);
 683                                                                 cp.m_normalWorldOnB = newNormal;
 684                                                                 // Reproject collision point along normal. (what about cp.m_distance1?)
 685                                                                 cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1;
 686                                                                 cp.m_localPointB = colObj0Wrap->getWorldTransform().invXform(cp.m_positionWorldOnB);
 687                                                         }
 688                                                 }
 689                                         }
 690                                 }
 691                         }
 692         }
 693
 694         btNearestPointInLineSegment(contact, v1, v2, nearest);
 695 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
 696         btDebugDrawLine(tr * nearest, tr * cp.m_localPointB, green);
 697 #endif  //BT_INTERNAL_EDGE_DEBUG_DRAW
 698
 699 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
 700         btDebugDrawLine(tr * v1 + upfix, tr * v2 + upfix, green);
 701 #endif
 702
 703         if (btFabs(info->m_edgeV1V2Angle) < triangleInfoMapPtr->m_maxEdgeAngleThreshold)
 704         {
 705 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
 706                 btDebugDrawLine(tr * contact, tr * (contact + cp.m_normalWorldOnB * 10), black);
 707 #endif  //BT_INTERNAL_EDGE_DEBUG_DRAW
 708
 709                 btScalar len = (contact - nearest).length();
 710                 if (len < triangleInfoMapPtr->m_edgeDistanceThreshold)
 711                         if (bestedge == 1)
 712                         {
 713                                 isNearEdge = true;
 714 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
 715                                 btDebugDrawLine(tr * nearest, tr * (nearest + tri_normal * 10), white);
 716 #endif  //BT_INTERNAL_EDGE_DEBUG_DRAW
 717
 718                                 btVector3 edge(v1 - v2);
 719
 720                                 isNearEdge = true;
 721
 722                                 if (info->m_edgeV1V2Angle == btScalar(0))
 723                                 {
 724                                         numConcaveEdgeHits++;
 725                                 }
 726                                 else
 727                                 {
 728                                         bool isEdgeConvex = (info->m_flags & TRI_INFO_V1V2_CONVEX) != 0;
 729                                         btScalar swapFactor = isEdgeConvex ? btScalar(1) : btScalar(-1);
 730 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
 731                                         btDebugDrawLine(tr * nearest, tr * (nearest + swapFactor * tri_normal * 10), white);
 732 #endif  //BT_INTERNAL_EDGE_DEBUG_DRAW
 733
 734                                         btVector3 nA = swapFactor * tri_normal;
 735
 736                                         btQuaternion orn(edge, info->m_edgeV1V2Angle);
 737                                         btVector3 computedNormalB = quatRotate(orn, tri_normal);
 738                                         if (info->m_flags & TRI_INFO_V1V2_SWAP_NORMALB)
 739                                                 computedNormalB *= -1;
 740                                         btVector3 nB = swapFactor * computedNormalB;
 741
 742 #ifdef DEBUG_INTERNAL_EDGE
 743                                         {
 744                                                 btDebugDrawLine(cp.getPositionWorldOnB(), cp.getPositionWorldOnB() + tr.getBasis() * (nB * 20), red);
 745                                         }
 746 #endif  //DEBUG_INTERNAL_EDGE
 747
 748                                         btScalar NdotA = localContactNormalOnB.dot(nA);
 749                                         btScalar NdotB = localContactNormalOnB.dot(nB);
 750                                         bool backFacingNormal = (NdotA < triangleInfoMapPtr->m_convexEpsilon) && (NdotB < triangleInfoMapPtr->m_convexEpsilon);
 751
 752                                         if (backFacingNormal)
 753                                         {
 754                                                 numConcaveEdgeHits++;
 755                                         }
 756                                         else
 757                                         {
 758                                                 numConvexEdgeHits++;
 759                                                 btVector3 localContactNormalOnB = colObj0Wrap->getWorldTransform().getBasis().transpose() * cp.m_normalWorldOnB;
 760                                                 btVector3 clampedLocalNormal;
 761                                                 bool isClamped = btClampNormal(edge, swapFactor * tri_normal, localContactNormalOnB, info->m_edgeV1V2Angle, clampedLocalNormal);
 762                                                 if (isClamped)
 763                                                 {
 764                                                         if (((normalAdjustFlags & BT_TRIANGLE_CONVEX_DOUBLE_SIDED) != 0) || (clampedLocalNormal.dot(frontFacing * tri_normal) > 0))
 765                                                         {
 766                                                                 btVector3 newNormal = colObj0Wrap->getWorldTransform().getBasis() * clampedLocalNormal;
 767                                                                 //                                      cp.m_distance1 = cp.m_distance1 * newNormal.dot(cp.m_normalWorldOnB);
 768                                                                 cp.m_normalWorldOnB = newNormal;
 769                                                                 // Reproject collision point along normal.
 770                                                                 cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1;
 771                                                                 cp.m_localPointB = colObj0Wrap->getWorldTransform().invXform(cp.m_positionWorldOnB);
 772                                                         }
 773                                                 }
 774                                         }
 775                                 }
 776                         }
 777         }
 778
 779         btNearestPointInLineSegment(contact, v2, v0, nearest);
 780 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
 781         btDebugDrawLine(tr * nearest, tr * cp.m_localPointB, blue);
 782 #endif  //BT_INTERNAL_EDGE_DEBUG_DRAW
 783 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
 784         btDebugDrawLine(tr * v2 + upfix, tr * v0 + upfix, blue);
 785 #endif
 786
 787         if (btFabs(info->m_edgeV2V0Angle) < triangleInfoMapPtr->m_maxEdgeAngleThreshold)
 788         {
 789 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
 790                 btDebugDrawLine(tr * contact, tr * (contact + cp.m_normalWorldOnB * 10), black);
 791 #endif  //BT_INTERNAL_EDGE_DEBUG_DRAW
 792
 793                 btScalar len = (contact - nearest).length();
 794                 if (len < triangleInfoMapPtr->m_edgeDistanceThreshold)
 795                         if (bestedge == 2)
 796                         {
 797                                 isNearEdge = true;
 798 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
 799                                 btDebugDrawLine(tr * nearest, tr * (nearest + tri_normal * 10), white);
 800 #endif  //BT_INTERNAL_EDGE_DEBUG_DRAW
 801
 802                                 btVector3 edge(v2 - v0);
 803
 804                                 if (info->m_edgeV2V0Angle == btScalar(0))
 805                                 {
 806                                         numConcaveEdgeHits++;
 807                                 }
 808                                 else
 809                                 {
 810                                         bool isEdgeConvex = (info->m_flags & TRI_INFO_V2V0_CONVEX) != 0;
 811                                         btScalar swapFactor = isEdgeConvex ? btScalar(1) : btScalar(-1);
 812 #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
 813                                         btDebugDrawLine(tr * nearest, tr * (nearest + swapFactor * tri_normal * 10), white);
 814 #endif  //BT_INTERNAL_EDGE_DEBUG_DRAW
 815
 816                                         btVector3 nA = swapFactor * tri_normal;
 817                                         btQuaternion orn(edge, info->m_edgeV2V0Angle);
 818                                         btVector3 computedNormalB = quatRotate(orn, tri_normal);
 819                                         if (info->m_flags & TRI_INFO_V2V0_SWAP_NORMALB)
 820                                                 computedNormalB *= -1;
 821                                         btVector3 nB = swapFactor * computedNormalB;
 822
 823 #ifdef DEBUG_INTERNAL_EDGE
 824                                         {
 825                                                 btDebugDrawLine(cp.getPositionWorldOnB(), cp.getPositionWorldOnB() + tr.getBasis() * (nB * 20), red);
 826                                         }
 827 #endif  //DEBUG_INTERNAL_EDGE
 828
 829                                         btScalar NdotA = localContactNormalOnB.dot(nA);
 830                                         btScalar NdotB = localContactNormalOnB.dot(nB);
 831                                         bool backFacingNormal = (NdotA < triangleInfoMapPtr->m_convexEpsilon) && (NdotB < triangleInfoMapPtr->m_convexEpsilon);
 832
 833                                         if (backFacingNormal)
 834                                         {
 835                                                 numConcaveEdgeHits++;
 836                                         }
 837                                         else
 838                                         {
 839                                                 numConvexEdgeHits++;
 840                                                 //                              printf("hitting convex edge\n");
 841
 842                                                 btVector3 localContactNormalOnB = colObj0Wrap->getWorldTransform().getBasis().transpose() * cp.m_normalWorldOnB;
 843                                                 btVector3 clampedLocalNormal;
 844                                                 bool isClamped = btClampNormal(edge, swapFactor * tri_normal, localContactNormalOnB, info->m_edgeV2V0Angle, clampedLocalNormal);
 845                                                 if (isClamped)
 846                                                 {
 847                                                         if (((normalAdjustFlags & BT_TRIANGLE_CONVEX_DOUBLE_SIDED) != 0) || (clampedLocalNormal.dot(frontFacing * tri_normal) > 0))
 848                                                         {
 849                                                                 btVector3 newNormal = colObj0Wrap->getWorldTransform().getBasis() * clampedLocalNormal;
 850                                                                 //                                      cp.m_distance1 = cp.m_distance1 * newNormal.dot(cp.m_normalWorldOnB);
 851                                                                 cp.m_normalWorldOnB = newNormal;
 852                                                                 // Reproject collision point along normal.
 853                                                                 cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1;
 854                                                                 cp.m_localPointB = colObj0Wrap->getWorldTransform().invXform(cp.m_positionWorldOnB);
 855                                                         }
 856                                                 }
 857                                         }
 858                                 }
 859                         }
 860         }
 861
 862 #ifdef DEBUG_INTERNAL_EDGE
 863         {
 864                 btVector3 color(0, 1, 1);
 865                 btDebugDrawLine(cp.getPositionWorldOnB(), cp.getPositionWorldOnB() + cp.m_normalWorldOnB * 10, color);
 866         }
 867 #endif  //DEBUG_INTERNAL_EDGE
 868
 869         if (isNearEdge)
 870         {
 871                 if (numConcaveEdgeHits > 0)
 872                 {
 873                         if ((normalAdjustFlags & BT_TRIANGLE_CONCAVE_DOUBLE_SIDED) != 0)
 874                         {
 875                                 //fix tri_normal so it pointing the same direction as the current local contact normal
 876                                 if (tri_normal.dot(localContactNormalOnB) < 0)
 877                                 {
 878                                         tri_normal *= -1;
 879                                 }
 880                                 cp.m_normalWorldOnB = colObj0Wrap->getWorldTransform().getBasis() * tri_normal;
 881                         }
 882                         else
 883                         {
 884                                 btVector3 newNormal = tri_normal * frontFacing;
 885                                 //if the tri_normal is pointing opposite direction as the current local contact normal, skip it
 886                                 btScalar d = newNormal.dot(localContactNormalOnB);
 887                                 if (d < 0)
 888                                 {
 889                                         return;
 890                                 }
 891                                 //modify the normal to be the triangle normal (or backfacing normal)
 892                                 cp.m_normalWorldOnB = colObj0Wrap->getWorldTransform().getBasis() * newNormal;
 893                         }
 894
 895                         // Reproject collision point along normal.
 896                         cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1;
 897                         cp.m_localPointB = colObj0Wrap->getWorldTransform().invXform(cp.m_positionWorldOnB);
 898                 }
 899         }
 900 }