1 /* 2 Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/ 3 4 This software is provided 'as-is', without any express or implied warranty. 5 In no event will the authors be held liable for any damages arising from the use of this software. 6 Permission is granted to anyone to use this software for any purpose, 7 including commercial applications, and to alter it and redistribute it freely, 8 subject to the following restrictions: 9 10 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. 11 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. 12 3. This notice may not be removed or altered from any source distribution. 13 */ 14 15 16 #ifndef BT_TRANSFORM_UTIL_H 17 #define BT_TRANSFORM_UTIL_H 18 19 #include "btTransform.h" 20 #define ANGULAR_MOTION_THRESHOLD btScalar(0.5)*SIMD_HALF_PI 21 22 23 24 25 SIMD_FORCE_INLINE btVector3 btAabbSupport(const btVector3& halfExtents,const btVector3& supportDir) 26 { 27 return btVector3(supportDir.x() < btScalar(0.0) ? -halfExtents.x() : halfExtents.x(), 28 supportDir.y() < btScalar(0.0) ? -halfExtents.y() : halfExtents.y(), 29 supportDir.z() < btScalar(0.0) ? -halfExtents.z() : halfExtents.z()); 30 } 31 32 33 34 35 36 37 /// Utils related to temporal transforms 38 class btTransformUtil 39 { 40 41 public: 42 43 static void integrateTransform(const btTransform& curTrans,const btVector3& linvel,const btVector3& angvel,btScalar timeStep,btTransform& predictedTransform) 44 { 45 predictedTransform.setOrigin(curTrans.getOrigin() + linvel * timeStep); 46 // #define QUATERNION_DERIVATIVE 47 #ifdef QUATERNION_DERIVATIVE 48 btQuaternion predictedOrn = curTrans.getRotation(); 49 predictedOrn += (angvel * predictedOrn) * (timeStep * btScalar(0.5)); 50 predictedOrn.normalize(); 51 #else 52 //Exponential map 53 //google for "Practical Parameterization of Rotations Using the Exponential Map", F. Sebastian Grassia 54 55 btVector3 axis; 56 btScalar fAngle = angvel.length(); 57 //limit the angular motion 58 if (fAngle*timeStep > ANGULAR_MOTION_THRESHOLD) 59 { 60 fAngle = ANGULAR_MOTION_THRESHOLD / timeStep; 61 } 62 63 if ( fAngle < btScalar(0.001) ) 64 { 65 // use Taylor's expansions of sync function 66 axis = angvel*( btScalar(0.5)*timeStep-(timeStep*timeStep*timeStep)*(btScalar(0.020833333333))*fAngle*fAngle ); 67 } 68 else 69 { 70 // sync(fAngle) = sin(c*fAngle)/t 71 axis = angvel*( btSin(btScalar(0.5)*fAngle*timeStep)/fAngle ); 72 } 73 btQuaternion dorn (axis.x(),axis.y(),axis.z(),btCos( fAngle*timeStep*btScalar(0.5) )); 74 btQuaternion orn0 = curTrans.getRotation(); 75 76 btQuaternion predictedOrn = dorn * orn0; 77 predictedOrn.normalize(); 78 #endif 79 predictedTransform.setRotation(predictedOrn); 80 } 81 82 static void calculateVelocityQuaternion(const btVector3& pos0,const btVector3& pos1,const btQuaternion& orn0,const btQuaternion& orn1,btScalar timeStep,btVector3& linVel,btVector3& angVel) 83 { 84 linVel = (pos1 - pos0) / timeStep; 85 btVector3 axis; 86 btScalar angle; 87 if (orn0 != orn1) 88 { 89 calculateDiffAxisAngleQuaternion(orn0,orn1,axis,angle); 90 angVel = axis * angle / timeStep; 91 } else 92 { 93 angVel.setValue(0,0,0); 94 } 95 } 96 97 static void calculateDiffAxisAngleQuaternion(const btQuaternion& orn0,const btQuaternion& orn1a,btVector3& axis,btScalar& angle) 98 { 99 btQuaternion orn1 = orn0.nearest(orn1a); 100 btQuaternion dorn = orn1 * orn0.inverse(); 101 angle = dorn.getAngle(); 102 axis = btVector3(dorn.x(),dorn.y(),dorn.z()); 103 axis[3] = btScalar(0.); 104 //check for axis length 105 btScalar len = axis.length2(); 106 if (len < SIMD_EPSILON*SIMD_EPSILON) 107 axis = btVector3(btScalar(1.),btScalar(0.),btScalar(0.)); 108 else 109 axis /= btSqrt(len); 110 } 111 112 static void calculateVelocity(const btTransform& transform0,const btTransform& transform1,btScalar timeStep,btVector3& linVel,btVector3& angVel) 113 { 114 linVel = (transform1.getOrigin() - transform0.getOrigin()) / timeStep; 115 btVector3 axis; 116 btScalar angle; 117 calculateDiffAxisAngle(transform0,transform1,axis,angle); 118 angVel = axis * angle / timeStep; 119 } 120 121 static void calculateDiffAxisAngle(const btTransform& transform0,const btTransform& transform1,btVector3& axis,btScalar& angle) 122 { 123 btMatrix3x3 dmat = transform1.getBasis() * transform0.getBasis().inverse(); 124 btQuaternion dorn; 125 dmat.getRotation(dorn); 126 127 ///floating point inaccuracy can lead to w component > 1..., which breaks 128 dorn.normalize(); 129 130 angle = dorn.getAngle(); 131 axis = btVector3(dorn.x(),dorn.y(),dorn.z()); 132 axis[3] = btScalar(0.); 133 //check for axis length 134 btScalar len = axis.length2(); 135 if (len < SIMD_EPSILON*SIMD_EPSILON) 136 axis = btVector3(btScalar(1.),btScalar(0.),btScalar(0.)); 137 else 138 axis /= btSqrt(len); 139 } 140 141 }; 142 143 144 ///The btConvexSeparatingDistanceUtil can help speed up convex collision detection 145 ///by conservatively updating a cached separating distance/vector instead of re-calculating the closest distance 146 class btConvexSeparatingDistanceUtil 147 { 148 btQuaternion m_ornA; 149 btQuaternion m_ornB; 150 btVector3 m_posA; 151 btVector3 m_posB; 152 153 btVector3 m_separatingNormal; 154 155 btScalar m_boundingRadiusA; 156 btScalar m_boundingRadiusB; 157 btScalar m_separatingDistance; 158 159 public: 160 161 btConvexSeparatingDistanceUtil(btScalar boundingRadiusA,btScalar boundingRadiusB) 162 :m_boundingRadiusA(boundingRadiusA), 163 m_boundingRadiusB(boundingRadiusB), 164 m_separatingDistance(0.f) 165 { 166 } 167 168 btScalar getConservativeSeparatingDistance() 169 { 170 return m_separatingDistance; 171 } 172 173 void updateSeparatingDistance(const btTransform& transA,const btTransform& transB) 174 { 175 const btVector3& toPosA = transA.getOrigin(); 176 const btVector3& toPosB = transB.getOrigin(); 177 btQuaternion toOrnA = transA.getRotation(); 178 btQuaternion toOrnB = transB.getRotation(); 179 180 if (m_separatingDistance>0.f) 181 { 182 183 184 btVector3 linVelA,angVelA,linVelB,angVelB; 185 btTransformUtil::calculateVelocityQuaternion(m_posA,toPosA,m_ornA,toOrnA,btScalar(1.),linVelA,angVelA); 186 btTransformUtil::calculateVelocityQuaternion(m_posB,toPosB,m_ornB,toOrnB,btScalar(1.),linVelB,angVelB); 187 btScalar maxAngularProjectedVelocity = angVelA.length() * m_boundingRadiusA + angVelB.length() * m_boundingRadiusB; 188 btVector3 relLinVel = (linVelB-linVelA); 189 btScalar relLinVelocLength = relLinVel.dot(m_separatingNormal); 190 if (relLinVelocLength<0.f) 191 { 192 relLinVelocLength = 0.f; 193 } 194 195 btScalar projectedMotion = maxAngularProjectedVelocity +relLinVelocLength; 196 m_separatingDistance -= projectedMotion; 197 } 198 199 m_posA = toPosA; 200 m_posB = toPosB; 201 m_ornA = toOrnA; 202 m_ornB = toOrnB; 203 } 204 205 void initSeparatingDistance(const btVector3& separatingVector,btScalar separatingDistance,const btTransform& transA,const btTransform& transB) 206 { 207 m_separatingDistance = separatingDistance; 208 209 if (m_separatingDistance>0.f) 210 { 211 m_separatingNormal = separatingVector; 212 213 const btVector3& toPosA = transA.getOrigin(); 214 const btVector3& toPosB = transB.getOrigin(); 215 btQuaternion toOrnA = transA.getRotation(); 216 btQuaternion toOrnB = transB.getRotation(); 217 m_posA = toPosA; 218 m_posB = toPosB; 219 m_ornA = toOrnA; 220 m_ornB = toOrnB; 221 } 222 } 223 224 }; 225 226 227 #endif //BT_TRANSFORM_UTIL_H 228 229
