| 1 | /* |
|---|
| 2 | Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/ |
|---|
| 3 | |
|---|
| 4 | This software is provided 'as-is', without any express or implied warranty. |
|---|
| 5 | In no event will the authors be held liable for any damages arising from the use of this software. |
|---|
| 6 | Permission is granted to anyone to use this software for any purpose, |
|---|
| 7 | including commercial applications, and to alter it and redistribute it freely, |
|---|
| 8 | subject to the following restrictions: |
|---|
| 9 | |
|---|
| 10 | 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. |
|---|
| 11 | 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. |
|---|
| 12 | 3. This notice may not be removed or altered from any source distribution. |
|---|
| 13 | */ |
|---|
| 14 | |
|---|
| 15 | |
|---|
| 16 | #ifndef btMatrix3x3_H |
|---|
| 17 | #define btMatrix3x3_H |
|---|
| 18 | |
|---|
| 19 | #include "btScalar.h" |
|---|
| 20 | |
|---|
| 21 | #include "btVector3.h" |
|---|
| 22 | #include "btQuaternion.h" |
|---|
| 23 | |
|---|
| 24 | |
|---|
| 25 | |
|---|
| 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 | 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 */ |
|---|
| 108 | void setFromOpenGLSubMatrix(const btScalar *m) |
|---|
| 109 | { |
|---|
| 110 | m_el[0].setValue(m[0],m[4],m[8]); |
|---|
| 111 | m_el[1].setValue(m[1],m[5],m[9]); |
|---|
| 112 | m_el[2].setValue(m[2],m[6],m[10]); |
|---|
| 113 | |
|---|
| 114 | } |
|---|
| 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*/ |
|---|
| 125 | void setValue(const btScalar& xx, const btScalar& xy, const btScalar& xz, |
|---|
| 126 | const btScalar& yx, const btScalar& yy, const btScalar& yz, |
|---|
| 127 | const btScalar& zx, const btScalar& zy, const btScalar& zz) |
|---|
| 128 | { |
|---|
| 129 | m_el[0].setValue(xx,xy,xz); |
|---|
| 130 | m_el[1].setValue(yx,yy,yz); |
|---|
| 131 | m_el[2].setValue(zx,zy,zz); |
|---|
| 132 | } |
|---|
| 133 | |
|---|
| 134 | /** @brief Set the matrix from a quaternion |
|---|
| 135 | * @param q The Quaternion to match */ |
|---|
| 136 | void setRotation(const btQuaternion& q) |
|---|
| 137 | { |
|---|
| 138 | btScalar d = q.length2(); |
|---|
| 139 | btFullAssert(d != btScalar(0.0)); |
|---|
| 140 | btScalar s = btScalar(2.0) / d; |
|---|
| 141 | btScalar xs = q.x() * s, ys = q.y() * s, zs = q.z() * s; |
|---|
| 142 | btScalar wx = q.w() * xs, wy = q.w() * ys, wz = q.w() * zs; |
|---|
| 143 | btScalar xx = q.x() * xs, xy = q.x() * ys, xz = q.x() * zs; |
|---|
| 144 | btScalar yy = q.y() * ys, yz = q.y() * zs, zz = q.z() * zs; |
|---|
| 145 | setValue(btScalar(1.0) - (yy + zz), xy - wz, xz + wy, |
|---|
| 146 | xy + wz, btScalar(1.0) - (xx + zz), yz - wx, |
|---|
| 147 | xz - wy, yz + wx, btScalar(1.0) - (xx + yy)); |
|---|
| 148 | } |
|---|
| 149 | |
|---|
| 150 | |
|---|
| 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 | */ |
|---|
| 156 | void setEulerYPR(const btScalar& yaw, const btScalar& pitch, const btScalar& roll) |
|---|
| 157 | { |
|---|
| 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 | * |
|---|
| 166 | * These angles are used to produce a rotation matrix. The euler |
|---|
| 167 | * angles are applied in ZYX order. I.e a vector is first rotated |
|---|
| 168 | * about X then Y and then Z |
|---|
| 169 | **/ |
|---|
| 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 |
|---|
| 172 | btScalar ci ( btCos(eulerX)); |
|---|
| 173 | btScalar cj ( btCos(eulerY)); |
|---|
| 174 | btScalar ch ( btCos(eulerZ)); |
|---|
| 175 | btScalar si ( btSin(eulerX)); |
|---|
| 176 | btScalar sj ( btSin(eulerY)); |
|---|
| 177 | btScalar sh ( btSin(eulerZ)); |
|---|
| 178 | btScalar cc = ci * ch; |
|---|
| 179 | btScalar cs = ci * sh; |
|---|
| 180 | btScalar sc = si * ch; |
|---|
| 181 | btScalar ss = si * sh; |
|---|
| 182 | |
|---|
| 183 | 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() |
|---|
| 190 | { |
|---|
| 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)); |
|---|
| 194 | } |
|---|
| 195 | |
|---|
| 196 | static const btMatrix3x3& getIdentity() |
|---|
| 197 | { |
|---|
| 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 array |
|---|
| 205 | * @param m The array to be filled */ |
|---|
| 206 | void getOpenGLSubMatrix(btScalar *m) const |
|---|
| 207 | { |
|---|
| 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 quaternion |
|---|
| 223 | * @param q The quaternion which will be set */ |
|---|
| 224 | void getRotation(btQuaternion& q) const |
|---|
| 225 | { |
|---|
| 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 | else |
|---|
| 240 | { |
|---|
| 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 setEulerYPR |
|---|
| 259 | * @param yaw Yaw around Y axis |
|---|
| 260 | * @param pitch Pitch around X axis |
|---|
| 261 | * @param roll around Z axis */ |
|---|
| 262 | void getEulerYPR(btScalar& yaw, btScalar& pitch, btScalar& roll) const |
|---|
| 263 | { |
|---|
| 264 | |
|---|
| 265 | // first use the normal calculus |
|---|
| 266 | 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 = +/-HalfPI |
|---|
| 271 | if (btFabs(pitch)==SIMD_HALF_PI) |
|---|
| 272 | { |
|---|
| 273 | if (yaw>0) |
|---|
| 274 | yaw-=SIMD_PI; |
|---|
| 275 | else |
|---|
| 276 | yaw+=SIMD_PI; |
|---|
| 277 | |
|---|
| 278 | if (roll>0) |
|---|
| 279 | roll-=SIMD_PI; |
|---|
| 280 | else |
|---|
| 281 | roll+=SIMD_PI; |
|---|
| 282 | } |
|---|
| 283 | }; |
|---|
| 284 | |
|---|
| 285 | |
|---|
| 286 | /**@brief Get the matrix represented as euler angles around ZYX |
|---|
| 287 | * @param yaw Yaw around X axis |
|---|
| 288 | * @param pitch Pitch around Y axis |
|---|
| 289 | * @param roll around X axis |
|---|
| 290 | * @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) const |
|---|
| 292 | { |
|---|
| 293 | struct Euler{btScalar yaw, pitch, roll;}; |
|---|
| 294 | Euler euler_out; |
|---|
| 295 | Euler euler_out2; //second solution |
|---|
| 296 | //get the pointer to the raw data |
|---|
| 297 | |
|---|
| 298 | // Check that pitch is not at a singularity |
|---|
| 299 | if (btFabs(m_el[2].x()) >= 1) |
|---|
| 300 | { |
|---|
| 301 | euler_out.yaw = 0; |
|---|
| 302 | euler_out2.yaw = 0; |
|---|
| 303 | |
|---|
| 304 | // From difference of angles formula |
|---|
| 305 | btScalar delta = btAtan2(m_el[0].x(),m_el[0].z()); |
|---|
| 306 | if (m_el[2].x() > 0) //gimbal locked up |
|---|
| 307 | { |
|---|
| 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 down |
|---|
| 314 | { |
|---|
| 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 | else |
|---|
| 322 | { |
|---|
| 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 | else |
|---|
| 344 | { |
|---|
| 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 matrix |
|---|
| 352 | * @param s Scaling vector The elements of the vector will scale each column */ |
|---|
| 353 | |
|---|
| 354 | btMatrix3x3 scaled(const btVector3& s) const |
|---|
| 355 | { |
|---|
| 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) const |
|---|
| 376 | { |
|---|
| 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) const |
|---|
| 380 | { |
|---|
| 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) const |
|---|
| 384 | { |
|---|
| 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 original |
|---|
| 391 | * coordinate system, i.e., old_this = rot * new_this * rot^T. |
|---|
| 392 | * @param threshold See iteration |
|---|
| 393 | * @param iteration The iteration stops when all off-diagonal elements are less than the threshold multiplied |
|---|
| 394 | * 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 | // find off-diagonal element [p][q] with largest magnitude |
|---|
| 404 | int p = 0; |
|---|
| 405 | int q = 1; |
|---|
| 406 | int r = 2; |
|---|
| 407 | btScalar max = btFabs(m_el[0][1]); |
|---|
| 408 | btScalar v = btFabs(m_el[0][2]); |
|---|
| 409 | if (v > max) |
|---|
| 410 | { |
|---|
| 411 | q = 2; |
|---|
| 412 | r = 1; |
|---|
| 413 | max = v; |
|---|
| 414 | } |
|---|
| 415 | v = btFabs(m_el[1][2]); |
|---|
| 416 | if (v > max) |
|---|
| 417 | { |
|---|
| 418 | p = 1; |
|---|
| 419 | q = 2; |
|---|
| 420 | r = 0; |
|---|
| 421 | max = v; |
|---|
| 422 | } |
|---|
| 423 | |
|---|
| 424 | btScalar t = threshold * (btFabs(m_el[0][0]) + btFabs(m_el[1][1]) + btFabs(m_el[2][2])); |
|---|
| 425 | if (max <= t) |
|---|
| 426 | { |
|---|
| 427 | if (max <= SIMD_EPSILON * t) |
|---|
| 428 | { |
|---|
| 429 | return; |
|---|
| 430 | } |
|---|
| 431 | step = 1; |
|---|
| 432 | } |
|---|
| 433 | |
|---|
| 434 | // compute Jacobi rotation J which leads to a zero for element [p][q] |
|---|
| 435 | btScalar mpq = m_el[p][q]; |
|---|
| 436 | btScalar theta = (m_el[q][q] - m_el[p][p]) / (2 * mpq); |
|---|
| 437 | btScalar theta2 = theta * theta; |
|---|
| 438 | btScalar cos; |
|---|
| 439 | btScalar sin; |
|---|
| 440 | if (theta2 * theta2 < btScalar(10 / SIMD_EPSILON)) |
|---|
| 441 | { |
|---|
| 442 | t = (theta >= 0) ? 1 / (theta + btSqrt(1 + theta2)) |
|---|
| 443 | : 1 / (theta - btSqrt(1 + theta2)); |
|---|
| 444 | cos = 1 / btSqrt(1 + t * t); |
|---|
| 445 | sin = cos * t; |
|---|
| 446 | } |
|---|
| 447 | else |
|---|
| 448 | { |
|---|
| 449 | // approximation for large theta-value, i.e., a nearly diagonal matrix |
|---|
| 450 | t = 1 / (theta * (2 + btScalar(0.5) / theta2)); |
|---|
| 451 | cos = 1 - btScalar(0.5) * t * t; |
|---|
| 452 | sin = cos * t; |
|---|
| 453 | } |
|---|
| 454 | |
|---|
| 455 | // apply rotation to matrix (this = J^T * this * J) |
|---|
| 456 | m_el[p][q] = m_el[q][p] = 0; |
|---|
| 457 | m_el[p][p] -= t * mpq; |
|---|
| 458 | m_el[q][q] += t * mpq; |
|---|
| 459 | btScalar mrp = m_el[r][p]; |
|---|
| 460 | btScalar mrq = m_el[r][q]; |
|---|
| 461 | m_el[r][p] = m_el[p][r] = cos * mrp - sin * mrq; |
|---|
| 462 | m_el[r][q] = m_el[q][r] = cos * mrq + sin * mrp; |
|---|
| 463 | |
|---|
| 464 | // apply rotation to rot (rot = rot * J) |
|---|
| 465 | for (int i = 0; i < 3; i++) |
|---|
| 466 | { |
|---|
| 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 | } |
|---|
| 474 | } |
|---|
| 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 | }; |
|---|
| 493 | |
|---|
| 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 |
|---|
