00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016 #ifndef TF_MATRIX3x3_H
00017 #define TF_MATRIX3x3_H
00018
00019 #include "Vector3.h"
00020 #include "Quaternion.h"
00021
00022 namespace tf
00023 {
00024
00025
00026 #define Matrix3x3Data Matrix3x3DoubleData
00027
00028
00031 class Matrix3x3 {
00032
00034 Vector3 m_el[3];
00035
00036 public:
00038 Matrix3x3 () {}
00039
00040
00041
00043 explicit Matrix3x3(const Quaternion& q) { setRotation(q); }
00044
00045
00046
00047
00048
00049
00050
00052 Matrix3x3(const tfScalar& xx, const tfScalar& xy, const tfScalar& xz,
00053 const tfScalar& yx, const tfScalar& yy, const tfScalar& yz,
00054 const tfScalar& zx, const tfScalar& zy, const tfScalar& zz)
00055 {
00056 setValue(xx, xy, xz,
00057 yx, yy, yz,
00058 zx, zy, zz);
00059 }
00061 TFSIMD_FORCE_INLINE Matrix3x3 (const Matrix3x3& other)
00062 {
00063 m_el[0] = other.m_el[0];
00064 m_el[1] = other.m_el[1];
00065 m_el[2] = other.m_el[2];
00066 }
00067
00068
00070 TFSIMD_FORCE_INLINE Matrix3x3& operator=(const Matrix3x3& other)
00071 {
00072 m_el[0] = other.m_el[0];
00073 m_el[1] = other.m_el[1];
00074 m_el[2] = other.m_el[2];
00075 return *this;
00076 }
00077
00078
00081 TFSIMD_FORCE_INLINE Vector3 getColumn(int i) const
00082 {
00083 return Vector3(m_el[0][i],m_el[1][i],m_el[2][i]);
00084 }
00085
00086
00089 TFSIMD_FORCE_INLINE const Vector3& getRow(int i) const
00090 {
00091 tfFullAssert(0 <= i && i < 3);
00092 return m_el[i];
00093 }
00094
00097 TFSIMD_FORCE_INLINE Vector3& operator[](int i)
00098 {
00099 tfFullAssert(0 <= i && i < 3);
00100 return m_el[i];
00101 }
00102
00105 TFSIMD_FORCE_INLINE const Vector3& operator[](int i) const
00106 {
00107 tfFullAssert(0 <= i && i < 3);
00108 return m_el[i];
00109 }
00110
00114 Matrix3x3& operator*=(const Matrix3x3& m);
00115
00118 void setFromOpenGLSubMatrix(const tfScalar *m)
00119 {
00120 m_el[0].setValue(m[0],m[4],m[8]);
00121 m_el[1].setValue(m[1],m[5],m[9]);
00122 m_el[2].setValue(m[2],m[6],m[10]);
00123
00124 }
00135 void setValue(const tfScalar& xx, const tfScalar& xy, const tfScalar& xz,
00136 const tfScalar& yx, const tfScalar& yy, const tfScalar& yz,
00137 const tfScalar& zx, const tfScalar& zy, const tfScalar& zz)
00138 {
00139 m_el[0].setValue(xx,xy,xz);
00140 m_el[1].setValue(yx,yy,yz);
00141 m_el[2].setValue(zx,zy,zz);
00142 }
00143
00146 void setRotation(const Quaternion& q)
00147 {
00148 tfScalar d = q.length2();
00149 tfFullAssert(d != tfScalar(0.0));
00150 tfScalar s = tfScalar(2.0) / d;
00151 tfScalar xs = q.x() * s, ys = q.y() * s, zs = q.z() * s;
00152 tfScalar wx = q.w() * xs, wy = q.w() * ys, wz = q.w() * zs;
00153 tfScalar xx = q.x() * xs, xy = q.x() * ys, xz = q.x() * zs;
00154 tfScalar yy = q.y() * ys, yz = q.y() * zs, zz = q.z() * zs;
00155 setValue(tfScalar(1.0) - (yy + zz), xy - wz, xz + wy,
00156 xy + wz, tfScalar(1.0) - (xx + zz), yz - wx,
00157 xz - wy, yz + wx, tfScalar(1.0) - (xx + yy));
00158 }
00159
00160
00166 void setEulerZYX(const tfScalar& yaw, const tfScalar& pitch, const tfScalar& roll) __attribute__((deprecated))
00167 {
00168 setEulerYPR(yaw, pitch, roll);
00169 }
00170
00180 void setEulerYPR(tfScalar eulerZ, tfScalar eulerY,tfScalar eulerX) {
00181 tfScalar ci ( tfCos(eulerX));
00182 tfScalar cj ( tfCos(eulerY));
00183 tfScalar ch ( tfCos(eulerZ));
00184 tfScalar si ( tfSin(eulerX));
00185 tfScalar sj ( tfSin(eulerY));
00186 tfScalar sh ( tfSin(eulerZ));
00187 tfScalar cc = ci * ch;
00188 tfScalar cs = ci * sh;
00189 tfScalar sc = si * ch;
00190 tfScalar ss = si * sh;
00191
00192 setValue(cj * ch, sj * sc - cs, sj * cc + ss,
00193 cj * sh, sj * ss + cc, sj * cs - sc,
00194 -sj, cj * si, cj * ci);
00195 }
00196
00203 void setRPY(tfScalar roll, tfScalar pitch,tfScalar yaw) {
00204 setEulerYPR(yaw, pitch, roll);
00205 }
00206
00208 void setIdentity()
00209 {
00210 setValue(tfScalar(1.0), tfScalar(0.0), tfScalar(0.0),
00211 tfScalar(0.0), tfScalar(1.0), tfScalar(0.0),
00212 tfScalar(0.0), tfScalar(0.0), tfScalar(1.0));
00213 }
00214
00215 static const Matrix3x3& getIdentity()
00216 {
00217 static const Matrix3x3 identityMatrix(tfScalar(1.0), tfScalar(0.0), tfScalar(0.0),
00218 tfScalar(0.0), tfScalar(1.0), tfScalar(0.0),
00219 tfScalar(0.0), tfScalar(0.0), tfScalar(1.0));
00220 return identityMatrix;
00221 }
00222
00225 void getOpenGLSubMatrix(tfScalar *m) const
00226 {
00227 m[0] = tfScalar(m_el[0].x());
00228 m[1] = tfScalar(m_el[1].x());
00229 m[2] = tfScalar(m_el[2].x());
00230 m[3] = tfScalar(0.0);
00231 m[4] = tfScalar(m_el[0].y());
00232 m[5] = tfScalar(m_el[1].y());
00233 m[6] = tfScalar(m_el[2].y());
00234 m[7] = tfScalar(0.0);
00235 m[8] = tfScalar(m_el[0].z());
00236 m[9] = tfScalar(m_el[1].z());
00237 m[10] = tfScalar(m_el[2].z());
00238 m[11] = tfScalar(0.0);
00239 }
00240
00243 void getRotation(Quaternion& q) const
00244 {
00245 tfScalar trace = m_el[0].x() + m_el[1].y() + m_el[2].z();
00246 tfScalar temp[4];
00247
00248 if (trace > tfScalar(0.0))
00249 {
00250 tfScalar s = tfSqrt(trace + tfScalar(1.0));
00251 temp[3]=(s * tfScalar(0.5));
00252 s = tfScalar(0.5) / s;
00253
00254 temp[0]=((m_el[2].y() - m_el[1].z()) * s);
00255 temp[1]=((m_el[0].z() - m_el[2].x()) * s);
00256 temp[2]=((m_el[1].x() - m_el[0].y()) * s);
00257 }
00258 else
00259 {
00260 int i = m_el[0].x() < m_el[1].y() ?
00261 (m_el[1].y() < m_el[2].z() ? 2 : 1) :
00262 (m_el[0].x() < m_el[2].z() ? 2 : 0);
00263 int j = (i + 1) % 3;
00264 int k = (i + 2) % 3;
00265
00266 tfScalar s = tfSqrt(m_el[i][i] - m_el[j][j] - m_el[k][k] + tfScalar(1.0));
00267 temp[i] = s * tfScalar(0.5);
00268 s = tfScalar(0.5) / s;
00269
00270 temp[3] = (m_el[k][j] - m_el[j][k]) * s;
00271 temp[j] = (m_el[j][i] + m_el[i][j]) * s;
00272 temp[k] = (m_el[k][i] + m_el[i][k]) * s;
00273 }
00274 q.setValue(temp[0],temp[1],temp[2],temp[3]);
00275 }
00276
00282 __attribute__((deprecated)) void getEulerZYX(tfScalar& yaw, tfScalar& pitch, tfScalar& roll, unsigned int solution_number = 1) const
00283 {
00284 getEulerYPR(yaw, pitch, roll, solution_number);
00285 };
00286
00287
00292 void getEulerYPR(tfScalar& yaw, tfScalar& pitch, tfScalar& roll, unsigned int solution_number = 1) const
00293 {
00294 struct Euler
00295 {
00296 tfScalar yaw;
00297 tfScalar pitch;
00298 tfScalar roll;
00299 };
00300
00301 Euler euler_out;
00302 Euler euler_out2;
00303
00304
00305
00306
00307 if (tfFabs(m_el[2].x()) >= 1)
00308 {
00309 euler_out.yaw = 0;
00310 euler_out2.yaw = 0;
00311
00312
00313 tfScalar delta = tfAtan2(m_el[2].y(),m_el[2].z());
00314 if (m_el[2].x() < 0)
00315 {
00316 euler_out.pitch = TFSIMD_PI / tfScalar(2.0);
00317 euler_out2.pitch = TFSIMD_PI / tfScalar(2.0);
00318 euler_out.roll = delta;
00319 euler_out2.roll = delta;
00320 }
00321 else
00322 {
00323 euler_out.pitch = -TFSIMD_PI / tfScalar(2.0);
00324 euler_out2.pitch = -TFSIMD_PI / tfScalar(2.0);
00325 euler_out.roll = delta;
00326 euler_out2.roll = delta;
00327 }
00328 }
00329 else
00330 {
00331 euler_out.pitch = - tfAsin(m_el[2].x());
00332 euler_out2.pitch = TFSIMD_PI - euler_out.pitch;
00333
00334 euler_out.roll = tfAtan2(m_el[2].y()/tfCos(euler_out.pitch),
00335 m_el[2].z()/tfCos(euler_out.pitch));
00336 euler_out2.roll = tfAtan2(m_el[2].y()/tfCos(euler_out2.pitch),
00337 m_el[2].z()/tfCos(euler_out2.pitch));
00338
00339 euler_out.yaw = tfAtan2(m_el[1].x()/tfCos(euler_out.pitch),
00340 m_el[0].x()/tfCos(euler_out.pitch));
00341 euler_out2.yaw = tfAtan2(m_el[1].x()/tfCos(euler_out2.pitch),
00342 m_el[0].x()/tfCos(euler_out2.pitch));
00343 }
00344
00345 if (solution_number == 1)
00346 {
00347 yaw = euler_out.yaw;
00348 pitch = euler_out.pitch;
00349 roll = euler_out.roll;
00350 }
00351 else
00352 {
00353 yaw = euler_out2.yaw;
00354 pitch = euler_out2.pitch;
00355 roll = euler_out2.roll;
00356 }
00357 }
00358
00364 void getRPY(tfScalar& roll, tfScalar& pitch, tfScalar& yaw, unsigned int solution_number = 1) const
00365 {
00366 getEulerYPR(yaw, pitch, roll, solution_number);
00367 }
00368
00372 Matrix3x3 scaled(const Vector3& s) const
00373 {
00374 return Matrix3x3(m_el[0].x() * s.x(), m_el[0].y() * s.y(), m_el[0].z() * s.z(),
00375 m_el[1].x() * s.x(), m_el[1].y() * s.y(), m_el[1].z() * s.z(),
00376 m_el[2].x() * s.x(), m_el[2].y() * s.y(), m_el[2].z() * s.z());
00377 }
00378
00380 tfScalar determinant() const;
00382 Matrix3x3 adjoint() const;
00384 Matrix3x3 absolute() const;
00386 Matrix3x3 transpose() const;
00388 Matrix3x3 inverse() const;
00389
00390 Matrix3x3 transposeTimes(const Matrix3x3& m) const;
00391 Matrix3x3 timesTranspose(const Matrix3x3& m) const;
00392
00393 TFSIMD_FORCE_INLINE tfScalar tdotx(const Vector3& v) const
00394 {
00395 return m_el[0].x() * v.x() + m_el[1].x() * v.y() + m_el[2].x() * v.z();
00396 }
00397 TFSIMD_FORCE_INLINE tfScalar tdoty(const Vector3& v) const
00398 {
00399 return m_el[0].y() * v.x() + m_el[1].y() * v.y() + m_el[2].y() * v.z();
00400 }
00401 TFSIMD_FORCE_INLINE tfScalar tdotz(const Vector3& v) const
00402 {
00403 return m_el[0].z() * v.x() + m_el[1].z() * v.y() + m_el[2].z() * v.z();
00404 }
00405
00406
00416 void diagonalize(Matrix3x3& rot, tfScalar threshold, int maxSteps)
00417 {
00418 rot.setIdentity();
00419 for (int step = maxSteps; step > 0; step--)
00420 {
00421
00422 int p = 0;
00423 int q = 1;
00424 int r = 2;
00425 tfScalar max = tfFabs(m_el[0][1]);
00426 tfScalar v = tfFabs(m_el[0][2]);
00427 if (v > max)
00428 {
00429 q = 2;
00430 r = 1;
00431 max = v;
00432 }
00433 v = tfFabs(m_el[1][2]);
00434 if (v > max)
00435 {
00436 p = 1;
00437 q = 2;
00438 r = 0;
00439 max = v;
00440 }
00441
00442 tfScalar t = threshold * (tfFabs(m_el[0][0]) + tfFabs(m_el[1][1]) + tfFabs(m_el[2][2]));
00443 if (max <= t)
00444 {
00445 if (max <= TFSIMD_EPSILON * t)
00446 {
00447 return;
00448 }
00449 step = 1;
00450 }
00451
00452
00453 tfScalar mpq = m_el[p][q];
00454 tfScalar theta = (m_el[q][q] - m_el[p][p]) / (2 * mpq);
00455 tfScalar theta2 = theta * theta;
00456 tfScalar cos;
00457 tfScalar sin;
00458 if (theta2 * theta2 < tfScalar(10 / TFSIMD_EPSILON))
00459 {
00460 t = (theta >= 0) ? 1 / (theta + tfSqrt(1 + theta2))
00461 : 1 / (theta - tfSqrt(1 + theta2));
00462 cos = 1 / tfSqrt(1 + t * t);
00463 sin = cos * t;
00464 }
00465 else
00466 {
00467
00468 t = 1 / (theta * (2 + tfScalar(0.5) / theta2));
00469 cos = 1 - tfScalar(0.5) * t * t;
00470 sin = cos * t;
00471 }
00472
00473
00474 m_el[p][q] = m_el[q][p] = 0;
00475 m_el[p][p] -= t * mpq;
00476 m_el[q][q] += t * mpq;
00477 tfScalar mrp = m_el[r][p];
00478 tfScalar mrq = m_el[r][q];
00479 m_el[r][p] = m_el[p][r] = cos * mrp - sin * mrq;
00480 m_el[r][q] = m_el[q][r] = cos * mrq + sin * mrp;
00481
00482
00483 for (int i = 0; i < 3; i++)
00484 {
00485 Vector3& row = rot[i];
00486 mrp = row[p];
00487 mrq = row[q];
00488 row[p] = cos * mrp - sin * mrq;
00489 row[q] = cos * mrq + sin * mrp;
00490 }
00491 }
00492 }
00493
00494
00495
00496
00504 tfScalar cofac(int r1, int c1, int r2, int c2) const
00505 {
00506 return m_el[r1][c1] * m_el[r2][c2] - m_el[r1][c2] * m_el[r2][c1];
00507 }
00508
00509 void serialize(struct Matrix3x3Data& dataOut) const;
00510
00511 void serializeFloat(struct Matrix3x3FloatData& dataOut) const;
00512
00513 void deSerialize(const struct Matrix3x3Data& dataIn);
00514
00515 void deSerializeFloat(const struct Matrix3x3FloatData& dataIn);
00516
00517 void deSerializeDouble(const struct Matrix3x3DoubleData& dataIn);
00518
00519 };
00520
00521
00522 TFSIMD_FORCE_INLINE Matrix3x3&
00523 Matrix3x3::operator*=(const Matrix3x3& m)
00524 {
00525 setValue(m.tdotx(m_el[0]), m.tdoty(m_el[0]), m.tdotz(m_el[0]),
00526 m.tdotx(m_el[1]), m.tdoty(m_el[1]), m.tdotz(m_el[1]),
00527 m.tdotx(m_el[2]), m.tdoty(m_el[2]), m.tdotz(m_el[2]));
00528 return *this;
00529 }
00530
00531 TFSIMD_FORCE_INLINE tfScalar
00532 Matrix3x3::determinant() const
00533 {
00534 return tfTriple((*this)[0], (*this)[1], (*this)[2]);
00535 }
00536
00537
00538 TFSIMD_FORCE_INLINE Matrix3x3
00539 Matrix3x3::absolute() const
00540 {
00541 return Matrix3x3(
00542 tfFabs(m_el[0].x()), tfFabs(m_el[0].y()), tfFabs(m_el[0].z()),
00543 tfFabs(m_el[1].x()), tfFabs(m_el[1].y()), tfFabs(m_el[1].z()),
00544 tfFabs(m_el[2].x()), tfFabs(m_el[2].y()), tfFabs(m_el[2].z()));
00545 }
00546
00547 TFSIMD_FORCE_INLINE Matrix3x3
00548 Matrix3x3::transpose() const
00549 {
00550 return Matrix3x3(m_el[0].x(), m_el[1].x(), m_el[2].x(),
00551 m_el[0].y(), m_el[1].y(), m_el[2].y(),
00552 m_el[0].z(), m_el[1].z(), m_el[2].z());
00553 }
00554
00555 TFSIMD_FORCE_INLINE Matrix3x3
00556 Matrix3x3::adjoint() const
00557 {
00558 return Matrix3x3(cofac(1, 1, 2, 2), cofac(0, 2, 2, 1), cofac(0, 1, 1, 2),
00559 cofac(1, 2, 2, 0), cofac(0, 0, 2, 2), cofac(0, 2, 1, 0),
00560 cofac(1, 0, 2, 1), cofac(0, 1, 2, 0), cofac(0, 0, 1, 1));
00561 }
00562
00563 TFSIMD_FORCE_INLINE Matrix3x3
00564 Matrix3x3::inverse() const
00565 {
00566 Vector3 co(cofac(1, 1, 2, 2), cofac(1, 2, 2, 0), cofac(1, 0, 2, 1));
00567 tfScalar det = (*this)[0].dot(co);
00568 tfFullAssert(det != tfScalar(0.0));
00569 tfScalar s = tfScalar(1.0) / det;
00570 return Matrix3x3(co.x() * s, cofac(0, 2, 2, 1) * s, cofac(0, 1, 1, 2) * s,
00571 co.y() * s, cofac(0, 0, 2, 2) * s, cofac(0, 2, 1, 0) * s,
00572 co.z() * s, cofac(0, 1, 2, 0) * s, cofac(0, 0, 1, 1) * s);
00573 }
00574
00575 TFSIMD_FORCE_INLINE Matrix3x3
00576 Matrix3x3::transposeTimes(const Matrix3x3& m) const
00577 {
00578 return Matrix3x3(
00579 m_el[0].x() * m[0].x() + m_el[1].x() * m[1].x() + m_el[2].x() * m[2].x(),
00580 m_el[0].x() * m[0].y() + m_el[1].x() * m[1].y() + m_el[2].x() * m[2].y(),
00581 m_el[0].x() * m[0].z() + m_el[1].x() * m[1].z() + m_el[2].x() * m[2].z(),
00582 m_el[0].y() * m[0].x() + m_el[1].y() * m[1].x() + m_el[2].y() * m[2].x(),
00583 m_el[0].y() * m[0].y() + m_el[1].y() * m[1].y() + m_el[2].y() * m[2].y(),
00584 m_el[0].y() * m[0].z() + m_el[1].y() * m[1].z() + m_el[2].y() * m[2].z(),
00585 m_el[0].z() * m[0].x() + m_el[1].z() * m[1].x() + m_el[2].z() * m[2].x(),
00586 m_el[0].z() * m[0].y() + m_el[1].z() * m[1].y() + m_el[2].z() * m[2].y(),
00587 m_el[0].z() * m[0].z() + m_el[1].z() * m[1].z() + m_el[2].z() * m[2].z());
00588 }
00589
00590 TFSIMD_FORCE_INLINE Matrix3x3
00591 Matrix3x3::timesTranspose(const Matrix3x3& m) const
00592 {
00593 return Matrix3x3(
00594 m_el[0].dot(m[0]), m_el[0].dot(m[1]), m_el[0].dot(m[2]),
00595 m_el[1].dot(m[0]), m_el[1].dot(m[1]), m_el[1].dot(m[2]),
00596 m_el[2].dot(m[0]), m_el[2].dot(m[1]), m_el[2].dot(m[2]));
00597
00598 }
00599
00600 TFSIMD_FORCE_INLINE Vector3
00601 operator*(const Matrix3x3& m, const Vector3& v)
00602 {
00603 return Vector3(m[0].dot(v), m[1].dot(v), m[2].dot(v));
00604 }
00605
00606
00607 TFSIMD_FORCE_INLINE Vector3
00608 operator*(const Vector3& v, const Matrix3x3& m)
00609 {
00610 return Vector3(m.tdotx(v), m.tdoty(v), m.tdotz(v));
00611 }
00612
00613 TFSIMD_FORCE_INLINE Matrix3x3
00614 operator*(const Matrix3x3& m1, const Matrix3x3& m2)
00615 {
00616 return Matrix3x3(
00617 m2.tdotx( m1[0]), m2.tdoty( m1[0]), m2.tdotz( m1[0]),
00618 m2.tdotx( m1[1]), m2.tdoty( m1[1]), m2.tdotz( m1[1]),
00619 m2.tdotx( m1[2]), m2.tdoty( m1[2]), m2.tdotz( m1[2]));
00620 }
00621
00622
00623
00624
00625
00626
00627
00628
00629
00630
00631
00632
00633
00634
00635
00636
00639 TFSIMD_FORCE_INLINE bool operator==(const Matrix3x3& m1, const Matrix3x3& m2)
00640 {
00641 return ( m1[0][0] == m2[0][0] && m1[1][0] == m2[1][0] && m1[2][0] == m2[2][0] &&
00642 m1[0][1] == m2[0][1] && m1[1][1] == m2[1][1] && m1[2][1] == m2[2][1] &&
00643 m1[0][2] == m2[0][2] && m1[1][2] == m2[1][2] && m1[2][2] == m2[2][2] );
00644 }
00645
00647 struct Matrix3x3FloatData
00648 {
00649 Vector3FloatData m_el[3];
00650 };
00651
00653 struct Matrix3x3DoubleData
00654 {
00655 Vector3DoubleData m_el[3];
00656 };
00657
00658
00659
00660
00661 TFSIMD_FORCE_INLINE void Matrix3x3::serialize(struct Matrix3x3Data& dataOut) const
00662 {
00663 for (int i=0;i<3;i++)
00664 m_el[i].serialize(dataOut.m_el[i]);
00665 }
00666
00667 TFSIMD_FORCE_INLINE void Matrix3x3::serializeFloat(struct Matrix3x3FloatData& dataOut) const
00668 {
00669 for (int i=0;i<3;i++)
00670 m_el[i].serializeFloat(dataOut.m_el[i]);
00671 }
00672
00673
00674 TFSIMD_FORCE_INLINE void Matrix3x3::deSerialize(const struct Matrix3x3Data& dataIn)
00675 {
00676 for (int i=0;i<3;i++)
00677 m_el[i].deSerialize(dataIn.m_el[i]);
00678 }
00679
00680 TFSIMD_FORCE_INLINE void Matrix3x3::deSerializeFloat(const struct Matrix3x3FloatData& dataIn)
00681 {
00682 for (int i=0;i<3;i++)
00683 m_el[i].deSerializeFloat(dataIn.m_el[i]);
00684 }
00685
00686 TFSIMD_FORCE_INLINE void Matrix3x3::deSerializeDouble(const struct Matrix3x3DoubleData& dataIn)
00687 {
00688 for (int i=0;i<3;i++)
00689 m_el[i].deSerializeDouble(dataIn.m_el[i]);
00690 }
00691
00692 }
00693
00694 #endif //TF_MATRIX3x3_H
00695