summaryrefslogtreecommitdiff
path: root/thirdparty/bullet/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'thirdparty/bullet/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp')
-rw-r--r--thirdparty/bullet/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp96
1 files changed, 47 insertions, 49 deletions
diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp b/thirdparty/bullet/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp
index cce2d95bcf..ba1bc06b69 100644
--- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp
+++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp
@@ -23,25 +23,24 @@ subject to the following restrictions:
//#include <stdio.h>
-btConvexPlaneCollisionAlgorithm::btConvexPlaneCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* col0Wrap,const btCollisionObjectWrapper* col1Wrap, bool isSwapped, int numPerturbationIterations,int minimumPointsPerturbationThreshold)
-: btCollisionAlgorithm(ci),
-m_ownManifold(false),
-m_manifoldPtr(mf),
-m_isSwapped(isSwapped),
-m_numPerturbationIterations(numPerturbationIterations),
-m_minimumPointsPerturbationThreshold(minimumPointsPerturbationThreshold)
+btConvexPlaneCollisionAlgorithm::btConvexPlaneCollisionAlgorithm(btPersistentManifold* mf, const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* col0Wrap, const btCollisionObjectWrapper* col1Wrap, bool isSwapped, int numPerturbationIterations, int minimumPointsPerturbationThreshold)
+ : btCollisionAlgorithm(ci),
+ m_ownManifold(false),
+ m_manifoldPtr(mf),
+ m_isSwapped(isSwapped),
+ m_numPerturbationIterations(numPerturbationIterations),
+ m_minimumPointsPerturbationThreshold(minimumPointsPerturbationThreshold)
{
- const btCollisionObjectWrapper* convexObjWrap = m_isSwapped? col1Wrap : col0Wrap;
- const btCollisionObjectWrapper* planeObjWrap = m_isSwapped? col0Wrap : col1Wrap;
+ const btCollisionObjectWrapper* convexObjWrap = m_isSwapped ? col1Wrap : col0Wrap;
+ const btCollisionObjectWrapper* planeObjWrap = m_isSwapped ? col0Wrap : col1Wrap;
- if (!m_manifoldPtr && m_dispatcher->needsCollision(convexObjWrap->getCollisionObject(),planeObjWrap->getCollisionObject()))
+ if (!m_manifoldPtr && m_dispatcher->needsCollision(convexObjWrap->getCollisionObject(), planeObjWrap->getCollisionObject()))
{
- m_manifoldPtr = m_dispatcher->getNewManifold(convexObjWrap->getCollisionObject(),planeObjWrap->getCollisionObject());
+ m_manifoldPtr = m_dispatcher->getNewManifold(convexObjWrap->getCollisionObject(), planeObjWrap->getCollisionObject());
m_ownManifold = true;
}
}
-
btConvexPlaneCollisionAlgorithm::~btConvexPlaneCollisionAlgorithm()
{
if (m_ownManifold)
@@ -51,32 +50,32 @@ btConvexPlaneCollisionAlgorithm::~btConvexPlaneCollisionAlgorithm()
}
}
-void btConvexPlaneCollisionAlgorithm::collideSingleContact (const btQuaternion& perturbeRot, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+void btConvexPlaneCollisionAlgorithm::collideSingleContact(const btQuaternion& perturbeRot, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut)
{
- const btCollisionObjectWrapper* convexObjWrap = m_isSwapped? body1Wrap : body0Wrap;
- const btCollisionObjectWrapper* planeObjWrap = m_isSwapped? body0Wrap: body1Wrap;
+ const btCollisionObjectWrapper* convexObjWrap = m_isSwapped ? body1Wrap : body0Wrap;
+ const btCollisionObjectWrapper* planeObjWrap = m_isSwapped ? body0Wrap : body1Wrap;
- btConvexShape* convexShape = (btConvexShape*) convexObjWrap->getCollisionShape();
- btStaticPlaneShape* planeShape = (btStaticPlaneShape*) planeObjWrap->getCollisionShape();
+ btConvexShape* convexShape = (btConvexShape*)convexObjWrap->getCollisionShape();
+ btStaticPlaneShape* planeShape = (btStaticPlaneShape*)planeObjWrap->getCollisionShape();
- bool hasCollision = false;
+ bool hasCollision = false;
const btVector3& planeNormal = planeShape->getPlaneNormal();
const btScalar& planeConstant = planeShape->getPlaneConstant();
-
+
btTransform convexWorldTransform = convexObjWrap->getWorldTransform();
btTransform convexInPlaneTrans;
- convexInPlaneTrans= planeObjWrap->getWorldTransform().inverse() * convexWorldTransform;
+ convexInPlaneTrans = planeObjWrap->getWorldTransform().inverse() * convexWorldTransform;
//now perturbe the convex-world transform
- convexWorldTransform.getBasis()*=btMatrix3x3(perturbeRot);
+ convexWorldTransform.getBasis() *= btMatrix3x3(perturbeRot);
btTransform planeInConvex;
- planeInConvex= convexWorldTransform.inverse() * planeObjWrap->getWorldTransform();
-
- btVector3 vtx = convexShape->localGetSupportingVertex(planeInConvex.getBasis()*-planeNormal);
+ planeInConvex = convexWorldTransform.inverse() * planeObjWrap->getWorldTransform();
+
+ btVector3 vtx = convexShape->localGetSupportingVertex(planeInConvex.getBasis() * -planeNormal);
btVector3 vtxInPlane = convexInPlaneTrans(vtx);
btScalar distance = (planeNormal.dot(vtxInPlane) - planeConstant);
- btVector3 vtxInPlaneProjected = vtxInPlane - distance*planeNormal;
+ btVector3 vtxInPlaneProjected = vtxInPlane - distance * planeNormal;
btVector3 vtxInPlaneWorld = planeObjWrap->getWorldTransform() * vtxInPlaneProjected;
hasCollision = distance < m_manifoldPtr->getContactBreakingThreshold();
@@ -86,70 +85,69 @@ void btConvexPlaneCollisionAlgorithm::collideSingleContact (const btQuaternion&
/// report a contact. internally this will be kept persistent, and contact reduction is done
btVector3 normalOnSurfaceB = planeObjWrap->getWorldTransform().getBasis() * planeNormal;
btVector3 pOnB = vtxInPlaneWorld;
- resultOut->addContactPoint(normalOnSurfaceB,pOnB,distance);
+ resultOut->addContactPoint(normalOnSurfaceB, pOnB, distance);
}
}
-
-void btConvexPlaneCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+void btConvexPlaneCollisionAlgorithm::processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut)
{
(void)dispatchInfo;
if (!m_manifoldPtr)
return;
- const btCollisionObjectWrapper* convexObjWrap = m_isSwapped? body1Wrap : body0Wrap;
- const btCollisionObjectWrapper* planeObjWrap = m_isSwapped? body0Wrap: body1Wrap;
+ const btCollisionObjectWrapper* convexObjWrap = m_isSwapped ? body1Wrap : body0Wrap;
+ const btCollisionObjectWrapper* planeObjWrap = m_isSwapped ? body0Wrap : body1Wrap;
- btConvexShape* convexShape = (btConvexShape*) convexObjWrap->getCollisionShape();
- btStaticPlaneShape* planeShape = (btStaticPlaneShape*) planeObjWrap->getCollisionShape();
+ btConvexShape* convexShape = (btConvexShape*)convexObjWrap->getCollisionShape();
+ btStaticPlaneShape* planeShape = (btStaticPlaneShape*)planeObjWrap->getCollisionShape();
bool hasCollision = false;
const btVector3& planeNormal = planeShape->getPlaneNormal();
const btScalar& planeConstant = planeShape->getPlaneConstant();
btTransform planeInConvex;
- planeInConvex= convexObjWrap->getWorldTransform().inverse() * planeObjWrap->getWorldTransform();
+ planeInConvex = convexObjWrap->getWorldTransform().inverse() * planeObjWrap->getWorldTransform();
btTransform convexInPlaneTrans;
- convexInPlaneTrans= planeObjWrap->getWorldTransform().inverse() * convexObjWrap->getWorldTransform();
+ convexInPlaneTrans = planeObjWrap->getWorldTransform().inverse() * convexObjWrap->getWorldTransform();
- btVector3 vtx = convexShape->localGetSupportingVertex(planeInConvex.getBasis()*-planeNormal);
+ btVector3 vtx = convexShape->localGetSupportingVertex(planeInConvex.getBasis() * -planeNormal);
btVector3 vtxInPlane = convexInPlaneTrans(vtx);
btScalar distance = (planeNormal.dot(vtxInPlane) - planeConstant);
- btVector3 vtxInPlaneProjected = vtxInPlane - distance*planeNormal;
+ btVector3 vtxInPlaneProjected = vtxInPlane - distance * planeNormal;
btVector3 vtxInPlaneWorld = planeObjWrap->getWorldTransform() * vtxInPlaneProjected;
- hasCollision = distance < m_manifoldPtr->getContactBreakingThreshold();
+ hasCollision = distance < m_manifoldPtr->getContactBreakingThreshold()+ resultOut->m_closestPointDistanceThreshold;
resultOut->setPersistentManifold(m_manifoldPtr);
if (hasCollision)
{
/// report a contact. internally this will be kept persistent, and contact reduction is done
btVector3 normalOnSurfaceB = planeObjWrap->getWorldTransform().getBasis() * planeNormal;
btVector3 pOnB = vtxInPlaneWorld;
- resultOut->addContactPoint(normalOnSurfaceB,pOnB,distance);
+ resultOut->addContactPoint(normalOnSurfaceB, pOnB, distance);
}
//the perturbation algorithm doesn't work well with implicit surfaces such as spheres, cylinder and cones:
//they keep on rolling forever because of the additional off-center contact points
//so only enable the feature for polyhedral shapes (btBoxShape, btConvexHullShape etc)
- if (convexShape->isPolyhedral() && resultOut->getPersistentManifold()->getNumContacts()<m_minimumPointsPerturbationThreshold)
+ if (convexShape->isPolyhedral() && resultOut->getPersistentManifold()->getNumContacts() < m_minimumPointsPerturbationThreshold)
{
- btVector3 v0,v1;
- btPlaneSpace1(planeNormal,v0,v1);
+ btVector3 v0, v1;
+ btPlaneSpace1(planeNormal, v0, v1);
//now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects
const btScalar angleLimit = 0.125f * SIMD_PI;
btScalar perturbeAngle;
btScalar radius = convexShape->getAngularMotionDisc();
perturbeAngle = gContactBreakingThreshold / radius;
- if ( perturbeAngle > angleLimit )
- perturbeAngle = angleLimit;
+ if (perturbeAngle > angleLimit)
+ perturbeAngle = angleLimit;
- btQuaternion perturbeRot(v0,perturbeAngle);
- for (int i=0;i<m_numPerturbationIterations;i++)
+ btQuaternion perturbeRot(v0, perturbeAngle);
+ for (int i = 0; i < m_numPerturbationIterations; i++)
{
- btScalar iterationAngle = i*(SIMD_2_PI/btScalar(m_numPerturbationIterations));
- btQuaternion rotq(planeNormal,iterationAngle);
- collideSingleContact(rotq.inverse()*perturbeRot*rotq,body0Wrap,body1Wrap,dispatchInfo,resultOut);
+ btScalar iterationAngle = i * (SIMD_2_PI / btScalar(m_numPerturbationIterations));
+ btQuaternion rotq(planeNormal, iterationAngle);
+ collideSingleContact(rotq.inverse() * perturbeRot * rotq, body0Wrap, body1Wrap, dispatchInfo, resultOut);
}
}
@@ -162,7 +160,7 @@ void btConvexPlaneCollisionAlgorithm::processCollision (const btCollisionObjectW
}
}
-btScalar btConvexPlaneCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+btScalar btConvexPlaneCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0, btCollisionObject* col1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut)
{
(void)resultOut;
(void)dispatchInfo;
generated by cgit v1.2.3 (git 2.25.1) at 2024-12-16 09:59:53 +0000