16 #ifndef TF2_MATRIX3x3_H 17 #define TF2_MATRIX3x3_H 28 #define Matrix3x3Data Matrix3x3DoubleData 65 m_el[0] = other.
m_el[0];
66 m_el[1] = other.
m_el[1];
67 m_el[2] = other.
m_el[2];
74 m_el[0] = other.
m_el[0];
75 m_el[1] = other.
m_el[1];
76 m_el[2] = other.
m_el[2];
85 return Vector3(m_el[0][i],m_el[1][i],m_el[2][i]);
122 m_el[0].setValue(m[0],m[4],m[8]);
123 m_el[1].setValue(m[1],m[5],m[9]);
124 m_el[2].setValue(m[2],m[6],m[10]);
141 m_el[0].setValue(xx,xy,xz);
142 m_el[1].setValue(yx,yy,yz);
143 m_el[2].setValue(zx,zy,zz);
153 tf2Scalar xs = q.x() * s, ys = q.y() * s, zs = q.z() * s;
154 tf2Scalar wx = q.w() * xs, wy = q.w() * ys, wz = q.w() * zs;
155 tf2Scalar xx = q.x() * xs, xy = q.x() * ys, xz = q.x() * zs;
156 tf2Scalar yy = q.y() * ys, yz = q.y() * zs, zz = q.z() * zs;
158 xy + wz,
tf2Scalar(1.0) - (xx + zz), yz - wx,
159 xz - wy, yz + wx,
tf2Scalar(1.0) - (xx + yy));
194 setValue(cj * ch, sj * sc - cs, sj * cc + ss,
195 cj * sh, sj * ss + cc, sj * cs - sc,
196 -sj, cj * si, cj * ci);
222 return identityMatrix;
247 tf2Scalar trace = m_el[0].x() + m_el[1].y() + m_el[2].z();
256 temp[0]=((m_el[2].y() - m_el[1].z()) * s);
257 temp[1]=((m_el[0].z() - m_el[2].x()) * s);
258 temp[2]=((m_el[1].x() - m_el[0].y()) * s);
262 int i = m_el[0].x() < m_el[1].y() ?
263 (m_el[1].y() < m_el[2].z() ? 2 : 1) :
264 (m_el[0].x() < m_el[2].z() ? 2 : 0);
272 temp[3] = (m_el[k][j] - m_el[j][k]) * s;
273 temp[j] = (m_el[j][i] + m_el[i][j]) * s;
274 temp[k] = (m_el[k][i] + m_el[i][k]) * s;
276 q.setValue(temp[0],temp[1],temp[2],temp[3]);
320 euler_out.roll = delta;
321 euler_out2.roll = delta;
327 euler_out.roll = delta;
328 euler_out2.roll = delta;
333 euler_out.pitch = -
tf2Asin(m_el[2].x());
334 euler_out2.pitch =
TF2SIMD_PI - euler_out.pitch;
337 m_el[2].z()/
tf2Cos(euler_out.pitch));
339 m_el[2].z()/
tf2Cos(euler_out2.pitch));
342 m_el[0].x()/
tf2Cos(euler_out.pitch));
344 m_el[0].x()/
tf2Cos(euler_out2.pitch));
347 if (solution_number == 1)
350 pitch = euler_out.pitch;
351 roll = euler_out.roll;
355 yaw = euler_out2.yaw;
356 pitch = euler_out2.pitch;
357 roll = euler_out2.roll;
376 return Matrix3x3(m_el[0].x() * s.x(), m_el[0].y() * s.y(), m_el[0].z() * s.z(),
377 m_el[1].x() * s.x(), m_el[1].y() * s.y(), m_el[1].z() * s.z(),
378 m_el[2].x() * s.x(), m_el[2].y() * s.y(), m_el[2].z() * s.z());
397 return m_el[0].x() * v.x() + m_el[1].x() * v.y() + m_el[2].x() * v.z();
401 return m_el[0].y() * v.x() + m_el[1].y() * v.y() + m_el[2].y() * v.z();
405 return m_el[0].z() * v.x() + m_el[1].z() * v.y() + m_el[2].z() * v.z();
456 tf2Scalar theta = (m_el[q][q] - m_el[p][p]) / (2 * mpq);
462 t = (theta >= 0) ? 1 / (theta +
tf2Sqrt(1 + theta2))
463 : 1 / (theta -
tf2Sqrt(1 + theta2));
470 t = 1 / (theta * (2 +
tf2Scalar(0.5) / theta2));
476 m_el[p][q] = m_el[q][p] = 0;
477 m_el[p][p] -= t * mpq;
478 m_el[q][q] += t * mpq;
481 m_el[r][p] = m_el[p][r] = cos * mrp - sin * mrq;
482 m_el[r][q] = m_el[q][r] = cos * mrq + sin * mrp;
485 for (
int i = 0; i < 3; i++)
487 Vector3& row = rot[i];
490 row[p] = cos * mrp - sin * mrq;
491 row[q] = cos * mrq + sin * mrp;
508 return m_el[r1][c1] * m_el[r2][c2] - m_el[r1][c2] * m_el[r2][c1];
536 return tf2Triple((*
this)[0], (*
this)[1], (*
this)[2]);
560 return Matrix3x3(
cofac(1, 1, 2, 2),
cofac(0, 2, 2, 1),
cofac(0, 1, 1, 2),
561 cofac(1, 2, 2, 0),
cofac(0, 0, 2, 2),
cofac(0, 2, 1, 0),
562 cofac(1, 0, 2, 1),
cofac(0, 1, 2, 0),
cofac(0, 0, 1, 1));
568 Vector3 co(
cofac(1, 1, 2, 2),
cofac(1, 2, 2, 0),
cofac(1, 0, 2, 1));
573 co.y() * s,
cofac(0, 0, 2, 2) * s,
cofac(0, 2, 1, 0) * s,
574 co.z() * s,
cofac(0, 1, 2, 0) * s,
cofac(0, 0, 1, 1) * s);
581 m_el[0].x() * m[0].x() +
m_el[1].x() * m[1].x() +
m_el[2].x() * m[2].x(),
582 m_el[0].x() * m[0].y() +
m_el[1].x() * m[1].y() +
m_el[2].x() * m[2].y(),
583 m_el[0].x() * m[0].z() +
m_el[1].x() * m[1].z() +
m_el[2].x() * m[2].z(),
584 m_el[0].y() * m[0].x() +
m_el[1].y() * m[1].x() +
m_el[2].y() * m[2].x(),
585 m_el[0].y() * m[0].y() +
m_el[1].y() * m[1].y() +
m_el[2].y() * m[2].y(),
586 m_el[0].y() * m[0].z() +
m_el[1].y() * m[1].z() +
m_el[2].y() * m[2].z(),
587 m_el[0].z() * m[0].x() +
m_el[1].z() * m[1].x() +
m_el[2].z() * m[2].x(),
588 m_el[0].z() * m[0].y() +
m_el[1].z() * m[1].y() +
m_el[2].z() * m[2].y(),
589 m_el[0].z() * m[0].z() +
m_el[1].z() * m[1].z() +
m_el[2].z() * m[2].z());
605 return Vector3(m[0].
dot(v), m[1].
dot(v), m[2].
dot(v));
643 return ( m1[0][0] == m2[0][0] && m1[1][0] == m2[1][0] && m1[2][0] == m2[2][0] &&
644 m1[0][1] == m2[0][1] && m1[1][1] == m2[1][1] && m1[2][1] == m2[2][1] &&
645 m1[0][2] == m2[0][2] && m1[1][2] == m2[1][2] && m1[2][2] == m2[2][2] );
665 for (
int i=0;i<3;i++)
671 for (
int i=0;i<3;i++)
678 for (
int i=0;i<3;i++)
684 for (
int i=0;i<3;i++)
690 for (
int i=0;i<3;i++)
695 #endif //TF2_MATRIX3x3_H
#define TF2SIMD_FORCE_INLINE
ROS_DEPRECATED void setEulerZYX(const tf2Scalar &yaw, const tf2Scalar &pitch, const tf2Scalar &roll)
Set the matrix from euler angles using YPR around ZYX respectively.
ROS_DEPRECATED void getEulerZYX(tf2Scalar &yaw, tf2Scalar &pitch, tf2Scalar &roll, unsigned int solution_number=1) const
Get the matrix represented as euler angles around ZYX.
static const Matrix3x3 & getIdentity()
TF2SIMD_FORCE_INLINE bool operator==(const Matrix3x3 &m1, const Matrix3x3 &m2)
Equality operator between two matrices It will test all elements are equal.
Matrix3x3 transposeTimes(const Matrix3x3 &m) const
TF2SIMD_FORCE_INLINE tf2Scalar tf2Cos(tf2Scalar x)
TF2SIMD_FORCE_INLINE const Vector3 & operator[](int i) const
Get a const reference to a row of the matrix as a vector.
tf2Scalar determinant() const
Return the determinant of the matrix.
void serialize(struct Matrix3x3Data &dataOut) const
Vector3DoubleData m_el[3]
Matrix3x3 transpose() const
Return the transpose of the matrix.
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 timesTranspose(const Matrix3x3 &m) const
void deSerialize(const struct Matrix3x3Data &dataIn)
void deSerializeDouble(const struct Matrix3x3DoubleData &dataIn)
Matrix3x3(const Quaternion &q)
Constructor from Quaternion.
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.
tf2Scalar cofac(int r1, int c1, int r2, int c2) const
Calculate the matrix cofactor.
Matrix3x3()
No initializaion constructor.
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 setRPY(tf2Scalar roll, tf2Scalar pitch, tf2Scalar yaw)
Set the matrix using RPY about XYZ fixed axes.
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.
Matrix3x3 scaled(const Vector3 &s) const
Create a scaled copy of the matrix.
void deSerializeFloat(const struct Matrix3x3FloatData &dataIn)
TF2SIMD_FORCE_INLINE tf2Scalar tf2Asin(tf2Scalar x)
TF2SIMD_FORCE_INLINE Vector3 & operator[](int i)
Get a mutable reference to a row of the matrix as a vector.
TF2SIMD_FORCE_INLINE tf2Scalar tdotz(const Vector3 &v) const
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.
void serializeFloat(struct Matrix3x3FloatData &dataOut) const
TF2SIMD_FORCE_INLINE tf2Scalar tdoty(const Vector3 &v) const
TF2SIMD_FORCE_INLINE tf2Scalar tf2Sin(tf2Scalar x)
TF2SIMD_FORCE_INLINE tf2Scalar tdotx(const Vector3 &v) const
Matrix3x3 adjoint() const
Return the adjoint of the matrix.
TF2SIMD_FORCE_INLINE tf2Scalar tf2Sqrt(tf2Scalar x)
TF2SIMD_FORCE_INLINE const Vector3 & getRow(int i) const
Get a row of the matrix as a vector.
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 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.
void setEulerYPR(tf2Scalar eulerZ, tf2Scalar eulerY, tf2Scalar eulerX)
Set the matrix from euler angles YPR around ZYX axes.
TF2SIMD_FORCE_INLINE Vector3 getColumn(int i) const
Get a column of the matrix as a vector.
Matrix3x3 absolute() const
Return the matrix with all values non negative.
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 length2() const
Return the length squared of the quaternion.
Matrix3x3 inverse() const
Return the inverse of the matrix.
void getRotation(Quaternion &q) const
Get the matrix represented as a quaternion.
TF2SIMD_FORCE_INLINE Matrix3x3 & operator=(const Matrix3x3 &other)
Assignment Operator.
The Quaternion implements quaternion to perform linear algebra rotations in combination with Matrix3x...
void getOpenGLSubMatrix(tf2Scalar *m) const
Fill the values of the matrix into a 9 element array.
void setIdentity()
Set the matrix to the identity.