#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" #include "BulletCollision/NarrowPhaseCollision/btManifoldPoint.h" #include "LinearMath/btIDebugDraw.h" #include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h" //#define DEBUG_INTERNAL_EDGE #ifdef DEBUG_INTERNAL_EDGE #include <stdio.h> #endif //DEBUG_INTERNAL_EDGE #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW static btIDebugDraw* gDebugDrawer = 0; void btSetDebugDrawer(btIDebugDraw* debugDrawer) { gDebugDrawer = debugDrawer; } static void btDebugDrawLine(const btVector3& from, const btVector3& to, const btVector3& color) { if (gDebugDrawer) gDebugDrawer->drawLine(from, to, color); } #endif //BT_INTERNAL_EDGE_DEBUG_DRAW static int btGetHash(int partId, int 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) { const btVector3 refAxis0 = edgeA; const btVector3 refAxis1 = normalA; const btVector3 swingAxis = normalB; btScalar angle = btAtan2(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1)); return angle; } struct btConnectivityProcessor : public btTriangleCallback { int m_partIdA; int m_triangleIndexA; btVector3* m_triangleVerticesA; btTriangleInfoMap* m_triangleInfoMap; virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex) { //skip self-collisions if ((m_partIdA == partId) && (m_triangleIndexA == triangleIndex)) return; //skip duplicates (disabled for now) //if ((m_partIdA <= partId) && (m_triangleIndexA <= triangleIndex)) // return; //search for shared vertices and edges int numshared = 0; 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(); if (crossBSqr < m_triangleInfoMap->m_equalVertexThreshold) return; 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) return; #if 0 printf("triangle A[0] = (%f,%f,%f)\ntriangle A[1] = (%f,%f,%f)\ntriangle A[2] = (%f,%f,%f)\n", m_triangleVerticesA[0].getX(),m_triangleVerticesA[0].getY(),m_triangleVerticesA[0].getZ(), m_triangleVerticesA[1].getX(),m_triangleVerticesA[1].getY(),m_triangleVerticesA[1].getZ(), m_triangleVerticesA[2].getX(),m_triangleVerticesA[2].getY(),m_triangleVerticesA[2].getZ()); printf("partId=%d, triangleIndex=%d\n",partId,triangleIndex); printf("triangle B[0] = (%f,%f,%f)\ntriangle B[1] = (%f,%f,%f)\ntriangle B[2] = (%f,%f,%f)\n", triangle[0].getX(),triangle[0].getY(),triangle[0].getZ(), triangle[1].getX(),triangle[1].getY(),triangle[1].getZ(), triangle[2].getX(),triangle[2].getY(),triangle[2].getZ()); #endif for (int i = 0; i < 3; i++) { for (int j = 0; j < 3; j++) { if ((m_triangleVerticesA[i] - triangle[j]).length2() < m_triangleInfoMap->m_equalVertexThreshold) { sharedVertsA[numshared] = i; sharedVertsB[numshared] = j; numshared++; ///degenerate case if (numshared >= 3) return; } } ///degenerate case if (numshared >= 3) return; } switch (numshared) { case 0: { break; } case 1: { //shared vertex break; } 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 if (sharedVertsA[0] == 0 && sharedVertsA[1] == 2) { sharedVertsA[0] = 2; sharedVertsA[1] = 0; int tmp = sharedVertsB[1]; sharedVertsB[1] = sharedVertsB[0]; sharedVertsB[0] = tmp; } int hash = btGetHash(m_partIdA, m_triangleIndexA); btTriangleInfo* info = m_triangleInfoMap->find(hash); if (!info) { btTriangleInfo tmp; m_triangleInfoMap->insert(hash, tmp); info = m_triangleInfoMap->find(hash); } int sumvertsA = sharedVertsA[0] + sharedVertsA[1]; int otherIndexA = 3 - sumvertsA; 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 tB(triangle[sharedVertsB[1]], triangle[sharedVertsB[0]], triangle[otherIndexB]); //btTriangleShape tB(triangle[0],triangle[1],triangle[2]); btVector3 normalA; btVector3 normalB; tA.calcNormal(normalA); tB.calcNormal(normalB); edge.normalize(); btVector3 edgeCrossA = edge.cross(normalA).normalize(); { btVector3 tmp = m_triangleVerticesA[otherIndexA] - m_triangleVerticesA[sharedVertsA[0]]; if (edgeCrossA.dot(tmp) < 0) { edgeCrossA *= -1; } } btVector3 edgeCrossB = edge.cross(normalB).normalize(); { btVector3 tmp = triangle[otherIndexB] - triangle[sharedVertsB[0]]; if (edgeCrossB.dot(tmp) < 0) { edgeCrossB *= -1; } } btScalar angle2 = 0; btScalar ang4 = 0.f; btVector3 calculatedEdge = edgeCrossA.cross(edgeCrossB); btScalar len2 = calculatedEdge.length2(); btScalar correctedAngle(0); //btVector3 calculatedNormalB = normalA; bool isConvex = false; if (len2 < m_triangleInfoMap->m_planarEpsilon) { angle2 = 0.f; ang4 = 0.f; } else { calculatedEdge.normalize(); btVector3 calculatedNormalA = calculatedEdge.cross(edgeCrossA); calculatedNormalA.normalize(); 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.); correctedAngle = isConvex ? ang4 : -ang4; } //alternatively use //btVector3 calculatedNormalB2 = quatRotate(orn,normalA); switch (sumvertsA) { case 1: { 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) { computedNormalB *= -1; info->m_flags |= TRI_INFO_V0V1_SWAP_NORMALB; } #ifdef DEBUG_INTERNAL_EDGE if ((computedNormalB - normalB).length() > 0.0001) { printf("warning: normals not identical\n"); } #endif //DEBUG_INTERNAL_EDGE info->m_edgeV0V1Angle = -correctedAngle; if (isConvex) info->m_flags |= TRI_INFO_V0V1_CONVEX; break; } case 2: { btVector3 edge = m_triangleVerticesA[2] - m_triangleVerticesA[0]; btQuaternion orn(edge, -correctedAngle); btVector3 computedNormalB = quatRotate(orn, normalA); if (computedNormalB.dot(normalB) < 0) { computedNormalB *= -1; info->m_flags |= TRI_INFO_V2V0_SWAP_NORMALB; } #ifdef DEBUG_INTERNAL_EDGE if ((computedNormalB - normalB).length() > 0.0001) { printf("warning: normals not identical\n"); } #endif //DEBUG_INTERNAL_EDGE info->m_edgeV2V0Angle = -correctedAngle; if (isConvex) info->m_flags |= TRI_INFO_V2V0_CONVEX; break; } case 3: { 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; } #ifdef DEBUG_INTERNAL_EDGE if ((computedNormalB - normalB).length() > 0.0001) { printf("warning: normals not identical\n"); } #endif //DEBUG_INTERNAL_EDGE info->m_edgeV1V2Angle = -correctedAngle; if (isConvex) info->m_flags |= TRI_INFO_V1V2_CONVEX; break; } } break; } 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) { //the user pointer shouldn't already be used for other purposes, we intend to store connectivity info there! if (trimeshShape->getTriangleInfoMap()) return; trimeshShape->setTriangleInfoMap(triangleInfoMap); btStridingMeshInterface* meshInterface = trimeshShape->getMeshInterface(); const btVector3& meshScaling = meshInterface->getScaling(); for (int partId = 0; partId < meshInterface->getNumSubParts(); partId++) { const unsigned char* vertexbase = 0; int numverts = 0; PHY_ScalarType type = PHY_INTEGER; int stride = 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; for (int triangleIndex = 0; triangleIndex < numfaces; triangleIndex++) { unsigned int* gfxbase = (unsigned int*)(indexbase + triangleIndex * indexstride); for (int j = 2; j >= 0; 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); triangleVerts[j] = btVector3( 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())); } } 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]); aabbMax.setMax(triangleVerts[1]); aabbMin.setMin(triangleVerts[2]); aabbMax.setMax(triangleVerts[2]); btConnectivityProcessor connectivityProcessor; connectivityProcessor.m_partIdA = partId; connectivityProcessor.m_triangleIndexA = triangleIndex; connectivityProcessor.m_triangleVerticesA = &triangleVerts[0]; connectivityProcessor.m_triangleInfoMap = triangleInfoMap; 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) { btVector3 lineDelta = line1 - line0; // Handle degenerate lines if (lineDelta.fuzzyZero()) { nearestPoint = line0; } else { btScalar delta = (point - line0).dot(lineDelta) / (lineDelta).dot(lineDelta); // Clamp the point to conform to the segment's endpoints if (delta < 0) delta = 0; else if (delta > 1) delta = 1; nearestPoint = line0 + lineDelta * delta; } } 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); if (correctedEdgeAngle < 0) { if (curAngle < correctedEdgeAngle) { btScalar diffAngle = correctedEdgeAngle - curAngle; btQuaternion rotation(edge, diffAngle); clampedLocalNormal = btMatrix3x3(rotation) * localContactNormalOnB; return true; } } if (correctedEdgeAngle >= 0) { if (curAngle > correctedEdgeAngle) { 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) { //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 (!triangleInfoMapPtr) return; 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; 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 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 tri_normal; tri_shape->calcNormal(tri_normal); //btScalar dot = tri_normal.dot(cp.m_normalWorldOnB); btVector3 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 bool isNearEdge = false; int numConcaveEdgeHits = 0; int numConvexEdgeHits = 0; btVector3 localContactNormalOnB = colObj0Wrap->getWorldTransform().getBasis().transpose() * cp.m_normalWorldOnB; localContactNormalOnB.normalize(); //is this necessary? // Get closest edge 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; } } // 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; } } // 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; } } #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) { #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW 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 { 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; 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); #ifdef 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 (((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); #ifdef 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 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; if (info->m_edgeV1V2Angle == btScalar(0)) { 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 btScalar NdotA = localContactNormalOnB.dot(nA); btScalar NdotB = localContactNormalOnB.dot(nB); bool backFacingNormal = (NdotA < triangleInfoMapPtr->m_convexEpsilon) && (NdotB < triangleInfoMapPtr->m_convexEpsilon); if (backFacingNormal) { 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) { 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); #ifdef 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 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); if (info->m_edgeV2V0Angle == btScalar(0)) { numConcaveEdgeHits++; } else { 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); if (backFacingNormal) { numConcaveEdgeHits++; } else { 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) { 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); } #endif //DEBUG_INTERNAL_EDGE if (isNearEdge) { if (numConcaveEdgeHits > 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 { 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) { return; } //modify the normal to be the triangle normal (or backfacing normal) 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); } } }