Added support for separating axis test for polyhedral shapes
Added initial support for polyhedral contact clipping. This clipping takes a separating normal, that can be computed using either SAT or GJK/EPA. To enable clipping, use btPolyhedralConvexShape::initializePolyhedralFeatures(); (needs to be enabled for both convex shapes) No concave trimesh support for SAT/clipping yet. To enable SAT, see the toggle in btConvexConvexAlgorithm. Fixes in contact normal in btGjkPairDetector. Hopefully this doesn't cause any regression (we need unit tests!)
This commit is contained in:
erwin.coumans
@@ -46,6 +46,7 @@ SET(BulletCollision_SRCS
|
||||
CollisionShapes/btConvexHullShape.cpp
|
||||
CollisionShapes/btConvexInternalShape.cpp
|
||||
CollisionShapes/btConvexPointCloudShape.cpp
|
||||
CollisionShapes/btConvexPolyhedron.cpp
|
||||
CollisionShapes/btConvexShape.cpp
|
||||
CollisionShapes/btConvex2dShape.cpp
|
||||
CollisionShapes/btConvexTriangleMeshShape.cpp
|
||||
@@ -92,6 +93,7 @@ SET(BulletCollision_SRCS
|
||||
NarrowPhaseCollision/btRaycastCallback.cpp
|
||||
NarrowPhaseCollision/btSubSimplexConvexCast.cpp
|
||||
NarrowPhaseCollision/btVoronoiSimplexSolver.cpp
|
||||
NarrowPhaseCollision/btPolyhedralContactClipping.cpp
|
||||
)
|
||||
|
||||
SET(Root_HDRS
|
||||
@@ -150,6 +152,7 @@ SET(CollisionShapes_HDRS
|
||||
CollisionShapes/btConvexHullShape.h
|
||||
CollisionShapes/btConvexInternalShape.h
|
||||
CollisionShapes/btConvexPointCloudShape.h
|
||||
CollisionShapes/btConvexPolyhedron.h
|
||||
CollisionShapes/btConvexShape.h
|
||||
CollisionShapes/btConvex2dShape.h
|
||||
CollisionShapes/btConvexTriangleMeshShape.h
|
||||
@@ -224,6 +227,7 @@ SET(NarrowPhaseCollision_HDRS
|
||||
NarrowPhaseCollision/btSimplexSolverInterface.h
|
||||
NarrowPhaseCollision/btSubSimplexConvexCast.h
|
||||
NarrowPhaseCollision/btVoronoiSimplexSolver.h
|
||||
NarrowPhaseCollision/btPolyhedralContactClipping.h
|
||||
)
|
||||
|
||||
SET(BulletCollision_HDRS
|
||||
|
||||
@@ -48,7 +48,7 @@ subject to the following restrictions:
|
||||
|
||||
#include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h"
|
||||
#include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h"
|
||||
|
||||
#include "BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.h"
|
||||
|
||||
|
||||
///////////
|
||||
@@ -330,7 +330,6 @@ void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btColl
|
||||
}
|
||||
#endif //BT_DISABLE_CAPSULE_CAPSULE_COLLIDER
|
||||
|
||||
|
||||
#ifdef USE_SEPDISTANCE_UTIL2
|
||||
if (dispatchInfo.m_useConvexConservativeDistanceUtil)
|
||||
{
|
||||
@@ -389,6 +388,54 @@ void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btColl
|
||||
}
|
||||
#endif //USE_SEPDISTANCE_UTIL2
|
||||
|
||||
|
||||
if (min0->isPolyhedral() && min1->isPolyhedral())
|
||||
{
|
||||
btPolyhedralConvexShape* polyhedronA = (btPolyhedralConvexShape*) min0;
|
||||
btPolyhedralConvexShape* polyhedronB = (btPolyhedralConvexShape*) min1;
|
||||
if (polyhedronA->getConvexPolyhedron() && polyhedronB->getConvexPolyhedron())
|
||||
{
|
||||
|
||||
btScalar maxDist = 0.f;
|
||||
|
||||
if (dispatchInfo.m_convexMaxDistanceUseCPT)
|
||||
{
|
||||
maxDist = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactProcessingThreshold();
|
||||
} else
|
||||
{
|
||||
maxDist = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactBreakingThreshold();
|
||||
}
|
||||
|
||||
maxDist =0.f;
|
||||
btVector3 sepNormalWorldSpace;
|
||||
//#define USE_SAT_TEST
|
||||
#ifdef USE_SAT_TEST
|
||||
bool foundSepAxis = btPolyhedralContactClipping::findSeparatingAxis(
|
||||
*polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(),
|
||||
body0->getWorldTransform(),
|
||||
body1->getWorldTransform(),
|
||||
sepNormalWorldSpace);
|
||||
#else
|
||||
bool foundSepAxis = true;
|
||||
sepNormalWorldSpace = gjkPairDetector.getCachedSeparatingAxis().normalized();
|
||||
#endif //USE_SAT_TEST
|
||||
if (foundSepAxis)
|
||||
{
|
||||
btPolyhedralContactClipping::clipFaceContacts(sepNormalWorldSpace, *polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(),
|
||||
body0->getWorldTransform(),
|
||||
body1->getWorldTransform(), maxDist, *resultOut);
|
||||
|
||||
if (m_ownManifold)
|
||||
{
|
||||
resultOut->refreshContactPoints();
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
//now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects
|
||||
|
||||
//perform perturbation when more then 'm_minimumPointsPerturbationThreshold' points
|
||||
|
||||
@@ -145,7 +145,7 @@ public:
|
||||
|
||||
virtual void getVertex(int i,btVector3& vtx) const
|
||||
{
|
||||
btVector3 halfExtents = getHalfExtentsWithoutMargin();
|
||||
btVector3 halfExtents = getHalfExtentsWithMargin();
|
||||
|
||||
vtx = btVector3(
|
||||
halfExtents.x() * (1-(i&1)) - halfExtents.x() * (i&1),
|
||||
|
||||
185
src/BulletCollision/CollisionShapes/btConvexPolyhedron.cpp
Normal file
185
src/BulletCollision/CollisionShapes/btConvexPolyhedron.cpp
Normal file
@@ -0,0 +1,185 @@
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2011 Advanced Micro Devices, Inc. http://bulletphysics.org
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
|
||||
///This file was written by Erwin Coumans
|
||||
///Separating axis rest based on work from Pierre Terdiman, see
|
||||
///And contact clipping based on work from Simon Hobbs
|
||||
|
||||
|
||||
#include "btConvexPolyhedron.h"
|
||||
#include "LinearMath/btHashMap.h"
|
||||
|
||||
btConvexPolyhedron::btConvexPolyhedron()
|
||||
{
|
||||
|
||||
}
|
||||
btConvexPolyhedron::~btConvexPolyhedron()
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
|
||||
inline bool IsAlmostZero(const btVector3& v)
|
||||
{
|
||||
if(fabsf(v.x())>1e-6 || fabsf(v.y())>1e-6 || fabsf(v.z())>1e-6) return false;
|
||||
return true;
|
||||
}
|
||||
|
||||
struct btInternalVertexPair
|
||||
{
|
||||
btInternalVertexPair(short int v0,short int v1)
|
||||
:m_v0(v0),
|
||||
m_v1(v1)
|
||||
{
|
||||
if (m_v1>m_v0)
|
||||
btSwap(m_v0,m_v1);
|
||||
}
|
||||
short int m_v0;
|
||||
short int m_v1;
|
||||
int getHash() const
|
||||
{
|
||||
return m_v0+(m_v1<<16);
|
||||
}
|
||||
bool equals(const btInternalVertexPair& other) const
|
||||
{
|
||||
return m_v0==other.m_v0 && m_v1==other.m_v1;
|
||||
}
|
||||
};
|
||||
|
||||
struct btInternalEdge
|
||||
{
|
||||
btInternalEdge()
|
||||
:m_face0(-1),
|
||||
m_face1(-1)
|
||||
{
|
||||
}
|
||||
short int m_face0;
|
||||
short int m_face1;
|
||||
};
|
||||
|
||||
//
|
||||
|
||||
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;
|
||||
|
||||
}
|
||||
|
||||
|
||||
void btConvexPolyhedron::project(const btTransform& trans, const btVector3& dir, float& min, float& max) const
|
||||
{
|
||||
min = FLT_MAX;
|
||||
max = -FLT_MAX;
|
||||
int numVerts = m_vertices.size();
|
||||
for(int i=0;i<numVerts;i++)
|
||||
{
|
||||
btVector3 pt = trans * m_vertices[i];
|
||||
float dp = pt.dot(dir);
|
||||
if(dp < min) min = dp;
|
||||
if(dp > max) max = dp;
|
||||
}
|
||||
if(min>max)
|
||||
{
|
||||
float tmp = min;
|
||||
min = max;
|
||||
max = tmp;
|
||||
}
|
||||
}
|
||||
54
src/BulletCollision/CollisionShapes/btConvexPolyhedron.h
Normal file
54
src/BulletCollision/CollisionShapes/btConvexPolyhedron.h
Normal file
@@ -0,0 +1,54 @@
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2011 Advanced Micro Devices, Inc. http://bulletphysics.org
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
|
||||
///This file was written by Erwin Coumans
|
||||
|
||||
|
||||
#ifndef _BT_POLYHEDRAL_FEATURES_H
|
||||
#define _BT_POLYHEDRAL_FEATURES_H
|
||||
|
||||
#include "LinearMath/btTransform.h"
|
||||
#include "LinearMath/btAlignedObjectArray.h"
|
||||
|
||||
|
||||
struct btFace
|
||||
{
|
||||
btAlignedObjectArray<int> m_indices;
|
||||
btAlignedObjectArray<int> m_connectedFaces;
|
||||
float m_plane[4];
|
||||
};
|
||||
|
||||
|
||||
class btConvexPolyhedron
|
||||
{
|
||||
public:
|
||||
btConvexPolyhedron();
|
||||
virtual ~btConvexPolyhedron();
|
||||
|
||||
btAlignedObjectArray<btVector3> m_vertices;
|
||||
btAlignedObjectArray<btFace> m_faces;
|
||||
btAlignedObjectArray<btVector3> m_uniqueEdges;
|
||||
btVector3 m_localCenter;
|
||||
|
||||
void initialize();
|
||||
|
||||
void project(const btTransform& trans, const btVector3& dir, float& min, float& max) const;
|
||||
};
|
||||
|
||||
|
||||
#endif //_BT_POLYHEDRAL_FEATURES_H
|
||||
|
||||
|
||||
@@ -14,12 +14,140 @@ subject to the following restrictions:
|
||||
*/
|
||||
|
||||
#include "BulletCollision/CollisionShapes/btPolyhedralConvexShape.h"
|
||||
#include "btConvexPolyhedron.h"
|
||||
#include "LinearMath/btConvexHullComputer.h"
|
||||
#include <new.h>
|
||||
|
||||
btPolyhedralConvexShape::btPolyhedralConvexShape() :btConvexInternalShape()
|
||||
btPolyhedralConvexShape::btPolyhedralConvexShape() :btConvexInternalShape(),
|
||||
m_polyhedron(0)
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
btPolyhedralConvexShape::~btPolyhedralConvexShape()
|
||||
{
|
||||
if (m_polyhedron)
|
||||
{
|
||||
btAlignedFree(m_polyhedron);
|
||||
}
|
||||
}
|
||||
|
||||
bool btPolyhedralConvexShape::initializePolyhedralFeatures()
|
||||
{
|
||||
if (m_polyhedron)
|
||||
btAlignedFree(m_polyhedron);
|
||||
|
||||
void* mem = btAlignedAlloc(sizeof(btConvexPolyhedron),16);
|
||||
m_polyhedron = new (mem) btConvexPolyhedron;
|
||||
|
||||
btAlignedObjectArray<btVector3> tmpVertices;
|
||||
for (int i=0;i<getNumVertices();i++)
|
||||
{
|
||||
btVector3& newVertex = tmpVertices.expand();
|
||||
getVertex(i,newVertex);
|
||||
}
|
||||
|
||||
btConvexHullComputer conv;
|
||||
conv.compute(&tmpVertices[0].getX(), sizeof(btVector3),tmpVertices.size(),0.f,0.f);
|
||||
|
||||
btAlignedObjectArray<btVector3> faceNormals;
|
||||
int numFaces = conv.faces.size();
|
||||
faceNormals.resize(numFaces);
|
||||
btConvexHullComputer* convexUtil = &conv;
|
||||
|
||||
|
||||
|
||||
m_polyhedron->m_faces.resize(numFaces);
|
||||
int numVertices = convexUtil->vertices.size();
|
||||
m_polyhedron->m_vertices.resize(numVertices);
|
||||
for (int p=0;p<numVertices;p++)
|
||||
{
|
||||
m_polyhedron->m_vertices[p] = convexUtil->vertices[p];
|
||||
}
|
||||
|
||||
for (int i=0;i<numFaces;i++)
|
||||
{
|
||||
int face = convexUtil->faces[i];
|
||||
//printf("face=%d\n",face);
|
||||
const btConvexHullComputer::Edge* firstEdge = &convexUtil->edges[face];
|
||||
const btConvexHullComputer::Edge* edge = firstEdge;
|
||||
|
||||
btVector3 edges[3];
|
||||
int numEdges = 0;
|
||||
//compute face normals
|
||||
|
||||
btScalar maxCross2 = 0.f;
|
||||
int chosenEdge = -1;
|
||||
|
||||
do
|
||||
{
|
||||
|
||||
int src = edge->getSourceVertex();
|
||||
m_polyhedron->m_faces[i].m_indices.push_back(src);
|
||||
int targ = edge->getTargetVertex();
|
||||
btVector3 wa = convexUtil->vertices[src];
|
||||
|
||||
btVector3 wb = convexUtil->vertices[targ];
|
||||
btVector3 newEdge = wb-wa;
|
||||
if (!newEdge.fuzzyZero())
|
||||
{
|
||||
newEdge.normalize();
|
||||
if (!numEdges)
|
||||
{
|
||||
edges[numEdges++] = newEdge;
|
||||
} else
|
||||
{
|
||||
btVector3 cr = (edges[0].cross(newEdge));
|
||||
btScalar cr2 = cr.length2();
|
||||
if (cr2 > maxCross2)
|
||||
{
|
||||
chosenEdge = m_polyhedron->m_faces[i].m_indices.size();
|
||||
numEdges=1;//replace current edge
|
||||
edges[numEdges++] = newEdge;
|
||||
maxCross2=cr2;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
edge = edge->getNextEdgeOfFace();
|
||||
} while (edge!=firstEdge);
|
||||
|
||||
btScalar planeEq = 1e30f;
|
||||
|
||||
|
||||
if (numEdges==2)
|
||||
{
|
||||
faceNormals[i] = edges[0].cross(edges[1]);
|
||||
faceNormals[i].normalize();
|
||||
m_polyhedron->m_faces[i].m_plane[0] = -faceNormals[i].getX();
|
||||
m_polyhedron->m_faces[i].m_plane[1] = -faceNormals[i].getY();
|
||||
m_polyhedron->m_faces[i].m_plane[2] = -faceNormals[i].getZ();
|
||||
m_polyhedron->m_faces[i].m_plane[3] = planeEq;
|
||||
|
||||
}
|
||||
else
|
||||
{
|
||||
btAssert(0);//degenerate?
|
||||
faceNormals[i].setZero();
|
||||
}
|
||||
|
||||
for (int v=0;v<m_polyhedron->m_faces[i].m_indices.size();v++)
|
||||
{
|
||||
btScalar eq = m_polyhedron->m_vertices[m_polyhedron->m_faces[i].m_indices[v]].dot(faceNormals[i]);
|
||||
if (planeEq>eq)
|
||||
{
|
||||
planeEq=eq;
|
||||
}
|
||||
}
|
||||
m_polyhedron->m_faces[i].m_plane[3] = planeEq;
|
||||
}
|
||||
|
||||
|
||||
m_polyhedron->initialize();
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
btVector3 btPolyhedralConvexShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0)const
|
||||
{
|
||||
@@ -191,3 +319,6 @@ void btPolyhedralConvexAabbCachingShape::recalcLocalAabb()
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -18,18 +18,32 @@ subject to the following restrictions:
|
||||
|
||||
#include "LinearMath/btMatrix3x3.h"
|
||||
#include "btConvexInternalShape.h"
|
||||
class btConvexPolyhedron;
|
||||
|
||||
|
||||
///The btPolyhedralConvexShape is an internal interface class for polyhedral convex shapes.
|
||||
class btPolyhedralConvexShape : public btConvexInternalShape
|
||||
{
|
||||
|
||||
|
||||
protected:
|
||||
|
||||
btConvexPolyhedron* m_polyhedron;
|
||||
|
||||
public:
|
||||
|
||||
btPolyhedralConvexShape();
|
||||
|
||||
virtual ~btPolyhedralConvexShape();
|
||||
|
||||
///optional method mainly used to generate multiple contact points by clipping polyhedral features (faces/edges)
|
||||
virtual bool initializePolyhedralFeatures();
|
||||
|
||||
const btConvexPolyhedron* getConvexPolyhedron() const
|
||||
{
|
||||
return m_polyhedron;
|
||||
}
|
||||
|
||||
//brute force implementations
|
||||
|
||||
virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const;
|
||||
|
||||
@@ -254,20 +254,21 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
|
||||
}
|
||||
#endif //
|
||||
|
||||
m_cachedSeparatingAxis = newCachedSeparatingAxis;
|
||||
|
||||
//redundant m_simplexSolver->compute_points(pointOnA, pointOnB);
|
||||
|
||||
//are we getting any closer ?
|
||||
if (previousSquaredDistance - squaredDistance <= SIMD_EPSILON * previousSquaredDistance)
|
||||
{
|
||||
m_simplexSolver->backup_closest(m_cachedSeparatingAxis);
|
||||
// m_simplexSolver->backup_closest(m_cachedSeparatingAxis);
|
||||
checkSimplex = true;
|
||||
m_degenerateSimplex = 12;
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
m_cachedSeparatingAxis = newCachedSeparatingAxis;
|
||||
|
||||
//degeneracy, this is typically due to invalid/uninitialized worldtransforms for a btCollisionObject
|
||||
if (m_curIter++ > gGjkMaxIter)
|
||||
{
|
||||
@@ -294,7 +295,7 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
|
||||
if (!check)
|
||||
{
|
||||
//do we need this backup_closest here ?
|
||||
m_simplexSolver->backup_closest(m_cachedSeparatingAxis);
|
||||
// m_simplexSolver->backup_closest(m_cachedSeparatingAxis);
|
||||
m_degenerateSimplex = 13;
|
||||
break;
|
||||
}
|
||||
@@ -303,7 +304,7 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
|
||||
if (checkSimplex)
|
||||
{
|
||||
m_simplexSolver->compute_points(pointOnA, pointOnB);
|
||||
normalInB = pointOnA-pointOnB;
|
||||
normalInB = m_cachedSeparatingAxis;
|
||||
btScalar lenSqr =m_cachedSeparatingAxis.length2();
|
||||
|
||||
//valid normal
|
||||
|
||||
@@ -0,0 +1,337 @@
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2011 Advanced Micro Devices, Inc. http://bulletphysics.org
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
|
||||
///This file was written by Erwin Coumans
|
||||
///Separating axis rest based on work from Pierre Terdiman, see
|
||||
///And contact clipping based on work from Simon Hobbs
|
||||
|
||||
|
||||
#include "btPolyhedralContactClipping.h"
|
||||
#include "BulletCollision/CollisionShapes/btConvexPolyhedron.h"
|
||||
|
||||
#include <float.h> //for FLT_MAX
|
||||
|
||||
|
||||
// Clips a face to the back of a plane
|
||||
void btPolyhedralContactClipping::clipFace(const btVertexArray& pVtxIn, btVertexArray& ppVtxOut, const btVector3& planeNormalWS,btScalar planeEqWS)
|
||||
{
|
||||
|
||||
int ve;
|
||||
btScalar ds, de;
|
||||
int numVerts = pVtxIn.size();
|
||||
if (numVerts < 2)
|
||||
return;
|
||||
|
||||
btVector3 firstVertex=pVtxIn[pVtxIn.size()-1];
|
||||
btVector3 endVertex = pVtxIn[0];
|
||||
|
||||
ds = planeNormalWS.dot(firstVertex)+planeEqWS;
|
||||
|
||||
for (ve = 0; ve < numVerts; ve++)
|
||||
{
|
||||
endVertex=pVtxIn[ve];
|
||||
|
||||
de = planeNormalWS.dot(endVertex)+planeEqWS;
|
||||
|
||||
if (ds<0)
|
||||
{
|
||||
if (de<0)
|
||||
{
|
||||
// Start < 0, end < 0, so output endVertex
|
||||
ppVtxOut.push_back(endVertex);
|
||||
}
|
||||
else
|
||||
{
|
||||
// Start < 0, end >= 0, so output intersection
|
||||
ppVtxOut.push_back( firstVertex.lerp(endVertex,btScalar(ds * 1.f/(ds - de))));
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if (de<0)
|
||||
{
|
||||
// Start >= 0, end < 0 so output intersection and end
|
||||
ppVtxOut.push_back(firstVertex.lerp(endVertex,btScalar(ds * 1.f/(ds - de))));
|
||||
ppVtxOut.push_back(endVertex);
|
||||
}
|
||||
}
|
||||
firstVertex = endVertex;
|
||||
ds = de;
|
||||
}
|
||||
}
|
||||
#include <stdio.h>
|
||||
|
||||
|
||||
static bool TestSepAxis(const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, const btVector3& sep_axis, float& depth)
|
||||
{
|
||||
float Min0,Max0;
|
||||
float Min1,Max1;
|
||||
hullA.project(transA,sep_axis, Min0, Max0);
|
||||
hullB.project(transB, sep_axis, Min1, Max1);
|
||||
|
||||
if(Max0<Min1 || Max1<Min0)
|
||||
return false;
|
||||
|
||||
float d0 = Max0 - Min1;
|
||||
assert(d0>=0.0f);
|
||||
float d1 = Max1 - Min0;
|
||||
assert(d1>=0.0f);
|
||||
depth = d0<d1 ? d0:d1;
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
|
||||
static int gActualSATPairTests=0;
|
||||
|
||||
inline bool IsAlmostZero(const btVector3& v)
|
||||
{
|
||||
if(fabsf(v.x())>1e-6 || fabsf(v.y())>1e-6 || fabsf(v.z())>1e-6) return false;
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
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.mLocalCenter;
|
||||
const btVector3 c1 = transB * hullB.mLocalCenter;
|
||||
const btVector3 DeltaC2 = c0 - c1;
|
||||
#endif
|
||||
|
||||
float 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;
|
||||
|
||||
curPlaneTests++;
|
||||
#ifdef TEST_INTERNAL_OBJECTS
|
||||
gExpectedNbTests++;
|
||||
if(gUseInternalObject && !TestInternalObjects(transA,transB,DeltaC2, faceANormalWS, hullA, hullB, dmin))
|
||||
continue;
|
||||
gActualNbTests++;
|
||||
#endif
|
||||
|
||||
float 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;
|
||||
|
||||
curPlaneTests++;
|
||||
#ifdef TEST_INTERNAL_OBJECTS
|
||||
gExpectedNbTests++;
|
||||
if(gUseInternalObject && !TestInternalObjects(transA,transB,DeltaC2, WorldNormal, hullA, hullB, dmin))
|
||||
continue;
|
||||
gActualNbTests++;
|
||||
#endif
|
||||
|
||||
float 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();
|
||||
|
||||
#ifdef TEST_INTERNAL_OBJECTS
|
||||
gExpectedNbTests++;
|
||||
if(gUseInternalObject && !TestInternalObjects(transA,transB,DeltaC2, Cross, hullA, hullB, dmin))
|
||||
continue;
|
||||
gActualNbTests++;
|
||||
#endif
|
||||
|
||||
float dist;
|
||||
if(!TestSepAxis( hullA, hullB, transA,transB, Cross, dist))
|
||||
return false;
|
||||
|
||||
if(dist<dmin)
|
||||
{
|
||||
dmin = dist;
|
||||
sep = Cross;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
void btPolyhedralContactClipping::clipFaceContacts(const btVector3& separatingNormal, const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, const btScalar maxDist, btDiscreteCollisionDetectorInterface::Result& resultOut)
|
||||
{
|
||||
|
||||
btScalar curMaxDist=maxDist;
|
||||
int closestFaceA=-1, closestFaceB=-1;
|
||||
|
||||
{
|
||||
btScalar dmax = -FLT_MAX;
|
||||
for(int face=0;face<hullB.m_faces.size();face++)
|
||||
{
|
||||
const btVector3 Normal(hullB.m_faces[face].m_plane[0], hullB.m_faces[face].m_plane[1], hullB.m_faces[face].m_plane[2]);
|
||||
const btVector3 WorldNormal = transB.getBasis() * Normal;
|
||||
|
||||
btScalar d = WorldNormal.dot(separatingNormal);
|
||||
if (d > dmax)
|
||||
{
|
||||
dmax = d;
|
||||
closestFaceB = face;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
{
|
||||
btScalar dmin = FLT_MAX;
|
||||
for(int face=0;face<hullA.m_faces.size();face++)
|
||||
{
|
||||
const btVector3 Normal(hullA.m_faces[face].m_plane[0], hullA.m_faces[face].m_plane[1], hullA.m_faces[face].m_plane[2]);
|
||||
const btVector3 faceANormalWS = transA.getBasis() * Normal;
|
||||
|
||||
btScalar d = faceANormalWS.dot(separatingNormal);
|
||||
if (d < dmin)
|
||||
{
|
||||
dmin = d;
|
||||
closestFaceA = face;
|
||||
}
|
||||
}
|
||||
}
|
||||
if (closestFaceA<0 || closestFaceB<0)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
// setup initial clip face (minimizing face from hull B)
|
||||
btVertexArray worldVertsB1;
|
||||
btVertexArray worldVertsB2;
|
||||
|
||||
btVertexArray* pVtxIn = &worldVertsB1;
|
||||
btVertexArray* pVtxOut = &worldVertsB2;
|
||||
|
||||
const btFace& polyA = hullA.m_faces[closestFaceA];
|
||||
{
|
||||
const btFace& polyB = hullB.m_faces[closestFaceB];
|
||||
const int numVertices = polyB.m_indices.size();
|
||||
for(int e0=0;e0<numVertices;e0++)
|
||||
{
|
||||
const btVector3& b = hullB.m_vertices[polyB.m_indices[e0]];
|
||||
pVtxIn->push_back(transB*b);
|
||||
}
|
||||
}
|
||||
pVtxOut->reserve(pVtxIn->size());
|
||||
|
||||
// clip polygon to back of planes of all faces of hull A that are adjacent to witness face
|
||||
int numContacts = pVtxIn->size();
|
||||
int numVerticesA = polyA.m_indices.size();
|
||||
for(int e0=0;e0<numVerticesA;e0++)
|
||||
{
|
||||
const btVector3& a = hullA.m_vertices[polyA.m_indices[e0]];
|
||||
const btVector3& b = hullA.m_vertices[polyA.m_indices[(e0+1)%numVerticesA]];
|
||||
const btVector3 edge0 = a - b;
|
||||
const btVector3 WorldEdge0 = transA.getBasis() * edge0;
|
||||
int otherFace = polyA.m_connectedFaces[e0];
|
||||
btVector3 localPlaneNormal (hullA.m_faces[otherFace].m_plane[0],hullA.m_faces[otherFace].m_plane[1],hullA.m_faces[otherFace].m_plane[2]);
|
||||
btScalar localPlaneEq = hullA.m_faces[otherFace].m_plane[3];
|
||||
|
||||
btVector3 planeNormalWS = transA.getBasis()*localPlaneNormal;
|
||||
btScalar planeEqWS=localPlaneEq-planeNormalWS.dot(transA.getOrigin());
|
||||
//clip face
|
||||
|
||||
clipFace(*pVtxIn, *pVtxOut,planeNormalWS,planeEqWS);
|
||||
btSwap(pVtxIn,pVtxOut);
|
||||
pVtxOut->resize(0);
|
||||
}
|
||||
|
||||
|
||||
|
||||
//#define ONLY_REPORT_DEEPEST_POINT
|
||||
|
||||
btVector3 point;
|
||||
|
||||
|
||||
// only keep points that are behind the witness face
|
||||
{
|
||||
btVector3 localPlaneNormal (polyA.m_plane[0],polyA.m_plane[1],polyA.m_plane[2]);
|
||||
btScalar localPlaneEq = polyA.m_plane[3];
|
||||
btVector3 planeNormalWS = transA.getBasis()*localPlaneNormal;
|
||||
btScalar planeEqWS=localPlaneEq-planeNormalWS.dot(transA.getOrigin());
|
||||
for (int i=0;i<pVtxIn->size();i++)
|
||||
{
|
||||
|
||||
btScalar depth = planeNormalWS.dot(pVtxIn->at(i))+planeEqWS;
|
||||
if (depth <=0)
|
||||
{
|
||||
btVector3 point = pVtxIn->at(i);
|
||||
#ifdef ONLY_REPORT_DEEPEST_POINT
|
||||
curMaxDist = depth;
|
||||
#else
|
||||
#if 0
|
||||
if (depth<-3)
|
||||
{
|
||||
printf("error in btPolyhedralContactClipping depth = %f\n", depth);
|
||||
printf("likely wrong separatingNormal passed in\n");
|
||||
}
|
||||
#endif
|
||||
resultOut.addContactPoint(separatingNormal,point,depth);
|
||||
#endif
|
||||
}
|
||||
}
|
||||
}
|
||||
#ifdef ONLY_REPORT_DEEPEST_POINT
|
||||
if (curMaxDist<maxDist)
|
||||
{
|
||||
resultOut.addContactPoint(separatingNormal,point,curMaxDist);
|
||||
}
|
||||
#endif //ONLY_REPORT_DEEPEST_POINT
|
||||
|
||||
}
|
||||
@@ -0,0 +1,43 @@
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2011 Advanced Micro Devices, Inc. http://bulletphysics.org
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
|
||||
///This file was written by Erwin Coumans
|
||||
|
||||
|
||||
#ifndef __POLYHEDRAL_CONTACT_CLIPPING_H
|
||||
#define __POLYHEDRAL_CONTACT_CLIPPING_H
|
||||
|
||||
|
||||
#include "LinearMath/btAlignedObjectArray.h"
|
||||
#include "LinearMath/btTransform.h"
|
||||
#include "btDiscreteCollisionDetectorInterface.h"
|
||||
|
||||
class btConvexPolyhedron;
|
||||
|
||||
typedef btAlignedObjectArray<btVector3> btVertexArray;
|
||||
|
||||
// Clips a face to the back of a plane
|
||||
struct btPolyhedralContactClipping
|
||||
{
|
||||
static void clipFaceContacts(const btVector3& separatingNormal, const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, const btScalar maxDist, btDiscreteCollisionDetectorInterface::Result& resultOut);
|
||||
|
||||
static void clipFace(const btVertexArray& pVtxIn, btVertexArray& ppVtxOut, const btVector3& planeNormalWS,btScalar planeEqWS);
|
||||
|
||||
static bool findSeparatingAxis( const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, btVector3& sep);
|
||||
};
|
||||
|
||||
#endif // __POLYHEDRAL_CONTACT_CLIPPING_H
|
||||
|
||||
Reference in New Issue
Block a user