Merge pull request #24740 from OBKF/update-bullet-physics

Update Bullet physics to commit 126b676

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;
 }
-