- Timestamp:
- Dec 13, 2008, 11:45:51 PM (16 years ago)
- File:
-
- 1 edited
Legend:
- Unmodified
- Added
- Removed
-
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 {
Note: See TracChangeset
for help on using the changeset viewer.
