- Timestamp:
- Apr 28, 2011, 7:15:14 AM (14 years ago)
- File:
-
- 1 edited
Legend:
- Unmodified
- Added
- Removed
-
code/trunk/src/external/bullet/LinearMath/btTransformUtil.h
r5781 r8351 22 22 23 23 24 #define SIMDSQRT12 btScalar(0.7071067811865475244008443621048490)25 26 #define btRecipSqrt(x) ((btScalar)(btScalar(1.0)/btSqrt(btScalar(x)))) /* reciprocal square root */27 24 28 25 SIMD_FORCE_INLINE btVector3 btAabbSupport(const btVector3& halfExtents,const btVector3& supportDir) … … 34 31 35 32 36 SIMD_FORCE_INLINE void btPlaneSpace1 (const btVector3& n, btVector3& p, btVector3& q) 37 { 38 if (btFabs(n.z()) > SIMDSQRT12) { 39 // choose p in y-z plane 40 btScalar a = n[1]*n[1] + n[2]*n[2]; 41 btScalar k = btRecipSqrt (a); 42 p.setValue(0,-n[2]*k,n[1]*k); 43 // set q = n x p 44 q.setValue(a*k,-n[0]*p[2],n[0]*p[1]); 45 } 46 else { 47 // choose p in x-y plane 48 btScalar a = n.x()*n.x() + n.y()*n.y(); 49 btScalar k = btRecipSqrt (a); 50 p.setValue(-n.y()*k,n.x()*k,0); 51 // set q = n x p 52 q.setValue(-n.z()*p.y(),n.z()*p.x(),a*k); 53 } 54 } 33 55 34 56 35 … … 118 97 static void calculateDiffAxisAngleQuaternion(const btQuaternion& orn0,const btQuaternion& orn1a,btVector3& axis,btScalar& angle) 119 98 { 120 btQuaternion orn1 = orn0. farthest(orn1a);99 btQuaternion orn1 = orn0.nearest(orn1a); 121 100 btQuaternion dorn = orn1 * orn0.inverse(); 122 ///floating point inaccuracy can lead to w component > 1..., which breaks123 dorn.normalize();124 101 angle = dorn.getAngle(); 125 102 axis = btVector3(dorn.x(),dorn.y(),dorn.z()); … … 210 187 btScalar maxAngularProjectedVelocity = angVelA.length() * m_boundingRadiusA + angVelB.length() * m_boundingRadiusB; 211 188 btVector3 relLinVel = (linVelB-linVelA); 212 btScalar relLinVelocLength = (linVelB-linVelA).dot(m_separatingNormal);189 btScalar relLinVelocLength = relLinVel.dot(m_separatingNormal); 213 190 if (relLinVelocLength<0.f) 214 191 { … … 228 205 void initSeparatingDistance(const btVector3& separatingVector,btScalar separatingDistance,const btTransform& transA,const btTransform& transB) 229 206 { 230 m_separatingNormal = separatingVector;231 207 m_separatingDistance = separatingDistance; 232 233 const btVector3& toPosA = transA.getOrigin(); 234 const btVector3& toPosB = transB.getOrigin(); 235 btQuaternion toOrnA = transA.getRotation(); 236 btQuaternion toOrnB = transB.getRotation(); 237 m_posA = toPosA; 238 m_posB = toPosB; 239 m_ornA = toOrnA; 240 m_ornB = toOrnB; 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 } 241 222 } 242 223
Note: See TracChangeset
for help on using the changeset viewer.