diff options
Diffstat (limited to 'extern/bullet2/src/LinearMath/btTransformUtil.h')
-rw-r--r-- | extern/bullet2/src/LinearMath/btTransformUtil.h | 138 |
1 files changed, 138 insertions, 0 deletions
diff --git a/extern/bullet2/src/LinearMath/btTransformUtil.h b/extern/bullet2/src/LinearMath/btTransformUtil.h new file mode 100644 index 00000000000..bc42fd166b6 --- /dev/null +++ b/extern/bullet2/src/LinearMath/btTransformUtil.h @@ -0,0 +1,138 @@ +/* +Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +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. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + +#ifndef SIMD_TRANSFORM_UTIL_H +#define SIMD_TRANSFORM_UTIL_H + +#include "btTransform.h" +#define ANGULAR_MOTION_THRESHOLD btScalar(0.5)*SIMD_HALF_PI + + + +#define SIMDSQRT12 btScalar(0.7071067811865475244008443621048490) + +#define btRecipSqrt(x) ((btScalar)(btScalar(1.0)/btSqrt(btScalar(x)))) /* reciprocal square root */ + +inline btVector3 btAabbSupport(const btVector3& halfExtents,const btVector3& supportDir) +{ + return btVector3(supportDir.x() < btScalar(0.0) ? -halfExtents.x() : halfExtents.x(), + supportDir.y() < btScalar(0.0) ? -halfExtents.y() : halfExtents.y(), + supportDir.z() < btScalar(0.0) ? -halfExtents.z() : halfExtents.z()); +} + + +inline void btPlaneSpace1 (const btVector3& n, btVector3& p, btVector3& q) +{ + if (btFabs(n.z()) > SIMDSQRT12) { + // choose p in y-z plane + btScalar a = n[1]*n[1] + n[2]*n[2]; + btScalar k = btRecipSqrt (a); + p.setValue(0,-n[2]*k,n[1]*k); + // set q = n x p + q.setValue(a*k,-n[0]*p[2],n[0]*p[1]); + } + else { + // choose p in x-y plane + btScalar a = n.x()*n.x() + n.y()*n.y(); + btScalar k = btRecipSqrt (a); + p.setValue(-n.y()*k,n.x()*k,0); + // set q = n x p + q.setValue(-n.z()*p.y(),n.z()*p.x(),a*k); + } +} + + + +/// Utils related to temporal transforms +class btTransformUtil +{ + +public: + + static void integrateTransform(const btTransform& curTrans,const btVector3& linvel,const btVector3& angvel,btScalar timeStep,btTransform& predictedTransform) + { + predictedTransform.setOrigin(curTrans.getOrigin() + linvel * timeStep); +// #define QUATERNION_DERIVATIVE + #ifdef QUATERNION_DERIVATIVE + btQuaternion predictedOrn = curTrans.getRotation(); + predictedOrn += (angvel * predictedOrn) * (timeStep * btScalar(0.5)); + predictedOrn.normalize(); + #else + //exponential map + btVector3 axis; + btScalar fAngle = angvel.length(); + //limit the angular motion + if (fAngle*timeStep > ANGULAR_MOTION_THRESHOLD) + { + fAngle = ANGULAR_MOTION_THRESHOLD / timeStep; + } + + if ( fAngle < btScalar(0.001) ) + { + // use Taylor's expansions of sync function + axis = angvel*( btScalar(0.5)*timeStep-(timeStep*timeStep*timeStep)*(btScalar(0.020833333333))*fAngle*fAngle ); + } + else + { + // sync(fAngle) = sin(c*fAngle)/t + axis = angvel*( btSin(btScalar(0.5)*fAngle*timeStep)/fAngle ); + } + btQuaternion dorn (axis.x(),axis.y(),axis.z(),btCos( fAngle*timeStep*btScalar(0.5) )); + btQuaternion orn0 = curTrans.getRotation(); + + btQuaternion predictedOrn = dorn * orn0; + predictedOrn.normalize(); + #endif + predictedTransform.setRotation(predictedOrn); + } + + static void calculateVelocity(const btTransform& transform0,const btTransform& transform1,btScalar timeStep,btVector3& linVel,btVector3& angVel) + { + linVel = (transform1.getOrigin() - transform0.getOrigin()) / timeStep; + btVector3 axis; + btScalar angle; + calculateDiffAxisAngle(transform0,transform1,axis,angle); + angVel = axis * angle / timeStep; + } + + 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 + + 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); + } + +}; + +#endif //SIMD_TRANSFORM_UTIL_H + |