Changeset 8351 for code/trunk/src/external/bullet/LinearMath/btMatrix3x3.h
- Timestamp:
- Apr 28, 2011, 7:15:14 AM (13 years ago)
- File:
-
- 1 edited
Legend:
- Unmodified
- Added
- Removed
-
code/trunk/src/external/bullet/LinearMath/btMatrix3x3.h
r5781 r8351 14 14 15 15 16 #ifndef btMatrix3x3_H 17 #define btMatrix3x3_H 18 19 #include "btScalar.h" 16 #ifndef BT_MATRIX3x3_H 17 #define BT_MATRIX3x3_H 20 18 21 19 #include "btVector3.h" 22 20 #include "btQuaternion.h" 23 21 22 #ifdef BT_USE_DOUBLE_PRECISION 23 #define btMatrix3x3Data btMatrix3x3DoubleData 24 #else 25 #define btMatrix3x3Data btMatrix3x3FloatData 26 #endif //BT_USE_DOUBLE_PRECISION 24 27 25 28 26 29 /**@brief The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with btQuaternion, btTransform and btVector3. 27 30 * Make sure to only include a pure orthogonal matrix without scaling. */ 28 31 class btMatrix3x3 { 29 public: 30 /** @brief No initializaion constructor */ 31 btMatrix3x3 () {} 32 33 // explicit btMatrix3x3(const btScalar *m) { setFromOpenGLSubMatrix(m); } 34 35 /**@brief Constructor from Quaternion */ 36 explicit btMatrix3x3(const btQuaternion& q) { setRotation(q); } 37 /* 38 template <typename btScalar> 39 Matrix3x3(const btScalar& yaw, const btScalar& pitch, const btScalar& roll) 40 { 41 setEulerYPR(yaw, pitch, roll); 42 } 43 */ 44 /** @brief Constructor with row major formatting */ 45 btMatrix3x3(const btScalar& xx, const btScalar& xy, const btScalar& xz, 46 const btScalar& yx, const btScalar& yy, const btScalar& yz, 47 const btScalar& zx, const btScalar& zy, const btScalar& zz) 48 { 49 setValue(xx, xy, xz, 50 yx, yy, yz, 51 zx, zy, zz); 52 } 53 /** @brief Copy constructor */ 54 SIMD_FORCE_INLINE btMatrix3x3 (const btMatrix3x3& other) 55 { 56 m_el[0] = other.m_el[0]; 57 m_el[1] = other.m_el[1]; 58 m_el[2] = other.m_el[2]; 59 } 60 /** @brief Assignment Operator */ 61 SIMD_FORCE_INLINE btMatrix3x3& operator=(const btMatrix3x3& other) 62 { 63 m_el[0] = other.m_el[0]; 64 m_el[1] = other.m_el[1]; 65 m_el[2] = other.m_el[2]; 66 return *this; 67 } 68 69 /** @brief Get a column of the matrix as a vector 70 * @param i Column number 0 indexed */ 71 SIMD_FORCE_INLINE btVector3 getColumn(int i) const 72 { 73 return btVector3(m_el[0][i],m_el[1][i],m_el[2][i]); 74 } 75 76 77 /** @brief Get a row of the matrix as a vector 78 * @param i Row number 0 indexed */ 79 SIMD_FORCE_INLINE const btVector3& getRow(int i) const 80 { 81 btFullAssert(0 <= i && i < 3); 82 return m_el[i]; 83 } 84 85 /** @brief Get a mutable reference to a row of the matrix as a vector 86 * @param i Row number 0 indexed */ 87 SIMD_FORCE_INLINE btVector3& operator[](int i) 88 { 89 btFullAssert(0 <= i && i < 3); 90 return m_el[i]; 91 } 92 93 /** @brief Get a const reference to a row of the matrix as a vector 94 * @param i Row number 0 indexed */ 95 SIMD_FORCE_INLINE const btVector3& operator[](int i) const 96 { 97 btFullAssert(0 <= i && i < 3); 98 return m_el[i]; 99 } 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 */ 104 btMatrix3x3& operator*=(const btMatrix3x3& m); 105 106 /** @brief Set from a carray of btScalars 107 * @param m A pointer to the beginning of an array of 9 btScalars */ 32 33 ///Data storage for the matrix, each vector is a row of the matrix 34 btVector3 m_el[3]; 35 36 public: 37 /** @brief No initializaion constructor */ 38 btMatrix3x3 () {} 39 40 // explicit btMatrix3x3(const btScalar *m) { setFromOpenGLSubMatrix(m); } 41 42 /**@brief Constructor from Quaternion */ 43 explicit btMatrix3x3(const btQuaternion& q) { setRotation(q); } 44 /* 45 template <typename btScalar> 46 Matrix3x3(const btScalar& yaw, const btScalar& pitch, const btScalar& roll) 47 { 48 setEulerYPR(yaw, pitch, roll); 49 } 50 */ 51 /** @brief Constructor with row major formatting */ 52 btMatrix3x3(const btScalar& xx, const btScalar& xy, const btScalar& xz, 53 const btScalar& yx, const btScalar& yy, const btScalar& yz, 54 const btScalar& zx, const btScalar& zy, const btScalar& zz) 55 { 56 setValue(xx, xy, xz, 57 yx, yy, yz, 58 zx, zy, zz); 59 } 60 /** @brief Copy constructor */ 61 SIMD_FORCE_INLINE btMatrix3x3 (const btMatrix3x3& other) 62 { 63 m_el[0] = other.m_el[0]; 64 m_el[1] = other.m_el[1]; 65 m_el[2] = other.m_el[2]; 66 } 67 /** @brief Assignment Operator */ 68 SIMD_FORCE_INLINE btMatrix3x3& operator=(const btMatrix3x3& other) 69 { 70 m_el[0] = other.m_el[0]; 71 m_el[1] = other.m_el[1]; 72 m_el[2] = other.m_el[2]; 73 return *this; 74 } 75 76 /** @brief Get a column of the matrix as a vector 77 * @param i Column number 0 indexed */ 78 SIMD_FORCE_INLINE btVector3 getColumn(int i) const 79 { 80 return btVector3(m_el[0][i],m_el[1][i],m_el[2][i]); 81 } 82 83 84 /** @brief Get a row of the matrix as a vector 85 * @param i Row number 0 indexed */ 86 SIMD_FORCE_INLINE const btVector3& getRow(int i) const 87 { 88 btFullAssert(0 <= i && i < 3); 89 return m_el[i]; 90 } 91 92 /** @brief Get a mutable reference to a row of the matrix as a vector 93 * @param i Row number 0 indexed */ 94 SIMD_FORCE_INLINE btVector3& operator[](int i) 95 { 96 btFullAssert(0 <= i && i < 3); 97 return m_el[i]; 98 } 99 100 /** @brief Get a const reference to a row of the matrix as a vector 101 * @param i Row number 0 indexed */ 102 SIMD_FORCE_INLINE const btVector3& operator[](int i) const 103 { 104 btFullAssert(0 <= i && i < 3); 105 return m_el[i]; 106 } 107 108 /** @brief Multiply by the target matrix on the right 109 * @param m Rotation matrix to be applied 110 * Equivilant to this = this * m */ 111 btMatrix3x3& operator*=(const btMatrix3x3& m); 112 113 /** @brief Set from a carray of btScalars 114 * @param m A pointer to the beginning of an array of 9 btScalars */ 108 115 void setFromOpenGLSubMatrix(const btScalar *m) 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 116 { 117 m_el[0].setValue(m[0],m[4],m[8]); 118 m_el[1].setValue(m[1],m[5],m[9]); 119 m_el[2].setValue(m[2],m[6],m[10]); 120 121 } 122 /** @brief Set the values of the matrix explicitly (row major) 123 * @param xx Top left 124 * @param xy Top Middle 125 * @param xz Top Right 126 * @param yx Middle Left 127 * @param yy Middle Middle 128 * @param yz Middle Right 129 * @param zx Bottom Left 130 * @param zy Bottom Middle 131 * @param zz Bottom Right*/ 132 void setValue(const btScalar& xx, const btScalar& xy, const btScalar& xz, 133 const btScalar& yx, const btScalar& yy, const btScalar& yz, 134 const btScalar& zx, const btScalar& zy, const btScalar& zz) 135 { 136 m_el[0].setValue(xx,xy,xz); 137 m_el[1].setValue(yx,yy,yz); 138 m_el[2].setValue(zx,zy,zz); 139 } 140 141 /** @brief Set the matrix from a quaternion 142 * @param q The Quaternion to match */ 143 void setRotation(const btQuaternion& q) 144 { 145 btScalar d = q.length2(); 146 btFullAssert(d != btScalar(0.0)); 147 btScalar s = btScalar(2.0) / d; 148 btScalar xs = q.x() * s, ys = q.y() * s, zs = q.z() * s; 149 btScalar wx = q.w() * xs, wy = q.w() * ys, wz = q.w() * zs; 150 btScalar xx = q.x() * xs, xy = q.x() * ys, xz = q.x() * zs; 151 btScalar yy = q.y() * ys, yz = q.y() * zs, zz = q.z() * zs; 152 setValue(btScalar(1.0) - (yy + zz), xy - wz, xz + wy, 153 xy + wz, btScalar(1.0) - (xx + zz), yz - wx, 154 xz - wy, yz + wx, btScalar(1.0) - (xx + yy)); 155 } 156 157 158 /** @brief Set the matrix from euler angles using YPR around YXZ respectively 159 * @param yaw Yaw about Y axis 160 * @param pitch Pitch about X axis 161 * @param roll Roll about Z axis 162 */ 163 void setEulerYPR(const btScalar& yaw, const btScalar& pitch, const btScalar& roll) 164 { 165 setEulerZYX(roll, pitch, yaw); 166 } 160 167 161 168 /** @brief Set the matrix from euler angles YPR around ZYX axes 162 163 164 165 166 167 168 169 169 * @param eulerX Roll about X axis 170 * @param eulerY Pitch around Y axis 171 * @param eulerZ Yaw aboud Z axis 172 * 173 * These angles are used to produce a rotation matrix. The euler 174 * angles are applied in ZYX order. I.e a vector is first rotated 175 * about X then Y and then Z 176 **/ 170 177 void setEulerZYX(btScalar eulerX,btScalar eulerY,btScalar eulerZ) { 171 178 ///@todo proposed to reverse this since it's labeled zyx but takes arguments xyz and it will match all other parts of the code 172 179 btScalar ci ( btCos(eulerX)); 173 180 btScalar cj ( btCos(eulerY)); … … 180 187 btScalar sc = si * ch; 181 188 btScalar ss = si * sh; 182 189 183 190 setValue(cj * ch, sj * sc - cs, sj * cc + ss, 184 cj * sh, sj * ss + cc, sj * cs - sc, 185 -sj, cj * si, cj * ci); 186 } 187 188 /**@brief Set the matrix to the identity */ 189 void setIdentity() 191 cj * sh, sj * ss + cc, sj * cs - sc, 192 -sj, cj * si, cj * ci); 193 } 194 195 /**@brief Set the matrix to the identity */ 196 void setIdentity() 197 { 198 setValue(btScalar(1.0), btScalar(0.0), btScalar(0.0), 199 btScalar(0.0), btScalar(1.0), btScalar(0.0), 200 btScalar(0.0), btScalar(0.0), btScalar(1.0)); 201 } 202 203 static const btMatrix3x3& getIdentity() 204 { 205 static const btMatrix3x3 identityMatrix(btScalar(1.0), btScalar(0.0), btScalar(0.0), 206 btScalar(0.0), btScalar(1.0), btScalar(0.0), 207 btScalar(0.0), btScalar(0.0), btScalar(1.0)); 208 return identityMatrix; 209 } 210 211 /**@brief Fill the values of the matrix into a 9 element array 212 * @param m The array to be filled */ 213 void getOpenGLSubMatrix(btScalar *m) const 214 { 215 m[0] = btScalar(m_el[0].x()); 216 m[1] = btScalar(m_el[1].x()); 217 m[2] = btScalar(m_el[2].x()); 218 m[3] = btScalar(0.0); 219 m[4] = btScalar(m_el[0].y()); 220 m[5] = btScalar(m_el[1].y()); 221 m[6] = btScalar(m_el[2].y()); 222 m[7] = btScalar(0.0); 223 m[8] = btScalar(m_el[0].z()); 224 m[9] = btScalar(m_el[1].z()); 225 m[10] = btScalar(m_el[2].z()); 226 m[11] = btScalar(0.0); 227 } 228 229 /**@brief Get the matrix represented as a quaternion 230 * @param q The quaternion which will be set */ 231 void getRotation(btQuaternion& q) const 232 { 233 btScalar trace = m_el[0].x() + m_el[1].y() + m_el[2].z(); 234 btScalar temp[4]; 235 236 if (trace > btScalar(0.0)) 237 { 238 btScalar s = btSqrt(trace + btScalar(1.0)); 239 temp[3]=(s * btScalar(0.5)); 240 s = btScalar(0.5) / s; 241 242 temp[0]=((m_el[2].y() - m_el[1].z()) * s); 243 temp[1]=((m_el[0].z() - m_el[2].x()) * s); 244 temp[2]=((m_el[1].x() - m_el[0].y()) * s); 245 } 246 else 247 { 248 int i = m_el[0].x() < m_el[1].y() ? 249 (m_el[1].y() < m_el[2].z() ? 2 : 1) : 250 (m_el[0].x() < m_el[2].z() ? 2 : 0); 251 int j = (i + 1) % 3; 252 int k = (i + 2) % 3; 253 254 btScalar s = btSqrt(m_el[i][i] - m_el[j][j] - m_el[k][k] + btScalar(1.0)); 255 temp[i] = s * btScalar(0.5); 256 s = btScalar(0.5) / s; 257 258 temp[3] = (m_el[k][j] - m_el[j][k]) * s; 259 temp[j] = (m_el[j][i] + m_el[i][j]) * s; 260 temp[k] = (m_el[k][i] + m_el[i][k]) * s; 261 } 262 q.setValue(temp[0],temp[1],temp[2],temp[3]); 263 } 264 265 /**@brief Get the matrix represented as euler angles around YXZ, roundtrip with setEulerYPR 266 * @param yaw Yaw around Y axis 267 * @param pitch Pitch around X axis 268 * @param roll around Z axis */ 269 void getEulerYPR(btScalar& yaw, btScalar& pitch, btScalar& roll) const 270 { 271 272 // first use the normal calculus 273 yaw = btScalar(btAtan2(m_el[1].x(), m_el[0].x())); 274 pitch = btScalar(btAsin(-m_el[2].x())); 275 roll = btScalar(btAtan2(m_el[2].y(), m_el[2].z())); 276 277 // on pitch = +/-HalfPI 278 if (btFabs(pitch)==SIMD_HALF_PI) 279 { 280 if (yaw>0) 281 yaw-=SIMD_PI; 282 else 283 yaw+=SIMD_PI; 284 285 if (roll>0) 286 roll-=SIMD_PI; 287 else 288 roll+=SIMD_PI; 289 } 290 }; 291 292 293 /**@brief Get the matrix represented as euler angles around ZYX 294 * @param yaw Yaw around X axis 295 * @param pitch Pitch around Y axis 296 * @param roll around X axis 297 * @param solution_number Which solution of two possible solutions ( 1 or 2) are possible values*/ 298 void getEulerZYX(btScalar& yaw, btScalar& pitch, btScalar& roll, unsigned int solution_number = 1) const 299 { 300 struct Euler 301 { 302 btScalar yaw; 303 btScalar pitch; 304 btScalar roll; 305 }; 306 307 Euler euler_out; 308 Euler euler_out2; //second solution 309 //get the pointer to the raw data 310 311 // Check that pitch is not at a singularity 312 if (btFabs(m_el[2].x()) >= 1) 313 { 314 euler_out.yaw = 0; 315 euler_out2.yaw = 0; 316 317 // From difference of angles formula 318 btScalar delta = btAtan2(m_el[0].x(),m_el[0].z()); 319 if (m_el[2].x() > 0) //gimbal locked up 320 { 321 euler_out.pitch = SIMD_PI / btScalar(2.0); 322 euler_out2.pitch = SIMD_PI / btScalar(2.0); 323 euler_out.roll = euler_out.pitch + delta; 324 euler_out2.roll = euler_out.pitch + delta; 325 } 326 else // gimbal locked down 327 { 328 euler_out.pitch = -SIMD_PI / btScalar(2.0); 329 euler_out2.pitch = -SIMD_PI / btScalar(2.0); 330 euler_out.roll = -euler_out.pitch + delta; 331 euler_out2.roll = -euler_out.pitch + delta; 332 } 333 } 334 else 335 { 336 euler_out.pitch = - btAsin(m_el[2].x()); 337 euler_out2.pitch = SIMD_PI - euler_out.pitch; 338 339 euler_out.roll = btAtan2(m_el[2].y()/btCos(euler_out.pitch), 340 m_el[2].z()/btCos(euler_out.pitch)); 341 euler_out2.roll = btAtan2(m_el[2].y()/btCos(euler_out2.pitch), 342 m_el[2].z()/btCos(euler_out2.pitch)); 343 344 euler_out.yaw = btAtan2(m_el[1].x()/btCos(euler_out.pitch), 345 m_el[0].x()/btCos(euler_out.pitch)); 346 euler_out2.yaw = btAtan2(m_el[1].x()/btCos(euler_out2.pitch), 347 m_el[0].x()/btCos(euler_out2.pitch)); 348 } 349 350 if (solution_number == 1) 190 351 { 191 setValue(btScalar(1.0), btScalar(0.0), btScalar(0.0),192 btScalar(0.0), btScalar(1.0), btScalar(0.0),193 btScalar(0.0), btScalar(0.0), btScalar(1.0));352 yaw = euler_out.yaw; 353 pitch = euler_out.pitch; 354 roll = euler_out.roll; 194 355 } 195 196 static const btMatrix3x3& getIdentity() 356 else 357 { 358 yaw = euler_out2.yaw; 359 pitch = euler_out2.pitch; 360 roll = euler_out2.roll; 361 } 362 } 363 364 /**@brief Create a scaled copy of the matrix 365 * @param s Scaling vector The elements of the vector will scale each column */ 366 367 btMatrix3x3 scaled(const btVector3& s) const 368 { 369 return btMatrix3x3(m_el[0].x() * s.x(), m_el[0].y() * s.y(), m_el[0].z() * s.z(), 370 m_el[1].x() * s.x(), m_el[1].y() * s.y(), m_el[1].z() * s.z(), 371 m_el[2].x() * s.x(), m_el[2].y() * s.y(), m_el[2].z() * s.z()); 372 } 373 374 /**@brief Return the determinant of the matrix */ 375 btScalar determinant() const; 376 /**@brief Return the adjoint of the matrix */ 377 btMatrix3x3 adjoint() const; 378 /**@brief Return the matrix with all values non negative */ 379 btMatrix3x3 absolute() const; 380 /**@brief Return the transpose of the matrix */ 381 btMatrix3x3 transpose() const; 382 /**@brief Return the inverse of the matrix */ 383 btMatrix3x3 inverse() const; 384 385 btMatrix3x3 transposeTimes(const btMatrix3x3& m) const; 386 btMatrix3x3 timesTranspose(const btMatrix3x3& m) const; 387 388 SIMD_FORCE_INLINE btScalar tdotx(const btVector3& v) const 389 { 390 return m_el[0].x() * v.x() + m_el[1].x() * v.y() + m_el[2].x() * v.z(); 391 } 392 SIMD_FORCE_INLINE btScalar tdoty(const btVector3& v) const 393 { 394 return m_el[0].y() * v.x() + m_el[1].y() * v.y() + m_el[2].y() * v.z(); 395 } 396 SIMD_FORCE_INLINE btScalar tdotz(const btVector3& v) const 397 { 398 return m_el[0].z() * v.x() + m_el[1].z() * v.y() + m_el[2].z() * v.z(); 399 } 400 401 402 /**@brief diagonalizes this matrix by the Jacobi method. 403 * @param rot stores the rotation from the coordinate system in which the matrix is diagonal to the original 404 * coordinate system, i.e., old_this = rot * new_this * rot^T. 405 * @param threshold See iteration 406 * @param iteration The iteration stops when all off-diagonal elements are less than the threshold multiplied 407 * by the sum of the absolute values of the diagonal, or when maxSteps have been executed. 408 * 409 * Note that this matrix is assumed to be symmetric. 410 */ 411 void diagonalize(btMatrix3x3& rot, btScalar threshold, int maxSteps) 412 { 413 rot.setIdentity(); 414 for (int step = maxSteps; step > 0; step--) 197 415 { 198 static const btMatrix3x3 identityMatrix(btScalar(1.0), btScalar(0.0), btScalar(0.0),199 btScalar(0.0), btScalar(1.0), btScalar(0.0),200 btScalar(0.0), btScalar(0.0), btScalar(1.0));201 return identityMatrix;202 }203 204 /**@brief Fill the values of the matrix into a 9 element array205 * @param m The array to be filled */206 void getOpenGLSubMatrix(btScalar *m) const207 {208 m[0] = btScalar(m_el[0].x());209 m[1] = btScalar(m_el[1].x());210 m[2] = btScalar(m_el[2].x());211 m[3] = btScalar(0.0);212 m[4] = btScalar(m_el[0].y());213 m[5] = btScalar(m_el[1].y());214 m[6] = btScalar(m_el[2].y());215 m[7] = btScalar(0.0);216 m[8] = btScalar(m_el[0].z());217 m[9] = btScalar(m_el[1].z());218 m[10] = btScalar(m_el[2].z());219 m[11] = btScalar(0.0);220 }221 222 /**@brief Get the matrix represented as a quaternion223 * @param q The quaternion which will be set */224 void getRotation(btQuaternion& q) const225 {226 btScalar trace = m_el[0].x() + m_el[1].y() + m_el[2].z();227 btScalar temp[4];228 229 if (trace > btScalar(0.0))230 {231 btScalar s = btSqrt(trace + btScalar(1.0));232 temp[3]=(s * btScalar(0.5));233 s = btScalar(0.5) / s;234 235 temp[0]=((m_el[2].y() - m_el[1].z()) * s);236 temp[1]=((m_el[0].z() - m_el[2].x()) * s);237 temp[2]=((m_el[1].x() - m_el[0].y()) * s);238 }239 else240 {241 int i = m_el[0].x() < m_el[1].y() ?242 (m_el[1].y() < m_el[2].z() ? 2 : 1) :243 (m_el[0].x() < m_el[2].z() ? 2 : 0);244 int j = (i + 1) % 3;245 int k = (i + 2) % 3;246 247 btScalar s = btSqrt(m_el[i][i] - m_el[j][j] - m_el[k][k] + btScalar(1.0));248 temp[i] = s * btScalar(0.5);249 s = btScalar(0.5) / s;250 251 temp[3] = (m_el[k][j] - m_el[j][k]) * s;252 temp[j] = (m_el[j][i] + m_el[i][j]) * s;253 temp[k] = (m_el[k][i] + m_el[i][k]) * s;254 }255 q.setValue(temp[0],temp[1],temp[2],temp[3]);256 }257 258 /**@brief Get the matrix represented as euler angles around YXZ, roundtrip with setEulerYPR259 * @param yaw Yaw around Y axis260 * @param pitch Pitch around X axis261 * @param roll around Z axis */262 void getEulerYPR(btScalar& yaw, btScalar& pitch, btScalar& roll) const263 {264 265 // first use the normal calculus266 yaw = btScalar(btAtan2(m_el[1].x(), m_el[0].x()));267 pitch = btScalar(btAsin(-m_el[2].x()));268 roll = btScalar(btAtan2(m_el[2].y(), m_el[2].z()));269 270 // on pitch = +/-HalfPI271 if (btFabs(pitch)==SIMD_HALF_PI)272 {273 if (yaw>0)274 yaw-=SIMD_PI;275 else276 yaw+=SIMD_PI;277 278 if (roll>0)279 roll-=SIMD_PI;280 else281 roll+=SIMD_PI;282 }283 };284 285 286 /**@brief Get the matrix represented as euler angles around ZYX287 * @param yaw Yaw around X axis288 * @param pitch Pitch around Y axis289 * @param roll around X axis290 * @param solution_number Which solution of two possible solutions ( 1 or 2) are possible values*/291 void getEulerZYX(btScalar& yaw, btScalar& pitch, btScalar& roll, unsigned int solution_number = 1) const292 {293 struct Euler{btScalar yaw, pitch, roll;};294 Euler euler_out;295 Euler euler_out2; //second solution296 //get the pointer to the raw data297 298 // Check that pitch is not at a singularity299 if (btFabs(m_el[2].x()) >= 1)300 {301 euler_out.yaw = 0;302 euler_out2.yaw = 0;303 304 // From difference of angles formula305 btScalar delta = btAtan2(m_el[0].x(),m_el[0].z());306 if (m_el[2].x() > 0) //gimbal locked up307 {308 euler_out.pitch = SIMD_PI / btScalar(2.0);309 euler_out2.pitch = SIMD_PI / btScalar(2.0);310 euler_out.roll = euler_out.pitch + delta;311 euler_out2.roll = euler_out.pitch + delta;312 }313 else // gimbal locked down314 {315 euler_out.pitch = -SIMD_PI / btScalar(2.0);316 euler_out2.pitch = -SIMD_PI / btScalar(2.0);317 euler_out.roll = -euler_out.pitch + delta;318 euler_out2.roll = -euler_out.pitch + delta;319 }320 }321 else322 {323 euler_out.pitch = - btAsin(m_el[2].x());324 euler_out2.pitch = SIMD_PI - euler_out.pitch;325 326 euler_out.roll = btAtan2(m_el[2].y()/btCos(euler_out.pitch),327 m_el[2].z()/btCos(euler_out.pitch));328 euler_out2.roll = btAtan2(m_el[2].y()/btCos(euler_out2.pitch),329 m_el[2].z()/btCos(euler_out2.pitch));330 331 euler_out.yaw = btAtan2(m_el[1].x()/btCos(euler_out.pitch),332 m_el[0].x()/btCos(euler_out.pitch));333 euler_out2.yaw = btAtan2(m_el[1].x()/btCos(euler_out2.pitch),334 m_el[0].x()/btCos(euler_out2.pitch));335 }336 337 if (solution_number == 1)338 {339 yaw = euler_out.yaw;340 pitch = euler_out.pitch;341 roll = euler_out.roll;342 }343 else344 {345 yaw = euler_out2.yaw;346 pitch = euler_out2.pitch;347 roll = euler_out2.roll;348 }349 }350 351 /**@brief Create a scaled copy of the matrix352 * @param s Scaling vector The elements of the vector will scale each column */353 354 btMatrix3x3 scaled(const btVector3& s) const355 {356 return btMatrix3x3(m_el[0].x() * s.x(), m_el[0].y() * s.y(), m_el[0].z() * s.z(),357 m_el[1].x() * s.x(), m_el[1].y() * s.y(), m_el[1].z() * s.z(),358 m_el[2].x() * s.x(), m_el[2].y() * s.y(), m_el[2].z() * s.z());359 }360 361 /**@brief Return the determinant of the matrix */362 btScalar determinant() const;363 /**@brief Return the adjoint of the matrix */364 btMatrix3x3 adjoint() const;365 /**@brief Return the matrix with all values non negative */366 btMatrix3x3 absolute() const;367 /**@brief Return the transpose of the matrix */368 btMatrix3x3 transpose() const;369 /**@brief Return the inverse of the matrix */370 btMatrix3x3 inverse() const;371 372 btMatrix3x3 transposeTimes(const btMatrix3x3& m) const;373 btMatrix3x3 timesTranspose(const btMatrix3x3& m) const;374 375 SIMD_FORCE_INLINE btScalar tdotx(const btVector3& v) const376 {377 return m_el[0].x() * v.x() + m_el[1].x() * v.y() + m_el[2].x() * v.z();378 }379 SIMD_FORCE_INLINE btScalar tdoty(const btVector3& v) const380 {381 return m_el[0].y() * v.x() + m_el[1].y() * v.y() + m_el[2].y() * v.z();382 }383 SIMD_FORCE_INLINE btScalar tdotz(const btVector3& v) const384 {385 return m_el[0].z() * v.x() + m_el[1].z() * v.y() + m_el[2].z() * v.z();386 }387 388 389 /**@brief diagonalizes this matrix by the Jacobi method.390 * @param rot stores the rotation from the coordinate system in which the matrix is diagonal to the original391 * coordinate system, i.e., old_this = rot * new_this * rot^T.392 * @param threshold See iteration393 * @param iteration The iteration stops when all off-diagonal elements are less than the threshold multiplied394 * by the sum of the absolute values of the diagonal, or when maxSteps have been executed.395 *396 * Note that this matrix is assumed to be symmetric.397 */398 void diagonalize(btMatrix3x3& rot, btScalar threshold, int maxSteps)399 {400 rot.setIdentity();401 for (int step = maxSteps; step > 0; step--)402 {403 416 // find off-diagonal element [p][q] with largest magnitude 404 417 int p = 0; … … 409 422 if (v > max) 410 423 { 411 412 413 424 q = 2; 425 r = 1; 426 max = v; 414 427 } 415 428 v = btFabs(m_el[1][2]); 416 429 if (v > max) 417 430 { 418 419 420 421 431 p = 1; 432 q = 2; 433 r = 0; 434 max = v; 422 435 } 423 436 … … 425 438 if (max <= t) 426 439 { 427 428 429 430 431 440 if (max <= SIMD_EPSILON * t) 441 { 442 return; 443 } 444 step = 1; 432 445 } 433 446 … … 440 453 if (theta2 * theta2 < btScalar(10 / SIMD_EPSILON)) 441 454 { 442 443 444 445 455 t = (theta >= 0) ? 1 / (theta + btSqrt(1 + theta2)) 456 : 1 / (theta - btSqrt(1 + theta2)); 457 cos = 1 / btSqrt(1 + t * t); 458 sin = cos * t; 446 459 } 447 460 else 448 461 { 449 450 451 452 462 // approximation for large theta-value, i.e., a nearly diagonal matrix 463 t = 1 / (theta * (2 + btScalar(0.5) / theta2)); 464 cos = 1 - btScalar(0.5) * t * t; 465 sin = cos * t; 453 466 } 454 467 … … 465 478 for (int i = 0; i < 3; i++) 466 479 { 467 btVector3& row = rot[i]; 468 mrp = row[p]; 469 mrq = row[q]; 470 row[p] = cos * mrp - sin * mrq; 471 row[q] = cos * mrq + sin * mrp; 472 } 473 } 480 btVector3& row = rot[i]; 481 mrp = row[p]; 482 mrq = row[q]; 483 row[p] = cos * mrp - sin * mrq; 484 row[q] = cos * mrq + sin * mrp; 485 } 474 486 } 475 476 477 478 protected: 479 /**@brief Calculate the matrix cofactor 480 * @param r1 The first row to use for calculating the cofactor 481 * @param c1 The first column to use for calculating the cofactor 482 * @param r1 The second row to use for calculating the cofactor 483 * @param c1 The second column to use for calculating the cofactor 484 * See http://en.wikipedia.org/wiki/Cofactor_(linear_algebra) for more details 485 */ 486 btScalar cofac(int r1, int c1, int r2, int c2) const 487 { 488 return m_el[r1][c1] * m_el[r2][c2] - m_el[r1][c2] * m_el[r2][c1]; 489 } 490 ///Data storage for the matrix, each vector is a row of the matrix 491 btVector3 m_el[3]; 492 }; 487 } 488 489 490 491 492 /**@brief Calculate the matrix cofactor 493 * @param r1 The first row to use for calculating the cofactor 494 * @param c1 The first column to use for calculating the cofactor 495 * @param r1 The second row to use for calculating the cofactor 496 * @param c1 The second column to use for calculating the cofactor 497 * See http://en.wikipedia.org/wiki/Cofactor_(linear_algebra) for more details 498 */ 499 btScalar cofac(int r1, int c1, int r2, int c2) const 500 { 501 return m_el[r1][c1] * m_el[r2][c2] - m_el[r1][c2] * m_el[r2][c1]; 502 } 503 504 void serialize(struct btMatrix3x3Data& dataOut) const; 505 506 void serializeFloat(struct btMatrix3x3FloatData& dataOut) const; 507 508 void deSerialize(const struct btMatrix3x3Data& dataIn); 509 510 void deSerializeFloat(const struct btMatrix3x3FloatData& dataIn); 511 512 void deSerializeDouble(const struct btMatrix3x3DoubleData& dataIn); 513 514 }; 515 516 517 SIMD_FORCE_INLINE btMatrix3x3& 518 btMatrix3x3::operator*=(const btMatrix3x3& m) 519 { 520 setValue(m.tdotx(m_el[0]), m.tdoty(m_el[0]), m.tdotz(m_el[0]), 521 m.tdotx(m_el[1]), m.tdoty(m_el[1]), m.tdotz(m_el[1]), 522 m.tdotx(m_el[2]), m.tdoty(m_el[2]), m.tdotz(m_el[2])); 523 return *this; 524 } 525 526 SIMD_FORCE_INLINE btScalar 527 btMatrix3x3::determinant() const 528 { 529 return btTriple((*this)[0], (*this)[1], (*this)[2]); 530 } 531 532 533 SIMD_FORCE_INLINE btMatrix3x3 534 btMatrix3x3::absolute() const 535 { 536 return btMatrix3x3( 537 btFabs(m_el[0].x()), btFabs(m_el[0].y()), btFabs(m_el[0].z()), 538 btFabs(m_el[1].x()), btFabs(m_el[1].y()), btFabs(m_el[1].z()), 539 btFabs(m_el[2].x()), btFabs(m_el[2].y()), btFabs(m_el[2].z())); 540 } 541 542 SIMD_FORCE_INLINE btMatrix3x3 543 btMatrix3x3::transpose() const 544 { 545 return btMatrix3x3(m_el[0].x(), m_el[1].x(), m_el[2].x(), 546 m_el[0].y(), m_el[1].y(), m_el[2].y(), 547 m_el[0].z(), m_el[1].z(), m_el[2].z()); 548 } 549 550 SIMD_FORCE_INLINE btMatrix3x3 551 btMatrix3x3::adjoint() const 552 { 553 return btMatrix3x3(cofac(1, 1, 2, 2), cofac(0, 2, 2, 1), cofac(0, 1, 1, 2), 554 cofac(1, 2, 2, 0), cofac(0, 0, 2, 2), cofac(0, 2, 1, 0), 555 cofac(1, 0, 2, 1), cofac(0, 1, 2, 0), cofac(0, 0, 1, 1)); 556 } 557 558 SIMD_FORCE_INLINE btMatrix3x3 559 btMatrix3x3::inverse() const 560 { 561 btVector3 co(cofac(1, 1, 2, 2), cofac(1, 2, 2, 0), cofac(1, 0, 2, 1)); 562 btScalar det = (*this)[0].dot(co); 563 btFullAssert(det != btScalar(0.0)); 564 btScalar s = btScalar(1.0) / det; 565 return btMatrix3x3(co.x() * s, cofac(0, 2, 2, 1) * s, cofac(0, 1, 1, 2) * s, 566 co.y() * s, cofac(0, 0, 2, 2) * s, cofac(0, 2, 1, 0) * s, 567 co.z() * s, cofac(0, 1, 2, 0) * s, cofac(0, 0, 1, 1) * s); 568 } 569 570 SIMD_FORCE_INLINE btMatrix3x3 571 btMatrix3x3::transposeTimes(const btMatrix3x3& m) const 572 { 573 return btMatrix3x3( 574 m_el[0].x() * m[0].x() + m_el[1].x() * m[1].x() + m_el[2].x() * m[2].x(), 575 m_el[0].x() * m[0].y() + m_el[1].x() * m[1].y() + m_el[2].x() * m[2].y(), 576 m_el[0].x() * m[0].z() + m_el[1].x() * m[1].z() + m_el[2].x() * m[2].z(), 577 m_el[0].y() * m[0].x() + m_el[1].y() * m[1].x() + m_el[2].y() * m[2].x(), 578 m_el[0].y() * m[0].y() + m_el[1].y() * m[1].y() + m_el[2].y() * m[2].y(), 579 m_el[0].y() * m[0].z() + m_el[1].y() * m[1].z() + m_el[2].y() * m[2].z(), 580 m_el[0].z() * m[0].x() + m_el[1].z() * m[1].x() + m_el[2].z() * m[2].x(), 581 m_el[0].z() * m[0].y() + m_el[1].z() * m[1].y() + m_el[2].z() * m[2].y(), 582 m_el[0].z() * m[0].z() + m_el[1].z() * m[1].z() + m_el[2].z() * m[2].z()); 583 } 584 585 SIMD_FORCE_INLINE btMatrix3x3 586 btMatrix3x3::timesTranspose(const btMatrix3x3& m) const 587 { 588 return btMatrix3x3( 589 m_el[0].dot(m[0]), m_el[0].dot(m[1]), m_el[0].dot(m[2]), 590 m_el[1].dot(m[0]), m_el[1].dot(m[1]), m_el[1].dot(m[2]), 591 m_el[2].dot(m[0]), m_el[2].dot(m[1]), m_el[2].dot(m[2])); 592 593 } 594 595 SIMD_FORCE_INLINE btVector3 596 operator*(const btMatrix3x3& m, const btVector3& v) 597 { 598 return btVector3(m[0].dot(v), m[1].dot(v), m[2].dot(v)); 599 } 600 601 602 SIMD_FORCE_INLINE btVector3 603 operator*(const btVector3& v, const btMatrix3x3& m) 604 { 605 return btVector3(m.tdotx(v), m.tdoty(v), m.tdotz(v)); 606 } 607 608 SIMD_FORCE_INLINE btMatrix3x3 609 operator*(const btMatrix3x3& m1, const btMatrix3x3& m2) 610 { 611 return btMatrix3x3( 612 m2.tdotx( m1[0]), m2.tdoty( m1[0]), m2.tdotz( m1[0]), 613 m2.tdotx( m1[1]), m2.tdoty( m1[1]), m2.tdotz( m1[1]), 614 m2.tdotx( m1[2]), m2.tdoty( m1[2]), m2.tdotz( m1[2])); 615 } 616 617 /* 618 SIMD_FORCE_INLINE btMatrix3x3 btMultTransposeLeft(const btMatrix3x3& m1, const btMatrix3x3& m2) { 619 return btMatrix3x3( 620 m1[0][0] * m2[0][0] + m1[1][0] * m2[1][0] + m1[2][0] * m2[2][0], 621 m1[0][0] * m2[0][1] + m1[1][0] * m2[1][1] + m1[2][0] * m2[2][1], 622 m1[0][0] * m2[0][2] + m1[1][0] * m2[1][2] + m1[2][0] * m2[2][2], 623 m1[0][1] * m2[0][0] + m1[1][1] * m2[1][0] + m1[2][1] * m2[2][0], 624 m1[0][1] * m2[0][1] + m1[1][1] * m2[1][1] + m1[2][1] * m2[2][1], 625 m1[0][1] * m2[0][2] + m1[1][1] * m2[1][2] + m1[2][1] * m2[2][2], 626 m1[0][2] * m2[0][0] + m1[1][2] * m2[1][0] + m1[2][2] * m2[2][0], 627 m1[0][2] * m2[0][1] + m1[1][2] * m2[1][1] + m1[2][2] * m2[2][1], 628 m1[0][2] * m2[0][2] + m1[1][2] * m2[1][2] + m1[2][2] * m2[2][2]); 629 } 630 */ 631 632 /**@brief Equality operator between two matrices 633 * It will test all elements are equal. */ 634 SIMD_FORCE_INLINE bool operator==(const btMatrix3x3& m1, const btMatrix3x3& m2) 635 { 636 return ( m1[0][0] == m2[0][0] && m1[1][0] == m2[1][0] && m1[2][0] == m2[2][0] && 637 m1[0][1] == m2[0][1] && m1[1][1] == m2[1][1] && m1[2][1] == m2[2][1] && 638 m1[0][2] == m2[0][2] && m1[1][2] == m2[1][2] && m1[2][2] == m2[2][2] ); 639 } 640 641 ///for serialization 642 struct btMatrix3x3FloatData 643 { 644 btVector3FloatData m_el[3]; 645 }; 646 647 ///for serialization 648 struct btMatrix3x3DoubleData 649 { 650 btVector3DoubleData m_el[3]; 651 }; 652 653 493 654 494 SIMD_FORCE_INLINE btMatrix3x3& 495 btMatrix3x3::operator*=(const btMatrix3x3& m) 496 { 497 setValue(m.tdotx(m_el[0]), m.tdoty(m_el[0]), m.tdotz(m_el[0]), 498 m.tdotx(m_el[1]), m.tdoty(m_el[1]), m.tdotz(m_el[1]), 499 m.tdotx(m_el[2]), m.tdoty(m_el[2]), m.tdotz(m_el[2])); 500 return *this; 501 } 502 503 SIMD_FORCE_INLINE btScalar 504 btMatrix3x3::determinant() const 505 { 506 return triple((*this)[0], (*this)[1], (*this)[2]); 507 } 508 509 510 SIMD_FORCE_INLINE btMatrix3x3 511 btMatrix3x3::absolute() const 512 { 513 return btMatrix3x3( 514 btFabs(m_el[0].x()), btFabs(m_el[0].y()), btFabs(m_el[0].z()), 515 btFabs(m_el[1].x()), btFabs(m_el[1].y()), btFabs(m_el[1].z()), 516 btFabs(m_el[2].x()), btFabs(m_el[2].y()), btFabs(m_el[2].z())); 517 } 518 519 SIMD_FORCE_INLINE btMatrix3x3 520 btMatrix3x3::transpose() const 521 { 522 return btMatrix3x3(m_el[0].x(), m_el[1].x(), m_el[2].x(), 523 m_el[0].y(), m_el[1].y(), m_el[2].y(), 524 m_el[0].z(), m_el[1].z(), m_el[2].z()); 525 } 526 527 SIMD_FORCE_INLINE btMatrix3x3 528 btMatrix3x3::adjoint() const 529 { 530 return btMatrix3x3(cofac(1, 1, 2, 2), cofac(0, 2, 2, 1), cofac(0, 1, 1, 2), 531 cofac(1, 2, 2, 0), cofac(0, 0, 2, 2), cofac(0, 2, 1, 0), 532 cofac(1, 0, 2, 1), cofac(0, 1, 2, 0), cofac(0, 0, 1, 1)); 533 } 534 535 SIMD_FORCE_INLINE btMatrix3x3 536 btMatrix3x3::inverse() const 537 { 538 btVector3 co(cofac(1, 1, 2, 2), cofac(1, 2, 2, 0), cofac(1, 0, 2, 1)); 539 btScalar det = (*this)[0].dot(co); 540 btFullAssert(det != btScalar(0.0)); 541 btScalar s = btScalar(1.0) / det; 542 return btMatrix3x3(co.x() * s, cofac(0, 2, 2, 1) * s, cofac(0, 1, 1, 2) * s, 543 co.y() * s, cofac(0, 0, 2, 2) * s, cofac(0, 2, 1, 0) * s, 544 co.z() * s, cofac(0, 1, 2, 0) * s, cofac(0, 0, 1, 1) * s); 545 } 546 547 SIMD_FORCE_INLINE btMatrix3x3 548 btMatrix3x3::transposeTimes(const btMatrix3x3& m) const 549 { 550 return btMatrix3x3( 551 m_el[0].x() * m[0].x() + m_el[1].x() * m[1].x() + m_el[2].x() * m[2].x(), 552 m_el[0].x() * m[0].y() + m_el[1].x() * m[1].y() + m_el[2].x() * m[2].y(), 553 m_el[0].x() * m[0].z() + m_el[1].x() * m[1].z() + m_el[2].x() * m[2].z(), 554 m_el[0].y() * m[0].x() + m_el[1].y() * m[1].x() + m_el[2].y() * m[2].x(), 555 m_el[0].y() * m[0].y() + m_el[1].y() * m[1].y() + m_el[2].y() * m[2].y(), 556 m_el[0].y() * m[0].z() + m_el[1].y() * m[1].z() + m_el[2].y() * m[2].z(), 557 m_el[0].z() * m[0].x() + m_el[1].z() * m[1].x() + m_el[2].z() * m[2].x(), 558 m_el[0].z() * m[0].y() + m_el[1].z() * m[1].y() + m_el[2].z() * m[2].y(), 559 m_el[0].z() * m[0].z() + m_el[1].z() * m[1].z() + m_el[2].z() * m[2].z()); 560 } 561 562 SIMD_FORCE_INLINE btMatrix3x3 563 btMatrix3x3::timesTranspose(const btMatrix3x3& m) const 564 { 565 return btMatrix3x3( 566 m_el[0].dot(m[0]), m_el[0].dot(m[1]), m_el[0].dot(m[2]), 567 m_el[1].dot(m[0]), m_el[1].dot(m[1]), m_el[1].dot(m[2]), 568 m_el[2].dot(m[0]), m_el[2].dot(m[1]), m_el[2].dot(m[2])); 569 570 } 571 572 SIMD_FORCE_INLINE btVector3 573 operator*(const btMatrix3x3& m, const btVector3& v) 574 { 575 return btVector3(m[0].dot(v), m[1].dot(v), m[2].dot(v)); 576 } 577 578 579 SIMD_FORCE_INLINE btVector3 580 operator*(const btVector3& v, const btMatrix3x3& m) 581 { 582 return btVector3(m.tdotx(v), m.tdoty(v), m.tdotz(v)); 583 } 584 585 SIMD_FORCE_INLINE btMatrix3x3 586 operator*(const btMatrix3x3& m1, const btMatrix3x3& m2) 587 { 588 return btMatrix3x3( 589 m2.tdotx( m1[0]), m2.tdoty( m1[0]), m2.tdotz( m1[0]), 590 m2.tdotx( m1[1]), m2.tdoty( m1[1]), m2.tdotz( m1[1]), 591 m2.tdotx( m1[2]), m2.tdoty( m1[2]), m2.tdotz( m1[2])); 592 } 593 594 /* 595 SIMD_FORCE_INLINE btMatrix3x3 btMultTransposeLeft(const btMatrix3x3& m1, const btMatrix3x3& m2) { 596 return btMatrix3x3( 597 m1[0][0] * m2[0][0] + m1[1][0] * m2[1][0] + m1[2][0] * m2[2][0], 598 m1[0][0] * m2[0][1] + m1[1][0] * m2[1][1] + m1[2][0] * m2[2][1], 599 m1[0][0] * m2[0][2] + m1[1][0] * m2[1][2] + m1[2][0] * m2[2][2], 600 m1[0][1] * m2[0][0] + m1[1][1] * m2[1][0] + m1[2][1] * m2[2][0], 601 m1[0][1] * m2[0][1] + m1[1][1] * m2[1][1] + m1[2][1] * m2[2][1], 602 m1[0][1] * m2[0][2] + m1[1][1] * m2[1][2] + m1[2][1] * m2[2][2], 603 m1[0][2] * m2[0][0] + m1[1][2] * m2[1][0] + m1[2][2] * m2[2][0], 604 m1[0][2] * m2[0][1] + m1[1][2] * m2[1][1] + m1[2][2] * m2[2][1], 605 m1[0][2] * m2[0][2] + m1[1][2] * m2[1][2] + m1[2][2] * m2[2][2]); 606 } 607 */ 608 609 /**@brief Equality operator between two matrices 610 * It will test all elements are equal. */ 611 SIMD_FORCE_INLINE bool operator==(const btMatrix3x3& m1, const btMatrix3x3& m2) 612 { 613 return ( m1[0][0] == m2[0][0] && m1[1][0] == m2[1][0] && m1[2][0] == m2[2][0] && 614 m1[0][1] == m2[0][1] && m1[1][1] == m2[1][1] && m1[2][1] == m2[2][1] && 615 m1[0][2] == m2[0][2] && m1[1][2] == m2[1][2] && m1[2][2] == m2[2][2] ); 616 } 617 618 #endif 655 656 SIMD_FORCE_INLINE void btMatrix3x3::serialize(struct btMatrix3x3Data& dataOut) const 657 { 658 for (int i=0;i<3;i++) 659 m_el[i].serialize(dataOut.m_el[i]); 660 } 661 662 SIMD_FORCE_INLINE void btMatrix3x3::serializeFloat(struct btMatrix3x3FloatData& dataOut) const 663 { 664 for (int i=0;i<3;i++) 665 m_el[i].serializeFloat(dataOut.m_el[i]); 666 } 667 668 669 SIMD_FORCE_INLINE void btMatrix3x3::deSerialize(const struct btMatrix3x3Data& dataIn) 670 { 671 for (int i=0;i<3;i++) 672 m_el[i].deSerialize(dataIn.m_el[i]); 673 } 674 675 SIMD_FORCE_INLINE void btMatrix3x3::deSerializeFloat(const struct btMatrix3x3FloatData& dataIn) 676 { 677 for (int i=0;i<3;i++) 678 m_el[i].deSerializeFloat(dataIn.m_el[i]); 679 } 680 681 SIMD_FORCE_INLINE void btMatrix3x3::deSerializeDouble(const struct btMatrix3x3DoubleData& dataIn) 682 { 683 for (int i=0;i<3;i++) 684 m_el[i].deSerializeDouble(dataIn.m_el[i]); 685 } 686 687 #endif //BT_MATRIX3x3_H 688
Note: See TracChangeset
for help on using the changeset viewer.
