Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

perform GrahamScanConvexHull2D around an arbitrary oriented 2D plane in 3D, to fix some convex hull face merging problems

Browse Source 
add compound shape support to BulletXmlWorldImporter and fix some compile issue under Debian (hopefully)
object picking change in the demos: create a ball-socket picking constraint when holding shift while mouse dragging, otherwise a fixed (6dof) constraint
add assert in constraint solver, when both objects have their inertia tensor rows set to zero
btPolyhedralContactClipping: add edge-edge contact point in  findSeparatingAxis (similar to the default GJK case)
This commit is contained in:
erwin.coumans
2012-09-28 07:14:48 +00:00
parent 837e5cb5e0
commit 60bf599246
24 changed files with 385 additions and 153 deletions
Download Patch File Download Diff File Expand all files Collapse all files

165
src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.cpp
View File

@@ -77,12 +77,15 @@ void btPolyhedralContactClipping::clipFace(const btVertexArray& pVtxIn, btVertex
}
static bool TestSepAxis(const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, const btVector3& sep_axis, btScalar& depth)
static bool TestSepAxis(const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, const btVector3& sep_axis, btScalar& depth, btVector3& witnessPointA, btVector3& witnessPointB)
{
btScalar Min0,Max0;
btScalar Min1,Max1;
hullA.project(transA,sep_axis, Min0, Max0);
hullB.project(transB, sep_axis, Min1, Max1);
btVector3 witnesPtMinA,witnesPtMaxA;
btVector3 witnesPtMinB,witnesPtMaxB;
hullA.project(transA,sep_axis, Min0, Max0,witnesPtMinA,witnesPtMaxA);
hullB.project(transB, sep_axis, Min1, Max1,witnesPtMinB,witnesPtMaxB);
if(Max0<Min1 || Max1<Min0)
return false;
@@ -91,7 +94,19 @@ static bool TestSepAxis(const btConvexPolyhedron& hullA, const btConvexPolyhedro
btAssert(d0>=0.0f);
btScalar d1 = Max1 - Min0;
btAssert(d1>=0.0f);
depth = d0<d1 ? d0:d1;
if (d0<d1)
{
depth = d0;
witnessPointA = witnesPtMaxA;
witnessPointB = witnesPtMinB;
} else
{
depth = d1;
witnessPointA = witnesPtMinA;
witnessPointB = witnesPtMaxB;
}
return true;
}
@@ -163,8 +178,66 @@ void InverseTransformPoint3x3(btVector3& out, const btVector3& in, const btTrans
}
#endif //TEST_INTERNAL_OBJECTS
SIMD_FORCE_INLINE void btSegmentsClosestPoints(
btVector3& ptsVector,
btVector3& offsetA,
btVector3& offsetB,
btScalar& tA, btScalar& tB,
const btVector3& translation,
const btVector3& dirA, btScalar hlenA,
const btVector3& dirB, btScalar hlenB )
{
// compute the parameters of the closest points on each line segment
btScalar dirA_dot_dirB = btDot(dirA,dirB);
btScalar dirA_dot_trans = btDot(dirA,translation);
btScalar dirB_dot_trans = btDot(dirB,translation);
btScalar denom = 1.0f - dirA_dot_dirB * dirA_dot_dirB;
if ( denom == 0.0f ) {
tA = 0.0f;
} else {
tA = ( dirA_dot_trans - dirB_dot_trans * dirA_dot_dirB ) / denom;
if ( tA < -hlenA )
tA = -hlenA;
else if ( tA > hlenA )
tA = hlenA;
}
tB = tA * dirA_dot_dirB - dirB_dot_trans;
if ( tB < -hlenB ) {
tB = -hlenB;
tA = tB * dirA_dot_dirB + dirA_dot_trans;
if ( tA < -hlenA )
tA = -hlenA;
else if ( tA > hlenA )
tA = hlenA;
} else if ( tB > hlenB ) {
tB = hlenB;
tA = tB * dirA_dot_dirB + dirA_dot_trans;
if ( tA < -hlenA )
tA = -hlenA;
else if ( tA > hlenA )
tA = hlenA;
}
// compute the closest points relative to segment centers.
offsetA = dirA * tA;
offsetB = dirB * tB;
ptsVector = translation - offsetA + offsetB;
}
bool btPolyhedralContactClipping::findSeparatingAxis( const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, btVector3& sep)
bool btPolyhedralContactClipping::findSeparatingAxis( const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, btVector3& sep, btDiscreteCollisionDetectorInterface::Result& resultOut)
{
gActualSATPairTests++;
@@ -182,9 +255,9 @@ bool btPolyhedralContactClipping::findSeparatingAxis( const btConvexPolyhedron&
for(int i=0;i<numFacesA;i++)
{
const btVector3 Normal(hullA.m_faces[i].m_plane[0], hullA.m_faces[i].m_plane[1], hullA.m_faces[i].m_plane[2]);
const btVector3 faceANormalWS = transA.getBasis() * Normal;
btVector3 faceANormalWS = transA.getBasis() * Normal;
if (DeltaC2.dot(faceANormalWS)<0)
continue;
faceANormalWS*=-1.f;
curPlaneTests++;
#ifdef TEST_INTERNAL_OBJECTS
@@ -195,7 +268,8 @@ bool btPolyhedralContactClipping::findSeparatingAxis( const btConvexPolyhedron&
#endif
btScalar d;
if(!TestSepAxis( hullA, hullB, transA,transB, faceANormalWS, d))
btVector3 wA,wB;
if(!TestSepAxis( hullA, hullB, transA,transB, faceANormalWS, d,wA,wB))
return false;
if(d<dmin)
@@ -210,9 +284,9 @@ bool btPolyhedralContactClipping::findSeparatingAxis( const btConvexPolyhedron&
for(int i=0;i<numFacesB;i++)
{
const btVector3 Normal(hullB.m_faces[i].m_plane[0], hullB.m_faces[i].m_plane[1], hullB.m_faces[i].m_plane[2]);
const btVector3 WorldNormal = transB.getBasis() * Normal;
btVector3 WorldNormal = transB.getBasis() * Normal;
if (DeltaC2.dot(WorldNormal)<0)
continue;
WorldNormal *=-1.f;
curPlaneTests++;
#ifdef TEST_INTERNAL_OBJECTS
@@ -223,7 +297,8 @@ bool btPolyhedralContactClipping::findSeparatingAxis( const btConvexPolyhedron&
#endif
btScalar d;
if(!TestSepAxis(hullA, hullB,transA,transB, WorldNormal,d))
btVector3 wA,wB;
if(!TestSepAxis(hullA, hullB,transA,transB, WorldNormal,d,wA,wB))
return false;
if(d<dmin)
@@ -234,6 +309,12 @@ bool btPolyhedralContactClipping::findSeparatingAxis( const btConvexPolyhedron&
}
btVector3 edgeAstart,edgeAend,edgeBstart,edgeBend;
int edgeA=-1;
int edgeB=-1;
btVector3 worldEdgeA;
btVector3 worldEdgeB;
btVector3 witnessPointA,witnessPointB;
int curEdgeEdge = 0;
// Test edges
@@ -252,7 +333,7 @@ bool btPolyhedralContactClipping::findSeparatingAxis( const btConvexPolyhedron&
{
Cross = Cross.normalize();
if (DeltaC2.dot(Cross)<0)
continue;
Cross *= -1.f;
#ifdef TEST_INTERNAL_OBJECTS
@@ -263,20 +344,68 @@ bool btPolyhedralContactClipping::findSeparatingAxis( const btConvexPolyhedron&
#endif
btScalar dist;
if(!TestSepAxis( hullA, hullB, transA,transB, Cross, dist))
btVector3 wA,wB;
if(!TestSepAxis( hullA, hullB, transA,transB, Cross, dist,wA,wB))
return false;
if(dist<dmin)
{
dmin = dist;
sep = Cross;
edgeA=e0;
edgeB=e1;
worldEdgeA = WorldEdge0;
worldEdgeB = WorldEdge1;
witnessPointA=wA;
witnessPointB=wB;
}
}
}
}
if((DeltaC2.dot(sep))>0.0f)
if (edgeA>=0&&edgeB>=0)
{
// printf("edge-edge\n");
//add an edge-edge contact
btVector3 ptsVector;
btVector3 offsetA;
btVector3 offsetB;
btScalar tA;
btScalar tB;
btVector3 translation = witnessPointB-witnessPointA;
btVector3 dirA = worldEdgeA;
btVector3 dirB = worldEdgeB;
btScalar hlenB = 1e30f;
btScalar hlenA = 1e30f;
btSegmentsClosestPoints(ptsVector,offsetA,offsetB,tA,tB,
translation,
dirA, hlenA,
dirB,hlenB);
btScalar nlSqrt = ptsVector.length2();
if (nlSqrt>SIMD_EPSILON)
{
btScalar nl = btSqrt(nlSqrt);
ptsVector *= 1.f/nl;
if (ptsVector.dot(DeltaC2)<0.f)
{
ptsVector*=-1.f;
}
btVector3 ptOnB = witnessPointB + offsetB;
btScalar distance = nl;
resultOut.addContactPoint(ptsVector, ptOnB,-distance);
}
}
if((DeltaC2.dot(sep))<0.0f)
sep = -sep;
return true;
@@ -359,8 +488,8 @@ void btPolyhedralContactClipping::clipFaceAgainstHull(const btVector3& separatin
btScalar planeEqWS=localPlaneEq-planeNormalWS.dot(transA.getOrigin());
for (int i=0;i<pVtxIn->size();i++)
{
btScalar depth = planeNormalWS.dot(pVtxIn->at(i))+planeEqWS;
btVector3 vtx = pVtxIn->at(i);
btScalar depth = planeNormalWS.dot(vtx)+planeEqWS;
if (depth <=minDist)
{
// printf("clamped: depth=%f to minDist=%f\n",depth,minDist);
@@ -395,6 +524,9 @@ void btPolyhedralContactClipping::clipFaceAgainstHull(const btVector3& separatin
}
void btPolyhedralContactClipping::clipHullAgainstHull(const btVector3& separatingNormal1, const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, const btScalar minDist, btScalar maxDist,btDiscreteCollisionDetectorInterface::Result& resultOut)
{
@@ -404,6 +536,7 @@ void btPolyhedralContactClipping::clipHullAgainstHull(const btVector3& separatin
//const btVector3 DeltaC2 = c0 - c1;
int closestFaceB=-1;
btScalar dmax = -FLT_MAX;
{

2
src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.h
View File

@@ -35,7 +35,7 @@ struct btPolyhedralContactClipping
static void clipHullAgainstHull(const btVector3& separatingNormal, const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, const btScalar minDist, btScalar maxDist, btDiscreteCollisionDetectorInterface::Result& resultOut);
static void clipFaceAgainstHull(const btVector3& separatingNormal, const btConvexPolyhedron& hullA, const btTransform& transA, btVertexArray& worldVertsB1, const btScalar minDist, btScalar maxDist,btDiscreteCollisionDetectorInterface::Result& resultOut);
static bool findSeparatingAxis( const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, btVector3& sep);
static bool findSeparatingAxis( const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, btVector3& sep, btDiscreteCollisionDetectorInterface::Result& resultOut);
///the clipFace method is used internally
static void clipFace(const btVertexArray& pVtxIn, btVertexArray& ppVtxOut, const btVector3& planeNormalWS,btScalar planeEqWS);