diff options
Diffstat (limited to 'extern/bullet2/src/BulletCollision/NarrowPhaseCollision')
21 files changed, 297 insertions, 247 deletions
diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp index ae3ce42e77f..2c565734e97 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp @@ -49,28 +49,28 @@ bool btContinuousConvexCollision::calcTimeOfImpact( /// compute linear and angular velocity for this interval, to interpolate btVector3 linVelA,angVelA,linVelB,angVelB; - btTransformUtil::calculateVelocity(fromA,toA,1.f,linVelA,angVelA); - btTransformUtil::calculateVelocity(fromB,toB,1.f,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_convexB->getAngularMotionDisc(); btScalar maxAngularProjectedVelocity = angVelA.length() * boundingRadiusA + angVelB.length() * boundingRadiusB; - float radius = 0.001f; + btScalar radius = btScalar(0.001); - btScalar lambda = 0.f; + btScalar lambda = btScalar(0.); btVector3 v(1,0,0); int maxIter = MAX_ITERATIONS; btVector3 n; - n.setValue(0.f,0.f,0.f); + n.setValue(btScalar(0.),btScalar(0.),btScalar(0.)); bool hasResult = false; btVector3 c; - float lastLambda = lambda; - //float epsilon = 0.001f; + btScalar lastLambda = lambda; + //btScalar epsilon = btScalar(0.001); int numIter = 0; //first solution, using GJK @@ -79,8 +79,8 @@ bool btContinuousConvexCollision::calcTimeOfImpact( btTransform identityTrans; identityTrans.setIdentity(); - btSphereShape raySphere(0.0f); - raySphere.setMargin(0.f); + btSphereShape raySphere(btScalar(0.0)); + raySphere.setMargin(btScalar(0.)); // result.drawCoordSystem(sphereTr); @@ -116,23 +116,23 @@ bool btContinuousConvexCollision::calcTimeOfImpact( if (numIter > maxIter) return false; //todo: report a failure - float dLambda = 0.f; + btScalar dLambda = btScalar(0.); //calculate safe moving fraction from distance / (linear+rotational velocity) - //float clippedDist = GEN_min(angularConservativeRadius,dist); - //float clippedDist = dist; + //btScalar clippedDist = GEN_min(angularConservativeRadius,dist); + //btScalar clippedDist = dist; - float projectedLinearVelocity = (linVelB-linVelA).dot(n); + btScalar projectedLinearVelocity = (linVelB-linVelA).dot(n); dLambda = dist / (projectedLinearVelocity+ maxAngularProjectedVelocity); lambda = lambda + dLambda; - if (lambda > 1.f) + if (lambda > btScalar(1.)) return false; - if (lambda < 0.f) + if (lambda < btScalar(0.)) return false; //todo: next check with relative epsilon @@ -159,7 +159,7 @@ bool btContinuousConvexCollision::calcTimeOfImpact( gjk.getClosestPoints(input,pointCollector,0); if (pointCollector.m_hasResult) { - if (pointCollector.m_distance < 0.f) + if (pointCollector.m_distance < btScalar(0.)) { //degenerate ?! result.m_fraction = lastLambda; @@ -188,9 +188,9 @@ bool btContinuousConvexCollision::calcTimeOfImpact( //todo: //if movement away from normal, discard result btVector3 move = transBLocalTo.getOrigin() - transBLocalFrom.getOrigin(); - if (result.m_fraction < 1.f) + if (result.m_fraction < btScalar(1.)) { - if (move.dot(result.m_normal) <= 0.f) + if (move.dot(result.m_normal) <= btScalar(0.)) { } } diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexCast.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexCast.h index 4258d829cca..3101b59993d 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexCast.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexCast.h @@ -17,11 +17,11 @@ subject to the following restrictions: #ifndef CONVEX_CAST_H #define CONVEX_CAST_H -#include <LinearMath/btTransform.h> -#include <LinearMath/btVector3.h> -#include <LinearMath/btScalar.h> +#include "../../LinearMath/btTransform.h" +#include "../../LinearMath/btVector3.h" +#include "../../LinearMath/btScalar.h" class btMinkowskiSumShape; -#include "LinearMath/btIDebugDraw.h" +#include "../../LinearMath/btIDebugDraw.h" /// btConvexCast is an interface for Casting class btConvexCast @@ -37,11 +37,11 @@ public: { //virtual bool addRayResult(const btVector3& normal,btScalar fraction) = 0; - virtual void DebugDraw(btScalar fraction) {} - virtual void drawCoordSystem(const btTransform& trans) {} + virtual void DebugDraw(btScalar fraction) {(void)fraction;} + virtual void drawCoordSystem(const btTransform& trans) {(void)trans;} CastResult() - :m_fraction(1e30f), + :m_fraction(btScalar(1e30)), m_debugDrawer(0) { } diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h index 188c8258c8e..7caeba4be45 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h @@ -21,7 +21,7 @@ class btStackAlloc; class btVector3; #include "btSimplexSolverInterface.h" class btConvexShape; -#include "LinearMath/btPoint3.h" +#include "../../LinearMath/btPoint3.h" class btTransform; ///ConvexPenetrationDepthSolver provides an interface for penetration depth calculation. diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h index e9db97d1b13..15000c1ab61 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h @@ -16,8 +16,8 @@ subject to the following restrictions: #ifndef DISCRETE_COLLISION_DETECTOR1_INTERFACE_H #define DISCRETE_COLLISION_DETECTOR1_INTERFACE_H -#include "LinearMath/btTransform.h" -#include "LinearMath/btVector3.h" +#include "../../LinearMath/btTransform.h" +#include "../../LinearMath/btVector3.h" class btStackAlloc; /// This interface is made to be used by an iterative approach to do TimeOfImpact calculations @@ -30,19 +30,18 @@ struct btDiscreteCollisionDetectorInterface struct Result { - void operator delete(void* ptr) {}; virtual ~Result(){} ///setShapeIdentifiers provides experimental support for per-triangle material / custom material combiner virtual void setShapeIdentifiers(int partId0,int index0, int partId1,int index1)=0; - virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,float depth)=0; + virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth)=0; }; struct ClosestPointInput { ClosestPointInput() - :m_maximumDistanceSquared(1e30f), + :m_maximumDistanceSquared(btScalar(1e30)), m_stackAlloc(0) { } @@ -69,13 +68,13 @@ struct btStorageResult : public btDiscreteCollisionDetectorInterface::Result btVector3 m_closestPointInB; btScalar m_distance; //negative means penetration ! - btStorageResult() : m_distance(1e30f) + btStorageResult() : m_distance(btScalar(1e30)) { } virtual ~btStorageResult() {}; - virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,float depth) + virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth) { if (depth < m_distance) { diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.cpp index bf465b61857..93edffeafd6 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.cpp +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.cpp @@ -60,9 +60,9 @@ bool btGjkConvexCast::calcTimeOfImpact( - float radius = 0.01f; + btScalar radius = btScalar(0.01); - btScalar lambda = 0.f; + btScalar lambda = btScalar(0.); btVector3 s = rayFromLocalA.getOrigin(); btVector3 r = rayToLocalA.getOrigin()-rayFromLocalA.getOrigin(); btVector3 x = s; @@ -71,7 +71,7 @@ bool btGjkConvexCast::calcTimeOfImpact( bool hasResult = false; btVector3 c; - float lastLambda = lambda; + btScalar lastLambda = lambda; //first solution, using GJK @@ -81,8 +81,8 @@ bool btGjkConvexCast::calcTimeOfImpact( btTransform identityTrans; identityTrans.setIdentity(); - btSphereShape raySphere(0.0f); - raySphere.setMargin(0.f); + btSphereShape raySphere(btScalar(0.0)); + raySphere.setMargin(btScalar(0.)); btTransform sphereTr; sphereTr.setIdentity(); @@ -112,7 +112,7 @@ bool btGjkConvexCast::calcTimeOfImpact( if (dist < radius) { //penetration - lastLambda = 1.f; + lastLambda = btScalar(1.); } //not close enough @@ -143,7 +143,7 @@ bool btGjkConvexCast::calcTimeOfImpact( gjk.getClosestPoints(input,pointCollector,0); if (pointCollector.m_hasResult) { - if (pointCollector.m_distance < 0.f) + if (pointCollector.m_distance < btScalar(0.)) { //degeneracy, report a hit result.m_fraction = lastLambda; @@ -160,7 +160,7 @@ bool btGjkConvexCast::calcTimeOfImpact( } - if (lastLambda < 1.f) + if (lastLambda < btScalar(1.)) { result.m_fraction = lastLambda; diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h index 66b34b88363..3905c45e6d6 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h @@ -18,9 +18,9 @@ subject to the following restrictions: #ifndef GJK_CONVEX_CAST_H #define GJK_CONVEX_CAST_H -#include <BulletCollision/CollisionShapes/btCollisionMargin.h> +#include "../CollisionShapes/btCollisionMargin.h" -#include "LinearMath/btVector3.h" +#include "../../LinearMath/btVector3.h" #include "btConvexCast.h" class btConvexShape; class btMinkowskiSumShape; diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa.h index 359829f8f31..759b30bb17f 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa.h @@ -21,7 +21,7 @@ Nov.2006 #ifndef _05E48D53_04E0_49ad_BB0A_D74FE62E7366_ #define _05E48D53_04E0_49ad_BB0A_D74FE62E7366_ -#include "BulletCollision/CollisionShapes/btConvexShape.h" +#include "../CollisionShapes/btConvexShape.h" class btStackAlloc; diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp index 3d252751c9a..87330493b60 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp @@ -26,8 +26,11 @@ bool btGjkEpaPenetrationDepthSolver::calcPenDepth( btSimplexSolverInterface& sim class btIDebugDraw* debugDraw, btStackAlloc* stackAlloc ) { + (void)debugDraw; + (void)v; + (void)simplexSolver; - const btScalar radialmargin(0.f); + const btScalar radialmargin(btScalar(0.)); btGjkEpaSolver::sResults results; if(btGjkEpaSolver::Collide( pConvexA,transformA, diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp index 0ce04dcfad8..f1f3f7f7f6c 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp @@ -27,7 +27,7 @@ subject to the following restrictions: #endif //must be above the machine epsilon -#define REL_ERROR2 1.0e-6f +#define REL_ERROR2 btScalar(1.0e-6) //temp globals, to improve GJK/EPA/penetration calculations int gNumDeepPenetrationChecks = 0; @@ -36,7 +36,7 @@ int gNumGjkChecks = 0; btGjkPairDetector::btGjkPairDetector(btConvexShape* objectA,btConvexShape* objectB,btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver* penetrationDepthSolver) -:m_cachedSeparatingAxis(0.f,0.f,1.f), +:m_cachedSeparatingAxis(btScalar(0.),btScalar(0.),btScalar(1.)), m_penetrationDepthSolver(penetrationDepthSolver), m_simplexSolver(simplexSolver), m_minkowskiA(objectA), @@ -49,25 +49,25 @@ m_catchDegeneracies(1) void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw) { - btScalar distance=0.f; - btVector3 normalInB(0.f,0.f,0.f); + btScalar distance=btScalar(0.); + btVector3 normalInB(btScalar(0.),btScalar(0.),btScalar(0.)); btVector3 pointOnA,pointOnB; btTransform localTransA = input.m_transformA; btTransform localTransB = input.m_transformB; - btVector3 positionOffset = (localTransA.getOrigin() + localTransB.getOrigin()) * 0.5f; + btVector3 positionOffset = (localTransA.getOrigin() + localTransB.getOrigin()) * btScalar(0.5); localTransA.getOrigin() -= positionOffset; localTransB.getOrigin() -= positionOffset; - float marginA = m_minkowskiA->getMargin(); - float marginB = m_minkowskiB->getMargin(); + btScalar marginA = m_minkowskiA->getMargin(); + btScalar marginB = m_minkowskiB->getMargin(); gNumGjkChecks++; //for CCD we don't use margins if (m_ignoreMargin) { - marginA = 0.f; - marginB = 0.f; + marginA = btScalar(0.); + marginB = btScalar(0.); } m_curIter = 0; @@ -83,7 +83,7 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& { btScalar squaredDistance = SIMD_INFINITY; - btScalar delta = 0.f; + btScalar delta = btScalar(0.); btScalar margin = marginA + marginB; @@ -91,7 +91,8 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& m_simplexSolver->reset(); - while (true) + for ( ; ; ) + //while (true) { btVector3 seperatingAxisInA = (-m_cachedSeparatingAxis)* input.m_transformA.getBasis(); @@ -120,12 +121,12 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& break; } // are we getting any closer ? - float f0 = squaredDistance - delta; - float f1 = squaredDistance * REL_ERROR2; + btScalar f0 = squaredDistance - delta; + btScalar f1 = squaredDistance * REL_ERROR2; if (f0 <= f1) { - if (f0 <= 0.f) + if (f0 <= btScalar(0.)) { m_degenerateSimplex = 2; } @@ -191,7 +192,7 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& { m_simplexSolver->compute_points(pointOnA, pointOnB); normalInB = pointOnA-pointOnB; - float lenSqr = m_cachedSeparatingAxis.length2(); + btScalar lenSqr = m_cachedSeparatingAxis.length2(); //valid normal if (lenSqr < 0.0001) { @@ -199,14 +200,14 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& } if (lenSqr > SIMD_EPSILON*SIMD_EPSILON) { - float rlen = 1.f / btSqrt(lenSqr ); + btScalar rlen = btScalar(1.) / btSqrt(lenSqr ); normalInB *= rlen; //normalize btScalar s = btSqrt(squaredDistance); btAssert(s > btScalar(0.0)); pointOnA -= m_cachedSeparatingAxis * (marginA / s); pointOnB += m_cachedSeparatingAxis * (marginB / s); - distance = ((1.f/rlen) - margin); + distance = ((btScalar(1.)/rlen) - margin); isValid = true; m_lastUsedMethod = 1; @@ -243,11 +244,11 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& if (isValid2) { btVector3 tmpNormalInB = tmpPointOnB-tmpPointOnA; - float lenSqr = tmpNormalInB.length2(); + btScalar lenSqr = tmpNormalInB.length2(); if (lenSqr > (SIMD_EPSILON*SIMD_EPSILON)) { tmpNormalInB /= btSqrt(lenSqr); - float distance2 = -(tmpPointOnA-tmpPointOnB).length(); + btScalar distance2 = -(tmpPointOnA-tmpPointOnB).length(); //only replace valid penetrations when the result is deeper (check) if (!isValid || (distance2 < distance)) { diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h index 09c1669bd78..af0fe32f6c7 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h @@ -20,9 +20,8 @@ subject to the following restrictions: #define GJK_PAIR_DETECTOR_H #include "btDiscreteCollisionDetectorInterface.h" -#include "LinearMath/btPoint3.h" - -#include <BulletCollision/CollisionShapes/btCollisionMargin.h> +#include "../../LinearMath/btPoint3.h" +#include "../CollisionShapes/btCollisionMargin.h" class btConvexShape; #include "btSimplexSolverInterface.h" diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h index 00a9206fef0..f6a893151da 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h @@ -16,8 +16,8 @@ subject to the following restrictions: #ifndef MANIFOLD_CONTACT_POINT_H #define MANIFOLD_CONTACT_POINT_H -#include "LinearMath/btVector3.h" -#include "LinearMath/btTransformUtil.h" +#include "../../LinearMath/btVector3.h" +#include "../../LinearMath/btTransformUtil.h" @@ -29,7 +29,8 @@ class btManifoldPoint { public: btManifoldPoint() - :m_userPersistentData(0) + :m_userPersistentData(0), + m_lifeTime(0) { } @@ -40,8 +41,8 @@ class btManifoldPoint m_localPointB( pointB ), m_normalWorldOnB( normal ), m_distance1( distance ), - m_combinedFriction(0.f), - m_combinedRestitution(0.f), + m_combinedFriction(btScalar(0.)), + m_combinedRestitution(btScalar(0.)), m_userPersistentData(0), m_lifeTime(0) { @@ -58,16 +59,16 @@ class btManifoldPoint btVector3 m_positionWorldOnA; btVector3 m_normalWorldOnB; - float m_distance1; - float m_combinedFriction; - float m_combinedRestitution; + btScalar m_distance1; + btScalar m_combinedFriction; + btScalar m_combinedRestitution; - void* m_userPersistentData; + mutable void* m_userPersistentData; int m_lifeTime;//lifetime of the contactpoint in frames - float getDistance() const + btScalar getDistance() const { return m_distance1; } @@ -76,17 +77,17 @@ class btManifoldPoint return m_lifeTime; } - btVector3 getPositionWorldOnA() { + const btVector3& getPositionWorldOnA() const { return m_positionWorldOnA; // return m_positionWorldOnB + m_normalWorldOnB * m_distance1; } - const btVector3& getPositionWorldOnB() + const btVector3& getPositionWorldOnB() const { return m_positionWorldOnB; } - void setDistance(float dist) + void setDistance(btScalar dist) { m_distance1 = dist; } diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp index de6749a9b72..c4bab3a134a 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp @@ -25,48 +25,48 @@ subject to the following restrictions: #define NUM_UNITSPHERE_POINTS 42 static btVector3 sPenetrationDirections[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2] = { -btVector3(0.000000f , -0.000000f,-1.000000f), -btVector3(0.723608f , -0.525725f,-0.447219f), -btVector3(-0.276388f , -0.850649f,-0.447219f), -btVector3(-0.894426f , -0.000000f,-0.447216f), -btVector3(-0.276388f , 0.850649f,-0.447220f), -btVector3(0.723608f , 0.525725f,-0.447219f), -btVector3(0.276388f , -0.850649f,0.447220f), -btVector3(-0.723608f , -0.525725f,0.447219f), -btVector3(-0.723608f , 0.525725f,0.447219f), -btVector3(0.276388f , 0.850649f,0.447219f), -btVector3(0.894426f , 0.000000f,0.447216f), -btVector3(-0.000000f , 0.000000f,1.000000f), -btVector3(0.425323f , -0.309011f,-0.850654f), -btVector3(-0.162456f , -0.499995f,-0.850654f), -btVector3(0.262869f , -0.809012f,-0.525738f), -btVector3(0.425323f , 0.309011f,-0.850654f), -btVector3(0.850648f , -0.000000f,-0.525736f), -btVector3(-0.525730f , -0.000000f,-0.850652f), -btVector3(-0.688190f , -0.499997f,-0.525736f), -btVector3(-0.162456f , 0.499995f,-0.850654f), -btVector3(-0.688190f , 0.499997f,-0.525736f), -btVector3(0.262869f , 0.809012f,-0.525738f), -btVector3(0.951058f , 0.309013f,0.000000f), -btVector3(0.951058f , -0.309013f,0.000000f), -btVector3(0.587786f , -0.809017f,0.000000f), -btVector3(0.000000f , -1.000000f,0.000000f), -btVector3(-0.587786f , -0.809017f,0.000000f), -btVector3(-0.951058f , -0.309013f,-0.000000f), -btVector3(-0.951058f , 0.309013f,-0.000000f), -btVector3(-0.587786f , 0.809017f,-0.000000f), -btVector3(-0.000000f , 1.000000f,-0.000000f), -btVector3(0.587786f , 0.809017f,-0.000000f), -btVector3(0.688190f , -0.499997f,0.525736f), -btVector3(-0.262869f , -0.809012f,0.525738f), -btVector3(-0.850648f , 0.000000f,0.525736f), -btVector3(-0.262869f , 0.809012f,0.525738f), -btVector3(0.688190f , 0.499997f,0.525736f), -btVector3(0.525730f , 0.000000f,0.850652f), -btVector3(0.162456f , -0.499995f,0.850654f), -btVector3(-0.425323f , -0.309011f,0.850654f), -btVector3(-0.425323f , 0.309011f,0.850654f), -btVector3(0.162456f , 0.499995f,0.850654f) +btVector3(btScalar(0.000000) , btScalar(-0.000000),btScalar(-1.000000)), +btVector3(btScalar(0.723608) , btScalar(-0.525725),btScalar(-0.447219)), +btVector3(btScalar(-0.276388) , btScalar(-0.850649),btScalar(-0.447219)), +btVector3(btScalar(-0.894426) , btScalar(-0.000000),btScalar(-0.447216)), +btVector3(btScalar(-0.276388) , btScalar(0.850649),btScalar(-0.447220)), +btVector3(btScalar(0.723608) , btScalar(0.525725),btScalar(-0.447219)), +btVector3(btScalar(0.276388) , btScalar(-0.850649),btScalar(0.447220)), +btVector3(btScalar(-0.723608) , btScalar(-0.525725),btScalar(0.447219)), +btVector3(btScalar(-0.723608) , btScalar(0.525725),btScalar(0.447219)), +btVector3(btScalar(0.276388) , btScalar(0.850649),btScalar(0.447219)), +btVector3(btScalar(0.894426) , btScalar(0.000000),btScalar(0.447216)), +btVector3(btScalar(-0.000000) , btScalar(0.000000),btScalar(1.000000)), +btVector3(btScalar(0.425323) , btScalar(-0.309011),btScalar(-0.850654)), +btVector3(btScalar(-0.162456) , btScalar(-0.499995),btScalar(-0.850654)), +btVector3(btScalar(0.262869) , btScalar(-0.809012),btScalar(-0.525738)), +btVector3(btScalar(0.425323) , btScalar(0.309011),btScalar(-0.850654)), +btVector3(btScalar(0.850648) , btScalar(-0.000000),btScalar(-0.525736)), +btVector3(btScalar(-0.525730) , btScalar(-0.000000),btScalar(-0.850652)), +btVector3(btScalar(-0.688190) , btScalar(-0.499997),btScalar(-0.525736)), +btVector3(btScalar(-0.162456) , btScalar(0.499995),btScalar(-0.850654)), +btVector3(btScalar(-0.688190) , btScalar(0.499997),btScalar(-0.525736)), +btVector3(btScalar(0.262869) , btScalar(0.809012),btScalar(-0.525738)), +btVector3(btScalar(0.951058) , btScalar(0.309013),btScalar(0.000000)), +btVector3(btScalar(0.951058) , btScalar(-0.309013),btScalar(0.000000)), +btVector3(btScalar(0.587786) , btScalar(-0.809017),btScalar(0.000000)), +btVector3(btScalar(0.000000) , btScalar(-1.000000),btScalar(0.000000)), +btVector3(btScalar(-0.587786) , btScalar(-0.809017),btScalar(0.000000)), +btVector3(btScalar(-0.951058) , btScalar(-0.309013),btScalar(-0.000000)), +btVector3(btScalar(-0.951058) , btScalar(0.309013),btScalar(-0.000000)), +btVector3(btScalar(-0.587786) , btScalar(0.809017),btScalar(-0.000000)), +btVector3(btScalar(-0.000000) , btScalar(1.000000),btScalar(-0.000000)), +btVector3(btScalar(0.587786) , btScalar(0.809017),btScalar(-0.000000)), +btVector3(btScalar(0.688190) , btScalar(-0.499997),btScalar(0.525736)), +btVector3(btScalar(-0.262869) , btScalar(-0.809012),btScalar(0.525738)), +btVector3(btScalar(-0.850648) , btScalar(0.000000),btScalar(0.525736)), +btVector3(btScalar(-0.262869) , btScalar(0.809012),btScalar(0.525738)), +btVector3(btScalar(0.688190) , btScalar(0.499997),btScalar(0.525736)), +btVector3(btScalar(0.525730) , btScalar(0.000000),btScalar(0.850652)), +btVector3(btScalar(0.162456) , btScalar(-0.499995),btScalar(0.850654)), +btVector3(btScalar(-0.425323) , btScalar(-0.309011),btScalar(0.850654)), +btVector3(btScalar(-0.425323) , btScalar(0.309011),btScalar(0.850654)), +btVector3(btScalar(0.162456) , btScalar(0.499995),btScalar(0.850654)) }; @@ -78,6 +78,9 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s ) { + (void)stackAlloc; + (void)v; + struct btIntermediateResult : public btDiscreteCollisionDetectorInterface::Result { @@ -88,13 +91,17 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s btVector3 m_normalOnBInWorld; btVector3 m_pointInWorld; - float m_depth; + btScalar m_depth; bool m_hasResult; virtual void setShapeIdentifiers(int partId0,int index0, int partId1,int index1) { + (void)partId0; + (void)index0; + (void)partId1; + (void)index1; } - void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,float depth) + void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth) { m_normalOnBInWorld = normalOnBInWorld; m_pointInWorld = pointInWorld; @@ -104,7 +111,7 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s }; //just take fixed number of orientation, and sample the penetration depth in that direction - float minProj = 1e30f; + btScalar minProj = btScalar(1e30); btVector3 minNorm; btVector3 minVertex; btVector3 minA,minB; @@ -180,7 +187,7 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s pWorld = transA(pInA); qWorld = transB(qInB); w = qWorld - pWorld; - float delta = norm.dot(w); + btScalar delta = norm.dot(w); //find smallest delta if (delta < minProj) { @@ -234,7 +241,7 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s pWorld = transA(pInA); qWorld = transB(qInB); w = qWorld - pWorld; - float delta = norm.dot(w); + btScalar delta = norm.dot(w); //find smallest delta if (delta < minProj) { @@ -251,7 +258,7 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s minA += minNorm*convexA->getMargin(); minB -= minNorm*convexB->getMargin(); //no penetration - if (minProj < 0.f) + if (minProj < btScalar(0.)) return false; minProj += (convexA->getMargin() + convexB->getMargin()); @@ -268,7 +275,7 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s debugDraw->drawLine(minA,minB,color); color = btVector3 (1,1,1); btVector3 vec = minB-minA; - float prj2 = minNorm.dot(vec); + btScalar prj2 = minNorm.dot(vec); debugDraw->drawLine(minA,minA+(minNorm*minProj),color); } @@ -292,16 +299,16 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s input.m_transformA = displacedTrans; input.m_transformB = transB; - input.m_maximumDistanceSquared = 1e30f;//minProj; + input.m_maximumDistanceSquared = btScalar(1e30);//minProj; btIntermediateResult res; gjkdet.getClosestPoints(input,res,debugDraw); - float correctedMinNorm = minProj - res.m_depth; + btScalar correctedMinNorm = minProj - res.m_depth; //the penetration depth is over-estimated, relax it - float penetration_relaxation= 1.f; + btScalar penetration_relaxation= btScalar(1.); minNorm*=penetration_relaxation; if (res.m_hasResult) diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp index ee2be163063..08cb3ed334d 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp @@ -18,7 +18,7 @@ subject to the following restrictions: #include "LinearMath/btTransform.h" #include <assert.h> -float gContactBreakingThreshold = 0.02f; +btScalar gContactBreakingThreshold = btScalar(0.02); ContactDestroyedCallback gContactDestroyedCallback = 0; @@ -100,7 +100,7 @@ int btPersistentManifold::sortCachedPoints(const btManifoldPoint& pt) int maxPenetrationIndex = -1; #define KEEP_DEEPEST_POINT 1 #ifdef KEEP_DEEPEST_POINT - float maxPenetration = pt.getDistance(); + btScalar maxPenetration = pt.getDistance(); for (int i=0;i<4;i++) { if (m_pointCache[i].getDistance() < maxPenetration) @@ -111,7 +111,7 @@ int btPersistentManifold::sortCachedPoints(const btManifoldPoint& pt) } #endif //KEEP_DEEPEST_POINT - btScalar res0(0.f),res1(0.f),res2(0.f),res3(0.f); + btScalar res0(btScalar(0.)),res1(btScalar(0.)),res2(btScalar(0.)),res3(btScalar(0.)); if (maxPenetrationIndex != 0) { btVector3 a0 = pt.m_localPointA-m_pointCache[1].m_localPointA; @@ -193,7 +193,7 @@ void btPersistentManifold::AddManifoldPoint(const btManifoldPoint& newPoint) replaceContactPoint(newPoint,insertIndex); } -float btPersistentManifold::getContactBreakingThreshold() const +btScalar btPersistentManifold::getContactBreakingThreshold() const { return gContactBreakingThreshold; } diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h index c2372b648f6..a5918b84db3 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h @@ -17,14 +17,14 @@ subject to the following restrictions: #define PERSISTENT_MANIFOLD_H -#include "LinearMath/btVector3.h" -#include "LinearMath/btTransform.h" +#include "../../LinearMath/btVector3.h" +#include "../../LinearMath/btTransform.h" #include "btManifoldPoint.h" struct btCollisionResult; ///contact breaking and merging threshold -extern float gContactBreakingThreshold; +extern btScalar gContactBreakingThreshold; typedef bool (*ContactDestroyedCallback)(void* userPersistentData); extern ContactDestroyedCallback gContactDestroyedCallback; @@ -36,7 +36,7 @@ extern ContactDestroyedCallback gContactDestroyedCallback; ///btPersistentManifold maintains contact points, and reduces them to 4. ///It does contact filtering/contact reduction. -class btPersistentManifold +ATTRIBUTE_ALIGNED16( class) btPersistentManifold { btManifoldPoint m_pointCache[MANIFOLD_CACHE_SIZE]; @@ -97,7 +97,7 @@ public: } /// todo: get this margin from the current physics / collision environment - float getContactBreakingThreshold() const; + btScalar getContactBreakingThreshold() const; int getCacheEntry(const btManifoldPoint& newPoint) const; @@ -108,18 +108,36 @@ public: clearUserCache(m_pointCache[index]); int lastUsedIndex = getNumContacts() - 1; - m_pointCache[index] = m_pointCache[lastUsedIndex]; - //get rid of duplicated userPersistentData pointer - m_pointCache[lastUsedIndex].m_userPersistentData = 0; +// m_pointCache[index] = m_pointCache[lastUsedIndex]; + if(index != lastUsedIndex) + { + m_pointCache[index] = m_pointCache[lastUsedIndex]; + //get rid of duplicated userPersistentData pointer + m_pointCache[lastUsedIndex].m_userPersistentData = 0; + } + + btAssert(m_pointCache[lastUsedIndex].m_userPersistentData==0); m_cachedPoints--; } void replaceContactPoint(const btManifoldPoint& newPoint,int insertIndex) { - assert(validContactDistance(newPoint)); + btAssert(validContactDistance(newPoint)); - clearUserCache(m_pointCache[insertIndex]); +#define MAINTAIN_PERSISTENCY 1 +#ifdef MAINTAIN_PERSISTENCY + int lifeTime = m_pointCache[insertIndex].getLifeTime(); + btAssert(lifeTime>=0); + void* cache = m_pointCache[insertIndex].m_userPersistentData; m_pointCache[insertIndex] = newPoint; + + m_pointCache[insertIndex].m_userPersistentData = cache; + m_pointCache[insertIndex].m_lifeTime = lifeTime; +#else + clearUserCache(m_pointCache[insertIndex]); + m_pointCache[insertIndex] = newPoint; + +#endif } bool validContactDistance(const btManifoldPoint& pt) const @@ -133,7 +151,10 @@ public: -}; +} +; + + diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPointCollector.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPointCollector.h index a51e2d5e13b..6262f44b9f1 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPointCollector.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPointCollector.h @@ -31,16 +31,20 @@ struct btPointCollector : public btDiscreteCollisionDetectorInterface::Result bool m_hasResult; btPointCollector () - : m_distance(1e30f),m_hasResult(false) + : m_distance(btScalar(1e30)),m_hasResult(false) { } virtual void setShapeIdentifiers(int partId0,int index0, int partId1,int index1) { + (void)partId0; + (void)index0; + (void)partId1; + (void)index1; //?? } - virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,float depth) + virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth) { if (depth< m_distance) { diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp index 88ee005792c..31b91467777 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp @@ -20,7 +20,7 @@ btTriangleRaycastCallback::btTriangleRaycastCallback(const btVector3& from,const : m_from(from), m_to(to), - m_hitFraction(1.f) + m_hitFraction(btScalar(1.)) { } @@ -40,19 +40,19 @@ void btTriangleRaycastCallback::processTriangle(btVector3* triangle,int partId, btVector3 triangleNormal; triangleNormal = v10.cross( v20 ); - const float dist = vert0.dot(triangleNormal); - float dist_a = triangleNormal.dot(m_from) ; + const btScalar dist = vert0.dot(triangleNormal); + btScalar dist_a = triangleNormal.dot(m_from) ; dist_a-= dist; - float dist_b = triangleNormal.dot(m_to); + btScalar dist_b = triangleNormal.dot(m_to); dist_b -= dist; - if ( dist_a * dist_b >= 0.0f) + if ( dist_a * dist_b >= btScalar(0.0) ) { return ; // same sign } - const float proj_length=dist_a-dist_b; - const float distance = (dist_a)/(proj_length); + const btScalar proj_length=dist_a-dist_b; + const btScalar distance = (dist_a)/(proj_length); // Now we have the intersection point on the plane, we'll see if it's inside the triangle // Add an epsilon as a tolerance for the raycast, // in case the ray hits exacly on the edge of the triangle. @@ -62,27 +62,27 @@ void btTriangleRaycastCallback::processTriangle(btVector3* triangle,int partId, { - float edge_tolerance =triangleNormal.length2(); - edge_tolerance *= -0.0001f; + btScalar edge_tolerance =triangleNormal.length2(); + edge_tolerance *= btScalar(-0.0001); btVector3 point; point.setInterpolate3( m_from, m_to, distance); { btVector3 v0p; v0p = vert0 - point; btVector3 v1p; v1p = vert1 - point; btVector3 cp0; cp0 = v0p.cross( v1p ); - if ( (float)(cp0.dot(triangleNormal)) >=edge_tolerance) + if ( (btScalar)(cp0.dot(triangleNormal)) >=edge_tolerance) { btVector3 v2p; v2p = vert2 - point; btVector3 cp1; cp1 = v1p.cross( v2p); - if ( (float)(cp1.dot(triangleNormal)) >=edge_tolerance) + if ( (btScalar)(cp1.dot(triangleNormal)) >=edge_tolerance) { btVector3 cp2; cp2 = v2p.cross(v0p); - if ( (float)(cp2.dot(triangleNormal)) >=edge_tolerance) + if ( (btScalar)(cp2.dot(triangleNormal)) >=edge_tolerance) { if ( dist_a > 0 ) diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h index fbb51d30522..a0bbc9f8fe9 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h @@ -16,7 +16,7 @@ subject to the following restrictions: #ifndef RAYCAST_TRI_CALLBACK_H #define RAYCAST_TRI_CALLBACK_H -#include "BulletCollision/CollisionShapes/btTriangleCallback.h" +#include "../CollisionShapes/btTriangleCallback.h" struct btBroadphaseProxy; @@ -28,13 +28,13 @@ public: btVector3 m_from; btVector3 m_to; - float m_hitFraction; + btScalar m_hitFraction; btTriangleRaycastCallback(const btVector3& from,const btVector3& to); virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex); - virtual float reportHit(const btVector3& hitNormalLocal, float hitFraction, int partId, int triangleIndex ) = 0; + virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex ) = 0; }; diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h index cf65f46505b..58393b2eab9 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h @@ -18,8 +18,8 @@ subject to the following restrictions: #ifndef SIMPLEX_SOLVER_INTERFACE_H #define SIMPLEX_SOLVER_INTERFACE_H -#include "LinearMath/btVector3.h" -#include "LinearMath/btPoint3.h" +#include "../../LinearMath/btVector3.h" +#include "../../LinearMath/btPoint3.h" #define NO_VIRTUAL_INTERFACE 1 #ifdef NO_VIRTUAL_INTERFACE diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp index dc995ca1f72..1ab336b4d54 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp @@ -28,8 +28,11 @@ m_convexA(convexA),m_convexB(convexB) ///Typically the conservative advancement reaches solution in a few iterations, clip it to 32 for degenerate cases. ///See discussion about this here http://continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=565 +#ifdef BT_USE_DOUBLE_PRECISION +#define MAX_ITERATIONS 64 +#else #define MAX_ITERATIONS 32 - +#endif bool btSubsimplexConvexCast::calcTimeOfImpact( const btTransform& fromA, const btTransform& toA, @@ -52,9 +55,9 @@ bool btSubsimplexConvexCast::calcTimeOfImpact( convex->setTransformB(btTransform(rayFromLocalA.getBasis())); - //float radius = 0.01f; + //btScalar radius = btScalar(0.01); - btScalar lambda = 0.f; + btScalar lambda = btScalar(0.); //todo: need to verify this: //because of minkowski difference, we need the inverse direction @@ -69,27 +72,30 @@ bool btSubsimplexConvexCast::calcTimeOfImpact( int maxIter = MAX_ITERATIONS; btVector3 n; - n.setValue(0.f,0.f,0.f); + n.setValue(btScalar(0.),btScalar(0.),btScalar(0.)); bool hasResult = false; btVector3 c; - float lastLambda = lambda; - + btScalar lastLambda = lambda; - float dist2 = v.length2(); - float epsilon = 0.0001f; + btScalar dist2 = v.length2(); +#ifdef BT_USE_DOUBLE_PRECISION + btScalar epsilon = btScalar(0.0001); +#else + btScalar epsilon = btScalar(0.0001); +#endif //BT_USE_DOUBLE_PRECISION btVector3 w,p; - float VdotR; + btScalar VdotR; while ( (dist2 > epsilon) && maxIter--) { p = convex->localGetSupportingVertex( v); w = x - p; - float VdotW = v.dot(w); + btScalar VdotW = v.dot(w); - if ( VdotW > 0.f) + if ( VdotW > btScalar(0.)) { VdotR = v.dot(r); @@ -117,7 +123,7 @@ bool btSubsimplexConvexCast::calcTimeOfImpact( //printf("numverts = %i\n",m_simplexSolver->numVertices()); } else { - dist2 = 0.f; + dist2 = btScalar(0.); } } diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp index 23d66a3bbc8..105b7eccefa 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp @@ -70,7 +70,7 @@ void btVoronoiSimplexSolver::reset() m_cachedValidClosest = false; m_numVertices = 0; m_needsUpdate = true; - m_lastW = btVector3(1e30f,1e30f,1e30f); + m_lastW = btVector3(btScalar(1e30),btScalar(1e30),btScalar(1e30)); m_cachedBC.reset(); } @@ -109,7 +109,7 @@ bool btVoronoiSimplexSolver::updateClosestVectorAndPoints() m_cachedP2 = m_simplexPointsQ[0]; m_cachedV = m_cachedP1-m_cachedP2; //== m_simplexVectorW[0] m_cachedBC.reset(); - m_cachedBC.setBarycentricCoordinates(1.f,0.f,0.f,0.f); + m_cachedBC.setBarycentricCoordinates(btScalar(1.),btScalar(0.),btScalar(0.),btScalar(0.)); m_cachedValidClosest = m_cachedBC.isValid(); break; }; @@ -120,13 +120,13 @@ bool btVoronoiSimplexSolver::updateClosestVectorAndPoints() const btVector3& to = m_simplexVectorW[1]; btVector3 nearest; - btVector3 p (0.f,0.f,0.f); + btVector3 p (btScalar(0.),btScalar(0.),btScalar(0.)); btVector3 diff = p - from; btVector3 v = to - from; - float t = v.dot(diff); + btScalar t = v.dot(diff); if (t > 0) { - float dotVV = v.dot(v); + btScalar dotVV = v.dot(v); if (t < dotVV) { t /= dotVV; diff -= t*v; @@ -156,38 +156,36 @@ bool btVoronoiSimplexSolver::updateClosestVectorAndPoints() m_cachedValidClosest = m_cachedBC.isValid(); break; } - case 3: - { - //closest point origin from triangle - btVector3 p (0.f,0.f,0.f); - - const btVector3& a = m_simplexVectorW[0]; - const btVector3& b = m_simplexVectorW[1]; - const btVector3& c = m_simplexVectorW[2]; + case 3: + { + //closest point origin from triangle + btVector3 p (btScalar(0.),btScalar(0.),btScalar(0.)); - closestPtPointTriangle(p,a,b,c,m_cachedBC); - m_cachedP1 = m_simplexPointsP[0] * m_cachedBC.m_barycentricCoords[0] + - m_simplexPointsP[1] * m_cachedBC.m_barycentricCoords[1] + - m_simplexPointsP[2] * m_cachedBC.m_barycentricCoords[2] + - m_simplexPointsP[3] * m_cachedBC.m_barycentricCoords[3]; + const btVector3& a = m_simplexVectorW[0]; + const btVector3& b = m_simplexVectorW[1]; + const btVector3& c = m_simplexVectorW[2]; - m_cachedP2 = m_simplexPointsQ[0] * m_cachedBC.m_barycentricCoords[0] + - m_simplexPointsQ[1] * m_cachedBC.m_barycentricCoords[1] + - m_simplexPointsQ[2] * m_cachedBC.m_barycentricCoords[2] + - m_simplexPointsQ[3] * m_cachedBC.m_barycentricCoords[3]; + closestPtPointTriangle(p,a,b,c,m_cachedBC); + m_cachedP1 = m_simplexPointsP[0] * m_cachedBC.m_barycentricCoords[0] + + m_simplexPointsP[1] * m_cachedBC.m_barycentricCoords[1] + + m_simplexPointsP[2] * m_cachedBC.m_barycentricCoords[2]; - m_cachedV = m_cachedP1-m_cachedP2; + m_cachedP2 = m_simplexPointsQ[0] * m_cachedBC.m_barycentricCoords[0] + + m_simplexPointsQ[1] * m_cachedBC.m_barycentricCoords[1] + + m_simplexPointsQ[2] * m_cachedBC.m_barycentricCoords[2]; - reduceVertices (m_cachedBC.m_usedVertices); - m_cachedValidClosest = m_cachedBC.isValid(); + m_cachedV = m_cachedP1-m_cachedP2; - break; + reduceVertices (m_cachedBC.m_usedVertices); + m_cachedValidClosest = m_cachedBC.isValid(); + + break; } case 4: { - btVector3 p (0.f,0.f,0.f); + btVector3 p (btScalar(0.),btScalar(0.),btScalar(0.)); const btVector3& a = m_simplexVectorW[0]; const btVector3& b = m_simplexVectorW[1]; @@ -222,7 +220,7 @@ bool btVoronoiSimplexSolver::updateClosestVectorAndPoints() { m_cachedValidClosest = true; //degenerate case == false, penetration = true + zero - m_cachedV.setValue(0.f,0.f,0.f); + m_cachedV.setValue(btScalar(0.),btScalar(0.),btScalar(0.)); } break; } @@ -256,7 +254,7 @@ bool btVoronoiSimplexSolver::closest(btVector3& v) btScalar btVoronoiSimplexSolver::maxVertex() { int i, numverts = numVertices(); - btScalar maxV = 0.f; + btScalar maxV = btScalar(0.); for (i=0;i<numverts;i++) { btScalar curLen2 = m_simplexVectorW[i].length2(); @@ -288,7 +286,7 @@ bool btVoronoiSimplexSolver::inSimplex(const btVector3& w) { bool found = false; int i, numverts = numVertices(); - //btScalar maxV = 0.f; + //btScalar maxV = btScalar(0.); //w is in the current (reduced) simplex for (i=0;i<numverts;i++) @@ -335,9 +333,9 @@ bool btVoronoiSimplexSolver::closestPtPointTriangle(const btPoint3& p, const btP btVector3 ab = b - a; btVector3 ac = c - a; btVector3 ap = p - a; - float d1 = ab.dot(ap); - float d2 = ac.dot(ap); - if (d1 <= 0.0f && d2 <= 0.0f) + btScalar d1 = ab.dot(ap); + btScalar d2 = ac.dot(ap); + if (d1 <= btScalar(0.0) && d2 <= btScalar(0.0)) { result.m_closestPointOnSimplex = a; result.m_usedVertices.usedVertexA = true; @@ -347,9 +345,9 @@ bool btVoronoiSimplexSolver::closestPtPointTriangle(const btPoint3& p, const btP // Check if P in vertex region outside B btVector3 bp = p - b; - float d3 = ab.dot(bp); - float d4 = ac.dot(bp); - if (d3 >= 0.0f && d4 <= d3) + btScalar d3 = ab.dot(bp); + btScalar d4 = ac.dot(bp); + if (d3 >= btScalar(0.0) && d4 <= d3) { result.m_closestPointOnSimplex = b; result.m_usedVertices.usedVertexB = true; @@ -358,9 +356,9 @@ bool btVoronoiSimplexSolver::closestPtPointTriangle(const btPoint3& p, const btP return true; // b; // barycentric coordinates (0,1,0) } // Check if P in edge region of AB, if so return projection of P onto AB - float vc = d1*d4 - d3*d2; - if (vc <= 0.0f && d1 >= 0.0f && d3 <= 0.0f) { - float v = d1 / (d1 - d3); + btScalar vc = d1*d4 - d3*d2; + if (vc <= btScalar(0.0) && d1 >= btScalar(0.0) && d3 <= btScalar(0.0)) { + btScalar v = d1 / (d1 - d3); result.m_closestPointOnSimplex = a + v * ab; result.m_usedVertices.usedVertexA = true; result.m_usedVertices.usedVertexB = true; @@ -371,9 +369,9 @@ bool btVoronoiSimplexSolver::closestPtPointTriangle(const btPoint3& p, const btP // Check if P in vertex region outside C btVector3 cp = p - c; - float d5 = ab.dot(cp); - float d6 = ac.dot(cp); - if (d6 >= 0.0f && d5 <= d6) + btScalar d5 = ab.dot(cp); + btScalar d6 = ac.dot(cp); + if (d6 >= btScalar(0.0) && d5 <= d6) { result.m_closestPointOnSimplex = c; result.m_usedVertices.usedVertexC = true; @@ -382,9 +380,9 @@ bool btVoronoiSimplexSolver::closestPtPointTriangle(const btPoint3& p, const btP } // Check if P in edge region of AC, if so return projection of P onto AC - float vb = d5*d2 - d1*d6; - if (vb <= 0.0f && d2 >= 0.0f && d6 <= 0.0f) { - float w = d2 / (d2 - d6); + btScalar vb = d5*d2 - d1*d6; + if (vb <= btScalar(0.0) && d2 >= btScalar(0.0) && d6 <= btScalar(0.0)) { + btScalar w = d2 / (d2 - d6); result.m_closestPointOnSimplex = a + w * ac; result.m_usedVertices.usedVertexA = true; result.m_usedVertices.usedVertexC = true; @@ -394,9 +392,9 @@ bool btVoronoiSimplexSolver::closestPtPointTriangle(const btPoint3& p, const btP } // Check if P in edge region of BC, if so return projection of P onto BC - float va = d3*d6 - d5*d4; - if (va <= 0.0f && (d4 - d3) >= 0.0f && (d5 - d6) >= 0.0f) { - float w = (d4 - d3) / ((d4 - d3) + (d5 - d6)); + btScalar va = d3*d6 - d5*d4; + if (va <= btScalar(0.0) && (d4 - d3) >= btScalar(0.0) && (d5 - d6) >= btScalar(0.0)) { + btScalar w = (d4 - d3) / ((d4 - d3) + (d5 - d6)); result.m_closestPointOnSimplex = b + w * (c - b); result.m_usedVertices.usedVertexB = true; @@ -407,9 +405,9 @@ bool btVoronoiSimplexSolver::closestPtPointTriangle(const btPoint3& p, const btP } // P inside face region. Compute Q through its barycentric coordinates (u,v,w) - float denom = 1.0f / (va + vb + vc); - float v = vb * denom; - float w = vc * denom; + btScalar denom = btScalar(1.0) / (va + vb + vc); + btScalar v = vb * denom; + btScalar w = vc * denom; result.m_closestPointOnSimplex = a + ab * v + ac * w; result.m_usedVertices.usedVertexA = true; @@ -418,7 +416,7 @@ bool btVoronoiSimplexSolver::closestPtPointTriangle(const btPoint3& p, const btP result.setBarycentricCoordinates(1-v-w,v,w); return true; -// return a + ab * v + ac * w; // = u*a + v*b + w*c, u = va * denom = 1.0f - v - w +// return a + ab * v + ac * w; // = u*a + v*b + w*c, u = va * denom = btScalar(1.0) - v - w } @@ -431,18 +429,26 @@ int btVoronoiSimplexSolver::pointOutsideOfPlane(const btPoint3& p, const btPoint { btVector3 normal = (b-a).cross(c-a); - float signp = (p - a).dot(normal); // [AP AB AC] - float signd = (d - a).dot( normal); // [AD AB AC] + btScalar signp = (p - a).dot(normal); // [AP AB AC] + btScalar signd = (d - a).dot( normal); // [AD AB AC] #ifdef CATCH_DEGENERATE_TETRAHEDRON - if (signd * signd < (1e-4f * 1e-4f)) +#ifdef BT_USE_DOUBLE_PRECISION +if (signd * signd < (btScalar(1e-8) * btScalar(1e-8))) + { + return -1; + } +#else + if (signd * signd < (btScalar(1e-4) * btScalar(1e-4))) { // printf("affine dependent/degenerate\n");// return -1; } #endif + +#endif // Points on opposite sides if expression signs are opposite - return signp * signd < 0.f; + return signp * signd < btScalar(0.); } @@ -475,14 +481,14 @@ bool btVoronoiSimplexSolver::closestPtPointTetrahedron(const btPoint3& p, const } - float bestSqDist = FLT_MAX; + btScalar bestSqDist = FLT_MAX; // If point outside face abc then compute closest point on abc if (pointOutsideABC) { closestPtPointTriangle(p, a, b, c,tempResult); btPoint3 q = tempResult.m_closestPointOnSimplex; - float sqDist = (q - p).dot( q - p); + btScalar sqDist = (q - p).dot( q - p); // Update best closest point if (squared) distance is less than current best if (sqDist < bestSqDist) { bestSqDist = sqDist; @@ -510,13 +516,14 @@ bool btVoronoiSimplexSolver::closestPtPointTetrahedron(const btPoint3& p, const btPoint3 q = tempResult.m_closestPointOnSimplex; //convert result bitmask! - float sqDist = (q - p).dot( q - p); + btScalar sqDist = (q - p).dot( q - p); if (sqDist < bestSqDist) { bestSqDist = sqDist; finalResult.m_closestPointOnSimplex = q; finalResult.m_usedVertices.reset(); finalResult.m_usedVertices.usedVertexA = tempResult.m_usedVertices.usedVertexA; + finalResult.m_usedVertices.usedVertexC = tempResult.m_usedVertices.usedVertexB; finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexC; finalResult.setBarycentricCoordinates( @@ -537,15 +544,16 @@ bool btVoronoiSimplexSolver::closestPtPointTetrahedron(const btPoint3& p, const btPoint3 q = tempResult.m_closestPointOnSimplex; //convert result bitmask! - float sqDist = (q - p).dot( q - p); + btScalar sqDist = (q - p).dot( q - p); if (sqDist < bestSqDist) { bestSqDist = sqDist; finalResult.m_closestPointOnSimplex = q; finalResult.m_usedVertices.reset(); finalResult.m_usedVertices.usedVertexA = tempResult.m_usedVertices.usedVertexA; - finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexB; finalResult.m_usedVertices.usedVertexB = tempResult.m_usedVertices.usedVertexC; + + finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexB; finalResult.setBarycentricCoordinates( tempResult.m_barycentricCoords[VERTA], tempResult.m_barycentricCoords[VERTC], @@ -563,15 +571,16 @@ bool btVoronoiSimplexSolver::closestPtPointTetrahedron(const btPoint3& p, const closestPtPointTriangle(p, b, d, c,tempResult); btPoint3 q = tempResult.m_closestPointOnSimplex; //convert result bitmask! - float sqDist = (q - p).dot( q - p); + btScalar sqDist = (q - p).dot( q - p); if (sqDist < bestSqDist) { bestSqDist = sqDist; finalResult.m_closestPointOnSimplex = q; finalResult.m_usedVertices.reset(); + // finalResult.m_usedVertices.usedVertexB = tempResult.m_usedVertices.usedVertexA; - finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexB; finalResult.m_usedVertices.usedVertexC = tempResult.m_usedVertices.usedVertexC; + finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexB; finalResult.setBarycentricCoordinates( 0, diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h index 8b743996915..356d335bc93 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h @@ -55,7 +55,7 @@ struct btSubSimplexClosestResult //stores the simplex vertex-usage, using the MASK, // if m_usedVertices & MASK then the related vertex is used btUsageBitfield m_usedVertices; - float m_barycentricCoords[4]; + btScalar m_barycentricCoords[4]; bool m_degenerate; void reset() @@ -66,15 +66,15 @@ struct btSubSimplexClosestResult } bool isValid() { - bool valid = (m_barycentricCoords[0] >= 0.f) && - (m_barycentricCoords[1] >= 0.f) && - (m_barycentricCoords[2] >= 0.f) && - (m_barycentricCoords[3] >= 0.f); + bool valid = (m_barycentricCoords[0] >= btScalar(0.)) && + (m_barycentricCoords[1] >= btScalar(0.)) && + (m_barycentricCoords[2] >= btScalar(0.)) && + (m_barycentricCoords[3] >= btScalar(0.)); return valid; } - void setBarycentricCoordinates(float a=0.f,float b=0.f,float c=0.f,float d=0.f) + void setBarycentricCoordinates(btScalar a=btScalar(0.),btScalar b=btScalar(0.),btScalar c=btScalar(0.),btScalar d=btScalar(0.)) { m_barycentricCoords[0] = a; m_barycentricCoords[1] = b; |