tf2
master
tf2 maintains the relationship between coordinate frames in a tree structure buffered in time, and lets the user transform points, vectors, etc between any two coordinate frames at any desired point in time.
|
Go to the documentation of this file.
16 #ifndef TF2_MATRIX3x3_H
17 #define TF2_MATRIX3x3_H
28 #define Matrix3x3Data Matrix3x3DoubleData
68 m_el[0] = other.m_el[0];
69 m_el[1] = other.m_el[1];
70 m_el[2] = other.m_el[2];
77 m_el[0] = other.m_el[0];
78 m_el[1] = other.m_el[1];
79 m_el[2] = other.m_el[2];
88 return Vector3(m_el[0][i],m_el[1][i],m_el[2][i]);
127 m_el[0].setValue(m[0],m[4],m[8]);
128 m_el[1].setValue(m[1],m[5],m[9]);
129 m_el[2].setValue(m[2],m[6],m[10]);
147 m_el[0].setValue(xx,xy,xz);
148 m_el[1].setValue(yx,yy,yz);
149 m_el[2].setValue(zx,zy,zz);
160 tf2Scalar xs = q.x() * s, ys = q.y() * s, zs = q.z() * s;
161 tf2Scalar wx = q.w() * xs, wy = q.w() * ys, wz = q.w() * zs;
162 tf2Scalar xx = q.x() * xs, xy = q.x() * ys, xz = q.x() * zs;
163 tf2Scalar yy = q.y() * ys, yz = q.y() * zs, zz = q.z() * zs;
165 xy + wz,
tf2Scalar(1.0) - (xx + zz), yz - wx,
166 xz - wy, yz + wx,
tf2Scalar(1.0) - (xx + yy));
191 setValue(cj * ch, sj * sc - cs, sj * cc + ss,
192 cj * sh, sj * ss + cc, sj * cs - sc,
193 -sj, cj * si, cj * ci);
222 return identityMatrix;
249 tf2Scalar trace = m_el[0].x() + m_el[1].y() + m_el[2].z();
258 temp[0]=((m_el[2].y() - m_el[1].z()) * s);
259 temp[1]=((m_el[0].z() - m_el[2].x()) * s);
260 temp[2]=((m_el[1].x() - m_el[0].y()) * s);
264 int i = m_el[0].x() < m_el[1].y() ?
265 (m_el[1].y() < m_el[2].z() ? 2 : 1) :
266 (m_el[0].x() < m_el[2].z() ? 2 : 0);
274 temp[3] = (m_el[k][j] - m_el[j][k]) * s;
275 temp[j] = (m_el[j][i] + m_el[i][j]) * s;
276 temp[k] = (m_el[k][i] + m_el[i][k]) * s;
278 q.setValue(temp[0],temp[1],temp[2],temp[3]);
312 euler_out.roll = delta;
313 euler_out2.roll = delta;
319 euler_out.roll = delta;
320 euler_out2.roll = delta;
325 euler_out.pitch = -
tf2Asin(m_el[2].x());
326 euler_out2.pitch =
TF2SIMD_PI - euler_out.pitch;
329 m_el[2].z()/
tf2Cos(euler_out.pitch));
331 m_el[2].z()/
tf2Cos(euler_out2.pitch));
334 m_el[0].x()/
tf2Cos(euler_out.pitch));
336 m_el[0].x()/
tf2Cos(euler_out2.pitch));
339 if (solution_number == 1)
342 pitch = euler_out.pitch;
343 roll = euler_out.roll;
347 yaw = euler_out2.yaw;
348 pitch = euler_out2.pitch;
349 roll = euler_out2.roll;
370 return Matrix3x3(m_el[0].x() * s.x(), m_el[0].y() * s.y(), m_el[0].z() * s.z(),
371 m_el[1].x() * s.x(), m_el[1].y() * s.y(), m_el[1].z() * s.z(),
372 m_el[2].x() * s.x(), m_el[2].y() * s.y(), m_el[2].z() * s.z());
398 return m_el[0].x() * v.x() + m_el[1].x() * v.y() + m_el[2].x() * v.z();
402 return m_el[0].y() * v.x() + m_el[1].y() * v.y() + m_el[2].y() * v.z();
406 return m_el[0].z() * v.x() + m_el[1].z() * v.y() + m_el[2].z() * v.z();
423 for (
int step = maxSteps; step > 0; step--)
458 tf2Scalar theta = (m_el[q][q] - m_el[p][p]) / (2 * mpq);
464 t = (theta >= 0) ? 1 / (theta +
tf2Sqrt(1 + theta2))
465 : 1 / (theta -
tf2Sqrt(1 + theta2));
472 t = 1 / (theta * (2 +
tf2Scalar(0.5) / theta2));
478 m_el[p][q] = m_el[q][p] = 0;
479 m_el[p][p] -= t * mpq;
480 m_el[q][q] += t * mpq;
483 m_el[r][p] = m_el[p][r] = cos * mrp - sin * mrq;
484 m_el[r][q] = m_el[q][r] = cos * mrq + sin * mrp;
487 for (
int i = 0; i < 3; i++)
489 Vector3& row = rot[i];
492 row[p] = cos * mrp - sin * mrq;
493 row[q] = cos * mrq + sin * mrp;
511 return m_el[r1][c1] * m_el[r2][c2] - m_el[r1][c2] * m_el[r2][c1];
544 return tf2Triple((*
this)[0], (*
this)[1], (*
this)[2]);
560 return Matrix3x3(m_el[0].x(), m_el[1].x(), m_el[2].x(),
561 m_el[0].y(), m_el[1].y(), m_el[2].y(),
562 m_el[0].z(), m_el[1].z(), m_el[2].z());
568 return Matrix3x3(
cofac(1, 1, 2, 2),
cofac(0, 2, 2, 1),
cofac(0, 1, 1, 2),
569 cofac(1, 2, 2, 0),
cofac(0, 0, 2, 2),
cofac(0, 2, 1, 0),
570 cofac(1, 0, 2, 1),
cofac(0, 1, 2, 0),
cofac(0, 0, 1, 1));
576 Vector3 co(
cofac(1, 1, 2, 2),
cofac(1, 2, 2, 0),
cofac(1, 0, 2, 1));
581 co.y() * s,
cofac(0, 0, 2, 2) * s,
cofac(0, 2, 1, 0) * s,
582 co.z() * s,
cofac(0, 1, 2, 0) * s,
cofac(0, 0, 1, 1) * s);
589 m_el[0].x() * m[0].x() + m_el[1].x() * m[1].x() + m_el[2].x() * m[2].x(),
590 m_el[0].x() * m[0].y() + m_el[1].x() * m[1].y() + m_el[2].x() * m[2].y(),
591 m_el[0].x() * m[0].z() + m_el[1].x() * m[1].z() + m_el[2].x() * m[2].z(),
592 m_el[0].y() * m[0].x() + m_el[1].y() * m[1].x() + m_el[2].y() * m[2].x(),
593 m_el[0].y() * m[0].y() + m_el[1].y() * m[1].y() + m_el[2].y() * m[2].y(),
594 m_el[0].y() * m[0].z() + m_el[1].y() * m[1].z() + m_el[2].y() * m[2].z(),
595 m_el[0].z() * m[0].x() + m_el[1].z() * m[1].x() + m_el[2].z() * m[2].x(),
596 m_el[0].z() * m[0].y() + m_el[1].z() * m[1].y() + m_el[2].z() * m[2].y(),
597 m_el[0].z() * m[0].z() + m_el[1].z() * m[1].z() + m_el[2].z() * m[2].z());
604 m_el[0].
dot(m[0]), m_el[0].
dot(m[1]), m_el[0].
dot(m[2]),
605 m_el[1].
dot(m[0]), m_el[1].
dot(m[1]), m_el[1].
dot(m[2]),
606 m_el[2].
dot(m[0]), m_el[2].
dot(m[1]), m_el[2].
dot(m[2]));
613 return Vector3(m[0].
dot(v), m[1].
dot(v), m[2].
dot(v));
651 return ( m1[0][0] == m2[0][0] && m1[1][0] == m2[1][0] && m1[2][0] == m2[2][0] &&
652 m1[0][1] == m2[0][1] && m1[1][1] == m2[1][1] && m1[2][1] == m2[2][1] &&
653 m1[0][2] == m2[0][2] && m1[1][2] == m2[1][2] && m1[2][2] == m2[2][2] );
673 for (
int i=0;i<3;i++)
679 for (
int i=0;i<3;i++)
686 for (
int i=0;i<3;i++)
692 for (
int i=0;i<3;i++)
698 for (
int i=0;i<3;i++)
703 #endif //TF2_MATRIX3x3_H
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.
Definition: Matrix3x3.h:286
TF2SIMD_FORCE_INLINE tf2Scalar dot(const Quaternion &q1, const Quaternion &q2)
Calculate the dot product between two quaternions.
Definition: Quaternion.h:395
void diagonalize(Matrix3x3 &rot, tf2Scalar threshold, int maxSteps)
diagonalizes this matrix by the Jacobi method.
Definition: Matrix3x3.h:420
void setRPY(tf2Scalar roll, tf2Scalar pitch, tf2Scalar yaw)
Set the matrix using RPY about XYZ fixed axes.
Definition: Matrix3x3.h:203
TF2SIMD_FORCE_INLINE tf2Scalar tf2Asin(tf2Scalar x)
Definition: Scalar.h:183
Matrix3x3 & operator*=(const Matrix3x3 &m)
Multiply by the target matrix on the right.
Definition: Matrix3x3.h:533
TF2SIMD_FORCE_INLINE tf2Scalar tf2Atan2(tf2Scalar x, tf2Scalar y)
Definition: Scalar.h:185
TF2SIMD_FORCE_INLINE Vector3 getColumn(int i) const
Get a column of the matrix as a vector.
Definition: Matrix3x3.h:86
Matrix3x3()
No initializaion constructor.
Definition: Matrix3x3.h:41
void deSerializeFloat(const struct Matrix3x3FloatData &dataIn)
Definition: Matrix3x3.h:690
Matrix3x3 inverse() const
Return the inverse of the matrix.
Definition: Matrix3x3.h:574
void getRotation(Quaternion &q) const
Get the matrix represented as a quaternion.
Definition: Matrix3x3.h:247
TF2SIMD_FORCE_INLINE tf2Scalar tdotz(const Vector3 &v) const
Definition: Matrix3x3.h:404
#define TF2SIMD_EPSILON
Definition: Scalar.h:202
TF2SIMD_FORCE_INLINE tf2Scalar tf2Sin(tf2Scalar x)
Definition: Scalar.h:180
TF2SIMD_FORCE_INLINE Matrix3x3 & operator=(const Matrix3x3 &other)
Assignment Operator.
Definition: Matrix3x3.h:75
Definition: Vector3.h:686
TF2SIMD_FORCE_INLINE tf2Scalar tf2Sqrt(tf2Scalar x)
Definition: Scalar.h:177
static const Matrix3x3 & getIdentity()
Definition: Matrix3x3.h:217
void getOpenGLSubMatrix(tf2Scalar *m) const
Fill the values of the matrix into a 9 element array.
Definition: Matrix3x3.h:228
void setRotation(const Quaternion &q)
Set the matrix from a quaternion.
Definition: Matrix3x3.h:155
Matrix3x3 transposeTimes(const Matrix3x3 &m) const
Definition: Matrix3x3.h:586
TF2SIMD_FORCE_INLINE tf2Scalar tdotx(const Vector3 &v) const
Definition: Matrix3x3.h:396
Definition: Vector3.h:681
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)
Definition: Matrix3x3.h:143
TF2SIMD_FORCE_INLINE Matrix3x3(const Matrix3x3 &other)
Copy constructor.
Definition: Matrix3x3.h:66
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.
Definition: Matrix3x3.h:359
#define tf2FullAssert(x)
Definition: Scalar.h:148
for serialization
Definition: Matrix3x3.h:657
void deSerialize(const struct Matrix3x3Data &dataIn)
Definition: Matrix3x3.h:684
TF2SIMD_FORCE_INLINE bool operator==(const Matrix3x3 &m1, const Matrix3x3 &m2)
Equality operator between two matrices It will test all elements are equal.
Definition: Matrix3x3.h:649
void deSerializeDouble(const struct Matrix3x3DoubleData &dataIn)
Definition: Matrix3x3.h:696
void setEulerYPR(tf2Scalar eulerZ, tf2Scalar eulerY, tf2Scalar eulerX)
Set the matrix from euler angles YPR around ZYX axes.
Definition: Matrix3x3.h:179
TF2SIMD_FORCE_INLINE Vector3 operator*(const Matrix3x3 &m, const Vector3 &v)
Definition: Matrix3x3.h:611
Matrix3x3(const Quaternion &q)
Constructor from Quaternion.
Definition: Matrix3x3.h:47
Matrix3x3 absolute() const
Return the matrix with all values non negative.
Definition: Matrix3x3.h:549
#define TF2SIMD_FORCE_INLINE
Definition: Scalar.h:129
for serialization
Definition: Matrix3x3.h:663
const TF2SIMD_FORCE_INLINE Vector3 & getRow(int i) const
Get a row of the matrix as a vector.
Definition: Matrix3x3.h:94
TF2SIMD_FORCE_INLINE tf2Scalar tf2Cos(tf2Scalar x)
Definition: Scalar.h:179
void setIdentity()
Set the matrix to the identity.
Definition: Matrix3x3.h:209
Matrix3x3 scaled(const Vector3 &s) const
Create a scaled copy of the matrix.
Definition: Matrix3x3.h:368
tf2Scalar determinant() const
Return the determinant of the matrix.
Definition: Matrix3x3.h:542
#define Matrix3x3Data
Definition: Matrix3x3.h:28
const TF2SIMD_FORCE_INLINE Vector3 & operator[](int i) const
Get a const reference to a row of the matrix as a vector.
Definition: Matrix3x3.h:110
Vector3DoubleData m_el[3]
Definition: Matrix3x3.h:665
TF2SIMD_FORCE_INLINE tf2Scalar tf2Triple(const Vector3 &v1, const Vector3 &v2, const Vector3 &v3)
Definition: Vector3.h:449
Definition: buffer_core.h:55
TF2SIMD_FORCE_INLINE tf2Scalar tf2Fabs(tf2Scalar x)
Definition: Scalar.h:178
void setFromOpenGLSubMatrix(const tf2Scalar *m)
Set from a carray of tf2Scalars.
Definition: Matrix3x3.h:125
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.
Definition: Matrix3x3.h:57
void serializeFloat(struct Matrix3x3FloatData &dataOut) const
Definition: Matrix3x3.h:677
#define TF2_PUBLIC
Definition: visibility_control.h:58
The Quaternion implements quaternion to perform linear algebra rotations in combination with Matrix3x...
Definition: Quaternion.h:29
Matrix3x3 timesTranspose(const Matrix3x3 &m) const
Definition: Matrix3x3.h:601
void serialize(struct Matrix3x3Data &dataOut) const
Definition: Matrix3x3.h:671
Matrix3x3 transpose() const
Return the transpose of the matrix.
Definition: Matrix3x3.h:558
double tf2Scalar
The tf2Scalar type abstracts floating point numbers, to easily switch between double and single float...
Definition: Scalar.h:159
#define TF2SIMD_PI
Definition: Scalar.h:193
The Matrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with Q...
Definition: Matrix3x3.h:33
Vector3FloatData m_el[3]
Definition: Matrix3x3.h:659
TF2SIMD_FORCE_INLINE Vector3 & operator[](int i)
Get a mutable reference to a row of the matrix as a vector.
Definition: Matrix3x3.h:102
Matrix3x3 adjoint() const
Return the adjoint of the matrix.
Definition: Matrix3x3.h:566
TF2SIMD_FORCE_INLINE tf2Scalar tdoty(const Vector3 &v) const
Definition: Matrix3x3.h:400
tf2Scalar cofac(int r1, int c1, int r2, int c2) const
Calculate the matrix cofactor.
Definition: Matrix3x3.h:509
tf2Scalar length2() const
Return the length squared of the quaternion.
Definition: Quaternion.h:152