Changeset 2430 for code/branches/physics/src/bullet/LinearMath/btTransformUtil.h

Timestamp:

Dec 13, 2008, 11:45:51 PM (16 years ago)

Author:

rgrieder

Message:

Updated to Bullet 2.73 (first part).

File:

• code/branches/physics/src/bullet/LinearMath/btTransformUtil.h (modified) (3 diffs)

code/branches/physics/src/bullet/LinearMath/btTransformUtil.h

@@ -101 +101 @@
         }
 
+        static void     calculateVelocityQuaternion(const btVector3& pos0,const btVector3& pos1,const btQuaternion& orn0,const btQuaternion& orn1,btScalar timeStep,btVector3& linVel,btVector3& angVel)
+        {
+                linVel = (pos1 - pos0) / timeStep;
+                btVector3 axis;
+                btScalar  angle;
+                if (orn0 != orn1)
+                {
+                        calculateDiffAxisAngleQuaternion(orn0,orn1,axis,angle);
+                        angVel = axis * angle / timeStep;
+                } else
+                {
+                        angVel.setValue(0,0,0);
+                }
+        }
+
+        static void calculateDiffAxisAngleQuaternion(const btQuaternion& orn0,const btQuaternion& orn1a,btVector3& axis,btScalar& angle)
+        {
+                btQuaternion orn1 = orn0.farthest(orn1a);
+                btQuaternion dorn = orn1 * orn0.inverse();
+                ///floating point inaccuracy can lead to w component > 1..., which breaks 
+                dorn.normalize();
+                angle = dorn.getAngle();
+                axis = btVector3(dorn.x(),dorn.y(),dorn.z());
+                axis[3] = btScalar(0.);
+                //check for axis length
+                btScalar len = axis.length2();
+                if (len < SIMD_EPSILON*SIMD_EPSILON)
+                        axis = btVector3(btScalar(1.),btScalar(0.),btScalar(0.));
+                else
+                        axis /= btSqrt(len);
+        }
+
         static void     calculateVelocity(const btTransform& transform0,const btTransform& transform1,btScalar timeStep,btVector3& linVel,btVector3& angVel)
         {
@@ -112 +144 @@
         static void calculateDiffAxisAngle(const btTransform& transform0,const btTransform& transform1,btVector3& axis,btScalar& angle)
         {
-        
-        #ifdef USE_QUATERNION_DIFF
-                btQuaternion orn0 = transform0.getRotation();
-                btQuaternion orn1a = transform1.getRotation();
-                btQuaternion orn1 = orn0.farthest(orn1a);
-                btQuaternion dorn = orn1 * orn0.inverse();
-#else
                 btMatrix3x3 dmat = transform1.getBasis() * transform0.getBasis().inverse();
                 btQuaternion dorn;
                 dmat.getRotation(dorn);
-#endif//USE_QUATERNION_DIFF
-        
+
                 ///floating point inaccuracy can lead to w component > 1..., which breaks 
-
                 dorn.normalize();
                 
@@ -141 +164 @@
 };
 
+
+///The btConvexSeparatingDistanceUtil can help speed up convex collision detection 
+///by conservatively updating a cached separating distance/vector instead of re-calculating the closest distance
+class   btConvexSeparatingDistanceUtil
+{
+        btQuaternion    m_ornA;
+        btQuaternion    m_ornB;
+        btVector3       m_posA;
+        btVector3       m_posB;
+        
+        btVector3       m_separatingNormal;
+
+        btScalar        m_boundingRadiusA;
+        btScalar        m_boundingRadiusB;
+        btScalar        m_separatingDistance;
+
+public:
+
+        btConvexSeparatingDistanceUtil(btScalar boundingRadiusA,btScalar        boundingRadiusB)
+                :m_boundingRadiusA(boundingRadiusA),
+                m_boundingRadiusB(boundingRadiusB),
+                m_separatingDistance(0.f)
+        {
+        }
+
+        btScalar        getConservativeSeparatingDistance()
+        {
+                return m_separatingDistance;
+        }
+
+        void    updateSeparatingDistance(const btTransform& transA,const btTransform& transB)
+        {
+                const btVector3& toPosA = transA.getOrigin();
+                const btVector3& toPosB = transB.getOrigin();
+                btQuaternion toOrnA = transA.getRotation();
+                btQuaternion toOrnB = transB.getRotation();
+
+                if (m_separatingDistance>0.f)
+                {
+                        
+
+                        btVector3 linVelA,angVelA,linVelB,angVelB;
+                        btTransformUtil::calculateVelocityQuaternion(m_posA,toPosA,m_ornA,toOrnA,btScalar(1.),linVelA,angVelA);
+                        btTransformUtil::calculateVelocityQuaternion(m_posB,toPosB,m_ornB,toOrnB,btScalar(1.),linVelB,angVelB);
+                        btScalar maxAngularProjectedVelocity = angVelA.length() * m_boundingRadiusA + angVelB.length() * m_boundingRadiusB;
+                        btVector3 relLinVel = (linVelB-linVelA);
+                        btScalar relLinVelocLength = (linVelB-linVelA).dot(m_separatingNormal);
+                        if (relLinVelocLength<0.f)
+                        {
+                                relLinVelocLength = 0.f;
+                        }
+        
+                        btScalar        projectedMotion = maxAngularProjectedVelocity +relLinVelocLength;
+                        m_separatingDistance -= projectedMotion;
+                }
+        
+                m_posA = toPosA;
+                m_posB = toPosB;
+                m_ornA = toOrnA;
+                m_ornB = toOrnB;
+        }
+
+        void    initSeparatingDistance(const btVector3& separatingVector,btScalar separatingDistance,const btTransform& transA,const btTransform& transB)
+        {
+                m_separatingNormal = separatingVector;
+                m_separatingDistance = separatingDistance;
+                
+                const btVector3& toPosA = transA.getOrigin();
+                const btVector3& toPosB = transB.getOrigin();
+                btQuaternion toOrnA = transA.getRotation();
+                btQuaternion toOrnB = transB.getRotation();
+                m_posA = toPosA;
+                m_posB = toPosB;
+                m_ornA = toOrnA;
+                m_ornB = toOrnB;
+        }
+
+};
+
+
 #endif //SIMD_TRANSFORM_UTIL_H