/* | |
Bullet Continuous Collision Detection and Physics Library | |
* The b2CollidePolygons routines are Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com | |
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. | |
*/ | |
///btBox2dBox2dCollisionAlgorithm, with modified b2CollidePolygons routines from the Box2D library. | |
///The modifications include: switching from b2Vec to btVector3, redefinition of b2Dot, b2Cross | |
#include "BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.h" | |
#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" | |
#include "BulletCollision/CollisionShapes/btBoxShape.h" | |
#include "BulletCollision/CollisionDispatch/btCollisionObject.h" | |
#include "BulletCollision/CollisionDispatch/btBoxBoxDetector.h" | |
#include "BulletCollision/CollisionShapes/btBox2dShape.h" | |
#define USE_PERSISTENT_CONTACTS 1 | |
btBox2dBox2dCollisionAlgorithm::btBox2dBox2dCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* obj0,btCollisionObject* obj1) | |
: btActivatingCollisionAlgorithm(ci,obj0,obj1), | |
m_ownManifold(false), | |
m_manifoldPtr(mf) | |
{ | |
if (!m_manifoldPtr && m_dispatcher->needsCollision(obj0,obj1)) | |
{ | |
m_manifoldPtr = m_dispatcher->getNewManifold(obj0,obj1); | |
m_ownManifold = true; | |
} | |
} | |
btBox2dBox2dCollisionAlgorithm::~btBox2dBox2dCollisionAlgorithm() | |
{ | |
if (m_ownManifold) | |
{ | |
if (m_manifoldPtr) | |
m_dispatcher->releaseManifold(m_manifoldPtr); | |
} | |
} | |
void b2CollidePolygons(btManifoldResult* manifold, const btBox2dShape* polyA, const btTransform& xfA, const btBox2dShape* polyB, const btTransform& xfB); | |
//#include <stdio.h> | |
void btBox2dBox2dCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) | |
{ | |
if (!m_manifoldPtr) | |
return; | |
btCollisionObject* col0 = body0; | |
btCollisionObject* col1 = body1; | |
btBox2dShape* box0 = (btBox2dShape*)col0->getCollisionShape(); | |
btBox2dShape* box1 = (btBox2dShape*)col1->getCollisionShape(); | |
resultOut->setPersistentManifold(m_manifoldPtr); | |
b2CollidePolygons(resultOut,box0,col0->getWorldTransform(),box1,col1->getWorldTransform()); | |
// refreshContactPoints is only necessary when using persistent contact points. otherwise all points are newly added | |
if (m_ownManifold) | |
{ | |
resultOut->refreshContactPoints(); | |
} | |
} | |
btScalar btBox2dBox2dCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* /*body0*/,btCollisionObject* /*body1*/,const btDispatcherInfo& /*dispatchInfo*/,btManifoldResult* /*resultOut*/) | |
{ | |
//not yet | |
return 1.f; | |
} | |
struct ClipVertex | |
{ | |
btVector3 v; | |
int id; | |
//b2ContactID id; | |
//b2ContactID id; | |
}; | |
#define b2Dot(a,b) (a).dot(b) | |
#define b2Mul(a,b) (a)*(b) | |
#define b2MulT(a,b) (a).transpose()*(b) | |
#define b2Cross(a,b) (a).cross(b) | |
#define btCrossS(a,s) btVector3(s * a.getY(), -s * a.getX(),0.f) | |
int b2_maxManifoldPoints =2; | |
static int ClipSegmentToLine(ClipVertex vOut[2], ClipVertex vIn[2], | |
const btVector3& normal, btScalar offset) | |
{ | |
// Start with no output points | |
int numOut = 0; | |
// Calculate the distance of end points to the line | |
btScalar distance0 = b2Dot(normal, vIn[0].v) - offset; | |
btScalar distance1 = b2Dot(normal, vIn[1].v) - offset; | |
// If the points are behind the plane | |
if (distance0 <= 0.0f) vOut[numOut++] = vIn[0]; | |
if (distance1 <= 0.0f) vOut[numOut++] = vIn[1]; | |
// If the points are on different sides of the plane | |
if (distance0 * distance1 < 0.0f) | |
{ | |
// Find intersection point of edge and plane | |
btScalar interp = distance0 / (distance0 - distance1); | |
vOut[numOut].v = vIn[0].v + interp * (vIn[1].v - vIn[0].v); | |
if (distance0 > 0.0f) | |
{ | |
vOut[numOut].id = vIn[0].id; | |
} | |
else | |
{ | |
vOut[numOut].id = vIn[1].id; | |
} | |
++numOut; | |
} | |
return numOut; | |
} | |
// Find the separation between poly1 and poly2 for a give edge normal on poly1. | |
static btScalar EdgeSeparation(const btBox2dShape* poly1, const btTransform& xf1, int edge1, | |
const btBox2dShape* poly2, const btTransform& xf2) | |
{ | |
int count1 = poly1->getVertexCount(); | |
const btVector3* vertices1 = poly1->getVertices(); | |
const btVector3* normals1 = poly1->getNormals(); | |
int count2 = poly2->getVertexCount(); | |
const btVector3* vertices2 = poly2->getVertices(); | |
btAssert(0 <= edge1 && edge1 < count1); | |
// Convert normal from poly1's frame into poly2's frame. | |
btVector3 normal1World = b2Mul(xf1.getBasis(), normals1[edge1]); | |
btVector3 normal1 = b2MulT(xf2.getBasis(), normal1World); | |
// Find support vertex on poly2 for -normal. | |
int index = 0; | |
btScalar minDot = BT_LARGE_FLOAT; | |
for (int i = 0; i < count2; ++i) | |
{ | |
btScalar dot = b2Dot(vertices2[i], normal1); | |
if (dot < minDot) | |
{ | |
minDot = dot; | |
index = i; | |
} | |
} | |
btVector3 v1 = b2Mul(xf1, vertices1[edge1]); | |
btVector3 v2 = b2Mul(xf2, vertices2[index]); | |
btScalar separation = b2Dot(v2 - v1, normal1World); | |
return separation; | |
} | |
// Find the max separation between poly1 and poly2 using edge normals from poly1. | |
static btScalar FindMaxSeparation(int* edgeIndex, | |
const btBox2dShape* poly1, const btTransform& xf1, | |
const btBox2dShape* poly2, const btTransform& xf2) | |
{ | |
int count1 = poly1->getVertexCount(); | |
const btVector3* normals1 = poly1->getNormals(); | |
// Vector pointing from the centroid of poly1 to the centroid of poly2. | |
btVector3 d = b2Mul(xf2, poly2->getCentroid()) - b2Mul(xf1, poly1->getCentroid()); | |
btVector3 dLocal1 = b2MulT(xf1.getBasis(), d); | |
// Find edge normal on poly1 that has the largest projection onto d. | |
int edge = 0; | |
btScalar maxDot = -BT_LARGE_FLOAT; | |
for (int i = 0; i < count1; ++i) | |
{ | |
btScalar dot = b2Dot(normals1[i], dLocal1); | |
if (dot > maxDot) | |
{ | |
maxDot = dot; | |
edge = i; | |
} | |
} | |
// Get the separation for the edge normal. | |
btScalar s = EdgeSeparation(poly1, xf1, edge, poly2, xf2); | |
if (s > 0.0f) | |
{ | |
return s; | |
} | |
// Check the separation for the previous edge normal. | |
int prevEdge = edge - 1 >= 0 ? edge - 1 : count1 - 1; | |
btScalar sPrev = EdgeSeparation(poly1, xf1, prevEdge, poly2, xf2); | |
if (sPrev > 0.0f) | |
{ | |
return sPrev; | |
} | |
// Check the separation for the next edge normal. | |
int nextEdge = edge + 1 < count1 ? edge + 1 : 0; | |
btScalar sNext = EdgeSeparation(poly1, xf1, nextEdge, poly2, xf2); | |
if (sNext > 0.0f) | |
{ | |
return sNext; | |
} | |
// Find the best edge and the search direction. | |
int bestEdge; | |
btScalar bestSeparation; | |
int increment; | |
if (sPrev > s && sPrev > sNext) | |
{ | |
increment = -1; | |
bestEdge = prevEdge; | |
bestSeparation = sPrev; | |
} | |
else if (sNext > s) | |
{ | |
increment = 1; | |
bestEdge = nextEdge; | |
bestSeparation = sNext; | |
} | |
else | |
{ | |
*edgeIndex = edge; | |
return s; | |
} | |
// Perform a local search for the best edge normal. | |
for ( ; ; ) | |
{ | |
if (increment == -1) | |
edge = bestEdge - 1 >= 0 ? bestEdge - 1 : count1 - 1; | |
else | |
edge = bestEdge + 1 < count1 ? bestEdge + 1 : 0; | |
s = EdgeSeparation(poly1, xf1, edge, poly2, xf2); | |
if (s > 0.0f) | |
{ | |
return s; | |
} | |
if (s > bestSeparation) | |
{ | |
bestEdge = edge; | |
bestSeparation = s; | |
} | |
else | |
{ | |
break; | |
} | |
} | |
*edgeIndex = bestEdge; | |
return bestSeparation; | |
} | |
static void FindIncidentEdge(ClipVertex c[2], | |
const btBox2dShape* poly1, const btTransform& xf1, int edge1, | |
const btBox2dShape* poly2, const btTransform& xf2) | |
{ | |
int count1 = poly1->getVertexCount(); | |
const btVector3* normals1 = poly1->getNormals(); | |
int count2 = poly2->getVertexCount(); | |
const btVector3* vertices2 = poly2->getVertices(); | |
const btVector3* normals2 = poly2->getNormals(); | |
btAssert(0 <= edge1 && edge1 < count1); | |
// Get the normal of the reference edge in poly2's frame. | |
btVector3 normal1 = b2MulT(xf2.getBasis(), b2Mul(xf1.getBasis(), normals1[edge1])); | |
// Find the incident edge on poly2. | |
int index = 0; | |
btScalar minDot = BT_LARGE_FLOAT; | |
for (int i = 0; i < count2; ++i) | |
{ | |
btScalar dot = b2Dot(normal1, normals2[i]); | |
if (dot < minDot) | |
{ | |
minDot = dot; | |
index = i; | |
} | |
} | |
// Build the clip vertices for the incident edge. | |
int i1 = index; | |
int i2 = i1 + 1 < count2 ? i1 + 1 : 0; | |
c[0].v = b2Mul(xf2, vertices2[i1]); | |
// c[0].id.features.referenceEdge = (unsigned char)edge1; | |
// c[0].id.features.incidentEdge = (unsigned char)i1; | |
// c[0].id.features.incidentVertex = 0; | |
c[1].v = b2Mul(xf2, vertices2[i2]); | |
// c[1].id.features.referenceEdge = (unsigned char)edge1; | |
// c[1].id.features.incidentEdge = (unsigned char)i2; | |
// c[1].id.features.incidentVertex = 1; | |
} | |
// Find edge normal of max separation on A - return if separating axis is found | |
// Find edge normal of max separation on B - return if separation axis is found | |
// Choose reference edge as min(minA, minB) | |
// Find incident edge | |
// Clip | |
// The normal points from 1 to 2 | |
void b2CollidePolygons(btManifoldResult* manifold, | |
const btBox2dShape* polyA, const btTransform& xfA, | |
const btBox2dShape* polyB, const btTransform& xfB) | |
{ | |
int edgeA = 0; | |
btScalar separationA = FindMaxSeparation(&edgeA, polyA, xfA, polyB, xfB); | |
if (separationA > 0.0f) | |
return; | |
int edgeB = 0; | |
btScalar separationB = FindMaxSeparation(&edgeB, polyB, xfB, polyA, xfA); | |
if (separationB > 0.0f) | |
return; | |
const btBox2dShape* poly1; // reference poly | |
const btBox2dShape* poly2; // incident poly | |
btTransform xf1, xf2; | |
int edge1; // reference edge | |
unsigned char flip; | |
const btScalar k_relativeTol = 0.98f; | |
const btScalar k_absoluteTol = 0.001f; | |
// TODO_ERIN use "radius" of poly for absolute tolerance. | |
if (separationB > k_relativeTol * separationA + k_absoluteTol) | |
{ | |
poly1 = polyB; | |
poly2 = polyA; | |
xf1 = xfB; | |
xf2 = xfA; | |
edge1 = edgeB; | |
flip = 1; | |
} | |
else | |
{ | |
poly1 = polyA; | |
poly2 = polyB; | |
xf1 = xfA; | |
xf2 = xfB; | |
edge1 = edgeA; | |
flip = 0; | |
} | |
ClipVertex incidentEdge[2]; | |
FindIncidentEdge(incidentEdge, poly1, xf1, edge1, poly2, xf2); | |
int count1 = poly1->getVertexCount(); | |
const btVector3* vertices1 = poly1->getVertices(); | |
btVector3 v11 = vertices1[edge1]; | |
btVector3 v12 = edge1 + 1 < count1 ? vertices1[edge1+1] : vertices1[0]; | |
btVector3 dv = v12 - v11; | |
btVector3 sideNormal = b2Mul(xf1.getBasis(), v12 - v11); | |
sideNormal.normalize(); | |
btVector3 frontNormal = btCrossS(sideNormal, 1.0f); | |
v11 = b2Mul(xf1, v11); | |
v12 = b2Mul(xf1, v12); | |
btScalar frontOffset = b2Dot(frontNormal, v11); | |
btScalar sideOffset1 = -b2Dot(sideNormal, v11); | |
btScalar sideOffset2 = b2Dot(sideNormal, v12); | |
// Clip incident edge against extruded edge1 side edges. | |
ClipVertex clipPoints1[2]; | |
clipPoints1[0].v.setValue(0,0,0); | |
clipPoints1[1].v.setValue(0,0,0); | |
ClipVertex clipPoints2[2]; | |
clipPoints2[0].v.setValue(0,0,0); | |
clipPoints2[1].v.setValue(0,0,0); | |
int np; | |
// Clip to box side 1 | |
np = ClipSegmentToLine(clipPoints1, incidentEdge, -sideNormal, sideOffset1); | |
if (np < 2) | |
return; | |
// Clip to negative box side 1 | |
np = ClipSegmentToLine(clipPoints2, clipPoints1, sideNormal, sideOffset2); | |
if (np < 2) | |
{ | |
return; | |
} | |
// Now clipPoints2 contains the clipped points. | |
btVector3 manifoldNormal = flip ? -frontNormal : frontNormal; | |
int pointCount = 0; | |
for (int i = 0; i < b2_maxManifoldPoints; ++i) | |
{ | |
btScalar separation = b2Dot(frontNormal, clipPoints2[i].v) - frontOffset; | |
if (separation <= 0.0f) | |
{ | |
//b2ManifoldPoint* cp = manifold->points + pointCount; | |
//btScalar separation = separation; | |
//cp->localPoint1 = b2MulT(xfA, clipPoints2[i].v); | |
//cp->localPoint2 = b2MulT(xfB, clipPoints2[i].v); | |
manifold->addContactPoint(-manifoldNormal,clipPoints2[i].v,separation); | |
// cp->id = clipPoints2[i].id; | |
// cp->id.features.flip = flip; | |
++pointCount; | |
} | |
} | |
// manifold->pointCount = pointCount;} | |
} |