diff options
Diffstat (limited to 'extern/bullet2/src/LinearMath/btTransformUtil.h')
-rw-r--r-- | extern/bullet2/src/LinearMath/btTransformUtil.h | 51 |
1 files changed, 22 insertions, 29 deletions
diff --git a/extern/bullet2/src/LinearMath/btTransformUtil.h b/extern/bullet2/src/LinearMath/btTransformUtil.h index 39fa830f4df..bc42fd166b6 100644 --- a/extern/bullet2/src/LinearMath/btTransformUtil.h +++ b/extern/bullet2/src/LinearMath/btTransformUtil.h @@ -16,48 +16,40 @@ subject to the following restrictions: #ifndef SIMD_TRANSFORM_UTIL_H #define SIMD_TRANSFORM_UTIL_H -#include "LinearMath/btTransform.h" -#define ANGULAR_MOTION_THRESHOLD 0.5f*SIMD_HALF_PI +#include "btTransform.h" +#define ANGULAR_MOTION_THRESHOLD btScalar(0.5)*SIMD_HALF_PI #define SIMDSQRT12 btScalar(0.7071067811865475244008443621048490) -#define btRecipSqrt(x) ((float)(1.0f/btSqrt(float(x)))) /* reciprocal square root */ +#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.0f) ? -halfExtents.x() : halfExtents.x(), - supportDir.y() < btScalar(0.0f) ? -halfExtents.y() : halfExtents.y(), - supportDir.z() < btScalar(0.0f) ? -halfExtents.z() : halfExtents.z()); + 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[2]) > SIMDSQRT12) { + 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[0] = 0; - p[1] = -n[2]*k; - p[2] = n[1]*k; + p.setValue(0,-n[2]*k,n[1]*k); // set q = n x p - q[0] = a*k; - q[1] = -n[0]*p[2]; - q[2] = n[0]*p[1]; + q.setValue(a*k,-n[0]*p[2],n[0]*p[1]); } else { // choose p in x-y plane - btScalar a = n[0]*n[0] + n[1]*n[1]; + btScalar a = n.x()*n.x() + n.y()*n.y(); btScalar k = btRecipSqrt (a); - p[0] = -n[1]*k; - p[1] = n[0]*k; - p[2] = 0; + p.setValue(-n.y()*k,n.x()*k,0); // set q = n x p - q[0] = -n[2]*p[1]; - q[1] = n[2]*p[0]; - q[2] = a*k; + q.setValue(-n.z()*p.y(),n.z()*p.x(),a*k); } } @@ -74,9 +66,9 @@ public: predictedTransform.setOrigin(curTrans.getOrigin() + linvel * timeStep); // #define QUATERNION_DERIVATIVE #ifdef QUATERNION_DERIVATIVE - btQuaternion orn = curTrans.getRotation(); - orn += (angvel * orn) * (timeStep * 0.5f); - orn.normalize(); + btQuaternion predictedOrn = curTrans.getRotation(); + predictedOrn += (angvel * predictedOrn) * (timeStep * btScalar(0.5)); + predictedOrn.normalize(); #else //exponential map btVector3 axis; @@ -87,20 +79,21 @@ public: fAngle = ANGULAR_MOTION_THRESHOLD / timeStep; } - if ( fAngle < 0.001f ) + if ( fAngle < btScalar(0.001) ) { // use Taylor's expansions of sync function - axis = angvel*( 0.5f*timeStep-(timeStep*timeStep*timeStep)*(0.020833333333f)*fAngle*fAngle ); + axis = angvel*( btScalar(0.5)*timeStep-(timeStep*timeStep*timeStep)*(btScalar(0.020833333333))*fAngle*fAngle ); } else { // sync(fAngle) = sin(c*fAngle)/t - axis = angvel*( btSin(0.5f*fAngle*timeStep)/fAngle ); + axis = angvel*( btSin(btScalar(0.5)*fAngle*timeStep)/fAngle ); } - btQuaternion dorn (axis.x(),axis.y(),axis.z(),btCos( fAngle*timeStep*0.5f )); + 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); } @@ -130,11 +123,11 @@ public: angle = dorn.getAngle(); axis = btVector3(dorn.x(),dorn.y(),dorn.z()); - axis[3] = 0.f; + axis[3] = btScalar(0.); //check for axis length btScalar len = axis.length2(); if (len < SIMD_EPSILON*SIMD_EPSILON) - axis = btVector3(1.f,0.f,0.f); + axis = btVector3(btScalar(1.),btScalar(0.),btScalar(0.)); else axis /= btSqrt(len); } |