First version of btInternalEdgeUtility to filter internal edge collisions.
See also http://code.google.com/p/bullet/issues/detail?id=27 for issue description and an example how to use this (a modified version of Bullet/Demos/ConcaveDemo) This demo will be committed to Bullet/Demos/ConcaveDemo later.
This commit is contained in:
erwin.coumans
678
src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp
Normal file
678
src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp
Normal file
@@ -0,0 +1,678 @@
|
||||
#include "btInternalEdgeUtility.h"
|
||||
|
||||
#include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h"
|
||||
#include "BulletCollision/CollisionShapes/btTriangleShape.h"
|
||||
#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
|
||||
#include "BulletCollision/NarrowPhaseCollision/btManifoldPoint.h"
|
||||
#include "LinearMath/btIDebugDraw.h"
|
||||
|
||||
|
||||
|
||||
#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
|
||||
//quick hack for debug drawing
|
||||
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};
|
||||
|
||||
#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 j=0;j<3;j++)
|
||||
{
|
||||
for (int i=0;i<3;i++)
|
||||
{
|
||||
if ( (m_triangleVerticesA[i]-triangle[j]).length2() < m_triangleInfoMap->m_equalVertexThreshold)
|
||||
{
|
||||
sharedVertsA[numshared] = i;
|
||||
sharedVertsB[numshared] = j;
|
||||
numshared++;
|
||||
}
|
||||
}
|
||||
}
|
||||
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;
|
||||
btQuaternion orn2(calculatedEdge,-correctedAngle);
|
||||
calculatedNormalB = btMatrix3x3(orn2)*normalA;
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
//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);
|
||||
if (computedNormalB.dot(calculatedNormalB)<0)
|
||||
info->m_flags |= TRI_INFO_V0V1_SWAP_NORMALB;
|
||||
|
||||
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(calculatedNormalB)<0)
|
||||
info->m_flags |= TRI_INFO_V2V0_SWAP_NORMALB;
|
||||
|
||||
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(calculatedNormalB)<0)
|
||||
info->m_flags |= TRI_INFO_V1V2_SWAP_NORMALB;
|
||||
|
||||
info->m_edgeV1V2Angle = correctedAngle;
|
||||
|
||||
if (isConvex)
|
||||
info->m_flags |= TRI_INFO_V1V2_CONVEX;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
default:
|
||||
{
|
||||
// printf("warning: duplicate triangle\n");
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
};
|
||||
/////////////////////////////////////////////////////////
|
||||
/////////////////////////////////////////////////////////
|
||||
|
||||
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->getUserPointer())
|
||||
return;
|
||||
|
||||
trimeshShape->setUserPointer(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 = indicestype==PHY_SHORT?((unsigned short*)gfxbase)[j]:gfxbase[j];
|
||||
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);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
// 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 (curAngle < -correctedEdgeAngle)
|
||||
{
|
||||
btScalar diffAngle = correctedEdgeAngle-curAngle;
|
||||
btQuaternion rotation(edge,diffAngle );
|
||||
clampedLocalNormal = btMatrix3x3(rotation)*localContactNormalOnB;
|
||||
return true;
|
||||
|
||||
} else
|
||||
{
|
||||
if (curAngle < correctedEdgeAngle)
|
||||
{
|
||||
//printf("angle within valid convex range, don't clamp it!\n");
|
||||
return false;
|
||||
} else
|
||||
{
|
||||
// printf("too large angle\n");
|
||||
btScalar diffAngle = correctedEdgeAngle-curAngle;
|
||||
|
||||
btQuaternion rotation(edge,diffAngle );
|
||||
clampedLocalNormal = btMatrix3x3(rotation)*localContactNormalOnB;
|
||||
//clamp it?
|
||||
return true;
|
||||
}
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
|
||||
|
||||
/// Changes a btManifoldPoint collision normal to the normal from the mesh.
|
||||
void btAdjustInternalEdgeContacts(btManifoldPoint& cp, const btCollisionObject* colObj0,const btCollisionObject* colObj1, int partId0, int index0)
|
||||
{
|
||||
|
||||
btAssert(colObj0->getCollisionShape()->getShapeType() == TRIANGLE_SHAPE_PROXYTYPE);
|
||||
if (colObj0->getCollisionShape()->getShapeType() != TRIANGLE_SHAPE_PROXYTYPE)
|
||||
return;
|
||||
|
||||
btTriangleInfoMap* triangleInfoMapPtr = (btTriangleInfoMap*) colObj0->getRootCollisionShape()->getUserPointer();
|
||||
if (!triangleInfoMapPtr)
|
||||
return;
|
||||
|
||||
int hash = btGetHash(partId0,index0);
|
||||
|
||||
|
||||
btTriangleInfo* info = triangleInfoMapPtr->find(hash);
|
||||
if (!info)
|
||||
return;
|
||||
|
||||
const btTriangleShape* tri_shape = static_cast<const btTriangleShape*>(colObj0->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);
|
||||
const btTransform& tr = colObj0->getWorldTransform();
|
||||
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
|
||||
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 = colObj0->getWorldTransform().getBasis().transpose() * cp.m_normalWorldOnB;
|
||||
localContactNormalOnB.normalize();//is this necessary?
|
||||
|
||||
if ((info->m_edgeV0V1Angle)< SIMD_2_PI)
|
||||
{
|
||||
#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)
|
||||
{
|
||||
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 = computedNormalB;
|
||||
|
||||
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 clampedLocalNormal;
|
||||
bool isClamped = btClampNormal(edge,swapFactor*tri_normal,localContactNormalOnB, swapFactor*info->m_edgeV0V1Angle,clampedLocalNormal);
|
||||
if (isClamped)
|
||||
{
|
||||
btVector3 newNormal = colObj0->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 = colObj0->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
|
||||
|
||||
if ((info->m_edgeV1V2Angle)< SIMD_2_PI)
|
||||
{
|
||||
#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)
|
||||
{
|
||||
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 = computedNormalB;
|
||||
|
||||
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 = colObj0->getWorldTransform().getBasis().transpose() * cp.m_normalWorldOnB;
|
||||
btVector3 clampedLocalNormal;
|
||||
bool isClamped = btClampNormal(edge,swapFactor*tri_normal,localContactNormalOnB, swapFactor*info->m_edgeV1V2Angle,clampedLocalNormal);
|
||||
if (isClamped)
|
||||
{
|
||||
btVector3 newNormal = colObj0->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 = colObj0->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
|
||||
|
||||
if ((info->m_edgeV2V0Angle)< SIMD_2_PI)
|
||||
{
|
||||
|
||||
#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)
|
||||
{
|
||||
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 = computedNormalB;
|
||||
|
||||
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 = colObj0->getWorldTransform().getBasis().transpose() * cp.m_normalWorldOnB;
|
||||
btVector3 clampedLocalNormal;
|
||||
bool isClamped = btClampNormal(edge,swapFactor*tri_normal,localContactNormalOnB,swapFactor* info->m_edgeV2V0Angle,clampedLocalNormal);
|
||||
if (isClamped)
|
||||
{
|
||||
btVector3 newNormal = colObj0->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 = colObj0->getWorldTransform().invXform(cp.m_positionWorldOnB);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
if (isNearEdge)
|
||||
{
|
||||
|
||||
if (numConcaveEdgeHits>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;
|
||||
}
|
||||
//for concave edge hits, just modify the normal to be the triangle normal
|
||||
cp.m_normalWorldOnB = colObj0->getWorldTransform().getBasis() * tri_normal;
|
||||
// Reproject collision point along normal.
|
||||
cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1;
|
||||
cp.m_localPointB = colObj0->getWorldTransform().invXform(cp.m_positionWorldOnB);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,85 @@
|
||||
|
||||
#ifndef BT_INTERNAL_EDGE_UTILITY_H
|
||||
#define BT_INTERNAL_EDGE_UTILITY_H
|
||||
|
||||
#include <LinearMath/btHashMap.h>
|
||||
#include "LinearMath/btVector3.h"
|
||||
|
||||
///The btInternalEdgeUtility helps to avoid or reduce artifacts due to wrong collision normals caused by internal edges.
|
||||
///See also http://code.google.com/p/bullet/issues/detail?id=27
|
||||
|
||||
class btBvhTriangleMeshShape;
|
||||
class btCollisionObject;
|
||||
class btManifoldPoint;
|
||||
class btIDebugDraw;
|
||||
|
||||
///for btTriangleInfo m_flags
|
||||
#define TRI_INFO_V0V1_CONVEX 1
|
||||
#define TRI_INFO_V1V2_CONVEX 2
|
||||
#define TRI_INFO_V2V0_CONVEX 4
|
||||
|
||||
#define TRI_INFO_V0V1_SWAP_NORMALB 8
|
||||
#define TRI_INFO_V1V2_SWAP_NORMALB 16
|
||||
#define TRI_INFO_V2V0_SWAP_NORMALB 32
|
||||
|
||||
|
||||
///The btTriangleInfo structure stores information to adjust collision normals to avoid collisions against internal edges
|
||||
///it can be generated using
|
||||
struct btTriangleInfo
|
||||
{
|
||||
btTriangleInfo()
|
||||
{
|
||||
m_edgeV0V1Angle = SIMD_2_PI;
|
||||
m_edgeV1V2Angle = SIMD_2_PI;
|
||||
m_edgeV2V0Angle = SIMD_2_PI;
|
||||
m_flags=0;
|
||||
}
|
||||
|
||||
int m_flags;
|
||||
|
||||
btScalar m_edgeV0V1Angle;
|
||||
btScalar m_edgeV1V2Angle;
|
||||
btScalar m_edgeV2V0Angle;
|
||||
|
||||
};
|
||||
|
||||
typedef btHashMap<btHashInt,btTriangleInfo> btInternalTriangleInfoMap;
|
||||
|
||||
|
||||
///The btTriangleInfoMap stores edge angle information for some triangles. You can compute this information yourself or using btGenerateInternalEdgeInfo.
|
||||
struct btTriangleInfoMap : public btInternalTriangleInfoMap
|
||||
{
|
||||
btScalar m_convexEpsilon;///used to determine if an edge or contact normal is convex, using the dot product
|
||||
btScalar m_planarEpsilon; ///used to determine if a triangle edge is planar with zero angle
|
||||
btScalar m_equalVertexThreshold; ///used to compute connectivity: if the distance between two vertices is smaller than m_equalVertexThreshold, they are considered to be 'shared'
|
||||
btScalar m_edgeDistanceThreshold; ///used to determine edge contacts: if the closest distance between a contact point and an edge is smaller than this distance threshold it is considered to "hit the edge"
|
||||
|
||||
btTriangleInfoMap()
|
||||
{
|
||||
m_convexEpsilon = 0.00f;
|
||||
m_planarEpsilon = 0.0001f;
|
||||
m_equalVertexThreshold = btScalar(0.0001)*btScalar(0.0001);
|
||||
m_edgeDistanceThreshold = btScalar(0.1);
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
|
||||
///Call btGenerateInternalEdgeInfo to create triangle info, store in the shape 'userInfo'
|
||||
void btGenerateInternalEdgeInfo (btBvhTriangleMeshShape*trimeshShape, btTriangleInfoMap* triangleInfoMap);
|
||||
|
||||
|
||||
///Call the btFixMeshNormal to adjust the collision normal, using the triangle info map (generated using btGenerateInternalEdgeInfo)
|
||||
///If this info map is missing, or the triangle is not store in this map, nothing will be done
|
||||
void btAdjustInternalEdgeContacts(btManifoldPoint& cp, const btCollisionObject* trimeshColObj0,const btCollisionObject* otherColObj1, int partId0, int index0);
|
||||
|
||||
///Enable the BT_INTERNAL_EDGE_DEBUG_DRAW define and call btSetDebugDrawer, to get visual info to see if the internal edge utility works properly.
|
||||
///If the utility doesn't work properly, you might have to adjust the threshold values in btTriangleInfoMap
|
||||
#define BT_INTERNAL_EDGE_DEBUG_DRAW
|
||||
|
||||
#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
|
||||
void btSetDebugDrawer(btIDebugDraw* debugDrawer);
|
||||
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
|
||||
|
||||
|
||||
#endif //BT_INTERNAL_EDGE_UTILITY_H
|
||||
Reference in New Issue
Block a user