diff options
Diffstat (limited to 'extern/bullet2/src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp')
| -rw-r--r-- | extern/bullet2/src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp | 846 |
1 files changed, 454 insertions, 392 deletions
diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp index 6cba442ca5d..a71700f58af 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp @@ -1,6 +1,8 @@ #include "btInternalEdgeUtility.h" #include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h" +#include "BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h" + #include "BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.h" #include "BulletCollision/CollisionShapes/btTriangleShape.h" #include "BulletCollision/CollisionDispatch/btCollisionObject.h" @@ -12,50 +14,44 @@ #ifdef DEBUG_INTERNAL_EDGE #include <stdio.h> -#endif //DEBUG_INTERNAL_EDGE - +#endif //DEBUG_INTERNAL_EDGE #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW static btIDebugDraw* gDebugDrawer = 0; -void btSetDebugDrawer(btIDebugDraw* debugDrawer) +void btSetDebugDrawer(btIDebugDraw* debugDrawer) { gDebugDrawer = debugDrawer; } -static void btDebugDrawLine(const btVector3& from,const btVector3& to, const btVector3& color) +static void btDebugDrawLine(const btVector3& from, const btVector3& to, const btVector3& color) { if (gDebugDrawer) - gDebugDrawer->drawLine(from,to,color); + gDebugDrawer->drawLine(from, to, color); } -#endif //BT_INTERNAL_EDGE_DEBUG_DRAW - +#endif //BT_INTERNAL_EDGE_DEBUG_DRAW -static int btGetHash(int partId, int triangleIndex) +static int btGetHash(int partId, int triangleIndex) { - int hash = (partId<<(31-MAX_NUM_PARTS_IN_BITS)) | triangleIndex; + int hash = (partId << (31 - MAX_NUM_PARTS_IN_BITS)) | triangleIndex; return hash; } - - -static btScalar btGetAngle(const btVector3& edgeA, const btVector3& normalA,const btVector3& normalB) +static btScalar btGetAngle(const btVector3& edgeA, const btVector3& normalA, const btVector3& normalB) { - const btVector3 refAxis0 = edgeA; - const btVector3 refAxis1 = normalA; + const btVector3 refAxis0 = edgeA; + const btVector3 refAxis1 = normalA; const btVector3 swingAxis = normalB; btScalar angle = btAtan2(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1)); - return angle; + return angle; } - struct btConnectivityProcessor : public btTriangleCallback { - int m_partIdA; - int m_triangleIndexA; - btVector3* m_triangleVerticesA; - btTriangleInfoMap* m_triangleInfoMap; - + int m_partIdA; + int m_triangleIndexA; + btVector3* m_triangleVerticesA; + btTriangleInfoMap* m_triangleInfoMap; virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex) { @@ -69,18 +65,17 @@ struct btConnectivityProcessor : public btTriangleCallback //search for shared vertices and edges int numshared = 0; - int sharedVertsA[3]={-1,-1,-1}; - int sharedVertsB[3]={-1,-1,-1}; + int sharedVertsA[3] = {-1, -1, -1}; + int sharedVertsB[3] = {-1, -1, -1}; ///skip degenerate triangles - btScalar crossBSqr = ((triangle[1]-triangle[0]).cross(triangle[2]-triangle[0])).length2(); + btScalar crossBSqr = ((triangle[1] - triangle[0]).cross(triangle[2] - triangle[0])).length2(); if (crossBSqr < m_triangleInfoMap->m_equalVertexThreshold) return; - - btScalar crossASqr = ((m_triangleVerticesA[1]-m_triangleVerticesA[0]).cross(m_triangleVerticesA[2]-m_triangleVerticesA[0])).length2(); + btScalar crossASqr = ((m_triangleVerticesA[1] - m_triangleVerticesA[0]).cross(m_triangleVerticesA[2] - m_triangleVerticesA[0])).length2(); ///skip degenerate triangles - if (crossASqr< m_triangleInfoMap->m_equalVertexThreshold) + if (crossASqr < m_triangleInfoMap->m_equalVertexThreshold) return; #if 0 @@ -96,36 +91,36 @@ struct btConnectivityProcessor : public btTriangleCallback triangle[2].getX(),triangle[2].getY(),triangle[2].getZ()); #endif - for (int i=0;i<3;i++) + for (int i = 0; i < 3; i++) { - for (int j=0;j<3;j++) + for (int j = 0; j < 3; j++) { - if ( (m_triangleVerticesA[i]-triangle[j]).length2() < m_triangleInfoMap->m_equalVertexThreshold) + if ((m_triangleVerticesA[i] - triangle[j]).length2() < m_triangleInfoMap->m_equalVertexThreshold) { sharedVertsA[numshared] = i; sharedVertsB[numshared] = j; numshared++; ///degenerate case - if(numshared >= 3) + if (numshared >= 3) return; } } ///degenerate case - if(numshared >= 3) + if (numshared >= 3) return; } switch (numshared) { - case 0: + case 0: { break; } - case 1: + case 1: { //shared vertex break; } - case 2: + case 2: { //shared edge //we need to make sure the edge is in the order V2V0 and not V0V2 so that the signs are correct @@ -138,26 +133,25 @@ struct btConnectivityProcessor : public btTriangleCallback sharedVertsB[0] = tmp; } - int hash = btGetHash(m_partIdA,m_triangleIndexA); + int hash = btGetHash(m_partIdA, m_triangleIndexA); btTriangleInfo* info = m_triangleInfoMap->find(hash); if (!info) { btTriangleInfo tmp; - m_triangleInfoMap->insert(hash,tmp); + m_triangleInfoMap->insert(hash, tmp); info = m_triangleInfoMap->find(hash); } - int sumvertsA = sharedVertsA[0]+sharedVertsA[1]; - int otherIndexA = 3-sumvertsA; + int sumvertsA = sharedVertsA[0] + sharedVertsA[1]; + int otherIndexA = 3 - sumvertsA; - - btVector3 edge(m_triangleVerticesA[sharedVertsA[1]]-m_triangleVerticesA[sharedVertsA[0]]); + btVector3 edge(m_triangleVerticesA[sharedVertsA[1]] - m_triangleVerticesA[sharedVertsA[0]]); - btTriangleShape tA(m_triangleVerticesA[0],m_triangleVerticesA[1],m_triangleVerticesA[2]); - int otherIndexB = 3-(sharedVertsB[0]+sharedVertsB[1]); + btTriangleShape tA(m_triangleVerticesA[0], m_triangleVerticesA[1], m_triangleVerticesA[2]); + int otherIndexB = 3 - (sharedVertsB[0] + sharedVertsB[1]); - btTriangleShape tB(triangle[sharedVertsB[1]],triangle[sharedVertsB[0]],triangle[otherIndexB]); + btTriangleShape tB(triangle[sharedVertsB[1]], triangle[sharedVertsB[0]], triangle[otherIndexB]); //btTriangleShape tB(triangle[0],triangle[1],triangle[2]); btVector3 normalA; @@ -168,26 +162,25 @@ struct btConnectivityProcessor : public btTriangleCallback btVector3 edgeCrossA = edge.cross(normalA).normalize(); { - btVector3 tmp = m_triangleVerticesA[otherIndexA]-m_triangleVerticesA[sharedVertsA[0]]; + btVector3 tmp = m_triangleVerticesA[otherIndexA] - m_triangleVerticesA[sharedVertsA[0]]; if (edgeCrossA.dot(tmp) < 0) { - edgeCrossA*=-1; + edgeCrossA *= -1; } } btVector3 edgeCrossB = edge.cross(normalB).normalize(); { - btVector3 tmp = triangle[otherIndexB]-triangle[sharedVertsB[0]]; + btVector3 tmp = triangle[otherIndexB] - triangle[sharedVertsB[0]]; if (edgeCrossB.dot(tmp) < 0) { - edgeCrossB*=-1; + edgeCrossB *= -1; } } - btScalar angle2 = 0; - btScalar ang4 = 0.f; - + btScalar angle2 = 0; + btScalar ang4 = 0.f; btVector3 calculatedEdge = edgeCrossA.cross(edgeCrossB); btScalar len2 = calculatedEdge.length2(); @@ -196,52 +189,47 @@ struct btConnectivityProcessor : public btTriangleCallback //btVector3 calculatedNormalB = normalA; bool isConvex = false; - if (len2<m_triangleInfoMap->m_planarEpsilon) + if (len2 < m_triangleInfoMap->m_planarEpsilon) { angle2 = 0.f; ang4 = 0.f; - } else + } + else { - calculatedEdge.normalize(); btVector3 calculatedNormalA = calculatedEdge.cross(edgeCrossA); calculatedNormalA.normalize(); - angle2 = btGetAngle(calculatedNormalA,edgeCrossA,edgeCrossB); - ang4 = SIMD_PI-angle2; + angle2 = btGetAngle(calculatedNormalA, edgeCrossA, edgeCrossB); + ang4 = SIMD_PI - angle2; btScalar dotA = normalA.dot(edgeCrossB); ///@todo: check if we need some epsilon, due to floating point imprecision - isConvex = (dotA<0.); + isConvex = (dotA < 0.); correctedAngle = isConvex ? ang4 : -ang4; } - - - - - //alternatively use + //alternatively use //btVector3 calculatedNormalB2 = quatRotate(orn,normalA); - switch (sumvertsA) { - case 1: + case 1: { - btVector3 edge = m_triangleVerticesA[0]-m_triangleVerticesA[1]; - btQuaternion orn(edge,-correctedAngle); - btVector3 computedNormalB = quatRotate(orn,normalA); + btVector3 edge = m_triangleVerticesA[0] - m_triangleVerticesA[1]; + btQuaternion orn(edge, -correctedAngle); + btVector3 computedNormalB = quatRotate(orn, normalA); btScalar bla = computedNormalB.dot(normalB); - if (bla<0) + if (bla < 0) { - computedNormalB*=-1; + computedNormalB *= -1; info->m_flags |= TRI_INFO_V0V1_SWAP_NORMALB; } #ifdef DEBUG_INTERNAL_EDGE - if ((computedNormalB-normalB).length()>0.0001) + if ((computedNormalB - normalB).length() > 0.0001) { printf("warning: normals not identical\n"); } -#endif//DEBUG_INTERNAL_EDGE +#endif //DEBUG_INTERNAL_EDGE info->m_edgeV0V1Angle = -correctedAngle; @@ -249,44 +237,44 @@ struct btConnectivityProcessor : public btTriangleCallback info->m_flags |= TRI_INFO_V0V1_CONVEX; break; } - case 2: + case 2: { - btVector3 edge = m_triangleVerticesA[2]-m_triangleVerticesA[0]; - btQuaternion orn(edge,-correctedAngle); - btVector3 computedNormalB = quatRotate(orn,normalA); - if (computedNormalB.dot(normalB)<0) + btVector3 edge = m_triangleVerticesA[2] - m_triangleVerticesA[0]; + btQuaternion orn(edge, -correctedAngle); + btVector3 computedNormalB = quatRotate(orn, normalA); + if (computedNormalB.dot(normalB) < 0) { - computedNormalB*=-1; + computedNormalB *= -1; info->m_flags |= TRI_INFO_V2V0_SWAP_NORMALB; } #ifdef DEBUG_INTERNAL_EDGE - if ((computedNormalB-normalB).length()>0.0001) + if ((computedNormalB - normalB).length() > 0.0001) { printf("warning: normals not identical\n"); } -#endif //DEBUG_INTERNAL_EDGE +#endif //DEBUG_INTERNAL_EDGE info->m_edgeV2V0Angle = -correctedAngle; if (isConvex) info->m_flags |= TRI_INFO_V2V0_CONVEX; - break; + break; } - case 3: + case 3: { - btVector3 edge = m_triangleVerticesA[1]-m_triangleVerticesA[2]; - btQuaternion orn(edge,-correctedAngle); - btVector3 computedNormalB = quatRotate(orn,normalA); - if (computedNormalB.dot(normalB)<0) + btVector3 edge = m_triangleVerticesA[1] - m_triangleVerticesA[2]; + btQuaternion orn(edge, -correctedAngle); + btVector3 computedNormalB = quatRotate(orn, normalA); + if (computedNormalB.dot(normalB) < 0) { info->m_flags |= TRI_INFO_V1V2_SWAP_NORMALB; - computedNormalB*=-1; + computedNormalB *= -1; } #ifdef DEBUG_INTERNAL_EDGE - if ((computedNormalB-normalB).length()>0.0001) + if ((computedNormalB - normalB).length() > 0.0001) { printf("warning: normals not identical\n"); } -#endif //DEBUG_INTERNAL_EDGE +#endif //DEBUG_INTERNAL_EDGE info->m_edgeV1V2Angle = -correctedAngle; if (isConvex) @@ -297,18 +285,50 @@ struct btConnectivityProcessor : public btTriangleCallback break; } - default: + default: { // printf("warning: duplicate triangle\n"); } - } } }; + + +struct b3ProcessAllTrianglesHeightfield: public btTriangleCallback +{ + btHeightfieldTerrainShape* m_heightfieldShape; + btTriangleInfoMap* m_triangleInfoMap; + + + b3ProcessAllTrianglesHeightfield(btHeightfieldTerrainShape* heightFieldShape, btTriangleInfoMap* triangleInfoMap) + :m_heightfieldShape(heightFieldShape), + m_triangleInfoMap(triangleInfoMap) + { + } + virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex) + { + btConnectivityProcessor connectivityProcessor; + connectivityProcessor.m_partIdA = partId; + connectivityProcessor.m_triangleIndexA = triangleIndex; + connectivityProcessor.m_triangleVerticesA = triangle; + connectivityProcessor.m_triangleInfoMap = m_triangleInfoMap; + btVector3 aabbMin, aabbMax; + aabbMin.setValue(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT)); + aabbMax.setValue(btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT)); + aabbMin.setMin(triangle[0]); + aabbMax.setMax(triangle[0]); + aabbMin.setMin(triangle[1]); + aabbMax.setMax(triangle[1]); + aabbMin.setMin(triangle[2]); + aabbMax.setMax(triangle[2]); + + m_heightfieldShape->processAllTriangles(&connectivityProcessor, aabbMin, aabbMax); + } +}; ///////////////////////////////////////////////////////// ///////////////////////////////////////////////////////// -void btGenerateInternalEdgeInfo (btBvhTriangleMeshShape*trimeshShape, btTriangleInfoMap* triangleInfoMap) +void btGenerateInternalEdgeInfo(btBvhTriangleMeshShape* trimeshShape, btTriangleInfoMap* triangleInfoMap) { //the user pointer shouldn't already be used for other purposes, we intend to store connectivity info there! if (trimeshShape->getTriangleInfoMap()) @@ -319,46 +339,51 @@ void btGenerateInternalEdgeInfo (btBvhTriangleMeshShape*trimeshShape, btTriangle btStridingMeshInterface* meshInterface = trimeshShape->getMeshInterface(); const btVector3& meshScaling = meshInterface->getScaling(); - for (int partId = 0; partId< meshInterface->getNumSubParts();partId++) + for (int partId = 0; partId < meshInterface->getNumSubParts(); partId++) { - const unsigned char *vertexbase = 0; + const unsigned char* vertexbase = 0; int numverts = 0; PHY_ScalarType type = PHY_INTEGER; int stride = 0; - const unsigned char *indexbase = 0; + const unsigned char* indexbase = 0; int indexstride = 0; int numfaces = 0; PHY_ScalarType indicestype = PHY_INTEGER; //PHY_ScalarType indexType=0; btVector3 triangleVerts[3]; - meshInterface->getLockedReadOnlyVertexIndexBase(&vertexbase,numverts, type,stride,&indexbase,indexstride,numfaces,indicestype,partId); - btVector3 aabbMin,aabbMax; + meshInterface->getLockedReadOnlyVertexIndexBase(&vertexbase, numverts, type, stride, &indexbase, indexstride, numfaces, indicestype, partId); + btVector3 aabbMin, aabbMax; - for (int triangleIndex = 0 ; triangleIndex < numfaces;triangleIndex++) + for (int triangleIndex = 0; triangleIndex < numfaces; triangleIndex++) { - unsigned int* gfxbase = (unsigned int*)(indexbase+triangleIndex*indexstride); + unsigned int* gfxbase = (unsigned int*)(indexbase + triangleIndex * indexstride); - for (int j=2;j>=0;j--) + for (int j = 2; j >= 0; j--) { - - int graphicsindex = indicestype==PHY_SHORT?((unsigned short*)gfxbase)[j]:gfxbase[j]; + int graphicsindex; + switch (indicestype) { + case PHY_INTEGER: graphicsindex = gfxbase[j]; break; + case PHY_SHORT: graphicsindex = ((unsigned short*)gfxbase)[j]; break; + case PHY_UCHAR: graphicsindex = ((unsigned char*)gfxbase)[j]; break; + default: btAssert(0); + } if (type == PHY_FLOAT) { - float* graphicsbase = (float*)(vertexbase+graphicsindex*stride); + float* graphicsbase = (float*)(vertexbase + graphicsindex * stride); triangleVerts[j] = btVector3( - graphicsbase[0]*meshScaling.getX(), - graphicsbase[1]*meshScaling.getY(), - graphicsbase[2]*meshScaling.getZ()); + graphicsbase[0] * meshScaling.getX(), + graphicsbase[1] * meshScaling.getY(), + graphicsbase[2] * meshScaling.getZ()); } else { - double* graphicsbase = (double*)(vertexbase+graphicsindex*stride); - triangleVerts[j] = btVector3( btScalar(graphicsbase[0]*meshScaling.getX()), btScalar(graphicsbase[1]*meshScaling.getY()), btScalar(graphicsbase[2]*meshScaling.getZ())); + double* graphicsbase = (double*)(vertexbase + graphicsindex * stride); + triangleVerts[j] = btVector3(btScalar(graphicsbase[0] * meshScaling.getX()), btScalar(graphicsbase[1] * meshScaling.getY()), btScalar(graphicsbase[2] * meshScaling.getZ())); } } - aabbMin.setValue(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)); - aabbMax.setValue(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT)); + aabbMin.setValue(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT)); + aabbMax.setValue(btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT)); aabbMin.setMin(triangleVerts[0]); aabbMax.setMax(triangleVerts[0]); aabbMin.setMin(triangleVerts[1]); @@ -370,131 +395,177 @@ void btGenerateInternalEdgeInfo (btBvhTriangleMeshShape*trimeshShape, btTriangle connectivityProcessor.m_partIdA = partId; connectivityProcessor.m_triangleIndexA = triangleIndex; connectivityProcessor.m_triangleVerticesA = &triangleVerts[0]; - connectivityProcessor.m_triangleInfoMap = triangleInfoMap; + connectivityProcessor.m_triangleInfoMap = triangleInfoMap; - trimeshShape->processAllTriangles(&connectivityProcessor,aabbMin,aabbMax); + trimeshShape->processAllTriangles(&connectivityProcessor, aabbMin, aabbMax); } - } - } +void btGenerateInternalEdgeInfo(btHeightfieldTerrainShape* heightfieldShape, btTriangleInfoMap* triangleInfoMap) +{ + + //the user pointer shouldn't already be used for other purposes, we intend to store connectivity info there! + if (heightfieldShape->getTriangleInfoMap()) + return; + + heightfieldShape->setTriangleInfoMap(triangleInfoMap); + + //get all the triangles of the heightfield + btVector3 aabbMin, aabbMax; + + aabbMax.setValue(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT)); + aabbMin.setValue(btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT)); + + b3ProcessAllTrianglesHeightfield processHeightfield(heightfieldShape, triangleInfoMap); + heightfieldShape->processAllTriangles(&processHeightfield, aabbMin, aabbMax); + +} // Given a point and a line segment (defined by two points), compute the closest point // in the line. Cap the point at the endpoints of the line segment. -void btNearestPointInLineSegment(const btVector3 &point, const btVector3& line0, const btVector3& line1, btVector3& nearestPoint) +void btNearestPointInLineSegment(const btVector3& point, const btVector3& line0, const btVector3& line1, btVector3& nearestPoint) { - btVector3 lineDelta = line1 - line0; + btVector3 lineDelta = line1 - line0; // Handle degenerate lines - if ( lineDelta.fuzzyZero()) + if (lineDelta.fuzzyZero()) { nearestPoint = line0; } else { - btScalar delta = (point-line0).dot(lineDelta) / (lineDelta).dot(lineDelta); + btScalar delta = (point - line0).dot(lineDelta) / (lineDelta).dot(lineDelta); // Clamp the point to conform to the segment's endpoints - if ( delta < 0 ) + if (delta < 0) delta = 0; - else if ( delta > 1 ) + else if (delta > 1) delta = 1; - nearestPoint = line0 + lineDelta*delta; + nearestPoint = line0 + lineDelta * delta; } } - - - -bool btClampNormal(const btVector3& edge,const btVector3& tri_normal_org,const btVector3& localContactNormalOnB, btScalar correctedEdgeAngle, btVector3 & clampedLocalNormal) +bool btClampNormal(const btVector3& edge, const btVector3& tri_normal_org, const btVector3& localContactNormalOnB, btScalar correctedEdgeAngle, btVector3& clampedLocalNormal) { btVector3 tri_normal = tri_normal_org; //we only have a local triangle normal, not a local contact normal -> only normal in world space... //either compute the current angle all in local space, or all in world space btVector3 edgeCross = edge.cross(tri_normal).normalize(); - btScalar curAngle = btGetAngle(edgeCross,tri_normal,localContactNormalOnB); + btScalar curAngle = btGetAngle(edgeCross, tri_normal, localContactNormalOnB); - if (correctedEdgeAngle<0) + if (correctedEdgeAngle < 0) { if (curAngle < correctedEdgeAngle) { - btScalar diffAngle = correctedEdgeAngle-curAngle; - btQuaternion rotation(edge,diffAngle ); - clampedLocalNormal = btMatrix3x3(rotation)*localContactNormalOnB; + btScalar diffAngle = correctedEdgeAngle - curAngle; + btQuaternion rotation(edge, diffAngle); + clampedLocalNormal = btMatrix3x3(rotation) * localContactNormalOnB; return true; } } - if (correctedEdgeAngle>=0) + if (correctedEdgeAngle >= 0) { if (curAngle > correctedEdgeAngle) { - btScalar diffAngle = correctedEdgeAngle-curAngle; - btQuaternion rotation(edge,diffAngle ); - clampedLocalNormal = btMatrix3x3(rotation)*localContactNormalOnB; + btScalar diffAngle = correctedEdgeAngle - curAngle; + btQuaternion rotation(edge, diffAngle); + clampedLocalNormal = btMatrix3x3(rotation) * localContactNormalOnB; return true; } } return false; } - - /// Changes a btManifoldPoint collision normal to the normal from the mesh. -void btAdjustInternalEdgeContacts(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap,const btCollisionObjectWrapper* colObj1Wrap, int partId0, int index0, int normalAdjustFlags) +void btAdjustInternalEdgeContacts(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap, const btCollisionObjectWrapper* colObj1Wrap, int partId0, int index0, int normalAdjustFlags) { //btAssert(colObj0->getCollisionShape()->getShapeType() == TRIANGLE_SHAPE_PROXYTYPE); if (colObj0Wrap->getCollisionShape()->getShapeType() != TRIANGLE_SHAPE_PROXYTYPE) return; + + btTriangleInfoMap* triangleInfoMapPtr = 0; + + if (colObj0Wrap->getCollisionObject()->getCollisionShape()->getShapeType() == TERRAIN_SHAPE_PROXYTYPE) + { + btHeightfieldTerrainShape* heightfield = (btHeightfieldTerrainShape*)colObj0Wrap->getCollisionObject()->getCollisionShape(); + triangleInfoMapPtr = heightfield->getTriangleInfoMap(); + +//#define USE_HEIGHTFIELD_TRIANGLES +#ifdef USE_HEIGHTFIELD_TRIANGLES + btVector3 newNormal = btVector3(0, 0, 1); + + const btTriangleShape* tri_shape = static_cast<const btTriangleShape*>(colObj0Wrap->getCollisionShape()); + btVector3 tri_normal; + tri_shape->calcNormal(tri_normal); + newNormal = tri_normal; + // cp.m_distance1 = cp.m_distance1 * newNormal.dot(cp.m_normalWorldOnB); + cp.m_normalWorldOnB = newNormal; + // Reproject collision point along normal. (what about cp.m_distance1?) + cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1; + cp.m_localPointB = colObj0Wrap->getWorldTransform().invXform(cp.m_positionWorldOnB); + return; +#endif + } + + btBvhTriangleMeshShape* trimesh = 0; + + if (colObj0Wrap->getCollisionObject()->getCollisionShape()->getShapeType() == SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE) + { + trimesh = ((btScaledBvhTriangleMeshShape*)colObj0Wrap->getCollisionObject()->getCollisionShape())->getChildShape(); + } + else + { + if (colObj0Wrap->getCollisionObject()->getCollisionShape()->getShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE) + { + trimesh = (btBvhTriangleMeshShape*)colObj0Wrap->getCollisionObject()->getCollisionShape(); + } + } + if (trimesh) + { + triangleInfoMapPtr = (btTriangleInfoMap*)trimesh->getTriangleInfoMap(); + } + - if( colObj0Wrap->getCollisionObject()->getCollisionShape()->getShapeType() == SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE ) - trimesh = ((btScaledBvhTriangleMeshShape*)colObj0Wrap->getCollisionObject()->getCollisionShape())->getChildShape(); - else - trimesh = (btBvhTriangleMeshShape*)colObj0Wrap->getCollisionObject()->getCollisionShape(); - - btTriangleInfoMap* triangleInfoMapPtr = (btTriangleInfoMap*) trimesh->getTriangleInfoMap(); if (!triangleInfoMapPtr) return; - int hash = btGetHash(partId0,index0); - + int hash = btGetHash(partId0, index0); btTriangleInfo* info = triangleInfoMapPtr->find(hash); if (!info) return; - btScalar frontFacing = (normalAdjustFlags & BT_TRIANGLE_CONVEX_BACKFACE_MODE)==0? 1.f : -1.f; - + btScalar frontFacing = (normalAdjustFlags & BT_TRIANGLE_CONVEX_BACKFACE_MODE) == 0 ? 1.f : -1.f; + const btTriangleShape* tri_shape = static_cast<const btTriangleShape*>(colObj0Wrap->getCollisionShape()); - btVector3 v0,v1,v2; - tri_shape->getVertex(0,v0); - tri_shape->getVertex(1,v1); - tri_shape->getVertex(2,v2); + btVector3 v0, v1, v2; + tri_shape->getVertex(0, v0); + tri_shape->getVertex(1, v1); + tri_shape->getVertex(2, v2); //btVector3 center = (v0+v1+v2)*btScalar(1./3.); - btVector3 red(1,0,0), green(0,1,0),blue(0,0,1),white(1,1,1),black(0,0,0); + btVector3 red(1, 0, 0), green(0, 1, 0), blue(0, 0, 1), white(1, 1, 1), black(0, 0, 0); btVector3 tri_normal; tri_shape->calcNormal(tri_normal); //btScalar dot = tri_normal.dot(cp.m_normalWorldOnB); btVector3 nearest; - btNearestPointInLineSegment(cp.m_localPointB,v0,v1,nearest); + btNearestPointInLineSegment(cp.m_localPointB, v0, v1, nearest); btVector3 contact = cp.m_localPointB; #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW const btTransform& tr = colObj0->getWorldTransform(); - btDebugDrawLine(tr*nearest,tr*cp.m_localPointB,red); -#endif //BT_INTERNAL_EDGE_DEBUG_DRAW - - + btDebugDrawLine(tr * nearest, tr * cp.m_localPointB, red); +#endif //BT_INTERNAL_EDGE_DEBUG_DRAW bool isNearEdge = false; @@ -502,334 +573,325 @@ void btAdjustInternalEdgeContacts(btManifoldPoint& cp, const btCollisionObjectWr int numConvexEdgeHits = 0; btVector3 localContactNormalOnB = colObj0Wrap->getWorldTransform().getBasis().transpose() * cp.m_normalWorldOnB; - localContactNormalOnB.normalize();//is this necessary? - + localContactNormalOnB.normalize(); //is this necessary? + // Get closest edge - int bestedge=-1; - btScalar disttobestedge=BT_LARGE_FLOAT; + int bestedge = -1; + btScalar disttobestedge = BT_LARGE_FLOAT; // // Edge 0 -> 1 - if (btFabs(info->m_edgeV0V1Angle)< triangleInfoMapPtr->m_maxEdgeAngleThreshold) - { - btVector3 nearest; - btNearestPointInLineSegment( cp.m_localPointB, v0, v1, nearest ); - btScalar len=(contact-nearest).length(); - // - if( len < disttobestedge ) - { - bestedge=0; - disttobestedge=len; - } - } + if (btFabs(info->m_edgeV0V1Angle) < triangleInfoMapPtr->m_maxEdgeAngleThreshold) + { + btVector3 nearest; + btNearestPointInLineSegment(cp.m_localPointB, v0, v1, nearest); + btScalar len = (contact - nearest).length(); + // + if (len < disttobestedge) + { + bestedge = 0; + disttobestedge = len; + } + } // Edge 1 -> 2 - if (btFabs(info->m_edgeV1V2Angle)< triangleInfoMapPtr->m_maxEdgeAngleThreshold) - { - btVector3 nearest; - btNearestPointInLineSegment( cp.m_localPointB, v1, v2, nearest ); - btScalar len=(contact-nearest).length(); - // - if( len < disttobestedge ) - { - bestedge=1; - disttobestedge=len; - } - } + if (btFabs(info->m_edgeV1V2Angle) < triangleInfoMapPtr->m_maxEdgeAngleThreshold) + { + btVector3 nearest; + btNearestPointInLineSegment(cp.m_localPointB, v1, v2, nearest); + btScalar len = (contact - nearest).length(); + // + if (len < disttobestedge) + { + bestedge = 1; + disttobestedge = len; + } + } // Edge 2 -> 0 - if (btFabs(info->m_edgeV2V0Angle)< triangleInfoMapPtr->m_maxEdgeAngleThreshold) - { - btVector3 nearest; - btNearestPointInLineSegment( cp.m_localPointB, v2, v0, nearest ); - btScalar len=(contact-nearest).length(); - // - if( len < disttobestedge ) - { - bestedge=2; - disttobestedge=len; - } - } - + if (btFabs(info->m_edgeV2V0Angle) < triangleInfoMapPtr->m_maxEdgeAngleThreshold) + { + btVector3 nearest; + btNearestPointInLineSegment(cp.m_localPointB, v2, v0, nearest); + btScalar len = (contact - nearest).length(); + // + if (len < disttobestedge) + { + bestedge = 2; + disttobestedge = len; + } + } + #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW - btVector3 upfix=tri_normal * btVector3(0.1f,0.1f,0.1f); - btDebugDrawLine(tr * v0 + upfix, tr * v1 + upfix, red ); -#endif - if (btFabs(info->m_edgeV0V1Angle)< triangleInfoMapPtr->m_maxEdgeAngleThreshold) + btVector3 upfix = tri_normal * btVector3(0.1f, 0.1f, 0.1f); + btDebugDrawLine(tr * v0 + upfix, tr * v1 + upfix, red); +#endif + if (btFabs(info->m_edgeV0V1Angle) < triangleInfoMapPtr->m_maxEdgeAngleThreshold) { #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW - btDebugDrawLine(tr*contact,tr*(contact+cp.m_normalWorldOnB*10),black); + btDebugDrawLine(tr * contact, tr * (contact + cp.m_normalWorldOnB * 10), black); #endif - btScalar len = (contact-nearest).length(); - if(len<triangleInfoMapPtr->m_edgeDistanceThreshold) - if( bestedge==0 ) - { - btVector3 edge(v0-v1); - isNearEdge = true; - - if (info->m_edgeV0V1Angle==btScalar(0)) - { - numConcaveEdgeHits++; - } else + btScalar len = (contact - nearest).length(); + if (len < triangleInfoMapPtr->m_edgeDistanceThreshold) + if (bestedge == 0) { + btVector3 edge(v0 - v1); + isNearEdge = true; - bool isEdgeConvex = (info->m_flags & TRI_INFO_V0V1_CONVEX); - btScalar swapFactor = isEdgeConvex ? btScalar(1) : btScalar(-1); - #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW - btDebugDrawLine(tr*nearest,tr*(nearest+swapFactor*tri_normal*10),white); - #endif //BT_INTERNAL_EDGE_DEBUG_DRAW + if (info->m_edgeV0V1Angle == btScalar(0)) + { + numConcaveEdgeHits++; + } + else + { + bool isEdgeConvex = (info->m_flags & TRI_INFO_V0V1_CONVEX); + btScalar swapFactor = isEdgeConvex ? btScalar(1) : btScalar(-1); +#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW + btDebugDrawLine(tr * nearest, tr * (nearest + swapFactor * tri_normal * 10), white); +#endif //BT_INTERNAL_EDGE_DEBUG_DRAW - btVector3 nA = swapFactor * tri_normal; + btVector3 nA = swapFactor * tri_normal; - btQuaternion orn(edge,info->m_edgeV0V1Angle); - btVector3 computedNormalB = quatRotate(orn,tri_normal); - if (info->m_flags & TRI_INFO_V0V1_SWAP_NORMALB) - computedNormalB*=-1; - btVector3 nB = swapFactor*computedNormalB; + btQuaternion orn(edge, info->m_edgeV0V1Angle); + btVector3 computedNormalB = quatRotate(orn, tri_normal); + if (info->m_flags & TRI_INFO_V0V1_SWAP_NORMALB) + computedNormalB *= -1; + btVector3 nB = swapFactor * computedNormalB; - btScalar NdotA = localContactNormalOnB.dot(nA); - btScalar NdotB = localContactNormalOnB.dot(nB); - bool backFacingNormal = (NdotA< triangleInfoMapPtr->m_convexEpsilon) && (NdotB<triangleInfoMapPtr->m_convexEpsilon); + btScalar NdotA = localContactNormalOnB.dot(nA); + btScalar NdotB = localContactNormalOnB.dot(nB); + bool backFacingNormal = (NdotA < triangleInfoMapPtr->m_convexEpsilon) && (NdotB < triangleInfoMapPtr->m_convexEpsilon); #ifdef DEBUG_INTERNAL_EDGE - { - - btDebugDrawLine(cp.getPositionWorldOnB(),cp.getPositionWorldOnB()+tr.getBasis()*(nB*20),red); - } -#endif //DEBUG_INTERNAL_EDGE - + { + btDebugDrawLine(cp.getPositionWorldOnB(), cp.getPositionWorldOnB() + tr.getBasis() * (nB * 20), red); + } +#endif //DEBUG_INTERNAL_EDGE - if (backFacingNormal) - { - numConcaveEdgeHits++; - } - else - { - numConvexEdgeHits++; - btVector3 clampedLocalNormal; - bool isClamped = btClampNormal(edge,swapFactor*tri_normal,localContactNormalOnB, info->m_edgeV0V1Angle,clampedLocalNormal); - if (isClamped) + if (backFacingNormal) { - if (((normalAdjustFlags & BT_TRIANGLE_CONVEX_DOUBLE_SIDED)!=0) || (clampedLocalNormal.dot(frontFacing*tri_normal)>0)) + numConcaveEdgeHits++; + } + else + { + numConvexEdgeHits++; + btVector3 clampedLocalNormal; + bool isClamped = btClampNormal(edge, swapFactor * tri_normal, localContactNormalOnB, info->m_edgeV0V1Angle, clampedLocalNormal); + if (isClamped) { - btVector3 newNormal = colObj0Wrap->getWorldTransform().getBasis() * clampedLocalNormal; - // cp.m_distance1 = cp.m_distance1 * newNormal.dot(cp.m_normalWorldOnB); - cp.m_normalWorldOnB = newNormal; - // Reproject collision point along normal. (what about cp.m_distance1?) - cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1; - cp.m_localPointB = colObj0Wrap->getWorldTransform().invXform(cp.m_positionWorldOnB); - + if (((normalAdjustFlags & BT_TRIANGLE_CONVEX_DOUBLE_SIDED) != 0) || (clampedLocalNormal.dot(frontFacing * tri_normal) > 0)) + { + btVector3 newNormal = colObj0Wrap->getWorldTransform().getBasis() * clampedLocalNormal; + // cp.m_distance1 = cp.m_distance1 * newNormal.dot(cp.m_normalWorldOnB); + cp.m_normalWorldOnB = newNormal; + // Reproject collision point along normal. (what about cp.m_distance1?) + cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1; + cp.m_localPointB = colObj0Wrap->getWorldTransform().invXform(cp.m_positionWorldOnB); + } } } } } - } } - btNearestPointInLineSegment(contact,v1,v2,nearest); + btNearestPointInLineSegment(contact, v1, v2, nearest); #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW - btDebugDrawLine(tr*nearest,tr*cp.m_localPointB,green); -#endif //BT_INTERNAL_EDGE_DEBUG_DRAW + btDebugDrawLine(tr * nearest, tr * cp.m_localPointB, green); +#endif //BT_INTERNAL_EDGE_DEBUG_DRAW #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW - btDebugDrawLine(tr * v1 + upfix, tr * v2 + upfix , green ); -#endif + btDebugDrawLine(tr * v1 + upfix, tr * v2 + upfix, green); +#endif - if (btFabs(info->m_edgeV1V2Angle)< triangleInfoMapPtr->m_maxEdgeAngleThreshold) + if (btFabs(info->m_edgeV1V2Angle) < triangleInfoMapPtr->m_maxEdgeAngleThreshold) { #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW - btDebugDrawLine(tr*contact,tr*(contact+cp.m_normalWorldOnB*10),black); -#endif //BT_INTERNAL_EDGE_DEBUG_DRAW - - - - btScalar len = (contact-nearest).length(); - if(len<triangleInfoMapPtr->m_edgeDistanceThreshold) - if( bestedge==1 ) - { - isNearEdge = true; -#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW - btDebugDrawLine(tr*nearest,tr*(nearest+tri_normal*10),white); -#endif //BT_INTERNAL_EDGE_DEBUG_DRAW - - btVector3 edge(v1-v2); - - isNearEdge = true; + btDebugDrawLine(tr * contact, tr * (contact + cp.m_normalWorldOnB * 10), black); +#endif //BT_INTERNAL_EDGE_DEBUG_DRAW - if (info->m_edgeV1V2Angle == btScalar(0)) + btScalar len = (contact - nearest).length(); + if (len < triangleInfoMapPtr->m_edgeDistanceThreshold) + if (bestedge == 1) { - numConcaveEdgeHits++; - } else - { - bool isEdgeConvex = (info->m_flags & TRI_INFO_V1V2_CONVEX)!=0; - btScalar swapFactor = isEdgeConvex ? btScalar(1) : btScalar(-1); - #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW - btDebugDrawLine(tr*nearest,tr*(nearest+swapFactor*tri_normal*10),white); - #endif //BT_INTERNAL_EDGE_DEBUG_DRAW - - btVector3 nA = swapFactor * tri_normal; - - btQuaternion orn(edge,info->m_edgeV1V2Angle); - btVector3 computedNormalB = quatRotate(orn,tri_normal); - if (info->m_flags & TRI_INFO_V1V2_SWAP_NORMALB) - computedNormalB*=-1; - btVector3 nB = swapFactor*computedNormalB; - -#ifdef DEBUG_INTERNAL_EDGE - { - btDebugDrawLine(cp.getPositionWorldOnB(),cp.getPositionWorldOnB()+tr.getBasis()*(nB*20),red); - } -#endif //DEBUG_INTERNAL_EDGE + isNearEdge = true; +#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW + btDebugDrawLine(tr * nearest, tr * (nearest + tri_normal * 10), white); +#endif //BT_INTERNAL_EDGE_DEBUG_DRAW + btVector3 edge(v1 - v2); - btScalar NdotA = localContactNormalOnB.dot(nA); - btScalar NdotB = localContactNormalOnB.dot(nB); - bool backFacingNormal = (NdotA< triangleInfoMapPtr->m_convexEpsilon) && (NdotB<triangleInfoMapPtr->m_convexEpsilon); + isNearEdge = true; - if (backFacingNormal) + if (info->m_edgeV1V2Angle == btScalar(0)) { numConcaveEdgeHits++; } else { - numConvexEdgeHits++; - btVector3 localContactNormalOnB = colObj0Wrap->getWorldTransform().getBasis().transpose() * cp.m_normalWorldOnB; - btVector3 clampedLocalNormal; - bool isClamped = btClampNormal(edge,swapFactor*tri_normal,localContactNormalOnB, info->m_edgeV1V2Angle,clampedLocalNormal); - if (isClamped) + bool isEdgeConvex = (info->m_flags & TRI_INFO_V1V2_CONVEX) != 0; + btScalar swapFactor = isEdgeConvex ? btScalar(1) : btScalar(-1); +#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW + btDebugDrawLine(tr * nearest, tr * (nearest + swapFactor * tri_normal * 10), white); +#endif //BT_INTERNAL_EDGE_DEBUG_DRAW + + btVector3 nA = swapFactor * tri_normal; + + btQuaternion orn(edge, info->m_edgeV1V2Angle); + btVector3 computedNormalB = quatRotate(orn, tri_normal); + if (info->m_flags & TRI_INFO_V1V2_SWAP_NORMALB) + computedNormalB *= -1; + btVector3 nB = swapFactor * computedNormalB; + +#ifdef DEBUG_INTERNAL_EDGE + { + btDebugDrawLine(cp.getPositionWorldOnB(), cp.getPositionWorldOnB() + tr.getBasis() * (nB * 20), red); + } +#endif //DEBUG_INTERNAL_EDGE + + btScalar NdotA = localContactNormalOnB.dot(nA); + btScalar NdotB = localContactNormalOnB.dot(nB); + bool backFacingNormal = (NdotA < triangleInfoMapPtr->m_convexEpsilon) && (NdotB < triangleInfoMapPtr->m_convexEpsilon); + + if (backFacingNormal) + { + numConcaveEdgeHits++; + } + else { - if (((normalAdjustFlags & BT_TRIANGLE_CONVEX_DOUBLE_SIDED)!=0) || (clampedLocalNormal.dot(frontFacing*tri_normal)>0)) + numConvexEdgeHits++; + btVector3 localContactNormalOnB = colObj0Wrap->getWorldTransform().getBasis().transpose() * cp.m_normalWorldOnB; + btVector3 clampedLocalNormal; + bool isClamped = btClampNormal(edge, swapFactor * tri_normal, localContactNormalOnB, info->m_edgeV1V2Angle, clampedLocalNormal); + if (isClamped) { - btVector3 newNormal = colObj0Wrap->getWorldTransform().getBasis() * clampedLocalNormal; - // cp.m_distance1 = cp.m_distance1 * newNormal.dot(cp.m_normalWorldOnB); - cp.m_normalWorldOnB = newNormal; - // Reproject collision point along normal. - cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1; - cp.m_localPointB = colObj0Wrap->getWorldTransform().invXform(cp.m_positionWorldOnB); + if (((normalAdjustFlags & BT_TRIANGLE_CONVEX_DOUBLE_SIDED) != 0) || (clampedLocalNormal.dot(frontFacing * tri_normal) > 0)) + { + btVector3 newNormal = colObj0Wrap->getWorldTransform().getBasis() * clampedLocalNormal; + // cp.m_distance1 = cp.m_distance1 * newNormal.dot(cp.m_normalWorldOnB); + cp.m_normalWorldOnB = newNormal; + // Reproject collision point along normal. + cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1; + cp.m_localPointB = colObj0Wrap->getWorldTransform().invXform(cp.m_positionWorldOnB); + } } } } } - } } - btNearestPointInLineSegment(contact,v2,v0,nearest); + btNearestPointInLineSegment(contact, v2, v0, nearest); #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW - btDebugDrawLine(tr*nearest,tr*cp.m_localPointB,blue); -#endif //BT_INTERNAL_EDGE_DEBUG_DRAW + btDebugDrawLine(tr * nearest, tr * cp.m_localPointB, blue); +#endif //BT_INTERNAL_EDGE_DEBUG_DRAW #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW - btDebugDrawLine(tr * v2 + upfix, tr * v0 + upfix , blue ); -#endif + btDebugDrawLine(tr * v2 + upfix, tr * v0 + upfix, blue); +#endif - if (btFabs(info->m_edgeV2V0Angle)< triangleInfoMapPtr->m_maxEdgeAngleThreshold) + if (btFabs(info->m_edgeV2V0Angle) < triangleInfoMapPtr->m_maxEdgeAngleThreshold) { - -#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW - btDebugDrawLine(tr*contact,tr*(contact+cp.m_normalWorldOnB*10),black); -#endif //BT_INTERNAL_EDGE_DEBUG_DRAW - - btScalar len = (contact-nearest).length(); - if(len<triangleInfoMapPtr->m_edgeDistanceThreshold) - if( bestedge==2 ) - { - isNearEdge = true; #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW - btDebugDrawLine(tr*nearest,tr*(nearest+tri_normal*10),white); -#endif //BT_INTERNAL_EDGE_DEBUG_DRAW - - btVector3 edge(v2-v0); + btDebugDrawLine(tr * contact, tr * (contact + cp.m_normalWorldOnB * 10), black); +#endif //BT_INTERNAL_EDGE_DEBUG_DRAW - if (info->m_edgeV2V0Angle==btScalar(0)) - { - numConcaveEdgeHits++; - } else + btScalar len = (contact - nearest).length(); + if (len < triangleInfoMapPtr->m_edgeDistanceThreshold) + if (bestedge == 2) { + isNearEdge = true; +#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW + btDebugDrawLine(tr * nearest, tr * (nearest + tri_normal * 10), white); +#endif //BT_INTERNAL_EDGE_DEBUG_DRAW - bool isEdgeConvex = (info->m_flags & TRI_INFO_V2V0_CONVEX)!=0; - btScalar swapFactor = isEdgeConvex ? btScalar(1) : btScalar(-1); - #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW - btDebugDrawLine(tr*nearest,tr*(nearest+swapFactor*tri_normal*10),white); - #endif //BT_INTERNAL_EDGE_DEBUG_DRAW - - btVector3 nA = swapFactor * tri_normal; - btQuaternion orn(edge,info->m_edgeV2V0Angle); - btVector3 computedNormalB = quatRotate(orn,tri_normal); - if (info->m_flags & TRI_INFO_V2V0_SWAP_NORMALB) - computedNormalB*=-1; - btVector3 nB = swapFactor*computedNormalB; - -#ifdef DEBUG_INTERNAL_EDGE - { - btDebugDrawLine(cp.getPositionWorldOnB(),cp.getPositionWorldOnB()+tr.getBasis()*(nB*20),red); - } -#endif //DEBUG_INTERNAL_EDGE - - btScalar NdotA = localContactNormalOnB.dot(nA); - btScalar NdotB = localContactNormalOnB.dot(nB); - bool backFacingNormal = (NdotA< triangleInfoMapPtr->m_convexEpsilon) && (NdotB<triangleInfoMapPtr->m_convexEpsilon); + btVector3 edge(v2 - v0); - if (backFacingNormal) + if (info->m_edgeV2V0Angle == btScalar(0)) { numConcaveEdgeHits++; } else { - numConvexEdgeHits++; - // printf("hitting convex edge\n"); + bool isEdgeConvex = (info->m_flags & TRI_INFO_V2V0_CONVEX) != 0; + btScalar swapFactor = isEdgeConvex ? btScalar(1) : btScalar(-1); +#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW + btDebugDrawLine(tr * nearest, tr * (nearest + swapFactor * tri_normal * 10), white); +#endif //BT_INTERNAL_EDGE_DEBUG_DRAW + + btVector3 nA = swapFactor * tri_normal; + btQuaternion orn(edge, info->m_edgeV2V0Angle); + btVector3 computedNormalB = quatRotate(orn, tri_normal); + if (info->m_flags & TRI_INFO_V2V0_SWAP_NORMALB) + computedNormalB *= -1; + btVector3 nB = swapFactor * computedNormalB; + +#ifdef DEBUG_INTERNAL_EDGE + { + btDebugDrawLine(cp.getPositionWorldOnB(), cp.getPositionWorldOnB() + tr.getBasis() * (nB * 20), red); + } +#endif //DEBUG_INTERNAL_EDGE + btScalar NdotA = localContactNormalOnB.dot(nA); + btScalar NdotB = localContactNormalOnB.dot(nB); + bool backFacingNormal = (NdotA < triangleInfoMapPtr->m_convexEpsilon) && (NdotB < triangleInfoMapPtr->m_convexEpsilon); - btVector3 localContactNormalOnB = colObj0Wrap->getWorldTransform().getBasis().transpose() * cp.m_normalWorldOnB; - btVector3 clampedLocalNormal; - bool isClamped = btClampNormal(edge,swapFactor*tri_normal,localContactNormalOnB,info->m_edgeV2V0Angle,clampedLocalNormal); - if (isClamped) + if (backFacingNormal) + { + numConcaveEdgeHits++; + } + else { - if (((normalAdjustFlags & BT_TRIANGLE_CONVEX_DOUBLE_SIDED)!=0) || (clampedLocalNormal.dot(frontFacing*tri_normal)>0)) + numConvexEdgeHits++; + // printf("hitting convex edge\n"); + + btVector3 localContactNormalOnB = colObj0Wrap->getWorldTransform().getBasis().transpose() * cp.m_normalWorldOnB; + btVector3 clampedLocalNormal; + bool isClamped = btClampNormal(edge, swapFactor * tri_normal, localContactNormalOnB, info->m_edgeV2V0Angle, clampedLocalNormal); + if (isClamped) { - btVector3 newNormal = colObj0Wrap->getWorldTransform().getBasis() * clampedLocalNormal; - // cp.m_distance1 = cp.m_distance1 * newNormal.dot(cp.m_normalWorldOnB); - cp.m_normalWorldOnB = newNormal; - // Reproject collision point along normal. - cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1; - cp.m_localPointB = colObj0Wrap->getWorldTransform().invXform(cp.m_positionWorldOnB); + if (((normalAdjustFlags & BT_TRIANGLE_CONVEX_DOUBLE_SIDED) != 0) || (clampedLocalNormal.dot(frontFacing * tri_normal) > 0)) + { + btVector3 newNormal = colObj0Wrap->getWorldTransform().getBasis() * clampedLocalNormal; + // cp.m_distance1 = cp.m_distance1 * newNormal.dot(cp.m_normalWorldOnB); + cp.m_normalWorldOnB = newNormal; + // Reproject collision point along normal. + cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1; + cp.m_localPointB = colObj0Wrap->getWorldTransform().invXform(cp.m_positionWorldOnB); + } } } - } + } } - - - } } #ifdef DEBUG_INTERNAL_EDGE { - btVector3 color(0,1,1); - btDebugDrawLine(cp.getPositionWorldOnB(),cp.getPositionWorldOnB()+cp.m_normalWorldOnB*10,color); + btVector3 color(0, 1, 1); + btDebugDrawLine(cp.getPositionWorldOnB(), cp.getPositionWorldOnB() + cp.m_normalWorldOnB * 10, color); } -#endif //DEBUG_INTERNAL_EDGE +#endif //DEBUG_INTERNAL_EDGE if (isNearEdge) { - - if (numConcaveEdgeHits>0) + if (numConcaveEdgeHits > 0) { - if ((normalAdjustFlags & BT_TRIANGLE_CONCAVE_DOUBLE_SIDED)!=0) + if ((normalAdjustFlags & BT_TRIANGLE_CONCAVE_DOUBLE_SIDED) != 0) { //fix tri_normal so it pointing the same direction as the current local contact normal if (tri_normal.dot(localContactNormalOnB) < 0) { tri_normal *= -1; } - cp.m_normalWorldOnB = colObj0Wrap->getWorldTransform().getBasis()*tri_normal; - } else + cp.m_normalWorldOnB = colObj0Wrap->getWorldTransform().getBasis() * tri_normal; + } + else { - btVector3 newNormal = tri_normal *frontFacing; + btVector3 newNormal = tri_normal * frontFacing; //if the tri_normal is pointing opposite direction as the current local contact normal, skip it - btScalar d = newNormal.dot(localContactNormalOnB) ; - if (d< 0) + btScalar d = newNormal.dot(localContactNormalOnB); + if (d < 0) { return; } //modify the normal to be the triangle normal (or backfacing normal) - cp.m_normalWorldOnB = colObj0Wrap->getWorldTransform().getBasis() *newNormal; + cp.m_normalWorldOnB = colObj0Wrap->getWorldTransform().getBasis() * newNormal; } - + // Reproject collision point along normal. cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1; cp.m_localPointB = colObj0Wrap->getWorldTransform().invXform(cp.m_positionWorldOnB); |