summaryrefslogtreecommitdiff
path: root/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp')
-rw-r--r--thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp125
1 files changed, 60 insertions, 65 deletions
diff --git a/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp b/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp
index 3481fec850..38df8d4808 100644
--- a/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp
+++ b/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp
@@ -13,7 +13,6 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
-
#include "btContinuousConvexCollision.h"
#include "BulletCollision/CollisionShapes/btConvexShape.h"
#include "BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h"
@@ -24,59 +23,60 @@ subject to the following restrictions:
#include "btPointCollector.h"
#include "BulletCollision/CollisionShapes/btStaticPlaneShape.h"
-
-
-btContinuousConvexCollision::btContinuousConvexCollision ( const btConvexShape* convexA,const btConvexShape* convexB,btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* penetrationDepthSolver)
-:m_simplexSolver(simplexSolver),
-m_penetrationDepthSolver(penetrationDepthSolver),
-m_convexA(convexA),m_convexB1(convexB),m_planeShape(0)
+btContinuousConvexCollision::btContinuousConvexCollision(const btConvexShape* convexA, const btConvexShape* convexB, btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* penetrationDepthSolver)
+ : m_simplexSolver(simplexSolver),
+ m_penetrationDepthSolver(penetrationDepthSolver),
+ m_convexA(convexA),
+ m_convexB1(convexB),
+ m_planeShape(0)
{
}
-
-btContinuousConvexCollision::btContinuousConvexCollision( const btConvexShape* convexA,const btStaticPlaneShape* plane)
-:m_simplexSolver(0),
-m_penetrationDepthSolver(0),
-m_convexA(convexA),m_convexB1(0),m_planeShape(plane)
+btContinuousConvexCollision::btContinuousConvexCollision(const btConvexShape* convexA, const btStaticPlaneShape* plane)
+ : m_simplexSolver(0),
+ m_penetrationDepthSolver(0),
+ m_convexA(convexA),
+ m_convexB1(0),
+ m_planeShape(plane)
{
}
-
/// This maximum should not be necessary. It allows for untested/degenerate cases in production code.
/// You don't want your game ever to lock-up.
#define MAX_ITERATIONS 64
-void btContinuousConvexCollision::computeClosestPoints( const btTransform& transA, const btTransform& transB,btPointCollector& pointCollector)
+void btContinuousConvexCollision::computeClosestPoints(const btTransform& transA, const btTransform& transB, btPointCollector& pointCollector)
{
if (m_convexB1)
{
m_simplexSolver->reset();
- btGjkPairDetector gjk(m_convexA,m_convexB1,m_convexA->getShapeType(),m_convexB1->getShapeType(),m_convexA->getMargin(),m_convexB1->getMargin(),m_simplexSolver,m_penetrationDepthSolver);
+ btGjkPairDetector gjk(m_convexA, m_convexB1, m_convexA->getShapeType(), m_convexB1->getShapeType(), m_convexA->getMargin(), m_convexB1->getMargin(), m_simplexSolver, m_penetrationDepthSolver);
btGjkPairDetector::ClosestPointInput input;
input.m_transformA = transA;
input.m_transformB = transB;
- gjk.getClosestPoints(input,pointCollector,0);
- } else
+ gjk.getClosestPoints(input, pointCollector, 0);
+ }
+ else
{
//convex versus plane
const btConvexShape* convexShape = m_convexA;
const btStaticPlaneShape* planeShape = m_planeShape;
-
+
const btVector3& planeNormal = planeShape->getPlaneNormal();
const btScalar& planeConstant = planeShape->getPlaneConstant();
-
+
btTransform convexWorldTransform = transA;
btTransform convexInPlaneTrans;
- convexInPlaneTrans= transB.inverse() * convexWorldTransform;
+ convexInPlaneTrans = transB.inverse() * convexWorldTransform;
btTransform planeInConvex;
- planeInConvex= convexWorldTransform.inverse() * transB;
-
- btVector3 vtx = convexShape->localGetSupportingVertex(planeInConvex.getBasis()*-planeNormal);
+ planeInConvex = convexWorldTransform.inverse() * transB;
+
+ 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 = transB * vtxInPlaneProjected;
btVector3 normalOnSurfaceB = transB.getBasis() * planeNormal;
@@ -87,36 +87,33 @@ void btContinuousConvexCollision::computeClosestPoints( const btTransform& trans
}
}
-bool btContinuousConvexCollision::calcTimeOfImpact(
- const btTransform& fromA,
- const btTransform& toA,
- const btTransform& fromB,
- const btTransform& toB,
- CastResult& result)
+bool btContinuousConvexCollision::calcTimeOfImpact(
+ const btTransform& fromA,
+ const btTransform& toA,
+ const btTransform& fromB,
+ const btTransform& toB,
+ CastResult& result)
{
-
-
/// compute linear and angular velocity for this interval, to interpolate
- btVector3 linVelA,angVelA,linVelB,angVelB;
- btTransformUtil::calculateVelocity(fromA,toA,btScalar(1.),linVelA,angVelA);
- btTransformUtil::calculateVelocity(fromB,toB,btScalar(1.),linVelB,angVelB);
-
+ btVector3 linVelA, angVelA, linVelB, angVelB;
+ btTransformUtil::calculateVelocity(fromA, toA, btScalar(1.), linVelA, angVelA);
+ btTransformUtil::calculateVelocity(fromB, toB, btScalar(1.), linVelB, angVelB);
btScalar boundingRadiusA = m_convexA->getAngularMotionDisc();
- btScalar boundingRadiusB = m_convexB1?m_convexB1->getAngularMotionDisc():0.f;
+ btScalar boundingRadiusB = m_convexB1 ? m_convexB1->getAngularMotionDisc() : 0.f;
btScalar maxAngularProjectedVelocity = angVelA.length() * boundingRadiusA + angVelB.length() * boundingRadiusB;
- btVector3 relLinVel = (linVelB-linVelA);
+ btVector3 relLinVel = (linVelB - linVelA);
+
+ btScalar relLinVelocLength = (linVelB - linVelA).length();
- btScalar relLinVelocLength = (linVelB-linVelA).length();
-
- if ((relLinVelocLength+maxAngularProjectedVelocity) == 0.f)
+ if ((relLinVelocLength + maxAngularProjectedVelocity) == 0.f)
return false;
btScalar lambda = btScalar(0.);
btVector3 n;
- n.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
+ n.setValue(btScalar(0.), btScalar(0.), btScalar(0.));
bool hasResult = false;
btVector3 c;
@@ -126,14 +123,13 @@ bool btContinuousConvexCollision::calcTimeOfImpact(
int numIter = 0;
//first solution, using GJK
-
btScalar radius = 0.001f;
-// result.drawCoordSystem(sphereTr);
+ // result.drawCoordSystem(sphereTr);
- btPointCollector pointCollector1;
+ btPointCollector pointCollector1;
- {
- computeClosestPoints(fromA,fromB,pointCollector1);
+ {
+ computeClosestPoints(fromA, fromB, pointCollector1);
hasResult = pointCollector1.m_hasResult;
c = pointCollector1.m_pointInWorld;
@@ -145,7 +141,7 @@ bool btContinuousConvexCollision::calcTimeOfImpact(
dist = pointCollector1.m_distance + result.m_allowedPenetration;
n = pointCollector1.m_normalOnBInWorld;
btScalar projectedLinearVelocity = relLinVel.dot(n);
- if ((projectedLinearVelocity+ maxAngularProjectedVelocity)<=SIMD_EPSILON)
+ if ((projectedLinearVelocity + maxAngularProjectedVelocity) <= SIMD_EPSILON)
return false;
//not close enough
@@ -153,18 +149,17 @@ bool btContinuousConvexCollision::calcTimeOfImpact(
{
if (result.m_debugDrawer)
{
- result.m_debugDrawer->drawSphere(c,0.2f,btVector3(1,1,1));
+ result.m_debugDrawer->drawSphere(c, 0.2f, btVector3(1, 1, 1));
}
btScalar dLambda = btScalar(0.);
projectedLinearVelocity = relLinVel.dot(n);
-
//don't report time of impact for motion away from the contact normal (or causes minor penetration)
- if ((projectedLinearVelocity+ maxAngularProjectedVelocity)<=SIMD_EPSILON)
+ if ((projectedLinearVelocity + maxAngularProjectedVelocity) <= SIMD_EPSILON)
return false;
-
- dLambda = dist / (projectedLinearVelocity+ maxAngularProjectedVelocity);
+
+ dLambda = dist / (projectedLinearVelocity + maxAngularProjectedVelocity);
lambda += dLambda;
@@ -181,28 +176,29 @@ bool btContinuousConvexCollision::calcTimeOfImpact(
lastLambda = lambda;
//interpolate to next lambda
- btTransform interpolatedTransA,interpolatedTransB,relativeTrans;
+ btTransform interpolatedTransA, interpolatedTransB, relativeTrans;
- btTransformUtil::integrateTransform(fromA,linVelA,angVelA,lambda,interpolatedTransA);
- btTransformUtil::integrateTransform(fromB,linVelB,angVelB,lambda,interpolatedTransB);
+ btTransformUtil::integrateTransform(fromA, linVelA, angVelA, lambda, interpolatedTransA);
+ btTransformUtil::integrateTransform(fromB, linVelB, angVelB, lambda, interpolatedTransB);
relativeTrans = interpolatedTransB.inverseTimes(interpolatedTransA);
if (result.m_debugDrawer)
{
- result.m_debugDrawer->drawSphere(interpolatedTransA.getOrigin(),0.2f,btVector3(1,0,0));
+ result.m_debugDrawer->drawSphere(interpolatedTransA.getOrigin(), 0.2f, btVector3(1, 0, 0));
}
- result.DebugDraw( lambda );
+ result.DebugDraw(lambda);
- btPointCollector pointCollector;
- computeClosestPoints(interpolatedTransA,interpolatedTransB,pointCollector);
+ btPointCollector pointCollector;
+ computeClosestPoints(interpolatedTransA, interpolatedTransB, pointCollector);
if (pointCollector.m_hasResult)
{
- dist = pointCollector.m_distance+result.m_allowedPenetration;
- c = pointCollector.m_pointInWorld;
+ dist = pointCollector.m_distance + result.m_allowedPenetration;
+ c = pointCollector.m_pointInWorld;
n = pointCollector.m_normalOnBInWorld;
- } else
+ }
+ else
{
result.reportFailure(-1, numIter);
return false;
@@ -215,7 +211,7 @@ bool btContinuousConvexCollision::calcTimeOfImpact(
return false;
}
}
-
+
result.m_fraction = lambda;
result.m_normal = n;
result.m_hitPoint = c;
@@ -224,4 +220,3 @@ bool btContinuousConvexCollision::calcTimeOfImpact(
return false;
}
-
generated by cgit v1.2.3 (git 2.25.1) at 2024-12-16 21:37:33 +0000