diff --git a/test_tf2/test/buffer_core_test.cpp b/test_tf2/test/buffer_core_test.cpp index ad3ec28c9..751e45c12 100644 --- a/test_tf2/test/buffer_core_test.cpp +++ b/test_tf2/test/buffer_core_test.cpp @@ -44,10 +44,10 @@ #include #include #include -#include -#include -#include -#include +#include +#include +#include +#include #include #include diff --git a/test_tf2/test/test_convert.cpp b/test_tf2/test/test_convert.cpp index 6e15bba77..09c712154 100644 --- a/test_tf2/test/test_convert.cpp +++ b/test_tf2/test/test_convert.cpp @@ -38,8 +38,8 @@ #include #include #include -#include -#include +#include +#include #include #include #include diff --git a/test_tf2/test/test_message_filter.cpp b/test_tf2/test/test_message_filter.cpp index 29c23ce6b..1c948263e 100644 --- a/test_tf2/test/test_message_filter.cpp +++ b/test_tf2/test/test_message_filter.cpp @@ -40,10 +40,10 @@ #include #include #include -#include -#include -#include -#include +#include +#include +#include +#include #include #include #include diff --git a/test_tf2/test/test_static_publisher.cpp b/test_tf2/test/test_static_publisher.cpp index bd1274667..0373cf2b9 100644 --- a/test_tf2/test/test_static_publisher.cpp +++ b/test_tf2/test/test_static_publisher.cpp @@ -36,9 +36,9 @@ #include #include -#include -#include -#include +#include +#include +#include #include #include #include diff --git a/test_tf2/test/test_tf2_bullet.cpp b/test_tf2/test/test_tf2_bullet.cpp index 8b30b055d..70d0895b9 100644 --- a/test_tf2/test/test_tf2_bullet.cpp +++ b/test_tf2/test/test_tf2_bullet.cpp @@ -33,7 +33,7 @@ #include #include #include -#include +#include std::unique_ptr tf_buffer = nullptr; static const double EPS = 1e-3; diff --git a/test_tf2/test/test_utils.cpp b/test_tf2/test/test_utils.cpp index 55595b244..12db4919a 100644 --- a/test_tf2/test/test_utils.cpp +++ b/test_tf2/test/test_utils.cpp @@ -18,8 +18,8 @@ #include #include #include -#include -#include +#include +#include #include double epsilon = 1e-9; diff --git a/tf2/CMakeLists.txt b/tf2/CMakeLists.txt index 2c2818546..b5a06f0eb 100644 --- a/tf2/CMakeLists.txt +++ b/tf2/CMakeLists.txt @@ -62,6 +62,13 @@ if(BUILD_TESTING) include/tf2/LinearMath/Scalar.h include/tf2/LinearMath/Transform.h include/tf2/LinearMath/Vector3.h + include/tf2/LinearMath/Matrix3x3.hpp + include/tf2/LinearMath/MinMax.hpp + include/tf2/LinearMath/QuadWord.hpp + include/tf2/LinearMath/Quaternion.hpp + include/tf2/LinearMath/Scalar.hpp + include/tf2/LinearMath/Transform.hpp + include/tf2/LinearMath/Vector3.hpp ) ament_copyright(EXCLUDE ${_linter_excludes}) diff --git a/tf2/include/tf2/LinearMath/Matrix3x3.h b/tf2/include/tf2/LinearMath/Matrix3x3.h index 7067af41c..ad85c5410 100644 --- a/tf2/include/tf2/LinearMath/Matrix3x3.h +++ b/tf2/include/tf2/LinearMath/Matrix3x3.h @@ -13,691 +13,9 @@ subject to the following restrictions: */ -#ifndef TF2_MATRIX3x3_H -#define TF2_MATRIX3x3_H +#ifndef TF2__LINEARMATH__MATRIX3X3_H_ +#define TF2__LINEARMATH__MATRIX3X3_H_ -#include "Vector3.h" -#include "Quaternion.h" +#include -#include "tf2/visibility_control.h" - -namespace tf2 -{ - - -#define Matrix3x3Data Matrix3x3DoubleData - - -/**@brief The Matrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with Quaternion, Transform and Vector3. -* Make sure to only include a pure orthogonal matrix without scaling. */ -class Matrix3x3 { - - ///Data storage for the matrix, each vector is a row of the matrix - Vector3 m_el[3]; - -public: - /** @brief No initializaion constructor */ - TF2_PUBLIC - Matrix3x3 () {} - - // explicit Matrix3x3(const tf2Scalar *m) { setFromOpenGLSubMatrix(m); } - - /**@brief Constructor from Quaternion */ - TF2_PUBLIC - explicit Matrix3x3(const Quaternion& q) { setRotation(q); } - /* - template - Matrix3x3(const tf2Scalar& yaw, const tf2Scalar& pitch, const tf2Scalar& roll) - { - setEulerYPR(yaw, pitch, roll); - } - */ - /** @brief Constructor with row major formatting */ - TF2_PUBLIC - 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) - { - setValue(xx, xy, xz, - yx, yy, yz, - zx, zy, zz); - } - /** @brief Copy constructor */ - TF2SIMD_FORCE_INLINE Matrix3x3 (const Matrix3x3& other) - { - m_el[0] = other.m_el[0]; - m_el[1] = other.m_el[1]; - m_el[2] = other.m_el[2]; - } - - - /** @brief Assignment Operator */ - TF2SIMD_FORCE_INLINE Matrix3x3& operator=(const Matrix3x3& other) - { - m_el[0] = other.m_el[0]; - m_el[1] = other.m_el[1]; - m_el[2] = other.m_el[2]; - return *this; - } - - - /** @brief Get a column of the matrix as a vector - * @param i Column number 0 indexed */ - TF2SIMD_FORCE_INLINE Vector3 getColumn(int i) const - { - return Vector3(m_el[0][i],m_el[1][i],m_el[2][i]); - } - - - /** @brief Get a row of the matrix as a vector - * @param i Row number 0 indexed */ - TF2SIMD_FORCE_INLINE const Vector3& getRow(int i) const - { - tf2FullAssert(0 <= i && i < 3); - return m_el[i]; - } - - /** @brief Get a mutable reference to a row of the matrix as a vector - * @param i Row number 0 indexed */ - TF2SIMD_FORCE_INLINE Vector3& operator[](int i) - { - tf2FullAssert(0 <= i && i < 3); - return m_el[i]; - } - - /** @brief Get a const reference to a row of the matrix as a vector - * @param i Row number 0 indexed */ - TF2SIMD_FORCE_INLINE const Vector3& operator[](int i) const - { - tf2FullAssert(0 <= i && i < 3); - return m_el[i]; - } - - /** @brief Multiply by the target matrix on the right - * @param m Rotation matrix to be applied - * Equivilant to this = this * m */ - TF2_PUBLIC - Matrix3x3& operator*=(const Matrix3x3& m); - - /** @brief Set from a carray of tf2Scalars - * @param m A pointer to the beginning of an array of 9 tf2Scalars */ - TF2_PUBLIC - void setFromOpenGLSubMatrix(const tf2Scalar *m) - { - m_el[0].setValue(m[0],m[4],m[8]); - m_el[1].setValue(m[1],m[5],m[9]); - m_el[2].setValue(m[2],m[6],m[10]); - - } - /** @brief Set the values of the matrix explicitly (row major) - * @param xx Top left - * @param xy Top Middle - * @param xz Top Right - * @param yx Middle Left - * @param yy Middle Middle - * @param yz Middle Right - * @param zx Bottom Left - * @param zy Bottom Middle - * @param zz Bottom Right*/ - TF2_PUBLIC - 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) - { - m_el[0].setValue(xx,xy,xz); - m_el[1].setValue(yx,yy,yz); - m_el[2].setValue(zx,zy,zz); - } - - /** @brief Set the matrix from a quaternion - * @param q The Quaternion to match */ - TF2_PUBLIC - void setRotation(const Quaternion& q) - { - tf2Scalar d = q.length2(); - tf2FullAssert(d != tf2Scalar(0.0)); - tf2Scalar s = tf2Scalar(2.0) / d; - tf2Scalar xs = q.x() * s, ys = q.y() * s, zs = q.z() * s; - tf2Scalar wx = q.w() * xs, wy = q.w() * ys, wz = q.w() * zs; - tf2Scalar xx = q.x() * xs, xy = q.x() * ys, xz = q.x() * zs; - tf2Scalar yy = q.y() * ys, yz = q.y() * zs, zz = q.z() * zs; - setValue(tf2Scalar(1.0) - (yy + zz), xy - wz, xz + wy, - xy + wz, tf2Scalar(1.0) - (xx + zz), yz - wx, - xz - wy, yz + wx, tf2Scalar(1.0) - (xx + yy)); - } - - /** @brief Set the matrix from euler angles YPR around ZYX axes - * @param eulerZ Yaw aboud Z axis - * @param eulerY Pitch around Y axis - * @param eulerX Roll about X axis - * - * These angles are used to produce a rotation matrix. The euler - * angles are applied in ZYX order. I.e a vector is first rotated - * about X then Y and then Z - **/ - TF2_PUBLIC - void setEulerYPR(tf2Scalar eulerZ, tf2Scalar eulerY,tf2Scalar eulerX) { - tf2Scalar ci ( tf2Cos(eulerX)); - tf2Scalar cj ( tf2Cos(eulerY)); - tf2Scalar ch ( tf2Cos(eulerZ)); - tf2Scalar si ( tf2Sin(eulerX)); - tf2Scalar sj ( tf2Sin(eulerY)); - tf2Scalar sh ( tf2Sin(eulerZ)); - tf2Scalar cc = ci * ch; - tf2Scalar cs = ci * sh; - tf2Scalar sc = si * ch; - tf2Scalar ss = si * sh; - - setValue(cj * ch, sj * sc - cs, sj * cc + ss, - cj * sh, sj * ss + cc, sj * cs - sc, - -sj, cj * si, cj * ci); - } - - /** @brief Set the matrix using RPY about XYZ fixed axes - * @param roll Roll about X axis - * @param pitch Pitch around Y axis - * @param yaw Yaw aboud Z axis - * - **/ - TF2_PUBLIC - void setRPY(tf2Scalar roll, tf2Scalar pitch,tf2Scalar yaw) { - setEulerYPR(yaw, pitch, roll); - } - - /**@brief Set the matrix to the identity */ - TF2_PUBLIC - void setIdentity() - { - setValue(tf2Scalar(1.0), tf2Scalar(0.0), tf2Scalar(0.0), - tf2Scalar(0.0), tf2Scalar(1.0), tf2Scalar(0.0), - tf2Scalar(0.0), tf2Scalar(0.0), tf2Scalar(1.0)); - } - - TF2_PUBLIC - static const Matrix3x3& getIdentity() - { - static const Matrix3x3 identityMatrix(tf2Scalar(1.0), tf2Scalar(0.0), tf2Scalar(0.0), - tf2Scalar(0.0), tf2Scalar(1.0), tf2Scalar(0.0), - tf2Scalar(0.0), tf2Scalar(0.0), tf2Scalar(1.0)); - return identityMatrix; - } - - /**@brief Fill the values of the matrix into a 9 element array - * @param m The array to be filled */ - TF2_PUBLIC - void getOpenGLSubMatrix(tf2Scalar *m) const - { - m[0] = tf2Scalar(m_el[0].x()); - m[1] = tf2Scalar(m_el[1].x()); - m[2] = tf2Scalar(m_el[2].x()); - m[3] = tf2Scalar(0.0); - m[4] = tf2Scalar(m_el[0].y()); - m[5] = tf2Scalar(m_el[1].y()); - m[6] = tf2Scalar(m_el[2].y()); - m[7] = tf2Scalar(0.0); - m[8] = tf2Scalar(m_el[0].z()); - m[9] = tf2Scalar(m_el[1].z()); - m[10] = tf2Scalar(m_el[2].z()); - m[11] = tf2Scalar(0.0); - } - - /**@brief Get the matrix represented as a quaternion - * @param q The quaternion which will be set */ - TF2_PUBLIC - void getRotation(Quaternion& q) const - { - tf2Scalar trace = m_el[0].x() + m_el[1].y() + m_el[2].z(); - tf2Scalar temp[4]; - - if (trace > tf2Scalar(0.0)) - { - tf2Scalar s = tf2Sqrt(trace + tf2Scalar(1.0)); - temp[3]=(s * tf2Scalar(0.5)); - s = tf2Scalar(0.5) / s; - - temp[0]=((m_el[2].y() - m_el[1].z()) * s); - temp[1]=((m_el[0].z() - m_el[2].x()) * s); - temp[2]=((m_el[1].x() - m_el[0].y()) * s); - } - else - { - int i = m_el[0].x() < m_el[1].y() ? - (m_el[1].y() < m_el[2].z() ? 2 : 1) : - (m_el[0].x() < m_el[2].z() ? 2 : 0); - int j = (i + 1) % 3; - int k = (i + 2) % 3; - - tf2Scalar s = tf2Sqrt(m_el[i][i] - m_el[j][j] - m_el[k][k] + tf2Scalar(1.0)); - temp[i] = s * tf2Scalar(0.5); - s = tf2Scalar(0.5) / s; - - temp[3] = (m_el[k][j] - m_el[j][k]) * s; - temp[j] = (m_el[j][i] + m_el[i][j]) * s; - temp[k] = (m_el[k][i] + m_el[i][k]) * s; - } - q.setValue(temp[0],temp[1],temp[2],temp[3]); - } - - /**@brief Get the matrix represented as euler angles around YXZ, roundtrip with setEulerYPR - * @param yaw Yaw around Z axis - * @param pitch Pitch around Y axis - * @param roll around X axis */ - TF2_PUBLIC - void getEulerYPR(tf2Scalar& yaw, tf2Scalar& pitch, tf2Scalar& roll, unsigned int solution_number = 1) const - { - struct Euler - { - tf2Scalar yaw; - tf2Scalar pitch; - tf2Scalar roll; - }; - - Euler euler_out; - Euler euler_out2; //second solution - //get the pointer to the raw data - - // Check that pitch is not at a singularity - // Check that pitch is not at a singularity - if (tf2Fabs(m_el[2].x()) >= 1) - { - euler_out.yaw = 0; - euler_out2.yaw = 0; - - // From difference of angles formula - tf2Scalar delta = tf2Atan2(m_el[2].y(),m_el[2].z()); - if (m_el[2].x() < 0) //gimbal locked down - { - euler_out.pitch = TF2SIMD_PI / tf2Scalar(2.0); - euler_out2.pitch = TF2SIMD_PI / tf2Scalar(2.0); - euler_out.roll = delta; - euler_out2.roll = delta; - } - else // gimbal locked up - { - euler_out.pitch = -TF2SIMD_PI / tf2Scalar(2.0); - euler_out2.pitch = -TF2SIMD_PI / tf2Scalar(2.0); - euler_out.roll = delta; - euler_out2.roll = delta; - } - } - else - { - euler_out.pitch = - tf2Asin(m_el[2].x()); - euler_out2.pitch = TF2SIMD_PI - euler_out.pitch; - - euler_out.roll = tf2Atan2(m_el[2].y()/tf2Cos(euler_out.pitch), - m_el[2].z()/tf2Cos(euler_out.pitch)); - euler_out2.roll = tf2Atan2(m_el[2].y()/tf2Cos(euler_out2.pitch), - m_el[2].z()/tf2Cos(euler_out2.pitch)); - - euler_out.yaw = tf2Atan2(m_el[1].x()/tf2Cos(euler_out.pitch), - m_el[0].x()/tf2Cos(euler_out.pitch)); - euler_out2.yaw = tf2Atan2(m_el[1].x()/tf2Cos(euler_out2.pitch), - m_el[0].x()/tf2Cos(euler_out2.pitch)); - } - - if (solution_number == 1) - { - yaw = euler_out.yaw; - pitch = euler_out.pitch; - roll = euler_out.roll; - } - else - { - yaw = euler_out2.yaw; - pitch = euler_out2.pitch; - roll = euler_out2.roll; - } - } - - /**@brief Get the matrix represented as roll pitch and yaw about fixed axes XYZ - * @param roll around X axis - * @param pitch Pitch around Y axis - * @param yaw Yaw around Z axis - * @param solution_number Which solution of two possible solutions ( 1 or 2) are possible values*/ - TF2_PUBLIC - void getRPY(tf2Scalar& roll, tf2Scalar& pitch, tf2Scalar& yaw, unsigned int solution_number = 1) const - { - getEulerYPR(yaw, pitch, roll, solution_number); - } - - /**@brief Create a scaled copy of the matrix - * @param s Scaling vector The elements of the vector will scale each column */ - - TF2_PUBLIC - Matrix3x3 scaled(const Vector3& s) const - { - return Matrix3x3(m_el[0].x() * s.x(), m_el[0].y() * s.y(), m_el[0].z() * s.z(), - m_el[1].x() * s.x(), m_el[1].y() * s.y(), m_el[1].z() * s.z(), - m_el[2].x() * s.x(), m_el[2].y() * s.y(), m_el[2].z() * s.z()); - } - - /**@brief Return the determinant of the matrix */ - TF2_PUBLIC - tf2Scalar determinant() const; - /**@brief Return the adjoint of the matrix */ - TF2_PUBLIC - Matrix3x3 adjoint() const; - /**@brief Return the matrix with all values non negative */ - TF2_PUBLIC - Matrix3x3 absolute() const; - /**@brief Return the transpose of the matrix */ - TF2_PUBLIC - Matrix3x3 transpose() const; - /**@brief Return the inverse of the matrix */ - TF2_PUBLIC - Matrix3x3 inverse() const; - - TF2_PUBLIC - Matrix3x3 transposeTimes(const Matrix3x3& m) const; - TF2_PUBLIC - Matrix3x3 timesTranspose(const Matrix3x3& m) const; - - TF2SIMD_FORCE_INLINE tf2Scalar tdotx(const Vector3& v) const - { - return m_el[0].x() * v.x() + m_el[1].x() * v.y() + m_el[2].x() * v.z(); - } - TF2SIMD_FORCE_INLINE tf2Scalar tdoty(const Vector3& v) const - { - return m_el[0].y() * v.x() + m_el[1].y() * v.y() + m_el[2].y() * v.z(); - } - TF2SIMD_FORCE_INLINE tf2Scalar tdotz(const Vector3& v) const - { - return m_el[0].z() * v.x() + m_el[1].z() * v.y() + m_el[2].z() * v.z(); - } - - - /**@brief diagonalizes this matrix by the Jacobi method. - * @param rot stores the rotation from the coordinate system in which the matrix is diagonal to the original - * coordinate system, i.e., old_this = rot * new_this * rot^T. - * @param threshold See iteration - * @param iteration The iteration stops when all off-diagonal elements are less than the threshold multiplied - * by the sum of the absolute values of the diagonal, or when maxSteps have been executed. - * - * Note that this matrix is assumed to be symmetric. - */ - TF2_PUBLIC - void diagonalize(Matrix3x3& rot, tf2Scalar threshold, int maxSteps) - { - rot.setIdentity(); - for (int step = maxSteps; step > 0; step--) - { - // find off-diagonal element [p][q] with largest magnitude - int p = 0; - int q = 1; - int r = 2; - tf2Scalar max = tf2Fabs(m_el[0][1]); - tf2Scalar v = tf2Fabs(m_el[0][2]); - if (v > max) - { - q = 2; - r = 1; - max = v; - } - v = tf2Fabs(m_el[1][2]); - if (v > max) - { - p = 1; - q = 2; - r = 0; - max = v; - } - - tf2Scalar t = threshold * (tf2Fabs(m_el[0][0]) + tf2Fabs(m_el[1][1]) + tf2Fabs(m_el[2][2])); - if (max <= t) - { - if (max <= TF2SIMD_EPSILON * t) - { - return; - } - step = 1; - } - - // compute Jacobi rotation J which leads to a zero for element [p][q] - tf2Scalar mpq = m_el[p][q]; - tf2Scalar theta = (m_el[q][q] - m_el[p][p]) / (2 * mpq); - tf2Scalar theta2 = theta * theta; - tf2Scalar cos; - tf2Scalar sin; - if (theta2 * theta2 < tf2Scalar(10 / TF2SIMD_EPSILON)) - { - t = (theta >= 0) ? 1 / (theta + tf2Sqrt(1 + theta2)) - : 1 / (theta - tf2Sqrt(1 + theta2)); - cos = 1 / tf2Sqrt(1 + t * t); - sin = cos * t; - } - else - { - // approximation for large theta-value, i.e., a nearly diagonal matrix - t = 1 / (theta * (2 + tf2Scalar(0.5) / theta2)); - cos = 1 - tf2Scalar(0.5) * t * t; - sin = cos * t; - } - - // apply rotation to matrix (this = J^T * this * J) - m_el[p][q] = m_el[q][p] = 0; - m_el[p][p] -= t * mpq; - m_el[q][q] += t * mpq; - tf2Scalar mrp = m_el[r][p]; - tf2Scalar mrq = m_el[r][q]; - m_el[r][p] = m_el[p][r] = cos * mrp - sin * mrq; - m_el[r][q] = m_el[q][r] = cos * mrq + sin * mrp; - - // apply rotation to rot (rot = rot * J) - for (int i = 0; i < 3; i++) - { - Vector3& row = rot[i]; - mrp = row[p]; - mrq = row[q]; - row[p] = cos * mrp - sin * mrq; - row[q] = cos * mrq + sin * mrp; - } - } - } - - - - - /**@brief Calculate the matrix cofactor - * @param r1 The first row to use for calculating the cofactor - * @param c1 The first column to use for calculating the cofactor - * @param r1 The second row to use for calculating the cofactor - * @param c1 The second column to use for calculating the cofactor - * See http://en.wikipedia.org/wiki/Cofactor_(linear_algebra) for more details - */ - TF2_PUBLIC - tf2Scalar cofac(int r1, int c1, int r2, int c2) const - { - return m_el[r1][c1] * m_el[r2][c2] - m_el[r1][c2] * m_el[r2][c1]; - } - - TF2_PUBLIC - void serialize(struct Matrix3x3Data& dataOut) const; - - TF2_PUBLIC - void serializeFloat(struct Matrix3x3FloatData& dataOut) const; - - TF2_PUBLIC - void deSerialize(const struct Matrix3x3Data& dataIn); - - TF2_PUBLIC - void deSerializeFloat(const struct Matrix3x3FloatData& dataIn); - - TF2_PUBLIC - void deSerializeDouble(const struct Matrix3x3DoubleData& dataIn); - -}; - - -TF2SIMD_FORCE_INLINE Matrix3x3& -Matrix3x3::operator*=(const Matrix3x3& m) -{ - setValue(m.tdotx(m_el[0]), m.tdoty(m_el[0]), m.tdotz(m_el[0]), - m.tdotx(m_el[1]), m.tdoty(m_el[1]), m.tdotz(m_el[1]), - m.tdotx(m_el[2]), m.tdoty(m_el[2]), m.tdotz(m_el[2])); - return *this; -} - -TF2SIMD_FORCE_INLINE tf2Scalar -Matrix3x3::determinant() const -{ - return tf2Triple((*this)[0], (*this)[1], (*this)[2]); -} - - -TF2SIMD_FORCE_INLINE Matrix3x3 -Matrix3x3::absolute() const -{ - return Matrix3x3( - tf2Fabs(m_el[0].x()), tf2Fabs(m_el[0].y()), tf2Fabs(m_el[0].z()), - tf2Fabs(m_el[1].x()), tf2Fabs(m_el[1].y()), tf2Fabs(m_el[1].z()), - tf2Fabs(m_el[2].x()), tf2Fabs(m_el[2].y()), tf2Fabs(m_el[2].z())); -} - -TF2SIMD_FORCE_INLINE Matrix3x3 -Matrix3x3::transpose() const -{ - return Matrix3x3(m_el[0].x(), m_el[1].x(), m_el[2].x(), - m_el[0].y(), m_el[1].y(), m_el[2].y(), - m_el[0].z(), m_el[1].z(), m_el[2].z()); -} - -TF2SIMD_FORCE_INLINE Matrix3x3 -Matrix3x3::adjoint() const -{ - return Matrix3x3(cofac(1, 1, 2, 2), cofac(0, 2, 2, 1), cofac(0, 1, 1, 2), - cofac(1, 2, 2, 0), cofac(0, 0, 2, 2), cofac(0, 2, 1, 0), - cofac(1, 0, 2, 1), cofac(0, 1, 2, 0), cofac(0, 0, 1, 1)); -} - -TF2SIMD_FORCE_INLINE Matrix3x3 -Matrix3x3::inverse() const -{ - Vector3 co(cofac(1, 1, 2, 2), cofac(1, 2, 2, 0), cofac(1, 0, 2, 1)); - tf2Scalar det = (*this)[0].dot(co); - tf2FullAssert(det != tf2Scalar(0.0)); - tf2Scalar s = tf2Scalar(1.0) / det; - return Matrix3x3(co.x() * s, cofac(0, 2, 2, 1) * s, cofac(0, 1, 1, 2) * s, - co.y() * s, cofac(0, 0, 2, 2) * s, cofac(0, 2, 1, 0) * s, - co.z() * s, cofac(0, 1, 2, 0) * s, cofac(0, 0, 1, 1) * s); -} - -TF2SIMD_FORCE_INLINE Matrix3x3 -Matrix3x3::transposeTimes(const Matrix3x3& m) const -{ - return Matrix3x3( - m_el[0].x() * m[0].x() + m_el[1].x() * m[1].x() + m_el[2].x() * m[2].x(), - m_el[0].x() * m[0].y() + m_el[1].x() * m[1].y() + m_el[2].x() * m[2].y(), - m_el[0].x() * m[0].z() + m_el[1].x() * m[1].z() + m_el[2].x() * m[2].z(), - m_el[0].y() * m[0].x() + m_el[1].y() * m[1].x() + m_el[2].y() * m[2].x(), - m_el[0].y() * m[0].y() + m_el[1].y() * m[1].y() + m_el[2].y() * m[2].y(), - m_el[0].y() * m[0].z() + m_el[1].y() * m[1].z() + m_el[2].y() * m[2].z(), - m_el[0].z() * m[0].x() + m_el[1].z() * m[1].x() + m_el[2].z() * m[2].x(), - m_el[0].z() * m[0].y() + m_el[1].z() * m[1].y() + m_el[2].z() * m[2].y(), - m_el[0].z() * m[0].z() + m_el[1].z() * m[1].z() + m_el[2].z() * m[2].z()); -} - -TF2SIMD_FORCE_INLINE Matrix3x3 -Matrix3x3::timesTranspose(const Matrix3x3& m) const -{ - return Matrix3x3( - m_el[0].dot(m[0]), m_el[0].dot(m[1]), m_el[0].dot(m[2]), - m_el[1].dot(m[0]), m_el[1].dot(m[1]), m_el[1].dot(m[2]), - m_el[2].dot(m[0]), m_el[2].dot(m[1]), m_el[2].dot(m[2])); - -} - -TF2SIMD_FORCE_INLINE Vector3 -operator*(const Matrix3x3& m, const Vector3& v) -{ - return Vector3(m[0].dot(v), m[1].dot(v), m[2].dot(v)); -} - - -TF2SIMD_FORCE_INLINE Vector3 -operator*(const Vector3& v, const Matrix3x3& m) -{ - return Vector3(m.tdotx(v), m.tdoty(v), m.tdotz(v)); -} - -TF2SIMD_FORCE_INLINE Matrix3x3 -operator*(const Matrix3x3& m1, const Matrix3x3& m2) -{ - return Matrix3x3( - m2.tdotx( m1[0]), m2.tdoty( m1[0]), m2.tdotz( m1[0]), - m2.tdotx( m1[1]), m2.tdoty( m1[1]), m2.tdotz( m1[1]), - m2.tdotx( m1[2]), m2.tdoty( m1[2]), m2.tdotz( m1[2])); -} - -/* -TF2SIMD_FORCE_INLINE Matrix3x3 tf2MultTransposeLeft(const Matrix3x3& m1, const Matrix3x3& m2) { -return Matrix3x3( -m1[0][0] * m2[0][0] + m1[1][0] * m2[1][0] + m1[2][0] * m2[2][0], -m1[0][0] * m2[0][1] + m1[1][0] * m2[1][1] + m1[2][0] * m2[2][1], -m1[0][0] * m2[0][2] + m1[1][0] * m2[1][2] + m1[2][0] * m2[2][2], -m1[0][1] * m2[0][0] + m1[1][1] * m2[1][0] + m1[2][1] * m2[2][0], -m1[0][1] * m2[0][1] + m1[1][1] * m2[1][1] + m1[2][1] * m2[2][1], -m1[0][1] * m2[0][2] + m1[1][1] * m2[1][2] + m1[2][1] * m2[2][2], -m1[0][2] * m2[0][0] + m1[1][2] * m2[1][0] + m1[2][2] * m2[2][0], -m1[0][2] * m2[0][1] + m1[1][2] * m2[1][1] + m1[2][2] * m2[2][1], -m1[0][2] * m2[0][2] + m1[1][2] * m2[1][2] + m1[2][2] * m2[2][2]); -} -*/ - -/**@brief Equality operator between two matrices -* It will test all elements are equal. */ -TF2SIMD_FORCE_INLINE bool operator==(const Matrix3x3& m1, const Matrix3x3& m2) -{ - return ( m1[0][0] == m2[0][0] && m1[1][0] == m2[1][0] && m1[2][0] == m2[2][0] && - m1[0][1] == m2[0][1] && m1[1][1] == m2[1][1] && m1[2][1] == m2[2][1] && - m1[0][2] == m2[0][2] && m1[1][2] == m2[1][2] && m1[2][2] == m2[2][2] ); -} - -///for serialization -struct Matrix3x3FloatData -{ - Vector3FloatData m_el[3]; -}; - -///for serialization -struct Matrix3x3DoubleData -{ - Vector3DoubleData m_el[3]; -}; - - - - -TF2SIMD_FORCE_INLINE void Matrix3x3::serialize(struct Matrix3x3Data& dataOut) const -{ - for (int i=0;i<3;i++) - m_el[i].serialize(dataOut.m_el[i]); -} - -TF2SIMD_FORCE_INLINE void Matrix3x3::serializeFloat(struct Matrix3x3FloatData& dataOut) const -{ - for (int i=0;i<3;i++) - m_el[i].serializeFloat(dataOut.m_el[i]); -} - - -TF2SIMD_FORCE_INLINE void Matrix3x3::deSerialize(const struct Matrix3x3Data& dataIn) -{ - for (int i=0;i<3;i++) - m_el[i].deSerialize(dataIn.m_el[i]); -} - -TF2SIMD_FORCE_INLINE void Matrix3x3::deSerializeFloat(const struct Matrix3x3FloatData& dataIn) -{ - for (int i=0;i<3;i++) - m_el[i].deSerializeFloat(dataIn.m_el[i]); -} - -TF2SIMD_FORCE_INLINE void Matrix3x3::deSerializeDouble(const struct Matrix3x3DoubleData& dataIn) -{ - for (int i=0;i<3;i++) - m_el[i].deSerializeDouble(dataIn.m_el[i]); -} - -} -#endif //TF2_MATRIX3x3_H +#endif // TF2__LINEARMATH__MATRIX3X3_H_ diff --git a/tf2/include/tf2/LinearMath/Matrix3x3.hpp b/tf2/include/tf2/LinearMath/Matrix3x3.hpp new file mode 100644 index 000000000..ae2eb0a4a --- /dev/null +++ b/tf2/include/tf2/LinearMath/Matrix3x3.hpp @@ -0,0 +1,703 @@ +/* +Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +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. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + +#ifndef TF2__LINEARMATH__MATRIX3X3_HPP_ +#define TF2__LINEARMATH__MATRIX3X3_HPP_ + +#include "Vector3.hpp" +#include "Quaternion.hpp" + +#include "tf2/visibility_control.h" + +namespace tf2 +{ + + +#define Matrix3x3Data Matrix3x3DoubleData + + +/**@brief The Matrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with Quaternion, Transform and Vector3. +* Make sure to only include a pure orthogonal matrix without scaling. */ +class Matrix3x3 { + + ///Data storage for the matrix, each vector is a row of the matrix + Vector3 m_el[3]; + +public: + /** @brief No initializaion constructor */ + TF2_PUBLIC + Matrix3x3 () {} + + // explicit Matrix3x3(const tf2Scalar *m) { setFromOpenGLSubMatrix(m); } + + /**@brief Constructor from Quaternion */ + TF2_PUBLIC + explicit Matrix3x3(const Quaternion& q) { setRotation(q); } + /* + template + Matrix3x3(const tf2Scalar& yaw, const tf2Scalar& pitch, const tf2Scalar& roll) + { + setEulerYPR(yaw, pitch, roll); + } + */ + /** @brief Constructor with row major formatting */ + TF2_PUBLIC + 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) + { + setValue(xx, xy, xz, + yx, yy, yz, + zx, zy, zz); + } + /** @brief Copy constructor */ + TF2SIMD_FORCE_INLINE Matrix3x3 (const Matrix3x3& other) + { + m_el[0] = other.m_el[0]; + m_el[1] = other.m_el[1]; + m_el[2] = other.m_el[2]; + } + + + /** @brief Assignment Operator */ + TF2SIMD_FORCE_INLINE Matrix3x3& operator=(const Matrix3x3& other) + { + m_el[0] = other.m_el[0]; + m_el[1] = other.m_el[1]; + m_el[2] = other.m_el[2]; + return *this; + } + + + /** @brief Get a column of the matrix as a vector + * @param i Column number 0 indexed */ + TF2SIMD_FORCE_INLINE Vector3 getColumn(int i) const + { + return Vector3(m_el[0][i],m_el[1][i],m_el[2][i]); + } + + + /** @brief Get a row of the matrix as a vector + * @param i Row number 0 indexed */ + TF2SIMD_FORCE_INLINE const Vector3& getRow(int i) const + { + tf2FullAssert(0 <= i && i < 3); + return m_el[i]; + } + + /** @brief Get a mutable reference to a row of the matrix as a vector + * @param i Row number 0 indexed */ + TF2SIMD_FORCE_INLINE Vector3& operator[](int i) + { + tf2FullAssert(0 <= i && i < 3); + return m_el[i]; + } + + /** @brief Get a const reference to a row of the matrix as a vector + * @param i Row number 0 indexed */ + TF2SIMD_FORCE_INLINE const Vector3& operator[](int i) const + { + tf2FullAssert(0 <= i && i < 3); + return m_el[i]; + } + + /** @brief Multiply by the target matrix on the right + * @param m Rotation matrix to be applied + * Equivilant to this = this * m */ + TF2_PUBLIC + Matrix3x3& operator*=(const Matrix3x3& m); + + /** @brief Set from a carray of tf2Scalars + * @param m A pointer to the beginning of an array of 9 tf2Scalars */ + TF2_PUBLIC + void setFromOpenGLSubMatrix(const tf2Scalar *m) + { + m_el[0].setValue(m[0],m[4],m[8]); + m_el[1].setValue(m[1],m[5],m[9]); + m_el[2].setValue(m[2],m[6],m[10]); + + } + /** @brief Set the values of the matrix explicitly (row major) + * @param xx Top left + * @param xy Top Middle + * @param xz Top Right + * @param yx Middle Left + * @param yy Middle Middle + * @param yz Middle Right + * @param zx Bottom Left + * @param zy Bottom Middle + * @param zz Bottom Right*/ + TF2_PUBLIC + 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) + { + m_el[0].setValue(xx,xy,xz); + m_el[1].setValue(yx,yy,yz); + m_el[2].setValue(zx,zy,zz); + } + + /** @brief Set the matrix from a quaternion + * @param q The Quaternion to match */ + TF2_PUBLIC + void setRotation(const Quaternion& q) + { + tf2Scalar d = q.length2(); + tf2FullAssert(d != tf2Scalar(0.0)); + tf2Scalar s = tf2Scalar(2.0) / d; + tf2Scalar xs = q.x() * s, ys = q.y() * s, zs = q.z() * s; + tf2Scalar wx = q.w() * xs, wy = q.w() * ys, wz = q.w() * zs; + tf2Scalar xx = q.x() * xs, xy = q.x() * ys, xz = q.x() * zs; + tf2Scalar yy = q.y() * ys, yz = q.y() * zs, zz = q.z() * zs; + setValue(tf2Scalar(1.0) - (yy + zz), xy - wz, xz + wy, + xy + wz, tf2Scalar(1.0) - (xx + zz), yz - wx, + xz - wy, yz + wx, tf2Scalar(1.0) - (xx + yy)); + } + + /** @brief Set the matrix from euler angles YPR around ZYX axes + * @param eulerZ Yaw aboud Z axis + * @param eulerY Pitch around Y axis + * @param eulerX Roll about X axis + * + * These angles are used to produce a rotation matrix. The euler + * angles are applied in ZYX order. I.e a vector is first rotated + * about X then Y and then Z + **/ + TF2_PUBLIC + void setEulerYPR(tf2Scalar eulerZ, tf2Scalar eulerY,tf2Scalar eulerX) { + tf2Scalar ci ( tf2Cos(eulerX)); + tf2Scalar cj ( tf2Cos(eulerY)); + tf2Scalar ch ( tf2Cos(eulerZ)); + tf2Scalar si ( tf2Sin(eulerX)); + tf2Scalar sj ( tf2Sin(eulerY)); + tf2Scalar sh ( tf2Sin(eulerZ)); + tf2Scalar cc = ci * ch; + tf2Scalar cs = ci * sh; + tf2Scalar sc = si * ch; + tf2Scalar ss = si * sh; + + setValue(cj * ch, sj * sc - cs, sj * cc + ss, + cj * sh, sj * ss + cc, sj * cs - sc, + -sj, cj * si, cj * ci); + } + + /** @brief Set the matrix using RPY about XYZ fixed axes + * @param roll Roll about X axis + * @param pitch Pitch around Y axis + * @param yaw Yaw aboud Z axis + * + **/ + TF2_PUBLIC + void setRPY(tf2Scalar roll, tf2Scalar pitch,tf2Scalar yaw) { + setEulerYPR(yaw, pitch, roll); + } + + /**@brief Set the matrix to the identity */ + TF2_PUBLIC + void setIdentity() + { + setValue(tf2Scalar(1.0), tf2Scalar(0.0), tf2Scalar(0.0), + tf2Scalar(0.0), tf2Scalar(1.0), tf2Scalar(0.0), + tf2Scalar(0.0), tf2Scalar(0.0), tf2Scalar(1.0)); + } + + TF2_PUBLIC + static const Matrix3x3& getIdentity() + { + static const Matrix3x3 identityMatrix(tf2Scalar(1.0), tf2Scalar(0.0), tf2Scalar(0.0), + tf2Scalar(0.0), tf2Scalar(1.0), tf2Scalar(0.0), + tf2Scalar(0.0), tf2Scalar(0.0), tf2Scalar(1.0)); + return identityMatrix; + } + + /**@brief Fill the values of the matrix into a 9 element array + * @param m The array to be filled */ + TF2_PUBLIC + void getOpenGLSubMatrix(tf2Scalar *m) const + { + m[0] = tf2Scalar(m_el[0].x()); + m[1] = tf2Scalar(m_el[1].x()); + m[2] = tf2Scalar(m_el[2].x()); + m[3] = tf2Scalar(0.0); + m[4] = tf2Scalar(m_el[0].y()); + m[5] = tf2Scalar(m_el[1].y()); + m[6] = tf2Scalar(m_el[2].y()); + m[7] = tf2Scalar(0.0); + m[8] = tf2Scalar(m_el[0].z()); + m[9] = tf2Scalar(m_el[1].z()); + m[10] = tf2Scalar(m_el[2].z()); + m[11] = tf2Scalar(0.0); + } + + /**@brief Get the matrix represented as a quaternion + * @param q The quaternion which will be set */ + TF2_PUBLIC + void getRotation(Quaternion& q) const + { + tf2Scalar trace = m_el[0].x() + m_el[1].y() + m_el[2].z(); + tf2Scalar temp[4]; + + if (trace > tf2Scalar(0.0)) + { + tf2Scalar s = tf2Sqrt(trace + tf2Scalar(1.0)); + temp[3]=(s * tf2Scalar(0.5)); + s = tf2Scalar(0.5) / s; + + temp[0]=((m_el[2].y() - m_el[1].z()) * s); + temp[1]=((m_el[0].z() - m_el[2].x()) * s); + temp[2]=((m_el[1].x() - m_el[0].y()) * s); + } + else + { + int i = m_el[0].x() < m_el[1].y() ? + (m_el[1].y() < m_el[2].z() ? 2 : 1) : + (m_el[0].x() < m_el[2].z() ? 2 : 0); + int j = (i + 1) % 3; + int k = (i + 2) % 3; + + tf2Scalar s = tf2Sqrt(m_el[i][i] - m_el[j][j] - m_el[k][k] + tf2Scalar(1.0)); + temp[i] = s * tf2Scalar(0.5); + s = tf2Scalar(0.5) / s; + + temp[3] = (m_el[k][j] - m_el[j][k]) * s; + temp[j] = (m_el[j][i] + m_el[i][j]) * s; + temp[k] = (m_el[k][i] + m_el[i][k]) * s; + } + q.setValue(temp[0],temp[1],temp[2],temp[3]); + } + + /**@brief Get the matrix represented as euler angles around YXZ, roundtrip with setEulerYPR + * @param yaw Yaw around Z axis + * @param pitch Pitch around Y axis + * @param roll around X axis */ + TF2_PUBLIC + void getEulerYPR(tf2Scalar& yaw, tf2Scalar& pitch, tf2Scalar& roll, unsigned int solution_number = 1) const + { + struct Euler + { + tf2Scalar yaw; + tf2Scalar pitch; + tf2Scalar roll; + }; + + Euler euler_out; + Euler euler_out2; //second solution + //get the pointer to the raw data + + // Check that pitch is not at a singularity + // Check that pitch is not at a singularity + if (tf2Fabs(m_el[2].x()) >= 1) + { + euler_out.yaw = 0; + euler_out2.yaw = 0; + + // From difference of angles formula + tf2Scalar delta = tf2Atan2(m_el[2].y(),m_el[2].z()); + if (m_el[2].x() < 0) //gimbal locked down + { + euler_out.pitch = TF2SIMD_PI / tf2Scalar(2.0); + euler_out2.pitch = TF2SIMD_PI / tf2Scalar(2.0); + euler_out.roll = delta; + euler_out2.roll = delta; + } + else // gimbal locked up + { + euler_out.pitch = -TF2SIMD_PI / tf2Scalar(2.0); + euler_out2.pitch = -TF2SIMD_PI / tf2Scalar(2.0); + euler_out.roll = delta; + euler_out2.roll = delta; + } + } + else + { + euler_out.pitch = - tf2Asin(m_el[2].x()); + euler_out2.pitch = TF2SIMD_PI - euler_out.pitch; + + euler_out.roll = tf2Atan2(m_el[2].y()/tf2Cos(euler_out.pitch), + m_el[2].z()/tf2Cos(euler_out.pitch)); + euler_out2.roll = tf2Atan2(m_el[2].y()/tf2Cos(euler_out2.pitch), + m_el[2].z()/tf2Cos(euler_out2.pitch)); + + euler_out.yaw = tf2Atan2(m_el[1].x()/tf2Cos(euler_out.pitch), + m_el[0].x()/tf2Cos(euler_out.pitch)); + euler_out2.yaw = tf2Atan2(m_el[1].x()/tf2Cos(euler_out2.pitch), + m_el[0].x()/tf2Cos(euler_out2.pitch)); + } + + if (solution_number == 1) + { + yaw = euler_out.yaw; + pitch = euler_out.pitch; + roll = euler_out.roll; + } + else + { + yaw = euler_out2.yaw; + pitch = euler_out2.pitch; + roll = euler_out2.roll; + } + } + + /**@brief Get the matrix represented as roll pitch and yaw about fixed axes XYZ + * @param roll around X axis + * @param pitch Pitch around Y axis + * @param yaw Yaw around Z axis + * @param solution_number Which solution of two possible solutions ( 1 or 2) are possible values*/ + TF2_PUBLIC + void getRPY(tf2Scalar& roll, tf2Scalar& pitch, tf2Scalar& yaw, unsigned int solution_number = 1) const + { + getEulerYPR(yaw, pitch, roll, solution_number); + } + + /**@brief Create a scaled copy of the matrix + * @param s Scaling vector The elements of the vector will scale each column */ + + TF2_PUBLIC + Matrix3x3 scaled(const Vector3& s) const + { + return Matrix3x3(m_el[0].x() * s.x(), m_el[0].y() * s.y(), m_el[0].z() * s.z(), + m_el[1].x() * s.x(), m_el[1].y() * s.y(), m_el[1].z() * s.z(), + m_el[2].x() * s.x(), m_el[2].y() * s.y(), m_el[2].z() * s.z()); + } + + /**@brief Return the determinant of the matrix */ + TF2_PUBLIC + tf2Scalar determinant() const; + /**@brief Return the adjoint of the matrix */ + TF2_PUBLIC + Matrix3x3 adjoint() const; + /**@brief Return the matrix with all values non negative */ + TF2_PUBLIC + Matrix3x3 absolute() const; + /**@brief Return the transpose of the matrix */ + TF2_PUBLIC + Matrix3x3 transpose() const; + /**@brief Return the inverse of the matrix */ + TF2_PUBLIC + Matrix3x3 inverse() const; + + TF2_PUBLIC + Matrix3x3 transposeTimes(const Matrix3x3& m) const; + TF2_PUBLIC + Matrix3x3 timesTranspose(const Matrix3x3& m) const; + + TF2SIMD_FORCE_INLINE tf2Scalar tdotx(const Vector3& v) const + { + return m_el[0].x() * v.x() + m_el[1].x() * v.y() + m_el[2].x() * v.z(); + } + TF2SIMD_FORCE_INLINE tf2Scalar tdoty(const Vector3& v) const + { + return m_el[0].y() * v.x() + m_el[1].y() * v.y() + m_el[2].y() * v.z(); + } + TF2SIMD_FORCE_INLINE tf2Scalar tdotz(const Vector3& v) const + { + return m_el[0].z() * v.x() + m_el[1].z() * v.y() + m_el[2].z() * v.z(); + } + + + /**@brief diagonalizes this matrix by the Jacobi method. + * @param rot stores the rotation from the coordinate system in which the matrix is diagonal to the original + * coordinate system, i.e., old_this = rot * new_this * rot^T. + * @param threshold See iteration + * @param iteration The iteration stops when all off-diagonal elements are less than the threshold multiplied + * by the sum of the absolute values of the diagonal, or when maxSteps have been executed. + * + * Note that this matrix is assumed to be symmetric. + */ + TF2_PUBLIC + void diagonalize(Matrix3x3& rot, tf2Scalar threshold, int maxSteps) + { + rot.setIdentity(); + for (int step = maxSteps; step > 0; step--) + { + // find off-diagonal element [p][q] with largest magnitude + int p = 0; + int q = 1; + int r = 2; + tf2Scalar max = tf2Fabs(m_el[0][1]); + tf2Scalar v = tf2Fabs(m_el[0][2]); + if (v > max) + { + q = 2; + r = 1; + max = v; + } + v = tf2Fabs(m_el[1][2]); + if (v > max) + { + p = 1; + q = 2; + r = 0; + max = v; + } + + tf2Scalar t = threshold * (tf2Fabs(m_el[0][0]) + tf2Fabs(m_el[1][1]) + tf2Fabs(m_el[2][2])); + if (max <= t) + { + if (max <= TF2SIMD_EPSILON * t) + { + return; + } + step = 1; + } + + // compute Jacobi rotation J which leads to a zero for element [p][q] + tf2Scalar mpq = m_el[p][q]; + tf2Scalar theta = (m_el[q][q] - m_el[p][p]) / (2 * mpq); + tf2Scalar theta2 = theta * theta; + tf2Scalar cos; + tf2Scalar sin; + if (theta2 * theta2 < tf2Scalar(10 / TF2SIMD_EPSILON)) + { + t = (theta >= 0) ? 1 / (theta + tf2Sqrt(1 + theta2)) + : 1 / (theta - tf2Sqrt(1 + theta2)); + cos = 1 / tf2Sqrt(1 + t * t); + sin = cos * t; + } + else + { + // approximation for large theta-value, i.e., a nearly diagonal matrix + t = 1 / (theta * (2 + tf2Scalar(0.5) / theta2)); + cos = 1 - tf2Scalar(0.5) * t * t; + sin = cos * t; + } + + // apply rotation to matrix (this = J^T * this * J) + m_el[p][q] = m_el[q][p] = 0; + m_el[p][p] -= t * mpq; + m_el[q][q] += t * mpq; + tf2Scalar mrp = m_el[r][p]; + tf2Scalar mrq = m_el[r][q]; + m_el[r][p] = m_el[p][r] = cos * mrp - sin * mrq; + m_el[r][q] = m_el[q][r] = cos * mrq + sin * mrp; + + // apply rotation to rot (rot = rot * J) + for (int i = 0; i < 3; i++) + { + Vector3& row = rot[i]; + mrp = row[p]; + mrq = row[q]; + row[p] = cos * mrp - sin * mrq; + row[q] = cos * mrq + sin * mrp; + } + } + } + + + + + /**@brief Calculate the matrix cofactor + * @param r1 The first row to use for calculating the cofactor + * @param c1 The first column to use for calculating the cofactor + * @param r1 The second row to use for calculating the cofactor + * @param c1 The second column to use for calculating the cofactor + * See http://en.wikipedia.org/wiki/Cofactor_(linear_algebra) for more details + */ + TF2_PUBLIC + tf2Scalar cofac(int r1, int c1, int r2, int c2) const + { + return m_el[r1][c1] * m_el[r2][c2] - m_el[r1][c2] * m_el[r2][c1]; + } + + TF2_PUBLIC + void serialize(struct Matrix3x3Data& dataOut) const; + + TF2_PUBLIC + void serializeFloat(struct Matrix3x3FloatData& dataOut) const; + + TF2_PUBLIC + void deSerialize(const struct Matrix3x3Data& dataIn); + + TF2_PUBLIC + void deSerializeFloat(const struct Matrix3x3FloatData& dataIn); + + TF2_PUBLIC + void deSerializeDouble(const struct Matrix3x3DoubleData& dataIn); + +}; + + +TF2SIMD_FORCE_INLINE Matrix3x3& +Matrix3x3::operator*=(const Matrix3x3& m) +{ + setValue(m.tdotx(m_el[0]), m.tdoty(m_el[0]), m.tdotz(m_el[0]), + m.tdotx(m_el[1]), m.tdoty(m_el[1]), m.tdotz(m_el[1]), + m.tdotx(m_el[2]), m.tdoty(m_el[2]), m.tdotz(m_el[2])); + return *this; +} + +TF2SIMD_FORCE_INLINE tf2Scalar +Matrix3x3::determinant() const +{ + return tf2Triple((*this)[0], (*this)[1], (*this)[2]); +} + + +TF2SIMD_FORCE_INLINE Matrix3x3 +Matrix3x3::absolute() const +{ + return Matrix3x3( + tf2Fabs(m_el[0].x()), tf2Fabs(m_el[0].y()), tf2Fabs(m_el[0].z()), + tf2Fabs(m_el[1].x()), tf2Fabs(m_el[1].y()), tf2Fabs(m_el[1].z()), + tf2Fabs(m_el[2].x()), tf2Fabs(m_el[2].y()), tf2Fabs(m_el[2].z())); +} + +TF2SIMD_FORCE_INLINE Matrix3x3 +Matrix3x3::transpose() const +{ + return Matrix3x3(m_el[0].x(), m_el[1].x(), m_el[2].x(), + m_el[0].y(), m_el[1].y(), m_el[2].y(), + m_el[0].z(), m_el[1].z(), m_el[2].z()); +} + +TF2SIMD_FORCE_INLINE Matrix3x3 +Matrix3x3::adjoint() const +{ + return Matrix3x3(cofac(1, 1, 2, 2), cofac(0, 2, 2, 1), cofac(0, 1, 1, 2), + cofac(1, 2, 2, 0), cofac(0, 0, 2, 2), cofac(0, 2, 1, 0), + cofac(1, 0, 2, 1), cofac(0, 1, 2, 0), cofac(0, 0, 1, 1)); +} + +TF2SIMD_FORCE_INLINE Matrix3x3 +Matrix3x3::inverse() const +{ + Vector3 co(cofac(1, 1, 2, 2), cofac(1, 2, 2, 0), cofac(1, 0, 2, 1)); + tf2Scalar det = (*this)[0].dot(co); + tf2FullAssert(det != tf2Scalar(0.0)); + tf2Scalar s = tf2Scalar(1.0) / det; + return Matrix3x3(co.x() * s, cofac(0, 2, 2, 1) * s, cofac(0, 1, 1, 2) * s, + co.y() * s, cofac(0, 0, 2, 2) * s, cofac(0, 2, 1, 0) * s, + co.z() * s, cofac(0, 1, 2, 0) * s, cofac(0, 0, 1, 1) * s); +} + +TF2SIMD_FORCE_INLINE Matrix3x3 +Matrix3x3::transposeTimes(const Matrix3x3& m) const +{ + return Matrix3x3( + m_el[0].x() * m[0].x() + m_el[1].x() * m[1].x() + m_el[2].x() * m[2].x(), + m_el[0].x() * m[0].y() + m_el[1].x() * m[1].y() + m_el[2].x() * m[2].y(), + m_el[0].x() * m[0].z() + m_el[1].x() * m[1].z() + m_el[2].x() * m[2].z(), + m_el[0].y() * m[0].x() + m_el[1].y() * m[1].x() + m_el[2].y() * m[2].x(), + m_el[0].y() * m[0].y() + m_el[1].y() * m[1].y() + m_el[2].y() * m[2].y(), + m_el[0].y() * m[0].z() + m_el[1].y() * m[1].z() + m_el[2].y() * m[2].z(), + m_el[0].z() * m[0].x() + m_el[1].z() * m[1].x() + m_el[2].z() * m[2].x(), + m_el[0].z() * m[0].y() + m_el[1].z() * m[1].y() + m_el[2].z() * m[2].y(), + m_el[0].z() * m[0].z() + m_el[1].z() * m[1].z() + m_el[2].z() * m[2].z()); +} + +TF2SIMD_FORCE_INLINE Matrix3x3 +Matrix3x3::timesTranspose(const Matrix3x3& m) const +{ + return Matrix3x3( + m_el[0].dot(m[0]), m_el[0].dot(m[1]), m_el[0].dot(m[2]), + m_el[1].dot(m[0]), m_el[1].dot(m[1]), m_el[1].dot(m[2]), + m_el[2].dot(m[0]), m_el[2].dot(m[1]), m_el[2].dot(m[2])); + +} + +TF2SIMD_FORCE_INLINE Vector3 +operator*(const Matrix3x3& m, const Vector3& v) +{ + return Vector3(m[0].dot(v), m[1].dot(v), m[2].dot(v)); +} + + +TF2SIMD_FORCE_INLINE Vector3 +operator*(const Vector3& v, const Matrix3x3& m) +{ + return Vector3(m.tdotx(v), m.tdoty(v), m.tdotz(v)); +} + +TF2SIMD_FORCE_INLINE Matrix3x3 +operator*(const Matrix3x3& m1, const Matrix3x3& m2) +{ + return Matrix3x3( + m2.tdotx( m1[0]), m2.tdoty( m1[0]), m2.tdotz( m1[0]), + m2.tdotx( m1[1]), m2.tdoty( m1[1]), m2.tdotz( m1[1]), + m2.tdotx( m1[2]), m2.tdoty( m1[2]), m2.tdotz( m1[2])); +} + +/* +TF2SIMD_FORCE_INLINE Matrix3x3 tf2MultTransposeLeft(const Matrix3x3& m1, const Matrix3x3& m2) { +return Matrix3x3( +m1[0][0] * m2[0][0] + m1[1][0] * m2[1][0] + m1[2][0] * m2[2][0], +m1[0][0] * m2[0][1] + m1[1][0] * m2[1][1] + m1[2][0] * m2[2][1], +m1[0][0] * m2[0][2] + m1[1][0] * m2[1][2] + m1[2][0] * m2[2][2], +m1[0][1] * m2[0][0] + m1[1][1] * m2[1][0] + m1[2][1] * m2[2][0], +m1[0][1] * m2[0][1] + m1[1][1] * m2[1][1] + m1[2][1] * m2[2][1], +m1[0][1] * m2[0][2] + m1[1][1] * m2[1][2] + m1[2][1] * m2[2][2], +m1[0][2] * m2[0][0] + m1[1][2] * m2[1][0] + m1[2][2] * m2[2][0], +m1[0][2] * m2[0][1] + m1[1][2] * m2[1][1] + m1[2][2] * m2[2][1], +m1[0][2] * m2[0][2] + m1[1][2] * m2[1][2] + m1[2][2] * m2[2][2]); +} +*/ + +/**@brief Equality operator between two matrices +* It will test all elements are equal. */ +TF2SIMD_FORCE_INLINE bool operator==(const Matrix3x3& m1, const Matrix3x3& m2) +{ + return ( m1[0][0] == m2[0][0] && m1[1][0] == m2[1][0] && m1[2][0] == m2[2][0] && + m1[0][1] == m2[0][1] && m1[1][1] == m2[1][1] && m1[2][1] == m2[2][1] && + m1[0][2] == m2[0][2] && m1[1][2] == m2[1][2] && m1[2][2] == m2[2][2] ); +} + +///for serialization +struct Matrix3x3FloatData +{ + Vector3FloatData m_el[3]; +}; + +///for serialization +struct Matrix3x3DoubleData +{ + Vector3DoubleData m_el[3]; +}; + + + + +TF2SIMD_FORCE_INLINE void Matrix3x3::serialize(struct Matrix3x3Data& dataOut) const +{ + for (int i=0;i<3;i++) + m_el[i].serialize(dataOut.m_el[i]); +} + +TF2SIMD_FORCE_INLINE void Matrix3x3::serializeFloat(struct Matrix3x3FloatData& dataOut) const +{ + for (int i=0;i<3;i++) + m_el[i].serializeFloat(dataOut.m_el[i]); +} + + +TF2SIMD_FORCE_INLINE void Matrix3x3::deSerialize(const struct Matrix3x3Data& dataIn) +{ + for (int i=0;i<3;i++) + m_el[i].deSerialize(dataIn.m_el[i]); +} + +TF2SIMD_FORCE_INLINE void Matrix3x3::deSerializeFloat(const struct Matrix3x3FloatData& dataIn) +{ + for (int i=0;i<3;i++) + m_el[i].deSerializeFloat(dataIn.m_el[i]); +} + +TF2SIMD_FORCE_INLINE void Matrix3x3::deSerializeDouble(const struct Matrix3x3DoubleData& dataIn) +{ + for (int i=0;i<3;i++) + m_el[i].deSerializeDouble(dataIn.m_el[i]); +} + +} +#endif // TF2__LINEARMATH__MATRIX3X3_HPP_ diff --git a/tf2/include/tf2/LinearMath/MinMax.h b/tf2/include/tf2/LinearMath/MinMax.h index 260f1bc64..0f6df3ce8 100644 --- a/tf2/include/tf2/LinearMath/MinMax.h +++ b/tf2/include/tf2/LinearMath/MinMax.h @@ -13,59 +13,9 @@ subject to the following restrictions: */ +#ifndef TF2__LINEARMATH__MINMAX_H_ +#define TF2__LINEARMATH__MINMAX_H_ -#ifndef GEN_MINMAX_H -#define GEN_MINMAX_H +#include -#include "Scalar.h" - -template -TF2SIMD_FORCE_INLINE const T& tf2Min(const T& a, const T& b) -{ - return a < b ? a : b ; -} - -template -TF2SIMD_FORCE_INLINE const T& tf2Max(const T& a, const T& b) -{ - return a > b ? a : b; -} - -template -TF2SIMD_FORCE_INLINE const T& GEN_clamped(const T& a, const T& lb, const T& ub) -{ - return a < lb ? lb : (ub < a ? ub : a); -} - -template -TF2SIMD_FORCE_INLINE void tf2SetMin(T& a, const T& b) -{ - if (b < a) - { - a = b; - } -} - -template -TF2SIMD_FORCE_INLINE void tf2SetMax(T& a, const T& b) -{ - if (a < b) - { - a = b; - } -} - -template -TF2SIMD_FORCE_INLINE void GEN_clamp(T& a, const T& lb, const T& ub) -{ - if (a < lb) - { - a = lb; - } - else if (ub < a) - { - a = ub; - } -} - -#endif +#endif // TF2__LINEARMATH__MINMAX_H_ diff --git a/tf2/include/tf2/LinearMath/MinMax.hpp b/tf2/include/tf2/LinearMath/MinMax.hpp new file mode 100644 index 000000000..6c3746a8a --- /dev/null +++ b/tf2/include/tf2/LinearMath/MinMax.hpp @@ -0,0 +1,71 @@ +/* +Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +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. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + + +#ifndef TF2__LINEARMATH__MINMAX_HPP_ +#define TF2__LINEARMATH__MINMAX_HPP_ + +#include "Scalar.hpp" + +template +TF2SIMD_FORCE_INLINE const T& tf2Min(const T& a, const T& b) +{ + return a < b ? a : b ; +} + +template +TF2SIMD_FORCE_INLINE const T& tf2Max(const T& a, const T& b) +{ + return a > b ? a : b; +} + +template +TF2SIMD_FORCE_INLINE const T& GEN_clamped(const T& a, const T& lb, const T& ub) +{ + return a < lb ? lb : (ub < a ? ub : a); +} + +template +TF2SIMD_FORCE_INLINE void tf2SetMin(T& a, const T& b) +{ + if (b < a) + { + a = b; + } +} + +template +TF2SIMD_FORCE_INLINE void tf2SetMax(T& a, const T& b) +{ + if (a < b) + { + a = b; + } +} + +template +TF2SIMD_FORCE_INLINE void GEN_clamp(T& a, const T& lb, const T& ub) +{ + if (a < lb) + { + a = lb; + } + else if (ub < a) + { + a = ub; + } +} + +#endif // TF2__LINEARMATH__MINMAX_HPP_ diff --git a/tf2/include/tf2/LinearMath/QuadWord.h b/tf2/include/tf2/LinearMath/QuadWord.h index 2d14989c6..9327b575b 100644 --- a/tf2/include/tf2/LinearMath/QuadWord.h +++ b/tf2/include/tf2/LinearMath/QuadWord.h @@ -13,173 +13,9 @@ subject to the following restrictions: */ -#ifndef TF2SIMD_QUADWORD_H -#define TF2SIMD_QUADWORD_H +#ifndef TF2__LINEARMATH__QUADWORD_H_ +#define TF2__LINEARMATH__QUADWORD_H_ -#include "Scalar.h" -#include "MinMax.h" -#include "tf2/visibility_control.h" +#include - -#if defined (__CELLOS_LV2) && defined (__SPU__) -#include -#endif - -namespace tf2 -{ -/**@brief The QuadWord class is base class for Vector3 and Quaternion. - * Some issues under PS3 Linux with IBM 2.1 SDK, gcc compiler prevent from using aligned quadword. - */ -#ifndef USE_LIBSPE2 -ATTRIBUTE_ALIGNED16(class) QuadWord -#else -class QuadWord -#endif -{ -protected: - -#if defined (__SPU__) && defined (__CELLOS_LV2__) - union { - vec_float4 mVec128; - tf2Scalar m_floats[4]; - }; -public: - TF2_PUBLIC - vec_float4 get128() const - { - return mVec128; - } -protected: -#else //__CELLOS_LV2__ __SPU__ - tf2Scalar m_floats[4]; -#endif //__CELLOS_LV2__ __SPU__ - - public: - - - /**@brief Return the x value */ - TF2SIMD_FORCE_INLINE const tf2Scalar& getX() const { return m_floats[0]; } - /**@brief Return the y value */ - TF2SIMD_FORCE_INLINE const tf2Scalar& getY() const { return m_floats[1]; } - /**@brief Return the z value */ - TF2SIMD_FORCE_INLINE const tf2Scalar& getZ() const { return m_floats[2]; } - /**@brief Set the x value */ - TF2SIMD_FORCE_INLINE void setX(tf2Scalar x) { m_floats[0] = x;}; - /**@brief Set the y value */ - TF2SIMD_FORCE_INLINE void setY(tf2Scalar y) { m_floats[1] = y;}; - /**@brief Set the z value */ - TF2SIMD_FORCE_INLINE void setZ(tf2Scalar z) { m_floats[2] = z;}; - /**@brief Set the w value */ - TF2SIMD_FORCE_INLINE void setW(tf2Scalar w) { m_floats[3] = w;}; - /**@brief Return the x value */ - TF2SIMD_FORCE_INLINE const tf2Scalar& x() const { return m_floats[0]; } - /**@brief Return the y value */ - TF2SIMD_FORCE_INLINE const tf2Scalar& y() const { return m_floats[1]; } - /**@brief Return the z value */ - TF2SIMD_FORCE_INLINE const tf2Scalar& z() const { return m_floats[2]; } - /**@brief Return the w value */ - TF2SIMD_FORCE_INLINE const tf2Scalar& w() const { return m_floats[3]; } - - //TF2SIMD_FORCE_INLINE tf2Scalar& operator[](int i) { return (&m_floats[0])[i]; } - //TF2SIMD_FORCE_INLINE const tf2Scalar& operator[](int i) const { return (&m_floats[0])[i]; } - ///operator tf2Scalar*() replaces operator[], using implicit conversion. We added operator != and operator == to avoid pointer comparisons. - TF2SIMD_FORCE_INLINE operator tf2Scalar *() { return &m_floats[0]; } - TF2SIMD_FORCE_INLINE operator const tf2Scalar *() const { return &m_floats[0]; } - - TF2SIMD_FORCE_INLINE bool operator==(const QuadWord& other) const - { - return ((m_floats[3]==other.m_floats[3]) && (m_floats[2]==other.m_floats[2]) && (m_floats[1]==other.m_floats[1]) && (m_floats[0]==other.m_floats[0])); - } - - TF2SIMD_FORCE_INLINE bool operator!=(const QuadWord& other) const - { - return !(*this == other); - } - - /**@brief Set x,y,z and zero w - * @param x Value of x - * @param y Value of y - * @param z Value of z - */ - TF2SIMD_FORCE_INLINE void setValue(const tf2Scalar& x, const tf2Scalar& y, const tf2Scalar& z) - { - m_floats[0]=x; - m_floats[1]=y; - m_floats[2]=z; - m_floats[3] = 0.f; - } - -/* void getValue(tf2Scalar *m) const - { - m[0] = m_floats[0]; - m[1] = m_floats[1]; - m[2] = m_floats[2]; - } -*/ -/**@brief Set the values - * @param x Value of x - * @param y Value of y - * @param z Value of z - * @param w Value of w - */ - TF2SIMD_FORCE_INLINE void setValue(const tf2Scalar& x, const tf2Scalar& y, const tf2Scalar& z,const tf2Scalar& w) - { - m_floats[0]=x; - m_floats[1]=y; - m_floats[2]=z; - m_floats[3]=w; - } - /**@brief No initialization constructor */ - TF2SIMD_FORCE_INLINE QuadWord() - // :m_floats[0](tf2Scalar(0.)),m_floats[1](tf2Scalar(0.)),m_floats[2](tf2Scalar(0.)),m_floats[3](tf2Scalar(0.)) - { - } - - /**@brief Three argument constructor (zeros w) - * @param x Value of x - * @param y Value of y - * @param z Value of z - */ - TF2SIMD_FORCE_INLINE QuadWord(const tf2Scalar& x, const tf2Scalar& y, const tf2Scalar& z) - { - m_floats[0] = x, m_floats[1] = y, m_floats[2] = z, m_floats[3] = 0.0f; - } - -/**@brief Initializing constructor - * @param x Value of x - * @param y Value of y - * @param z Value of z - * @param w Value of w - */ - TF2SIMD_FORCE_INLINE QuadWord(const tf2Scalar& x, const tf2Scalar& y, const tf2Scalar& z,const tf2Scalar& w) - { - m_floats[0] = x, m_floats[1] = y, m_floats[2] = z, m_floats[3] = w; - } - - /**@brief Set each element to the max of the current values and the values of another QuadWord - * @param other The other QuadWord to compare with - */ - TF2SIMD_FORCE_INLINE void setMax(const QuadWord& other) - { - tf2SetMax(m_floats[0], other.m_floats[0]); - tf2SetMax(m_floats[1], other.m_floats[1]); - tf2SetMax(m_floats[2], other.m_floats[2]); - tf2SetMax(m_floats[3], other.m_floats[3]); - } - /**@brief Set each element to the min of the current values and the values of another QuadWord - * @param other The other QuadWord to compare with - */ - TF2SIMD_FORCE_INLINE void setMin(const QuadWord& other) - { - tf2SetMin(m_floats[0], other.m_floats[0]); - tf2SetMin(m_floats[1], other.m_floats[1]); - tf2SetMin(m_floats[2], other.m_floats[2]); - tf2SetMin(m_floats[3], other.m_floats[3]); - } - - - -}; - -} -#endif //TF2SIMD_QUADWORD_H +#endif // TF2__LINEARMATH__QUADWORD_H_ diff --git a/tf2/include/tf2/LinearMath/QuadWord.hpp b/tf2/include/tf2/LinearMath/QuadWord.hpp new file mode 100644 index 000000000..6806a3607 --- /dev/null +++ b/tf2/include/tf2/LinearMath/QuadWord.hpp @@ -0,0 +1,185 @@ +/* +Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +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. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + +#ifndef TF2__LINEARMATH__QUADWORD_HPP_ +#define TF2__LINEARMATH__QUADWORD_HPP_ + +#include "Scalar.hpp" +#include "MinMax.hpp" +#include "tf2/visibility_control.h" + + +#if defined (__CELLOS_LV2) && defined (__SPU__) +#include +#endif + +namespace tf2 +{ +/**@brief The QuadWord class is base class for Vector3 and Quaternion. + * Some issues under PS3 Linux with IBM 2.1 SDK, gcc compiler prevent from using aligned quadword. + */ +#ifndef USE_LIBSPE2 +ATTRIBUTE_ALIGNED16(class) QuadWord +#else +class QuadWord +#endif +{ +protected: + +#if defined (__SPU__) && defined (__CELLOS_LV2__) + union { + vec_float4 mVec128; + tf2Scalar m_floats[4]; + }; +public: + TF2_PUBLIC + vec_float4 get128() const + { + return mVec128; + } +protected: +#else //__CELLOS_LV2__ __SPU__ + tf2Scalar m_floats[4]; +#endif //__CELLOS_LV2__ __SPU__ + + public: + + + /**@brief Return the x value */ + TF2SIMD_FORCE_INLINE const tf2Scalar& getX() const { return m_floats[0]; } + /**@brief Return the y value */ + TF2SIMD_FORCE_INLINE const tf2Scalar& getY() const { return m_floats[1]; } + /**@brief Return the z value */ + TF2SIMD_FORCE_INLINE const tf2Scalar& getZ() const { return m_floats[2]; } + /**@brief Set the x value */ + TF2SIMD_FORCE_INLINE void setX(tf2Scalar x) { m_floats[0] = x;}; + /**@brief Set the y value */ + TF2SIMD_FORCE_INLINE void setY(tf2Scalar y) { m_floats[1] = y;}; + /**@brief Set the z value */ + TF2SIMD_FORCE_INLINE void setZ(tf2Scalar z) { m_floats[2] = z;}; + /**@brief Set the w value */ + TF2SIMD_FORCE_INLINE void setW(tf2Scalar w) { m_floats[3] = w;}; + /**@brief Return the x value */ + TF2SIMD_FORCE_INLINE const tf2Scalar& x() const { return m_floats[0]; } + /**@brief Return the y value */ + TF2SIMD_FORCE_INLINE const tf2Scalar& y() const { return m_floats[1]; } + /**@brief Return the z value */ + TF2SIMD_FORCE_INLINE const tf2Scalar& z() const { return m_floats[2]; } + /**@brief Return the w value */ + TF2SIMD_FORCE_INLINE const tf2Scalar& w() const { return m_floats[3]; } + + //TF2SIMD_FORCE_INLINE tf2Scalar& operator[](int i) { return (&m_floats[0])[i]; } + //TF2SIMD_FORCE_INLINE const tf2Scalar& operator[](int i) const { return (&m_floats[0])[i]; } + ///operator tf2Scalar*() replaces operator[], using implicit conversion. We added operator != and operator == to avoid pointer comparisons. + TF2SIMD_FORCE_INLINE operator tf2Scalar *() { return &m_floats[0]; } + TF2SIMD_FORCE_INLINE operator const tf2Scalar *() const { return &m_floats[0]; } + + TF2SIMD_FORCE_INLINE bool operator==(const QuadWord& other) const + { + return ((m_floats[3]==other.m_floats[3]) && (m_floats[2]==other.m_floats[2]) && (m_floats[1]==other.m_floats[1]) && (m_floats[0]==other.m_floats[0])); + } + + TF2SIMD_FORCE_INLINE bool operator!=(const QuadWord& other) const + { + return !(*this == other); + } + + /**@brief Set x,y,z and zero w + * @param x Value of x + * @param y Value of y + * @param z Value of z + */ + TF2SIMD_FORCE_INLINE void setValue(const tf2Scalar& x, const tf2Scalar& y, const tf2Scalar& z) + { + m_floats[0]=x; + m_floats[1]=y; + m_floats[2]=z; + m_floats[3] = 0.f; + } + +/* void getValue(tf2Scalar *m) const + { + m[0] = m_floats[0]; + m[1] = m_floats[1]; + m[2] = m_floats[2]; + } +*/ +/**@brief Set the values + * @param x Value of x + * @param y Value of y + * @param z Value of z + * @param w Value of w + */ + TF2SIMD_FORCE_INLINE void setValue(const tf2Scalar& x, const tf2Scalar& y, const tf2Scalar& z,const tf2Scalar& w) + { + m_floats[0]=x; + m_floats[1]=y; + m_floats[2]=z; + m_floats[3]=w; + } + /**@brief No initialization constructor */ + TF2SIMD_FORCE_INLINE QuadWord() + // :m_floats[0](tf2Scalar(0.)),m_floats[1](tf2Scalar(0.)),m_floats[2](tf2Scalar(0.)),m_floats[3](tf2Scalar(0.)) + { + } + + /**@brief Three argument constructor (zeros w) + * @param x Value of x + * @param y Value of y + * @param z Value of z + */ + TF2SIMD_FORCE_INLINE QuadWord(const tf2Scalar& x, const tf2Scalar& y, const tf2Scalar& z) + { + m_floats[0] = x, m_floats[1] = y, m_floats[2] = z, m_floats[3] = 0.0f; + } + +/**@brief Initializing constructor + * @param x Value of x + * @param y Value of y + * @param z Value of z + * @param w Value of w + */ + TF2SIMD_FORCE_INLINE QuadWord(const tf2Scalar& x, const tf2Scalar& y, const tf2Scalar& z,const tf2Scalar& w) + { + m_floats[0] = x, m_floats[1] = y, m_floats[2] = z, m_floats[3] = w; + } + + /**@brief Set each element to the max of the current values and the values of another QuadWord + * @param other The other QuadWord to compare with + */ + TF2SIMD_FORCE_INLINE void setMax(const QuadWord& other) + { + tf2SetMax(m_floats[0], other.m_floats[0]); + tf2SetMax(m_floats[1], other.m_floats[1]); + tf2SetMax(m_floats[2], other.m_floats[2]); + tf2SetMax(m_floats[3], other.m_floats[3]); + } + /**@brief Set each element to the min of the current values and the values of another QuadWord + * @param other The other QuadWord to compare with + */ + TF2SIMD_FORCE_INLINE void setMin(const QuadWord& other) + { + tf2SetMin(m_floats[0], other.m_floats[0]); + tf2SetMin(m_floats[1], other.m_floats[1]); + tf2SetMin(m_floats[2], other.m_floats[2]); + tf2SetMin(m_floats[3], other.m_floats[3]); + } + + + +}; + +} +#endif // TF2__LINEARMATH__QUADWORD_HPP_ diff --git a/tf2/include/tf2/LinearMath/Quaternion.h b/tf2/include/tf2/LinearMath/Quaternion.h index ef0caa86c..a58413f64 100644 --- a/tf2/include/tf2/LinearMath/Quaternion.h +++ b/tf2/include/tf2/LinearMath/Quaternion.h @@ -13,464 +13,9 @@ subject to the following restrictions: */ +#ifndef TF2__LINEARMATH__QUATERNION_H_ +#define TF2__LINEARMATH__QUATERNION_H_ -#ifndef TF2_QUATERNION_H_ -#define TF2_QUATERNION_H_ +#include - -#include "Vector3.h" -#include "QuadWord.h" -#include "tf2/visibility_control.h" - -namespace tf2 -{ - -/**@brief The Quaternion implements quaternion to perform linear algebra rotations in combination with Matrix3x3, Vector3 and Transform. */ -class Quaternion : public QuadWord { -public: - /**@brief No initialization constructor */ - TF2_PUBLIC - Quaternion() {} - - // template - // explicit Quaternion(const tf2Scalar *v) : Tuple4(v) {} - /**@brief Constructor from scalars */ - TF2_PUBLIC - Quaternion(const tf2Scalar& x, const tf2Scalar& y, const tf2Scalar& z, const tf2Scalar& w) - : QuadWord(x, y, z, w) - {} - /**@brief Axis angle Constructor - * @param axis The axis which the rotation is around - * @param angle The magnitude of the rotation around the angle (Radians) */ - TF2_PUBLIC - Quaternion(const Vector3& axis, const tf2Scalar& angle) - { - setRotation(axis, angle); - } - /**@brief Set the rotation using axis angle notation - * @param axis The axis around which to rotate - * @param angle The magnitude of the rotation in Radians */ - TF2_PUBLIC - void setRotation(const Vector3& axis, const tf2Scalar& angle) - { - tf2Scalar d = axis.length(); - tf2Assert(d != tf2Scalar(0.0)); - tf2Scalar s = tf2Sin(angle * tf2Scalar(0.5)) / d; - setValue(axis.x() * s, axis.y() * s, axis.z() * s, - tf2Cos(angle * tf2Scalar(0.5))); - } - /**@brief Set the quaternion using Euler angles - * @param yaw Angle around Y - * @param pitch Angle around X - * @param roll Angle around Z */ - TF2_PUBLIC - void setEuler(const tf2Scalar& yaw, const tf2Scalar& pitch, const tf2Scalar& roll) - { - tf2Scalar halfYaw = tf2Scalar(yaw) * tf2Scalar(0.5); - tf2Scalar halfPitch = tf2Scalar(pitch) * tf2Scalar(0.5); - tf2Scalar halfRoll = tf2Scalar(roll) * tf2Scalar(0.5); - tf2Scalar cosYaw = tf2Cos(halfYaw); - tf2Scalar sinYaw = tf2Sin(halfYaw); - tf2Scalar cosPitch = tf2Cos(halfPitch); - tf2Scalar sinPitch = tf2Sin(halfPitch); - tf2Scalar cosRoll = tf2Cos(halfRoll); - tf2Scalar sinRoll = tf2Sin(halfRoll); - setValue(cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw, - cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw, - sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw, - cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw); - } - /**@brief Set the quaternion using fixed axis RPY - * @param roll Angle around X - * @param pitch Angle around Y - * @param yaw Angle around Z*/ - TF2_PUBLIC - void setRPY(const tf2Scalar& roll, const tf2Scalar& pitch, const tf2Scalar& yaw) - { - tf2Scalar halfYaw = tf2Scalar(yaw) * tf2Scalar(0.5); - tf2Scalar halfPitch = tf2Scalar(pitch) * tf2Scalar(0.5); - tf2Scalar halfRoll = tf2Scalar(roll) * tf2Scalar(0.5); - tf2Scalar cosYaw = tf2Cos(halfYaw); - tf2Scalar sinYaw = tf2Sin(halfYaw); - tf2Scalar cosPitch = tf2Cos(halfPitch); - tf2Scalar sinPitch = tf2Sin(halfPitch); - tf2Scalar cosRoll = tf2Cos(halfRoll); - tf2Scalar sinRoll = tf2Sin(halfRoll); - setValue(sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw, //x - cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw, //y - cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw, //z - cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw); //formerly yzx - } - /**@brief Add two quaternions - * @param q The quaternion to add to this one */ - TF2SIMD_FORCE_INLINE Quaternion& operator+=(const Quaternion& q) - { - m_floats[0] += q.x(); m_floats[1] += q.y(); m_floats[2] += q.z(); m_floats[3] += q.m_floats[3]; - return *this; - } - - /**@brief Sutf2ract out a quaternion - * @param q The quaternion to sutf2ract from this one */ - TF2_PUBLIC - Quaternion& operator-=(const Quaternion& q) - { - m_floats[0] -= q.x(); m_floats[1] -= q.y(); m_floats[2] -= q.z(); m_floats[3] -= q.m_floats[3]; - return *this; - } - - /**@brief Scale this quaternion - * @param s The scalar to scale by */ - TF2_PUBLIC - Quaternion& operator*=(const tf2Scalar& s) - { - m_floats[0] *= s; m_floats[1] *= s; m_floats[2] *= s; m_floats[3] *= s; - return *this; - } - - /**@brief Multiply this quaternion by q on the right - * @param q The other quaternion - * Equivilant to this = this * q */ - TF2_PUBLIC - Quaternion& operator*=(const Quaternion& q) - { - setValue(m_floats[3] * q.x() + m_floats[0] * q.m_floats[3] + m_floats[1] * q.z() - m_floats[2] * q.y(), - m_floats[3] * q.y() + m_floats[1] * q.m_floats[3] + m_floats[2] * q.x() - m_floats[0] * q.z(), - m_floats[3] * q.z() + m_floats[2] * q.m_floats[3] + m_floats[0] * q.y() - m_floats[1] * q.x(), - m_floats[3] * q.m_floats[3] - m_floats[0] * q.x() - m_floats[1] * q.y() - m_floats[2] * q.z()); - return *this; - } - /**@brief Return the dot product between this quaternion and another - * @param q The other quaternion */ - TF2_PUBLIC - tf2Scalar dot(const Quaternion& q) const - { - return m_floats[0] * q.x() + m_floats[1] * q.y() + m_floats[2] * q.z() + m_floats[3] * q.m_floats[3]; - } - - /**@brief Return the length squared of the quaternion */ - TF2_PUBLIC - tf2Scalar length2() const - { - return dot(*this); - } - - /**@brief Return the length of the quaternion */ - TF2_PUBLIC - tf2Scalar length() const - { - return tf2Sqrt(length2()); - } - - /**@brief Normalize the quaternion - * Such that x^2 + y^2 + z^2 +w^2 = 1 */ - TF2_PUBLIC - Quaternion& normalize() - { - return *this /= length(); - } - - /**@brief Return a scaled version of this quaternion - * @param s The scale factor */ - TF2SIMD_FORCE_INLINE Quaternion - operator*(const tf2Scalar& s) const - { - return Quaternion(x() * s, y() * s, z() * s, m_floats[3] * s); - } - - - /**@brief Return an inversely scaled versionof this quaternion - * @param s The inverse scale factor */ - TF2_PUBLIC - Quaternion operator/(const tf2Scalar& s) const - { - tf2Assert(s != tf2Scalar(0.0)); - return *this * (tf2Scalar(1.0) / s); - } - - /**@brief Inversely scale this quaternion - * @param s The scale factor */ - TF2_PUBLIC - Quaternion& operator/=(const tf2Scalar& s) - { - tf2Assert(s != tf2Scalar(0.0)); - return *this *= tf2Scalar(1.0) / s; - } - - /**@brief Return a normalized version of this quaternion */ - TF2_PUBLIC - Quaternion normalized() const - { - return *this / length(); - } - /**@brief Return the ***half*** angle between this quaternion and the other - * @param q The other quaternion */ - TF2_PUBLIC - tf2Scalar angle(const Quaternion& q) const - { - tf2Scalar s = tf2Sqrt(length2() * q.length2()); - tf2Assert(s != tf2Scalar(0.0)); - return tf2Acos(dot(q) / s); - } - /**@brief Return the angle between this quaternion and the other along the shortest path - * @param q The other quaternion */ - TF2_PUBLIC - tf2Scalar angleShortestPath(const Quaternion& q) const - { - tf2Scalar s = tf2Sqrt(length2() * q.length2()); - tf2Assert(s != tf2Scalar(0.0)); - if (dot(q) < 0) // Take care of long angle case see http://en.wikipedia.org/wiki/Slerp - return tf2Acos(dot(-q) / s) * tf2Scalar(2.0); - else - return tf2Acos(dot(q) / s) * tf2Scalar(2.0); - } - /**@brief Return the angle [0, 2Pi] of rotation represented by this quaternion */ - TF2_PUBLIC - tf2Scalar getAngle() const - { - tf2Scalar s = tf2Scalar(2.) * tf2Acos(m_floats[3]); - return s; - } - - /**@brief Return the angle [0, Pi] of rotation represented by this quaternion along the shortest path */ - TF2_PUBLIC - tf2Scalar getAngleShortestPath() const - { - tf2Scalar s; - if (m_floats[3] >= 0) - s = tf2Scalar(2.) * tf2Acos(m_floats[3]); - else - s = tf2Scalar(2.) * tf2Acos(-m_floats[3]); - - return s; - } - - /**@brief Return the axis of the rotation represented by this quaternion */ - TF2_PUBLIC - Vector3 getAxis() const - { - tf2Scalar s_squared = tf2Scalar(1.) - tf2Pow(m_floats[3], tf2Scalar(2.)); - if (s_squared < tf2Scalar(10.) * TF2SIMD_EPSILON) //Check for divide by zero - return Vector3(1.0, 0.0, 0.0); // Arbitrary - tf2Scalar s = tf2Sqrt(s_squared); - return Vector3(m_floats[0] / s, m_floats[1] / s, m_floats[2] / s); - } - - /**@brief Return the inverse of this quaternion */ - TF2_PUBLIC - Quaternion inverse() const - { - return Quaternion(-m_floats[0], -m_floats[1], -m_floats[2], m_floats[3]); - } - - /**@brief Return the sum of this quaternion and the other - * @param q2 The other quaternion */ - TF2SIMD_FORCE_INLINE Quaternion - operator+(const Quaternion& q2) const - { - const Quaternion& q1 = *this; - return Quaternion(q1.x() + q2.x(), q1.y() + q2.y(), q1.z() + q2.z(), q1.m_floats[3] + q2.m_floats[3]); - } - - /**@brief Return the difference between this quaternion and the other - * @param q2 The other quaternion */ - TF2SIMD_FORCE_INLINE Quaternion - operator-(const Quaternion& q2) const - { - const Quaternion& q1 = *this; - return Quaternion(q1.x() - q2.x(), q1.y() - q2.y(), q1.z() - q2.z(), q1.m_floats[3] - q2.m_floats[3]); - } - - /**@brief Return the negative of this quaternion - * This simply negates each element */ - TF2SIMD_FORCE_INLINE Quaternion operator-() const - { - const Quaternion& q2 = *this; - return Quaternion( - q2.x(), - q2.y(), - q2.z(), - q2.m_floats[3]); - } - /**@todo document this and it's use */ - TF2SIMD_FORCE_INLINE Quaternion farthest( const Quaternion& qd) const - { - Quaternion diff,sum; - diff = *this - qd; - sum = *this + qd; - if( diff.dot(diff) > sum.dot(sum) ) - return qd; - return (-qd); - } - - /**@todo document this and it's use */ - TF2SIMD_FORCE_INLINE Quaternion nearest( const Quaternion& qd) const - { - Quaternion diff,sum; - diff = *this - qd; - sum = *this + qd; - if( diff.dot(diff) < sum.dot(sum) ) - return qd; - return (-qd); - } - - - /**@brief Return the quaternion which is the result of Spherical Linear Interpolation between this and the other quaternion - * @param q The other quaternion to interpolate with - * @param t The ratio between this and q to interpolate. If t = 0 the result is this, if t=1 the result is q. - * Slerp interpolates assuming constant velocity. */ - TF2_PUBLIC - Quaternion slerp(const Quaternion& q, const tf2Scalar& t) const - { - tf2Scalar theta = angleShortestPath(q) / tf2Scalar(2.0); - if (theta != tf2Scalar(0.0)) - { - tf2Scalar d = tf2Scalar(1.0) / tf2Sin(theta); - tf2Scalar s0 = tf2Sin((tf2Scalar(1.0) - t) * theta); - tf2Scalar s1 = tf2Sin(t * theta); - if (dot(q) < 0) // Take care of long angle case see http://en.wikipedia.org/wiki/Slerp - return Quaternion((m_floats[0] * s0 + -q.x() * s1) * d, - (m_floats[1] * s0 + -q.y() * s1) * d, - (m_floats[2] * s0 + -q.z() * s1) * d, - (m_floats[3] * s0 + -q.m_floats[3] * s1) * d); - else - return Quaternion((m_floats[0] * s0 + q.x() * s1) * d, - (m_floats[1] * s0 + q.y() * s1) * d, - (m_floats[2] * s0 + q.z() * s1) * d, - (m_floats[3] * s0 + q.m_floats[3] * s1) * d); - - } - else - { - return *this; - } - } - - TF2_PUBLIC - static const Quaternion& getIdentity() - { - static const Quaternion identityQuat(tf2Scalar(0.),tf2Scalar(0.),tf2Scalar(0.),tf2Scalar(1.)); - return identityQuat; - } - - TF2SIMD_FORCE_INLINE const tf2Scalar& getW() const { return m_floats[3]; } - - -}; - - -/**@brief Return the negative of a quaternion */ -TF2SIMD_FORCE_INLINE Quaternion -operator-(const Quaternion& q) -{ - return Quaternion(-q.x(), -q.y(), -q.z(), -q.w()); -} - - - -/**@brief Return the product of two quaternions */ -TF2SIMD_FORCE_INLINE Quaternion -operator*(const Quaternion& q1, const Quaternion& q2) { - return Quaternion(q1.w() * q2.x() + q1.x() * q2.w() + q1.y() * q2.z() - q1.z() * q2.y(), - q1.w() * q2.y() + q1.y() * q2.w() + q1.z() * q2.x() - q1.x() * q2.z(), - q1.w() * q2.z() + q1.z() * q2.w() + q1.x() * q2.y() - q1.y() * q2.x(), - q1.w() * q2.w() - q1.x() * q2.x() - q1.y() * q2.y() - q1.z() * q2.z()); -} - -TF2SIMD_FORCE_INLINE Quaternion -operator*(const Quaternion& q, const Vector3& w) -{ - return Quaternion( q.w() * w.x() + q.y() * w.z() - q.z() * w.y(), - q.w() * w.y() + q.z() * w.x() - q.x() * w.z(), - q.w() * w.z() + q.x() * w.y() - q.y() * w.x(), - -q.x() * w.x() - q.y() * w.y() - q.z() * w.z()); -} - -TF2SIMD_FORCE_INLINE Quaternion -operator*(const Vector3& w, const Quaternion& q) -{ - return Quaternion( w.x() * q.w() + w.y() * q.z() - w.z() * q.y(), - w.y() * q.w() + w.z() * q.x() - w.x() * q.z(), - w.z() * q.w() + w.x() * q.y() - w.y() * q.x(), - -w.x() * q.x() - w.y() * q.y() - w.z() * q.z()); -} - -/**@brief Calculate the dot product between two quaternions */ -TF2SIMD_FORCE_INLINE tf2Scalar -dot(const Quaternion& q1, const Quaternion& q2) -{ - return q1.dot(q2); -} - - -/**@brief Return the length of a quaternion */ -TF2SIMD_FORCE_INLINE tf2Scalar -length(const Quaternion& q) -{ - return q.length(); -} - -/**@brief Return the ***half*** angle between two quaternions*/ -TF2SIMD_FORCE_INLINE tf2Scalar -angle(const Quaternion& q1, const Quaternion& q2) -{ - return q1.angle(q2); -} - -/**@brief Return the shortest angle between two quaternions*/ -TF2SIMD_FORCE_INLINE tf2Scalar -angleShortestPath(const Quaternion& q1, const Quaternion& q2) -{ - return q1.angleShortestPath(q2); -} - -/**@brief Return the inverse of a quaternion*/ -TF2SIMD_FORCE_INLINE Quaternion -inverse(const Quaternion& q) -{ - return q.inverse(); -} - -/**@brief Return the result of spherical linear interpolation betwen two quaternions - * @param q1 The first quaternion - * @param q2 The second quaternion - * @param t The ration between q1 and q2. t = 0 return q1, t=1 returns q2 - * Slerp assumes constant velocity between positions. */ -TF2SIMD_FORCE_INLINE Quaternion -slerp(const Quaternion& q1, const Quaternion& q2, const tf2Scalar& t) -{ - return q1.slerp(q2, t); -} - -TF2SIMD_FORCE_INLINE Vector3 -quatRotate(const Quaternion& rotation, const Vector3& v) -{ - Quaternion q = rotation * v; - q *= rotation.inverse(); - return Vector3(q.getX(),q.getY(),q.getZ()); -} - -TF2SIMD_FORCE_INLINE Quaternion -shortestArcQuat(const Vector3& v0, const Vector3& v1) // Game Programming Gems 2.10. make sure v0,v1 are normalized -{ - Vector3 c = v0.cross(v1); - tf2Scalar d = v0.dot(v1); - - if (d < -1.0 + TF2SIMD_EPSILON) - { - Vector3 n,unused; - tf2PlaneSpace1(v0,n,unused); - return Quaternion(n.x(),n.y(),n.z(),0.0f); // just pick any vector that is orthogonal to v0 - } - - tf2Scalar s = tf2Sqrt((1.0f + d) * 2.0f); - tf2Scalar rs = 1.0f / s; - - return Quaternion(c.getX()*rs,c.getY()*rs,c.getZ()*rs,s * 0.5f); -} - -TF2SIMD_FORCE_INLINE Quaternion -shortestArcQuatNormalize2(Vector3& v0,Vector3& v1) -{ - v0.normalize(); - v1.normalize(); - return shortestArcQuat(v0,v1); -} - -} -#endif +#endif // TF2__LINEARMATH__QUATERNION_H_ diff --git a/tf2/include/tf2/LinearMath/Quaternion.hpp b/tf2/include/tf2/LinearMath/Quaternion.hpp new file mode 100644 index 000000000..c4c505b6e --- /dev/null +++ b/tf2/include/tf2/LinearMath/Quaternion.hpp @@ -0,0 +1,476 @@ +/* +Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +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. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + + +#ifndef TF2__LINEARMATH__QUATERNION_HPP_ +#define TF2__LINEARMATH__QUATERNION_HPP_ + + +#include "Vector3.hpp" +#include "QuadWord.hpp" +#include "tf2/visibility_control.h" + +namespace tf2 +{ + +/**@brief The Quaternion implements quaternion to perform linear algebra rotations in combination with Matrix3x3, Vector3 and Transform. */ +class Quaternion : public QuadWord { +public: + /**@brief No initialization constructor */ + TF2_PUBLIC + Quaternion() {} + + // template + // explicit Quaternion(const tf2Scalar *v) : Tuple4(v) {} + /**@brief Constructor from scalars */ + TF2_PUBLIC + Quaternion(const tf2Scalar& x, const tf2Scalar& y, const tf2Scalar& z, const tf2Scalar& w) + : QuadWord(x, y, z, w) + {} + /**@brief Axis angle Constructor + * @param axis The axis which the rotation is around + * @param angle The magnitude of the rotation around the angle (Radians) */ + TF2_PUBLIC + Quaternion(const Vector3& axis, const tf2Scalar& angle) + { + setRotation(axis, angle); + } + /**@brief Set the rotation using axis angle notation + * @param axis The axis around which to rotate + * @param angle The magnitude of the rotation in Radians */ + TF2_PUBLIC + void setRotation(const Vector3& axis, const tf2Scalar& angle) + { + tf2Scalar d = axis.length(); + tf2Assert(d != tf2Scalar(0.0)); + tf2Scalar s = tf2Sin(angle * tf2Scalar(0.5)) / d; + setValue(axis.x() * s, axis.y() * s, axis.z() * s, + tf2Cos(angle * tf2Scalar(0.5))); + } + /**@brief Set the quaternion using Euler angles + * @param yaw Angle around Y + * @param pitch Angle around X + * @param roll Angle around Z */ + TF2_PUBLIC + void setEuler(const tf2Scalar& yaw, const tf2Scalar& pitch, const tf2Scalar& roll) + { + tf2Scalar halfYaw = tf2Scalar(yaw) * tf2Scalar(0.5); + tf2Scalar halfPitch = tf2Scalar(pitch) * tf2Scalar(0.5); + tf2Scalar halfRoll = tf2Scalar(roll) * tf2Scalar(0.5); + tf2Scalar cosYaw = tf2Cos(halfYaw); + tf2Scalar sinYaw = tf2Sin(halfYaw); + tf2Scalar cosPitch = tf2Cos(halfPitch); + tf2Scalar sinPitch = tf2Sin(halfPitch); + tf2Scalar cosRoll = tf2Cos(halfRoll); + tf2Scalar sinRoll = tf2Sin(halfRoll); + setValue(cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw, + cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw, + sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw, + cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw); + } + /**@brief Set the quaternion using fixed axis RPY + * @param roll Angle around X + * @param pitch Angle around Y + * @param yaw Angle around Z*/ + TF2_PUBLIC + void setRPY(const tf2Scalar& roll, const tf2Scalar& pitch, const tf2Scalar& yaw) + { + tf2Scalar halfYaw = tf2Scalar(yaw) * tf2Scalar(0.5); + tf2Scalar halfPitch = tf2Scalar(pitch) * tf2Scalar(0.5); + tf2Scalar halfRoll = tf2Scalar(roll) * tf2Scalar(0.5); + tf2Scalar cosYaw = tf2Cos(halfYaw); + tf2Scalar sinYaw = tf2Sin(halfYaw); + tf2Scalar cosPitch = tf2Cos(halfPitch); + tf2Scalar sinPitch = tf2Sin(halfPitch); + tf2Scalar cosRoll = tf2Cos(halfRoll); + tf2Scalar sinRoll = tf2Sin(halfRoll); + setValue(sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw, //x + cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw, //y + cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw, //z + cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw); //formerly yzx + } + /**@brief Add two quaternions + * @param q The quaternion to add to this one */ + TF2SIMD_FORCE_INLINE Quaternion& operator+=(const Quaternion& q) + { + m_floats[0] += q.x(); m_floats[1] += q.y(); m_floats[2] += q.z(); m_floats[3] += q.m_floats[3]; + return *this; + } + + /**@brief Sutf2ract out a quaternion + * @param q The quaternion to sutf2ract from this one */ + TF2_PUBLIC + Quaternion& operator-=(const Quaternion& q) + { + m_floats[0] -= q.x(); m_floats[1] -= q.y(); m_floats[2] -= q.z(); m_floats[3] -= q.m_floats[3]; + return *this; + } + + /**@brief Scale this quaternion + * @param s The scalar to scale by */ + TF2_PUBLIC + Quaternion& operator*=(const tf2Scalar& s) + { + m_floats[0] *= s; m_floats[1] *= s; m_floats[2] *= s; m_floats[3] *= s; + return *this; + } + + /**@brief Multiply this quaternion by q on the right + * @param q The other quaternion + * Equivilant to this = this * q */ + TF2_PUBLIC + Quaternion& operator*=(const Quaternion& q) + { + setValue(m_floats[3] * q.x() + m_floats[0] * q.m_floats[3] + m_floats[1] * q.z() - m_floats[2] * q.y(), + m_floats[3] * q.y() + m_floats[1] * q.m_floats[3] + m_floats[2] * q.x() - m_floats[0] * q.z(), + m_floats[3] * q.z() + m_floats[2] * q.m_floats[3] + m_floats[0] * q.y() - m_floats[1] * q.x(), + m_floats[3] * q.m_floats[3] - m_floats[0] * q.x() - m_floats[1] * q.y() - m_floats[2] * q.z()); + return *this; + } + /**@brief Return the dot product between this quaternion and another + * @param q The other quaternion */ + TF2_PUBLIC + tf2Scalar dot(const Quaternion& q) const + { + return m_floats[0] * q.x() + m_floats[1] * q.y() + m_floats[2] * q.z() + m_floats[3] * q.m_floats[3]; + } + + /**@brief Return the length squared of the quaternion */ + TF2_PUBLIC + tf2Scalar length2() const + { + return dot(*this); + } + + /**@brief Return the length of the quaternion */ + TF2_PUBLIC + tf2Scalar length() const + { + return tf2Sqrt(length2()); + } + + /**@brief Normalize the quaternion + * Such that x^2 + y^2 + z^2 +w^2 = 1 */ + TF2_PUBLIC + Quaternion& normalize() + { + return *this /= length(); + } + + /**@brief Return a scaled version of this quaternion + * @param s The scale factor */ + TF2SIMD_FORCE_INLINE Quaternion + operator*(const tf2Scalar& s) const + { + return Quaternion(x() * s, y() * s, z() * s, m_floats[3] * s); + } + + + /**@brief Return an inversely scaled versionof this quaternion + * @param s The inverse scale factor */ + TF2_PUBLIC + Quaternion operator/(const tf2Scalar& s) const + { + tf2Assert(s != tf2Scalar(0.0)); + return *this * (tf2Scalar(1.0) / s); + } + + /**@brief Inversely scale this quaternion + * @param s The scale factor */ + TF2_PUBLIC + Quaternion& operator/=(const tf2Scalar& s) + { + tf2Assert(s != tf2Scalar(0.0)); + return *this *= tf2Scalar(1.0) / s; + } + + /**@brief Return a normalized version of this quaternion */ + TF2_PUBLIC + Quaternion normalized() const + { + return *this / length(); + } + /**@brief Return the ***half*** angle between this quaternion and the other + * @param q The other quaternion */ + TF2_PUBLIC + tf2Scalar angle(const Quaternion& q) const + { + tf2Scalar s = tf2Sqrt(length2() * q.length2()); + tf2Assert(s != tf2Scalar(0.0)); + return tf2Acos(dot(q) / s); + } + /**@brief Return the angle between this quaternion and the other along the shortest path + * @param q The other quaternion */ + TF2_PUBLIC + tf2Scalar angleShortestPath(const Quaternion& q) const + { + tf2Scalar s = tf2Sqrt(length2() * q.length2()); + tf2Assert(s != tf2Scalar(0.0)); + if (dot(q) < 0) // Take care of long angle case see http://en.wikipedia.org/wiki/Slerp + return tf2Acos(dot(-q) / s) * tf2Scalar(2.0); + else + return tf2Acos(dot(q) / s) * tf2Scalar(2.0); + } + /**@brief Return the angle [0, 2Pi] of rotation represented by this quaternion */ + TF2_PUBLIC + tf2Scalar getAngle() const + { + tf2Scalar s = tf2Scalar(2.) * tf2Acos(m_floats[3]); + return s; + } + + /**@brief Return the angle [0, Pi] of rotation represented by this quaternion along the shortest path */ + TF2_PUBLIC + tf2Scalar getAngleShortestPath() const + { + tf2Scalar s; + if (m_floats[3] >= 0) + s = tf2Scalar(2.) * tf2Acos(m_floats[3]); + else + s = tf2Scalar(2.) * tf2Acos(-m_floats[3]); + + return s; + } + + /**@brief Return the axis of the rotation represented by this quaternion */ + TF2_PUBLIC + Vector3 getAxis() const + { + tf2Scalar s_squared = tf2Scalar(1.) - tf2Pow(m_floats[3], tf2Scalar(2.)); + if (s_squared < tf2Scalar(10.) * TF2SIMD_EPSILON) //Check for divide by zero + return Vector3(1.0, 0.0, 0.0); // Arbitrary + tf2Scalar s = tf2Sqrt(s_squared); + return Vector3(m_floats[0] / s, m_floats[1] / s, m_floats[2] / s); + } + + /**@brief Return the inverse of this quaternion */ + TF2_PUBLIC + Quaternion inverse() const + { + return Quaternion(-m_floats[0], -m_floats[1], -m_floats[2], m_floats[3]); + } + + /**@brief Return the sum of this quaternion and the other + * @param q2 The other quaternion */ + TF2SIMD_FORCE_INLINE Quaternion + operator+(const Quaternion& q2) const + { + const Quaternion& q1 = *this; + return Quaternion(q1.x() + q2.x(), q1.y() + q2.y(), q1.z() + q2.z(), q1.m_floats[3] + q2.m_floats[3]); + } + + /**@brief Return the difference between this quaternion and the other + * @param q2 The other quaternion */ + TF2SIMD_FORCE_INLINE Quaternion + operator-(const Quaternion& q2) const + { + const Quaternion& q1 = *this; + return Quaternion(q1.x() - q2.x(), q1.y() - q2.y(), q1.z() - q2.z(), q1.m_floats[3] - q2.m_floats[3]); + } + + /**@brief Return the negative of this quaternion + * This simply negates each element */ + TF2SIMD_FORCE_INLINE Quaternion operator-() const + { + const Quaternion& q2 = *this; + return Quaternion( - q2.x(), - q2.y(), - q2.z(), - q2.m_floats[3]); + } + /**@todo document this and it's use */ + TF2SIMD_FORCE_INLINE Quaternion farthest( const Quaternion& qd) const + { + Quaternion diff,sum; + diff = *this - qd; + sum = *this + qd; + if( diff.dot(diff) > sum.dot(sum) ) + return qd; + return (-qd); + } + + /**@todo document this and it's use */ + TF2SIMD_FORCE_INLINE Quaternion nearest( const Quaternion& qd) const + { + Quaternion diff,sum; + diff = *this - qd; + sum = *this + qd; + if( diff.dot(diff) < sum.dot(sum) ) + return qd; + return (-qd); + } + + + /**@brief Return the quaternion which is the result of Spherical Linear Interpolation between this and the other quaternion + * @param q The other quaternion to interpolate with + * @param t The ratio between this and q to interpolate. If t = 0 the result is this, if t=1 the result is q. + * Slerp interpolates assuming constant velocity. */ + TF2_PUBLIC + Quaternion slerp(const Quaternion& q, const tf2Scalar& t) const + { + tf2Scalar theta = angleShortestPath(q) / tf2Scalar(2.0); + if (theta != tf2Scalar(0.0)) + { + tf2Scalar d = tf2Scalar(1.0) / tf2Sin(theta); + tf2Scalar s0 = tf2Sin((tf2Scalar(1.0) - t) * theta); + tf2Scalar s1 = tf2Sin(t * theta); + if (dot(q) < 0) // Take care of long angle case see http://en.wikipedia.org/wiki/Slerp + return Quaternion((m_floats[0] * s0 + -q.x() * s1) * d, + (m_floats[1] * s0 + -q.y() * s1) * d, + (m_floats[2] * s0 + -q.z() * s1) * d, + (m_floats[3] * s0 + -q.m_floats[3] * s1) * d); + else + return Quaternion((m_floats[0] * s0 + q.x() * s1) * d, + (m_floats[1] * s0 + q.y() * s1) * d, + (m_floats[2] * s0 + q.z() * s1) * d, + (m_floats[3] * s0 + q.m_floats[3] * s1) * d); + + } + else + { + return *this; + } + } + + TF2_PUBLIC + static const Quaternion& getIdentity() + { + static const Quaternion identityQuat(tf2Scalar(0.),tf2Scalar(0.),tf2Scalar(0.),tf2Scalar(1.)); + return identityQuat; + } + + TF2SIMD_FORCE_INLINE const tf2Scalar& getW() const { return m_floats[3]; } + + +}; + + +/**@brief Return the negative of a quaternion */ +TF2SIMD_FORCE_INLINE Quaternion +operator-(const Quaternion& q) +{ + return Quaternion(-q.x(), -q.y(), -q.z(), -q.w()); +} + + + +/**@brief Return the product of two quaternions */ +TF2SIMD_FORCE_INLINE Quaternion +operator*(const Quaternion& q1, const Quaternion& q2) { + return Quaternion(q1.w() * q2.x() + q1.x() * q2.w() + q1.y() * q2.z() - q1.z() * q2.y(), + q1.w() * q2.y() + q1.y() * q2.w() + q1.z() * q2.x() - q1.x() * q2.z(), + q1.w() * q2.z() + q1.z() * q2.w() + q1.x() * q2.y() - q1.y() * q2.x(), + q1.w() * q2.w() - q1.x() * q2.x() - q1.y() * q2.y() - q1.z() * q2.z()); +} + +TF2SIMD_FORCE_INLINE Quaternion +operator*(const Quaternion& q, const Vector3& w) +{ + return Quaternion( q.w() * w.x() + q.y() * w.z() - q.z() * w.y(), + q.w() * w.y() + q.z() * w.x() - q.x() * w.z(), + q.w() * w.z() + q.x() * w.y() - q.y() * w.x(), + -q.x() * w.x() - q.y() * w.y() - q.z() * w.z()); +} + +TF2SIMD_FORCE_INLINE Quaternion +operator*(const Vector3& w, const Quaternion& q) +{ + return Quaternion( w.x() * q.w() + w.y() * q.z() - w.z() * q.y(), + w.y() * q.w() + w.z() * q.x() - w.x() * q.z(), + w.z() * q.w() + w.x() * q.y() - w.y() * q.x(), + -w.x() * q.x() - w.y() * q.y() - w.z() * q.z()); +} + +/**@brief Calculate the dot product between two quaternions */ +TF2SIMD_FORCE_INLINE tf2Scalar +dot(const Quaternion& q1, const Quaternion& q2) +{ + return q1.dot(q2); +} + + +/**@brief Return the length of a quaternion */ +TF2SIMD_FORCE_INLINE tf2Scalar +length(const Quaternion& q) +{ + return q.length(); +} + +/**@brief Return the ***half*** angle between two quaternions*/ +TF2SIMD_FORCE_INLINE tf2Scalar +angle(const Quaternion& q1, const Quaternion& q2) +{ + return q1.angle(q2); +} + +/**@brief Return the shortest angle between two quaternions*/ +TF2SIMD_FORCE_INLINE tf2Scalar +angleShortestPath(const Quaternion& q1, const Quaternion& q2) +{ + return q1.angleShortestPath(q2); +} + +/**@brief Return the inverse of a quaternion*/ +TF2SIMD_FORCE_INLINE Quaternion +inverse(const Quaternion& q) +{ + return q.inverse(); +} + +/**@brief Return the result of spherical linear interpolation betwen two quaternions + * @param q1 The first quaternion + * @param q2 The second quaternion + * @param t The ration between q1 and q2. t = 0 return q1, t=1 returns q2 + * Slerp assumes constant velocity between positions. */ +TF2SIMD_FORCE_INLINE Quaternion +slerp(const Quaternion& q1, const Quaternion& q2, const tf2Scalar& t) +{ + return q1.slerp(q2, t); +} + +TF2SIMD_FORCE_INLINE Vector3 +quatRotate(const Quaternion& rotation, const Vector3& v) +{ + Quaternion q = rotation * v; + q *= rotation.inverse(); + return Vector3(q.getX(),q.getY(),q.getZ()); +} + +TF2SIMD_FORCE_INLINE Quaternion +shortestArcQuat(const Vector3& v0, const Vector3& v1) // Game Programming Gems 2.10. make sure v0,v1 are normalized +{ + Vector3 c = v0.cross(v1); + tf2Scalar d = v0.dot(v1); + + if (d < -1.0 + TF2SIMD_EPSILON) + { + Vector3 n,unused; + tf2PlaneSpace1(v0,n,unused); + return Quaternion(n.x(),n.y(),n.z(),0.0f); // just pick any vector that is orthogonal to v0 + } + + tf2Scalar s = tf2Sqrt((1.0f + d) * 2.0f); + tf2Scalar rs = 1.0f / s; + + return Quaternion(c.getX()*rs,c.getY()*rs,c.getZ()*rs,s * 0.5f); +} + +TF2SIMD_FORCE_INLINE Quaternion +shortestArcQuatNormalize2(Vector3& v0,Vector3& v1) +{ + v0.normalize(); + v1.normalize(); + return shortestArcQuat(v0,v1); +} + +} +#endif // TF2__LINEARMATH__QUATERNION_HPP_ diff --git a/tf2/include/tf2/LinearMath/Scalar.h b/tf2/include/tf2/LinearMath/Scalar.h index 39c1477de..ec3d55ad6 100644 --- a/tf2/include/tf2/LinearMath/Scalar.h +++ b/tf2/include/tf2/LinearMath/Scalar.h @@ -13,405 +13,9 @@ subject to the following restrictions: */ +#ifndef TF2__LINEARMATH__SCALAR_H_ +#define TF2__LINEARMATH__SCALAR_H_ -#ifndef TF2_SCALAR_H -#define TF2_SCALAR_H +#include -#ifdef TF2_MANAGED_CODE -//Aligned data types not supported in managed code -#pragma unmanaged -#endif - - -#include -#include //size_t for MSVC 6.0 -#include -#include -#include - -#if defined(DEBUG) || defined (_DEBUG) -#define TF2_DEBUG -#endif - - -#ifdef _WIN32 - - #if defined(__MINGW32__) || defined(__CYGWIN__) || (defined (_MSC_VER) && _MSC_VER < 1300) - - #define TF2SIMD_FORCE_INLINE inline - #define ATTRIBUTE_ALIGNED16(a) a - #define ATTRIBUTE_ALIGNED64(a) a - #define ATTRIBUTE_ALIGNED128(a) a - #else - //#define TF2_HAS_ALIGNED_ALLOCATOR - #pragma warning(disable : 4324) // disable padding warning -// #pragma warning(disable:4530) // Disable the exception disable but used in MSCV Stl warning. -// #pragma warning(disable:4996) //Turn off warnings about deprecated C routines -// #pragma warning(disable:4786) // Disable the "debug name too long" warning - - #define TF2SIMD_FORCE_INLINE __forceinline - #define ATTRIBUTE_ALIGNED16(a) __declspec(align(16)) a - #define ATTRIBUTE_ALIGNED64(a) __declspec(align(64)) a - #define ATTRIBUTE_ALIGNED128(a) __declspec (align(128)) a - #ifdef _XBOX - #define TF2_USE_VMX128 - - #include - #define TF2_HAVE_NATIVE_FSEL - #define tf2Fsel(a,b,c) __fsel((a),(b),(c)) - #else - - - #endif//_XBOX - - #endif //__MINGW32__ - - #include -#ifdef TF2_DEBUG - #define tf2Assert assert -#else - #define tf2Assert(x) -#endif - //tf2FullAssert is optional, slows down a lot - #define tf2FullAssert(x) - - #define tf2Likely(_c) _c - #define tf2Unlikely(_c) _c - -#else - -#if defined (__CELLOS_LV2__) - #define TF2SIMD_FORCE_INLINE inline - #define ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16))) - #define ATTRIBUTE_ALIGNED64(a) a __attribute__ ((aligned (64))) - #define ATTRIBUTE_ALIGNED128(a) a __attribute__ ((aligned (128))) - #ifndef assert - #include - #endif -#ifdef TF2_DEBUG - #define tf2Assert assert -#else - #define tf2Assert(x) -#endif - //tf2FullAssert is optional, slows down a lot - #define tf2FullAssert(x) - - #define tf2Likely(_c) _c - #define tf2Unlikely(_c) _c - -#else - -#ifdef USE_LIBSPE2 - - #define TF2SIMD_FORCE_INLINE __inline - #define ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16))) - #define ATTRIBUTE_ALIGNED64(a) a __attribute__ ((aligned (64))) - #define ATTRIBUTE_ALIGNED128(a) a __attribute__ ((aligned (128))) - #ifndef assert - #include - #endif -#ifdef TF2_DEBUG - #define tf2Assert assert -#else - #define tf2Assert(x) -#endif - //tf2FullAssert is optional, slows down a lot - #define tf2FullAssert(x) - - - #define tf2Likely(_c) __builtin_expect((_c), 1) - #define tf2Unlikely(_c) __builtin_expect((_c), 0) - - -#else - //non-windows systems - - #define TF2SIMD_FORCE_INLINE inline - ///@todo: check out alignment methods for other platforms/compilers - ///#define ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16))) - ///#define ATTRIBUTE_ALIGNED64(a) a __attribute__ ((aligned (64))) - ///#define ATTRIBUTE_ALIGNED128(a) a __attribute__ ((aligned (128))) - #define ATTRIBUTE_ALIGNED16(a) a - #define ATTRIBUTE_ALIGNED64(a) a - #define ATTRIBUTE_ALIGNED128(a) a - #ifndef assert - #include - #endif - -#if defined(DEBUG) || defined (_DEBUG) - #define tf2Assert assert -#else - #define tf2Assert(x) -#endif - - //tf2FullAssert is optional, slows down a lot - #define tf2FullAssert(x) - #define tf2Likely(_c) _c - #define tf2Unlikely(_c) _c - -#endif // LIBSPE2 - -#endif //__CELLOS_LV2__ -#endif - - -///The tf2Scalar type abstracts floating point numbers, to easily switch between double and single floating point precision. -typedef double tf2Scalar; -//this number could be bigger in double precision -#define TF2_LARGE_FLOAT 1e30 - - -#define TF2_DECLARE_ALIGNED_ALLOCATOR() \ - TF2SIMD_FORCE_INLINE void* operator new(size_t sizeInBytes) { return tf2AlignedAlloc(sizeInBytes,16); } \ - TF2SIMD_FORCE_INLINE void operator delete(void* ptr) { tf2AlignedFree(ptr); } \ - TF2SIMD_FORCE_INLINE void* operator new(size_t, void* ptr) { return ptr; } \ - TF2SIMD_FORCE_INLINE void operator delete(void*, void*) { } \ - TF2SIMD_FORCE_INLINE void* operator new[](size_t sizeInBytes) { return tf2AlignedAlloc(sizeInBytes,16); } \ - TF2SIMD_FORCE_INLINE void operator delete[](void* ptr) { tf2AlignedFree(ptr); } \ - TF2SIMD_FORCE_INLINE void* operator new[](size_t, void* ptr) { return ptr; } \ - TF2SIMD_FORCE_INLINE void operator delete[](void*, void*) { } \ - - - - -TF2SIMD_FORCE_INLINE tf2Scalar tf2Sqrt(tf2Scalar x) { return sqrt(x); } -TF2SIMD_FORCE_INLINE tf2Scalar tf2Fabs(tf2Scalar x) { return fabs(x); } -TF2SIMD_FORCE_INLINE tf2Scalar tf2Cos(tf2Scalar x) { return cos(x); } -TF2SIMD_FORCE_INLINE tf2Scalar tf2Sin(tf2Scalar x) { return sin(x); } -TF2SIMD_FORCE_INLINE tf2Scalar tf2Tan(tf2Scalar x) { return tan(x); } -TF2SIMD_FORCE_INLINE tf2Scalar tf2Acos(tf2Scalar x) { if (xtf2Scalar(1)) x=tf2Scalar(1); return acos(x); } -TF2SIMD_FORCE_INLINE tf2Scalar tf2Asin(tf2Scalar x) { if (xtf2Scalar(1)) x=tf2Scalar(1); return asin(x); } -TF2SIMD_FORCE_INLINE tf2Scalar tf2Atan(tf2Scalar x) { return atan(x); } -TF2SIMD_FORCE_INLINE tf2Scalar tf2Atan2(tf2Scalar x, tf2Scalar y) { return atan2(x, y); } -TF2SIMD_FORCE_INLINE tf2Scalar tf2Exp(tf2Scalar x) { return exp(x); } -TF2SIMD_FORCE_INLINE tf2Scalar tf2Log(tf2Scalar x) { return log(x); } -TF2SIMD_FORCE_INLINE tf2Scalar tf2Pow(tf2Scalar x,tf2Scalar y) { return pow(x,y); } -TF2SIMD_FORCE_INLINE tf2Scalar tf2Fmod(tf2Scalar x,tf2Scalar y) { return fmod(x,y); } - - -#define TF2SIMD_2_PI tf2Scalar(6.283185307179586232) -#define TF2SIMD_PI (TF2SIMD_2_PI * tf2Scalar(0.5)) -#define TF2SIMD_HALF_PI (TF2SIMD_2_PI * tf2Scalar(0.25)) -#define TF2SIMD_RADS_PER_DEG (TF2SIMD_2_PI / tf2Scalar(360.0)) -#define TF2SIMD_DEGS_PER_RAD (tf2Scalar(360.0) / TF2SIMD_2_PI) -#define TF2SIMDSQRT12 tf2Scalar(0.7071067811865475244008443621048490) - -#define tf2RecipSqrt(x) ((tf2Scalar)(tf2Scalar(1.0)/tf2Sqrt(tf2Scalar(x)))) /* reciprocal square root */ - - -#define TF2SIMD_EPSILON DBL_EPSILON -#define TF2SIMD_INFINITY DBL_MAX - -TF2SIMD_FORCE_INLINE tf2Scalar tf2Atan2Fast(tf2Scalar y, tf2Scalar x) -{ - tf2Scalar coeff_1 = TF2SIMD_PI / 4.0f; - tf2Scalar coeff_2 = 3.0f * coeff_1; - tf2Scalar abs_y = tf2Fabs(y); - tf2Scalar angle; - if (x >= 0.0f) { - tf2Scalar r = (x - abs_y) / (x + abs_y); - angle = coeff_1 - coeff_1 * r; - } else { - tf2Scalar r = (x + abs_y) / (abs_y - x); - angle = coeff_2 - coeff_1 * r; - } - return (y < 0.0f) ? -angle : angle; -} - -TF2SIMD_FORCE_INLINE bool tf2FuzzyZero(tf2Scalar x) { return tf2Fabs(x) < TF2SIMD_EPSILON; } - -TF2SIMD_FORCE_INLINE bool tf2Equal(tf2Scalar a, tf2Scalar eps) { - return (((a) <= eps) && !((a) < -eps)); -} -TF2SIMD_FORCE_INLINE bool tf2GreaterEqual (tf2Scalar a, tf2Scalar eps) { - return (!((a) <= eps)); -} - - -TF2SIMD_FORCE_INLINE int tf2IsNegative(tf2Scalar x) { - return x < tf2Scalar(0.0) ? 1 : 0; -} - -TF2SIMD_FORCE_INLINE tf2Scalar tf2Radians(tf2Scalar x) { return x * TF2SIMD_RADS_PER_DEG; } -TF2SIMD_FORCE_INLINE tf2Scalar tf2Degrees(tf2Scalar x) { return x * TF2SIMD_DEGS_PER_RAD; } - -#define TF2_DECLARE_HANDLE(name) typedef struct name##__ { int unused; } *name - -#ifndef tf2Fsel -TF2SIMD_FORCE_INLINE tf2Scalar tf2Fsel(tf2Scalar a, tf2Scalar b, tf2Scalar c) -{ - return a >= 0 ? b : c; -} -#endif -#define tf2Fsels(a,b,c) (tf2Scalar)tf2Fsel(a,b,c) - - -TF2SIMD_FORCE_INLINE bool tf2MachineIsLittleEndian() -{ - long int i = 1; - const char *p = (const char *) &i; - if (p[0] == 1) // Lowest address contains the least significant byte - return true; - else - return false; -} - - - -///tf2Select avoids branches, which makes performance much better for consoles like Playstation 3 and XBox 360 -///Thanks Phil Knight. See also http://www.cellperformance.com/articles/2006/04/more_techniques_for_eliminatin_1.html -TF2SIMD_FORCE_INLINE unsigned tf2Select(unsigned condition, unsigned valueIfConditionNonZero, unsigned valueIfConditionZero) -{ - // Set testNz to 0xFFFFFFFF if condition is nonzero, 0x00000000 if condition is zero - // Rely on positive value or'ed with its negative having sign bit on - // and zero value or'ed with its negative (which is still zero) having sign bit off - // Use arithmetic shift right, shifting the sign bit through all 32 bits - unsigned testNz = (unsigned)(((int)condition | -(int)condition) >> 31); - unsigned testEqz = ~testNz; - return ((valueIfConditionNonZero & testNz) | (valueIfConditionZero & testEqz)); -} -TF2SIMD_FORCE_INLINE int tf2Select(unsigned condition, int valueIfConditionNonZero, int valueIfConditionZero) -{ - unsigned testNz = (unsigned)(((int)condition | -(int)condition) >> 31); - unsigned testEqz = ~testNz; - return static_cast((valueIfConditionNonZero & testNz) | (valueIfConditionZero & testEqz)); -} -TF2SIMD_FORCE_INLINE float tf2Select(unsigned condition, float valueIfConditionNonZero, float valueIfConditionZero) -{ -#ifdef TF2_HAVE_NATIVE_FSEL - return (float)tf2Fsel((tf2Scalar)condition - tf2Scalar(1.0f), valueIfConditionNonZero, valueIfConditionZero); -#else - return (condition != 0) ? valueIfConditionNonZero : valueIfConditionZero; -#endif -} - -template TF2SIMD_FORCE_INLINE void tf2Swap(T& a, T& b) -{ - T tmp = a; - a = b; - b = tmp; -} - - -//PCK: endian swapping functions -TF2SIMD_FORCE_INLINE unsigned tf2SwapEndian(unsigned val) -{ - return (((val & 0xff000000) >> 24) | ((val & 0x00ff0000) >> 8) | ((val & 0x0000ff00) << 8) | ((val & 0x000000ff) << 24)); -} - -TF2SIMD_FORCE_INLINE unsigned short tf2SwapEndian(unsigned short val) -{ - return static_cast(((val & 0xff00) >> 8) | ((val & 0x00ff) << 8)); -} - -TF2SIMD_FORCE_INLINE unsigned tf2SwapEndian(int val) -{ - return tf2SwapEndian((unsigned)val); -} - -TF2SIMD_FORCE_INLINE unsigned short tf2SwapEndian(short val) -{ - return tf2SwapEndian((unsigned short) val); -} - -///tf2SwapFloat uses using char pointers to swap the endianness -////tf2SwapFloat/tf2SwapDouble will NOT return a float, because the machine might 'correct' invalid floating point values -///Not all values of sign/exponent/mantissa are valid floating point numbers according to IEEE 754. -///When a floating point unit is faced with an invalid value, it may actually change the value, or worse, throw an exception. -///In most systems, running user mode code, you wouldn't get an exception, but instead the hardware/os/runtime will 'fix' the number for you. -///so instead of returning a float/double, we return integer/long long integer -TF2SIMD_FORCE_INLINE unsigned int tf2SwapEndianFloat(float d) -{ - unsigned int a = 0; - unsigned char *dst = (unsigned char *)&a; - unsigned char *src = (unsigned char *)&d; - - dst[0] = src[3]; - dst[1] = src[2]; - dst[2] = src[1]; - dst[3] = src[0]; - return a; -} - -// unswap using char pointers -TF2SIMD_FORCE_INLINE float tf2UnswapEndianFloat(unsigned int a) -{ - float d = 0.0f; - unsigned char *src = (unsigned char *)&a; - unsigned char *dst = (unsigned char *)&d; - - dst[0] = src[3]; - dst[1] = src[2]; - dst[2] = src[1]; - dst[3] = src[0]; - - return d; -} - - -// swap using char pointers -TF2SIMD_FORCE_INLINE void tf2SwapEndianDouble(double d, unsigned char* dst) -{ - unsigned char *src = (unsigned char *)&d; - - dst[0] = src[7]; - dst[1] = src[6]; - dst[2] = src[5]; - dst[3] = src[4]; - dst[4] = src[3]; - dst[5] = src[2]; - dst[6] = src[1]; - dst[7] = src[0]; - -} - -// unswap using char pointers -TF2SIMD_FORCE_INLINE double tf2UnswapEndianDouble(const unsigned char *src) -{ - double d = 0.0; - unsigned char *dst = (unsigned char *)&d; - - dst[0] = src[7]; - dst[1] = src[6]; - dst[2] = src[5]; - dst[3] = src[4]; - dst[4] = src[3]; - dst[5] = src[2]; - dst[6] = src[1]; - dst[7] = src[0]; - - return d; -} - -// returns normalized value in range [-TF2SIMD_PI, TF2SIMD_PI] -TF2SIMD_FORCE_INLINE tf2Scalar tf2NormalizeAngle(tf2Scalar angleInRadians) -{ - angleInRadians = tf2Fmod(angleInRadians, TF2SIMD_2_PI); - if(angleInRadians < -TF2SIMD_PI) - { - return angleInRadians + TF2SIMD_2_PI; - } - else if(angleInRadians > TF2SIMD_PI) - { - return angleInRadians - TF2SIMD_2_PI; - } - else - { - return angleInRadians; - } -} - -///rudimentary class to provide type info -struct tf2TypedObject -{ - tf2TypedObject(int objectType) - :m_objectType(objectType) - { - } - int m_objectType; - inline int getObjectType() const - { - return m_objectType; - } -}; -#endif //TF2SIMD___SCALAR_H +#endif // TF2__LINEARMATH__SCALAR_H_ diff --git a/tf2/include/tf2/LinearMath/Scalar.hpp b/tf2/include/tf2/LinearMath/Scalar.hpp new file mode 100644 index 000000000..fdc2f9c44 --- /dev/null +++ b/tf2/include/tf2/LinearMath/Scalar.hpp @@ -0,0 +1,417 @@ +/* +Copyright (c) 2003-2009 Erwin Coumans http://bullet.googlecode.com + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +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. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + + +#ifndef TF2__LINEARMATH__SCALAR_HPP_ +#define TF2__LINEARMATH__SCALAR_HPP_ + +#ifdef TF2_MANAGED_CODE +//Aligned data types not supported in managed code +#pragma unmanaged +#endif + + +#include +#include //size_t for MSVC 6.0 +#include +#include +#include + +#if defined(DEBUG) || defined (_DEBUG) +#define TF2_DEBUG +#endif + + +#ifdef _WIN32 + + #if defined(__MINGW32__) || defined(__CYGWIN__) || (defined (_MSC_VER) && _MSC_VER < 1300) + + #define TF2SIMD_FORCE_INLINE inline + #define ATTRIBUTE_ALIGNED16(a) a + #define ATTRIBUTE_ALIGNED64(a) a + #define ATTRIBUTE_ALIGNED128(a) a + #else + //#define TF2_HAS_ALIGNED_ALLOCATOR + #pragma warning(disable : 4324) // disable padding warning +// #pragma warning(disable:4530) // Disable the exception disable but used in MSCV Stl warning. +// #pragma warning(disable:4996) //Turn off warnings about deprecated C routines +// #pragma warning(disable:4786) // Disable the "debug name too long" warning + + #define TF2SIMD_FORCE_INLINE __forceinline + #define ATTRIBUTE_ALIGNED16(a) __declspec(align(16)) a + #define ATTRIBUTE_ALIGNED64(a) __declspec(align(64)) a + #define ATTRIBUTE_ALIGNED128(a) __declspec (align(128)) a + #ifdef _XBOX + #define TF2_USE_VMX128 + + #include + #define TF2_HAVE_NATIVE_FSEL + #define tf2Fsel(a,b,c) __fsel((a),(b),(c)) + #else + + + #endif//_XBOX + + #endif //__MINGW32__ + + #include +#ifdef TF2_DEBUG + #define tf2Assert assert +#else + #define tf2Assert(x) +#endif + //tf2FullAssert is optional, slows down a lot + #define tf2FullAssert(x) + + #define tf2Likely(_c) _c + #define tf2Unlikely(_c) _c + +#else + +#if defined (__CELLOS_LV2__) + #define TF2SIMD_FORCE_INLINE inline + #define ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16))) + #define ATTRIBUTE_ALIGNED64(a) a __attribute__ ((aligned (64))) + #define ATTRIBUTE_ALIGNED128(a) a __attribute__ ((aligned (128))) + #ifndef assert + #include + #endif +#ifdef TF2_DEBUG + #define tf2Assert assert +#else + #define tf2Assert(x) +#endif + //tf2FullAssert is optional, slows down a lot + #define tf2FullAssert(x) + + #define tf2Likely(_c) _c + #define tf2Unlikely(_c) _c + +#else + +#ifdef USE_LIBSPE2 + + #define TF2SIMD_FORCE_INLINE __inline + #define ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16))) + #define ATTRIBUTE_ALIGNED64(a) a __attribute__ ((aligned (64))) + #define ATTRIBUTE_ALIGNED128(a) a __attribute__ ((aligned (128))) + #ifndef assert + #include + #endif +#ifdef TF2_DEBUG + #define tf2Assert assert +#else + #define tf2Assert(x) +#endif + //tf2FullAssert is optional, slows down a lot + #define tf2FullAssert(x) + + + #define tf2Likely(_c) __builtin_expect((_c), 1) + #define tf2Unlikely(_c) __builtin_expect((_c), 0) + + +#else + //non-windows systems + + #define TF2SIMD_FORCE_INLINE inline + ///@todo: check out alignment methods for other platforms/compilers + ///#define ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16))) + ///#define ATTRIBUTE_ALIGNED64(a) a __attribute__ ((aligned (64))) + ///#define ATTRIBUTE_ALIGNED128(a) a __attribute__ ((aligned (128))) + #define ATTRIBUTE_ALIGNED16(a) a + #define ATTRIBUTE_ALIGNED64(a) a + #define ATTRIBUTE_ALIGNED128(a) a + #ifndef assert + #include + #endif + +#if defined(DEBUG) || defined (_DEBUG) + #define tf2Assert assert +#else + #define tf2Assert(x) +#endif + + //tf2FullAssert is optional, slows down a lot + #define tf2FullAssert(x) + #define tf2Likely(_c) _c + #define tf2Unlikely(_c) _c + +#endif // LIBSPE2 + +#endif //__CELLOS_LV2__ +#endif + + +///The tf2Scalar type abstracts floating point numbers, to easily switch between double and single floating point precision. +typedef double tf2Scalar; +//this number could be bigger in double precision +#define TF2_LARGE_FLOAT 1e30 + + +#define TF2_DECLARE_ALIGNED_ALLOCATOR() \ + TF2SIMD_FORCE_INLINE void* operator new(size_t sizeInBytes) { return tf2AlignedAlloc(sizeInBytes,16); } \ + TF2SIMD_FORCE_INLINE void operator delete(void* ptr) { tf2AlignedFree(ptr); } \ + TF2SIMD_FORCE_INLINE void* operator new(size_t, void* ptr) { return ptr; } \ + TF2SIMD_FORCE_INLINE void operator delete(void*, void*) { } \ + TF2SIMD_FORCE_INLINE void* operator new[](size_t sizeInBytes) { return tf2AlignedAlloc(sizeInBytes,16); } \ + TF2SIMD_FORCE_INLINE void operator delete[](void* ptr) { tf2AlignedFree(ptr); } \ + TF2SIMD_FORCE_INLINE void* operator new[](size_t, void* ptr) { return ptr; } \ + TF2SIMD_FORCE_INLINE void operator delete[](void*, void*) { } \ + + + + +TF2SIMD_FORCE_INLINE tf2Scalar tf2Sqrt(tf2Scalar x) { return sqrt(x); } +TF2SIMD_FORCE_INLINE tf2Scalar tf2Fabs(tf2Scalar x) { return fabs(x); } +TF2SIMD_FORCE_INLINE tf2Scalar tf2Cos(tf2Scalar x) { return cos(x); } +TF2SIMD_FORCE_INLINE tf2Scalar tf2Sin(tf2Scalar x) { return sin(x); } +TF2SIMD_FORCE_INLINE tf2Scalar tf2Tan(tf2Scalar x) { return tan(x); } +TF2SIMD_FORCE_INLINE tf2Scalar tf2Acos(tf2Scalar x) { if (xtf2Scalar(1)) x=tf2Scalar(1); return acos(x); } +TF2SIMD_FORCE_INLINE tf2Scalar tf2Asin(tf2Scalar x) { if (xtf2Scalar(1)) x=tf2Scalar(1); return asin(x); } +TF2SIMD_FORCE_INLINE tf2Scalar tf2Atan(tf2Scalar x) { return atan(x); } +TF2SIMD_FORCE_INLINE tf2Scalar tf2Atan2(tf2Scalar x, tf2Scalar y) { return atan2(x, y); } +TF2SIMD_FORCE_INLINE tf2Scalar tf2Exp(tf2Scalar x) { return exp(x); } +TF2SIMD_FORCE_INLINE tf2Scalar tf2Log(tf2Scalar x) { return log(x); } +TF2SIMD_FORCE_INLINE tf2Scalar tf2Pow(tf2Scalar x,tf2Scalar y) { return pow(x,y); } +TF2SIMD_FORCE_INLINE tf2Scalar tf2Fmod(tf2Scalar x,tf2Scalar y) { return fmod(x,y); } + + +#define TF2SIMD_2_PI tf2Scalar(6.283185307179586232) +#define TF2SIMD_PI (TF2SIMD_2_PI * tf2Scalar(0.5)) +#define TF2SIMD_HALF_PI (TF2SIMD_2_PI * tf2Scalar(0.25)) +#define TF2SIMD_RADS_PER_DEG (TF2SIMD_2_PI / tf2Scalar(360.0)) +#define TF2SIMD_DEGS_PER_RAD (tf2Scalar(360.0) / TF2SIMD_2_PI) +#define TF2SIMDSQRT12 tf2Scalar(0.7071067811865475244008443621048490) + +#define tf2RecipSqrt(x) ((tf2Scalar)(tf2Scalar(1.0)/tf2Sqrt(tf2Scalar(x)))) /* reciprocal square root */ + + +#define TF2SIMD_EPSILON DBL_EPSILON +#define TF2SIMD_INFINITY DBL_MAX + +TF2SIMD_FORCE_INLINE tf2Scalar tf2Atan2Fast(tf2Scalar y, tf2Scalar x) +{ + tf2Scalar coeff_1 = TF2SIMD_PI / 4.0f; + tf2Scalar coeff_2 = 3.0f * coeff_1; + tf2Scalar abs_y = tf2Fabs(y); + tf2Scalar angle; + if (x >= 0.0f) { + tf2Scalar r = (x - abs_y) / (x + abs_y); + angle = coeff_1 - coeff_1 * r; + } else { + tf2Scalar r = (x + abs_y) / (abs_y - x); + angle = coeff_2 - coeff_1 * r; + } + return (y < 0.0f) ? -angle : angle; +} + +TF2SIMD_FORCE_INLINE bool tf2FuzzyZero(tf2Scalar x) { return tf2Fabs(x) < TF2SIMD_EPSILON; } + +TF2SIMD_FORCE_INLINE bool tf2Equal(tf2Scalar a, tf2Scalar eps) { + return (((a) <= eps) && !((a) < -eps)); +} +TF2SIMD_FORCE_INLINE bool tf2GreaterEqual (tf2Scalar a, tf2Scalar eps) { + return (!((a) <= eps)); +} + + +TF2SIMD_FORCE_INLINE int tf2IsNegative(tf2Scalar x) { + return x < tf2Scalar(0.0) ? 1 : 0; +} + +TF2SIMD_FORCE_INLINE tf2Scalar tf2Radians(tf2Scalar x) { return x * TF2SIMD_RADS_PER_DEG; } +TF2SIMD_FORCE_INLINE tf2Scalar tf2Degrees(tf2Scalar x) { return x * TF2SIMD_DEGS_PER_RAD; } + +#define TF2_DECLARE_HANDLE(name) typedef struct name##__ { int unused; } *name + +#ifndef tf2Fsel +TF2SIMD_FORCE_INLINE tf2Scalar tf2Fsel(tf2Scalar a, tf2Scalar b, tf2Scalar c) +{ + return a >= 0 ? b : c; +} +#endif +#define tf2Fsels(a,b,c) (tf2Scalar)tf2Fsel(a,b,c) + + +TF2SIMD_FORCE_INLINE bool tf2MachineIsLittleEndian() +{ + long int i = 1; + const char *p = (const char *) &i; + if (p[0] == 1) // Lowest address contains the least significant byte + return true; + else + return false; +} + + + +///tf2Select avoids branches, which makes performance much better for consoles like Playstation 3 and XBox 360 +///Thanks Phil Knight. See also http://www.cellperformance.com/articles/2006/04/more_techniques_for_eliminatin_1.html +TF2SIMD_FORCE_INLINE unsigned tf2Select(unsigned condition, unsigned valueIfConditionNonZero, unsigned valueIfConditionZero) +{ + // Set testNz to 0xFFFFFFFF if condition is nonzero, 0x00000000 if condition is zero + // Rely on positive value or'ed with its negative having sign bit on + // and zero value or'ed with its negative (which is still zero) having sign bit off + // Use arithmetic shift right, shifting the sign bit through all 32 bits + unsigned testNz = (unsigned)(((int)condition | -(int)condition) >> 31); + unsigned testEqz = ~testNz; + return ((valueIfConditionNonZero & testNz) | (valueIfConditionZero & testEqz)); +} +TF2SIMD_FORCE_INLINE int tf2Select(unsigned condition, int valueIfConditionNonZero, int valueIfConditionZero) +{ + unsigned testNz = (unsigned)(((int)condition | -(int)condition) >> 31); + unsigned testEqz = ~testNz; + return static_cast((valueIfConditionNonZero & testNz) | (valueIfConditionZero & testEqz)); +} +TF2SIMD_FORCE_INLINE float tf2Select(unsigned condition, float valueIfConditionNonZero, float valueIfConditionZero) +{ +#ifdef TF2_HAVE_NATIVE_FSEL + return (float)tf2Fsel((tf2Scalar)condition - tf2Scalar(1.0f), valueIfConditionNonZero, valueIfConditionZero); +#else + return (condition != 0) ? valueIfConditionNonZero : valueIfConditionZero; +#endif +} + +template TF2SIMD_FORCE_INLINE void tf2Swap(T& a, T& b) +{ + T tmp = a; + a = b; + b = tmp; +} + + +//PCK: endian swapping functions +TF2SIMD_FORCE_INLINE unsigned tf2SwapEndian(unsigned val) +{ + return (((val & 0xff000000) >> 24) | ((val & 0x00ff0000) >> 8) | ((val & 0x0000ff00) << 8) | ((val & 0x000000ff) << 24)); +} + +TF2SIMD_FORCE_INLINE unsigned short tf2SwapEndian(unsigned short val) +{ + return static_cast(((val & 0xff00) >> 8) | ((val & 0x00ff) << 8)); +} + +TF2SIMD_FORCE_INLINE unsigned tf2SwapEndian(int val) +{ + return tf2SwapEndian((unsigned)val); +} + +TF2SIMD_FORCE_INLINE unsigned short tf2SwapEndian(short val) +{ + return tf2SwapEndian((unsigned short) val); +} + +///tf2SwapFloat uses using char pointers to swap the endianness +////tf2SwapFloat/tf2SwapDouble will NOT return a float, because the machine might 'correct' invalid floating point values +///Not all values of sign/exponent/mantissa are valid floating point numbers according to IEEE 754. +///When a floating point unit is faced with an invalid value, it may actually change the value, or worse, throw an exception. +///In most systems, running user mode code, you wouldn't get an exception, but instead the hardware/os/runtime will 'fix' the number for you. +///so instead of returning a float/double, we return integer/long long integer +TF2SIMD_FORCE_INLINE unsigned int tf2SwapEndianFloat(float d) +{ + unsigned int a = 0; + unsigned char *dst = (unsigned char *)&a; + unsigned char *src = (unsigned char *)&d; + + dst[0] = src[3]; + dst[1] = src[2]; + dst[2] = src[1]; + dst[3] = src[0]; + return a; +} + +// unswap using char pointers +TF2SIMD_FORCE_INLINE float tf2UnswapEndianFloat(unsigned int a) +{ + float d = 0.0f; + unsigned char *src = (unsigned char *)&a; + unsigned char *dst = (unsigned char *)&d; + + dst[0] = src[3]; + dst[1] = src[2]; + dst[2] = src[1]; + dst[3] = src[0]; + + return d; +} + + +// swap using char pointers +TF2SIMD_FORCE_INLINE void tf2SwapEndianDouble(double d, unsigned char* dst) +{ + unsigned char *src = (unsigned char *)&d; + + dst[0] = src[7]; + dst[1] = src[6]; + dst[2] = src[5]; + dst[3] = src[4]; + dst[4] = src[3]; + dst[5] = src[2]; + dst[6] = src[1]; + dst[7] = src[0]; + +} + +// unswap using char pointers +TF2SIMD_FORCE_INLINE double tf2UnswapEndianDouble(const unsigned char *src) +{ + double d = 0.0; + unsigned char *dst = (unsigned char *)&d; + + dst[0] = src[7]; + dst[1] = src[6]; + dst[2] = src[5]; + dst[3] = src[4]; + dst[4] = src[3]; + dst[5] = src[2]; + dst[6] = src[1]; + dst[7] = src[0]; + + return d; +} + +// returns normalized value in range [-TF2SIMD_PI, TF2SIMD_PI] +TF2SIMD_FORCE_INLINE tf2Scalar tf2NormalizeAngle(tf2Scalar angleInRadians) +{ + angleInRadians = tf2Fmod(angleInRadians, TF2SIMD_2_PI); + if(angleInRadians < -TF2SIMD_PI) + { + return angleInRadians + TF2SIMD_2_PI; + } + else if(angleInRadians > TF2SIMD_PI) + { + return angleInRadians - TF2SIMD_2_PI; + } + else + { + return angleInRadians; + } +} + +///rudimentary class to provide type info +struct tf2TypedObject +{ + tf2TypedObject(int objectType) + :m_objectType(objectType) + { + } + int m_objectType; + inline int getObjectType() const + { + return m_objectType; + } +}; +#endif // TF2__LINEARMATH__SCALAR_HPP_ diff --git a/tf2/include/tf2/LinearMath/Transform.h b/tf2/include/tf2/LinearMath/Transform.h index b946225c2..9e5d22c81 100644 --- a/tf2/include/tf2/LinearMath/Transform.h +++ b/tf2/include/tf2/LinearMath/Transform.h @@ -13,309 +13,10 @@ subject to the following restrictions: */ +#ifndef TF2__LINEARMATH__TRANSFORM_H_ +#define TF2__LINEARMATH__TRANSFORM_H_ -#ifndef tf2_Transform_H -#define tf2_Transform_H - - -#include "Matrix3x3.h" -#include "tf2/visibility_control.h" - - -namespace tf2 -{ - -#define TransformData TransformDoubleData - - -/**@brief The Transform class supports rigid transforms with only translation and rotation and no scaling/shear. - *It can be used in combination with Vector3, Quaternion and Matrix3x3 linear algebra classes. */ -class Transform { - - ///Storage for the rotation - Matrix3x3 m_basis; - ///Storage for the translation - Vector3 m_origin; - -public: - - /**@brief No initialization constructor */ - TF2_PUBLIC - Transform() {} - /**@brief Constructor from Quaternion (optional Vector3 ) - * @param q Rotation from quaternion - * @param c Translation from Vector (default 0,0,0) */ - explicit TF2SIMD_FORCE_INLINE Transform(const Quaternion& q, - const Vector3& c = Vector3(tf2Scalar(0), tf2Scalar(0), tf2Scalar(0))) - : m_basis(q), - m_origin(c) - {} - - /**@brief Constructor from Matrix3x3 (optional Vector3) - * @param b Rotation from Matrix - * @param c Translation from Vector default (0,0,0)*/ - explicit TF2SIMD_FORCE_INLINE Transform(const Matrix3x3& b, - const Vector3& c = Vector3(tf2Scalar(0), tf2Scalar(0), tf2Scalar(0))) - : m_basis(b), - m_origin(c) - {} - /**@brief Copy constructor */ - TF2SIMD_FORCE_INLINE Transform (const Transform& other) - : m_basis(other.m_basis), - m_origin(other.m_origin) - { - } - /**@brief Assignment Operator */ - TF2SIMD_FORCE_INLINE Transform& operator=(const Transform& other) - { - m_basis = other.m_basis; - m_origin = other.m_origin; - return *this; - } - - /**@brief Set the current transform as the value of the product of two transforms - * @param t1 Transform 1 - * @param t2 Transform 2 - * This = Transform1 * Transform2 */ - TF2SIMD_FORCE_INLINE void mult(const Transform& t1, const Transform& t2) { - m_basis = t1.m_basis * t2.m_basis; - m_origin = t1(t2.m_origin); - } - -/* void multInverseLeft(const Transform& t1, const Transform& t2) { - Vector3 v = t2.m_origin - t1.m_origin; - m_basis = tf2MultTransposeLeft(t1.m_basis, t2.m_basis); - m_origin = v * t1.m_basis; - } - */ - -/**@brief Return the transform of the vector */ - TF2SIMD_FORCE_INLINE Vector3 operator()(const Vector3& x) const - { - return Vector3(m_basis[0].dot(x) + m_origin.x(), - m_basis[1].dot(x) + m_origin.y(), - m_basis[2].dot(x) + m_origin.z()); - } - - /**@brief Return the transform of the vector */ - TF2SIMD_FORCE_INLINE Vector3 operator*(const Vector3& x) const - { - return (*this)(x); - } - - /**@brief Return the transform of the Quaternion */ - TF2SIMD_FORCE_INLINE Quaternion operator*(const Quaternion& q) const - { - return getRotation() * q; - } - - /**@brief Return the basis matrix for the rotation */ - TF2SIMD_FORCE_INLINE Matrix3x3& getBasis() { return m_basis; } - /**@brief Return the basis matrix for the rotation */ - TF2SIMD_FORCE_INLINE const Matrix3x3& getBasis() const { return m_basis; } - - /**@brief Return the origin vector translation */ - TF2SIMD_FORCE_INLINE Vector3& getOrigin() { return m_origin; } - /**@brief Return the origin vector translation */ - TF2SIMD_FORCE_INLINE const Vector3& getOrigin() const { return m_origin; } - - /**@brief Return a quaternion representing the rotation */ - TF2_PUBLIC - Quaternion getRotation() const { - Quaternion q; - m_basis.getRotation(q); - return q; - } - - - /**@brief Set from an array - * @param m A pointer to a 15 element array (12 rotation(row major padded on the right by 1), and 3 translation */ - TF2_PUBLIC - void setFromOpenGLMatrix(const tf2Scalar *m) - { - m_basis.setFromOpenGLSubMatrix(m); - m_origin.setValue(m[12],m[13],m[14]); - } - - /**@brief Fill an array representation - * @param m A pointer to a 15 element array (12 rotation(row major padded on the right by 1), and 3 translation */ - TF2_PUBLIC - void getOpenGLMatrix(tf2Scalar *m) const - { - m_basis.getOpenGLSubMatrix(m); - m[12] = m_origin.x(); - m[13] = m_origin.y(); - m[14] = m_origin.z(); - m[15] = tf2Scalar(1.0); - } - - /**@brief Set the translational element - * @param origin The vector to set the translation to */ - TF2SIMD_FORCE_INLINE void setOrigin(const Vector3& origin) - { - m_origin = origin; - } - - TF2SIMD_FORCE_INLINE Vector3 invXform(const Vector3& inVec) const; - - - /**@brief Set the rotational element by Matrix3x3 */ - TF2SIMD_FORCE_INLINE void setBasis(const Matrix3x3& basis) - { - m_basis = basis; - } - - /**@brief Set the rotational element by Quaternion */ - TF2SIMD_FORCE_INLINE void setRotation(const Quaternion& q) - { - m_basis.setRotation(q); - } - - - /**@brief Set this transformation to the identity */ - TF2_PUBLIC - void setIdentity() - { - m_basis.setIdentity(); - m_origin.setValue(tf2Scalar(0.0), tf2Scalar(0.0), tf2Scalar(0.0)); - } - - /**@brief Multiply this Transform by another(this = this * another) - * @param t The other transform */ - TF2_PUBLIC - Transform& operator*=(const Transform& t) - { - m_origin += m_basis * t.m_origin; - m_basis *= t.m_basis; - return *this; - } - - /**@brief Return the inverse of this transform */ - TF2_PUBLIC - Transform inverse() const - { - Matrix3x3 inv = m_basis.transpose(); - return Transform(inv, inv * -m_origin); - } - - /**@brief Return the inverse of this transform times the other transform - * @param t The other transform - * return this.inverse() * the other */ - TF2_PUBLIC - Transform inverseTimes(const Transform& t) const; - - /**@brief Return the product of this transform and the other */ - TF2_PUBLIC - Transform operator*(const Transform& t) const; - - /**@brief Return an identity transform */ - TF2_PUBLIC - static const Transform& getIdentity() - { - static const Transform identityTransform(Matrix3x3::getIdentity()); - return identityTransform; - } - - TF2_PUBLIC - void serialize(struct TransformData& dataOut) const; - - TF2_PUBLIC - void serializeFloat(struct TransformFloatData& dataOut) const; - - TF2_PUBLIC - void deSerialize(const struct TransformData& dataIn); - - TF2_PUBLIC - void deSerializeDouble(const struct TransformDoubleData& dataIn); - - TF2_PUBLIC - void deSerializeFloat(const struct TransformFloatData& dataIn); - -}; - - -TF2SIMD_FORCE_INLINE Vector3 -Transform::invXform(const Vector3& inVec) const -{ - Vector3 v = inVec - m_origin; - return (m_basis.transpose() * v); -} - -TF2SIMD_FORCE_INLINE Transform -Transform::inverseTimes(const Transform& t) const -{ - Vector3 v = t.getOrigin() - m_origin; - return Transform(m_basis.transposeTimes(t.m_basis), - v * m_basis); -} - -TF2SIMD_FORCE_INLINE Transform -Transform::operator*(const Transform& t) const -{ - return Transform(m_basis * t.m_basis, - (*this)(t.m_origin)); -} - -/**@brief Test if two transforms have all elements equal */ -TF2SIMD_FORCE_INLINE bool operator==(const Transform& t1, const Transform& t2) -{ - return ( t1.getBasis() == t2.getBasis() && - t1.getOrigin() == t2.getOrigin() ); -} - - -///for serialization -struct TransformFloatData -{ - Matrix3x3FloatData m_basis; - Vector3FloatData m_origin; -}; - -struct TransformDoubleData -{ - Matrix3x3DoubleData m_basis; - Vector3DoubleData m_origin; -}; - - - -TF2SIMD_FORCE_INLINE void Transform::serialize(TransformData& dataOut) const -{ - m_basis.serialize(dataOut.m_basis); - m_origin.serialize(dataOut.m_origin); -} - -TF2SIMD_FORCE_INLINE void Transform::serializeFloat(TransformFloatData& dataOut) const -{ - m_basis.serializeFloat(dataOut.m_basis); - m_origin.serializeFloat(dataOut.m_origin); -} - - -TF2SIMD_FORCE_INLINE void Transform::deSerialize(const TransformData& dataIn) -{ - m_basis.deSerialize(dataIn.m_basis); - m_origin.deSerialize(dataIn.m_origin); -} - -TF2SIMD_FORCE_INLINE void Transform::deSerializeFloat(const TransformFloatData& dataIn) -{ - m_basis.deSerializeFloat(dataIn.m_basis); - m_origin.deSerializeFloat(dataIn.m_origin); -} - -TF2SIMD_FORCE_INLINE void Transform::deSerializeDouble(const TransformDoubleData& dataIn) -{ - m_basis.deSerializeDouble(dataIn.m_basis); - m_origin.deSerializeDouble(dataIn.m_origin); -} - -} - -#endif - - - - +#include +#endif // TF2__LINEARMATH__TRANSFORM_H_ diff --git a/tf2/include/tf2/LinearMath/Transform.hpp b/tf2/include/tf2/LinearMath/Transform.hpp new file mode 100644 index 000000000..0424eb961 --- /dev/null +++ b/tf2/include/tf2/LinearMath/Transform.hpp @@ -0,0 +1,315 @@ +/* +Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +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. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + + +#ifndef TF2__LINEARMATH__TRANSFORM_HPP_ +#define TF2__LINEARMATH__TRANSFORM_HPP_ + + +#include "Matrix3x3.hpp" +#include "tf2/visibility_control.h" + + +namespace tf2 +{ + +#define TransformData TransformDoubleData + + +/**@brief The Transform class supports rigid transforms with only translation and rotation and no scaling/shear. + *It can be used in combination with Vector3, Quaternion and Matrix3x3 linear algebra classes. */ +class Transform { + + ///Storage for the rotation + Matrix3x3 m_basis; + ///Storage for the translation + Vector3 m_origin; + +public: + + /**@brief No initialization constructor */ + TF2_PUBLIC + Transform() {} + /**@brief Constructor from Quaternion (optional Vector3 ) + * @param q Rotation from quaternion + * @param c Translation from Vector (default 0,0,0) */ + explicit TF2SIMD_FORCE_INLINE Transform(const Quaternion& q, + const Vector3& c = Vector3(tf2Scalar(0), tf2Scalar(0), tf2Scalar(0))) + : m_basis(q), + m_origin(c) + {} + + /**@brief Constructor from Matrix3x3 (optional Vector3) + * @param b Rotation from Matrix + * @param c Translation from Vector default (0,0,0)*/ + explicit TF2SIMD_FORCE_INLINE Transform(const Matrix3x3& b, + const Vector3& c = Vector3(tf2Scalar(0), tf2Scalar(0), tf2Scalar(0))) + : m_basis(b), + m_origin(c) + {} + /**@brief Copy constructor */ + TF2SIMD_FORCE_INLINE Transform (const Transform& other) + : m_basis(other.m_basis), + m_origin(other.m_origin) + { + } + /**@brief Assignment Operator */ + TF2SIMD_FORCE_INLINE Transform& operator=(const Transform& other) + { + m_basis = other.m_basis; + m_origin = other.m_origin; + return *this; + } + + /**@brief Set the current transform as the value of the product of two transforms + * @param t1 Transform 1 + * @param t2 Transform 2 + * This = Transform1 * Transform2 */ + TF2SIMD_FORCE_INLINE void mult(const Transform& t1, const Transform& t2) { + m_basis = t1.m_basis * t2.m_basis; + m_origin = t1(t2.m_origin); + } + +/* void multInverseLeft(const Transform& t1, const Transform& t2) { + Vector3 v = t2.m_origin - t1.m_origin; + m_basis = tf2MultTransposeLeft(t1.m_basis, t2.m_basis); + m_origin = v * t1.m_basis; + } + */ + +/**@brief Return the transform of the vector */ + TF2SIMD_FORCE_INLINE Vector3 operator()(const Vector3& x) const + { + return Vector3(m_basis[0].dot(x) + m_origin.x(), + m_basis[1].dot(x) + m_origin.y(), + m_basis[2].dot(x) + m_origin.z()); + } + + /**@brief Return the transform of the vector */ + TF2SIMD_FORCE_INLINE Vector3 operator*(const Vector3& x) const + { + return (*this)(x); + } + + /**@brief Return the transform of the Quaternion */ + TF2SIMD_FORCE_INLINE Quaternion operator*(const Quaternion& q) const + { + return getRotation() * q; + } + + /**@brief Return the basis matrix for the rotation */ + TF2SIMD_FORCE_INLINE Matrix3x3& getBasis() { return m_basis; } + /**@brief Return the basis matrix for the rotation */ + TF2SIMD_FORCE_INLINE const Matrix3x3& getBasis() const { return m_basis; } + + /**@brief Return the origin vector translation */ + TF2SIMD_FORCE_INLINE Vector3& getOrigin() { return m_origin; } + /**@brief Return the origin vector translation */ + TF2SIMD_FORCE_INLINE const Vector3& getOrigin() const { return m_origin; } + + /**@brief Return a quaternion representing the rotation */ + TF2_PUBLIC + Quaternion getRotation() const { + Quaternion q; + m_basis.getRotation(q); + return q; + } + + + /**@brief Set from an array + * @param m A pointer to a 15 element array (12 rotation(row major padded on the right by 1), and 3 translation */ + TF2_PUBLIC + void setFromOpenGLMatrix(const tf2Scalar *m) + { + m_basis.setFromOpenGLSubMatrix(m); + m_origin.setValue(m[12],m[13],m[14]); + } + + /**@brief Fill an array representation + * @param m A pointer to a 15 element array (12 rotation(row major padded on the right by 1), and 3 translation */ + TF2_PUBLIC + void getOpenGLMatrix(tf2Scalar *m) const + { + m_basis.getOpenGLSubMatrix(m); + m[12] = m_origin.x(); + m[13] = m_origin.y(); + m[14] = m_origin.z(); + m[15] = tf2Scalar(1.0); + } + + /**@brief Set the translational element + * @param origin The vector to set the translation to */ + TF2SIMD_FORCE_INLINE void setOrigin(const Vector3& origin) + { + m_origin = origin; + } + + TF2SIMD_FORCE_INLINE Vector3 invXform(const Vector3& inVec) const; + + + /**@brief Set the rotational element by Matrix3x3 */ + TF2SIMD_FORCE_INLINE void setBasis(const Matrix3x3& basis) + { + m_basis = basis; + } + + /**@brief Set the rotational element by Quaternion */ + TF2SIMD_FORCE_INLINE void setRotation(const Quaternion& q) + { + m_basis.setRotation(q); + } + + + /**@brief Set this transformation to the identity */ + TF2_PUBLIC + void setIdentity() + { + m_basis.setIdentity(); + m_origin.setValue(tf2Scalar(0.0), tf2Scalar(0.0), tf2Scalar(0.0)); + } + + /**@brief Multiply this Transform by another(this = this * another) + * @param t The other transform */ + TF2_PUBLIC + Transform& operator*=(const Transform& t) + { + m_origin += m_basis * t.m_origin; + m_basis *= t.m_basis; + return *this; + } + + /**@brief Return the inverse of this transform */ + TF2_PUBLIC + Transform inverse() const + { + Matrix3x3 inv = m_basis.transpose(); + return Transform(inv, inv * -m_origin); + } + + /**@brief Return the inverse of this transform times the other transform + * @param t The other transform + * return this.inverse() * the other */ + TF2_PUBLIC + Transform inverseTimes(const Transform& t) const; + + /**@brief Return the product of this transform and the other */ + TF2_PUBLIC + Transform operator*(const Transform& t) const; + + /**@brief Return an identity transform */ + TF2_PUBLIC + static const Transform& getIdentity() + { + static const Transform identityTransform(Matrix3x3::getIdentity()); + return identityTransform; + } + + TF2_PUBLIC + void serialize(struct TransformData& dataOut) const; + + TF2_PUBLIC + void serializeFloat(struct TransformFloatData& dataOut) const; + + TF2_PUBLIC + void deSerialize(const struct TransformData& dataIn); + + TF2_PUBLIC + void deSerializeDouble(const struct TransformDoubleData& dataIn); + + TF2_PUBLIC + void deSerializeFloat(const struct TransformFloatData& dataIn); + +}; + + +TF2SIMD_FORCE_INLINE Vector3 +Transform::invXform(const Vector3& inVec) const +{ + Vector3 v = inVec - m_origin; + return (m_basis.transpose() * v); +} + +TF2SIMD_FORCE_INLINE Transform +Transform::inverseTimes(const Transform& t) const +{ + Vector3 v = t.getOrigin() - m_origin; + return Transform(m_basis.transposeTimes(t.m_basis), + v * m_basis); +} + +TF2SIMD_FORCE_INLINE Transform +Transform::operator*(const Transform& t) const +{ + return Transform(m_basis * t.m_basis, + (*this)(t.m_origin)); +} + +/**@brief Test if two transforms have all elements equal */ +TF2SIMD_FORCE_INLINE bool operator==(const Transform& t1, const Transform& t2) +{ + return ( t1.getBasis() == t2.getBasis() && + t1.getOrigin() == t2.getOrigin() ); +} + + +///for serialization +struct TransformFloatData +{ + Matrix3x3FloatData m_basis; + Vector3FloatData m_origin; +}; + +struct TransformDoubleData +{ + Matrix3x3DoubleData m_basis; + Vector3DoubleData m_origin; +}; + + + +TF2SIMD_FORCE_INLINE void Transform::serialize(TransformData& dataOut) const +{ + m_basis.serialize(dataOut.m_basis); + m_origin.serialize(dataOut.m_origin); +} + +TF2SIMD_FORCE_INLINE void Transform::serializeFloat(TransformFloatData& dataOut) const +{ + m_basis.serializeFloat(dataOut.m_basis); + m_origin.serializeFloat(dataOut.m_origin); +} + + +TF2SIMD_FORCE_INLINE void Transform::deSerialize(const TransformData& dataIn) +{ + m_basis.deSerialize(dataIn.m_basis); + m_origin.deSerialize(dataIn.m_origin); +} + +TF2SIMD_FORCE_INLINE void Transform::deSerializeFloat(const TransformFloatData& dataIn) +{ + m_basis.deSerializeFloat(dataIn.m_basis); + m_origin.deSerializeFloat(dataIn.m_origin); +} + +TF2SIMD_FORCE_INLINE void Transform::deSerializeDouble(const TransformDoubleData& dataIn) +{ + m_basis.deSerializeDouble(dataIn.m_basis); + m_origin.deSerializeDouble(dataIn.m_origin); +} + +} + +#endif // TF2__LINEARMATH__TRANSFORM_HPP_ diff --git a/tf2/include/tf2/LinearMath/Vector3.h b/tf2/include/tf2/LinearMath/Vector3.h index 81e7883a6..46e09f40b 100644 --- a/tf2/include/tf2/LinearMath/Vector3.h +++ b/tf2/include/tf2/LinearMath/Vector3.h @@ -13,723 +13,10 @@ subject to the following restrictions: */ +#ifndef TF2__LINEARMATH__VECTOR3_H_ +#define TF2__LINEARMATH__VECTOR3_H_ -#ifndef TF2_VECTOR3_H -#define TF2_VECTOR3_H +#include +#endif // TF2__LINEARMATH__VECTOR3_H_ -#include "Scalar.h" -#include "MinMax.h" -#include "tf2/visibility_control.h" - -namespace tf2 -{ - -#define Vector3Data Vector3DoubleData -#define Vector3DataName "Vector3DoubleData" - - - - -/**@brief tf2::Vector3 can be used to represent 3D points and vectors. - * It has an un-used w component to suit 16-byte alignment when tf2::Vector3 is stored in containers. This extra component can be used by derived classes (Quaternion?) or by user - * Ideally, this class should be replaced by a platform optimized TF2SIMD version that keeps the data in registers - */ -ATTRIBUTE_ALIGNED16(class) Vector3 -{ -public: - -#if defined (__SPU__) && defined (__CELLOS_LV2__) - tf2Scalar m_floats[4]; -public: - TF2SIMD_FORCE_INLINE const vec_float4& get128() const - { - return *((const vec_float4*)&m_floats[0]); - } -public: -#else //__CELLOS_LV2__ __SPU__ -#ifdef TF2_USE_SSE // _WIN32 - union { - __m128 mVec128; - tf2Scalar m_floats[4]; - }; - TF2SIMD_FORCE_INLINE __m128 get128() const - { - return mVec128; - } - TF2SIMD_FORCE_INLINE void set128(__m128 v128) - { - mVec128 = v128; - } -#else - tf2Scalar m_floats[4]; -#endif -#endif //__CELLOS_LV2__ __SPU__ - - public: - - /**@brief No initialization constructor */ - TF2SIMD_FORCE_INLINE Vector3() {} - - - - /**@brief Constructor from scalars - * @param x X value - * @param y Y value - * @param z Z value - */ - TF2SIMD_FORCE_INLINE Vector3(const tf2Scalar& x, const tf2Scalar& y, const tf2Scalar& z) - { - m_floats[0] = x; - m_floats[1] = y; - m_floats[2] = z; - m_floats[3] = tf2Scalar(0.); - } - -/**@brief Add a vector to this one - * @param The vector to add to this one */ - TF2SIMD_FORCE_INLINE Vector3& operator+=(const Vector3& v) - { - - m_floats[0] += v.m_floats[0]; m_floats[1] += v.m_floats[1];m_floats[2] += v.m_floats[2]; - return *this; - } - - - /**@brief Sutf2ract a vector from this one - * @param The vector to sutf2ract */ - TF2SIMD_FORCE_INLINE Vector3& operator-=(const Vector3& v) - { - m_floats[0] -= v.m_floats[0]; m_floats[1] -= v.m_floats[1];m_floats[2] -= v.m_floats[2]; - return *this; - } - /**@brief Scale the vector - * @param s Scale factor */ - TF2SIMD_FORCE_INLINE Vector3& operator*=(const tf2Scalar& s) - { - m_floats[0] *= s; m_floats[1] *= s;m_floats[2] *= s; - return *this; - } - - /**@brief Inversely scale the vector - * @param s Scale factor to divide by */ - TF2SIMD_FORCE_INLINE Vector3& operator/=(const tf2Scalar& s) - { - tf2FullAssert(s != tf2Scalar(0.0)); - return *this *= tf2Scalar(1.0) / s; - } - - /**@brief Return the dot product - * @param v The other vector in the dot product */ - TF2SIMD_FORCE_INLINE tf2Scalar dot(const Vector3& v) const - { - return m_floats[0] * v.m_floats[0] + m_floats[1] * v.m_floats[1] +m_floats[2] * v.m_floats[2]; - } - - /**@brief Return the length of the vector squared */ - TF2SIMD_FORCE_INLINE tf2Scalar length2() const - { - return dot(*this); - } - - /**@brief Return the length of the vector */ - TF2SIMD_FORCE_INLINE tf2Scalar length() const - { - return tf2Sqrt(length2()); - } - - /**@brief Return the distance squared between the ends of this and another vector - * This is symantically treating the vector like a point */ - TF2SIMD_FORCE_INLINE tf2Scalar distance2(const Vector3& v) const; - - /**@brief Return the distance between the ends of this and another vector - * This is symantically treating the vector like a point */ - TF2SIMD_FORCE_INLINE tf2Scalar distance(const Vector3& v) const; - - /**@brief Normalize this vector - * x^2 + y^2 + z^2 = 1 */ - TF2SIMD_FORCE_INLINE Vector3& normalize() - { - return *this /= length(); - } - - /**@brief Return a normalized version of this vector */ - TF2SIMD_FORCE_INLINE Vector3 normalized() const; - - /**@brief Rotate this vector - * @param wAxis The axis to rotate about - * @param angle The angle to rotate by */ - TF2SIMD_FORCE_INLINE Vector3 rotate( const Vector3& wAxis, const tf2Scalar angle ) const; - - /**@brief Return the angle between this and another vector - * @param v The other vector */ - TF2SIMD_FORCE_INLINE tf2Scalar angle(const Vector3& v) const - { - tf2Scalar s = tf2Sqrt(length2() * v.length2()); - tf2FullAssert(s != tf2Scalar(0.0)); - return tf2Acos(dot(v) / s); - } - /**@brief Return a vector will the absolute values of each element */ - TF2SIMD_FORCE_INLINE Vector3 absolute() const - { - return Vector3( - tf2Fabs(m_floats[0]), - tf2Fabs(m_floats[1]), - tf2Fabs(m_floats[2])); - } - /**@brief Return the cross product between this and another vector - * @param v The other vector */ - TF2SIMD_FORCE_INLINE Vector3 cross(const Vector3& v) const - { - return Vector3( - m_floats[1] * v.m_floats[2] -m_floats[2] * v.m_floats[1], - m_floats[2] * v.m_floats[0] - m_floats[0] * v.m_floats[2], - m_floats[0] * v.m_floats[1] - m_floats[1] * v.m_floats[0]); - } - - TF2SIMD_FORCE_INLINE tf2Scalar triple(const Vector3& v1, const Vector3& v2) const - { - return m_floats[0] * (v1.m_floats[1] * v2.m_floats[2] - v1.m_floats[2] * v2.m_floats[1]) + - m_floats[1] * (v1.m_floats[2] * v2.m_floats[0] - v1.m_floats[0] * v2.m_floats[2]) + - m_floats[2] * (v1.m_floats[0] * v2.m_floats[1] - v1.m_floats[1] * v2.m_floats[0]); - } - - /**@brief Return the axis with the smallest value - * Note return values are 0,1,2 for x, y, or z */ - TF2SIMD_FORCE_INLINE int minAxis() const - { - return m_floats[0] < m_floats[1] ? (m_floats[0] return this, t=1 => return other) */ - TF2SIMD_FORCE_INLINE Vector3 lerp(const Vector3& v, const tf2Scalar& t) const - { - return Vector3(m_floats[0] + (v.m_floats[0] - m_floats[0]) * t, - m_floats[1] + (v.m_floats[1] - m_floats[1]) * t, - m_floats[2] + (v.m_floats[2] -m_floats[2]) * t); - } - - /**@brief Elementwise multiply this vector by the other - * @param v The other vector */ - TF2SIMD_FORCE_INLINE Vector3& operator*=(const Vector3& v) - { - m_floats[0] *= v.m_floats[0]; m_floats[1] *= v.m_floats[1];m_floats[2] *= v.m_floats[2]; - return *this; - } - - /**@brief Return the x value */ - TF2SIMD_FORCE_INLINE const tf2Scalar& getX() const { return m_floats[0]; } - /**@brief Return the y value */ - TF2SIMD_FORCE_INLINE const tf2Scalar& getY() const { return m_floats[1]; } - /**@brief Return the z value */ - TF2SIMD_FORCE_INLINE const tf2Scalar& getZ() const { return m_floats[2]; } - /**@brief Set the x value */ - TF2SIMD_FORCE_INLINE void setX(tf2Scalar x) { m_floats[0] = x;}; - /**@brief Set the y value */ - TF2SIMD_FORCE_INLINE void setY(tf2Scalar y) { m_floats[1] = y;}; - /**@brief Set the z value */ - TF2SIMD_FORCE_INLINE void setZ(tf2Scalar z) {m_floats[2] = z;}; - /**@brief Set the w value */ - TF2SIMD_FORCE_INLINE void setW(tf2Scalar w) { m_floats[3] = w;}; - /**@brief Return the x value */ - TF2SIMD_FORCE_INLINE const tf2Scalar& x() const { return m_floats[0]; } - /**@brief Return the y value */ - TF2SIMD_FORCE_INLINE const tf2Scalar& y() const { return m_floats[1]; } - /**@brief Return the z value */ - TF2SIMD_FORCE_INLINE const tf2Scalar& z() const { return m_floats[2]; } - /**@brief Return the w value */ - TF2SIMD_FORCE_INLINE const tf2Scalar& w() const { return m_floats[3]; } - - //TF2SIMD_FORCE_INLINE tf2Scalar& operator[](int i) { return (&m_floats[0])[i]; } - //TF2SIMD_FORCE_INLINE const tf2Scalar& operator[](int i) const { return (&m_floats[0])[i]; } - ///operator tf2Scalar*() replaces operator[], using implicit conversion. We added operator != and operator == to avoid pointer comparisons. - TF2SIMD_FORCE_INLINE operator tf2Scalar *() { return &m_floats[0]; } - TF2SIMD_FORCE_INLINE operator const tf2Scalar *() const { return &m_floats[0]; } - - TF2SIMD_FORCE_INLINE bool operator==(const Vector3& other) const - { - return ((m_floats[3]==other.m_floats[3]) && (m_floats[2]==other.m_floats[2]) && (m_floats[1]==other.m_floats[1]) && (m_floats[0]==other.m_floats[0])); - } - - TF2SIMD_FORCE_INLINE bool operator!=(const Vector3& other) const - { - return !(*this == other); - } - - /**@brief Set each element to the max of the current values and the values of another Vector3 - * @param other The other Vector3 to compare with - */ - TF2SIMD_FORCE_INLINE void setMax(const Vector3& other) - { - tf2SetMax(m_floats[0], other.m_floats[0]); - tf2SetMax(m_floats[1], other.m_floats[1]); - tf2SetMax(m_floats[2], other.m_floats[2]); - tf2SetMax(m_floats[3], other.w()); - } - /**@brief Set each element to the min of the current values and the values of another Vector3 - * @param other The other Vector3 to compare with - */ - TF2SIMD_FORCE_INLINE void setMin(const Vector3& other) - { - tf2SetMin(m_floats[0], other.m_floats[0]); - tf2SetMin(m_floats[1], other.m_floats[1]); - tf2SetMin(m_floats[2], other.m_floats[2]); - tf2SetMin(m_floats[3], other.w()); - } - - TF2SIMD_FORCE_INLINE void setValue(const tf2Scalar& x, const tf2Scalar& y, const tf2Scalar& z) - { - m_floats[0]=x; - m_floats[1]=y; - m_floats[2]=z; - m_floats[3] = tf2Scalar(0.); - } - - TF2_PUBLIC - void getSkewSymmetricMatrix(Vector3* v0,Vector3* v1,Vector3* v2) const - { - v0->setValue(0. ,-z() ,y()); - v1->setValue(z() ,0. ,-x()); - v2->setValue(-y() ,x() ,0.); - } - - TF2_PUBLIC - void setZero() - { - setValue(tf2Scalar(0.),tf2Scalar(0.),tf2Scalar(0.)); - } - - TF2SIMD_FORCE_INLINE bool isZero() const - { - return m_floats[0] == tf2Scalar(0) && m_floats[1] == tf2Scalar(0) && m_floats[2] == tf2Scalar(0); - } - - TF2SIMD_FORCE_INLINE bool fuzzyZero() const - { - return length2() < TF2SIMD_EPSILON; - } - - TF2SIMD_FORCE_INLINE void serialize(struct Vector3Data& dataOut) const; - - TF2SIMD_FORCE_INLINE void deSerialize(const struct Vector3Data& dataIn); - - TF2SIMD_FORCE_INLINE void serializeFloat(struct Vector3FloatData& dataOut) const; - - TF2SIMD_FORCE_INLINE void deSerializeFloat(const struct Vector3FloatData& dataIn); - - TF2SIMD_FORCE_INLINE void serializeDouble(struct Vector3DoubleData& dataOut) const; - - TF2SIMD_FORCE_INLINE void deSerializeDouble(const struct Vector3DoubleData& dataIn); - -}; - -/**@brief Return the sum of two vectors (Point symantics)*/ -TF2SIMD_FORCE_INLINE Vector3 -operator+(const Vector3& v1, const Vector3& v2) -{ - return Vector3(v1.m_floats[0] + v2.m_floats[0], v1.m_floats[1] + v2.m_floats[1], v1.m_floats[2] + v2.m_floats[2]); -} - -/**@brief Return the elementwise product of two vectors */ -TF2SIMD_FORCE_INLINE Vector3 -operator*(const Vector3& v1, const Vector3& v2) -{ - return Vector3(v1.m_floats[0] * v2.m_floats[0], v1.m_floats[1] * v2.m_floats[1], v1.m_floats[2] * v2.m_floats[2]); -} - -/**@brief Return the difference between two vectors */ -TF2SIMD_FORCE_INLINE Vector3 -operator-(const Vector3& v1, const Vector3& v2) -{ - return Vector3(v1.m_floats[0] - v2.m_floats[0], v1.m_floats[1] - v2.m_floats[1], v1.m_floats[2] - v2.m_floats[2]); -} -/**@brief Return the negative of the vector */ -TF2SIMD_FORCE_INLINE Vector3 -operator-(const Vector3& v) -{ - return Vector3(-v.m_floats[0], -v.m_floats[1], -v.m_floats[2]); -} - -/**@brief Return the vector scaled by s */ -TF2SIMD_FORCE_INLINE Vector3 -operator*(const Vector3& v, const tf2Scalar& s) -{ - return Vector3(v.m_floats[0] * s, v.m_floats[1] * s, v.m_floats[2] * s); -} - -/**@brief Return the vector scaled by s */ -TF2SIMD_FORCE_INLINE Vector3 -operator*(const tf2Scalar& s, const Vector3& v) -{ - return v * s; -} - -/**@brief Return the vector inversely scaled by s */ -TF2SIMD_FORCE_INLINE Vector3 -operator/(const Vector3& v, const tf2Scalar& s) -{ - tf2FullAssert(s != tf2Scalar(0.0)); - return v * (tf2Scalar(1.0) / s); -} - -/**@brief Return the vector inversely scaled by s */ -TF2SIMD_FORCE_INLINE Vector3 -operator/(const Vector3& v1, const Vector3& v2) -{ - return Vector3(v1.m_floats[0] / v2.m_floats[0],v1.m_floats[1] / v2.m_floats[1],v1.m_floats[2] / v2.m_floats[2]); -} - -/**@brief Return the dot product between two vectors */ -TF2SIMD_FORCE_INLINE tf2Scalar -tf2Dot(const Vector3& v1, const Vector3& v2) -{ - return v1.dot(v2); -} - - -/**@brief Return the distance squared between two vectors */ -TF2SIMD_FORCE_INLINE tf2Scalar -tf2Distance2(const Vector3& v1, const Vector3& v2) -{ - return v1.distance2(v2); -} - - -/**@brief Return the distance between two vectors */ -TF2SIMD_FORCE_INLINE tf2Scalar -tf2Distance(const Vector3& v1, const Vector3& v2) -{ - return v1.distance(v2); -} - -/**@brief Return the angle between two vectors */ -TF2SIMD_FORCE_INLINE tf2Scalar -tf2Angle(const Vector3& v1, const Vector3& v2) -{ - return v1.angle(v2); -} - -/**@brief Return the cross product of two vectors */ -TF2SIMD_FORCE_INLINE Vector3 -tf2Cross(const Vector3& v1, const Vector3& v2) -{ - return v1.cross(v2); -} - -TF2SIMD_FORCE_INLINE tf2Scalar -tf2Triple(const Vector3& v1, const Vector3& v2, const Vector3& v3) -{ - return v1.triple(v2, v3); -} - -/**@brief Return the linear interpolation between two vectors - * @param v1 One vector - * @param v2 The other vector - * @param t The ration of this to v (t = 0 => return v1, t=1 => return v2) */ -TF2SIMD_FORCE_INLINE Vector3 -lerp(const Vector3& v1, const Vector3& v2, const tf2Scalar& t) -{ - return v1.lerp(v2, t); -} - - - -TF2SIMD_FORCE_INLINE tf2Scalar Vector3::distance2(const Vector3& v) const -{ - return (v - *this).length2(); -} - -TF2SIMD_FORCE_INLINE tf2Scalar Vector3::distance(const Vector3& v) const -{ - return (v - *this).length(); -} - -TF2SIMD_FORCE_INLINE Vector3 Vector3::normalized() const -{ - return *this / length(); -} - -TF2SIMD_FORCE_INLINE Vector3 Vector3::rotate( const Vector3& wAxis, const tf2Scalar angle ) const -{ - // wAxis must be a unit lenght vector - - Vector3 o = wAxis * wAxis.dot( *this ); - Vector3 x = *this - o; - Vector3 y; - - y = wAxis.cross( *this ); - - return ( o + x * tf2Cos( angle ) + y * tf2Sin( angle ) ); -} - -class tf2Vector4 : public Vector3 -{ -public: - - TF2SIMD_FORCE_INLINE tf2Vector4() {} - - - TF2SIMD_FORCE_INLINE tf2Vector4(const tf2Scalar& x, const tf2Scalar& y, const tf2Scalar& z,const tf2Scalar& w) - : Vector3(x,y,z) - { - m_floats[3] = w; - } - - - TF2SIMD_FORCE_INLINE tf2Vector4 absolute4() const - { - return tf2Vector4( - tf2Fabs(m_floats[0]), - tf2Fabs(m_floats[1]), - tf2Fabs(m_floats[2]), - tf2Fabs(m_floats[3])); - } - - - - TF2_PUBLIC - tf2Scalar getW() const { return m_floats[3];} - - - TF2SIMD_FORCE_INLINE int maxAxis4() const - { - int maxIndex = -1; - tf2Scalar maxVal = tf2Scalar(-TF2_LARGE_FLOAT); - if (m_floats[0] > maxVal) - { - maxIndex = 0; - maxVal = m_floats[0]; - } - if (m_floats[1] > maxVal) - { - maxIndex = 1; - maxVal = m_floats[1]; - } - if (m_floats[2] > maxVal) - { - maxIndex = 2; - maxVal =m_floats[2]; - } - if (m_floats[3] > maxVal) - { - maxIndex = 3; - } - - - - - return maxIndex; - - } - - - TF2SIMD_FORCE_INLINE int minAxis4() const - { - int minIndex = -1; - tf2Scalar minVal = tf2Scalar(TF2_LARGE_FLOAT); - if (m_floats[0] < minVal) - { - minIndex = 0; - minVal = m_floats[0]; - } - if (m_floats[1] < minVal) - { - minIndex = 1; - minVal = m_floats[1]; - } - if (m_floats[2] < minVal) - { - minIndex = 2; - minVal =m_floats[2]; - } - if (m_floats[3] < minVal) - { - minIndex = 3; - } - - return minIndex; - - } - - - TF2SIMD_FORCE_INLINE int closestAxis4() const - { - return absolute4().maxAxis4(); - } - - - - - /**@brief Set x,y,z and zero w - * @param x Value of x - * @param y Value of y - * @param z Value of z - */ - - -/* void getValue(tf2Scalar *m) const - { - m[0] = m_floats[0]; - m[1] = m_floats[1]; - m[2] =m_floats[2]; - } -*/ -/**@brief Set the values - * @param x Value of x - * @param y Value of y - * @param z Value of z - * @param w Value of w - */ - TF2SIMD_FORCE_INLINE void setValue(const tf2Scalar& x, const tf2Scalar& y, const tf2Scalar& z,const tf2Scalar& w) - { - m_floats[0]=x; - m_floats[1]=y; - m_floats[2]=z; - m_floats[3]=w; - } - - -}; - - -///tf2SwapVector3Endian swaps vector endianness, useful for network and cross-platform serialization -TF2SIMD_FORCE_INLINE void tf2SwapScalarEndian(const tf2Scalar& sourceVal, tf2Scalar& destVal) -{ - unsigned char* dest = (unsigned char*) &destVal; - const unsigned char* src = reinterpret_cast(&sourceVal); - dest[0] = src[7]; - dest[1] = src[6]; - dest[2] = src[5]; - dest[3] = src[4]; - dest[4] = src[3]; - dest[5] = src[2]; - dest[6] = src[1]; - dest[7] = src[0]; -} -///tf2SwapVector3Endian swaps vector endianness, useful for network and cross-platform serialization -TF2SIMD_FORCE_INLINE void tf2SwapVector3Endian(const Vector3& sourceVec, Vector3& destVec) -{ - for (int i=0;i<4;i++) - { - tf2SwapScalarEndian(sourceVec[i],destVec[i]); - } - -} - -///tf2UnSwapVector3Endian swaps vector endianness, useful for network and cross-platform serialization -TF2SIMD_FORCE_INLINE void tf2UnSwapVector3Endian(Vector3& vector) -{ - - Vector3 swappedVec; - for (int i=0;i<4;i++) - { - tf2SwapScalarEndian(vector[i],swappedVec[i]); - } - vector = swappedVec; -} - -TF2SIMD_FORCE_INLINE void tf2PlaneSpace1 (const Vector3& n, Vector3& p, Vector3& q) -{ - if (tf2Fabs(n.z()) > TF2SIMDSQRT12) { - // choose p in y-z plane - tf2Scalar a = n[1]*n[1] + n[2]*n[2]; - tf2Scalar k = tf2RecipSqrt (a); - p.setValue(0,-n[2]*k,n[1]*k); - // set q = n x p - q.setValue(a*k,-n[0]*p[2],n[0]*p[1]); - } - else { - // choose p in x-y plane - tf2Scalar a = n.x()*n.x() + n.y()*n.y(); - tf2Scalar k = tf2RecipSqrt (a); - p.setValue(-n.y()*k,n.x()*k,0); - // set q = n x p - q.setValue(-n.z()*p.y(),n.z()*p.x(),a*k); - } -} - - -struct Vector3FloatData -{ - float m_floats[4]; -}; - -struct Vector3DoubleData -{ - double m_floats[4]; - -}; - -TF2SIMD_FORCE_INLINE void Vector3::serializeFloat(struct Vector3FloatData& dataOut) const -{ - ///could also do a memcpy, check if it is worth it - for (int i=0;i<4;i++) - dataOut.m_floats[i] = float(m_floats[i]); -} - -TF2SIMD_FORCE_INLINE void Vector3::deSerializeFloat(const struct Vector3FloatData& dataIn) -{ - for (int i=0;i<4;i++) - m_floats[i] = tf2Scalar(dataIn.m_floats[i]); -} - - -TF2SIMD_FORCE_INLINE void Vector3::serializeDouble(struct Vector3DoubleData& dataOut) const -{ - ///could also do a memcpy, check if it is worth it - for (int i=0;i<4;i++) - dataOut.m_floats[i] = double(m_floats[i]); -} - -TF2SIMD_FORCE_INLINE void Vector3::deSerializeDouble(const struct Vector3DoubleData& dataIn) -{ - for (int i=0;i<4;i++) - m_floats[i] = tf2Scalar(dataIn.m_floats[i]); -} - - -TF2SIMD_FORCE_INLINE void Vector3::serialize(struct Vector3Data& dataOut) const -{ - ///could also do a memcpy, check if it is worth it - for (int i=0;i<4;i++) - dataOut.m_floats[i] = m_floats[i]; -} - -TF2SIMD_FORCE_INLINE void Vector3::deSerialize(const struct Vector3Data& dataIn) -{ - for (int i=0;i<4;i++) - m_floats[i] = dataIn.m_floats[i]; -} - -} - -#endif //TF2_VECTOR3_H diff --git a/tf2/include/tf2/LinearMath/Vector3.hpp b/tf2/include/tf2/LinearMath/Vector3.hpp new file mode 100644 index 000000000..10590a276 --- /dev/null +++ b/tf2/include/tf2/LinearMath/Vector3.hpp @@ -0,0 +1,735 @@ +/* +Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +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. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + + +#ifndef TF2__LINEARMATH__VECTOR3_HPP_ +#define TF2__LINEARMATH__VECTOR3_HPP_ + + +#include "Scalar.hpp" +#include "MinMax.hpp" +#include "tf2/visibility_control.h" + +namespace tf2 +{ + +#define Vector3Data Vector3DoubleData +#define Vector3DataName "Vector3DoubleData" + + + + +/**@brief tf2::Vector3 can be used to represent 3D points and vectors. + * It has an un-used w component to suit 16-byte alignment when tf2::Vector3 is stored in containers. This extra component can be used by derived classes (Quaternion?) or by user + * Ideally, this class should be replaced by a platform optimized TF2SIMD version that keeps the data in registers + */ +ATTRIBUTE_ALIGNED16(class) Vector3 +{ +public: + +#if defined (__SPU__) && defined (__CELLOS_LV2__) + tf2Scalar m_floats[4]; +public: + TF2SIMD_FORCE_INLINE const vec_float4& get128() const + { + return *((const vec_float4*)&m_floats[0]); + } +public: +#else //__CELLOS_LV2__ __SPU__ +#ifdef TF2_USE_SSE // _WIN32 + union { + __m128 mVec128; + tf2Scalar m_floats[4]; + }; + TF2SIMD_FORCE_INLINE __m128 get128() const + { + return mVec128; + } + TF2SIMD_FORCE_INLINE void set128(__m128 v128) + { + mVec128 = v128; + } +#else + tf2Scalar m_floats[4]; +#endif +#endif //__CELLOS_LV2__ __SPU__ + + public: + + /**@brief No initialization constructor */ + TF2SIMD_FORCE_INLINE Vector3() {} + + + + /**@brief Constructor from scalars + * @param x X value + * @param y Y value + * @param z Z value + */ + TF2SIMD_FORCE_INLINE Vector3(const tf2Scalar& x, const tf2Scalar& y, const tf2Scalar& z) + { + m_floats[0] = x; + m_floats[1] = y; + m_floats[2] = z; + m_floats[3] = tf2Scalar(0.); + } + +/**@brief Add a vector to this one + * @param The vector to add to this one */ + TF2SIMD_FORCE_INLINE Vector3& operator+=(const Vector3& v) + { + + m_floats[0] += v.m_floats[0]; m_floats[1] += v.m_floats[1];m_floats[2] += v.m_floats[2]; + return *this; + } + + + /**@brief Sutf2ract a vector from this one + * @param The vector to sutf2ract */ + TF2SIMD_FORCE_INLINE Vector3& operator-=(const Vector3& v) + { + m_floats[0] -= v.m_floats[0]; m_floats[1] -= v.m_floats[1];m_floats[2] -= v.m_floats[2]; + return *this; + } + /**@brief Scale the vector + * @param s Scale factor */ + TF2SIMD_FORCE_INLINE Vector3& operator*=(const tf2Scalar& s) + { + m_floats[0] *= s; m_floats[1] *= s;m_floats[2] *= s; + return *this; + } + + /**@brief Inversely scale the vector + * @param s Scale factor to divide by */ + TF2SIMD_FORCE_INLINE Vector3& operator/=(const tf2Scalar& s) + { + tf2FullAssert(s != tf2Scalar(0.0)); + return *this *= tf2Scalar(1.0) / s; + } + + /**@brief Return the dot product + * @param v The other vector in the dot product */ + TF2SIMD_FORCE_INLINE tf2Scalar dot(const Vector3& v) const + { + return m_floats[0] * v.m_floats[0] + m_floats[1] * v.m_floats[1] +m_floats[2] * v.m_floats[2]; + } + + /**@brief Return the length of the vector squared */ + TF2SIMD_FORCE_INLINE tf2Scalar length2() const + { + return dot(*this); + } + + /**@brief Return the length of the vector */ + TF2SIMD_FORCE_INLINE tf2Scalar length() const + { + return tf2Sqrt(length2()); + } + + /**@brief Return the distance squared between the ends of this and another vector + * This is symantically treating the vector like a point */ + TF2SIMD_FORCE_INLINE tf2Scalar distance2(const Vector3& v) const; + + /**@brief Return the distance between the ends of this and another vector + * This is symantically treating the vector like a point */ + TF2SIMD_FORCE_INLINE tf2Scalar distance(const Vector3& v) const; + + /**@brief Normalize this vector + * x^2 + y^2 + z^2 = 1 */ + TF2SIMD_FORCE_INLINE Vector3& normalize() + { + return *this /= length(); + } + + /**@brief Return a normalized version of this vector */ + TF2SIMD_FORCE_INLINE Vector3 normalized() const; + + /**@brief Rotate this vector + * @param wAxis The axis to rotate about + * @param angle The angle to rotate by */ + TF2SIMD_FORCE_INLINE Vector3 rotate( const Vector3& wAxis, const tf2Scalar angle ) const; + + /**@brief Return the angle between this and another vector + * @param v The other vector */ + TF2SIMD_FORCE_INLINE tf2Scalar angle(const Vector3& v) const + { + tf2Scalar s = tf2Sqrt(length2() * v.length2()); + tf2FullAssert(s != tf2Scalar(0.0)); + return tf2Acos(dot(v) / s); + } + /**@brief Return a vector will the absolute values of each element */ + TF2SIMD_FORCE_INLINE Vector3 absolute() const + { + return Vector3( + tf2Fabs(m_floats[0]), + tf2Fabs(m_floats[1]), + tf2Fabs(m_floats[2])); + } + /**@brief Return the cross product between this and another vector + * @param v The other vector */ + TF2SIMD_FORCE_INLINE Vector3 cross(const Vector3& v) const + { + return Vector3( + m_floats[1] * v.m_floats[2] -m_floats[2] * v.m_floats[1], + m_floats[2] * v.m_floats[0] - m_floats[0] * v.m_floats[2], + m_floats[0] * v.m_floats[1] - m_floats[1] * v.m_floats[0]); + } + + TF2SIMD_FORCE_INLINE tf2Scalar triple(const Vector3& v1, const Vector3& v2) const + { + return m_floats[0] * (v1.m_floats[1] * v2.m_floats[2] - v1.m_floats[2] * v2.m_floats[1]) + + m_floats[1] * (v1.m_floats[2] * v2.m_floats[0] - v1.m_floats[0] * v2.m_floats[2]) + + m_floats[2] * (v1.m_floats[0] * v2.m_floats[1] - v1.m_floats[1] * v2.m_floats[0]); + } + + /**@brief Return the axis with the smallest value + * Note return values are 0,1,2 for x, y, or z */ + TF2SIMD_FORCE_INLINE int minAxis() const + { + return m_floats[0] < m_floats[1] ? (m_floats[0] return this, t=1 => return other) */ + TF2SIMD_FORCE_INLINE Vector3 lerp(const Vector3& v, const tf2Scalar& t) const + { + return Vector3(m_floats[0] + (v.m_floats[0] - m_floats[0]) * t, + m_floats[1] + (v.m_floats[1] - m_floats[1]) * t, + m_floats[2] + (v.m_floats[2] -m_floats[2]) * t); + } + + /**@brief Elementwise multiply this vector by the other + * @param v The other vector */ + TF2SIMD_FORCE_INLINE Vector3& operator*=(const Vector3& v) + { + m_floats[0] *= v.m_floats[0]; m_floats[1] *= v.m_floats[1];m_floats[2] *= v.m_floats[2]; + return *this; + } + + /**@brief Return the x value */ + TF2SIMD_FORCE_INLINE const tf2Scalar& getX() const { return m_floats[0]; } + /**@brief Return the y value */ + TF2SIMD_FORCE_INLINE const tf2Scalar& getY() const { return m_floats[1]; } + /**@brief Return the z value */ + TF2SIMD_FORCE_INLINE const tf2Scalar& getZ() const { return m_floats[2]; } + /**@brief Set the x value */ + TF2SIMD_FORCE_INLINE void setX(tf2Scalar x) { m_floats[0] = x;}; + /**@brief Set the y value */ + TF2SIMD_FORCE_INLINE void setY(tf2Scalar y) { m_floats[1] = y;}; + /**@brief Set the z value */ + TF2SIMD_FORCE_INLINE void setZ(tf2Scalar z) {m_floats[2] = z;}; + /**@brief Set the w value */ + TF2SIMD_FORCE_INLINE void setW(tf2Scalar w) { m_floats[3] = w;}; + /**@brief Return the x value */ + TF2SIMD_FORCE_INLINE const tf2Scalar& x() const { return m_floats[0]; } + /**@brief Return the y value */ + TF2SIMD_FORCE_INLINE const tf2Scalar& y() const { return m_floats[1]; } + /**@brief Return the z value */ + TF2SIMD_FORCE_INLINE const tf2Scalar& z() const { return m_floats[2]; } + /**@brief Return the w value */ + TF2SIMD_FORCE_INLINE const tf2Scalar& w() const { return m_floats[3]; } + + //TF2SIMD_FORCE_INLINE tf2Scalar& operator[](int i) { return (&m_floats[0])[i]; } + //TF2SIMD_FORCE_INLINE const tf2Scalar& operator[](int i) const { return (&m_floats[0])[i]; } + ///operator tf2Scalar*() replaces operator[], using implicit conversion. We added operator != and operator == to avoid pointer comparisons. + TF2SIMD_FORCE_INLINE operator tf2Scalar *() { return &m_floats[0]; } + TF2SIMD_FORCE_INLINE operator const tf2Scalar *() const { return &m_floats[0]; } + + TF2SIMD_FORCE_INLINE bool operator==(const Vector3& other) const + { + return ((m_floats[3]==other.m_floats[3]) && (m_floats[2]==other.m_floats[2]) && (m_floats[1]==other.m_floats[1]) && (m_floats[0]==other.m_floats[0])); + } + + TF2SIMD_FORCE_INLINE bool operator!=(const Vector3& other) const + { + return !(*this == other); + } + + /**@brief Set each element to the max of the current values and the values of another Vector3 + * @param other The other Vector3 to compare with + */ + TF2SIMD_FORCE_INLINE void setMax(const Vector3& other) + { + tf2SetMax(m_floats[0], other.m_floats[0]); + tf2SetMax(m_floats[1], other.m_floats[1]); + tf2SetMax(m_floats[2], other.m_floats[2]); + tf2SetMax(m_floats[3], other.w()); + } + /**@brief Set each element to the min of the current values and the values of another Vector3 + * @param other The other Vector3 to compare with + */ + TF2SIMD_FORCE_INLINE void setMin(const Vector3& other) + { + tf2SetMin(m_floats[0], other.m_floats[0]); + tf2SetMin(m_floats[1], other.m_floats[1]); + tf2SetMin(m_floats[2], other.m_floats[2]); + tf2SetMin(m_floats[3], other.w()); + } + + TF2SIMD_FORCE_INLINE void setValue(const tf2Scalar& x, const tf2Scalar& y, const tf2Scalar& z) + { + m_floats[0]=x; + m_floats[1]=y; + m_floats[2]=z; + m_floats[3] = tf2Scalar(0.); + } + + TF2_PUBLIC + void getSkewSymmetricMatrix(Vector3* v0,Vector3* v1,Vector3* v2) const + { + v0->setValue(0. ,-z() ,y()); + v1->setValue(z() ,0. ,-x()); + v2->setValue(-y() ,x() ,0.); + } + + TF2_PUBLIC + void setZero() + { + setValue(tf2Scalar(0.),tf2Scalar(0.),tf2Scalar(0.)); + } + + TF2SIMD_FORCE_INLINE bool isZero() const + { + return m_floats[0] == tf2Scalar(0) && m_floats[1] == tf2Scalar(0) && m_floats[2] == tf2Scalar(0); + } + + TF2SIMD_FORCE_INLINE bool fuzzyZero() const + { + return length2() < TF2SIMD_EPSILON; + } + + TF2SIMD_FORCE_INLINE void serialize(struct Vector3Data& dataOut) const; + + TF2SIMD_FORCE_INLINE void deSerialize(const struct Vector3Data& dataIn); + + TF2SIMD_FORCE_INLINE void serializeFloat(struct Vector3FloatData& dataOut) const; + + TF2SIMD_FORCE_INLINE void deSerializeFloat(const struct Vector3FloatData& dataIn); + + TF2SIMD_FORCE_INLINE void serializeDouble(struct Vector3DoubleData& dataOut) const; + + TF2SIMD_FORCE_INLINE void deSerializeDouble(const struct Vector3DoubleData& dataIn); + +}; + +/**@brief Return the sum of two vectors (Point symantics)*/ +TF2SIMD_FORCE_INLINE Vector3 +operator+(const Vector3& v1, const Vector3& v2) +{ + return Vector3(v1.m_floats[0] + v2.m_floats[0], v1.m_floats[1] + v2.m_floats[1], v1.m_floats[2] + v2.m_floats[2]); +} + +/**@brief Return the elementwise product of two vectors */ +TF2SIMD_FORCE_INLINE Vector3 +operator*(const Vector3& v1, const Vector3& v2) +{ + return Vector3(v1.m_floats[0] * v2.m_floats[0], v1.m_floats[1] * v2.m_floats[1], v1.m_floats[2] * v2.m_floats[2]); +} + +/**@brief Return the difference between two vectors */ +TF2SIMD_FORCE_INLINE Vector3 +operator-(const Vector3& v1, const Vector3& v2) +{ + return Vector3(v1.m_floats[0] - v2.m_floats[0], v1.m_floats[1] - v2.m_floats[1], v1.m_floats[2] - v2.m_floats[2]); +} +/**@brief Return the negative of the vector */ +TF2SIMD_FORCE_INLINE Vector3 +operator-(const Vector3& v) +{ + return Vector3(-v.m_floats[0], -v.m_floats[1], -v.m_floats[2]); +} + +/**@brief Return the vector scaled by s */ +TF2SIMD_FORCE_INLINE Vector3 +operator*(const Vector3& v, const tf2Scalar& s) +{ + return Vector3(v.m_floats[0] * s, v.m_floats[1] * s, v.m_floats[2] * s); +} + +/**@brief Return the vector scaled by s */ +TF2SIMD_FORCE_INLINE Vector3 +operator*(const tf2Scalar& s, const Vector3& v) +{ + return v * s; +} + +/**@brief Return the vector inversely scaled by s */ +TF2SIMD_FORCE_INLINE Vector3 +operator/(const Vector3& v, const tf2Scalar& s) +{ + tf2FullAssert(s != tf2Scalar(0.0)); + return v * (tf2Scalar(1.0) / s); +} + +/**@brief Return the vector inversely scaled by s */ +TF2SIMD_FORCE_INLINE Vector3 +operator/(const Vector3& v1, const Vector3& v2) +{ + return Vector3(v1.m_floats[0] / v2.m_floats[0],v1.m_floats[1] / v2.m_floats[1],v1.m_floats[2] / v2.m_floats[2]); +} + +/**@brief Return the dot product between two vectors */ +TF2SIMD_FORCE_INLINE tf2Scalar +tf2Dot(const Vector3& v1, const Vector3& v2) +{ + return v1.dot(v2); +} + + +/**@brief Return the distance squared between two vectors */ +TF2SIMD_FORCE_INLINE tf2Scalar +tf2Distance2(const Vector3& v1, const Vector3& v2) +{ + return v1.distance2(v2); +} + + +/**@brief Return the distance between two vectors */ +TF2SIMD_FORCE_INLINE tf2Scalar +tf2Distance(const Vector3& v1, const Vector3& v2) +{ + return v1.distance(v2); +} + +/**@brief Return the angle between two vectors */ +TF2SIMD_FORCE_INLINE tf2Scalar +tf2Angle(const Vector3& v1, const Vector3& v2) +{ + return v1.angle(v2); +} + +/**@brief Return the cross product of two vectors */ +TF2SIMD_FORCE_INLINE Vector3 +tf2Cross(const Vector3& v1, const Vector3& v2) +{ + return v1.cross(v2); +} + +TF2SIMD_FORCE_INLINE tf2Scalar +tf2Triple(const Vector3& v1, const Vector3& v2, const Vector3& v3) +{ + return v1.triple(v2, v3); +} + +/**@brief Return the linear interpolation between two vectors + * @param v1 One vector + * @param v2 The other vector + * @param t The ration of this to v (t = 0 => return v1, t=1 => return v2) */ +TF2SIMD_FORCE_INLINE Vector3 +lerp(const Vector3& v1, const Vector3& v2, const tf2Scalar& t) +{ + return v1.lerp(v2, t); +} + + + +TF2SIMD_FORCE_INLINE tf2Scalar Vector3::distance2(const Vector3& v) const +{ + return (v - *this).length2(); +} + +TF2SIMD_FORCE_INLINE tf2Scalar Vector3::distance(const Vector3& v) const +{ + return (v - *this).length(); +} + +TF2SIMD_FORCE_INLINE Vector3 Vector3::normalized() const +{ + return *this / length(); +} + +TF2SIMD_FORCE_INLINE Vector3 Vector3::rotate( const Vector3& wAxis, const tf2Scalar angle ) const +{ + // wAxis must be a unit lenght vector + + Vector3 o = wAxis * wAxis.dot( *this ); + Vector3 x = *this - o; + Vector3 y; + + y = wAxis.cross( *this ); + + return ( o + x * tf2Cos( angle ) + y * tf2Sin( angle ) ); +} + +class tf2Vector4 : public Vector3 +{ +public: + + TF2SIMD_FORCE_INLINE tf2Vector4() {} + + + TF2SIMD_FORCE_INLINE tf2Vector4(const tf2Scalar& x, const tf2Scalar& y, const tf2Scalar& z,const tf2Scalar& w) + : Vector3(x,y,z) + { + m_floats[3] = w; + } + + + TF2SIMD_FORCE_INLINE tf2Vector4 absolute4() const + { + return tf2Vector4( + tf2Fabs(m_floats[0]), + tf2Fabs(m_floats[1]), + tf2Fabs(m_floats[2]), + tf2Fabs(m_floats[3])); + } + + + + TF2_PUBLIC + tf2Scalar getW() const { return m_floats[3];} + + + TF2SIMD_FORCE_INLINE int maxAxis4() const + { + int maxIndex = -1; + tf2Scalar maxVal = tf2Scalar(-TF2_LARGE_FLOAT); + if (m_floats[0] > maxVal) + { + maxIndex = 0; + maxVal = m_floats[0]; + } + if (m_floats[1] > maxVal) + { + maxIndex = 1; + maxVal = m_floats[1]; + } + if (m_floats[2] > maxVal) + { + maxIndex = 2; + maxVal =m_floats[2]; + } + if (m_floats[3] > maxVal) + { + maxIndex = 3; + } + + + + + return maxIndex; + + } + + + TF2SIMD_FORCE_INLINE int minAxis4() const + { + int minIndex = -1; + tf2Scalar minVal = tf2Scalar(TF2_LARGE_FLOAT); + if (m_floats[0] < minVal) + { + minIndex = 0; + minVal = m_floats[0]; + } + if (m_floats[1] < minVal) + { + minIndex = 1; + minVal = m_floats[1]; + } + if (m_floats[2] < minVal) + { + minIndex = 2; + minVal =m_floats[2]; + } + if (m_floats[3] < minVal) + { + minIndex = 3; + } + + return minIndex; + + } + + + TF2SIMD_FORCE_INLINE int closestAxis4() const + { + return absolute4().maxAxis4(); + } + + + + + /**@brief Set x,y,z and zero w + * @param x Value of x + * @param y Value of y + * @param z Value of z + */ + + +/* void getValue(tf2Scalar *m) const + { + m[0] = m_floats[0]; + m[1] = m_floats[1]; + m[2] =m_floats[2]; + } +*/ +/**@brief Set the values + * @param x Value of x + * @param y Value of y + * @param z Value of z + * @param w Value of w + */ + TF2SIMD_FORCE_INLINE void setValue(const tf2Scalar& x, const tf2Scalar& y, const tf2Scalar& z,const tf2Scalar& w) + { + m_floats[0]=x; + m_floats[1]=y; + m_floats[2]=z; + m_floats[3]=w; + } + + +}; + + +///tf2SwapVector3Endian swaps vector endianness, useful for network and cross-platform serialization +TF2SIMD_FORCE_INLINE void tf2SwapScalarEndian(const tf2Scalar& sourceVal, tf2Scalar& destVal) +{ + unsigned char* dest = (unsigned char*) &destVal; + const unsigned char* src = reinterpret_cast(&sourceVal); + dest[0] = src[7]; + dest[1] = src[6]; + dest[2] = src[5]; + dest[3] = src[4]; + dest[4] = src[3]; + dest[5] = src[2]; + dest[6] = src[1]; + dest[7] = src[0]; +} +///tf2SwapVector3Endian swaps vector endianness, useful for network and cross-platform serialization +TF2SIMD_FORCE_INLINE void tf2SwapVector3Endian(const Vector3& sourceVec, Vector3& destVec) +{ + for (int i=0;i<4;i++) + { + tf2SwapScalarEndian(sourceVec[i],destVec[i]); + } + +} + +///tf2UnSwapVector3Endian swaps vector endianness, useful for network and cross-platform serialization +TF2SIMD_FORCE_INLINE void tf2UnSwapVector3Endian(Vector3& vector) +{ + + Vector3 swappedVec; + for (int i=0;i<4;i++) + { + tf2SwapScalarEndian(vector[i],swappedVec[i]); + } + vector = swappedVec; +} + +TF2SIMD_FORCE_INLINE void tf2PlaneSpace1 (const Vector3& n, Vector3& p, Vector3& q) +{ + if (tf2Fabs(n.z()) > TF2SIMDSQRT12) { + // choose p in y-z plane + tf2Scalar a = n[1]*n[1] + n[2]*n[2]; + tf2Scalar k = tf2RecipSqrt (a); + p.setValue(0,-n[2]*k,n[1]*k); + // set q = n x p + q.setValue(a*k,-n[0]*p[2],n[0]*p[1]); + } + else { + // choose p in x-y plane + tf2Scalar a = n.x()*n.x() + n.y()*n.y(); + tf2Scalar k = tf2RecipSqrt (a); + p.setValue(-n.y()*k,n.x()*k,0); + // set q = n x p + q.setValue(-n.z()*p.y(),n.z()*p.x(),a*k); + } +} + + +struct Vector3FloatData +{ + float m_floats[4]; +}; + +struct Vector3DoubleData +{ + double m_floats[4]; + +}; + +TF2SIMD_FORCE_INLINE void Vector3::serializeFloat(struct Vector3FloatData& dataOut) const +{ + ///could also do a memcpy, check if it is worth it + for (int i=0;i<4;i++) + dataOut.m_floats[i] = float(m_floats[i]); +} + +TF2SIMD_FORCE_INLINE void Vector3::deSerializeFloat(const struct Vector3FloatData& dataIn) +{ + for (int i=0;i<4;i++) + m_floats[i] = tf2Scalar(dataIn.m_floats[i]); +} + + +TF2SIMD_FORCE_INLINE void Vector3::serializeDouble(struct Vector3DoubleData& dataOut) const +{ + ///could also do a memcpy, check if it is worth it + for (int i=0;i<4;i++) + dataOut.m_floats[i] = double(m_floats[i]); +} + +TF2SIMD_FORCE_INLINE void Vector3::deSerializeDouble(const struct Vector3DoubleData& dataIn) +{ + for (int i=0;i<4;i++) + m_floats[i] = tf2Scalar(dataIn.m_floats[i]); +} + + +TF2SIMD_FORCE_INLINE void Vector3::serialize(struct Vector3Data& dataOut) const +{ + ///could also do a memcpy, check if it is worth it + for (int i=0;i<4;i++) + dataOut.m_floats[i] = m_floats[i]; +} + +TF2SIMD_FORCE_INLINE void Vector3::deSerialize(const struct Vector3Data& dataIn) +{ + for (int i=0;i<4;i++) + m_floats[i] = dataIn.m_floats[i]; +} + +} + +#endif // TF2__LINEARMATH__VECTOR3_HPP_ diff --git a/tf2/include/tf2/buffer_core.h b/tf2/include/tf2/buffer_core.h index 0c949e944..5652b6a96 100644 --- a/tf2/include/tf2/buffer_core.h +++ b/tf2/include/tf2/buffer_core.h @@ -32,436 +32,6 @@ #ifndef TF2__BUFFER_CORE_H_ #define TF2__BUFFER_CORE_H_ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "LinearMath/Transform.h" -#include "geometry_msgs/msg/transform_stamped.hpp" -#include "geometry_msgs/msg/velocity_stamped.hpp" -#include "rcutils/logging_macros.h" -#include "tf2/buffer_core_interface.h" -#include "tf2/exceptions.h" -#include "tf2/transform_storage.h" -#include "tf2/visibility_control.h" - -namespace tf2 -{ - -typedef std::pair P_TimeAndFrameID; -typedef uint64_t TransformableRequestHandle; - -class TimeCacheInterface; -using TimeCacheInterfacePtr = std::shared_ptr; - -enum TransformableResult -{ - TransformAvailable, - TransformFailure, -}; - -//!< The default amount of time to cache data in seconds -static constexpr Duration BUFFER_CORE_DEFAULT_CACHE_TIME = std::chrono::seconds(10); - -/** \brief A Class which provides coordinate transforms between any two frames in a system. - * - * This class provides a simple interface to allow recording and lookup of - * relationships between arbitrary frames of the system. - * - * libTF assumes that there is a tree of coordinate frame transforms which define the relationship between all coordinate frames. - * For example your typical robot would have a transform from global to real world. And then from base to hand, and from base to head. - * But Base to Hand really is composed of base to shoulder to elbow to wrist to hand. - * libTF is designed to take care of all the intermediate steps for you. - * - * Internal Representation - * libTF will store frames with the parameters necessary for generating the transform into that frame from it's parent and a reference to the parent frame. - * Frames are designated using an std::string - * 0 is a frame without a parent (the top of a tree) - * The positions of frames over time must be pushed in. - * - * All function calls which pass frame ids can potentially throw the exception tf::LookupException - */ -class BufferCore : public BufferCoreInterface -{ -public: - /************* Constants ***********************/ - //!< Maximum graph search depth (deeper graphs will be assumed to have loops) - TF2_PUBLIC - static const uint32_t MAX_GRAPH_DEPTH = 1000UL; - - /** Constructor - * \param cache_time How long to keep a history of transforms in nanoseconds - * - */ - TF2_PUBLIC - explicit BufferCore(tf2::Duration cache_time = BUFFER_CORE_DEFAULT_CACHE_TIME); - - TF2_PUBLIC - virtual ~BufferCore(void); - - /** \brief Clear all data */ - TF2_PUBLIC - void clear() override; - - /** \brief Add transform information to the tf data structure - * \param transform The transform to store - * \param authority The source of the information for this transform - * \param is_static Record this transform as a static transform. It will be good across all time. (This cannot be changed after the first call.) - * \return True unless an error occured - */ - TF2_PUBLIC - bool setTransform( - const geometry_msgs::msg::TransformStamped & transform, - const std::string & authority, bool is_static = false); - - /*********** Accessors *************/ - - /** \brief Get the transform between two frames by frame ID. - * \param target_frame The frame to which data should be transformed - * \param source_frame The frame where the data originated - * \param time The time at which the value of the transform is desired. (0 will get the latest) - * \return The transform between the frames - * - * Possible exceptions tf2::LookupException, tf2::ConnectivityException, - * tf2::ExtrapolationException, tf2::InvalidArgumentException - */ - TF2_PUBLIC - geometry_msgs::msg::TransformStamped - lookupTransform( - const std::string & target_frame, const std::string & source_frame, - const TimePoint & time) const override; - - /** \brief Get the transform between two frames by frame ID assuming fixed frame. - * \param target_frame The frame to which data should be transformed - * \param target_time The time to which the data should be transformed. (0 will get the latest) - * \param source_frame The frame where the data originated - * \param source_time The time at which the source_frame should be evaluated. (0 will get the latest) - * \param fixed_frame The frame in which to assume the transform is constant in time. - * \return The transform between the frames - * - * Possible exceptions tf2::LookupException, tf2::ConnectivityException, - * tf2::ExtrapolationException, tf2::InvalidArgumentException - */ - - TF2_PUBLIC - geometry_msgs::msg::TransformStamped - lookupTransform( - const std::string & target_frame, const TimePoint & target_time, - const std::string & source_frame, const TimePoint & source_time, - const std::string & fixed_frame) const override; - - TF2_PUBLIC - geometry_msgs::msg::VelocityStamped lookupVelocity( - const std::string & tracking_frame, const std::string & observation_frame, - const TimePoint & time, const tf2::Duration & averaging_interval) const; - - /** \brief Lookup the velocity of the moving_frame in the reference_frame - * \param reference_frame The frame in which to track - * \param moving_frame The frame to track - * \param time The time at which to get the velocity - * \param duration The period over which to average - * \param velocity The velocity output - * - * Possible exceptions TransformReference::LookupException, TransformReference::ConnectivityException, - * TransformReference::MaxDepthException - */ - TF2_PUBLIC - geometry_msgs::msg::VelocityStamped lookupVelocity( - const std::string & tracking_frame, const std::string & observation_frame, - const std::string & reference_frame, const tf2::Vector3 & reference_point, - const std::string & reference_point_frame, - const TimePoint & time, const tf2::Duration & duration) const; - - /** \brief Test if a transform is possible - * \param target_frame The frame into which to transform - * \param source_frame The frame from which to transform - * \param time The time at which to transform - * \param error_msg A pointer to a string which will be filled with why the transform failed, if not nullptr - * \return True if the transform is possible, false otherwise - */ - TF2_PUBLIC - bool canTransform( - const std::string & target_frame, const std::string & source_frame, - const TimePoint & time, std::string * error_msg = nullptr) const override; - - /** \brief Test if a transform is possible - * \param target_frame The frame into which to transform - * \param target_time The time into which to transform - * \param source_frame The frame from which to transform - * \param source_time The time from which to transform - * \param fixed_frame The frame in which to treat the transform as constant in time - * \param error_msg A pointer to a string which will be filled with why the transform failed, if not nullptr - * \return True if the transform is possible, false otherwise - */ - TF2_PUBLIC - bool canTransform( - const std::string & target_frame, const TimePoint & target_time, - const std::string & source_frame, const TimePoint & source_time, - const std::string & fixed_frame, std::string * error_msg = nullptr) const override; - - /** \brief Get all frames that exist in the system. - */ - TF2_PUBLIC - std::vector getAllFrameNames() const override; - - /** \brief A way to see what frames have been cached in yaml format - * Useful for debugging tools - */ - TF2_PUBLIC - std::string allFramesAsYAML(TimePoint current_time) const; - - /** Backwards compatibility for #84 - */ - TF2_PUBLIC - std::string allFramesAsYAML() const; - - /** \brief A way to see what frames have been cached - * Useful for debugging - */ - TF2_PUBLIC - std::string allFramesAsString() const; - - using TransformableCallback = std::function< - void (TransformableRequestHandle request_handle, const std::string & target_frame, - const std::string & source_frame, - TimePoint time, TransformableResult result)>; - - /// \brief Internal use only - TF2_PUBLIC - TransformableRequestHandle addTransformableRequest( - const TransformableCallback & cb, - const std::string & target_frame, - const std::string & source_frame, - TimePoint time); - /// \brief Internal use only - TF2_PUBLIC - void cancelTransformableRequest(TransformableRequestHandle handle); - - - // Tell the buffer that there are multiple threads servicing it. - // This is useful for derived classes to know if they can block or not. - TF2_PUBLIC - void setUsingDedicatedThread(bool value) {using_dedicated_thread_ = value;} - // Get the state of using_dedicated_thread_ - TF2_PUBLIC - bool isUsingDedicatedThread() const {return using_dedicated_thread_;} - - - /* Backwards compatability section for tf::Transformer you should not use these - */ - - /**@brief Check if a frame exists in the tree - * @param frame_id_str The frame id in question */ - TF2_PUBLIC - bool _frameExists(const std::string & frame_id_str) const; - - /**@brief Fill the parent of a frame. - * @param frame_id The frame id of the frame in question - * @param time The timepoint of the frame in question - * @param parent The reference to the string to fill the parent - * Returns true unless "NO_PARENT" */ - TF2_PUBLIC - bool _getParent(const std::string & frame_id, TimePoint time, std::string & parent) const; - - /** \brief A way to get a std::vector of available frame ids */ - TF2_PUBLIC - void _getFrameStrings(std::vector & ids) const; - - - TF2_PUBLIC - CompactFrameID _lookupFrameNumber(const std::string & frameid_str) const - { - return lookupFrameNumber(frameid_str); - } - TF2_PUBLIC - CompactFrameID _lookupOrInsertFrameNumber(const std::string & frameid_str) - { - return lookupOrInsertFrameNumber(frameid_str); - } - - TF2_PUBLIC - tf2::TF2Error _getLatestCommonTime( - CompactFrameID target_frame, CompactFrameID source_frame, - TimePoint & time, std::string * error_string) const - { - std::unique_lock lock(frame_mutex_); - return getLatestCommonTime(target_frame, source_frame, time, error_string); - } - - TF2_PUBLIC - CompactFrameID _validateFrameId( - const char * function_name_arg, - const std::string & frame_id) const - { - return validateFrameId(function_name_arg, frame_id); - } - - /**@brief Get the duration over which this transformer will cache */ - TF2_PUBLIC - tf2::Duration getCacheLength() {return cache_time_;} - - /** \brief Backwards compatabilityA way to see what frames have been cached - * Useful for debugging - */ - TF2_PUBLIC - std::string _allFramesAsDot(TimePoint current_time) const; - TF2_PUBLIC - std::string _allFramesAsDot() const; - - /** \brief Backwards compatabilityA way to see what frames are in a chain - * Useful for debugging - */ - TF2_PUBLIC - void _chainAsVector( - const std::string & target_frame, TimePoint target_time, - const std::string & source_frame, TimePoint source_time, - const std::string & fixed_frame, - std::vector & output) const; - -private: - /******************** Internal Storage ****************/ - - /** \brief The pointers to potential frames that the tree can be made of. - * The frames will be dynamically allocated at run time when set the first time. */ - typedef std::vector V_TimeCacheInterface; - V_TimeCacheInterface frames_; - - /** \brief A mutex to protect testing and allocating new frames on the above vector. */ - mutable std::mutex frame_mutex_; - - /** \brief A map from string frame ids to CompactFrameID */ - typedef std::unordered_map M_StringToCompactFrameID; - M_StringToCompactFrameID frameIDs_; - /** \brief A map from CompactFrameID frame_id_numbers to string for debugging and output */ - std::vector frameIDs_reverse_; - /** \brief A map to lookup the most recent authority for a given frame */ - std::map frame_authority_; - - - /// How long to cache transform history - tf2::Duration cache_time_; - - typedef uint32_t TransformableCallbackHandle; - - typedef std::unordered_map M_TransformableCallback; - M_TransformableCallback transformable_callbacks_; - uint32_t transformable_callbacks_counter_; - std::mutex transformable_callbacks_mutex_; - - struct TransformableRequest - { - TimePoint time; - TransformableRequestHandle request_handle; - TransformableCallbackHandle cb_handle; - CompactFrameID target_id; - CompactFrameID source_id; - std::string target_string; - std::string source_string; - }; - typedef std::vector V_TransformableRequest; - V_TransformableRequest transformable_requests_; - std::mutex transformable_requests_mutex_; - uint64_t transformable_requests_counter_; - - bool using_dedicated_thread_; - - /************************* Internal Functions ****************************/ - - /** \brief A way to see what frames have been cached - * Useful for debugging. Use this call internally. - */ - std::string allFramesAsStringNoLock() const; - - bool setTransformImpl( - const tf2::Transform & transform_in, const std::string frame_id, - const std::string child_frame_id, const TimePoint stamp, - const std::string & authority, bool is_static); - void lookupTransformImpl( - const std::string & target_frame, const std::string & source_frame, - const TimePoint & time_in, tf2::Transform & transform, TimePoint & time_out) const; - - void lookupTransformImpl( - const std::string & target_frame, const TimePoint & target_time, - const std::string & source_frame, const TimePoint & source_time, - const std::string & fixed_frame, tf2::Transform & transform, TimePoint & time_out) const; - - /** \brief An accessor to get a frame. - * \param c_frame_id The frameID of the desired Reference Frame - */ - TimeCacheInterfacePtr getFrame(CompactFrameID c_frame_id) const; - - TimeCacheInterfacePtr allocateFrame(CompactFrameID cfid, bool is_static); - - /** \brief Validate a frame ID format and look up its CompactFrameID. - * For invalid cases, produce an message. - * \param function_name_arg string to print out in the message, - * the current function and argument name being validated - * \param frame_id name of the tf frame to validate - * \param[out] error_msg if non-nullptr, fill with produced error messaging. - * Otherwise messages are logged as warning. - * \return The CompactFrameID of the frame or 0 if not found. - */ - CompactFrameID validateFrameId( - const char * function_name_arg, - const std::string & frame_id, - std::string * error_msg) const; - - /** \brief Validate a frame ID format and look it up its compact ID. - * Raise an exception for invalid cases. - * \param function_name_arg string to print out in the exception, - * the current function and argument name being validated - * \param frame_id name of the tf frame to validate - * \return The CompactFrameID of the existing frame. - * \throws InvalidArgumentException if the frame_id string has an invalid format - * \throws LookupException if frame_id did not exist - */ - CompactFrameID validateFrameId( - const char * function_name_arg, - const std::string & frame_id) const; - - /// String to number for frame lookup. Returns 0 if the frame was not found. - CompactFrameID lookupFrameNumber(const std::string & frameid_str) const; - - /// String to number for frame lookup with dynamic allocation of new frames - CompactFrameID lookupOrInsertFrameNumber(const std::string & frameid_str); - - /// Number to string frame lookup may throw LookupException if number invalid - const std::string & lookupFrameString(CompactFrameID frame_id_num) const; - - void createConnectivityErrorString( - CompactFrameID source_frame, CompactFrameID target_frame, - std::string * out) const; - - /**@brief Return the latest rostime which is common across the spanning set - * zero if fails to cross */ - tf2::TF2Error getLatestCommonTime( - CompactFrameID target_frame, CompactFrameID source_frame, - TimePoint & time, std::string * error_string) const; - - /**@brief Traverse the transform tree. If frame_chain is not nullptr, store the traversed frame tree in vector frame_chain. - * */ - template - tf2::TF2Error walkToTopParent( - F & f, TimePoint time, CompactFrameID target_id, - CompactFrameID source_id, std::string * error_string, - std::vector * frame_chain) const; - - void testTransformableRequests(); - - // Actual implementation to walk the transform tree and find out if a transform exists. - bool canTransformInternal( - CompactFrameID target_id, CompactFrameID source_id, - const TimePoint & time, std::string * error_msg) const; -}; -} // namespace tf2 +#include #endif // TF2__BUFFER_CORE_H_ diff --git a/tf2/include/tf2/buffer_core.hpp b/tf2/include/tf2/buffer_core.hpp new file mode 100644 index 000000000..04c6f644e --- /dev/null +++ b/tf2/include/tf2/buffer_core.hpp @@ -0,0 +1,467 @@ +// Copyright 2008, Willow Garage, Inc. All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + + +/** \author Tully Foote */ + +#ifndef TF2__BUFFER_CORE_HPP_ +#define TF2__BUFFER_CORE_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "LinearMath/Transform.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "geometry_msgs/msg/velocity_stamped.hpp" +#include "rcutils/logging_macros.h" +#include "tf2/buffer_core_interface.hpp" +#include "tf2/exceptions.hpp" +#include "tf2/transform_storage.hpp" +#include "tf2/visibility_control.h" + +namespace tf2 +{ + +typedef std::pair P_TimeAndFrameID; +typedef uint64_t TransformableRequestHandle; + +class TimeCacheInterface; +using TimeCacheInterfacePtr = std::shared_ptr; + +enum TransformableResult +{ + TransformAvailable, + TransformFailure, +}; + +//!< The default amount of time to cache data in seconds +static constexpr Duration BUFFER_CORE_DEFAULT_CACHE_TIME = std::chrono::seconds(10); + +/** \brief A Class which provides coordinate transforms between any two frames in a system. + * + * This class provides a simple interface to allow recording and lookup of + * relationships between arbitrary frames of the system. + * + * libTF assumes that there is a tree of coordinate frame transforms which define the relationship between all coordinate frames. + * For example your typical robot would have a transform from global to real world. And then from base to hand, and from base to head. + * But Base to Hand really is composed of base to shoulder to elbow to wrist to hand. + * libTF is designed to take care of all the intermediate steps for you. + * + * Internal Representation + * libTF will store frames with the parameters necessary for generating the transform into that frame from it's parent and a reference to the parent frame. + * Frames are designated using an std::string + * 0 is a frame without a parent (the top of a tree) + * The positions of frames over time must be pushed in. + * + * All function calls which pass frame ids can potentially throw the exception tf::LookupException + */ +class BufferCore : public BufferCoreInterface +{ +public: + /************* Constants ***********************/ + //!< Maximum graph search depth (deeper graphs will be assumed to have loops) + TF2_PUBLIC + static const uint32_t MAX_GRAPH_DEPTH = 1000UL; + + /** Constructor + * \param cache_time How long to keep a history of transforms in nanoseconds + * + */ + TF2_PUBLIC + explicit BufferCore(tf2::Duration cache_time = BUFFER_CORE_DEFAULT_CACHE_TIME); + + TF2_PUBLIC + virtual ~BufferCore(void); + + /** \brief Clear all data */ + TF2_PUBLIC + void clear() override; + + /** \brief Add transform information to the tf data structure + * \param transform The transform to store + * \param authority The source of the information for this transform + * \param is_static Record this transform as a static transform. It will be good across all time. (This cannot be changed after the first call.) + * \return True unless an error occured + */ + TF2_PUBLIC + bool setTransform( + const geometry_msgs::msg::TransformStamped & transform, + const std::string & authority, bool is_static = false); + + /*********** Accessors *************/ + + /** \brief Get the transform between two frames by frame ID. + * \param target_frame The frame to which data should be transformed + * \param source_frame The frame where the data originated + * \param time The time at which the value of the transform is desired. (0 will get the latest) + * \return The transform between the frames + * + * Possible exceptions tf2::LookupException, tf2::ConnectivityException, + * tf2::ExtrapolationException, tf2::InvalidArgumentException + */ + TF2_PUBLIC + geometry_msgs::msg::TransformStamped + lookupTransform( + const std::string & target_frame, const std::string & source_frame, + const TimePoint & time) const override; + + /** \brief Get the transform between two frames by frame ID assuming fixed frame. + * \param target_frame The frame to which data should be transformed + * \param target_time The time to which the data should be transformed. (0 will get the latest) + * \param source_frame The frame where the data originated + * \param source_time The time at which the source_frame should be evaluated. (0 will get the latest) + * \param fixed_frame The frame in which to assume the transform is constant in time. + * \return The transform between the frames + * + * Possible exceptions tf2::LookupException, tf2::ConnectivityException, + * tf2::ExtrapolationException, tf2::InvalidArgumentException + */ + + TF2_PUBLIC + geometry_msgs::msg::TransformStamped + lookupTransform( + const std::string & target_frame, const TimePoint & target_time, + const std::string & source_frame, const TimePoint & source_time, + const std::string & fixed_frame) const override; + + TF2_PUBLIC + geometry_msgs::msg::VelocityStamped lookupVelocity( + const std::string & tracking_frame, const std::string & observation_frame, + const TimePoint & time, const tf2::Duration & averaging_interval) const; + + /** \brief Lookup the velocity of the moving_frame in the reference_frame + * \param reference_frame The frame in which to track + * \param moving_frame The frame to track + * \param time The time at which to get the velocity + * \param duration The period over which to average + * \param velocity The velocity output + * + * Possible exceptions TransformReference::LookupException, TransformReference::ConnectivityException, + * TransformReference::MaxDepthException + */ + TF2_PUBLIC + geometry_msgs::msg::VelocityStamped lookupVelocity( + const std::string & tracking_frame, const std::string & observation_frame, + const std::string & reference_frame, const tf2::Vector3 & reference_point, + const std::string & reference_point_frame, + const TimePoint & time, const tf2::Duration & duration) const; + + /** \brief Test if a transform is possible + * \param target_frame The frame into which to transform + * \param source_frame The frame from which to transform + * \param time The time at which to transform + * \param error_msg A pointer to a string which will be filled with why the transform failed, if not nullptr + * \return True if the transform is possible, false otherwise + */ + TF2_PUBLIC + bool canTransform( + const std::string & target_frame, const std::string & source_frame, + const TimePoint & time, std::string * error_msg = nullptr) const override; + + /** \brief Test if a transform is possible + * \param target_frame The frame into which to transform + * \param target_time The time into which to transform + * \param source_frame The frame from which to transform + * \param source_time The time from which to transform + * \param fixed_frame The frame in which to treat the transform as constant in time + * \param error_msg A pointer to a string which will be filled with why the transform failed, if not nullptr + * \return True if the transform is possible, false otherwise + */ + TF2_PUBLIC + bool canTransform( + const std::string & target_frame, const TimePoint & target_time, + const std::string & source_frame, const TimePoint & source_time, + const std::string & fixed_frame, std::string * error_msg = nullptr) const override; + + /** \brief Get all frames that exist in the system. + */ + TF2_PUBLIC + std::vector getAllFrameNames() const override; + + /** \brief A way to see what frames have been cached in yaml format + * Useful for debugging tools + */ + TF2_PUBLIC + std::string allFramesAsYAML(TimePoint current_time) const; + + /** Backwards compatibility for #84 + */ + TF2_PUBLIC + std::string allFramesAsYAML() const; + + /** \brief A way to see what frames have been cached + * Useful for debugging + */ + TF2_PUBLIC + std::string allFramesAsString() const; + + using TransformableCallback = std::function< + void (TransformableRequestHandle request_handle, const std::string & target_frame, + const std::string & source_frame, + TimePoint time, TransformableResult result)>; + + /// \brief Internal use only + TF2_PUBLIC + TransformableRequestHandle addTransformableRequest( + const TransformableCallback & cb, + const std::string & target_frame, + const std::string & source_frame, + TimePoint time); + /// \brief Internal use only + TF2_PUBLIC + void cancelTransformableRequest(TransformableRequestHandle handle); + + + // Tell the buffer that there are multiple threads servicing it. + // This is useful for derived classes to know if they can block or not. + TF2_PUBLIC + void setUsingDedicatedThread(bool value) {using_dedicated_thread_ = value;} + // Get the state of using_dedicated_thread_ + TF2_PUBLIC + bool isUsingDedicatedThread() const {return using_dedicated_thread_;} + + + /* Backwards compatability section for tf::Transformer you should not use these + */ + + /**@brief Check if a frame exists in the tree + * @param frame_id_str The frame id in question */ + TF2_PUBLIC + bool _frameExists(const std::string & frame_id_str) const; + + /**@brief Fill the parent of a frame. + * @param frame_id The frame id of the frame in question + * @param time The timepoint of the frame in question + * @param parent The reference to the string to fill the parent + * Returns true unless "NO_PARENT" */ + TF2_PUBLIC + bool _getParent(const std::string & frame_id, TimePoint time, std::string & parent) const; + + /** \brief A way to get a std::vector of available frame ids */ + TF2_PUBLIC + void _getFrameStrings(std::vector & ids) const; + + + TF2_PUBLIC + CompactFrameID _lookupFrameNumber(const std::string & frameid_str) const + { + return lookupFrameNumber(frameid_str); + } + TF2_PUBLIC + CompactFrameID _lookupOrInsertFrameNumber(const std::string & frameid_str) + { + return lookupOrInsertFrameNumber(frameid_str); + } + + TF2_PUBLIC + tf2::TF2Error _getLatestCommonTime( + CompactFrameID target_frame, CompactFrameID source_frame, + TimePoint & time, std::string * error_string) const + { + std::unique_lock lock(frame_mutex_); + return getLatestCommonTime(target_frame, source_frame, time, error_string); + } + + TF2_PUBLIC + CompactFrameID _validateFrameId( + const char * function_name_arg, + const std::string & frame_id) const + { + return validateFrameId(function_name_arg, frame_id); + } + + /**@brief Get the duration over which this transformer will cache */ + TF2_PUBLIC + tf2::Duration getCacheLength() {return cache_time_;} + + /** \brief Backwards compatabilityA way to see what frames have been cached + * Useful for debugging + */ + TF2_PUBLIC + std::string _allFramesAsDot(TimePoint current_time) const; + TF2_PUBLIC + std::string _allFramesAsDot() const; + + /** \brief Backwards compatabilityA way to see what frames are in a chain + * Useful for debugging + */ + TF2_PUBLIC + void _chainAsVector( + const std::string & target_frame, TimePoint target_time, + const std::string & source_frame, TimePoint source_time, + const std::string & fixed_frame, + std::vector & output) const; + +private: + /******************** Internal Storage ****************/ + + /** \brief The pointers to potential frames that the tree can be made of. + * The frames will be dynamically allocated at run time when set the first time. */ + typedef std::vector V_TimeCacheInterface; + V_TimeCacheInterface frames_; + + /** \brief A mutex to protect testing and allocating new frames on the above vector. */ + mutable std::mutex frame_mutex_; + + /** \brief A map from string frame ids to CompactFrameID */ + typedef std::unordered_map M_StringToCompactFrameID; + M_StringToCompactFrameID frameIDs_; + /** \brief A map from CompactFrameID frame_id_numbers to string for debugging and output */ + std::vector frameIDs_reverse_; + /** \brief A map to lookup the most recent authority for a given frame */ + std::map frame_authority_; + + + /// How long to cache transform history + tf2::Duration cache_time_; + + typedef uint32_t TransformableCallbackHandle; + + typedef std::unordered_map M_TransformableCallback; + M_TransformableCallback transformable_callbacks_; + uint32_t transformable_callbacks_counter_; + std::mutex transformable_callbacks_mutex_; + + struct TransformableRequest + { + TimePoint time; + TransformableRequestHandle request_handle; + TransformableCallbackHandle cb_handle; + CompactFrameID target_id; + CompactFrameID source_id; + std::string target_string; + std::string source_string; + }; + typedef std::vector V_TransformableRequest; + V_TransformableRequest transformable_requests_; + std::mutex transformable_requests_mutex_; + uint64_t transformable_requests_counter_; + + bool using_dedicated_thread_; + + /************************* Internal Functions ****************************/ + + /** \brief A way to see what frames have been cached + * Useful for debugging. Use this call internally. + */ + std::string allFramesAsStringNoLock() const; + + bool setTransformImpl( + const tf2::Transform & transform_in, const std::string frame_id, + const std::string child_frame_id, const TimePoint stamp, + const std::string & authority, bool is_static); + void lookupTransformImpl( + const std::string & target_frame, const std::string & source_frame, + const TimePoint & time_in, tf2::Transform & transform, TimePoint & time_out) const; + + void lookupTransformImpl( + const std::string & target_frame, const TimePoint & target_time, + const std::string & source_frame, const TimePoint & source_time, + const std::string & fixed_frame, tf2::Transform & transform, TimePoint & time_out) const; + + /** \brief An accessor to get a frame. + * \param c_frame_id The frameID of the desired Reference Frame + */ + TimeCacheInterfacePtr getFrame(CompactFrameID c_frame_id) const; + + TimeCacheInterfacePtr allocateFrame(CompactFrameID cfid, bool is_static); + + /** \brief Validate a frame ID format and look up its CompactFrameID. + * For invalid cases, produce an message. + * \param function_name_arg string to print out in the message, + * the current function and argument name being validated + * \param frame_id name of the tf frame to validate + * \param[out] error_msg if non-nullptr, fill with produced error messaging. + * Otherwise messages are logged as warning. + * \return The CompactFrameID of the frame or 0 if not found. + */ + CompactFrameID validateFrameId( + const char * function_name_arg, + const std::string & frame_id, + std::string * error_msg) const; + + /** \brief Validate a frame ID format and look it up its compact ID. + * Raise an exception for invalid cases. + * \param function_name_arg string to print out in the exception, + * the current function and argument name being validated + * \param frame_id name of the tf frame to validate + * \return The CompactFrameID of the existing frame. + * \throws InvalidArgumentException if the frame_id string has an invalid format + * \throws LookupException if frame_id did not exist + */ + CompactFrameID validateFrameId( + const char * function_name_arg, + const std::string & frame_id) const; + + /// String to number for frame lookup. Returns 0 if the frame was not found. + CompactFrameID lookupFrameNumber(const std::string & frameid_str) const; + + /// String to number for frame lookup with dynamic allocation of new frames + CompactFrameID lookupOrInsertFrameNumber(const std::string & frameid_str); + + /// Number to string frame lookup may throw LookupException if number invalid + const std::string & lookupFrameString(CompactFrameID frame_id_num) const; + + void createConnectivityErrorString( + CompactFrameID source_frame, CompactFrameID target_frame, + std::string * out) const; + + /**@brief Return the latest rostime which is common across the spanning set + * zero if fails to cross */ + tf2::TF2Error getLatestCommonTime( + CompactFrameID target_frame, CompactFrameID source_frame, + TimePoint & time, std::string * error_string) const; + + /**@brief Traverse the transform tree. If frame_chain is not nullptr, store the traversed frame tree in vector frame_chain. + * */ + template + tf2::TF2Error walkToTopParent( + F & f, TimePoint time, CompactFrameID target_id, + CompactFrameID source_id, std::string * error_string, + std::vector * frame_chain) const; + + void testTransformableRequests(); + + // Actual implementation to walk the transform tree and find out if a transform exists. + bool canTransformInternal( + CompactFrameID target_id, CompactFrameID source_id, + const TimePoint & time, std::string * error_msg) const; +}; +} // namespace tf2 + +#endif // TF2__BUFFER_CORE_HPP_ diff --git a/tf2/include/tf2/buffer_core_interface.h b/tf2/include/tf2/buffer_core_interface.h index f68512a6c..a6f1eed10 100644 --- a/tf2/include/tf2/buffer_core_interface.h +++ b/tf2/include/tf2/buffer_core_interface.h @@ -28,117 +28,6 @@ #ifndef TF2__BUFFER_CORE_INTERFACE_H_ #define TF2__BUFFER_CORE_INTERFACE_H_ -#include -#include - -#include "geometry_msgs/msg/transform_stamped.hpp" - -#include "tf2/time.h" -#include "tf2/visibility_control.h" - -namespace tf2 -{ - -/** - * \brief Interface for providing coordinate transforms between any two frames in a system. - * - * This class provides a simple abstract interface for looking up relationships between arbitrary - * frames of a system. - */ -class BufferCoreInterface -{ -public: - TF2_PUBLIC - virtual - ~BufferCoreInterface() = default; - - /** - * \brief Clear internal state data. - */ - TF2_PUBLIC - virtual void - clear() = 0; - - /** - * \brief Get the transform between two frames by frame ID. - * \param target_frame The frame to which data should be transformed. - * \param source_frame The frame where the data originated. - * \param time The time at which the value of the transform is desired (0 will get the latest). - * \return The transform between the frames. - */ - TF2_PUBLIC - virtual geometry_msgs::msg::TransformStamped - lookupTransform( - const std::string & target_frame, - const std::string & source_frame, - const tf2::TimePoint & time) const = 0; - - /** - * \brief Get the transform between two frames by frame ID assuming fixed frame. - * \param target_frame The frame to which data should be transformed. - * \param target_time The time to which the data should be transformed (0 will get the latest). - * \param source_frame The frame where the data originated. - * \param source_time The time at which the source_frame should be evaluated - * (0 will get the latest). - * \param fixed_frame The frame in which to assume the transform is constant in time. - * \return The transform between the frames. - */ - TF2_PUBLIC - virtual geometry_msgs::msg::TransformStamped - lookupTransform( - const std::string & target_frame, - const tf2::TimePoint & target_time, - const std::string & source_frame, - const tf2::TimePoint & source_time, - const std::string & fixed_frame) const = 0; - - /** - * \brief Test if a transform is possible. - * \param target_frame The frame into which to transform. - * \param source_frame The frame from which to transform. - * \param time The time at which to transform. - * \param error_msg A pointer to a string which will be filled with why the transform failed. - * Ignored if nullptr. - * \return true if the transform is possible, false otherwise. - */ - TF2_PUBLIC - virtual bool - canTransform( - const std::string & target_frame, - const std::string & source_frame, - const tf2::TimePoint & time, - std::string * error_msg) const = 0; - - /** - * \brief Test if a transform is possible. - * \param target_frame The frame into which to transform. - * \param target_time The time into which to transform. - * \param source_frame The frame from which to transform. - * \param source_time The time from which to transform. - * \param fixed_frame The frame in which to treat the transform as constant in time. - * \param error_msg A pointer to a string which will be filled with why the transform failed. - * Ignored if nullptr. - * \return true if the transform is possible, false otherwise. - */ - TF2_PUBLIC - virtual bool - canTransform( - const std::string & target_frame, - const tf2::TimePoint & target_time, - const std::string & source_frame, - const tf2::TimePoint & source_time, - const std::string & fixed_frame, - std::string * error_msg) const = 0; - - /** - * \brief Get all frames that exist in the system. - * \return all frame names in a vector. - */ - TF2_PUBLIC - virtual std::vector - getAllFrameNames() const = 0; -}; // class BufferCoreInterface - -} // namespace tf2 +#include #endif // TF2__BUFFER_CORE_INTERFACE_H_ diff --git a/tf2/include/tf2/buffer_core_interface.hpp b/tf2/include/tf2/buffer_core_interface.hpp new file mode 100644 index 000000000..d9c718a8e --- /dev/null +++ b/tf2/include/tf2/buffer_core_interface.hpp @@ -0,0 +1,144 @@ +// Copyright 2019, Open Source Robotics Foundation, Inc. All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the Open Source Robotics Foundation nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +#ifndef TF2__BUFFER_CORE_INTERFACE_HPP_ +#define TF2__BUFFER_CORE_INTERFACE_HPP_ + +#include +#include + +#include "geometry_msgs/msg/transform_stamped.hpp" + +#include "tf2/time.hpp" +#include "tf2/visibility_control.h" + +namespace tf2 +{ + +/** + * \brief Interface for providing coordinate transforms between any two frames in a system. + * + * This class provides a simple abstract interface for looking up relationships between arbitrary + * frames of a system. + */ +class BufferCoreInterface +{ +public: + TF2_PUBLIC + virtual + ~BufferCoreInterface() = default; + + /** + * \brief Clear internal state data. + */ + TF2_PUBLIC + virtual void + clear() = 0; + + /** + * \brief Get the transform between two frames by frame ID. + * \param target_frame The frame to which data should be transformed. + * \param source_frame The frame where the data originated. + * \param time The time at which the value of the transform is desired (0 will get the latest). + * \return The transform between the frames. + */ + TF2_PUBLIC + virtual geometry_msgs::msg::TransformStamped + lookupTransform( + const std::string & target_frame, + const std::string & source_frame, + const tf2::TimePoint & time) const = 0; + + /** + * \brief Get the transform between two frames by frame ID assuming fixed frame. + * \param target_frame The frame to which data should be transformed. + * \param target_time The time to which the data should be transformed (0 will get the latest). + * \param source_frame The frame where the data originated. + * \param source_time The time at which the source_frame should be evaluated + * (0 will get the latest). + * \param fixed_frame The frame in which to assume the transform is constant in time. + * \return The transform between the frames. + */ + TF2_PUBLIC + virtual geometry_msgs::msg::TransformStamped + lookupTransform( + const std::string & target_frame, + const tf2::TimePoint & target_time, + const std::string & source_frame, + const tf2::TimePoint & source_time, + const std::string & fixed_frame) const = 0; + + /** + * \brief Test if a transform is possible. + * \param target_frame The frame into which to transform. + * \param source_frame The frame from which to transform. + * \param time The time at which to transform. + * \param error_msg A pointer to a string which will be filled with why the transform failed. + * Ignored if nullptr. + * \return true if the transform is possible, false otherwise. + */ + TF2_PUBLIC + virtual bool + canTransform( + const std::string & target_frame, + const std::string & source_frame, + const tf2::TimePoint & time, + std::string * error_msg) const = 0; + + /** + * \brief Test if a transform is possible. + * \param target_frame The frame into which to transform. + * \param target_time The time into which to transform. + * \param source_frame The frame from which to transform. + * \param source_time The time from which to transform. + * \param fixed_frame The frame in which to treat the transform as constant in time. + * \param error_msg A pointer to a string which will be filled with why the transform failed. + * Ignored if nullptr. + * \return true if the transform is possible, false otherwise. + */ + TF2_PUBLIC + virtual bool + canTransform( + const std::string & target_frame, + const tf2::TimePoint & target_time, + const std::string & source_frame, + const tf2::TimePoint & source_time, + const std::string & fixed_frame, + std::string * error_msg) const = 0; + + /** + * \brief Get all frames that exist in the system. + * \return all frame names in a vector. + */ + TF2_PUBLIC + virtual std::vector + getAllFrameNames() const = 0; +}; // class BufferCoreInterface + +} // namespace tf2 + +#endif // TF2__BUFFER_CORE_INTERFACE_HPP_ diff --git a/tf2/include/tf2/convert.h b/tf2/include/tf2/convert.h index f81a3c65d..4d83fd7a7 100644 --- a/tf2/include/tf2/convert.h +++ b/tf2/include/tf2/convert.h @@ -31,171 +31,6 @@ #ifndef TF2__CONVERT_H_ #define TF2__CONVERT_H_ -#include -#include -#include - -#include "geometry_msgs/msg/transform_stamped.hpp" -#include "rosidl_runtime_cpp/traits.hpp" -#include "tf2/exceptions.h" -#include "tf2/impl/convert.h" -#include "tf2/transform_datatypes.h" -#include "tf2/visibility_control.h" - -namespace tf2 -{ - -/**\brief The templated function expected to be able to do a transform - * - * This is the method which tf2 will use to try to apply a transform for any given datatype. - * \param data_in[in] The data to be transformed. - * \param data_out[inout] A reference to the output data. Note this can point to data in and the method should be mutation safe. - * \param transform[in] The transform to apply to data_in to fill data_out. - * - * This method needs to be implemented by client library developers - */ -template -void doTransform( - const T & data_in, T & data_out, - const geometry_msgs::msg::TransformStamped & transform); - -/**\brief Get the timestamp from data - * \param[in] t The data input. - * \return The timestamp associated with the data. - */ -template -tf2::TimePoint getTimestamp(const T & t); - -/**\brief Get the frame_id from data - * \param[in] t The data input. - * \return The frame_id associated with the data. - */ -template -std::string getFrameId(const T & t); - -/**\brief Get the covariance matrix from data - * \param[in] t The data input. - * \return The covariance matrix associated with the data. - */ -template -std::array, 6> getCovarianceMatrix(const T & t); - -/**\brief Get the frame_id from data - * - * An implementation for Stamped

datatypes. - * - * \param[in] t The data input. - * \return The frame_id associated with the data. - */ -template -tf2::TimePoint getTimestamp(const tf2::Stamped

& t) -{ - return t.stamp_; -} - -/**\brief Get the frame_id from data - * - * An implementation for Stamped

datatypes. - * - * \param[in] t The data input. - * \return The frame_id associated with the data. - */ -template -std::string getFrameId(const tf2::Stamped

& t) -{ - return t.frame_id_; -} - -/**\brief Get the covariance matrix from data - * - * An implementation for WithCovarianceStamped

datatypes. - * - * \param[in] t The data input. - * \return The covariance matrix associated with the data. - */ -template -std::array, 6> getCovarianceMatrix(const tf2::WithCovarianceStamped

& t) -{ - return t.cov_mat_; -} - -/**\brief Function that converts from one type to a ROS message type. It has to be - * implemented by each data type in tf2_* (except ROS messages) as it is - * used in the "convert" function. - * \param a an object of whatever type - * \return the conversion as a ROS message - */ -template -B toMsg(const A & a); - -/**\brief Function that converts from a ROS message type to another type. It has to be - * implemented by each data type in tf2_* (except ROS messages) as it is used - * in the "convert" function. - * \param a a ROS message to convert from - * \param b the object to convert to - */ -template -void fromMsg(const A & a, B & b); - -/**\brief Function that converts any type to any type (messages or not). - * Matching toMsg and from Msg conversion functions need to exist. - * If they don't exist or do not apply (for example, if your two - * classes are ROS messages), just write a specialization of the function. - * \param a an object to convert from - * \param b the object to convert to - */ -template -void convert(const A & a, B & b) -{ - impl::Converter::value, - rosidl_generator_traits::is_message::value>::convert(a, b); -} - -template -void convert(const A & a1, A & a2) -{ - if (&a1 != &a2) { - a2 = a1; - } -} - -/**\brief Function that converts from a row-major representation of a 6x6 - * covariance matrix to a nested array representation. - * \param row_major A row-major array of 36 covariance values. - * \return A nested array representation of 6x6 covariance values. - */ -inline -std::array, 6> covarianceRowMajorToNested( - const std::array & row_major) -{ - std::array, 6> nested_array; - std::array::const_iterator ss = row_major.begin(); - for (std::array & dd : nested_array) { - std::copy_n(ss, dd.size(), dd.begin()); - ss += dd.size(); - } - return nested_array; -} - -/**\brief Function that converts from a nested array representation of a 6x6 - * covariance matrix to a row-major representation. - * \param nested_array A nested array representation of 6x6 covariance values. - * \return A row-major array of 36 covariance values. - */ -inline -std::array covarianceNestedToRowMajor( - const std::array, 6> & nested_array) -{ - std::array row_major = {}; - size_t counter = 0; - for (const auto & arr : nested_array) { - for (const double & val : arr) { - row_major[counter] = val; - counter++; - } - } - return row_major; -} -} // namespace tf2 +#include #endif // TF2__CONVERT_H_ diff --git a/tf2/include/tf2/convert.hpp b/tf2/include/tf2/convert.hpp new file mode 100644 index 000000000..b56ac500f --- /dev/null +++ b/tf2/include/tf2/convert.hpp @@ -0,0 +1,201 @@ +// Copyright 2008, Willow Garage, Inc. All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the Willow Garage nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +/** \author Tully Foote */ + +#ifndef TF2__CONVERT_HPP_ +#define TF2__CONVERT_HPP_ + +#include +#include +#include + +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "rosidl_runtime_cpp/traits.hpp" +#include "tf2/exceptions.hpp" +#include "tf2/impl/convert.hpp" +#include "tf2/transform_datatypes.hpp" +#include "tf2/visibility_control.h" + +namespace tf2 +{ + +/**\brief The templated function expected to be able to do a transform + * + * This is the method which tf2 will use to try to apply a transform for any given datatype. + * \param data_in[in] The data to be transformed. + * \param data_out[inout] A reference to the output data. Note this can point to data in and the method should be mutation safe. + * \param transform[in] The transform to apply to data_in to fill data_out. + * + * This method needs to be implemented by client library developers + */ +template +void doTransform( + const T & data_in, T & data_out, + const geometry_msgs::msg::TransformStamped & transform); + +/**\brief Get the timestamp from data + * \param[in] t The data input. + * \return The timestamp associated with the data. + */ +template +tf2::TimePoint getTimestamp(const T & t); + +/**\brief Get the frame_id from data + * \param[in] t The data input. + * \return The frame_id associated with the data. + */ +template +std::string getFrameId(const T & t); + +/**\brief Get the covariance matrix from data + * \param[in] t The data input. + * \return The covariance matrix associated with the data. + */ +template +std::array, 6> getCovarianceMatrix(const T & t); + +/**\brief Get the frame_id from data + * + * An implementation for Stamped

datatypes. + * + * \param[in] t The data input. + * \return The frame_id associated with the data. + */ +template +tf2::TimePoint getTimestamp(const tf2::Stamped

& t) +{ + return t.stamp_; +} + +/**\brief Get the frame_id from data + * + * An implementation for Stamped

datatypes. + * + * \param[in] t The data input. + * \return The frame_id associated with the data. + */ +template +std::string getFrameId(const tf2::Stamped

& t) +{ + return t.frame_id_; +} + +/**\brief Get the covariance matrix from data + * + * An implementation for WithCovarianceStamped

datatypes. + * + * \param[in] t The data input. + * \return The covariance matrix associated with the data. + */ +template +std::array, 6> getCovarianceMatrix(const tf2::WithCovarianceStamped

& t) +{ + return t.cov_mat_; +} + +/**\brief Function that converts from one type to a ROS message type. It has to be + * implemented by each data type in tf2_* (except ROS messages) as it is + * used in the "convert" function. + * \param a an object of whatever type + * \return the conversion as a ROS message + */ +template +B toMsg(const A & a); + +/**\brief Function that converts from a ROS message type to another type. It has to be + * implemented by each data type in tf2_* (except ROS messages) as it is used + * in the "convert" function. + * \param a a ROS message to convert from + * \param b the object to convert to + */ +template +void fromMsg(const A & a, B & b); + +/**\brief Function that converts any type to any type (messages or not). + * Matching toMsg and from Msg conversion functions need to exist. + * If they don't exist or do not apply (for example, if your two + * classes are ROS messages), just write a specialization of the function. + * \param a an object to convert from + * \param b the object to convert to + */ +template +void convert(const A & a, B & b) +{ + impl::Converter::value, + rosidl_generator_traits::is_message::value>::convert(a, b); +} + +template +void convert(const A & a1, A & a2) +{ + if (&a1 != &a2) { + a2 = a1; + } +} + +/**\brief Function that converts from a row-major representation of a 6x6 + * covariance matrix to a nested array representation. + * \param row_major A row-major array of 36 covariance values. + * \return A nested array representation of 6x6 covariance values. + */ +inline +std::array, 6> covarianceRowMajorToNested( + const std::array & row_major) +{ + std::array, 6> nested_array; + std::array::const_iterator ss = row_major.begin(); + for (std::array & dd : nested_array) { + std::copy_n(ss, dd.size(), dd.begin()); + ss += dd.size(); + } + return nested_array; +} + +/**\brief Function that converts from a nested array representation of a 6x6 + * covariance matrix to a row-major representation. + * \param nested_array A nested array representation of 6x6 covariance values. + * \return A row-major array of 36 covariance values. + */ +inline +std::array covarianceNestedToRowMajor( + const std::array, 6> & nested_array) +{ + std::array row_major = {}; + size_t counter = 0; + for (const auto & arr : nested_array) { + for (const double & val : arr) { + row_major[counter] = val; + counter++; + } + } + return row_major; +} +} // namespace tf2 + +#endif // TF2__CONVERT_HPP_ diff --git a/tf2/include/tf2/exceptions.h b/tf2/include/tf2/exceptions.h index 000f072c4..0215d33a6 100644 --- a/tf2/include/tf2/exceptions.h +++ b/tf2/include/tf2/exceptions.h @@ -31,189 +31,6 @@ #ifndef TF2__EXCEPTIONS_H_ #define TF2__EXCEPTIONS_H_ -#include -#include -#include +#include -#include "tf2/visibility_control.h" - -namespace tf2 -{ - -// TODO(clalancette): We can remove these workarounds when we remove the -// deprecated TF2Error enums. -#if defined(_WIN32) -#pragma push_macro("NO_ERROR") -#undef NO_ERROR -#endif -#if defined(__APPLE__) || defined(__clang__) -// The clang compiler on Apple claims that [[deprecated]] on an enumerator value -// is a C++17 feature, when it was really introduced in C++14. Ignore that -// warning when defining the structure; this whole thing will go away when we -// remove the deprecated values. -#pragma clang diagnostic push -#pragma clang diagnostic ignored "-Wc++17-extensions" -#endif - -enum class TF2Error : std::uint8_t -{ - // While the TF2_ prefix here is a bit redundant, it also prevents us from - // colliding with Windows defines (specifically, NO_ERROR). - TF2_NO_ERROR = 0, - TF2_LOOKUP_ERROR = 1, - TF2_CONNECTIVITY_ERROR = 2, - TF2_EXTRAPOLATION_ERROR = 3, - TF2_INVALID_ARGUMENT_ERROR = 4, - TF2_TIMEOUT_ERROR = 5, - TF2_TRANSFORM_ERROR = 6, - TF2_BACKWARD_EXTRAPOLATION_ERROR = 7, - TF2_FORWARD_EXTRAPOLATION_ERROR = 8, - TF2_NO_DATA_FOR_EXTRAPOLATION_ERROR = 9, - - NO_ERROR [[deprecated("Use TF2_NO_ERROR instead")]] = 0, - LOOKUP_ERROR [[deprecated("Use TF2_LOOKUP_ERROR instead")]] = 1, - CONNECTIVITY_ERROR [[deprecated("Use TF2_CONNECTIVITY_ERROR instead")]] = 2, - EXTRAPOLATION_ERROR [[deprecated("Use TF2_EXTRAPOLATION_ERROR instead")]] = 3, - INVALID_ARGUMENT_ERROR [[deprecated("Use TF2_INVALID_ARGUMENT_ERROR instead")]] = 4, - TIMEOUT_ERROR [[deprecated("Use TF2_TIMEOUT_ERROR instead")]] = 5, - TRANSFORM_ERROR [[deprecated("Use TF2_TRANSFORM_ERROR instead")]] = 6 -}; - -// TODO(clalancette): We can remove these workarounds when we remove the -// deprecated TF2Error enums. -#if defined(__APPLE__) -#pragma clang diagnostic pop -#endif -#if defined(_WIN32) -#pragma pop_macro("NO_ERROR") -#endif - -/** \brief A base class for all tf2 exceptions - * This inherits from ros::exception - * which inherits from std::runtime_exception - */ -class TransformException : public std::runtime_error -{ -public: - TF2_PUBLIC - explicit TransformException(const std::string errorDescription) - : std::runtime_error(errorDescription) - { - } -}; - - -/** \brief An exception class to notify of no connection - * - * This is an exception class to be thrown in the case - * that the Reference Frame tree is not connected between - * the frames requested. */ -class ConnectivityException : public TransformException -{ -public: - TF2_PUBLIC - explicit ConnectivityException(const std::string errorDescription) - : tf2::TransformException(errorDescription) - { - } -}; - - -/** \brief An exception class to notify of bad frame number - * - * This is an exception class to be thrown in the case that - * a frame not in the graph has been attempted to be accessed. - * The most common reason for this is that the frame is not - * being published, or a parent frame was not set correctly - * causing the tree to be broken. - */ -class LookupException : public TransformException -{ -public: - TF2_PUBLIC - explicit LookupException(const std::string errorDescription) - : tf2::TransformException(errorDescription) - { - } -}; - -/** \brief An exception class to notify that the requested value would have required extrapolation beyond current limits. - * - */ -class ExtrapolationException : public TransformException -{ -public: - TF2_PUBLIC - explicit ExtrapolationException(const std::string errorDescription) - : tf2::TransformException(errorDescription) - { - } -}; - -/** \brief An exception class to notify that the requested value would have required extrapolation in the past. - * - */ -class BackwardExtrapolationException : public ExtrapolationException -{ -public: - TF2_PUBLIC - explicit BackwardExtrapolationException(const std::string errorDescription) - : ExtrapolationException(errorDescription) - { - } -}; - -/** \brief An exception class to notify that the requested value would have required extrapolation in the future. - * - */ -class ForwardExtrapolationException : public ExtrapolationException -{ -public: - TF2_PUBLIC - explicit ForwardExtrapolationException(const std::string errorDescription) - : ExtrapolationException(errorDescription) - { - } -}; - -/** \brief An exception class to notify that the requested value would have required extrapolation, but only zero or one data is available, so not enough for extrapolation. - * - */ -class NoDataForExtrapolationException : public ExtrapolationException -{ -public: - TF2_PUBLIC - explicit NoDataForExtrapolationException(const std::string errorDescription) - : ExtrapolationException(errorDescription) - { - } -}; - -/** \brief An exception class to notify that one of the arguments is invalid - * - * usually it's an uninitalized Quaternion (0,0,0,0) - * - */ -class InvalidArgumentException : public TransformException -{ -public: - TF2_PUBLIC - explicit InvalidArgumentException(const std::string errorDescription) - : tf2::TransformException(errorDescription) {} -}; - -/** \brief An exception class to notify that a timeout has occured - * - * - */ -class TimeoutException : public TransformException -{ -public: - TF2_PUBLIC - explicit TimeoutException(const std::string errorDescription) - : tf2::TransformException(errorDescription) - { - } -}; -} // namespace tf2 #endif // TF2__EXCEPTIONS_H_ diff --git a/tf2/include/tf2/exceptions.hpp b/tf2/include/tf2/exceptions.hpp new file mode 100644 index 000000000..690071f5e --- /dev/null +++ b/tf2/include/tf2/exceptions.hpp @@ -0,0 +1,219 @@ +// Copyright 2008, Willow Garage, Inc. All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the Willow Garage nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +/** \author Tully Foote */ + +#ifndef TF2__EXCEPTIONS_HPP_ +#define TF2__EXCEPTIONS_HPP_ + +#include +#include +#include + +#include "tf2/visibility_control.h" + +namespace tf2 +{ + +// TODO(clalancette): We can remove these workarounds when we remove the +// deprecated TF2Error enums. +#if defined(_WIN32) +#pragma push_macro("NO_ERROR") +#undef NO_ERROR +#endif +#if defined(__APPLE__) || defined(__clang__) +// The clang compiler on Apple claims that [[deprecated]] on an enumerator value +// is a C++17 feature, when it was really introduced in C++14. Ignore that +// warning when defining the structure; this whole thing will go away when we +// remove the deprecated values. +#pragma clang diagnostic push +#pragma clang diagnostic ignored "-Wc++17-extensions" +#endif + +enum class TF2Error : std::uint8_t +{ + // While the TF2_ prefix here is a bit redundant, it also prevents us from + // colliding with Windows defines (specifically, NO_ERROR). + TF2_NO_ERROR = 0, + TF2_LOOKUP_ERROR = 1, + TF2_CONNECTIVITY_ERROR = 2, + TF2_EXTRAPOLATION_ERROR = 3, + TF2_INVALID_ARGUMENT_ERROR = 4, + TF2_TIMEOUT_ERROR = 5, + TF2_TRANSFORM_ERROR = 6, + TF2_BACKWARD_EXTRAPOLATION_ERROR = 7, + TF2_FORWARD_EXTRAPOLATION_ERROR = 8, + TF2_NO_DATA_FOR_EXTRAPOLATION_ERROR = 9, + + NO_ERROR [[deprecated("Use TF2_NO_ERROR instead")]] = 0, + LOOKUP_ERROR [[deprecated("Use TF2_LOOKUP_ERROR instead")]] = 1, + CONNECTIVITY_ERROR [[deprecated("Use TF2_CONNECTIVITY_ERROR instead")]] = 2, + EXTRAPOLATION_ERROR [[deprecated("Use TF2_EXTRAPOLATION_ERROR instead")]] = 3, + INVALID_ARGUMENT_ERROR [[deprecated("Use TF2_INVALID_ARGUMENT_ERROR instead")]] = 4, + TIMEOUT_ERROR [[deprecated("Use TF2_TIMEOUT_ERROR instead")]] = 5, + TRANSFORM_ERROR [[deprecated("Use TF2_TRANSFORM_ERROR instead")]] = 6 +}; + +// TODO(clalancette): We can remove these workarounds when we remove the +// deprecated TF2Error enums. +#if defined(__APPLE__) +#pragma clang diagnostic pop +#endif +#if defined(_WIN32) +#pragma pop_macro("NO_ERROR") +#endif + +/** \brief A base class for all tf2 exceptions + * This inherits from ros::exception + * which inherits from std::runtime_exception + */ +class TransformException : public std::runtime_error +{ +public: + TF2_PUBLIC + explicit TransformException(const std::string errorDescription) + : std::runtime_error(errorDescription) + { + } +}; + + +/** \brief An exception class to notify of no connection + * + * This is an exception class to be thrown in the case + * that the Reference Frame tree is not connected between + * the frames requested. */ +class ConnectivityException : public TransformException +{ +public: + TF2_PUBLIC + explicit ConnectivityException(const std::string errorDescription) + : tf2::TransformException(errorDescription) + { + } +}; + + +/** \brief An exception class to notify of bad frame number + * + * This is an exception class to be thrown in the case that + * a frame not in the graph has been attempted to be accessed. + * The most common reason for this is that the frame is not + * being published, or a parent frame was not set correctly + * causing the tree to be broken. + */ +class LookupException : public TransformException +{ +public: + TF2_PUBLIC + explicit LookupException(const std::string errorDescription) + : tf2::TransformException(errorDescription) + { + } +}; + +/** \brief An exception class to notify that the requested value would have required extrapolation beyond current limits. + * + */ +class ExtrapolationException : public TransformException +{ +public: + TF2_PUBLIC + explicit ExtrapolationException(const std::string errorDescription) + : tf2::TransformException(errorDescription) + { + } +}; + +/** \brief An exception class to notify that the requested value would have required extrapolation in the past. + * + */ +class BackwardExtrapolationException : public ExtrapolationException +{ +public: + TF2_PUBLIC + explicit BackwardExtrapolationException(const std::string errorDescription) + : ExtrapolationException(errorDescription) + { + } +}; + +/** \brief An exception class to notify that the requested value would have required extrapolation in the future. + * + */ +class ForwardExtrapolationException : public ExtrapolationException +{ +public: + TF2_PUBLIC + explicit ForwardExtrapolationException(const std::string errorDescription) + : ExtrapolationException(errorDescription) + { + } +}; + +/** \brief An exception class to notify that the requested value would have required extrapolation, but only zero or one data is available, so not enough for extrapolation. + * + */ +class NoDataForExtrapolationException : public ExtrapolationException +{ +public: + TF2_PUBLIC + explicit NoDataForExtrapolationException(const std::string errorDescription) + : ExtrapolationException(errorDescription) + { + } +}; + +/** \brief An exception class to notify that one of the arguments is invalid + * + * usually it's an uninitalized Quaternion (0,0,0,0) + * + */ +class InvalidArgumentException : public TransformException +{ +public: + TF2_PUBLIC + explicit InvalidArgumentException(const std::string errorDescription) + : tf2::TransformException(errorDescription) {} +}; + +/** \brief An exception class to notify that a timeout has occured + * + * + */ +class TimeoutException : public TransformException +{ +public: + TF2_PUBLIC + explicit TimeoutException(const std::string errorDescription) + : tf2::TransformException(errorDescription) + { + } +}; +} // namespace tf2 +#endif // TF2__EXCEPTIONS_HPP_ diff --git a/tf2/include/tf2/impl/convert.h b/tf2/include/tf2/impl/convert.h index 3821564a0..8747e674d 100644 --- a/tf2/include/tf2/impl/convert.h +++ b/tf2/include/tf2/impl/convert.h @@ -29,52 +29,6 @@ #ifndef TF2__IMPL__CONVERT_H_ #define TF2__IMPL__CONVERT_H_ -namespace tf2 -{ -namespace impl -{ - -template -class Converter -{ -public: - template - static void convert(const A & a, B & b); -}; - -// The case where both A and B are messages should not happen: if you have two -// messages that are interchangeable, well, that's against the ROS purpose: -// only use one type. Worst comes to worst, specialize the original convert -// function for your types. -// if B == A, the templated version of convert with only one argument will be -// used. -// -template< > -template -inline void Converter::convert(const A & a, B & b); - -template< > -template -inline void Converter::convert(const A & a, B & b) -{ - fromMsg(a, b); -} - -template< > -template -inline void Converter::convert(const A & a, B & b) -{ - b = toMsg(a); -} - -template< > -template -inline void Converter::convert(const A & a, B & b) -{ - fromMsg(toMsg(a), b); -} - -} // namespace impl -} // namespace tf2 +#include #endif // TF2__IMPL__CONVERT_H_ diff --git a/tf2/include/tf2/impl/convert.hpp b/tf2/include/tf2/impl/convert.hpp new file mode 100644 index 000000000..1a1031504 --- /dev/null +++ b/tf2/include/tf2/impl/convert.hpp @@ -0,0 +1,80 @@ +// Copyright 2013, Open Source Robotics Foundation, Inc. All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the Open Source Robotics Foundation nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#ifndef TF2__IMPL__CONVERT_HPP_ +#define TF2__IMPL__CONVERT_HPP_ + +namespace tf2 +{ +namespace impl +{ + +template +class Converter +{ +public: + template + static void convert(const A & a, B & b); +}; + +// The case where both A and B are messages should not happen: if you have two +// messages that are interchangeable, well, that's against the ROS purpose: +// only use one type. Worst comes to worst, specialize the original convert +// function for your types. +// if B == A, the templated version of convert with only one argument will be +// used. +// +template< > +template +inline void Converter::convert(const A & a, B & b); + +template< > +template +inline void Converter::convert(const A & a, B & b) +{ + fromMsg(a, b); +} + +template< > +template +inline void Converter::convert(const A & a, B & b) +{ + b = toMsg(a); +} + +template< > +template +inline void Converter::convert(const A & a, B & b) +{ + fromMsg(toMsg(a), b); +} + +} // namespace impl +} // namespace tf2 + +#endif // TF2__IMPL__CONVERT_HPP_ diff --git a/tf2/include/tf2/impl/utils.h b/tf2/include/tf2/impl/utils.h index 8953ceed3..d68e384c3 100644 --- a/tf2/include/tf2/impl/utils.h +++ b/tf2/include/tf2/impl/utils.h @@ -15,156 +15,6 @@ #ifndef TF2__IMPL__UTILS_H_ #define TF2__IMPL__UTILS_H_ -#include -#include -#include -#include -#include +#include - -namespace tf2 -{ - -// Forward declare functions needed in this header -void fromMsg(const geometry_msgs::msg::Quaternion & in, tf2::Quaternion & out); - -namespace impl -{ - -/** Function needed for the generalization of toQuaternion - * \param q a tf2::Quaternion - * \return a copy of the same quaternion - */ -inline -tf2::Quaternion toQuaternion(const tf2::Quaternion & q) -{ - return q; -} - -/** Function needed for the generalization of toQuaternion - * \param q a geometry_msgs::msg::Quaternion - * \return a copy of the same quaternion as a tf2::Quaternion - */ -inline -tf2::Quaternion toQuaternion(const geometry_msgs::msg::Quaternion & q) -{ - tf2::Quaternion res; - fromMsg(q, res); - return res; -} - -/** Function needed for the generalization of toQuaternion - * \param q a geometry_msgs::msg::QuaternionStamped - * \return a copy of the same quaternion as a tf2::Quaternion - */ -inline -tf2::Quaternion toQuaternion(const geometry_msgs::msg::QuaternionStamped & q) -{ - tf2::Quaternion res; - fromMsg(q.quaternion, res); - return res; -} - -/** Function needed for the generalization of toQuaternion - * \param t some tf2::Stamped object - * \return a copy of the same quaternion as a tf2::Quaternion - */ -template -tf2::Quaternion toQuaternion(const tf2::Stamped & t) -{ - geometry_msgs::msg::QuaternionStamped q = toMsg, - geometry_msgs::msg::QuaternionStamped>(t); - return toQuaternion(q); -} - -/** Generic version of toQuaternion. It tries to convert the argument - * to a geometry_msgs::msg::Quaternion - * \param t some object - * \return a copy of the same quaternion as a tf2::Quaternion - */ -template -tf2::Quaternion toQuaternion(const T & t) -{ - geometry_msgs::msg::Quaternion q = toMsg(t); - return toQuaternion(q); -} - -/** The code below is blantantly copied from urdfdom_headers - * only the normalization has been added. - * It computes the Euler roll, pitch yaw from a tf2::Quaternion - * It is equivalent to tf2::Matrix3x3(q).getEulerYPR(yaw, pitch, roll); - * \param q a tf2::Quaternion - * \param yaw the computed yaw - * \param pitch the computed pitch - * \param roll the computed roll - */ -inline -void getEulerYPR(const tf2::Quaternion & q, double & yaw, double & pitch, double & roll) -{ - const double pi_2 = 1.57079632679489661923; - double sqw; - double sqx; - double sqy; - double sqz; - - sqx = q.x() * q.x(); - sqy = q.y() * q.y(); - sqz = q.z() * q.z(); - sqw = q.w() * q.w(); - - // Cases derived from https://orbitalstation.wordpress.com/tag/quaternion/ - // normalization added from urdfom_headers - double sarg = -2 * (q.x() * q.z() - q.w() * q.y()) / (sqx + sqy + sqz + sqw); - if (sarg <= -0.99999) { - pitch = -0.5 * pi_2; - roll = 0; - yaw = -2 * atan2(q.y(), q.x()); - } else if (sarg >= 0.99999) { - pitch = 0.5 * pi_2; - roll = 0; - yaw = 2 * atan2(q.y(), q.x()); - } else { - pitch = asin(sarg); - roll = atan2(2 * (q.y() * q.z() + q.w() * q.x()), sqw - sqx - sqy + sqz); - yaw = atan2(2 * (q.x() * q.y() + q.w() * q.z()), sqw + sqx - sqy - sqz); - } -} - -/** The code below is a simplified version of getEulerRPY that only - * returns the yaw. It is mostly useful in navigation where only yaw - * matters - * \param q a tf2::Quaternion - * \return the computed yaw - */ -inline -double getYaw(const tf2::Quaternion & q) -{ - double yaw; - - double sqw; - double sqx; - double sqy; - double sqz; - - sqx = q.x() * q.x(); - sqy = q.y() * q.y(); - sqz = q.z() * q.z(); - sqw = q.w() * q.w(); - - // Cases derived from https://orbitalstation.wordpress.com/tag/quaternion/ - // normalization added from urdfom_headers - double sarg = -2 * (q.x() * q.z() - q.w() * q.y()) / (sqx + sqy + sqz + sqw); - - if (sarg <= -0.99999) { - yaw = -2 * atan2(q.y(), q.x()); - } else if (sarg >= 0.99999) { - yaw = 2 * atan2(q.y(), q.x()); - } else { - yaw = atan2(2 * (q.x() * q.y() + q.w() * q.z()), sqw + sqx - sqy - sqz); - } - return yaw; -} - -} // namespace impl -} // namespace tf2 #endif // TF2__IMPL__UTILS_H_ diff --git a/tf2/include/tf2/impl/utils.hpp b/tf2/include/tf2/impl/utils.hpp new file mode 100644 index 000000000..03f688b66 --- /dev/null +++ b/tf2/include/tf2/impl/utils.hpp @@ -0,0 +1,170 @@ +// Copyright 2014 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TF2__IMPL__UTILS_HPP_ +#define TF2__IMPL__UTILS_HPP_ + +#include +#include +#include +#include +#include + + +namespace tf2 +{ + +// Forward declare functions needed in this header +void fromMsg(const geometry_msgs::msg::Quaternion & in, tf2::Quaternion & out); + +namespace impl +{ + +/** Function needed for the generalization of toQuaternion + * \param q a tf2::Quaternion + * \return a copy of the same quaternion + */ +inline +tf2::Quaternion toQuaternion(const tf2::Quaternion & q) +{ + return q; +} + +/** Function needed for the generalization of toQuaternion + * \param q a geometry_msgs::msg::Quaternion + * \return a copy of the same quaternion as a tf2::Quaternion + */ +inline +tf2::Quaternion toQuaternion(const geometry_msgs::msg::Quaternion & q) +{ + tf2::Quaternion res; + fromMsg(q, res); + return res; +} + +/** Function needed for the generalization of toQuaternion + * \param q a geometry_msgs::msg::QuaternionStamped + * \return a copy of the same quaternion as a tf2::Quaternion + */ +inline +tf2::Quaternion toQuaternion(const geometry_msgs::msg::QuaternionStamped & q) +{ + tf2::Quaternion res; + fromMsg(q.quaternion, res); + return res; +} + +/** Function needed for the generalization of toQuaternion + * \param t some tf2::Stamped object + * \return a copy of the same quaternion as a tf2::Quaternion + */ +template +tf2::Quaternion toQuaternion(const tf2::Stamped & t) +{ + geometry_msgs::msg::QuaternionStamped q = toMsg, + geometry_msgs::msg::QuaternionStamped>(t); + return toQuaternion(q); +} + +/** Generic version of toQuaternion. It tries to convert the argument + * to a geometry_msgs::msg::Quaternion + * \param t some object + * \return a copy of the same quaternion as a tf2::Quaternion + */ +template +tf2::Quaternion toQuaternion(const T & t) +{ + geometry_msgs::msg::Quaternion q = toMsg(t); + return toQuaternion(q); +} + +/** The code below is blantantly copied from urdfdom_headers + * only the normalization has been added. + * It computes the Euler roll, pitch yaw from a tf2::Quaternion + * It is equivalent to tf2::Matrix3x3(q).getEulerYPR(yaw, pitch, roll); + * \param q a tf2::Quaternion + * \param yaw the computed yaw + * \param pitch the computed pitch + * \param roll the computed roll + */ +inline +void getEulerYPR(const tf2::Quaternion & q, double & yaw, double & pitch, double & roll) +{ + const double pi_2 = 1.57079632679489661923; + double sqw; + double sqx; + double sqy; + double sqz; + + sqx = q.x() * q.x(); + sqy = q.y() * q.y(); + sqz = q.z() * q.z(); + sqw = q.w() * q.w(); + + // Cases derived from https://orbitalstation.wordpress.com/tag/quaternion/ + // normalization added from urdfom_headers + double sarg = -2 * (q.x() * q.z() - q.w() * q.y()) / (sqx + sqy + sqz + sqw); + if (sarg <= -0.99999) { + pitch = -0.5 * pi_2; + roll = 0; + yaw = -2 * atan2(q.y(), q.x()); + } else if (sarg >= 0.99999) { + pitch = 0.5 * pi_2; + roll = 0; + yaw = 2 * atan2(q.y(), q.x()); + } else { + pitch = asin(sarg); + roll = atan2(2 * (q.y() * q.z() + q.w() * q.x()), sqw - sqx - sqy + sqz); + yaw = atan2(2 * (q.x() * q.y() + q.w() * q.z()), sqw + sqx - sqy - sqz); + } +} + +/** The code below is a simplified version of getEulerRPY that only + * returns the yaw. It is mostly useful in navigation where only yaw + * matters + * \param q a tf2::Quaternion + * \return the computed yaw + */ +inline +double getYaw(const tf2::Quaternion & q) +{ + double yaw; + + double sqw; + double sqx; + double sqy; + double sqz; + + sqx = q.x() * q.x(); + sqy = q.y() * q.y(); + sqz = q.z() * q.z(); + sqw = q.w() * q.w(); + + // Cases derived from https://orbitalstation.wordpress.com/tag/quaternion/ + // normalization added from urdfom_headers + double sarg = -2 * (q.x() * q.z() - q.w() * q.y()) / (sqx + sqy + sqz + sqw); + + if (sarg <= -0.99999) { + yaw = -2 * atan2(q.y(), q.x()); + } else if (sarg >= 0.99999) { + yaw = 2 * atan2(q.y(), q.x()); + } else { + yaw = atan2(2 * (q.x() * q.y() + q.w() * q.z()), sqw + sqx - sqy - sqz); + } + return yaw; +} + +} // namespace impl +} // namespace tf2 +#endif // TF2__IMPL__UTILS_HPP_ diff --git a/tf2/include/tf2/time.h b/tf2/include/tf2/time.h index 408a0731c..2d69338b7 100644 --- a/tf2/include/tf2/time.h +++ b/tf2/include/tf2/time.h @@ -29,39 +29,6 @@ #ifndef TF2__TIME_H_ #define TF2__TIME_H_ -#include -#include -#include - -#include "tf2/visibility_control.h" - -namespace tf2 -{ -using Duration = std::chrono::nanoseconds; -using TimePoint = std::chrono::time_point; - -using IDuration = std::chrono::duration; -// This is the zero time in ROS -static const TimePoint TimePointZero = TimePoint(IDuration::zero()); - -TF2_PUBLIC -TimePoint get_now(); - -TF2_PUBLIC -Duration durationFromSec(double t_sec); - -TF2_PUBLIC -TimePoint timeFromSec(double t_sec); - -TF2_PUBLIC -double durationToSec(const tf2::Duration & input); - -TF2_PUBLIC -double timeToSec(const TimePoint & timepoint); - -TF2_PUBLIC -std::string displayTimePoint(const TimePoint & stamp); - -} // namespace tf2 +#include #endif // TF2__TIME_H_ diff --git a/tf2/include/tf2/time.hpp b/tf2/include/tf2/time.hpp new file mode 100644 index 000000000..c01aab84c --- /dev/null +++ b/tf2/include/tf2/time.hpp @@ -0,0 +1,67 @@ +// Copyright 2015-2016, Open Source Robotics Foundation, Inc. All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the Open Source Robotics Foundation nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#ifndef TF2__TIME_HPP_ +#define TF2__TIME_HPP_ + +#include +#include +#include + +#include "tf2/visibility_control.h" + +namespace tf2 +{ +using Duration = std::chrono::nanoseconds; +using TimePoint = std::chrono::time_point; + +using IDuration = std::chrono::duration; +// This is the zero time in ROS +static const TimePoint TimePointZero = TimePoint(IDuration::zero()); + +TF2_PUBLIC +TimePoint get_now(); + +TF2_PUBLIC +Duration durationFromSec(double t_sec); + +TF2_PUBLIC +TimePoint timeFromSec(double t_sec); + +TF2_PUBLIC +double durationToSec(const tf2::Duration & input); + +TF2_PUBLIC +double timeToSec(const TimePoint & timepoint); + +TF2_PUBLIC +std::string displayTimePoint(const TimePoint & stamp); + +} // namespace tf2 + +#endif // TF2__TIME_HPP_ diff --git a/tf2/include/tf2/time_cache.h b/tf2/include/tf2/time_cache.h index b99e0e5eb..a5aab7d59 100644 --- a/tf2/include/tf2/time_cache.h +++ b/tf2/include/tf2/time_cache.h @@ -1,4 +1,4 @@ -// Copyright 2008, Willow Garage, Inc. All rights reserved. +// Copyright 2015-2016, Open Source Robotics Foundation, Inc. All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: @@ -10,7 +10,7 @@ // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // -// * Neither the name of the Willow Garage nor the names of its +// * Neither the name of the Open Source Robotics Foundation nor the names of its // contributors may be used to endorse or promote products derived from // this software without specific prior written permission. // @@ -25,169 +25,10 @@ // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE // POSSIBILITY OF SUCH DAMAGE. -/** \author Tully Foote */ #ifndef TF2__TIME_CACHE_H_ #define TF2__TIME_CACHE_H_ -#include -#include -#include -#include -#include -#include +#include -#include "tf2/visibility_control.h" -#include "tf2/transform_storage.h" -#include "tf2/exceptions.h" - -namespace tf2 -{ -typedef std::pair P_TimeAndFrameID; - -class TimeCacheInterface -{ -public: - TF2_PUBLIC - virtual ~TimeCacheInterface() = default; - - /** \brief Access data from the cache - * returns false if data unavailable (should be thrown as lookup exception) - */ - TF2_PUBLIC - virtual bool getData( - tf2::TimePoint time, tf2::TransformStorage & data_out, - std::string * error_str = 0, TF2Error * error_code = 0) = 0; - - /** \brief Insert data into the cache */ - TF2_PUBLIC - virtual bool insertData(const tf2::TransformStorage & new_data) = 0; - - /** @brief Clear the list of stored values */ - TF2_PUBLIC - virtual void clearList() = 0; - - /** \brief Retrieve the parent at a specific time */ - TF2_PUBLIC - virtual CompactFrameID getParent( - tf2::TimePoint time, std::string * error_str = 0, TF2Error * error_code = 0) = 0; - - /** - * \brief Get the latest time stored in this cache, and the parent associated with it. Returns parent = 0 if no data. - */ - TF2_PUBLIC - virtual P_TimeAndFrameID getLatestTimeAndParent() = 0; - - /// Debugging information methods - /** @brief Get the length of the stored list */ - TF2_PUBLIC - virtual unsigned int getListLength() = 0; - - /** @brief Get the latest timestamp cached */ - TF2_PUBLIC - virtual tf2::TimePoint getLatestTimestamp() = 0; - - /** @brief Get the oldest timestamp cached */ - TF2_PUBLIC - virtual tf2::TimePoint getOldestTimestamp() = 0; -}; - -using TimeCacheInterfacePtr = std::shared_ptr; - -/// default value of 10 seconds storage -constexpr tf2::Duration TIMECACHE_DEFAULT_MAX_STORAGE_TIME = std::chrono::seconds(10); - -/** \brief A class to keep a sorted linked list in time (newest first, oldest - * last). - * This builds and maintains a list of timestamped - * data. And provides lookup functions to get - * data out as a function of time. */ -class TimeCache : public TimeCacheInterface -{ -public: - TF2_PUBLIC - explicit TimeCache(tf2::Duration max_storage_time = TIMECACHE_DEFAULT_MAX_STORAGE_TIME); - - /// Virtual methods - - TF2_PUBLIC - virtual bool getData( - tf2::TimePoint time, tf2::TransformStorage & data_out, - std::string * error_str = 0, TF2Error * error_code = 0); - TF2_PUBLIC - virtual bool insertData(const tf2::TransformStorage & new_data); - TF2_PUBLIC - virtual void clearList(); - TF2_PUBLIC - virtual tf2::CompactFrameID getParent( - tf2::TimePoint time, std::string * error_str = 0, TF2Error * error_code = 0); - TF2_PUBLIC - virtual P_TimeAndFrameID getLatestTimeAndParent(); - - /// Debugging information methods - TF2_PUBLIC - virtual unsigned int getListLength(); - TF2_PUBLIC - virtual TimePoint getLatestTimestamp(); - TF2_PUBLIC - virtual TimePoint getOldestTimestamp(); - -protected: - // (Internal) Return a reference to the internal list of tf2 frames, which - // are sorted in timestamp order. - // Any items with the same timestamp will be in reverse order of insertion. - TF2_PUBLIC - const std::list & getAllItems() const; - -private: - typedef std::list L_TransformStorage; - L_TransformStorage storage_; - - tf2::Duration max_storage_time_; - - - // A helper function for getData - // Assumes storage is already locked for it - inline uint8_t findClosest( - tf2::TransformStorage * & one, TransformStorage * & two, - tf2::TimePoint target_time, std::string * error_str = 0, TF2Error * error_code = 0); - - inline void interpolate( - const tf2::TransformStorage & one, const tf2::TransformStorage & two, - tf2::TimePoint time, tf2::TransformStorage & output); - - void pruneList(); -}; - -class StaticCache : public TimeCacheInterface -{ -public: - /// Virtual methods - TF2_PUBLIC - virtual bool getData( - TimePoint time, TransformStorage & data_out, - std::string * error_str = 0, TF2Error * error_code = 0); - // returns false if data unavailable (should be thrown as lookup exception - TF2_PUBLIC - virtual bool insertData(const TransformStorage & new_data); - TF2_PUBLIC - virtual void clearList(); - TF2_PUBLIC - virtual CompactFrameID getParent( - TimePoint time, std::string * error_str = 0, TF2Error * error_code = 0); - TF2_PUBLIC - virtual P_TimeAndFrameID getLatestTimeAndParent(); - - /// Debugging information methods - TF2_PUBLIC - virtual unsigned int getListLength(); - TF2_PUBLIC - virtual TimePoint getLatestTimestamp(); - TF2_PUBLIC - virtual TimePoint getOldestTimestamp(); - -private: - TransformStorage storage_; -}; -} // namespace tf2 #endif // TF2__TIME_CACHE_H_ diff --git a/tf2/include/tf2/time_cache.hpp b/tf2/include/tf2/time_cache.hpp new file mode 100644 index 000000000..cc7cc5d0a --- /dev/null +++ b/tf2/include/tf2/time_cache.hpp @@ -0,0 +1,193 @@ +// Copyright 2008, Willow Garage, Inc. All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the Willow Garage nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +/** \author Tully Foote */ + +#ifndef TF2__TIME_CACHE_HPP_ +#define TF2__TIME_CACHE_HPP_ + +#include +#include +#include +#include +#include +#include + +#include "tf2/visibility_control.h" +#include "tf2/transform_storage.hpp" +#include "tf2/exceptions.hpp" + +namespace tf2 +{ +typedef std::pair P_TimeAndFrameID; + +class TimeCacheInterface +{ +public: + TF2_PUBLIC + virtual ~TimeCacheInterface() = default; + + /** \brief Access data from the cache + * returns false if data unavailable (should be thrown as lookup exception) + */ + TF2_PUBLIC + virtual bool getData( + tf2::TimePoint time, tf2::TransformStorage & data_out, + std::string * error_str = 0, TF2Error * error_code = 0) = 0; + + /** \brief Insert data into the cache */ + TF2_PUBLIC + virtual bool insertData(const tf2::TransformStorage & new_data) = 0; + + /** @brief Clear the list of stored values */ + TF2_PUBLIC + virtual void clearList() = 0; + + /** \brief Retrieve the parent at a specific time */ + TF2_PUBLIC + virtual CompactFrameID getParent( + tf2::TimePoint time, std::string * error_str = 0, TF2Error * error_code = 0) = 0; + + /** + * \brief Get the latest time stored in this cache, and the parent associated with it. Returns parent = 0 if no data. + */ + TF2_PUBLIC + virtual P_TimeAndFrameID getLatestTimeAndParent() = 0; + + /// Debugging information methods + /** @brief Get the length of the stored list */ + TF2_PUBLIC + virtual unsigned int getListLength() = 0; + + /** @brief Get the latest timestamp cached */ + TF2_PUBLIC + virtual tf2::TimePoint getLatestTimestamp() = 0; + + /** @brief Get the oldest timestamp cached */ + TF2_PUBLIC + virtual tf2::TimePoint getOldestTimestamp() = 0; +}; + +using TimeCacheInterfacePtr = std::shared_ptr; + +/// default value of 10 seconds storage +constexpr tf2::Duration TIMECACHE_DEFAULT_MAX_STORAGE_TIME = std::chrono::seconds(10); + +/** \brief A class to keep a sorted linked list in time (newest first, oldest + * last). + * This builds and maintains a list of timestamped + * data. And provides lookup functions to get + * data out as a function of time. */ +class TimeCache : public TimeCacheInterface +{ +public: + TF2_PUBLIC + explicit TimeCache(tf2::Duration max_storage_time = TIMECACHE_DEFAULT_MAX_STORAGE_TIME); + + /// Virtual methods + + TF2_PUBLIC + virtual bool getData( + tf2::TimePoint time, tf2::TransformStorage & data_out, + std::string * error_str = 0, TF2Error * error_code = 0); + TF2_PUBLIC + virtual bool insertData(const tf2::TransformStorage & new_data); + TF2_PUBLIC + virtual void clearList(); + TF2_PUBLIC + virtual tf2::CompactFrameID getParent( + tf2::TimePoint time, std::string * error_str = 0, TF2Error * error_code = 0); + TF2_PUBLIC + virtual P_TimeAndFrameID getLatestTimeAndParent(); + + /// Debugging information methods + TF2_PUBLIC + virtual unsigned int getListLength(); + TF2_PUBLIC + virtual TimePoint getLatestTimestamp(); + TF2_PUBLIC + virtual TimePoint getOldestTimestamp(); + +protected: + // (Internal) Return a reference to the internal list of tf2 frames, which + // are sorted in timestamp order. + // Any items with the same timestamp will be in reverse order of insertion. + TF2_PUBLIC + const std::list & getAllItems() const; + +private: + typedef std::list L_TransformStorage; + L_TransformStorage storage_; + + tf2::Duration max_storage_time_; + + + // A helper function for getData + // Assumes storage is already locked for it + inline uint8_t findClosest( + tf2::TransformStorage * & one, TransformStorage * & two, + tf2::TimePoint target_time, std::string * error_str = 0, TF2Error * error_code = 0); + + inline void interpolate( + const tf2::TransformStorage & one, const tf2::TransformStorage & two, + tf2::TimePoint time, tf2::TransformStorage & output); + + void pruneList(); +}; + +class StaticCache : public TimeCacheInterface +{ +public: + /// Virtual methods + TF2_PUBLIC + virtual bool getData( + TimePoint time, TransformStorage & data_out, + std::string * error_str = 0, TF2Error * error_code = 0); + // returns false if data unavailable (should be thrown as lookup exception + TF2_PUBLIC + virtual bool insertData(const TransformStorage & new_data); + TF2_PUBLIC + virtual void clearList(); + TF2_PUBLIC + virtual CompactFrameID getParent( + TimePoint time, std::string * error_str = 0, TF2Error * error_code = 0); + TF2_PUBLIC + virtual P_TimeAndFrameID getLatestTimeAndParent(); + + /// Debugging information methods + TF2_PUBLIC + virtual unsigned int getListLength(); + TF2_PUBLIC + virtual TimePoint getLatestTimestamp(); + TF2_PUBLIC + virtual TimePoint getOldestTimestamp(); + +private: + TransformStorage storage_; +}; +} // namespace tf2 +#endif // TF2__TIME_CACHE_HPP_ diff --git a/tf2/include/tf2/transform_datatypes.h b/tf2/include/tf2/transform_datatypes.h index bbd9e164f..ae002fbf3 100644 --- a/tf2/include/tf2/transform_datatypes.h +++ b/tf2/include/tf2/transform_datatypes.h @@ -31,125 +31,6 @@ #ifndef TF2__TRANSFORM_DATATYPES_H_ #define TF2__TRANSFORM_DATATYPES_H_ -#include -#include -#include - -#include "tf2/time.h" - -namespace tf2 -{ - -/** \brief The data type which will be cross compatable with geometry_msgs - * This is the tf2 datatype equivilant of a MessageStamped */ -template -class Stamped : public T -{ -public: - TimePoint stamp_; ///< The timestamp associated with this data - std::string frame_id_; ///< The frame_id associated this data - - /** Default constructor */ - Stamped() - : frame_id_("NO_ID_STAMPED_DEFAULT_CONSTRUCTION") - { - } - - /** Full constructor */ - Stamped(const T & input, const TimePoint & timestamp, const std::string & frame_id) - : T(input), stamp_(timestamp), frame_id_(frame_id) - { - } - - /** Copy Constructor */ - Stamped(const Stamped & s) - : T(s), - stamp_(s.stamp_), - frame_id_(s.frame_id_) - { - } - - /** Set the data element */ - void setData(const T & input) {*static_cast(this) = input;} - - Stamped & operator=(const Stamped & s) - { - T::operator=(s); - this->stamp_ = s.stamp_; - this->frame_id_ = s.frame_id_; - return *this; - } -}; - -/** \brief Comparison Operator for Stamped datatypes */ -template -bool operator==(const Stamped & a, const Stamped & b) -{ - return a.frame_id_ == b.frame_id_ && a.stamp_ == b.stamp_ && - static_cast(a) == static_cast(b); -} - -/** \brief The data type which will be cross compatable with geometry_msgs - * This is the tf2 datatype equivalent of a MessageWithCovarianceStamped */ -template -class WithCovarianceStamped : public T -{ -public: - TimePoint stamp_; ///< The timestamp associated with this data - std::string frame_id_; ///< The frame_id associated this data - std::array, 6> cov_mat_; ///< The covariance matrix associated with this data // NOLINT - - /** Default constructor */ - WithCovarianceStamped() - : frame_id_("NO_ID_STAMPED_DEFAULT_CONSTRUCTION"), - cov_mat_{} - { - } - - /** Full constructor */ - WithCovarianceStamped( - const T & input, - const TimePoint & timestamp, - const std::string & frame_id, - const std::array, 6> & covariance_matrix - ) - : T(input), - stamp_(timestamp), - frame_id_(frame_id), - cov_mat_(covariance_matrix) - { - } - - /** Copy constructor */ - WithCovarianceStamped(const WithCovarianceStamped & w) - : T(w), - stamp_(w.stamp_), - frame_id_(w.frame_id_), - cov_mat_(w.cov_mat_) - { - } - - /** Set the data element */ - void setData(const T & input) {*static_cast(this) = input;} - - WithCovarianceStamped & operator=(const WithCovarianceStamped & w) - { - T::operator=(w); - this->stamp_ = w.stamp_; - this->frame_id_ = w.frame_id_; - this->cov_mat_ = w.cov_mat_; - return *this; - } -}; - -/** \brief Comparison operator for WithCovarianceStamped datatypes */ -template -bool operator==(const WithCovarianceStamped & a, const WithCovarianceStamped & b) -{ - return a.frame_id_ == b.frame_id_ && a.stamp_ == b.stamp_ && - a.cov_mat_ == b.cov_mat_ && static_cast(a) == static_cast(b); -} - -} // namespace tf2 +#include #endif // TF2__TRANSFORM_DATATYPES_H_ diff --git a/tf2/include/tf2/transform_datatypes.hpp b/tf2/include/tf2/transform_datatypes.hpp new file mode 100644 index 000000000..5cedbbc2c --- /dev/null +++ b/tf2/include/tf2/transform_datatypes.hpp @@ -0,0 +1,155 @@ +// Copyright 2008, Willow Garage, Inc. All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the Willow Garage nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +/** \author Tully Foote */ + +#ifndef TF2__TRANSFORM_DATATYPES_HPP_ +#define TF2__TRANSFORM_DATATYPES_HPP_ + +#include +#include +#include + +#include "tf2/time.hpp" + +namespace tf2 +{ + +/** \brief The data type which will be cross compatable with geometry_msgs + * This is the tf2 datatype equivilant of a MessageStamped */ +template +class Stamped : public T +{ +public: + TimePoint stamp_; ///< The timestamp associated with this data + std::string frame_id_; ///< The frame_id associated this data + + /** Default constructor */ + Stamped() + : frame_id_("NO_ID_STAMPED_DEFAULT_CONSTRUCTION") + { + } + + /** Full constructor */ + Stamped(const T & input, const TimePoint & timestamp, const std::string & frame_id) + : T(input), stamp_(timestamp), frame_id_(frame_id) + { + } + + /** Copy Constructor */ + Stamped(const Stamped & s) + : T(s), + stamp_(s.stamp_), + frame_id_(s.frame_id_) + { + } + + /** Set the data element */ + void setData(const T & input) {*static_cast(this) = input;} + + Stamped & operator=(const Stamped & s) + { + T::operator=(s); + this->stamp_ = s.stamp_; + this->frame_id_ = s.frame_id_; + return *this; + } +}; + +/** \brief Comparison Operator for Stamped datatypes */ +template +bool operator==(const Stamped & a, const Stamped & b) +{ + return a.frame_id_ == b.frame_id_ && a.stamp_ == b.stamp_ && + static_cast(a) == static_cast(b); +} + +/** \brief The data type which will be cross compatable with geometry_msgs + * This is the tf2 datatype equivalent of a MessageWithCovarianceStamped */ +template +class WithCovarianceStamped : public T +{ +public: + TimePoint stamp_; ///< The timestamp associated with this data + std::string frame_id_; ///< The frame_id associated this data + std::array, 6> cov_mat_; ///< The covariance matrix associated with this data // NOLINT + + /** Default constructor */ + WithCovarianceStamped() + : frame_id_("NO_ID_STAMPED_DEFAULT_CONSTRUCTION"), + cov_mat_{} + { + } + + /** Full constructor */ + WithCovarianceStamped( + const T & input, + const TimePoint & timestamp, + const std::string & frame_id, + const std::array, 6> & covariance_matrix + ) + : T(input), + stamp_(timestamp), + frame_id_(frame_id), + cov_mat_(covariance_matrix) + { + } + + /** Copy constructor */ + WithCovarianceStamped(const WithCovarianceStamped & w) + : T(w), + stamp_(w.stamp_), + frame_id_(w.frame_id_), + cov_mat_(w.cov_mat_) + { + } + + /** Set the data element */ + void setData(const T & input) {*static_cast(this) = input;} + + WithCovarianceStamped & operator=(const WithCovarianceStamped & w) + { + T::operator=(w); + this->stamp_ = w.stamp_; + this->frame_id_ = w.frame_id_; + this->cov_mat_ = w.cov_mat_; + return *this; + } +}; + +/** \brief Comparison operator for WithCovarianceStamped datatypes */ +template +bool operator==(const WithCovarianceStamped & a, const WithCovarianceStamped & b) +{ + return a.frame_id_ == b.frame_id_ && a.stamp_ == b.stamp_ && + a.cov_mat_ == b.cov_mat_ && static_cast(a) == static_cast(b); +} + +} // namespace tf2 + +#endif // TF2__TRANSFORM_DATATYPES_HPP_ diff --git a/tf2/include/tf2/transform_storage.h b/tf2/include/tf2/transform_storage.h index df0c4dcba..1f8d2872a 100644 --- a/tf2/include/tf2/transform_storage.h +++ b/tf2/include/tf2/transform_storage.h @@ -30,64 +30,6 @@ #ifndef TF2__TRANSFORM_STORAGE_H_ #define TF2__TRANSFORM_STORAGE_H_ -#include "tf2/LinearMath/Vector3.h" -#include "tf2/LinearMath/Quaternion.h" -#include "tf2/time.h" -#include "tf2/visibility_control.h" +#include -namespace tf2 -{ -typedef uint32_t CompactFrameID; - -/** \brief Storage for transforms and their parent */ -class TransformStorage -{ -public: - TF2_PUBLIC - TransformStorage(); - TF2_PUBLIC - TransformStorage( - const TimePoint & stamp, const Quaternion & q, const Vector3 & t, CompactFrameID frame_id, - CompactFrameID child_frame_id); - - TF2_PUBLIC - TransformStorage(const TransformStorage & rhs) - { - *this = rhs; - } - - TF2_PUBLIC - TransformStorage & operator=(const TransformStorage & rhs) - { - rotation_ = rhs.rotation_; - translation_ = rhs.translation_; - stamp_ = rhs.stamp_; - frame_id_ = rhs.frame_id_; - child_frame_id_ = rhs.child_frame_id_; - return *this; - } - - TF2_PUBLIC - bool operator==(const TransformStorage & rhs) const - { - return (this->rotation_ == rhs.rotation_) && - (this->translation_ == rhs.translation_) && - (this->stamp_ == rhs.stamp_) && - (this->frame_id_ == rhs.frame_id_) && - (this->child_frame_id_ == rhs.child_frame_id_); - } - - TF2_PUBLIC - bool operator!=(const TransformStorage & rhs) const - { - return !(*this == rhs); - } - - tf2::Quaternion rotation_; - tf2::Vector3 translation_; - TimePoint stamp_; - CompactFrameID frame_id_; - CompactFrameID child_frame_id_; -}; -} // namespace tf2 #endif // TF2__TRANSFORM_STORAGE_H_ diff --git a/tf2/include/tf2/transform_storage.hpp b/tf2/include/tf2/transform_storage.hpp new file mode 100644 index 000000000..2f285d845 --- /dev/null +++ b/tf2/include/tf2/transform_storage.hpp @@ -0,0 +1,93 @@ +// Copyright 2010, Willow Garage, Inc. All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the Willow Garage nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +/** \author Tully Foote */ + +#ifndef TF2__TRANSFORM_STORAGE_HPP_ +#define TF2__TRANSFORM_STORAGE_HPP_ + +#include "tf2/visibility_control.h" +#include "tf2/LinearMath/Vector3.hpp" +#include "tf2/LinearMath/Quaternion.hpp" +#include "tf2/time.hpp" + +namespace tf2 +{ +typedef uint32_t CompactFrameID; + +/** \brief Storage for transforms and their parent */ +class TransformStorage +{ +public: + TF2_PUBLIC + TransformStorage(); + TF2_PUBLIC + TransformStorage( + const TimePoint & stamp, const Quaternion & q, const Vector3 & t, CompactFrameID frame_id, + CompactFrameID child_frame_id); + + TF2_PUBLIC + TransformStorage(const TransformStorage & rhs) + { + *this = rhs; + } + + TF2_PUBLIC + TransformStorage & operator=(const TransformStorage & rhs) + { + rotation_ = rhs.rotation_; + translation_ = rhs.translation_; + stamp_ = rhs.stamp_; + frame_id_ = rhs.frame_id_; + child_frame_id_ = rhs.child_frame_id_; + return *this; + } + + TF2_PUBLIC + bool operator==(const TransformStorage & rhs) const + { + return (this->rotation_ == rhs.rotation_) && + (this->translation_ == rhs.translation_) && + (this->stamp_ == rhs.stamp_) && + (this->frame_id_ == rhs.frame_id_) && + (this->child_frame_id_ == rhs.child_frame_id_); + } + + TF2_PUBLIC + bool operator!=(const TransformStorage & rhs) const + { + return !(*this == rhs); + } + + tf2::Quaternion rotation_; + tf2::Vector3 translation_; + TimePoint stamp_; + CompactFrameID frame_id_; + CompactFrameID child_frame_id_; +}; +} // namespace tf2 +#endif // TF2__TRANSFORM_STORAGE_HPP_ diff --git a/tf2/include/tf2/utils.h b/tf2/include/tf2/utils.h index cbd3929e6..c259609bc 100644 --- a/tf2/include/tf2/utils.h +++ b/tf2/include/tf2/utils.h @@ -15,52 +15,6 @@ #ifndef TF2__UTILS_H_ #define TF2__UTILS_H_ -#include -#include -#include -#include +#include -namespace tf2 -{ -/** Return the yaw, pitch, roll of anything that can be converted to a tf2::Quaternion - * The conventions are the usual ROS ones defined in tf2/LineMath/Matrix3x3.h - * \param a the object to get data from (it represents a rotation/quaternion) - * \param yaw yaw - * \param pitch pitch - * \param roll roll - */ -template -void getEulerYPR(const A & a, double & yaw, double & pitch, double & roll) -{ - tf2::Quaternion q = impl::toQuaternion(a); - impl::getEulerYPR(q, yaw, pitch, roll); -} - -/** Return the yaw of anything that can be converted to a tf2::Quaternion - * The conventions are the usual ROS ones defined in tf2/LineMath/Matrix3x3.h - * This function is a specialization of getEulerYPR and is useful for its - * wide-spread use in navigation - * \param a the object to get data from (it represents a rotation/quaternion) - * \return yaw - */ -template -double getYaw(const A & a) -{ - tf2::Quaternion q = impl::toQuaternion(a); - return impl::getYaw(q); -} - -/** Return the identity for any type that can be converted to a tf2::Transform - * \return an object of class A that is an identity transform - */ -template -A getTransformIdentity() -{ - tf2::Transform t; - t.setIdentity(); - A a; - convert(t, a); - return a; -} -} // namespace tf2 #endif // TF2__UTILS_H_ diff --git a/tf2/include/tf2/utils.hpp b/tf2/include/tf2/utils.hpp new file mode 100644 index 000000000..f275d7759 --- /dev/null +++ b/tf2/include/tf2/utils.hpp @@ -0,0 +1,67 @@ +// Copyright 2014 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TF2__UTILS_HPP_ +#define TF2__UTILS_HPP_ + +#include + +#include +#include +#include + +namespace tf2 +{ +/** Return the yaw, pitch, roll of anything that can be converted to a tf2::Quaternion + * The conventions are the usual ROS ones defined in tf2/LineMath/Matrix3x3.h + * \param a the object to get data from (it represents a rotation/quaternion) + * \param yaw yaw + * \param pitch pitch + * \param roll roll + */ +template +void getEulerYPR(const A & a, double & yaw, double & pitch, double & roll) +{ + tf2::Quaternion q = impl::toQuaternion(a); + impl::getEulerYPR(q, yaw, pitch, roll); +} + +/** Return the yaw of anything that can be converted to a tf2::Quaternion + * The conventions are the usual ROS ones defined in tf2/LineMath/Matrix3x3.h + * This function is a specialization of getEulerYPR and is useful for its + * wide-spread use in navigation + * \param a the object to get data from (it represents a rotation/quaternion) + * \return yaw + */ +template +double getYaw(const A & a) +{ + tf2::Quaternion q = impl::toQuaternion(a); + return impl::getYaw(q); +} + +/** Return the identity for any type that can be converted to a tf2::Transform + * \return an object of class A that is an identity transform + */ +template +A getTransformIdentity() +{ + tf2::Transform t; + t.setIdentity(); + A a; + convert(t, a); + return a; +} +} // namespace tf2 +#endif // TF2__UTILS_HPP_ diff --git a/tf2/src/buffer_core.cpp b/tf2/src/buffer_core.cpp index f4333729c..887e637c4 100644 --- a/tf2/src/buffer_core.cpp +++ b/tf2/src/buffer_core.cpp @@ -40,13 +40,13 @@ #include -#include "tf2/buffer_core.h" -#include "tf2/time_cache.h" -#include "tf2/exceptions.h" +#include "tf2/buffer_core.hpp" +#include "tf2/time_cache.hpp" +#include "tf2/exceptions.hpp" -#include "tf2/LinearMath/Quaternion.h" -#include "tf2/LinearMath/Transform.h" -#include "tf2/LinearMath/Vector3.h" +#include "tf2/LinearMath/Quaternion.hpp" +#include "tf2/LinearMath/Transform.hpp" +#include "tf2/LinearMath/Vector3.hpp" #include "builtin_interfaces/msg/time.hpp" #include "geometry_msgs/msg/transform.hpp" diff --git a/tf2/src/cache.cpp b/tf2/src/cache.cpp index 607fc1523..a7910617c 100644 --- a/tf2/src/cache.cpp +++ b/tf2/src/cache.cpp @@ -35,12 +35,12 @@ #include #include -#include "tf2/time_cache.h" -#include "tf2/exceptions.h" +#include "tf2/time_cache.hpp" +#include "tf2/exceptions.hpp" -#include "tf2/LinearMath/Vector3.h" -#include "tf2/LinearMath/Quaternion.h" -#include "tf2/LinearMath/Transform.h" +#include "tf2/LinearMath/Vector3.hpp" +#include "tf2/LinearMath/Quaternion.hpp" +#include "tf2/LinearMath/Transform.hpp" namespace tf2 { diff --git a/tf2/src/static_cache.cpp b/tf2/src/static_cache.cpp index b771f1c74..2c2bad202 100644 --- a/tf2/src/static_cache.cpp +++ b/tf2/src/static_cache.cpp @@ -31,10 +31,10 @@ #include #include -#include "tf2/time_cache.h" -#include "tf2/exceptions.h" +#include "tf2/time_cache.hpp" +#include "tf2/exceptions.hpp" -#include "tf2/LinearMath/Transform.h" +#include "tf2/LinearMath/Transform.hpp" bool tf2::StaticCache::getData( tf2::TimePoint time, diff --git a/tf2/src/time.cpp b/tf2/src/time.cpp index dfa09b128..318e7b1bf 100644 --- a/tf2/src/time.cpp +++ b/tf2/src/time.cpp @@ -34,7 +34,7 @@ #include "rcutils/snprintf.h" #include "rcutils/strerror.h" -#include "tf2/time.h" +#include "tf2/time.hpp" tf2::TimePoint tf2::get_now() { diff --git a/tf2/test/cache_benchmark.cpp b/tf2/test/cache_benchmark.cpp index af7241f08..beebb2345 100644 --- a/tf2/test/cache_benchmark.cpp +++ b/tf2/test/cache_benchmark.cpp @@ -32,7 +32,7 @@ #include #include -#include "tf2/time_cache.h" +#include "tf2/time_cache.hpp" // Simulates 5 transforms, 10s worth of data at 200 Hz in a buffer that is // completely full. diff --git a/tf2/test/cache_unittest.cpp b/tf2/test/cache_unittest.cpp index e00aa0c10..71fbd9bd9 100644 --- a/tf2/test/cache_unittest.cpp +++ b/tf2/test/cache_unittest.cpp @@ -36,8 +36,8 @@ #include #include -#include "tf2/time_cache.h" -#include "tf2/LinearMath/Quaternion.h" +#include "tf2/time_cache.hpp" +#include "tf2/LinearMath/Quaternion.hpp" std::vector values; unsigned int step = 0; diff --git a/tf2/test/simple_tf2_core.cpp b/tf2/test/simple_tf2_core.cpp index e2ec56c91..b431f3886 100644 --- a/tf2/test/simple_tf2_core.cpp +++ b/tf2/test/simple_tf2_core.cpp @@ -38,11 +38,11 @@ #include "builtin_interfaces/msg/time.hpp" #include "geometry_msgs/msg/transform_stamped.hpp" -#include "tf2/buffer_core.h" -#include "tf2/convert.h" -#include "tf2/LinearMath/Vector3.h" -#include "tf2/exceptions.h" -#include "tf2/time.h" +#include "tf2/buffer_core.hpp" +#include "tf2/convert.hpp" +#include "tf2/LinearMath/Vector3.hpp" +#include "tf2/exceptions.hpp" +#include "tf2/time.hpp" TEST(tf2, setTransformFail) { diff --git a/tf2/test/static_cache_test.cpp b/tf2/test/static_cache_test.cpp index 24d429020..ad2f1469f 100644 --- a/tf2/test/static_cache_test.cpp +++ b/tf2/test/static_cache_test.cpp @@ -26,14 +26,14 @@ // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE // POSSIBILITY OF SUCH DAMAGE. -#include - #include #include #include #include +#include + void setIdentity(tf2::TransformStorage & stor) { stor.translation_.setValue(0.0, 0.0, 0.0); diff --git a/tf2/test/test_storage.cpp b/tf2/test/test_storage.cpp index 6a0d0eae5..55db70ed6 100644 --- a/tf2/test/test_storage.cpp +++ b/tf2/test/test_storage.cpp @@ -28,10 +28,10 @@ #include -#include "tf2/LinearMath/Quaternion.h" -#include "tf2/LinearMath/Vector3.h" -#include "tf2/time.h" -#include "tf2/transform_storage.h" +#include "tf2/LinearMath/Quaternion.hpp" +#include "tf2/LinearMath/Vector3.hpp" +#include "tf2/time.hpp" +#include "tf2/transform_storage.hpp" class TransformStorageTest : public ::testing::Test { diff --git a/tf2/test/test_time.cpp b/tf2/test/test_time.cpp index 1629c2d89..924eb1823 100644 --- a/tf2/test/test_time.cpp +++ b/tf2/test/test_time.cpp @@ -28,7 +28,7 @@ #include #include -#include "tf2/time.h" +#include "tf2/time.hpp" using namespace std::literals::chrono_literals; diff --git a/tf2_bullet/include/tf2_bullet/tf2_bullet.hpp b/tf2_bullet/include/tf2_bullet/tf2_bullet.hpp index 061e076f6..f0f3ef296 100644 --- a/tf2_bullet/include/tf2_bullet/tf2_bullet.hpp +++ b/tf2_bullet/include/tf2_bullet/tf2_bullet.hpp @@ -33,7 +33,7 @@ #include -#include "tf2/convert.h" +#include "tf2/convert.hpp" #include "LinearMath/btQuaternion.h" #include "LinearMath/btScalar.h" #include "LinearMath/btTransform.h" diff --git a/tf2_bullet/test/test_tf2_bullet.cpp b/tf2_bullet/test/test_tf2_bullet.cpp index 9678d8b2a..09a4f99b8 100644 --- a/tf2_bullet/test/test_tf2_bullet.cpp +++ b/tf2_bullet/test/test_tf2_bullet.cpp @@ -31,7 +31,7 @@ #include "tf2_bullet/tf2_bullet.hpp" #include "rclcpp/rclcpp.hpp" #include "gtest/gtest.h" -#include "tf2/convert.h" +#include "tf2/convert.hpp" TEST(TfBullet, ConvertVector) { diff --git a/tf2_eigen/include/tf2_eigen/tf2_eigen.hpp b/tf2_eigen/include/tf2_eigen/tf2_eigen.hpp index da5ee6057..8c5247ef9 100644 --- a/tf2_eigen/include/tf2_eigen/tf2_eigen.hpp +++ b/tf2_eigen/include/tf2_eigen/tf2_eigen.hpp @@ -38,7 +38,7 @@ #include "geometry_msgs/msg/quaternion_stamped.hpp" #include "geometry_msgs/msg/twist.hpp" -#include "tf2/convert.h" +#include "tf2/convert.hpp" #include "tf2_ros/buffer.h" #include "tf2_ros/buffer_interface.h" diff --git a/tf2_eigen/test/tf2_eigen-test.cpp b/tf2_eigen/test/tf2_eigen-test.cpp index 7bd02a1ea..d9ab12a60 100644 --- a/tf2_eigen/test/tf2_eigen-test.cpp +++ b/tf2_eigen/test/tf2_eigen-test.cpp @@ -61,8 +61,8 @@ #include "geometry_msgs/msg/transform_stamped.hpp" #include "rclcpp/clock.hpp" -#include "tf2/convert.h" -#include "tf2/transform_datatypes.h" +#include "tf2/convert.hpp" +#include "tf2/transform_datatypes.hpp" #include "tf2_eigen/tf2_eigen.hpp" #include "tf2_ros/buffer.h" #include "tf2_ros/transform_listener.h" diff --git a/tf2_eigen_kdl/include/tf2_eigen_kdl/tf2_eigen_kdl.hpp b/tf2_eigen_kdl/include/tf2_eigen_kdl/tf2_eigen_kdl.hpp index d600afc94..85450cf3a 100644 --- a/tf2_eigen_kdl/include/tf2_eigen_kdl/tf2_eigen_kdl.hpp +++ b/tf2_eigen_kdl/include/tf2_eigen_kdl/tf2_eigen_kdl.hpp @@ -55,7 +55,7 @@ #include "kdl/frames.hpp" -#include "tf2/impl/convert.h" +#include "tf2/impl/convert.hpp" #include "tf2_eigen_kdl/visibility_control.h" diff --git a/tf2_eigen_kdl/test/tf2_eigen_kdl_test.cpp b/tf2_eigen_kdl/test/tf2_eigen_kdl_test.cpp index 9e3608bfd..9f85f5d87 100644 --- a/tf2_eigen_kdl/test/tf2_eigen_kdl_test.cpp +++ b/tf2_eigen_kdl/test/tf2_eigen_kdl_test.cpp @@ -30,10 +30,11 @@ // ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE // POSSIBILITY OF SUCH DAMAGE. -#include -#include #include +#include +#include + using Vector6d = Eigen::Matrix; TEST(TfEigenKdl, TestRotationQuaternion) diff --git a/tf2_geometry_msgs/include/tf2_geometry_msgs/tf2_geometry_msgs.hpp b/tf2_geometry_msgs/include/tf2_geometry_msgs/tf2_geometry_msgs.hpp index 2a60fa3ae..784fa31fe 100644 --- a/tf2_geometry_msgs/include/tf2_geometry_msgs/tf2_geometry_msgs.hpp +++ b/tf2_geometry_msgs/include/tf2_geometry_msgs/tf2_geometry_msgs.hpp @@ -53,10 +53,10 @@ #include "geometry_msgs/msg/wrench_stamped.hpp" #include "kdl/frames.hpp" -#include "tf2/convert.h" -#include "tf2/LinearMath/Quaternion.h" -#include "tf2/LinearMath/Transform.h" -#include "tf2/LinearMath/Vector3.h" +#include "tf2/convert.hpp" +#include "tf2/LinearMath/Quaternion.hpp" +#include "tf2/LinearMath/Transform.hpp" +#include "tf2/LinearMath/Vector3.hpp" #include "tf2_ros/buffer_interface.h" namespace tf2 diff --git a/tf2_kdl/include/tf2_kdl/tf2_kdl.hpp b/tf2_kdl/include/tf2_kdl/tf2_kdl.hpp index b183b564b..a8ec978be 100644 --- a/tf2_kdl/include/tf2_kdl/tf2_kdl.hpp +++ b/tf2_kdl/include/tf2_kdl/tf2_kdl.hpp @@ -32,7 +32,7 @@ #ifndef TF2_KDL_HPP #define TF2_KDL_HPP -#include +#include #include #include #include diff --git a/tf2_kdl/test/test_tf2_kdl.cpp b/tf2_kdl/test/test_tf2_kdl.cpp index 33d083a8e..cad3cf5a2 100644 --- a/tf2_kdl/test/test_tf2_kdl.cpp +++ b/tf2_kdl/test/test_tf2_kdl.cpp @@ -43,7 +43,7 @@ #include #include #include "tf2_ros/buffer.h" -#include +#include std::unique_ptr tf_buffer; static const double EPS = 1e-3; diff --git a/tf2_py/src/tf2_py.cpp b/tf2_py/src/tf2_py.cpp index ab6c2b3ce..e34f6a79f 100644 --- a/tf2_py/src/tf2_py.cpp +++ b/tf2_py/src/tf2_py.cpp @@ -1,7 +1,7 @@ #include -#include -#include +#include +#include #include #include diff --git a/tf2_ros/include/tf2_ros/async_buffer_interface.h b/tf2_ros/include/tf2_ros/async_buffer_interface.h index 88950f050..4f79bd84a 100644 --- a/tf2_ros/include/tf2_ros/async_buffer_interface.h +++ b/tf2_ros/include/tf2_ros/async_buffer_interface.h @@ -36,9 +36,9 @@ #include #include "tf2_ros/visibility_control.h" -#include "tf2/buffer_core.h" -#include "tf2/time.h" -#include "tf2/transform_datatypes.h" +#include "tf2/buffer_core.hpp" +#include "tf2/time.hpp" +#include "tf2/transform_datatypes.hpp" #include "geometry_msgs/msg/transform_stamped.hpp" diff --git a/tf2_ros/include/tf2_ros/buffer.h b/tf2_ros/include/tf2_ros/buffer.h index 13571fa9b..45a0cb3b9 100644 --- a/tf2_ros/include/tf2_ros/buffer.h +++ b/tf2_ros/include/tf2_ros/buffer.h @@ -43,8 +43,8 @@ #include "tf2_ros/buffer_interface.h" #include "tf2_ros/create_timer_interface.h" #include "tf2_ros/visibility_control.h" -#include "tf2/buffer_core.h" -#include "tf2/time.h" +#include "tf2/buffer_core.hpp" +#include "tf2/time.hpp" #include "geometry_msgs/msg/transform_stamped.hpp" #include "tf2_msgs/srv/frame_graph.hpp" diff --git a/tf2_ros/include/tf2_ros/buffer_client.h b/tf2_ros/include/tf2_ros/buffer_client.h index ba2818115..5265d4bc2 100644 --- a/tf2_ros/include/tf2_ros/buffer_client.h +++ b/tf2_ros/include/tf2_ros/buffer_client.h @@ -43,7 +43,7 @@ #include "tf2_ros/buffer_interface.h" #include "tf2_ros/visibility_control.h" -#include "tf2/time.h" +#include "tf2/time.hpp" #include "geometry_msgs/msg/transform_stamped.hpp" #include "rclcpp_action/rclcpp_action.hpp" diff --git a/tf2_ros/include/tf2_ros/buffer_interface.h b/tf2_ros/include/tf2_ros/buffer_interface.h index 4f5e4c31f..34d2b5b0c 100644 --- a/tf2_ros/include/tf2_ros/buffer_interface.h +++ b/tf2_ros/include/tf2_ros/buffer_interface.h @@ -38,9 +38,9 @@ #include #include "tf2_ros/visibility_control.h" -#include "tf2/transform_datatypes.h" -#include "tf2/exceptions.h" -#include "tf2/convert.h" +#include "tf2/transform_datatypes.hpp" +#include "tf2/exceptions.hpp" +#include "tf2/convert.hpp" #include "builtin_interfaces/msg/duration.hpp" #include "builtin_interfaces/msg/time.hpp" diff --git a/tf2_ros/include/tf2_ros/buffer_server.h b/tf2_ros/include/tf2_ros/buffer_server.h index 14466691f..401f84300 100644 --- a/tf2_ros/include/tf2_ros/buffer_server.h +++ b/tf2_ros/include/tf2_ros/buffer_server.h @@ -44,8 +44,8 @@ #include #include -#include "tf2/time.h" -#include "tf2/buffer_core_interface.h" +#include "tf2/time.hpp" +#include "tf2/buffer_core_interface.hpp" #include "tf2_ros/visibility_control.h" #include "geometry_msgs/msg/transform_stamped.hpp" diff --git a/tf2_ros/include/tf2_ros/create_timer_interface.h b/tf2_ros/include/tf2_ros/create_timer_interface.h index 0d8d6fef5..6588e0986 100644 --- a/tf2_ros/include/tf2_ros/create_timer_interface.h +++ b/tf2_ros/include/tf2_ros/create_timer_interface.h @@ -35,7 +35,7 @@ #include #include -#include "tf2/time.h" +#include "tf2/time.hpp" #include "tf2_ros/visibility_control.h" diff --git a/tf2_ros/include/tf2_ros/create_timer_ros.h b/tf2_ros/include/tf2_ros/create_timer_ros.h index 549d98484..a44803e43 100644 --- a/tf2_ros/include/tf2_ros/create_timer_ros.h +++ b/tf2_ros/include/tf2_ros/create_timer_ros.h @@ -35,7 +35,7 @@ #include "tf2_ros/create_timer_interface.h" #include "tf2_ros/visibility_control.h" -#include "tf2/time.h" +#include "tf2/time.hpp" #include "rclcpp/rclcpp.hpp" diff --git a/tf2_ros/include/tf2_ros/message_filter.h b/tf2_ros/include/tf2_ros/message_filter.h index 06668185d..e7709a892 100644 --- a/tf2_ros/include/tf2_ros/message_filter.h +++ b/tf2_ros/include/tf2_ros/message_filter.h @@ -50,8 +50,8 @@ #include "message_filters/connection.h" #include "message_filters/message_traits.h" #include "message_filters/simple_filter.h" -#include "tf2/buffer_core_interface.h" -#include "tf2/time.h" +#include "tf2/buffer_core_interface.hpp" +#include "tf2/time.hpp" #include "tf2_ros/async_buffer_interface.h" #include "tf2_ros/buffer.h" diff --git a/tf2_ros/include/tf2_ros/transform_listener.h b/tf2_ros/include/tf2_ros/transform_listener.h index 59eb841e0..f7120ec55 100644 --- a/tf2_ros/include/tf2_ros/transform_listener.h +++ b/tf2_ros/include/tf2_ros/transform_listener.h @@ -37,8 +37,8 @@ #include #include -#include "tf2/buffer_core.h" -#include "tf2/time.h" +#include "tf2/buffer_core.hpp" +#include "tf2/time.hpp" #include "tf2_ros/visibility_control.h" #include "tf2_msgs/msg/tf_message.hpp" diff --git a/tf2_ros/src/buffer_server.cpp b/tf2_ros/src/buffer_server.cpp index 37f8a9be9..aa2b6da1c 100644 --- a/tf2_ros/src/buffer_server.cpp +++ b/tf2_ros/src/buffer_server.cpp @@ -35,8 +35,6 @@ * Author: Eitan Marder-Eppstein *********************************************************************/ -#include - #include // Only needed for toMsg() and fromMsg() #include @@ -45,6 +43,8 @@ #include #include +#include + namespace tf2_ros { void BufferServer::checkTransforms() diff --git a/tf2_ros/src/create_timer_ros.cpp b/tf2_ros/src/create_timer_ros.cpp index d90ce2053..2bc1e1dee 100644 --- a/tf2_ros/src/create_timer_ros.cpp +++ b/tf2_ros/src/create_timer_ros.cpp @@ -33,7 +33,7 @@ #include #include -#include "tf2/time.h" +#include "tf2/time.hpp" #include "rclcpp/create_timer.hpp" #include "rclcpp/rclcpp.hpp" diff --git a/tf2_ros/src/static_transform_broadcaster_program.cpp b/tf2_ros/src/static_transform_broadcaster_program.cpp index 3d0dcddf5..aaa6f8c12 100644 --- a/tf2_ros/src/static_transform_broadcaster_program.cpp +++ b/tf2_ros/src/static_transform_broadcaster_program.cpp @@ -37,8 +37,8 @@ #include "tf2_ros/static_transform_broadcaster_node.hpp" -#include "tf2/LinearMath/Quaternion.h" -#include "tf2/LinearMath/Vector3.h" +#include "tf2/LinearMath/Quaternion.hpp" +#include "tf2/LinearMath/Vector3.hpp" #include "rclcpp/rclcpp.hpp" diff --git a/tf2_sensor_msgs/include/tf2_sensor_msgs/tf2_sensor_msgs.hpp b/tf2_sensor_msgs/include/tf2_sensor_msgs/tf2_sensor_msgs.hpp index 68e10fa69..93fe5997f 100644 --- a/tf2_sensor_msgs/include/tf2_sensor_msgs/tf2_sensor_msgs.hpp +++ b/tf2_sensor_msgs/include/tf2_sensor_msgs/tf2_sensor_msgs.hpp @@ -49,8 +49,8 @@ #include "tf2_ros/buffer_interface.h" -#include "tf2/convert.h" -#include "tf2/time.h" +#include "tf2/convert.hpp" +#include "tf2/time.hpp" #include "sensor_msgs/msg/point_cloud2.hpp" #include "sensor_msgs/point_cloud2_iterator.hpp"