Back to index

supertuxkart  0.5+dfsg1
btTransformUtil.h
Go to the documentation of this file.
00001 /*
00002 Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans  http://continuousphysics.com/Bullet/
00003 
00004 This software is provided 'as-is', without any express or implied warranty.
00005 In no event will the authors be held liable for any damages arising from the use of this software.
00006 Permission is granted to anyone to use this software for any purpose, 
00007 including commercial applications, and to alter it and redistribute it freely, 
00008 subject to the following restrictions:
00009 
00010 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.
00011 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
00012 3. This notice may not be removed or altered from any source distribution.
00013 */
00014 
00015 
00016 #ifndef SIMD_TRANSFORM_UTIL_H
00017 #define SIMD_TRANSFORM_UTIL_H
00018 
00019 #include "btTransform.h"
00020 #define ANGULAR_MOTION_THRESHOLD btScalar(0.5)*SIMD_HALF_PI
00021 
00022 
00023 
00024 #define SIMDSQRT12 btScalar(0.7071067811865475244008443621048490)
00025 
00026 #define btRecipSqrt(x) ((btScalar)(btScalar(1.0)/btSqrt(btScalar(x))))              /* reciprocal square root */
00027 
00028 SIMD_FORCE_INLINE btVector3 btAabbSupport(const btVector3& halfExtents,const btVector3& supportDir)
00029 {
00030        return btVector3(supportDir.x() < btScalar(0.0) ? -halfExtents.x() : halfExtents.x(),
00031       supportDir.y() < btScalar(0.0) ? -halfExtents.y() : halfExtents.y(),
00032       supportDir.z() < btScalar(0.0) ? -halfExtents.z() : halfExtents.z()); 
00033 }
00034 
00035 
00036 SIMD_FORCE_INLINE void btPlaneSpace1 (const btVector3& n, btVector3& p, btVector3& q)
00037 {
00038   if (btFabs(n.z()) > SIMDSQRT12) {
00039     // choose p in y-z plane
00040     btScalar a = n[1]*n[1] + n[2]*n[2];
00041     btScalar k = btRecipSqrt (a);
00042     p.setValue(0,-n[2]*k,n[1]*k);
00043     // set q = n x p
00044     q.setValue(a*k,-n[0]*p[2],n[0]*p[1]);
00045   }
00046   else {
00047     // choose p in x-y plane
00048     btScalar a = n.x()*n.x() + n.y()*n.y();
00049     btScalar k = btRecipSqrt (a);
00050     p.setValue(-n.y()*k,n.x()*k,0);
00051     // set q = n x p
00052     q.setValue(-n.z()*p.y(),n.z()*p.x(),a*k);
00053   }
00054 }
00055 
00056 
00057 
00059 class btTransformUtil
00060 {
00061 
00062 public:
00063 
00064        static void integrateTransform(const btTransform& curTrans,const btVector3& linvel,const btVector3& angvel,btScalar timeStep,btTransform& predictedTransform)
00065        {
00066               predictedTransform.setOrigin(curTrans.getOrigin() + linvel * timeStep);
00067 //     #define QUATERNION_DERIVATIVE
00068        #ifdef QUATERNION_DERIVATIVE
00069               btQuaternion predictedOrn = curTrans.getRotation();
00070               predictedOrn += (angvel * predictedOrn) * (timeStep * btScalar(0.5));
00071               predictedOrn.normalize();
00072        #else
00073               //Exponential map
00074               //google for "Practical Parameterization of Rotations Using the Exponential Map", F. Sebastian Grassia
00075 
00076               btVector3 axis;
00077               btScalar      fAngle = angvel.length(); 
00078               //limit the angular motion
00079               if (fAngle*timeStep > ANGULAR_MOTION_THRESHOLD)
00080               {
00081                      fAngle = ANGULAR_MOTION_THRESHOLD / timeStep;
00082               }
00083 
00084               if ( fAngle < btScalar(0.001) )
00085               {
00086                      // use Taylor's expansions of sync function
00087                      axis   = angvel*( btScalar(0.5)*timeStep-(timeStep*timeStep*timeStep)*(btScalar(0.020833333333))*fAngle*fAngle );
00088               }
00089               else
00090               {
00091                      // sync(fAngle) = sin(c*fAngle)/t
00092                      axis   = angvel*( btSin(btScalar(0.5)*fAngle*timeStep)/fAngle );
00093               }
00094               btQuaternion dorn (axis.x(),axis.y(),axis.z(),btCos( fAngle*timeStep*btScalar(0.5) ));
00095               btQuaternion orn0 = curTrans.getRotation();
00096 
00097               btQuaternion predictedOrn = dorn * orn0;
00098               predictedOrn.normalize();
00099        #endif
00100               predictedTransform.setRotation(predictedOrn);
00101        }
00102 
00103        static void   calculateVelocity(const btTransform& transform0,const btTransform& transform1,btScalar timeStep,btVector3& linVel,btVector3& angVel)
00104        {
00105               linVel = (transform1.getOrigin() - transform0.getOrigin()) / timeStep;
00106               btVector3 axis;
00107               btScalar  angle;
00108               calculateDiffAxisAngle(transform0,transform1,axis,angle);
00109               angVel = axis * angle / timeStep;
00110        }
00111 
00112        static void calculateDiffAxisAngle(const btTransform& transform0,const btTransform& transform1,btVector3& axis,btScalar& angle)
00113        {
00114        
00115        #ifdef USE_QUATERNION_DIFF
00116               btQuaternion orn0 = transform0.getRotation();
00117               btQuaternion orn1a = transform1.getRotation();
00118               btQuaternion orn1 = orn0.farthest(orn1a);
00119               btQuaternion dorn = orn1 * orn0.inverse();
00120 #else
00121               btMatrix3x3 dmat = transform1.getBasis() * transform0.getBasis().inverse();
00122               btQuaternion dorn;
00123               dmat.getRotation(dorn);
00124 #endif//USE_QUATERNION_DIFF
00125        
00127 
00128               dorn.normalize();
00129               
00130               angle = dorn.getAngle();
00131               axis = btVector3(dorn.x(),dorn.y(),dorn.z());
00132               axis[3] = btScalar(0.);
00133               //check for axis length
00134               btScalar len = axis.length2();
00135               if (len < SIMD_EPSILON*SIMD_EPSILON)
00136                      axis = btVector3(btScalar(1.),btScalar(0.),btScalar(0.));
00137               else
00138                      axis /= btSqrt(len);
00139        }
00140 
00141 };
00142 
00143 #endif //SIMD_TRANSFORM_UTIL_H
00144