Changeset 2430 for code/branches/physics/src/bullet/LinearMath
- Timestamp:
- Dec 13, 2008, 11:45:51 PM (16 years ago)
- Location:
- code/branches/physics/src/bullet/LinearMath
- Files:
-
- 1 deleted
- 14 edited
-
CMakeLists.txt (modified) (2 diffs)
-
btAabbUtil2.h (modified) (7 diffs)
-
btConvexHull.cpp (modified) (14 diffs)
-
btConvexHull.h (modified) (4 diffs)
-
btMatrix3x3.h (modified) (19 diffs)
-
btPoint3.h (deleted)
-
btQuadWord.h (modified) (1 diff)
-
btQuaternion.h (modified) (19 diffs)
-
btQuickprof.cpp (modified) (3 diffs)
-
btQuickprof.h (modified) (3 diffs)
-
btScalar.h (modified) (5 diffs)
-
btStackAlloc.h (modified) (1 diff)
-
btTransform.h (modified) (20 diffs)
-
btTransformUtil.h (modified) (3 diffs)
-
btVector3.h (modified) (28 diffs)
Legend:
- Unmodified
- Added
- Removed
-
code/branches/physics/src/bullet/LinearMath/CMakeLists.txt
r2192 r2430 1 ADD_LIBRARY(LibLinearMath 2 btAlignedObjectArray.h 3 btList.h 4 btPoolAllocator.h 5 btRandom.h 6 btVector3.h 7 btDefaultMotionState.h 8 btMatrix3x3.h 9 btQuadWord.h 10 btHashMap.h 11 btScalar.h 12 btAabbUtil2.h 13 btConvexHull.h 1 SET(LinearMath_SRCS 14 2 btConvexHull.cpp 15 btMinMax.h16 btQuaternion.h17 btStackAlloc.h18 btGeometryUtil.h19 btMotionState.h20 btTransform.h21 btAlignedAllocator.h22 btIDebugDraw.h23 btPoint3.h24 btQuickprof.h25 btTransformUtil.h26 3 btQuickprof.cpp 27 4 btGeometryUtil.cpp … … 29 6 ) 30 7 8 ADD_LIBRARY(LinearMath ${LinearMath_SRCS}) -
code/branches/physics/src/bullet/LinearMath/btAabbUtil2.h
r2192 r2430 22 22 #include "btMinMax.h" 23 23 24 25 24 26 SIMD_FORCE_INLINE void AabbExpand (btVector3& aabbMin, 25 27 btVector3& aabbMax, … … 31 33 } 32 34 35 /// conservative test for overlap between two aabbs 36 SIMD_FORCE_INLINE bool TestPointAgainstAabb2(const btVector3 &aabbMin1, const btVector3 &aabbMax1, 37 const btVector3 &point) 38 { 39 bool overlap = true; 40 overlap = (aabbMin1.getX() > point.getX() || aabbMax1.getX() < point.getX()) ? false : overlap; 41 overlap = (aabbMin1.getZ() > point.getZ() || aabbMax1.getZ() < point.getZ()) ? false : overlap; 42 overlap = (aabbMin1.getY() > point.getY() || aabbMax1.getY() < point.getY()) ? false : overlap; 43 return overlap; 44 } 45 33 46 34 47 /// conservative test for overlap between two aabbs … … 37 50 { 38 51 bool overlap = true; 39 overlap = (aabbMin1 [0] > aabbMax2[0] || aabbMax1[0] < aabbMin2[0]) ? false : overlap;40 overlap = (aabbMin1 [2] > aabbMax2[2] || aabbMax1[2] < aabbMin2[2]) ? false : overlap;41 overlap = (aabbMin1 [1] > aabbMax2[1] || aabbMax1[1] < aabbMin2[1]) ? false : overlap;52 overlap = (aabbMin1.getX() > aabbMax2.getX() || aabbMax1.getX() < aabbMin2.getX()) ? false : overlap; 53 overlap = (aabbMin1.getZ() > aabbMax2.getZ() || aabbMax1.getZ() < aabbMin2.getZ()) ? false : overlap; 54 overlap = (aabbMin1.getY() > aabbMax2.getY() || aabbMax1.getY() < aabbMin2.getY()) ? false : overlap; 42 55 return overlap; 43 56 } … … 72 85 (p.getZ() > halfExtent.getZ() ? 0x20 : 0x0); 73 86 } 87 74 88 75 89 … … 83 97 { 84 98 btScalar tmax, tymin, tymax, tzmin, tzmax; 85 tmin = (bounds[raySign[0]] [0] - rayFrom[0]) * rayInvDirection[0];86 tmax = (bounds[1-raySign[0]] [0] - rayFrom[0]) * rayInvDirection[0];87 tymin = (bounds[raySign[1]] [1] - rayFrom[1]) * rayInvDirection[1];88 tymax = (bounds[1-raySign[1]] [1] - rayFrom[1]) * rayInvDirection[1];99 tmin = (bounds[raySign[0]].getX() - rayFrom.getX()) * rayInvDirection.getX(); 100 tmax = (bounds[1-raySign[0]].getX() - rayFrom.getX()) * rayInvDirection.getX(); 101 tymin = (bounds[raySign[1]].getY() - rayFrom.getY()) * rayInvDirection.getY(); 102 tymax = (bounds[1-raySign[1]].getY() - rayFrom.getY()) * rayInvDirection.getY(); 89 103 90 104 if ( (tmin > tymax) || (tymin > tmax) ) … … 97 111 tmax = tymax; 98 112 99 tzmin = (bounds[raySign[2]] [2] - rayFrom[2]) * rayInvDirection[2];100 tzmax = (bounds[1-raySign[2]] [2] - rayFrom[2]) * rayInvDirection[2];113 tzmin = (bounds[raySign[2]].getZ() - rayFrom.getZ()) * rayInvDirection.getZ(); 114 tzmax = (bounds[1-raySign[2]].getZ() - rayFrom.getZ()) * rayInvDirection.getZ(); 101 115 102 116 if ( (tmin > tzmax) || (tzmin > tmax) ) … … 197 211 } 198 212 213 #define USE_BANCHLESS 1 214 #ifdef USE_BANCHLESS 215 //This block replaces the block below and uses no branches, and replaces the 8 bit return with a 32 bit return for improved performance (~3x on XBox 360) 216 SIMD_FORCE_INLINE unsigned testQuantizedAabbAgainstQuantizedAabb(const unsigned short int* aabbMin1,const unsigned short int* aabbMax1,const unsigned short int* aabbMin2,const unsigned short int* aabbMax2) 217 { 218 return static_cast<unsigned int>(btSelect((unsigned)((aabbMin1[0] <= aabbMax2[0]) & (aabbMax1[0] >= aabbMin2[0]) 219 & (aabbMin1[2] <= aabbMax2[2]) & (aabbMax1[2] >= aabbMin2[2]) 220 & (aabbMin1[1] <= aabbMax2[1]) & (aabbMax1[1] >= aabbMin2[1])), 221 1, 0)); 222 } 223 #else 224 SIMD_FORCE_INLINE bool testQuantizedAabbAgainstQuantizedAabb(const unsigned short int* aabbMin1,const unsigned short int* aabbMax1,const unsigned short int* aabbMin2,const unsigned short int* aabbMax2) 225 { 226 bool overlap = true; 227 overlap = (aabbMin1[0] > aabbMax2[0] || aabbMax1[0] < aabbMin2[0]) ? false : overlap; 228 overlap = (aabbMin1[2] > aabbMax2[2] || aabbMax1[2] < aabbMin2[2]) ? false : overlap; 229 overlap = (aabbMin1[1] > aabbMax2[1] || aabbMax1[1] < aabbMin2[1]) ? false : overlap; 230 return overlap; 231 } 232 #endif //USE_BANCHLESS 199 233 200 234 #endif -
code/branches/physics/src/bullet/LinearMath/btConvexHull.cpp
r2192 r2430 343 343 } 344 344 345 class Tri;346 347 348 349 class Tri: public int3345 class btHullTriangle; 346 347 348 349 class btHullTriangle : public int3 350 350 { 351 351 public: … … 354 354 int vmax; 355 355 btScalar rise; 356 Tri(int a,int b,int c):int3(a,b,c),n(-1,-1,-1)356 btHullTriangle(int a,int b,int c):int3(a,b,c),n(-1,-1,-1) 357 357 { 358 358 vmax=-1; 359 359 rise = btScalar(0.0); 360 360 } 361 ~ Tri()361 ~btHullTriangle() 362 362 { 363 363 } … … 366 366 367 367 368 int & Tri::neib(int a,int b)368 int &btHullTriangle::neib(int a,int b) 369 369 { 370 370 static int er=-1; … … 380 380 return er; 381 381 } 382 void HullLibrary::b2bfix( Tri* s,Tri*t)382 void HullLibrary::b2bfix(btHullTriangle* s,btHullTriangle*t) 383 383 { 384 384 int i; … … 396 396 } 397 397 398 void HullLibrary::removeb2b( Tri* s,Tri*t)398 void HullLibrary::removeb2b(btHullTriangle* s,btHullTriangle*t) 399 399 { 400 400 b2bfix(s,t); … … 404 404 } 405 405 406 void HullLibrary::checkit( Tri*t)406 void HullLibrary::checkit(btHullTriangle *t) 407 407 { 408 408 (void)t; … … 428 428 } 429 429 430 Tri* HullLibrary::allocateTriangle(int a,int b,int c)431 { 432 void* mem = btAlignedAlloc(sizeof( Tri),16);433 Tri* tr = new (mem)Tri(a,b,c);430 btHullTriangle* HullLibrary::allocateTriangle(int a,int b,int c) 431 { 432 void* mem = btAlignedAlloc(sizeof(btHullTriangle),16); 433 btHullTriangle* tr = new (mem)btHullTriangle(a,b,c); 434 434 tr->id = m_tris.size(); 435 435 m_tris.push_back(tr); … … 438 438 } 439 439 440 void HullLibrary::deAllocateTriangle( Tri* tri)440 void HullLibrary::deAllocateTriangle(btHullTriangle* tri) 441 441 { 442 442 btAssert(m_tris[tri->id]==tri); 443 443 m_tris[tri->id]=NULL; 444 tri->~ Tri();444 tri->~btHullTriangle(); 445 445 btAlignedFree(tri); 446 446 } 447 447 448 448 449 void HullLibrary::extrude( Tri*t0,int v)449 void HullLibrary::extrude(btHullTriangle *t0,int v) 450 450 { 451 451 int3 t= *t0; 452 452 int n = m_tris.size(); 453 Tri* ta = allocateTriangle(v,t[1],t[2]);453 btHullTriangle* ta = allocateTriangle(v,t[1],t[2]); 454 454 ta->n = int3(t0->n[0],n+1,n+2); 455 455 m_tris[t0->n[0]]->neib(t[1],t[2]) = n+0; 456 Tri* tb = allocateTriangle(v,t[2],t[0]);456 btHullTriangle* tb = allocateTriangle(v,t[2],t[0]); 457 457 tb->n = int3(t0->n[1],n+2,n+0); 458 458 m_tris[t0->n[1]]->neib(t[2],t[0]) = n+1; 459 Tri* tc = allocateTriangle(v,t[0],t[1]);459 btHullTriangle* tc = allocateTriangle(v,t[0],t[1]); 460 460 tc->n = int3(t0->n[2],n+0,n+1); 461 461 m_tris[t0->n[2]]->neib(t[0],t[1]) = n+2; … … 470 470 } 471 471 472 Tri* HullLibrary::extrudable(btScalar epsilon)472 btHullTriangle* HullLibrary::extrudable(btScalar epsilon) 473 473 { 474 474 int i; 475 Tri*t=NULL;475 btHullTriangle *t=NULL; 476 476 for(i=0;i<m_tris.size();i++) 477 477 { … … 551 551 552 552 btVector3 center = (verts[p[0]]+verts[p[1]]+verts[p[2]]+verts[p[3]]) / btScalar(4.0); // a valid interior point 553 Tri*t0 = allocateTriangle(p[2],p[3],p[1]); t0->n=int3(2,3,1);554 Tri*t1 = allocateTriangle(p[3],p[2],p[0]); t1->n=int3(3,2,0);555 Tri*t2 = allocateTriangle(p[0],p[1],p[3]); t2->n=int3(0,1,3);556 Tri*t3 = allocateTriangle(p[1],p[0],p[2]); t3->n=int3(1,0,2);553 btHullTriangle *t0 = allocateTriangle(p[2],p[3],p[1]); t0->n=int3(2,3,1); 554 btHullTriangle *t1 = allocateTriangle(p[3],p[2],p[0]); t1->n=int3(3,2,0); 555 btHullTriangle *t2 = allocateTriangle(p[0],p[1],p[3]); t2->n=int3(0,1,3); 556 btHullTriangle *t3 = allocateTriangle(p[1],p[0],p[2]); t3->n=int3(1,0,2); 557 557 isextreme[p[0]]=isextreme[p[1]]=isextreme[p[2]]=isextreme[p[3]]=1; 558 558 checkit(t0);checkit(t1);checkit(t2);checkit(t3); … … 560 560 for(j=0;j<m_tris.size();j++) 561 561 { 562 Tri*t=m_tris[j];562 btHullTriangle *t=m_tris[j]; 563 563 btAssert(t); 564 564 btAssert(t->vmax<0); … … 567 567 t->rise = dot(n,verts[t->vmax]-verts[(*t)[0]]); 568 568 } 569 Tri*te;569 btHullTriangle *te; 570 570 vlimit-=4; 571 571 while(vlimit >0 && ((te=extrudable(epsilon)) != 0)) … … 595 595 if(above(verts,nt,center,btScalar(0.01)*epsilon) || cross(verts[nt[1]]-verts[nt[0]],verts[nt[2]]-verts[nt[1]]).length()< epsilon*epsilon*btScalar(0.1) ) 596 596 { 597 Tri*nb = m_tris[m_tris[j]->n[0]];597 btHullTriangle *nb = m_tris[m_tris[j]->n[0]]; 598 598 btAssert(nb);btAssert(!hasvert(*nb,v));btAssert(nb->id<j); 599 599 extrude(nb,v); … … 604 604 while(j--) 605 605 { 606 Tri*t=m_tris[j];606 btHullTriangle *t=m_tris[j]; 607 607 if(!t) continue; 608 608 if(t->vmax>=0) break; -
code/branches/physics/src/bullet/LinearMath/btConvexHull.h
r2192 r2430 138 138 ConvexH() 139 139 { 140 int i;141 i=0;142 140 } 143 141 ~ConvexH() 144 142 { 145 int i;146 i=0;147 143 } 148 144 btAlignedObjectArray<btVector3> vertices; … … 189 185 { 190 186 191 btAlignedObjectArray<class Tri*> m_tris;187 btAlignedObjectArray<class btHullTriangle*> m_tris; 192 188 193 189 public: … … 204 200 bool ComputeHull(unsigned int vcount,const btVector3 *vertices,PHullResult &result,unsigned int vlimit); 205 201 206 class Tri* allocateTriangle(int a,int b,int c);207 void deAllocateTriangle( Tri*);208 void b2bfix( Tri* s,Tri*t);209 210 void removeb2b( Tri* s,Tri*t);211 212 void checkit( Tri*t);213 214 Tri* extrudable(btScalar epsilon);202 class btHullTriangle* allocateTriangle(int a,int b,int c); 203 void deAllocateTriangle(btHullTriangle*); 204 void b2bfix(btHullTriangle* s,btHullTriangle*t); 205 206 void removeb2b(btHullTriangle* s,btHullTriangle*t); 207 208 void checkit(btHullTriangle *t); 209 210 btHullTriangle* extrudable(btScalar epsilon); 215 211 216 212 int calchull(btVector3 *verts,int verts_count, TUIntArray& tris_out, int &tris_count,int vlimit); … … 222 218 class ConvexH* ConvexHCrop(ConvexH& convex,const btPlane& slice); 223 219 224 void extrude(class Tri* t0,int v);220 void extrude(class btHullTriangle* t0,int v); 225 221 226 222 ConvexH* test_cube(); -
code/branches/physics/src/bullet/LinearMath/btMatrix3x3.h
r2192 r2430 24 24 25 25 26 / //The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with btQuaternion, btTransform and btVector3.27 ///Make sure to only include a pure orthogonal matrix without scaling. 26 /**@brief The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with btQuaternion, btTransform and btVector3. 27 * Make sure to only include a pure orthogonal matrix without scaling. */ 28 28 class btMatrix3x3 { 29 29 public: 30 /** @brief No initializaion constructor */ 30 31 btMatrix3x3 () {} 31 32 32 33 // explicit btMatrix3x3(const btScalar *m) { setFromOpenGLSubMatrix(m); } 33 34 35 /**@brief Constructor from Quaternion */ 34 36 explicit btMatrix3x3(const btQuaternion& q) { setRotation(q); } 35 37 /* … … 40 42 } 41 43 */ 44 /** @brief Constructor with row major formatting */ 42 45 btMatrix3x3(const btScalar& xx, const btScalar& xy, const btScalar& xz, 43 46 const btScalar& yx, const btScalar& yy, const btScalar& yz, … … 48 51 zx, zy, zz); 49 52 } 50 53 /** @brief Copy constructor */ 51 54 SIMD_FORCE_INLINE btMatrix3x3 (const btMatrix3x3& other) 52 55 { … … 55 58 m_el[2] = other.m_el[2]; 56 59 } 57 60 /** @brief Assignment Operator */ 58 61 SIMD_FORCE_INLINE btMatrix3x3& operator=(const btMatrix3x3& other) 59 62 { … … 64 67 } 65 68 69 /** @brief Get a column of the matrix as a vector 70 * @param i Column number 0 indexed */ 66 71 SIMD_FORCE_INLINE btVector3 getColumn(int i) const 67 72 { … … 70 75 71 76 72 77 /** @brief Get a row of the matrix as a vector 78 * @param i Row number 0 indexed */ 73 79 SIMD_FORCE_INLINE const btVector3& getRow(int i) const 74 80 { 81 btFullAssert(0 <= i && i < 3); 75 82 return m_el[i]; 76 83 } 77 84 78 85 /** @brief Get a mutable reference to a row of the matrix as a vector 86 * @param i Row number 0 indexed */ 79 87 SIMD_FORCE_INLINE btVector3& operator[](int i) 80 88 { … … 83 91 } 84 92 93 /** @brief Get a const reference to a row of the matrix as a vector 94 * @param i Row number 0 indexed */ 85 95 SIMD_FORCE_INLINE const btVector3& operator[](int i) const 86 96 { … … 89 99 } 90 100 101 /** @brief Multiply by the target matrix on the right 102 * @param m Rotation matrix to be applied 103 * Equivilant to this = this * m */ 91 104 btMatrix3x3& operator*=(const btMatrix3x3& m); 92 105 93 106 /** @brief Set from a carray of btScalars 107 * @param m A pointer to the beginning of an array of 9 btScalars */ 94 108 void setFromOpenGLSubMatrix(const btScalar *m) 95 109 { … … 99 113 100 114 } 101 115 /** @brief Set the values of the matrix explicitly (row major) 116 * @param xx Top left 117 * @param xy Top Middle 118 * @param xz Top Right 119 * @param yx Middle Left 120 * @param yy Middle Middle 121 * @param yz Middle Right 122 * @param zx Bottom Left 123 * @param zy Bottom Middle 124 * @param zz Bottom Right*/ 102 125 void setValue(const btScalar& xx, const btScalar& xy, const btScalar& xz, 103 126 const btScalar& yx, const btScalar& yy, const btScalar& yz, … … 108 131 m_el[2].setValue(zx,zy,zz); 109 132 } 110 133 134 /** @brief Set the matrix from a quaternion 135 * @param q The Quaternion to match */ 111 136 void setRotation(const btQuaternion& q) 112 137 { … … 124 149 125 150 126 151 /** @brief Set the matrix from euler angles using YPR around YXZ respectively 152 * @param yaw Yaw about Y axis 153 * @param pitch Pitch about X axis 154 * @param roll Roll about Z axis 155 */ 127 156 void setEulerYPR(const btScalar& yaw, const btScalar& pitch, const btScalar& roll) 128 157 { 129 130 btScalar cy(btCos(yaw)); 131 btScalar sy(btSin(yaw)); 132 btScalar cp(btCos(pitch)); 133 btScalar sp(btSin(pitch)); 134 btScalar cr(btCos(roll)); 135 btScalar sr(btSin(roll)); 136 btScalar cc = cy * cr; 137 btScalar cs = cy * sr; 138 btScalar sc = sy * cr; 139 btScalar ss = sy * sr; 140 setValue(cc - sp * ss, -cs - sp * sc, -sy * cp, 141 cp * sr, cp * cr, -sp, 142 sc + sp * cs, -ss + sp * cc, cy * cp); 143 144 } 145 146 /** 147 * setEulerZYX 148 * @param euler a const reference to a btVector3 of euler angles 158 setEulerZYX(roll, pitch, yaw); 159 } 160 161 /** @brief Set the matrix from euler angles YPR around ZYX axes 162 * @param eulerX Roll about X axis 163 * @param eulerY Pitch around Y axis 164 * @param eulerZ Yaw aboud Z axis 165 * 149 166 * These angles are used to produce a rotation matrix. The euler 150 167 * angles are applied in ZYX order. I.e a vector is first rotated 151 168 * about X then Y and then Z 152 169 **/ 153 154 void setEulerZYX(btScalar eulerX,btScalar eulerY,btScalar eulerZ) { 170 void setEulerZYX(btScalar eulerX,btScalar eulerY,btScalar eulerZ) { 171 ///@todo proposed to reverse this since it's labeled zyx but takes arguments xyz and it will match all other parts of the code 155 172 btScalar ci ( btCos(eulerX)); 156 173 btScalar cj ( btCos(eulerY)); … … 169 186 } 170 187 188 /**@brief Set the matrix to the identity */ 171 189 void setIdentity() 172 190 { … … 175 193 btScalar(0.0), btScalar(0.0), btScalar(1.0)); 176 194 } 177 195 /**@brief Fill the values of the matrix into a 9 element array 196 * @param m The array to be filled */ 178 197 void getOpenGLSubMatrix(btScalar *m) const 179 198 { … … 192 211 } 193 212 213 /**@brief Get the matrix represented as a quaternion 214 * @param q The quaternion which will be set */ 194 215 void getRotation(btQuaternion& q) const 195 216 { … … 225 246 q.setValue(temp[0],temp[1],temp[2],temp[3]); 226 247 } 227 228 void getEuler(btScalar& yaw, btScalar& pitch, btScalar& roll) const 248 249 /**@brief Get the matrix represented as euler angles around YXZ, roundtrip with setEulerYPR 250 * @param yaw Yaw around Y axis 251 * @param pitch Pitch around X axis 252 * @param roll around Z axis */ 253 void getEulerYPR(btScalar& yaw, btScalar& pitch, btScalar& roll) const 229 254 { 230 255 231 if (btScalar(m_el[1].z()) < btScalar(1)) 232 { 233 if (btScalar(m_el[1].z()) > -btScalar(1)) 234 { 235 yaw = btScalar(btAtan2(m_el[1].x(), m_el[0].x())); 236 pitch = btScalar(btAsin(-m_el[1].y())); 237 roll = btScalar(btAtan2(m_el[2].y(), m_el[2].z())); 238 } 239 else 240 { 241 yaw = btScalar(-btAtan2(-m_el[0].y(), m_el[0].z())); 242 pitch = SIMD_HALF_PI; 243 roll = btScalar(0.0); 244 } 245 } 246 else 247 { 248 yaw = btScalar(btAtan2(-m_el[0].y(), m_el[0].z())); 249 pitch = -SIMD_HALF_PI; 250 roll = btScalar(0.0); 251 } 252 } 253 254 255 256 // first use the normal calculus 257 yaw = btScalar(btAtan2(m_el[1].x(), m_el[0].x())); 258 pitch = btScalar(btAsin(-m_el[2].x())); 259 roll = btScalar(btAtan2(m_el[2].y(), m_el[2].z())); 260 261 // on pitch = +/-HalfPI 262 if (btFabs(pitch)==SIMD_HALF_PI) 263 { 264 if (yaw>0) 265 yaw-=SIMD_PI; 266 else 267 yaw+=SIMD_PI; 268 269 if (roll>0) 270 roll-=SIMD_PI; 271 else 272 roll+=SIMD_PI; 273 } 274 }; 275 276 277 /**@brief Get the matrix represented as euler angles around ZYX 278 * @param yaw Yaw around X axis 279 * @param pitch Pitch around Y axis 280 * @param roll around X axis 281 * @param solution_number Which solution of two possible solutions ( 1 or 2) are possible values*/ 282 void getEulerZYX(btScalar& yaw, btScalar& pitch, btScalar& roll, unsigned int solution_number = 1) const 283 { 284 struct Euler{btScalar yaw, pitch, roll;}; 285 Euler euler_out; 286 Euler euler_out2; //second solution 287 //get the pointer to the raw data 288 289 // Check that pitch is not at a singularity 290 if (btFabs(m_el[2].x()) >= 1) 291 { 292 euler_out.yaw = 0; 293 euler_out2.yaw = 0; 294 295 // From difference of angles formula 296 btScalar delta = btAtan2(m_el[0].x(),m_el[0].z()); 297 if (m_el[2].x() > 0) //gimbal locked up 298 { 299 euler_out.pitch = SIMD_PI / btScalar(2.0); 300 euler_out2.pitch = SIMD_PI / btScalar(2.0); 301 euler_out.roll = euler_out.pitch + delta; 302 euler_out2.roll = euler_out.pitch + delta; 303 } 304 else // gimbal locked down 305 { 306 euler_out.pitch = -SIMD_PI / btScalar(2.0); 307 euler_out2.pitch = -SIMD_PI / btScalar(2.0); 308 euler_out.roll = -euler_out.pitch + delta; 309 euler_out2.roll = -euler_out.pitch + delta; 310 } 311 } 312 else 313 { 314 euler_out.pitch = - btAsin(m_el[2].x()); 315 euler_out2.pitch = SIMD_PI - euler_out.pitch; 316 317 euler_out.roll = btAtan2(m_el[2].y()/btCos(euler_out.pitch), 318 m_el[2].z()/btCos(euler_out.pitch)); 319 euler_out2.roll = btAtan2(m_el[2].y()/btCos(euler_out2.pitch), 320 m_el[2].z()/btCos(euler_out2.pitch)); 321 322 euler_out.yaw = btAtan2(m_el[1].x()/btCos(euler_out.pitch), 323 m_el[0].x()/btCos(euler_out.pitch)); 324 euler_out2.yaw = btAtan2(m_el[1].x()/btCos(euler_out2.pitch), 325 m_el[0].x()/btCos(euler_out2.pitch)); 326 } 327 328 if (solution_number == 1) 329 { 330 yaw = euler_out.yaw; 331 pitch = euler_out.pitch; 332 roll = euler_out.roll; 333 } 334 else 335 { 336 yaw = euler_out2.yaw; 337 pitch = euler_out2.pitch; 338 roll = euler_out2.roll; 339 } 340 } 341 342 /**@brief Create a scaled copy of the matrix 343 * @param s Scaling vector The elements of the vector will scale each column */ 256 344 257 345 btMatrix3x3 scaled(const btVector3& s) const … … 262 350 } 263 351 352 /**@brief Return the determinant of the matrix */ 264 353 btScalar determinant() const; 354 /**@brief Return the adjoint of the matrix */ 265 355 btMatrix3x3 adjoint() const; 356 /**@brief Return the matrix with all values non negative */ 266 357 btMatrix3x3 absolute() const; 358 /**@brief Return the transpose of the matrix */ 267 359 btMatrix3x3 transpose() const; 360 /**@brief Return the inverse of the matrix */ 268 361 btMatrix3x3 inverse() const; 269 362 … … 285 378 286 379 287 ///diagonalizes this matrix by the Jacobi method. rot stores the rotation 288 ///from the coordinate system in which the matrix is diagonal to the original 289 ///coordinate system, i.e., old_this = rot * new_this * rot^T. The iteration 290 ///stops when all off-diagonal elements are less than the threshold multiplied 291 ///by the sum of the absolute values of the diagonal, or when maxSteps have 292 ///been executed. Note that this matrix is assumed to be symmetric. 380 /**@brief diagonalizes this matrix by the Jacobi method. 381 * @param rot stores the rotation from the coordinate system in which the matrix is diagonal to the original 382 * coordinate system, i.e., old_this = rot * new_this * rot^T. 383 * @param threshold See iteration 384 * @param iteration The iteration stops when all off-diagonal elements are less than the threshold multiplied 385 * by the sum of the absolute values of the diagonal, or when maxSteps have been executed. 386 * 387 * Note that this matrix is assumed to be symmetric. 388 */ 293 389 void diagonalize(btMatrix3x3& rot, btScalar threshold, int maxSteps) 294 390 { … … 372 468 373 469 protected: 470 /**@brief Calculate the matrix cofactor 471 * @param r1 The first row to use for calculating the cofactor 472 * @param c1 The first column to use for calculating the cofactor 473 * @param r1 The second row to use for calculating the cofactor 474 * @param c1 The second column to use for calculating the cofactor 475 * See http://en.wikipedia.org/wiki/Cofactor_(linear_algebra) for more details 476 */ 374 477 btScalar cofac(int r1, int c1, int r2, int c2) const 375 478 { 376 479 return m_el[r1][c1] * m_el[r2][c2] - m_el[r1][c2] * m_el[r2][c1]; 377 480 } 378 481 ///Data storage for the matrix, each vector is a row of the matrix 379 482 btVector3 m_el[3]; 380 483 }; … … 495 598 */ 496 599 600 /**@brief Equality operator between two matrices 601 * It will test all elements are equal. */ 497 602 SIMD_FORCE_INLINE bool operator==(const btMatrix3x3& m1, const btMatrix3x3& m2) 498 603 { -
code/branches/physics/src/bullet/LinearMath/btQuadWord.h
r2192 r2430 19 19 #include "btScalar.h" 20 20 #include "btMinMax.h" 21 #include <math.h>22 21 23 22 23 #if defined (__CELLOS_LV2) && defined (__SPU__) 24 #include <altivec.h> 25 #endif 24 26 25 ///The btQuadWordStorage class is base class for btVector3 and btQuaternion. 26 ///Some issues under PS3 Linux with IBM 2.1 SDK, gcc compiler prevent from using aligned quadword. todo: look into this 27 ///ATTRIBUTE_ALIGNED16(class) btQuadWordStorage 27 /**@brief The btQuadWordStorage class is base class for btVector3 and btQuaternion. 28 * Some issues under PS3 Linux with IBM 2.1 SDK, gcc compiler prevent from using aligned quadword. 29 */ 30 #ifndef USE_LIBSPE2 31 ATTRIBUTE_ALIGNED16(class) btQuadWordStorage 32 #else 28 33 class btQuadWordStorage 34 #endif 29 35 { 30 36 protected: 31 37 32 btScalar m_x; 33 btScalar m_y;34 btScalar m_z;35 btScalar m_unusedW;36 38 #if defined (__SPU__) && defined (__CELLOS_LV2__) 39 union { 40 vec_float4 mVec128; 41 btScalar m_floats[4]; 42 }; 37 43 public: 44 vec_float4 get128() const 45 { 46 return mVec128; 47 } 48 #else //__CELLOS_LV2__ __SPU__ 49 btScalar m_floats[4]; 50 #endif //__CELLOS_LV2__ __SPU__ 38 51 39 52 }; 40 53 41 42 ///btQuadWord is base-class for vectors, points 54 /** @brief The btQuadWord is base-class for vectors, points */ 43 55 class btQuadWord : public btQuadWordStorage 44 56 { 45 57 public: 46 47 // SIMD_FORCE_INLINE btScalar& operator[](int i) { return (&m_x)[i]; } 48 // SIMD_FORCE_INLINE const btScalar& operator[](int i) const { return (&m_x)[i]; } 58 49 59 50 SIMD_FORCE_INLINE const btScalar& getX() const { return m_x; } 60 /**@brief Return the x value */ 61 SIMD_FORCE_INLINE const btScalar& getX() const { return m_floats[0]; } 62 /**@brief Return the y value */ 63 SIMD_FORCE_INLINE const btScalar& getY() const { return m_floats[1]; } 64 /**@brief Return the z value */ 65 SIMD_FORCE_INLINE const btScalar& getZ() const { return m_floats[2]; } 66 /**@brief Set the x value */ 67 SIMD_FORCE_INLINE void setX(btScalar x) { m_floats[0] = x;}; 68 /**@brief Set the y value */ 69 SIMD_FORCE_INLINE void setY(btScalar y) { m_floats[1] = y;}; 70 /**@brief Set the z value */ 71 SIMD_FORCE_INLINE void setZ(btScalar z) { m_floats[2] = z;}; 72 /**@brief Set the w value */ 73 SIMD_FORCE_INLINE void setW(btScalar w) { m_floats[3] = w;}; 74 /**@brief Return the x value */ 75 SIMD_FORCE_INLINE const btScalar& x() const { return m_floats[0]; } 76 /**@brief Return the y value */ 77 SIMD_FORCE_INLINE const btScalar& y() const { return m_floats[1]; } 78 /**@brief Return the z value */ 79 SIMD_FORCE_INLINE const btScalar& z() const { return m_floats[2]; } 80 /**@brief Return the w value */ 81 SIMD_FORCE_INLINE const btScalar& w() const { return m_floats[3]; } 51 82 52 SIMD_FORCE_INLINE const btScalar& getY() const { return m_y; } 83 //SIMD_FORCE_INLINE btScalar& operator[](int i) { return (&m_floats[0])[i]; } 84 //SIMD_FORCE_INLINE const btScalar& operator[](int i) const { return (&m_floats[0])[i]; } 85 ///operator btScalar*() replaces operator[], using implicit conversion. We added operator != and operator == to avoid pointer comparisons. 86 SIMD_FORCE_INLINE operator btScalar *() { return &m_floats[0]; } 87 SIMD_FORCE_INLINE operator const btScalar *() const { return &m_floats[0]; } 53 88 54 SIMD_FORCE_INLINE const btScalar& getZ() const { return m_z; } 89 SIMD_FORCE_INLINE bool operator==(const btQuadWord& other) const 90 { 91 return ((m_floats[3]==other.m_floats[3]) && (m_floats[2]==other.m_floats[2]) && (m_floats[1]==other.m_floats[1]) && (m_floats[0]==other.m_floats[0])); 92 } 55 93 56 SIMD_FORCE_INLINE void setX(btScalar x) { m_x = x;}; 94 SIMD_FORCE_INLINE bool operator!=(const btQuadWord& other) const 95 { 96 return !(*this == other); 97 } 57 98 58 SIMD_FORCE_INLINE void setY(btScalar y) { m_y = y;}; 59 60 SIMD_FORCE_INLINE void setZ(btScalar z) { m_z = z;}; 61 62 SIMD_FORCE_INLINE void setW(btScalar w) { m_unusedW = w;}; 63 64 SIMD_FORCE_INLINE const btScalar& x() const { return m_x; } 65 66 SIMD_FORCE_INLINE const btScalar& y() const { return m_y; } 67 68 SIMD_FORCE_INLINE const btScalar& z() const { return m_z; } 69 70 SIMD_FORCE_INLINE const btScalar& w() const { return m_unusedW; } 71 72 73 SIMD_FORCE_INLINE operator btScalar *() { return &m_x; } 74 SIMD_FORCE_INLINE operator const btScalar *() const { return &m_x; } 75 76 77 99 /**@brief Set x,y,z and zero w 100 * @param x Value of x 101 * @param y Value of y 102 * @param z Value of z 103 */ 78 104 SIMD_FORCE_INLINE void setValue(const btScalar& x, const btScalar& y, const btScalar& z) 79 105 { 80 m_ x=x;81 m_ y=y;82 m_ z=z;83 m_ unusedW= 0.f;106 m_floats[0]=x; 107 m_floats[1]=y; 108 m_floats[2]=z; 109 m_floats[3] = 0.f; 84 110 } 85 111 86 112 /* void getValue(btScalar *m) const 87 113 { 88 m[0] = m_ x;89 m[1] = m_ y;90 m[2] = m_ z;114 m[0] = m_floats[0]; 115 m[1] = m_floats[1]; 116 m[2] = m_floats[2]; 91 117 } 92 118 */ 119 /**@brief Set the values 120 * @param x Value of x 121 * @param y Value of y 122 * @param z Value of z 123 * @param w Value of w 124 */ 93 125 SIMD_FORCE_INLINE void setValue(const btScalar& x, const btScalar& y, const btScalar& z,const btScalar& w) 94 126 { 95 m_ x=x;96 m_ y=y;97 m_ z=z;98 m_ unusedW=w;127 m_floats[0]=x; 128 m_floats[1]=y; 129 m_floats[2]=z; 130 m_floats[3]=w; 99 131 } 100 132 /**@brief No initialization constructor */ 101 133 SIMD_FORCE_INLINE btQuadWord() 102 // :m_ x(btScalar(0.)),m_y(btScalar(0.)),m_z(btScalar(0.)),m_unusedW(btScalar(0.))134 // :m_floats[0](btScalar(0.)),m_floats[1](btScalar(0.)),m_floats[2](btScalar(0.)),m_floats[3](btScalar(0.)) 103 135 { 104 136 } 105 137 /**@brief Copy constructor */ 106 138 SIMD_FORCE_INLINE btQuadWord(const btQuadWordStorage& q) 107 139 { 108 140 *((btQuadWordStorage*)this) = q; 109 141 } 110 142 /**@brief Three argument constructor (zeros w) 143 * @param x Value of x 144 * @param y Value of y 145 * @param z Value of z 146 */ 111 147 SIMD_FORCE_INLINE btQuadWord(const btScalar& x, const btScalar& y, const btScalar& z) 112 148 { 113 m_ x = x, m_y = y, m_z = z, m_unusedW= 0.0f;149 m_floats[0] = x, m_floats[1] = y, m_floats[2] = z, m_floats[3] = 0.0f; 114 150 } 115 151 152 /**@brief Initializing constructor 153 * @param x Value of x 154 * @param y Value of y 155 * @param z Value of z 156 * @param w Value of w 157 */ 116 158 SIMD_FORCE_INLINE btQuadWord(const btScalar& x, const btScalar& y, const btScalar& z,const btScalar& w) 117 159 { 118 m_ x = x, m_y = y, m_z = z, m_unusedW= w;160 m_floats[0] = x, m_floats[1] = y, m_floats[2] = z, m_floats[3] = w; 119 161 } 120 162 121 163 /**@brief Set each element to the max of the current values and the values of another btQuadWord 164 * @param other The other btQuadWord to compare with 165 */ 122 166 SIMD_FORCE_INLINE void setMax(const btQuadWord& other) 123 167 { 124 btSetMax(m_ x, other.m_x);125 btSetMax(m_ y, other.m_y);126 btSetMax(m_ z, other.m_z);127 btSetMax(m_ unusedW, other.m_unusedW);168 btSetMax(m_floats[0], other.m_floats[0]); 169 btSetMax(m_floats[1], other.m_floats[1]); 170 btSetMax(m_floats[2], other.m_floats[2]); 171 btSetMax(m_floats[3], other.m_floats[3]); 128 172 } 129 173 /**@brief Set each element to the min of the current values and the values of another btQuadWord 174 * @param other The other btQuadWord to compare with 175 */ 130 176 SIMD_FORCE_INLINE void setMin(const btQuadWord& other) 131 177 { 132 btSetMin(m_ x, other.m_x);133 btSetMin(m_ y, other.m_y);134 btSetMin(m_ z, other.m_z);135 btSetMin(m_ unusedW, other.m_unusedW);178 btSetMin(m_floats[0], other.m_floats[0]); 179 btSetMin(m_floats[1], other.m_floats[1]); 180 btSetMin(m_floats[2], other.m_floats[2]); 181 btSetMin(m_floats[3], other.m_floats[3]); 136 182 } 137 183 -
code/branches/physics/src/bullet/LinearMath/btQuaternion.h
r2192 r2430 18 18 #define SIMD__QUATERNION_H_ 19 19 20 20 21 #include "btVector3.h" 21 22 22 / //The btQuaternion implements quaternion to perform linear algebra rotations in combination with btMatrix3x3, btVector3 and btTransform.23 /**@brief The btQuaternion implements quaternion to perform linear algebra rotations in combination with btMatrix3x3, btVector3 and btTransform. */ 23 24 class btQuaternion : public btQuadWord { 24 25 public: 26 /**@brief No initialization constructor */ 25 27 btQuaternion() {} 26 28 27 29 // template <typename btScalar> 28 30 // explicit Quaternion(const btScalar *v) : Tuple4<btScalar>(v) {} 29 31 /**@brief Constructor from scalars */ 30 32 btQuaternion(const btScalar& x, const btScalar& y, const btScalar& z, const btScalar& w) 31 33 : btQuadWord(x, y, z, w) 32 34 {} 33 35 /**@brief Axis angle Constructor 36 * @param axis The axis which the rotation is around 37 * @param angle The magnitude of the rotation around the angle (Radians) */ 34 38 btQuaternion(const btVector3& axis, const btScalar& angle) 35 39 { 36 40 setRotation(axis, angle); 37 41 } 38 42 /**@brief Constructor from Euler angles 43 * @param yaw Angle around Y unless BT_EULER_DEFAULT_ZYX defined then Z 44 * @param pitch Angle around X unless BT_EULER_DEFAULT_ZYX defined then Y 45 * @param roll Angle around Z unless BT_EULER_DEFAULT_ZYX defined then X */ 39 46 btQuaternion(const btScalar& yaw, const btScalar& pitch, const btScalar& roll) 40 47 { 48 #ifndef BT_EULER_DEFAULT_ZYX 41 49 setEuler(yaw, pitch, roll); 42 } 43 50 #else 51 setEulerZYX(yaw, pitch, roll); 52 #endif 53 } 54 /**@brief Set the rotation using axis angle notation 55 * @param axis The axis around which to rotate 56 * @param angle The magnitude of the rotation in Radians */ 44 57 void setRotation(const btVector3& axis, const btScalar& angle) 45 58 { … … 50 63 btCos(angle * btScalar(0.5))); 51 64 } 52 65 /**@brief Set the quaternion using Euler angles 66 * @param yaw Angle around Y 67 * @param pitch Angle around X 68 * @param roll Angle around Z */ 53 69 void setEuler(const btScalar& yaw, const btScalar& pitch, const btScalar& roll) 54 70 { … … 67 83 cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw); 68 84 } 69 70 btQuaternion& operator+=(const btQuaternion& q) 71 { 72 m_x += q.x(); m_y += q.y(); m_z += q.z(); m_unusedW += q.m_unusedW; 85 /**@brief Set the quaternion using euler angles 86 * @param yaw Angle around Z 87 * @param pitch Angle around Y 88 * @param roll Angle around X */ 89 void setEulerZYX(const btScalar& yaw, const btScalar& pitch, const btScalar& roll) 90 { 91 btScalar halfYaw = btScalar(yaw) * btScalar(0.5); 92 btScalar halfPitch = btScalar(pitch) * btScalar(0.5); 93 btScalar halfRoll = btScalar(roll) * btScalar(0.5); 94 btScalar cosYaw = btCos(halfYaw); 95 btScalar sinYaw = btSin(halfYaw); 96 btScalar cosPitch = btCos(halfPitch); 97 btScalar sinPitch = btSin(halfPitch); 98 btScalar cosRoll = btCos(halfRoll); 99 btScalar sinRoll = btSin(halfRoll); 100 setValue(sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw, //x 101 cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw, //y 102 cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw, //z 103 cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw); //formerly yzx 104 } 105 /**@brief Add two quaternions 106 * @param q The quaternion to add to this one */ 107 SIMD_FORCE_INLINE btQuaternion& operator+=(const btQuaternion& q) 108 { 109 m_floats[0] += q.x(); m_floats[1] += q.y(); m_floats[2] += q.z(); m_floats[3] += q.m_floats[3]; 73 110 return *this; 74 111 } 75 112 113 /**@brief Subtract out a quaternion 114 * @param q The quaternion to subtract from this one */ 76 115 btQuaternion& operator-=(const btQuaternion& q) 77 116 { 78 m_ x -= q.x(); m_y -= q.y(); m_z -= q.z(); m_unusedW -= q.m_unusedW;117 m_floats[0] -= q.x(); m_floats[1] -= q.y(); m_floats[2] -= q.z(); m_floats[3] -= q.m_floats[3]; 79 118 return *this; 80 119 } 81 120 121 /**@brief Scale this quaternion 122 * @param s The scalar to scale by */ 82 123 btQuaternion& operator*=(const btScalar& s) 83 124 { 84 m_ x *= s; m_y *= s; m_z *= s; m_unusedW*= s;125 m_floats[0] *= s; m_floats[1] *= s; m_floats[2] *= s; m_floats[3] *= s; 85 126 return *this; 86 127 } 87 128 88 129 /**@brief Multiply this quaternion by q on the right 130 * @param q The other quaternion 131 * Equivilant to this = this * q */ 89 132 btQuaternion& operator*=(const btQuaternion& q) 90 133 { 91 setValue(m_ unusedW * q.x() + m_x * q.m_unusedW + m_y * q.z() - m_z* q.y(),92 m_ unusedW * q.y() + m_y * q.m_unusedW + m_z * q.x() - m_x* q.z(),93 m_ unusedW * q.z() + m_z * q.m_unusedW + m_x * q.y() - m_y* q.x(),94 m_ unusedW * q.m_unusedW - m_x * q.x() - m_y * q.y() - m_z* q.z());134 setValue(m_floats[3] * q.x() + m_floats[0] * q.m_floats[3] + m_floats[1] * q.z() - m_floats[2] * q.y(), 135 m_floats[3] * q.y() + m_floats[1] * q.m_floats[3] + m_floats[2] * q.x() - m_floats[0] * q.z(), 136 m_floats[3] * q.z() + m_floats[2] * q.m_floats[3] + m_floats[0] * q.y() - m_floats[1] * q.x(), 137 m_floats[3] * q.m_floats[3] - m_floats[0] * q.x() - m_floats[1] * q.y() - m_floats[2] * q.z()); 95 138 return *this; 96 139 } 97 140 /**@brief Return the dot product between this quaternion and another 141 * @param q The other quaternion */ 98 142 btScalar dot(const btQuaternion& q) const 99 143 { 100 return m_x * q.x() + m_y * q.y() + m_z * q.z() + m_unusedW * q.m_unusedW; 101 } 102 144 return m_floats[0] * q.x() + m_floats[1] * q.y() + m_floats[2] * q.z() + m_floats[3] * q.m_floats[3]; 145 } 146 147 /**@brief Return the length squared of the quaternion */ 103 148 btScalar length2() const 104 149 { … … 106 151 } 107 152 153 /**@brief Return the length of the quaternion */ 108 154 btScalar length() const 109 155 { … … 111 157 } 112 158 159 /**@brief Normalize the quaternion 160 * Such that x^2 + y^2 + z^2 +w^2 = 1 */ 113 161 btQuaternion& normalize() 114 162 { … … 116 164 } 117 165 166 /**@brief Return a scaled version of this quaternion 167 * @param s The scale factor */ 118 168 SIMD_FORCE_INLINE btQuaternion 119 169 operator*(const btScalar& s) const 120 170 { 121 return btQuaternion(x() * s, y() * s, z() * s, m_unusedW * s); 122 } 123 124 125 171 return btQuaternion(x() * s, y() * s, z() * s, m_floats[3] * s); 172 } 173 174 175 /**@brief Return an inversely scaled versionof this quaternion 176 * @param s The inverse scale factor */ 126 177 btQuaternion operator/(const btScalar& s) const 127 178 { … … 130 181 } 131 182 132 183 /**@brief Inversely scale this quaternion 184 * @param s The scale factor */ 133 185 btQuaternion& operator/=(const btScalar& s) 134 186 { … … 137 189 } 138 190 139 191 /**@brief Return a normalized version of this quaternion */ 140 192 btQuaternion normalized() const 141 193 { 142 194 return *this / length(); 143 195 } 144 196 /**@brief Return the angle between this quaternion and the other 197 * @param q The other quaternion */ 145 198 btScalar angle(const btQuaternion& q) const 146 199 { … … 149 202 return btAcos(dot(q) / s); 150 203 } 151 204 /**@brief Return the angle of rotation represented by this quaternion */ 152 205 btScalar getAngle() const 153 206 { 154 btScalar s = btScalar(2.) * btAcos(m_ unusedW);207 btScalar s = btScalar(2.) * btAcos(m_floats[3]); 155 208 return s; 156 209 } 157 210 158 211 159 212 /**@brief Return the inverse of this quaternion */ 160 213 btQuaternion inverse() const 161 214 { 162 return btQuaternion(-m_x, -m_y, -m_z, m_unusedW); 163 } 164 215 return btQuaternion(-m_floats[0], -m_floats[1], -m_floats[2], m_floats[3]); 216 } 217 218 /**@brief Return the sum of this quaternion and the other 219 * @param q2 The other quaternion */ 165 220 SIMD_FORCE_INLINE btQuaternion 166 221 operator+(const btQuaternion& q2) const 167 222 { 168 223 const btQuaternion& q1 = *this; 169 return btQuaternion(q1.x() + q2.x(), q1.y() + q2.y(), q1.z() + q2.z(), q1.m_unusedW + q2.m_unusedW); 170 } 171 224 return btQuaternion(q1.x() + q2.x(), q1.y() + q2.y(), q1.z() + q2.z(), q1.m_floats[3] + q2.m_floats[3]); 225 } 226 227 /**@brief Return the difference between this quaternion and the other 228 * @param q2 The other quaternion */ 172 229 SIMD_FORCE_INLINE btQuaternion 173 230 operator-(const btQuaternion& q2) const 174 231 { 175 232 const btQuaternion& q1 = *this; 176 return btQuaternion(q1.x() - q2.x(), q1.y() - q2.y(), q1.z() - q2.z(), q1.m_unusedW - q2.m_unusedW); 177 } 178 233 return btQuaternion(q1.x() - q2.x(), q1.y() - q2.y(), q1.z() - q2.z(), q1.m_floats[3] - q2.m_floats[3]); 234 } 235 236 /**@brief Return the negative of this quaternion 237 * This simply negates each element */ 179 238 SIMD_FORCE_INLINE btQuaternion operator-() const 180 239 { 181 240 const btQuaternion& q2 = *this; 182 return btQuaternion( - q2.x(), - q2.y(), - q2.z(), - q2.m_ unusedW);183 } 184 241 return btQuaternion( - q2.x(), - q2.y(), - q2.z(), - q2.m_floats[3]); 242 } 243 /**@todo document this and it's use */ 185 244 SIMD_FORCE_INLINE btQuaternion farthest( const btQuaternion& qd) const 186 245 { … … 193 252 } 194 253 254 /**@brief Return the quaternion which is the result of Spherical Linear Interpolation between this and the other quaternion 255 * @param q The other quaternion to interpolate with 256 * @param t The ratio between this and q to interpolate. If t = 0 the result is this, if t=1 the result is q. 257 * Slerp interpolates assuming constant velocity. */ 195 258 btQuaternion slerp(const btQuaternion& q, const btScalar& t) const 196 259 { … … 201 264 btScalar s0 = btSin((btScalar(1.0) - t) * theta); 202 265 btScalar s1 = btSin(t * theta); 203 return btQuaternion((m_ x* s0 + q.x() * s1) * d,204 (m_ y* s0 + q.y() * s1) * d,205 (m_ z* s0 + q.z() * s1) * d,206 (m_ unusedW * s0 + q.m_unusedW* s1) * d);266 return btQuaternion((m_floats[0] * s0 + q.x() * s1) * d, 267 (m_floats[1] * s0 + q.y() * s1) * d, 268 (m_floats[2] * s0 + q.z() * s1) * d, 269 (m_floats[3] * s0 + q.m_floats[3] * s1) * d); 207 270 } 208 271 else … … 212 275 } 213 276 214 SIMD_FORCE_INLINE const btScalar& getW() const { return m_ unusedW; }277 SIMD_FORCE_INLINE const btScalar& getW() const { return m_floats[3]; } 215 278 216 279 … … 218 281 219 282 220 283 /**@brief Return the negative of a quaternion */ 221 284 SIMD_FORCE_INLINE btQuaternion 222 285 operator-(const btQuaternion& q) … … 227 290 228 291 229 292 /**@brief Return the product of two quaternions */ 230 293 SIMD_FORCE_INLINE btQuaternion 231 294 operator*(const btQuaternion& q1, const btQuaternion& q2) { … … 254 317 } 255 318 319 /**@brief Calculate the dot product between two quaternions */ 256 320 SIMD_FORCE_INLINE btScalar 257 321 dot(const btQuaternion& q1, const btQuaternion& q2) … … 261 325 262 326 327 /**@brief Return the length of a quaternion */ 263 328 SIMD_FORCE_INLINE btScalar 264 329 length(const btQuaternion& q) … … 267 332 } 268 333 334 /**@brief Return the angle between two quaternions*/ 269 335 SIMD_FORCE_INLINE btScalar 270 336 angle(const btQuaternion& q1, const btQuaternion& q2) … … 273 339 } 274 340 275 341 /**@brief Return the inverse of a quaternion*/ 276 342 SIMD_FORCE_INLINE btQuaternion 277 343 inverse(const btQuaternion& q) … … 280 346 } 281 347 348 /**@brief Return the result of spherical linear interpolation betwen two quaternions 349 * @param q1 The first quaternion 350 * @param q2 The second quaternion 351 * @param t The ration between q1 and q2. t = 0 return q1, t=1 returns q2 352 * Slerp assumes constant velocity between positions. */ 282 353 SIMD_FORCE_INLINE btQuaternion 283 354 slerp(const btQuaternion& q1, const btQuaternion& q2, const btScalar& t) -
code/branches/physics/src/bullet/LinearMath/btQuickprof.cpp
r2192 r2430 111 111 TotalCalls = 0; 112 112 TotalTime = 0.0f; 113 gProfileClock.reset();113 114 114 115 115 if ( Child ) { … … 252 252 void CProfileManager::Reset( void ) 253 253 { 254 gProfileClock.reset(); 254 255 Root.Reset(); 255 256 Root.Call(); … … 279 280 } 280 281 282 #include <stdio.h> 283 284 void CProfileManager::dumpRecursive(CProfileIterator* profileIterator, int spacing) 285 { 286 profileIterator->First(); 287 if (profileIterator->Is_Done()) 288 return; 289 290 float accumulated_time=0,parent_time = profileIterator->Is_Root() ? CProfileManager::Get_Time_Since_Reset() : profileIterator->Get_Current_Parent_Total_Time(); 291 int i; 292 int frames_since_reset = CProfileManager::Get_Frame_Count_Since_Reset(); 293 for (i=0;i<spacing;i++) printf("."); 294 printf("----------------------------------\n"); 295 for (i=0;i<spacing;i++) printf("."); 296 printf("Profiling: %s (total running time: %.3f ms) ---\n", profileIterator->Get_Current_Parent_Name(), parent_time ); 297 float totalTime = 0.f; 298 299 300 int numChildren = 0; 301 302 for (i = 0; !profileIterator->Is_Done(); i++,profileIterator->Next()) 303 { 304 numChildren++; 305 float current_total_time = profileIterator->Get_Current_Total_Time(); 306 accumulated_time += current_total_time; 307 float fraction = parent_time > SIMD_EPSILON ? (current_total_time / parent_time) * 100 : 0.f; 308 { 309 int i; for (i=0;i<spacing;i++) printf("."); 310 } 311 printf("%d -- %s (%.2f %%) :: %.3f ms / frame (%d calls)\n",i, profileIterator->Get_Current_Name(), fraction,(current_total_time / (double)frames_since_reset),profileIterator->Get_Current_Total_Calls()); 312 totalTime += current_total_time; 313 //recurse into children 314 } 315 316 if (parent_time < accumulated_time) 317 { 318 printf("what's wrong\n"); 319 } 320 for (i=0;i<spacing;i++) printf("."); 321 printf("%s (%.3f %%) :: %.3f ms\n", "Unaccounted:",parent_time > SIMD_EPSILON ? ((parent_time - accumulated_time) / parent_time) * 100 : 0.f, parent_time - accumulated_time); 322 323 for (i=0;i<numChildren;i++) 324 { 325 profileIterator->Enter_Child(i); 326 dumpRecursive(profileIterator,spacing+3); 327 profileIterator->Enter_Parent(); 328 } 329 } 330 331 332 333 void CProfileManager::dumpAll() 334 { 335 CProfileIterator* profileIterator = 0; 336 profileIterator = CProfileManager::Get_Iterator(); 337 338 dumpRecursive(profileIterator,0); 339 340 CProfileManager::Release_Iterator(profileIterator); 341 } 342 343 344 281 345 #endif //USE_BT_CLOCK 282 346 -
code/branches/physics/src/bullet/LinearMath/btQuickprof.h
r2192 r2430 11 11 // Ogre (www.ogre3d.org). 12 12 13 14 13 15 #ifndef QUICK_PROF_H 14 16 #define QUICK_PROF_H 17 18 //To disable built-in profiling, please comment out next line 19 //#define BT_NO_PROFILE 1 20 #ifndef BT_NO_PROFILE 15 21 16 22 #include "btScalar.h" 17 23 #include "LinearMath/btAlignedAllocator.h" 18 24 #include <new> 19 //To disable built-in profiling, please comment out next line 20 //#define BT_NO_PROFILE 1 25 26 21 27 22 28 … … 322 328 static void Release_Iterator( CProfileIterator * iterator ) { delete ( iterator); } 323 329 330 static void dumpRecursive(CProfileIterator* profileIterator, int spacing); 331 332 static void dumpAll(); 333 324 334 private: 325 335 static CProfileNode Root; … … 345 355 }; 346 356 347 #if !defined(BT_NO_PROFILE) 357 348 358 #define BT_PROFILE( name ) CProfileSample __profile( name ) 349 #else 359 360 #else 361 350 362 #define BT_PROFILE( name ) 351 #endif 352 363 364 #endif //#ifndef BT_NO_PROFILE 353 365 354 366 -
code/branches/physics/src/bullet/LinearMath/btScalar.h
r2192 r2430 19 19 20 20 #include <math.h> 21 21 22 #include <stdlib.h>//size_t for MSVC 6.0 22 23 … … 25 26 #include <float.h> 26 27 27 #define BT_BULLET_VERSION 27 228 #define BT_BULLET_VERSION 273 28 29 29 30 inline int btGetVersion() … … 66 67 67 68 #include <assert.h> 68 #if defined(DEBUG) || defined (_DEBUG)69 #ifdef BT_DEBUG 69 70 #define btAssert assert 70 71 #else … … 86 87 #include <assert.h> 87 88 #endif 89 #ifdef BT_DEBUG 88 90 #define btAssert assert 91 #else 92 #define btAssert(x) 93 #endif 89 94 //btFullAssert is optional, slows down a lot 90 95 #define btFullAssert(x) … … 103 108 #include <assert.h> 104 109 #endif 110 #ifdef BT_DEBUG 105 111 #define btAssert assert 112 #else 113 #define btAssert(x) 114 #endif 106 115 //btFullAssert is optional, slows down a lot 107 116 #define btFullAssert(x) -
code/branches/physics/src/bullet/LinearMath/btStackAlloc.h
r2192 r2430 24 24 #include "btAlignedAllocator.h" 25 25 26 ///The btBlock class is an internal structure for the btStackAlloc memory allocator. 26 27 struct btBlock 27 28 { -
code/branches/physics/src/bullet/LinearMath/btTransform.h
r2192 r2430 22 22 23 23 24 / //The btTransform class supports rigid transforms with only translation and rotation and no scaling/shear.25 ///It can be used in combination with btVector3, btQuaternion and btMatrix3x3 linear algebra classes. 24 /**@brief The btTransform class supports rigid transforms with only translation and rotation and no scaling/shear. 25 *It can be used in combination with btVector3, btQuaternion and btMatrix3x3 linear algebra classes. */ 26 26 class btTransform { 27 27 … … 29 29 public: 30 30 31 31 /**@brief No initialization constructor */ 32 32 btTransform() {} 33 33 /**@brief Constructor from btQuaternion (optional btVector3 ) 34 * @param q Rotation from quaternion 35 * @param c Translation from Vector (default 0,0,0) */ 34 36 explicit SIMD_FORCE_INLINE btTransform(const btQuaternion& q, 35 37 const btVector3& c = btVector3(btScalar(0), btScalar(0), btScalar(0))) … … 38 40 {} 39 41 42 /**@brief Constructor from btMatrix3x3 (optional btVector3) 43 * @param b Rotation from Matrix 44 * @param c Translation from Vector default (0,0,0)*/ 40 45 explicit SIMD_FORCE_INLINE btTransform(const btMatrix3x3& b, 41 46 const btVector3& c = btVector3(btScalar(0), btScalar(0), btScalar(0))) … … 43 48 m_origin(c) 44 49 {} 45 50 /**@brief Copy constructor */ 46 51 SIMD_FORCE_INLINE btTransform (const btTransform& other) 47 52 : m_basis(other.m_basis), … … 49 54 { 50 55 } 51 56 /**@brief Assignment Operator */ 52 57 SIMD_FORCE_INLINE btTransform& operator=(const btTransform& other) 53 58 { … … 58 63 59 64 65 /**@brief Set the current transform as the value of the product of two transforms 66 * @param t1 Transform 1 67 * @param t2 Transform 2 68 * This = Transform1 * Transform2 */ 60 69 SIMD_FORCE_INLINE void mult(const btTransform& t1, const btTransform& t2) { 61 70 m_basis = t1.m_basis * t2.m_basis; … … 70 79 */ 71 80 72 81 /**@brief Return the transform of the vector */ 73 82 SIMD_FORCE_INLINE btVector3 operator()(const btVector3& x) const 74 83 { … … 78 87 } 79 88 89 /**@brief Return the transform of the vector */ 80 90 SIMD_FORCE_INLINE btVector3 operator*(const btVector3& x) const 81 91 { … … 83 93 } 84 94 95 /**@brief Return the transform of the btQuaternion */ 96 SIMD_FORCE_INLINE btQuaternion operator*(const btQuaternion& q) const 97 { 98 return getRotation() * q; 99 } 100 101 /**@brief Return the basis matrix for the rotation */ 85 102 SIMD_FORCE_INLINE btMatrix3x3& getBasis() { return m_basis; } 103 /**@brief Return the basis matrix for the rotation */ 86 104 SIMD_FORCE_INLINE const btMatrix3x3& getBasis() const { return m_basis; } 87 105 106 /**@brief Return the origin vector translation */ 88 107 SIMD_FORCE_INLINE btVector3& getOrigin() { return m_origin; } 108 /**@brief Return the origin vector translation */ 89 109 SIMD_FORCE_INLINE const btVector3& getOrigin() const { return m_origin; } 90 110 111 /**@brief Return a quaternion representing the rotation */ 91 112 btQuaternion getRotation() const { 92 113 btQuaternion q; … … 96 117 97 118 119 /**@brief Set from an array 120 * @param m A pointer to a 15 element array (12 rotation(row major padded on the right by 1), and 3 translation */ 98 121 void setFromOpenGLMatrix(const btScalar *m) 99 122 { … … 102 125 } 103 126 127 /**@brief Fill an array representation 128 * @param m A pointer to a 15 element array (12 rotation(row major padded on the right by 1), and 3 translation */ 104 129 void getOpenGLMatrix(btScalar *m) const 105 130 { … … 111 136 } 112 137 138 /**@brief Set the translational element 139 * @param origin The vector to set the translation to */ 113 140 SIMD_FORCE_INLINE void setOrigin(const btVector3& origin) 114 141 { … … 119 146 120 147 121 148 /**@brief Set the rotational element by btMatrix3x3 */ 122 149 SIMD_FORCE_INLINE void setBasis(const btMatrix3x3& basis) 123 150 { … … 125 152 } 126 153 154 /**@brief Set the rotational element by btQuaternion */ 127 155 SIMD_FORCE_INLINE void setRotation(const btQuaternion& q) 128 156 { … … 131 159 132 160 133 161 /**@brief Set this transformation to the identity */ 134 162 void setIdentity() 135 163 { … … 138 166 } 139 167 140 168 /**@brief Multiply this Transform by another(this = this * another) 169 * @param t The other transform */ 141 170 btTransform& operator*=(const btTransform& t) 142 171 { … … 146 175 } 147 176 177 /**@brief Return the inverse of this transform */ 148 178 btTransform inverse() const 149 179 { … … 152 182 } 153 183 184 /**@brief Return the inverse of this transform times the other transform 185 * @param t The other transform 186 * return this.inverse() * the other */ 154 187 btTransform inverseTimes(const btTransform& t) const; 155 188 189 /**@brief Return the product of this transform and the other */ 156 190 btTransform operator*(const btTransform& t) const; 157 191 192 /**@brief Return an identity transform */ 158 193 static btTransform getIdentity() 159 194 { … … 164 199 165 200 private: 166 201 ///Storage for the rotation 167 202 btMatrix3x3 m_basis; 203 ///Storage for the translation 168 204 btVector3 m_origin; 169 205 }; … … 192 228 } 193 229 230 /**@brief Test if two transforms have all elements equal */ 194 231 SIMD_FORCE_INLINE bool operator==(const btTransform& t1, const btTransform& t2) 195 232 { -
code/branches/physics/src/bullet/LinearMath/btTransformUtil.h
r2192 r2430 101 101 } 102 102 103 static void calculateVelocityQuaternion(const btVector3& pos0,const btVector3& pos1,const btQuaternion& orn0,const btQuaternion& orn1,btScalar timeStep,btVector3& linVel,btVector3& angVel) 104 { 105 linVel = (pos1 - pos0) / timeStep; 106 btVector3 axis; 107 btScalar angle; 108 if (orn0 != orn1) 109 { 110 calculateDiffAxisAngleQuaternion(orn0,orn1,axis,angle); 111 angVel = axis * angle / timeStep; 112 } else 113 { 114 angVel.setValue(0,0,0); 115 } 116 } 117 118 static void calculateDiffAxisAngleQuaternion(const btQuaternion& orn0,const btQuaternion& orn1a,btVector3& axis,btScalar& angle) 119 { 120 btQuaternion orn1 = orn0.farthest(orn1a); 121 btQuaternion dorn = orn1 * orn0.inverse(); 122 ///floating point inaccuracy can lead to w component > 1..., which breaks 123 dorn.normalize(); 124 angle = dorn.getAngle(); 125 axis = btVector3(dorn.x(),dorn.y(),dorn.z()); 126 axis[3] = btScalar(0.); 127 //check for axis length 128 btScalar len = axis.length2(); 129 if (len < SIMD_EPSILON*SIMD_EPSILON) 130 axis = btVector3(btScalar(1.),btScalar(0.),btScalar(0.)); 131 else 132 axis /= btSqrt(len); 133 } 134 103 135 static void calculateVelocity(const btTransform& transform0,const btTransform& transform1,btScalar timeStep,btVector3& linVel,btVector3& angVel) 104 136 { … … 112 144 static void calculateDiffAxisAngle(const btTransform& transform0,const btTransform& transform1,btVector3& axis,btScalar& angle) 113 145 { 114 115 #ifdef USE_QUATERNION_DIFF116 btQuaternion orn0 = transform0.getRotation();117 btQuaternion orn1a = transform1.getRotation();118 btQuaternion orn1 = orn0.farthest(orn1a);119 btQuaternion dorn = orn1 * orn0.inverse();120 #else121 146 btMatrix3x3 dmat = transform1.getBasis() * transform0.getBasis().inverse(); 122 147 btQuaternion dorn; 123 148 dmat.getRotation(dorn); 124 #endif//USE_QUATERNION_DIFF 125 149 126 150 ///floating point inaccuracy can lead to w component > 1..., which breaks 127 128 151 dorn.normalize(); 129 152 … … 141 164 }; 142 165 166 167 ///The btConvexSeparatingDistanceUtil can help speed up convex collision detection 168 ///by conservatively updating a cached separating distance/vector instead of re-calculating the closest distance 169 class btConvexSeparatingDistanceUtil 170 { 171 btQuaternion m_ornA; 172 btQuaternion m_ornB; 173 btVector3 m_posA; 174 btVector3 m_posB; 175 176 btVector3 m_separatingNormal; 177 178 btScalar m_boundingRadiusA; 179 btScalar m_boundingRadiusB; 180 btScalar m_separatingDistance; 181 182 public: 183 184 btConvexSeparatingDistanceUtil(btScalar boundingRadiusA,btScalar boundingRadiusB) 185 :m_boundingRadiusA(boundingRadiusA), 186 m_boundingRadiusB(boundingRadiusB), 187 m_separatingDistance(0.f) 188 { 189 } 190 191 btScalar getConservativeSeparatingDistance() 192 { 193 return m_separatingDistance; 194 } 195 196 void updateSeparatingDistance(const btTransform& transA,const btTransform& transB) 197 { 198 const btVector3& toPosA = transA.getOrigin(); 199 const btVector3& toPosB = transB.getOrigin(); 200 btQuaternion toOrnA = transA.getRotation(); 201 btQuaternion toOrnB = transB.getRotation(); 202 203 if (m_separatingDistance>0.f) 204 { 205 206 207 btVector3 linVelA,angVelA,linVelB,angVelB; 208 btTransformUtil::calculateVelocityQuaternion(m_posA,toPosA,m_ornA,toOrnA,btScalar(1.),linVelA,angVelA); 209 btTransformUtil::calculateVelocityQuaternion(m_posB,toPosB,m_ornB,toOrnB,btScalar(1.),linVelB,angVelB); 210 btScalar maxAngularProjectedVelocity = angVelA.length() * m_boundingRadiusA + angVelB.length() * m_boundingRadiusB; 211 btVector3 relLinVel = (linVelB-linVelA); 212 btScalar relLinVelocLength = (linVelB-linVelA).dot(m_separatingNormal); 213 if (relLinVelocLength<0.f) 214 { 215 relLinVelocLength = 0.f; 216 } 217 218 btScalar projectedMotion = maxAngularProjectedVelocity +relLinVelocLength; 219 m_separatingDistance -= projectedMotion; 220 } 221 222 m_posA = toPosA; 223 m_posB = toPosB; 224 m_ornA = toOrnA; 225 m_ornB = toOrnB; 226 } 227 228 void initSeparatingDistance(const btVector3& separatingVector,btScalar separatingDistance,const btTransform& transA,const btTransform& transB) 229 { 230 m_separatingNormal = separatingVector; 231 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; 241 } 242 243 }; 244 245 143 246 #endif //SIMD_TRANSFORM_UTIL_H 144 247 -
code/branches/physics/src/bullet/LinearMath/btVector3.h
r2192 r2430 20 20 #include "btQuadWord.h" 21 21 22 ///btVector3 can be used to represent 3D points and vectors. 23 ///It has an un-used w component to suit 16-byte alignment when btVector3 is stored in containers. This extra component can be used by derived classes (Quaternion?) or by user 24 ///Ideally, this class should be replaced by a platform optimized SIMD version that keeps the data in registers 22 /**@brief btVector3 can be used to represent 3D points and vectors. 23 * It has an un-used w component to suit 16-byte alignment when btVector3 is stored in containers. This extra component can be used by derived classes (Quaternion?) or by user 24 * Ideally, this class should be replaced by a platform optimized SIMD version that keeps the data in registers 25 */ 25 26 class btVector3 : public btQuadWord { 26 27 27 28 public: 29 /**@brief No initialization constructor */ 28 30 SIMD_FORCE_INLINE btVector3() {} 29 31 32 /**@brief Constructor from btQuadWordStorage (btVector3 inherits from this so is also valid) 33 * Note: Vector3 derives from btQuadWordStorage*/ 30 34 SIMD_FORCE_INLINE btVector3(const btQuadWordStorage& q) 31 35 : btQuadWord(q) … … 33 37 } 34 38 35 39 /**@brief Constructor from scalars 40 * @param x X value 41 * @param y Y value 42 * @param z Z value 43 */ 36 44 SIMD_FORCE_INLINE btVector3(const btScalar& x, const btScalar& y, const btScalar& z) 37 45 :btQuadWord(x,y,z,btScalar(0.)) … … 45 53 46 54 47 55 /**@brief Add a vector to this one 56 * @param The vector to add to this one */ 48 57 SIMD_FORCE_INLINE btVector3& operator+=(const btVector3& v) 49 58 { 50 59 51 m_ x += v.x(); m_y += v.y(); m_z+= v.z();60 m_floats[0] += v.x(); m_floats[1] += v.y(); m_floats[2] += v.z(); 52 61 return *this; 53 62 } 54 63 55 64 56 65 /**@brief Subtract a vector from this one 66 * @param The vector to subtract */ 57 67 SIMD_FORCE_INLINE btVector3& operator-=(const btVector3& v) 58 68 { 59 m_ x -= v.x(); m_y -= v.y(); m_z-= v.z();69 m_floats[0] -= v.x(); m_floats[1] -= v.y(); m_floats[2] -= v.z(); 60 70 return *this; 61 71 } 62 72 /**@brief Scale the vector 73 * @param s Scale factor */ 63 74 SIMD_FORCE_INLINE btVector3& operator*=(const btScalar& s) 64 75 { 65 m_ x *= s; m_y *= s; m_z*= s;76 m_floats[0] *= s; m_floats[1] *= s; m_floats[2] *= s; 66 77 return *this; 67 78 } 68 79 80 /**@brief Inversely scale the vector 81 * @param s Scale factor to divide by */ 69 82 SIMD_FORCE_INLINE btVector3& operator/=(const btScalar& s) 70 83 { … … 73 86 } 74 87 88 /**@brief Return the dot product 89 * @param v The other vector in the dot product */ 75 90 SIMD_FORCE_INLINE btScalar dot(const btVector3& v) const 76 91 { 77 return m_x * v.x() + m_y * v.y() + m_z * v.z(); 78 } 79 92 return m_floats[0] * v.x() + m_floats[1] * v.y() + m_floats[2] * v.z(); 93 } 94 95 /**@brief Return the length of the vector squared */ 80 96 SIMD_FORCE_INLINE btScalar length2() const 81 97 { … … 83 99 } 84 100 101 /**@brief Return the length of the vector */ 85 102 SIMD_FORCE_INLINE btScalar length() const 86 103 { … … 88 105 } 89 106 107 /**@brief Return the distance squared between the ends of this and another vector 108 * This is symantically treating the vector like a point */ 90 109 SIMD_FORCE_INLINE btScalar distance2(const btVector3& v) const; 91 110 111 /**@brief Return the distance between the ends of this and another vector 112 * This is symantically treating the vector like a point */ 92 113 SIMD_FORCE_INLINE btScalar distance(const btVector3& v) const; 93 114 115 /**@brief Normalize this vector 116 * x^2 + y^2 + z^2 = 1 */ 94 117 SIMD_FORCE_INLINE btVector3& normalize() 95 118 { … … 97 120 } 98 121 122 /**@brief Return a normalized version of this vector */ 99 123 SIMD_FORCE_INLINE btVector3 normalized() const; 100 124 125 /**@brief Rotate this vector 126 * @param wAxis The axis to rotate about 127 * @param angle The angle to rotate by */ 101 128 SIMD_FORCE_INLINE btVector3 rotate( const btVector3& wAxis, const btScalar angle ); 102 129 130 /**@brief Return the angle between this and another vector 131 * @param v The other vector */ 103 132 SIMD_FORCE_INLINE btScalar angle(const btVector3& v) const 104 133 { … … 107 136 return btAcos(dot(v) / s); 108 137 } 109 138 /**@brief Return a vector will the absolute values of each element */ 110 139 SIMD_FORCE_INLINE btVector3 absolute() const 111 140 { 112 141 return btVector3( 113 btFabs(m_x), 114 btFabs(m_y), 115 btFabs(m_z)); 116 } 117 142 btFabs(m_floats[0]), 143 btFabs(m_floats[1]), 144 btFabs(m_floats[2])); 145 } 146 /**@brief Return the cross product between this and another vector 147 * @param v The other vector */ 118 148 SIMD_FORCE_INLINE btVector3 cross(const btVector3& v) const 119 149 { 120 150 return btVector3( 121 m_ y * v.z() - m_z* v.y(),122 m_ z * v.x() - m_x* v.z(),123 m_ x * v.y() - m_y* v.x());151 m_floats[1] * v.z() - m_floats[2] * v.y(), 152 m_floats[2] * v.x() - m_floats[0] * v.z(), 153 m_floats[0] * v.y() - m_floats[1] * v.x()); 124 154 } 125 155 126 156 SIMD_FORCE_INLINE btScalar triple(const btVector3& v1, const btVector3& v2) const 127 157 { 128 return m_x * (v1.y() * v2.z() - v1.z() * v2.y()) + 129 m_y * (v1.z() * v2.x() - v1.x() * v2.z()) + 130 m_z * (v1.x() * v2.y() - v1.y() * v2.x()); 131 } 132 158 return m_floats[0] * (v1.y() * v2.z() - v1.z() * v2.y()) + 159 m_floats[1] * (v1.z() * v2.x() - v1.x() * v2.z()) + 160 m_floats[2] * (v1.x() * v2.y() - v1.y() * v2.x()); 161 } 162 163 /**@brief Return the axis with the smallest value 164 * Note return values are 0,1,2 for x, y, or z */ 133 165 SIMD_FORCE_INLINE int minAxis() const 134 166 { 135 return m_x < m_y ? (m_x < m_z ? 0 : 2) : (m_y < m_z ? 1 : 2); 136 } 137 167 return m_floats[0] < m_floats[1] ? (m_floats[0] < m_floats[2] ? 0 : 2) : (m_floats[1] < m_floats[2] ? 1 : 2); 168 } 169 170 /**@brief Return the axis with the largest value 171 * Note return values are 0,1,2 for x, y, or z */ 138 172 SIMD_FORCE_INLINE int maxAxis() const 139 173 { 140 return m_ x < m_y ? (m_y < m_z ? 2 : 1) : (m_x < m_z? 2 : 0);174 return m_floats[0] < m_floats[1] ? (m_floats[1] < m_floats[2] ? 2 : 1) : (m_floats[0] < m_floats[2] ? 2 : 0); 141 175 } 142 176 … … 154 188 { 155 189 btScalar s = btScalar(1.0) - rt; 156 m_ x= s * v0.x() + rt * v1.x();157 m_ y= s * v0.y() + rt * v1.y();158 m_ z= s * v0.z() + rt * v1.z();190 m_floats[0] = s * v0.x() + rt * v1.x(); 191 m_floats[1] = s * v0.y() + rt * v1.y(); 192 m_floats[2] = s * v0.z() + rt * v1.z(); 159 193 //don't do the unused w component 160 194 // m_co[3] = s * v0[3] + rt * v1[3]; 161 195 } 162 196 197 /**@brief Return the linear interpolation between this and another vector 198 * @param v The other vector 199 * @param t The ration of this to v (t = 0 => return this, t=1 => return other) */ 163 200 SIMD_FORCE_INLINE btVector3 lerp(const btVector3& v, const btScalar& t) const 164 201 { 165 return btVector3(m_x + (v.x() - m_x) * t, 166 m_y + (v.y() - m_y) * t, 167 m_z + (v.z() - m_z) * t); 168 } 169 170 202 return btVector3(m_floats[0] + (v.x() - m_floats[0]) * t, 203 m_floats[1] + (v.y() - m_floats[1]) * t, 204 m_floats[2] + (v.z() - m_floats[2]) * t); 205 } 206 207 /**@brief Elementwise multiply this vector by the other 208 * @param v The other vector */ 171 209 SIMD_FORCE_INLINE btVector3& operator*=(const btVector3& v) 172 210 { 173 m_ x *= v.x(); m_y *= v.y(); m_z*= v.z();211 m_floats[0] *= v.x(); m_floats[1] *= v.y(); m_floats[2] *= v.z(); 174 212 return *this; 175 213 } … … 179 217 }; 180 218 219 /**@brief Return the sum of two vectors (Point symantics)*/ 181 220 SIMD_FORCE_INLINE btVector3 182 221 operator+(const btVector3& v1, const btVector3& v2) … … 185 224 } 186 225 226 /**@brief Return the elementwise product of two vectors */ 187 227 SIMD_FORCE_INLINE btVector3 188 228 operator*(const btVector3& v1, const btVector3& v2) … … 191 231 } 192 232 233 /**@brief Return the difference between two vectors */ 193 234 SIMD_FORCE_INLINE btVector3 194 235 operator-(const btVector3& v1, const btVector3& v2) … … 196 237 return btVector3(v1.x() - v2.x(), v1.y() - v2.y(), v1.z() - v2.z()); 197 238 } 198 239 /**@brief Return the negative of the vector */ 199 240 SIMD_FORCE_INLINE btVector3 200 241 operator-(const btVector3& v) … … 203 244 } 204 245 246 /**@brief Return the vector scaled by s */ 205 247 SIMD_FORCE_INLINE btVector3 206 248 operator*(const btVector3& v, const btScalar& s) … … 209 251 } 210 252 253 /**@brief Return the vector scaled by s */ 211 254 SIMD_FORCE_INLINE btVector3 212 255 operator*(const btScalar& s, const btVector3& v) … … 215 258 } 216 259 260 /**@brief Return the vector inversely scaled by s */ 217 261 SIMD_FORCE_INLINE btVector3 218 262 operator/(const btVector3& v, const btScalar& s) … … 222 266 } 223 267 268 /**@brief Return the vector inversely scaled by s */ 224 269 SIMD_FORCE_INLINE btVector3 225 270 operator/(const btVector3& v1, const btVector3& v2) … … 228 273 } 229 274 275 /**@brief Return the dot product between two vectors */ 230 276 SIMD_FORCE_INLINE btScalar 231 277 dot(const btVector3& v1, const btVector3& v2) … … 235 281 236 282 237 283 /**@brief Return the distance squared between two vectors */ 238 284 SIMD_FORCE_INLINE btScalar 239 285 distance2(const btVector3& v1, const btVector3& v2) … … 243 289 244 290 291 /**@brief Return the distance between two vectors */ 245 292 SIMD_FORCE_INLINE btScalar 246 293 distance(const btVector3& v1, const btVector3& v2) … … 249 296 } 250 297 298 /**@brief Return the angle between two vectors */ 251 299 SIMD_FORCE_INLINE btScalar 252 300 angle(const btVector3& v1, const btVector3& v2) … … 255 303 } 256 304 305 /**@brief Return the cross product of two vectors */ 257 306 SIMD_FORCE_INLINE btVector3 258 307 cross(const btVector3& v1, const btVector3& v2) … … 267 316 } 268 317 318 /**@brief Return the linear interpolation between two vectors 319 * @param v1 One vector 320 * @param v2 The other vector 321 * @param t The ration of this to v (t = 0 => return v1, t=1 => return v2) */ 269 322 SIMD_FORCE_INLINE btVector3 270 323 lerp(const btVector3& v1, const btVector3& v2, const btScalar& t) … … 273 326 } 274 327 275 328 /**@brief Test if each element of the vector is equivalent */ 276 329 SIMD_FORCE_INLINE bool operator==(const btVector3& p1, const btVector3& p2) 277 330 { … … 317 370 : btVector3(x,y,z) 318 371 { 319 m_ unusedW= w;372 m_floats[3] = w; 320 373 } 321 374 … … 324 377 { 325 378 return btVector4( 326 btFabs(m_ x),327 btFabs(m_ y),328 btFabs(m_ z),329 btFabs(m_ unusedW));330 } 331 332 333 334 btScalar getW() const { return m_ unusedW;}379 btFabs(m_floats[0]), 380 btFabs(m_floats[1]), 381 btFabs(m_floats[2]), 382 btFabs(m_floats[3])); 383 } 384 385 386 387 btScalar getW() const { return m_floats[3];} 335 388 336 389 … … 339 392 int maxIndex = -1; 340 393 btScalar maxVal = btScalar(-1e30); 341 if (m_ x> maxVal)394 if (m_floats[0] > maxVal) 342 395 { 343 396 maxIndex = 0; 344 maxVal = m_ x;345 } 346 if (m_ y> maxVal)397 maxVal = m_floats[0]; 398 } 399 if (m_floats[1] > maxVal) 347 400 { 348 401 maxIndex = 1; 349 maxVal = m_ y;350 } 351 if (m_ z> maxVal)402 maxVal = m_floats[1]; 403 } 404 if (m_floats[2] > maxVal) 352 405 { 353 406 maxIndex = 2; 354 maxVal = m_ z;355 } 356 if (m_ unusedW> maxVal)407 maxVal = m_floats[2]; 408 } 409 if (m_floats[3] > maxVal) 357 410 { 358 411 maxIndex = 3; 359 maxVal = m_ unusedW;412 maxVal = m_floats[3]; 360 413 } 361 414 … … 372 425 int minIndex = -1; 373 426 btScalar minVal = btScalar(1e30); 374 if (m_ x< minVal)427 if (m_floats[0] < minVal) 375 428 { 376 429 minIndex = 0; 377 minVal = m_ x;378 } 379 if (m_ y< minVal)430 minVal = m_floats[0]; 431 } 432 if (m_floats[1] < minVal) 380 433 { 381 434 minIndex = 1; 382 minVal = m_ y;383 } 384 if (m_ z< minVal)435 minVal = m_floats[1]; 436 } 437 if (m_floats[2] < minVal) 385 438 { 386 439 minIndex = 2; 387 minVal = m_ z;388 } 389 if (m_ unusedW< minVal)440 minVal = m_floats[2]; 441 } 442 if (m_floats[3] < minVal) 390 443 { 391 444 minIndex = 3; 392 minVal = m_ unusedW;445 minVal = m_floats[3]; 393 446 } 394 447
Note: See TracChangeset
for help on using the changeset viewer.
