static bool ReferenceCode(const MyConvex& hull0, const MyConvex& hull1, float& dmin, btVector3& sep)
{
dmin = FLT_MAX;
int i;
// Test normals from hull0
for( i=0;i<hull0.mNbPolys;i++)
{
btVector3 Normal(hull0.mPolys[i].mPlane[0], hull0.mPolys[i].mPlane[1], hull0.mPolys[i].mPlane[2]);
// btVector3 WorldNormal = hull0.mTransform * Normal;
btVector3 WorldNormal = hull0.mTransform.getBasis() * Normal;
float d;
if(!TestSepAxis(WorldNormal, hull0, hull1, d))
return false;
if(d<dmin)
{
dmin = d;
sep = WorldNormal;
}
}
// Test normals from hull1
for( i=0;i<hull1.mNbPolys;i++)
{
btVector3 Normal(hull1.mPolys[i].mPlane[0], hull1.mPolys[i].mPlane[1], hull1.mPolys[i].mPlane[2]);
// btVector3 WorldNormal = hull1.mTransform * Normal;
btVector3 WorldNormal = hull1.mTransform.getBasis() * Normal;
float d;
if(!TestSepAxis(WorldNormal, hull0, hull1, d))
return false;
if(d<dmin)
{
dmin = d;
sep = WorldNormal;
}
}
// Test edges
for(int j=0;j<hull0.mNbPolys;j++)
{
const MyPoly& poly0 = hull0.mPolys[j];
for(int i=0;i<hull1.mNbPolys;i++)
{
const MyPoly& poly1 = hull1.mPolys[i];
for(int e0=0;e0<poly0.mNbVerts;e0++)
{
const btVector3& a = hull0.mVerts[poly0.mIndices[e0]];
const btVector3& b = hull0.mVerts[poly0.mIndices[(e0+1)%poly0.mNbVerts]];
btVector3 edge0 = a - b;
btVector3 WorldEdge0 = hull0.mTransform.getBasis() * edge0;
for(int e1=0;e1<poly1.mNbVerts;e1++)
{
const btVector3& c = hull1.mVerts[poly1.mIndices[e1]];
const btVector3& d = hull1.mVerts[poly1.mIndices[(e1+1)%poly1.mNbVerts]];
btVector3 edge1 = c - d;
btVector3 WorldEdge1 = hull1.mTransform.getBasis() * edge1;
btVector3 Cross = WorldEdge0.cross(WorldEdge1);
if(!IsAlmostZero(Cross))
{
Cross = Cross.normalize();
float d;
if(!TestSepAxis(Cross, hull0, hull1, d))
return false;
if(d<dmin)
{
dmin = d;
sep = Cross;
}
}
}
}
}
}
btVector3 DeltaC = (hull1.mTransform * hull1.mTransform.getOrigin()) - (hull0.mTransform * hull0.mTransform.getOrigin());
if((DeltaC.dot(sep))>0.0f) sep = -sep;
return true;
}
void btConvexPolyhedron::initialize()
{
btHashMap<btInternalVertexPair,btInternalEdge> edges;
btScalar TotalArea = 0.0f;
m_localCenter.setValue(0, 0, 0);
for(int i=0;i<m_faces.size();i++)
{
int numVertices = m_faces[i].m_indices.size();
int NbTris = numVertices;
for(int j=0;j<NbTris;j++)
{
int k = (j+1)%numVertices;
btInternalVertexPair vp(m_faces[i].m_indices[j],m_faces[i].m_indices[k]);
btInternalEdge* edptr = edges.find(vp);
btVector3 edge = m_vertices[vp.m_v1]-m_vertices[vp.m_v0];
edge.normalize();
bool found = false;
for (int p=0;p<m_uniqueEdges.size();p++)
{
if (IsAlmostZero(m_uniqueEdges[p]-edge) ||
IsAlmostZero(m_uniqueEdges[p]+edge))
{
found = true;
break;
}
}
if (!found)
{
m_uniqueEdges.push_back(edge);
}
if (edptr)
{
#if 0
btAssert(edptr->m_face0>=0);
btAssert(edptr->m_face1<0);
#else
// why I am getting this asserts on some maps?
#endif
edptr->m_face1 = i;
} else
{
btInternalEdge ed;
ed.m_face0 = i;
edges.insert(vp,ed);
}
}
}
#ifdef USE_CONNECTED_FACES
for(int i=0;i<m_faces.size();i++)
{
int numVertices = m_faces[i].m_indices.size();
m_faces[i].m_connectedFaces.resize(numVertices);
for(int j=0;j<numVertices;j++)
{
int k = (j+1)%numVertices;
btInternalVertexPair vp(m_faces[i].m_indices[j],m_faces[i].m_indices[k]);
btInternalEdge* edptr = edges.find(vp);
btAssert(edptr);
btAssert(edptr->m_face0>=0);
btAssert(edptr->m_face1>=0);
int connectedFace = (edptr->m_face0==i)?edptr->m_face1:edptr->m_face0;
m_faces[i].m_connectedFaces[j] = connectedFace;
}
}
#endif//USE_CONNECTED_FACES
for(int i=0;i<m_faces.size();i++)
{
int numVertices = m_faces[i].m_indices.size();
int NbTris = numVertices-2;
const btVector3& p0 = m_vertices[m_faces[i].m_indices[0]];
for(int j=1;j<=NbTris;j++)
{
int k = (j+1)%numVertices;
const btVector3& p1 = m_vertices[m_faces[i].m_indices[j]];
const btVector3& p2 = m_vertices[m_faces[i].m_indices[k]];
btScalar Area = ((p0 - p1).cross(p0 - p2)).length() * 0.5f;
btVector3 Center = (p0+p1+p2)/3.0f;
m_localCenter += Area * Center;
TotalArea += Area;
}
}
m_localCenter /= TotalArea;
#ifdef TEST_INTERNAL_OBJECTS
if(1)
{
m_radius = FLT_MAX;
for(int i=0;i<m_faces.size();i++)
{
const btVector3 Normal(m_faces[i].m_plane[0], m_faces[i].m_plane[1], m_faces[i].m_plane[2]);
const btScalar dist = btFabs(m_localCenter.dot(Normal) + m_faces[i].m_plane[3]);
if(dist<m_radius)
m_radius = dist;
}
btScalar MinX = FLT_MAX;
btScalar MinY = FLT_MAX;
btScalar MinZ = FLT_MAX;
btScalar MaxX = -FLT_MAX;
btScalar MaxY = -FLT_MAX;
btScalar MaxZ = -FLT_MAX;
for(int i=0; i<m_vertices.size(); i++)
{
const btVector3& pt = m_vertices[i];
if(pt.x()<MinX) MinX = pt.x();
if(pt.x()>MaxX) MaxX = pt.x();
if(pt.y()<MinY) MinY = pt.y();
if(pt.y()>MaxY) MaxY = pt.y();
if(pt.z()<MinZ) MinZ = pt.z();
if(pt.z()>MaxZ) MaxZ = pt.z();
}
mC.setValue(MaxX+MinX, MaxY+MinY, MaxZ+MinZ);
mE.setValue(MaxX-MinX, MaxY-MinY, MaxZ-MinZ);
// const btScalar r = m_radius / sqrtf(2.0f);
const btScalar r = m_radius / sqrtf(3.0f);
const int LargestExtent = mE.maxAxis();
const btScalar Step = (mE[LargestExtent]*0.5f - r)/1024.0f;
m_extents[0] = m_extents[1] = m_extents[2] = r;
m_extents[LargestExtent] = mE[LargestExtent]*0.5f;
bool FoundBox = false;
for(int j=0;j<1024;j++)
{
if(testContainment())
{
FoundBox = true;
break;
}
m_extents[LargestExtent] -= Step;
}
if(!FoundBox)
{
m_extents[0] = m_extents[1] = m_extents[2] = r;
}
else
{
// Refine the box
const btScalar Step = (m_radius - r)/1024.0f;
const int e0 = (1<<LargestExtent) & 3;
const int e1 = (1<<e0) & 3;
for(int j=0;j<1024;j++)
{
const btScalar Saved0 = m_extents[e0];
const btScalar Saved1 = m_extents[e1];
m_extents[e0] += Step;
m_extents[e1] += Step;
if(!testContainment())
{
m_extents[e0] = Saved0;
m_extents[e1] = Saved1;
break;
}
}
}
}
#endif
}
bool btPolyhedralContactClipping::findSeparatingAxis( const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, btVector3& sep)
{
gActualSATPairTests++;
//#ifdef TEST_INTERNAL_OBJECTS
const btVector3 c0 = transA * hullA.m_localCenter;
const btVector3 c1 = transB * hullB.m_localCenter;
const btVector3 DeltaC2 = c0 - c1;
//#endif
btScalar dmin = FLT_MAX;
int curPlaneTests=0;
int numFacesA = hullA.m_faces.size();
// Test normals from hullA
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;
if (DeltaC2.dot(faceANormalWS)<0)
continue;
curPlaneTests++;
#ifdef TEST_INTERNAL_OBJECTS
gExpectedNbTests++;
if(gUseInternalObject && !TestInternalObjects(transA,transB, DeltaC2, faceANormalWS, hullA, hullB, dmin))
continue;
gActualNbTests++;
#endif
btScalar d;
if(!TestSepAxis( hullA, hullB, transA,transB, faceANormalWS, d))
return false;
if(d<dmin)
{
dmin = d;
sep = faceANormalWS;
}
}
int numFacesB = hullB.m_faces.size();
// Test normals from hullB
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;
if (DeltaC2.dot(WorldNormal)<0)
continue;
curPlaneTests++;
#ifdef TEST_INTERNAL_OBJECTS
gExpectedNbTests++;
if(gUseInternalObject && !TestInternalObjects(transA,transB,DeltaC2, WorldNormal, hullA, hullB, dmin))
continue;
gActualNbTests++;
#endif
btScalar d;
if(!TestSepAxis(hullA, hullB,transA,transB, WorldNormal,d))
return false;
if(d<dmin)
{
dmin = d;
sep = WorldNormal;
}
}
btVector3 edgeAstart,edgeAend,edgeBstart,edgeBend;
int curEdgeEdge = 0;
// Test edges
for(int e0=0;e0<hullA.m_uniqueEdges.size();e0++)
{
const btVector3 edge0 = hullA.m_uniqueEdges[e0];
const btVector3 WorldEdge0 = transA.getBasis() * edge0;
for(int e1=0;e1<hullB.m_uniqueEdges.size();e1++)
{
const btVector3 edge1 = hullB.m_uniqueEdges[e1];
const btVector3 WorldEdge1 = transB.getBasis() * edge1;
btVector3 Cross = WorldEdge0.cross(WorldEdge1);
curEdgeEdge++;
if(!IsAlmostZero(Cross))
{
Cross = Cross.normalize();
if (DeltaC2.dot(Cross)<0)
continue;
#ifdef TEST_INTERNAL_OBJECTS
gExpectedNbTests++;
if(gUseInternalObject && !TestInternalObjects(transA,transB,DeltaC2, Cross, hullA, hullB, dmin))
continue;
gActualNbTests++;
#endif
btScalar dist;
if(!TestSepAxis( hullA, hullB, transA,transB, Cross, dist))
return false;
if(dist<dmin)
{
dmin = dist;
sep = Cross;
}
}
}
}
const btVector3 deltaC = transB.getOrigin() - transA.getOrigin();
if((deltaC.dot(sep))>0.0f)
sep = -sep;
return true;
}
void btConvexPolyhedron::initialize()
{
btHashMap<btInternalVertexPair, btInternalEdge> edges;
for (int i = 0; i < m_faces.size(); i++)
{
int numVertices = m_faces[i].m_indices.size();
int NbTris = numVertices;
for (int j = 0; j < NbTris; j++)
{
int k = (j + 1) % numVertices;
btInternalVertexPair vp(m_faces[i].m_indices[j], m_faces[i].m_indices[k]);
btInternalEdge* edptr = edges.find(vp);
btVector3 edge = m_vertices[vp.m_v1] - m_vertices[vp.m_v0];
edge.normalize();
bool found = false;
for (int p = 0; p < m_uniqueEdges.size(); p++)
{
if (IsAlmostZero(m_uniqueEdges[p] - edge) ||
IsAlmostZero(m_uniqueEdges[p] + edge))
{
found = true;
break;
}
}
if (!found)
{
m_uniqueEdges.push_back(edge);
}
if (edptr)
{
btAssert(edptr->m_face0 >= 0);
btAssert(edptr->m_face1 < 0);
edptr->m_face1 = i;
}
else
{
btInternalEdge ed;
ed.m_face0 = i;
edges.insert(vp, ed);
}
}
}
#ifdef USE_CONNECTED_FACES
for (int i = 0; i < m_faces.size(); i++)
{
int numVertices = m_faces[i].m_indices.size();
m_faces[i].m_connectedFaces.resize(numVertices);
for (int j = 0; j < numVertices; j++)
{
int k = (j + 1) % numVertices;
btInternalVertexPair vp(m_faces[i].m_indices[j], m_faces[i].m_indices[k]);
btInternalEdge* edptr = edges.find(vp);
btAssert(edptr);
btAssert(edptr->m_face0 >= 0);
btAssert(edptr->m_face1 >= 0);
int connectedFace = (edptr->m_face0 == i) ? edptr->m_face1 : edptr->m_face0;
m_faces[i].m_connectedFaces[j] = connectedFace;
}
}
#endif //USE_CONNECTED_FACES
initialize2();
}
bool btPolyhedralContactClipping::findSeparatingAxis( const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA, const btTransform& transB, btVector3& sep, btDiscreteCollisionDetectorInterface::Result& resultOut)
{
gActualSATPairTests++;
//#ifdef TEST_INTERNAL_OBJECTS
const btVector3 c0 = transA * hullA.m_localCenter;
const btVector3 c1 = transB * hullB.m_localCenter;
const btVector3 DeltaC2 = c0 - c1;
//#endif
btScalar dmin = FLT_MAX;
int curPlaneTests=0;
int numFacesA = hullA.m_faces.size();
// Test normals from hullA
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]);
btVector3 faceANormalWS = transA.getBasis() * Normal;
if (DeltaC2.dot(faceANormalWS)<0)
faceANormalWS*=-1.f;
curPlaneTests++;
#ifdef TEST_INTERNAL_OBJECTS
gExpectedNbTests++;
if(gUseInternalObject && !TestInternalObjects(transA, transB, DeltaC2, faceANormalWS, hullA, hullB, dmin))
continue;
gActualNbTests++;
#endif
btScalar d;
btVector3 wA, wB;
if(!TestSepAxis( hullA, hullB, transA, transB, faceANormalWS, d, wA, wB))
return false;
if(d<dmin)
{
dmin = d;
sep = faceANormalWS;
}
}
int numFacesB = hullB.m_faces.size();
// Test normals from hullB
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]);
btVector3 WorldNormal = transB.getBasis() * Normal;
if (DeltaC2.dot(WorldNormal)<0)
WorldNormal *=-1.f;
curPlaneTests++;
#ifdef TEST_INTERNAL_OBJECTS
gExpectedNbTests++;
if(gUseInternalObject && !TestInternalObjects(transA, transB, DeltaC2, WorldNormal, hullA, hullB, dmin))
continue;
gActualNbTests++;
#endif
btScalar d;
btVector3 wA, wB;
if(!TestSepAxis(hullA, hullB, transA, transB, WorldNormal, d,wA, wB))
return false;
if(d<dmin)
{
dmin = d;
sep = WorldNormal;
}
}
btVector3 edgeAstart, edgeAend, edgeBstart, edgeBend;
int edgeA=-1;
int edgeB=-1;
btVector3 worldEdgeA;
btVector3 worldEdgeB;
btVector3 witnessPointA, witnessPointB;
int curEdgeEdge = 0;
// Test edges
for(int e0=0;e0<hullA.m_uniqueEdges.size();e0++)
{
const btVector3 edge0 = hullA.m_uniqueEdges[e0];
const btVector3 WorldEdge0 = transA.getBasis() * edge0;
for(int e1=0;e1<hullB.m_uniqueEdges.size();e1++)
{
const btVector3 edge1 = hullB.m_uniqueEdges[e1];
const btVector3 WorldEdge1 = transB.getBasis() * edge1;
btVector3 Cross = WorldEdge0.cross(WorldEdge1);
curEdgeEdge++;
if(!IsAlmostZero(Cross))
{
Cross = Cross.normalize();
if (DeltaC2.dot(Cross)<0)
Cross *= -1.f;
#ifdef TEST_INTERNAL_OBJECTS
gExpectedNbTests++;
if(gUseInternalObject && !TestInternalObjects(transA, transB, DeltaC2, Cross, hullA, hullB, dmin))
continue;
gActualNbTests++;
#endif
btScalar 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 (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;
}
void btConvexPolyhedron::initialize()
{
btHashMap<btInternalVertexPair,btInternalEdge> edges;
float TotalArea = 0.0f;
m_localCenter.setValue(0, 0, 0);
for(int i=0;i<m_faces.size();i++)
{
int numVertices = m_faces[i].m_indices.size();
int NbTris = numVertices;
for(int j=0;j<NbTris;j++)
{
int k = (j+1)%numVertices;
btInternalVertexPair vp(m_faces[i].m_indices[j],m_faces[i].m_indices[k]);
btInternalEdge* edptr = edges.find(vp);
btVector3 edge = m_vertices[vp.m_v1]-m_vertices[vp.m_v0];
edge.normalize();
bool found = false;
for (int p=0;p<m_uniqueEdges.size();p++)
{
if (IsAlmostZero(m_uniqueEdges[p]-edge) ||
IsAlmostZero(m_uniqueEdges[p]+edge))
{
found = true;
break;
}
}
if (!found)
{
m_uniqueEdges.push_back(edge);
}
if (edptr)
{
btAssert(edptr->m_face0>=0);
btAssert(edptr->m_face1<0);
edptr->m_face1 = i;
} else
{
btInternalEdge ed;
ed.m_face0 = i;
edges.insert(vp,ed);
}
}
}
for(int i=0;i<m_faces.size();i++)
{
int numVertices = m_faces[i].m_indices.size();
m_faces[i].m_connectedFaces.resize(numVertices);
for(int j=0;j<numVertices;j++)
{
int k = (j+1)%numVertices;
btInternalVertexPair vp(m_faces[i].m_indices[j],m_faces[i].m_indices[k]);
btInternalEdge* edptr = edges.find(vp);
btAssert(edptr);
btAssert(edptr->m_face0>=0);
btAssert(edptr->m_face1>=0);
int connectedFace = (edptr->m_face0==i)?edptr->m_face1:edptr->m_face0;
m_faces[i].m_connectedFaces[j] = connectedFace;
}
}
for(int i=0;i<m_faces.size();i++)
{
int numVertices = m_faces[i].m_indices.size();
int NbTris = numVertices-2;
const btVector3& p0 = m_vertices[m_faces[i].m_indices[0]];
for(int j=1;j<=NbTris;j++)
{
int k = (j+1)%numVertices;
const btVector3& p1 = m_vertices[m_faces[i].m_indices[j]];
const btVector3& p2 = m_vertices[m_faces[i].m_indices[k]];
float Area = ((p0 - p1).cross(p0 - p2)).length() * 0.5f;
btVector3 Center = (p0+p1+p2)/3.0f;
m_localCenter += Area * Center;
TotalArea += Area;
}
}
m_localCenter /= TotalArea;
}
