16 #ifndef TF2_MATRIX3x3_H 17 #define TF2_MATRIX3x3_H 26 #define Matrix3x3Data Matrix3x3DoubleData 63 m_el[0] = other.
m_el[0];
64 m_el[1] = other.
m_el[1];
65 m_el[2] = other.
m_el[2];
72 m_el[0] = other.
m_el[0];
73 m_el[1] = other.
m_el[1];
74 m_el[2] = other.
m_el[2];
83 return Vector3(m_el[0][i],m_el[1][i],m_el[2][i]);
120 m_el[0].setValue(m[0],m[4],m[8]);
121 m_el[1].setValue(m[1],m[5],m[9]);
122 m_el[2].setValue(m[2],m[6],m[10]);
139 m_el[0].setValue(xx,xy,xz);
140 m_el[1].setValue(yx,yy,yz);
141 m_el[2].setValue(zx,zy,zz);
151 tf2Scalar xs = q.x() * s, ys = q.y() * s, zs = q.z() * s;
152 tf2Scalar wx = q.w() * xs, wy = q.w() * ys, wz = q.w() * zs;
153 tf2Scalar xx = q.x() * xs, xy = q.x() * ys, xz = q.x() * zs;
154 tf2Scalar yy = q.y() * ys, yz = q.y() * zs, zz = q.z() * zs;
156 xy + wz,
tf2Scalar(1.0) - (xx + zz), yz - wx,
157 xz - wy, yz + wx,
tf2Scalar(1.0) - (xx + yy));
192 setValue(cj * ch, sj * sc - cs, sj * cc + ss,
193 cj * sh, sj * ss + cc, sj * cs - sc,
194 -sj, cj * si, cj * ci);
220 return identityMatrix;
245 tf2Scalar trace = m_el[0].x() + m_el[1].y() + m_el[2].z();
254 temp[0]=((m_el[2].y() - m_el[1].z()) * s);
255 temp[1]=((m_el[0].z() - m_el[2].x()) * s);
256 temp[2]=((m_el[1].x() - m_el[0].y()) * s);
260 int i = m_el[0].x() < m_el[1].y() ?
261 (m_el[1].y() < m_el[2].z() ? 2 : 1) :
262 (m_el[0].x() < m_el[2].z() ? 2 : 0);
270 temp[3] = (m_el[k][j] - m_el[j][k]) * s;
271 temp[j] = (m_el[j][i] + m_el[i][j]) * s;
272 temp[k] = (m_el[k][i] + m_el[i][k]) * s;
274 q.setValue(temp[0],temp[1],temp[2],temp[3]);
318 euler_out.roll = delta;
319 euler_out2.roll = delta;
325 euler_out.roll = delta;
326 euler_out2.roll = delta;
331 euler_out.pitch = -
tf2Asin(m_el[2].x());
332 euler_out2.pitch =
TF2SIMD_PI - euler_out.pitch;
335 m_el[2].z()/
tf2Cos(euler_out.pitch));
337 m_el[2].z()/
tf2Cos(euler_out2.pitch));
340 m_el[0].x()/
tf2Cos(euler_out.pitch));
342 m_el[0].x()/
tf2Cos(euler_out2.pitch));
348 pitch = euler_out.pitch;
349 roll = euler_out.roll;
353 yaw = euler_out2.yaw;
354 pitch = euler_out2.pitch;
355 roll = euler_out2.roll;
374 return Matrix3x3(m_el[0].x() * s.x(), m_el[0].y() * s.y(), m_el[0].z() * s.z(),
375 m_el[1].x() * s.x(), m_el[1].y() * s.y(), m_el[1].z() * s.z(),
376 m_el[2].x() * s.x(), m_el[2].y() * s.y(), m_el[2].z() * s.z());
395 return m_el[0].x() * v.x() + m_el[1].x() * v.y() + m_el[2].x() * v.z();
399 return m_el[0].y() * v.x() + m_el[1].y() * v.y() + m_el[2].y() * v.z();
403 return m_el[0].z() * v.x() + m_el[1].z() * v.y() + m_el[2].z() * v.z();
454 tf2Scalar theta = (m_el[q][q] - m_el[p][p]) / (2 * mpq);
460 t = (theta >= 0) ? 1 / (theta +
tf2Sqrt(1 + theta2))
461 : 1 / (theta -
tf2Sqrt(1 + theta2));
468 t = 1 / (theta * (2 +
tf2Scalar(0.5) / theta2));
474 m_el[p][q] = m_el[q][p] = 0;
475 m_el[p][p] -= t * mpq;
476 m_el[q][q] += t * mpq;
479 m_el[r][p] = m_el[p][r] = cos * mrp - sin * mrq;
480 m_el[r][q] = m_el[q][r] = cos * mrq + sin * mrp;
483 for (
int i = 0; i < 3; i++)
485 Vector3& row = rot[i];
488 row[p] = cos * mrp - sin * mrq;
489 row[q] = cos * mrq + sin * mrp;
506 return m_el[r1][c1] * m_el[r2][c2] - m_el[r1][c2] * m_el[r2][c1];
534 return tf2Triple((*
this)[0], (*
this)[1], (*
this)[2]);
550 return Matrix3x3(m_el[0].x(), m_el[1].x(), m_el[2].x(),
551 m_el[0].y(), m_el[1].y(), m_el[2].y(),
552 m_el[0].z(), m_el[1].z(), m_el[2].z());
558 return Matrix3x3(
cofac(1, 1, 2, 2),
cofac(0, 2, 2, 1),
cofac(0, 1, 1, 2),
559 cofac(1, 2, 2, 0),
cofac(0, 0, 2, 2),
cofac(0, 2, 1, 0),
560 cofac(1, 0, 2, 1),
cofac(0, 1, 2, 0),
cofac(0, 0, 1, 1));
566 Vector3 co(
cofac(1, 1, 2, 2),
cofac(1, 2, 2, 0),
cofac(1, 0, 2, 1));
571 co.y() * s,
cofac(0, 0, 2, 2) * s,
cofac(0, 2, 1, 0) * s,
572 co.z() * s,
cofac(0, 1, 2, 0) * s,
cofac(0, 0, 1, 1) * s);
579 m_el[0].x() * m[0].x() + m_el[1].x() * m[1].x() + m_el[2].x() * m[2].x(),
580 m_el[0].x() * m[0].y() + m_el[1].x() * m[1].y() + m_el[2].x() * m[2].y(),
581 m_el[0].x() * m[0].z() + m_el[1].x() * m[1].z() + m_el[2].x() * m[2].z(),
582 m_el[0].y() * m[0].x() + m_el[1].y() * m[1].x() + m_el[2].y() * m[2].x(),
583 m_el[0].y() * m[0].y() + m_el[1].y() * m[1].y() + m_el[2].y() * m[2].y(),
584 m_el[0].y() * m[0].z() + m_el[1].y() * m[1].z() + m_el[2].y() * m[2].z(),
585 m_el[0].z() * m[0].x() + m_el[1].z() * m[1].x() + m_el[2].z() * m[2].x(),
586 m_el[0].z() * m[0].y() + m_el[1].z() * m[1].y() + m_el[2].z() * m[2].y(),
587 m_el[0].z() * m[0].z() + m_el[1].z() * m[1].z() + m_el[2].z() * m[2].z());
594 m_el[0].
dot(m[0]), m_el[0].
dot(m[1]), m_el[0].
dot(m[2]),
595 m_el[1].
dot(m[0]), m_el[1].
dot(m[1]), m_el[1].
dot(m[2]),
596 m_el[2].
dot(m[0]), m_el[2].
dot(m[1]), m_el[2].
dot(m[2]));
603 return Vector3(m[0].
dot(v), m[1].
dot(v), m[2].
dot(v));
641 return ( m1[0][0] == m2[0][0] && m1[1][0] == m2[1][0] && m1[2][0] == m2[2][0] &&
642 m1[0][1] == m2[0][1] && m1[1][1] == m2[1][1] && m1[2][1] == m2[2][1] &&
643 m1[0][2] == m2[0][2] && m1[1][2] == m2[1][2] && m1[2][2] == m2[2][2] );
663 for (
int i=0;i<3;i++)
669 for (
int i=0;i<3;i++)
676 for (
int i=0;i<3;i++)
682 for (
int i=0;i<3;i++)
688 for (
int i=0;i<3;i++)
693 #endif //TF2_MATRIX3x3_H Matrix3x3 timesTranspose(const Matrix3x3 &m) const
#define TF2SIMD_FORCE_INLINE
tf2Scalar tf2Scalar unsigned int solution_number
void setEulerZYX(const tf2Scalar &yaw, const tf2Scalar &pitch, const tf2Scalar &roll) __attribute__((deprecated))
Set the matrix from euler angles using YPR around ZYX respectively.
static const Matrix3x3 & getIdentity()
void getRPY(tf2Scalar &roll, tf2Scalar &pitch, tf2Scalar &yaw, unsigned int solution_number=1) const
Get the matrix represented as roll pitch and yaw about fixed axes XYZ.
TF2SIMD_FORCE_INLINE bool operator==(const Matrix3x3 &m1, const Matrix3x3 &m2)
Equality operator between two matrices It will test all elements are equal.
TF2SIMD_FORCE_INLINE tf2Scalar tf2Cos(tf2Scalar x)
void getOpenGLSubMatrix(tf2Scalar *m) const
Fill the values of the matrix into a 9 element array.
tf2Scalar determinant() const
Return the determinant of the matrix.
Vector3DoubleData m_el[3]
void setFromOpenGLSubMatrix(const tf2Scalar *m)
Set from a carray of tf2Scalars.
Matrix3x3(const tf2Scalar &xx, const tf2Scalar &xy, const tf2Scalar &xz, const tf2Scalar &yx, const tf2Scalar &yy, const tf2Scalar &yz, const tf2Scalar &zx, const tf2Scalar &zy, const tf2Scalar &zz)
Constructor with row major formatting.
Matrix3x3 adjoint() const
Return the adjoint of the matrix.
Matrix3x3 transposeTimes(const Matrix3x3 &m) const
void deSerialize(const struct Matrix3x3Data &dataIn)
void deSerializeDouble(const struct Matrix3x3DoubleData &dataIn)
Matrix3x3(const Quaternion &q)
Constructor from Quaternion.
TF2SIMD_FORCE_INLINE const Vector3 & getRow(int i) const
Get a row of the matrix as a vector.
double tf2Scalar
The tf2Scalar type abstracts floating point numbers, to easily switch between double and single float...
TF2SIMD_FORCE_INLINE tf2Scalar tf2Fabs(tf2Scalar x)
TF2SIMD_FORCE_INLINE tf2Scalar dot(const Quaternion &q1, const Quaternion &q2)
Calculate the dot product between two quaternions.
Matrix3x3 absolute() const
Return the matrix with all values non negative.
TF2SIMD_FORCE_INLINE tf2Scalar tdotz(const Vector3 &v) const
TF2SIMD_FORCE_INLINE tf2Scalar tdotx(const Vector3 &v) const
Matrix3x3 transpose() const
Return the transpose of the matrix.
Matrix3x3()
No initializaion constructor.
TF2SIMD_FORCE_INLINE tf2Scalar tdoty(const Vector3 &v) const
void setRPY(tf2Scalar roll, tf2Scalar pitch, tf2Scalar yaw)
Set the matrix using RPY about XYZ fixed axes.
tf2Scalar length2() const
Return the length squared of the quaternion.
The Matrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with Q...
void diagonalize(Matrix3x3 &rot, tf2Scalar threshold, int maxSteps)
diagonalizes this matrix by the Jacobi method.
void deSerializeFloat(const struct Matrix3x3FloatData &dataIn)
TF2SIMD_FORCE_INLINE const Vector3 & operator[](int i) const
Get a const reference to a row of the matrix as a vector.
TF2SIMD_FORCE_INLINE tf2Scalar tf2Asin(tf2Scalar x)
void serialize(struct Matrix3x3Data &dataOut) const
TF2SIMD_FORCE_INLINE Vector3 & operator[](int i)
Get a mutable reference to a row of the matrix as a vector.
TF2SIMD_FORCE_INLINE tf2Scalar tf2Atan2(tf2Scalar x, tf2Scalar y)
Vector3 m_el[3]
Data storage for the matrix, each vector is a row of the matrix.
tf2Scalar cofac(int r1, int c1, int r2, int c2) const
Calculate the matrix cofactor.
Matrix3x3 scaled(const Vector3 &s) const
Create a scaled copy of the matrix.
TF2SIMD_FORCE_INLINE tf2Scalar tf2Sin(tf2Scalar x)
TF2SIMD_FORCE_INLINE tf2Scalar tf2Sqrt(tf2Scalar x)
void setRotation(const Quaternion &q)
Set the matrix from a quaternion.
TF2SIMD_FORCE_INLINE Vector3 operator*(const Matrix3x3 &m, const Vector3 &v)
void setValue(const tf2Scalar &xx, const tf2Scalar &xy, const tf2Scalar &xz, const tf2Scalar &yx, const tf2Scalar &yy, const tf2Scalar &yz, const tf2Scalar &zx, const tf2Scalar &zy, const tf2Scalar &zz)
Set the values of the matrix explicitly (row major)
TF2SIMD_FORCE_INLINE Matrix3x3(const Matrix3x3 &other)
Copy constructor.
void getRotation(Quaternion &q) const
Get the matrix represented as a quaternion.
void setEulerYPR(tf2Scalar eulerZ, tf2Scalar eulerY, tf2Scalar eulerX)
Set the matrix from euler angles YPR around ZYX axes.
Matrix3x3 inverse() const
Return the inverse of the matrix.
TF2SIMD_FORCE_INLINE tf2Scalar tf2Triple(const Vector3 &v1, const Vector3 &v2, const Vector3 &v3)
Matrix3x3 & operator*=(const Matrix3x3 &m)
Multiply by the target matrix on the right.
tf2Scalar tf2Scalar & roll
__attribute__((deprecated)) void getEulerZYX(tf2Scalar &yaw
Get the matrix represented as euler angles around ZYX.
TF2SIMD_FORCE_INLINE Vector3 getColumn(int i) const
Get a column of the matrix as a vector.
void serializeFloat(struct Matrix3x3FloatData &dataOut) const
TF2SIMD_FORCE_INLINE Matrix3x3 & operator=(const Matrix3x3 &other)
Assignment Operator.
The Quaternion implements quaternion to perform linear algebra rotations in combination with Matrix3x...
void getEulerYPR(tf2Scalar &yaw, tf2Scalar &pitch, tf2Scalar &roll, unsigned int solution_number=1) const
Get the matrix represented as euler angles around YXZ, roundtrip with setEulerYPR.
void setIdentity()
Set the matrix to the identity.