diff options
Diffstat (limited to 'extern/bullet2/src/LinearMath/btTransformUtil.h')
| -rw-r--r-- | extern/bullet2/src/LinearMath/btTransformUtil.h | 123 |
1 files changed, 113 insertions, 10 deletions
diff --git a/extern/bullet2/src/LinearMath/btTransformUtil.h b/extern/bullet2/src/LinearMath/btTransformUtil.h index 86ee1da5edf..e8328da4ca6 100644 --- a/extern/bullet2/src/LinearMath/btTransformUtil.h +++ b/extern/bullet2/src/LinearMath/btTransformUtil.h @@ -100,6 +100,38 @@ public: predictedTransform.setRotation(predictedOrn); } + 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) { linVel = (transform1.getOrigin() - transform0.getOrigin()) / timeStep; @@ -111,20 +143,11 @@ public: 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 + ///floating point inaccuracy can lead to w component > 1..., which breaks dorn.normalize(); angle = dorn.getAngle(); @@ -140,5 +163,85 @@ public: }; + +///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 |