diff options
Diffstat (limited to 'src/BulletCollision/CollisionDispatch')
4 files changed, 845 insertions, 0 deletions
diff --git a/src/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.cpp b/src/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.cpp new file mode 100644 index 000000000..757a35973 --- /dev/null +++ b/src/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.cpp @@ -0,0 +1,437 @@ +/*
+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 "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;}
+}
diff --git a/src/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.h b/src/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.h new file mode 100644 index 000000000..2766c3fc2 --- /dev/null +++ b/src/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.h @@ -0,0 +1,66 @@ +/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+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.
+*/
+
+#ifndef BOX_2D_BOX_2D__COLLISION_ALGORITHM_H
+#define BOX_2D_BOX_2D__COLLISION_ALGORITHM_H
+
+#include "BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
+#include "BulletCollision/BroadphaseCollision/btDispatcher.h"
+#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h"
+
+class btPersistentManifold;
+
+///box-box collision detection
+class btBox2dBox2dCollisionAlgorithm : public btActivatingCollisionAlgorithm
+{
+ bool m_ownManifold;
+ btPersistentManifold* m_manifoldPtr;
+
+public:
+ btBox2dBox2dCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci)
+ : btActivatingCollisionAlgorithm(ci) {}
+
+ virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+ virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+ btBox2dBox2dCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1);
+
+ virtual ~btBox2dBox2dCollisionAlgorithm();
+
+ virtual void getAllContactManifolds(btManifoldArray& manifoldArray)
+ {
+ if (m_manifoldPtr && m_ownManifold)
+ {
+ manifoldArray.push_back(m_manifoldPtr);
+ }
+ }
+
+
+ struct CreateFunc :public btCollisionAlgorithmCreateFunc
+ {
+ virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
+ {
+ int bbsize = sizeof(btBox2dBox2dCollisionAlgorithm);
+ void* ptr = ci.m_dispatcher1->allocateCollisionAlgorithm(bbsize);
+ return new(ptr) btBox2dBox2dCollisionAlgorithm(0,ci,body0,body1);
+ }
+ };
+
+};
+
+#endif //BOX_2D_BOX_2D__COLLISION_ALGORITHM_H
+
diff --git a/src/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.cpp b/src/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.cpp new file mode 100644 index 000000000..9d5d530c7 --- /dev/null +++ b/src/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.cpp @@ -0,0 +1,247 @@ +/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+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.
+*/
+
+#include "btConvex2dConvex2dAlgorithm.h"
+
+//#include <stdio.h>
+#include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+#include "BulletCollision/CollisionShapes/btConvexShape.h"
+#include "BulletCollision/CollisionShapes/btCapsuleShape.h"
+
+
+#include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
+#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
+#include "BulletCollision/CollisionShapes/btBoxShape.h"
+#include "BulletCollision/CollisionDispatch/btManifoldResult.h"
+
+#include "BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h"
+#include "BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h"
+#include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h"
+#include "BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h"
+
+
+
+#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
+#include "BulletCollision/CollisionShapes/btSphereShape.h"
+
+#include "BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h"
+
+#include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h"
+#include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h"
+
+
+btConvex2dConvex2dAlgorithm::CreateFunc::CreateFunc(btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver)
+{
+ m_numPerturbationIterations = 0;
+ m_minimumPointsPerturbationThreshold = 3;
+ m_simplexSolver = simplexSolver;
+ m_pdSolver = pdSolver;
+}
+
+btConvex2dConvex2dAlgorithm::CreateFunc::~CreateFunc()
+{
+}
+
+btConvex2dConvex2dAlgorithm::btConvex2dConvex2dAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1,btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver,int numPerturbationIterations, int minimumPointsPerturbationThreshold)
+: btActivatingCollisionAlgorithm(ci,body0,body1),
+m_simplexSolver(simplexSolver),
+m_pdSolver(pdSolver),
+m_ownManifold (false),
+m_manifoldPtr(mf),
+m_lowLevelOfDetail(false),
+ m_numPerturbationIterations(numPerturbationIterations),
+m_minimumPointsPerturbationThreshold(minimumPointsPerturbationThreshold)
+{
+ (void)body0;
+ (void)body1;
+}
+
+
+
+
+btConvex2dConvex2dAlgorithm::~btConvex2dConvex2dAlgorithm()
+{
+ if (m_ownManifold)
+ {
+ if (m_manifoldPtr)
+ m_dispatcher->releaseManifold(m_manifoldPtr);
+ }
+}
+
+void btConvex2dConvex2dAlgorithm ::setLowLevelOfDetail(bool useLowLevel)
+{
+ m_lowLevelOfDetail = useLowLevel;
+}
+
+
+
+extern btScalar gContactBreakingThreshold;
+
+
+//
+// Convex-Convex collision algorithm
+//
+void btConvex2dConvex2dAlgorithm ::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+{
+
+ if (!m_manifoldPtr)
+ {
+ //swapped?
+ m_manifoldPtr = m_dispatcher->getNewManifold(body0,body1);
+ m_ownManifold = true;
+ }
+ resultOut->setPersistentManifold(m_manifoldPtr);
+
+ //comment-out next line to test multi-contact generation
+ //resultOut->getPersistentManifold()->clearManifold();
+
+
+ btConvexShape* min0 = static_cast<btConvexShape*>(body0->getCollisionShape());
+ btConvexShape* min1 = static_cast<btConvexShape*>(body1->getCollisionShape());
+
+ btVector3 normalOnB;
+ btVector3 pointOnBWorld;
+
+ {
+
+
+ btGjkPairDetector::ClosestPointInput input;
+
+ btGjkPairDetector gjkPairDetector(min0,min1,m_simplexSolver,m_pdSolver);
+ //TODO: if (dispatchInfo.m_useContinuous)
+ gjkPairDetector.setMinkowskiA(min0);
+ gjkPairDetector.setMinkowskiB(min1);
+
+ {
+ input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactBreakingThreshold();
+ input.m_maximumDistanceSquared*= input.m_maximumDistanceSquared;
+ }
+
+ input.m_stackAlloc = dispatchInfo.m_stackAllocator;
+ input.m_transformA = body0->getWorldTransform();
+ input.m_transformB = body1->getWorldTransform();
+
+ gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw);
+
+ btVector3 v0,v1;
+ btVector3 sepNormalWorldSpace;
+
+ }
+
+ if (m_ownManifold)
+ {
+ resultOut->refreshContactPoints();
+ }
+
+}
+
+
+
+
+btScalar btConvex2dConvex2dAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+{
+ (void)resultOut;
+ (void)dispatchInfo;
+ ///Rather then checking ALL pairs, only calculate TOI when motion exceeds threshold
+
+ ///Linear motion for one of objects needs to exceed m_ccdSquareMotionThreshold
+ ///col0->m_worldTransform,
+ btScalar resultFraction = btScalar(1.);
+
+
+ btScalar squareMot0 = (col0->getInterpolationWorldTransform().getOrigin() - col0->getWorldTransform().getOrigin()).length2();
+ btScalar squareMot1 = (col1->getInterpolationWorldTransform().getOrigin() - col1->getWorldTransform().getOrigin()).length2();
+
+ if (squareMot0 < col0->getCcdSquareMotionThreshold() &&
+ squareMot1 < col1->getCcdSquareMotionThreshold())
+ return resultFraction;
+
+
+ //An adhoc way of testing the Continuous Collision Detection algorithms
+ //One object is approximated as a sphere, to simplify things
+ //Starting in penetration should report no time of impact
+ //For proper CCD, better accuracy and handling of 'allowed' penetration should be added
+ //also the mainloop of the physics should have a kind of toi queue (something like Brian Mirtich's application of Timewarp for Rigidbodies)
+
+
+ /// Convex0 against sphere for Convex1
+ {
+ btConvexShape* convex0 = static_cast<btConvexShape*>(col0->getCollisionShape());
+
+ btSphereShape sphere1(col1->getCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation
+ btConvexCast::CastResult result;
+ btVoronoiSimplexSolver voronoiSimplex;
+ //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
+ ///Simplification, one object is simplified as a sphere
+ btGjkConvexCast ccd1( convex0 ,&sphere1,&voronoiSimplex);
+ //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
+ if (ccd1.calcTimeOfImpact(col0->getWorldTransform(),col0->getInterpolationWorldTransform(),
+ col1->getWorldTransform(),col1->getInterpolationWorldTransform(),result))
+ {
+
+ //store result.m_fraction in both bodies
+
+ if (col0->getHitFraction()> result.m_fraction)
+ col0->setHitFraction( result.m_fraction );
+
+ if (col1->getHitFraction() > result.m_fraction)
+ col1->setHitFraction( result.m_fraction);
+
+ if (resultFraction > result.m_fraction)
+ resultFraction = result.m_fraction;
+
+ }
+
+
+
+
+ }
+
+ /// Sphere (for convex0) against Convex1
+ {
+ btConvexShape* convex1 = static_cast<btConvexShape*>(col1->getCollisionShape());
+
+ btSphereShape sphere0(col0->getCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation
+ btConvexCast::CastResult result;
+ btVoronoiSimplexSolver voronoiSimplex;
+ //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
+ ///Simplification, one object is simplified as a sphere
+ btGjkConvexCast ccd1(&sphere0,convex1,&voronoiSimplex);
+ //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
+ if (ccd1.calcTimeOfImpact(col0->getWorldTransform(),col0->getInterpolationWorldTransform(),
+ col1->getWorldTransform(),col1->getInterpolationWorldTransform(),result))
+ {
+
+ //store result.m_fraction in both bodies
+
+ if (col0->getHitFraction() > result.m_fraction)
+ col0->setHitFraction( result.m_fraction);
+
+ if (col1->getHitFraction() > result.m_fraction)
+ col1->setHitFraction( result.m_fraction);
+
+ if (resultFraction > result.m_fraction)
+ resultFraction = result.m_fraction;
+
+ }
+ }
+
+ return resultFraction;
+
+}
+
diff --git a/src/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.h b/src/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.h new file mode 100644 index 000000000..f3916bcc5 --- /dev/null +++ b/src/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.h @@ -0,0 +1,95 @@ +/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+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.
+*/
+
+#ifndef CONVEX_2D_CONVEX_2D_ALGORITHM_H
+#define CONVEX_2D_CONVEX_2D_ALGORITHM_H
+
+#include "BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.h"
+#include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h"
+#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
+#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
+#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h"
+#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
+#include "LinearMath/btTransformUtil.h" //for btConvexSeparatingDistanceUtil
+
+class btConvexPenetrationDepthSolver;
+
+
+///The convex2dConvex2dAlgorithm collision algorithm support 2d collision detection for btConvex2dShape
+///Currently it requires the btMinkowskiPenetrationDepthSolver, it has support for 2d penetration depth computation
+class btConvex2dConvex2dAlgorithm : public btActivatingCollisionAlgorithm
+{
+ btSimplexSolverInterface* m_simplexSolver;
+ btConvexPenetrationDepthSolver* m_pdSolver;
+
+
+ bool m_ownManifold;
+ btPersistentManifold* m_manifoldPtr;
+ bool m_lowLevelOfDetail;
+
+ int m_numPerturbationIterations;
+ int m_minimumPointsPerturbationThreshold;
+
+public:
+
+ btConvex2dConvex2dAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1, btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver, int numPerturbationIterations, int minimumPointsPerturbationThreshold);
+
+
+ virtual ~btConvex2dConvex2dAlgorithm();
+
+ virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+ virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+ virtual void getAllContactManifolds(btManifoldArray& manifoldArray)
+ {
+ ///should we use m_ownManifold to avoid adding duplicates?
+ if (m_manifoldPtr && m_ownManifold)
+ manifoldArray.push_back(m_manifoldPtr);
+ }
+
+
+ void setLowLevelOfDetail(bool useLowLevel);
+
+
+ const btPersistentManifold* getManifold()
+ {
+ return m_manifoldPtr;
+ }
+
+ struct CreateFunc :public btCollisionAlgorithmCreateFunc
+ {
+
+ btConvexPenetrationDepthSolver* m_pdSolver;
+ btSimplexSolverInterface* m_simplexSolver;
+ int m_numPerturbationIterations;
+ int m_minimumPointsPerturbationThreshold;
+
+ CreateFunc(btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver);
+
+ virtual ~CreateFunc();
+
+ virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
+ {
+ void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btConvex2dConvex2dAlgorithm));
+ return new(mem) btConvex2dConvex2dAlgorithm(ci.m_manifold,ci,body0,body1,m_simplexSolver,m_pdSolver,m_numPerturbationIterations,m_minimumPointsPerturbationThreshold);
+ }
+ };
+
+
+};
+
+#endif //CONVEX_2D_CONVEX_2D_ALGORITHM_H
|