From 0bcf12e0ec9df4f41ab19f7bfceb47a9243f5524 Mon Sep 17 00:00:00 2001 From: CursedRock17 Date: Tue, 29 Oct 2024 22:16:35 -0400 Subject: [PATCH 1/3] Removing C Headers tf2 Jazzy Backport Signed-off-by: CursedRock17 --- test_tf2/test/buffer_core_test.cpp | 8 +- test_tf2/test/test_convert.cpp | 4 +- test_tf2/test/test_message_filter.cpp | 8 +- test_tf2/test/test_static_publisher.cpp | 6 +- test_tf2/test/test_tf2_bullet.cpp | 2 +- test_tf2/test/test_utils.cpp | 4 +- tf2/CMakeLists.txt | 7 + tf2/include/tf2/LinearMath/Matrix3x3.h | 694 +---------------- tf2/include/tf2/LinearMath/Matrix3x3.hpp | 703 +++++++++++++++++ tf2/include/tf2/LinearMath/MinMax.h | 62 +- tf2/include/tf2/LinearMath/MinMax.hpp | 71 ++ tf2/include/tf2/LinearMath/QuadWord.h | 176 +---- tf2/include/tf2/LinearMath/QuadWord.hpp | 185 +++++ tf2/include/tf2/LinearMath/Quaternion.h | 467 +---------- tf2/include/tf2/LinearMath/Quaternion.hpp | 476 ++++++++++++ tf2/include/tf2/LinearMath/Scalar.h | 408 +--------- tf2/include/tf2/LinearMath/Scalar.hpp | 417 ++++++++++ tf2/include/tf2/LinearMath/Transform.h | 311 +------- tf2/include/tf2/LinearMath/Transform.hpp | 315 ++++++++ tf2/include/tf2/LinearMath/Vector3.h | 725 +---------------- tf2/include/tf2/LinearMath/Vector3.hpp | 735 ++++++++++++++++++ tf2/include/tf2/buffer_core.h | 432 +--------- tf2/include/tf2/buffer_core.hpp | 467 +++++++++++ tf2/include/tf2/buffer_core_interface.h | 113 +-- tf2/include/tf2/buffer_core_interface.hpp | 144 ++++ tf2/include/tf2/convert.h | 167 +--- tf2/include/tf2/convert.hpp | 201 +++++ tf2/include/tf2/exceptions.h | 185 +---- tf2/include/tf2/exceptions.hpp | 219 ++++++ tf2/include/tf2/impl/convert.h | 48 +- tf2/include/tf2/impl/convert.hpp | 80 ++ tf2/include/tf2/impl/utils.h | 152 +--- tf2/include/tf2/impl/utils.hpp | 170 ++++ tf2/include/tf2/time.h | 35 +- tf2/include/tf2/time.hpp | 67 ++ tf2/include/tf2/time_cache.h | 165 +--- tf2/include/tf2/time_cache.hpp | 193 +++++ tf2/include/tf2/transform_datatypes.h | 121 +-- tf2/include/tf2/transform_datatypes.hpp | 155 ++++ tf2/include/tf2/transform_storage.h | 62 +- tf2/include/tf2/transform_storage.hpp | 93 +++ tf2/include/tf2/utils.h | 48 +- tf2/include/tf2/utils.hpp | 66 ++ tf2/include/tf2/visibility_control.h | 31 +- tf2/include/tf2/visibility_control.hpp | 63 ++ tf2/src/buffer_core.cpp | 12 +- tf2/src/cache.cpp | 10 +- tf2/src/static_cache.cpp | 6 +- tf2/src/time.cpp | 2 +- tf2/test/cache_benchmark.cpp | 2 +- tf2/test/cache_unittest.cpp | 4 +- tf2/test/simple_tf2_core.cpp | 10 +- tf2/test/static_cache_test.cpp | 2 +- tf2/test/test_storage.cpp | 8 +- tf2/test/test_time.cpp | 2 +- tf2_bullet/include/tf2_bullet/tf2_bullet.hpp | 2 +- tf2_bullet/test/test_tf2_bullet.cpp | 2 +- tf2_eigen/include/tf2_eigen/tf2_eigen.hpp | 2 +- tf2_eigen/test/tf2_eigen-test.cpp | 4 +- .../include/tf2_eigen_kdl/tf2_eigen_kdl.hpp | 2 +- tf2_eigen_kdl/test/tf2_eigen_kdl_test.cpp | 5 +- .../tf2_geometry_msgs/tf2_geometry_msgs.hpp | 8 +- tf2_kdl/include/tf2_kdl/tf2_kdl.hpp | 2 +- tf2_kdl/test/test_tf2_kdl.cpp | 2 +- tf2_py/src/tf2_py.cpp | 4 +- .../include/tf2_ros/async_buffer_interface.h | 6 +- tf2_ros/include/tf2_ros/buffer.h | 4 +- tf2_ros/include/tf2_ros/buffer_client.h | 2 +- tf2_ros/include/tf2_ros/buffer_interface.h | 6 +- tf2_ros/include/tf2_ros/buffer_server.h | 4 +- .../include/tf2_ros/create_timer_interface.h | 2 +- tf2_ros/include/tf2_ros/create_timer_ros.h | 2 +- tf2_ros/include/tf2_ros/message_filter.h | 4 +- tf2_ros/include/tf2_ros/transform_listener.h | 4 +- tf2_ros/src/buffer_server.cpp | 4 +- tf2_ros/src/create_timer_ros.cpp | 2 +- .../static_transform_broadcaster_program.cpp | 4 +- .../tf2_sensor_msgs/tf2_sensor_msgs.hpp | 4 +- 78 files changed, 4971 insertions(+), 4429 deletions(-) create mode 100644 tf2/include/tf2/LinearMath/Matrix3x3.hpp create mode 100644 tf2/include/tf2/LinearMath/MinMax.hpp create mode 100644 tf2/include/tf2/LinearMath/QuadWord.hpp create mode 100644 tf2/include/tf2/LinearMath/Quaternion.hpp create mode 100644 tf2/include/tf2/LinearMath/Scalar.hpp create mode 100644 tf2/include/tf2/LinearMath/Transform.hpp create mode 100644 tf2/include/tf2/LinearMath/Vector3.hpp create mode 100644 tf2/include/tf2/buffer_core.hpp create mode 100644 tf2/include/tf2/buffer_core_interface.hpp create mode 100644 tf2/include/tf2/convert.hpp create mode 100644 tf2/include/tf2/exceptions.hpp create mode 100644 tf2/include/tf2/impl/convert.hpp create mode 100644 tf2/include/tf2/impl/utils.hpp create mode 100644 tf2/include/tf2/time.hpp create mode 100644 tf2/include/tf2/time_cache.hpp create mode 100644 tf2/include/tf2/transform_datatypes.hpp create mode 100644 tf2/include/tf2/transform_storage.hpp create mode 100644 tf2/include/tf2/utils.hpp create mode 100644 tf2/include/tf2/visibility_control.hpp 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..6195b8b4c 100644 --- a/tf2/include/tf2/LinearMath/Matrix3x3.h +++ b/tf2/include/tf2/LinearMath/Matrix3x3.h @@ -3,8 +3,8 @@ Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousph 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, +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. @@ -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..16d6f435b --- /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.hpp" + +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..9f2da6e00 100644 --- a/tf2/include/tf2/LinearMath/MinMax.h +++ b/tf2/include/tf2/LinearMath/MinMax.h @@ -3,8 +3,8 @@ Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousph 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, +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. @@ -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..6a376382a --- /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..993fa114f 100644 --- a/tf2/include/tf2/LinearMath/QuadWord.h +++ b/tf2/include/tf2/LinearMath/QuadWord.h @@ -3,8 +3,8 @@ Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousph 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, +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. @@ -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..8e12e80b1 --- /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.hpp" + + +#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..9a2c2f955 100644 --- a/tf2/include/tf2/LinearMath/Quaternion.h +++ b/tf2/include/tf2/LinearMath/Quaternion.h @@ -3,8 +3,8 @@ Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousph 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, +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. @@ -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..6a688c7d9 --- /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.hpp" + +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..43cf12b99 100644 --- a/tf2/include/tf2/LinearMath/Scalar.h +++ b/tf2/include/tf2/LinearMath/Scalar.h @@ -3,8 +3,8 @@ 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, +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. @@ -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..a4f0a5be2 --- /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_HPPAS_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_HPPAVE_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_HPPALF_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_HPPANDLE(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_HPPAVE_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..5a7306398 100644 --- a/tf2/include/tf2/LinearMath/Transform.h +++ b/tf2/include/tf2/LinearMath/Transform.h @@ -3,8 +3,8 @@ Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousph 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, +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. @@ -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..7adf9450b --- /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.hpp" + + +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..9b8cafbcc 100644 --- a/tf2/include/tf2/LinearMath/Vector3.h +++ b/tf2/include/tf2/LinearMath/Vector3.h @@ -3,8 +3,8 @@ Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousph 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, +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. @@ -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..492a2540f --- /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.hpp" + +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..0664ba345 --- /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.hpp" + +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..73beb5b68 --- /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.hpp" + +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..89857a2de --- /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.hpp" + +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..11ebcd3d6 --- /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.hpp" + +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..5d5bfb1ca --- /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.hpp" + +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..dd17d0fc2 --- /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.hpp" +#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..44f8dc794 100644 --- a/tf2/include/tf2/transform_storage.h +++ b/tf2/include/tf2/transform_storage.h @@ -1,3 +1,4 @@ + // Copyright 2010, Willow Garage, Inc. All rights reserved. // // Redistribution and use in source and binary forms, with or without @@ -30,64 +31,7 @@ #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" - -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; - } +#include - 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..6e73f1d44 --- /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/LinearMath/Vector3.hpp" +#include "tf2/LinearMath/Quaternion.hpp" +#include "tf2/time.hpp" +#include "tf2/visibility_control.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..b4a1f7f46 --- /dev/null +++ b/tf2/include/tf2/utils.hpp @@ -0,0 +1,66 @@ +// 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/include/tf2/visibility_control.h b/tf2/include/tf2/visibility_control.h index cb4611891..9a5a1ec70 100644 --- a/tf2/include/tf2/visibility_control.h +++ b/tf2/include/tf2/visibility_control.h @@ -29,35 +29,6 @@ #ifndef TF2__VISIBILITY_CONTROL_H_ #define TF2__VISIBILITY_CONTROL_H_ -// This logic was borrowed (then namespaced) from the examples on the gcc wiki: -// https://gcc.gnu.org/wiki/Visibility - -#if defined _WIN32 || defined __CYGWIN__ - #ifdef __GNUC__ - #define TF2_EXPORT __attribute__ ((dllexport)) - #define TF2_IMPORT __attribute__ ((dllimport)) - #else - #define TF2_EXPORT __declspec(dllexport) - #define TF2_IMPORT __declspec(dllimport) - #endif - #ifdef TF2_BUILDING_DLL - #define TF2_PUBLIC TF2_EXPORT - #else - #define TF2_PUBLIC TF2_IMPORT - #endif - #define TF2_PUBLIC_TYPE TF2_PUBLIC - #define TF2_LOCAL -#else - #define TF2_EXPORT __attribute__ ((visibility("default"))) - #define TF2_IMPORT - #if __GNUC__ >= 4 - #define TF2_PUBLIC __attribute__ ((visibility("default"))) - #define TF2_LOCAL __attribute__ ((visibility("hidden"))) - #else - #define TF2_PUBLIC - #define TF2_LOCAL - #endif - #define TF2_PUBLIC_TYPE -#endif +#include #endif // TF2__VISIBILITY_CONTROL_H_ diff --git a/tf2/include/tf2/visibility_control.hpp b/tf2/include/tf2/visibility_control.hpp new file mode 100644 index 000000000..c011ec959 --- /dev/null +++ b/tf2/include/tf2/visibility_control.hpp @@ -0,0 +1,63 @@ +// Copyright 2017, 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__VISIBILITY_CONTROL_HPP_ +#define TF2__VISIBILITY_CONTROL_HPP_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ + #ifdef __GNUC__ + #define TF2_EXPORT __attribute__ ((dllexport)) + #define TF2_IMPORT __attribute__ ((dllimport)) + #else + #define TF2_EXPORT __declspec(dllexport) + #define TF2_IMPORT __declspec(dllimport) + #endif + #ifdef TF2_BUILDING_DLL + #define TF2_PUBLIC TF2_EXPORT + #else + #define TF2_PUBLIC TF2_IMPORT + #endif + #define TF2_PUBLIC_TYPE TF2_PUBLIC + #define TF2_LOCAL +#else + #define TF2_EXPORT __attribute__ ((visibility("default"))) + #define TF2_IMPORT + #if __GNUC__ >= 4 + #define TF2_PUBLIC __attribute__ ((visibility("default"))) + #define TF2_LOCAL __attribute__ ((visibility("hidden"))) + #else + #define TF2_PUBLIC + #define TF2_LOCAL + #endif + #define TF2_PUBLIC_TYPE +#endif + +#endif // TF2__VISIBILITY_CONTROL_HPP_ diff --git a/tf2/src/buffer_core.cpp b/tf2/src/buffer_core.cpp index 97fa96bca..89d7a861c 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..e03411a1a 100644 --- a/tf2/test/static_cache_test.cpp +++ b/tf2/test/static_cache_test.cpp @@ -26,7 +26,7 @@ // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE // POSSIBILITY OF SUCH DAMAGE. -#include +#include #include 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" From 24111a72e60edd656f2c50f0d41a1522867664f5 Mon Sep 17 00:00:00 2001 From: CursedRock17 Date: Wed, 30 Oct 2024 01:14:21 -0400 Subject: [PATCH 2/3] Fixing Header Order Signed-off-by: CursedRock17 Update Matrix3x3.h Signed-off-by: Lucas Wendland <82680922+CursedRock17@users.noreply.github.com> Update Matrix3x3.hpp Signed-off-by: Lucas Wendland <82680922+CursedRock17@users.noreply.github.com> Update MinMax.h Signed-off-by: Lucas Wendland <82680922+CursedRock17@users.noreply.github.com> Update MinMax.hpp Signed-off-by: Lucas Wendland <82680922+CursedRock17@users.noreply.github.com> Update QuadWord.h Signed-off-by: Lucas Wendland <82680922+CursedRock17@users.noreply.github.com> Update QuadWord.hpp Signed-off-by: Lucas Wendland <82680922+CursedRock17@users.noreply.github.com> Update Quaternion.h Signed-off-by: Lucas Wendland <82680922+CursedRock17@users.noreply.github.com> Update Quaternion.hpp Signed-off-by: Lucas Wendland <82680922+CursedRock17@users.noreply.github.com> Update Scalar.h Signed-off-by: Lucas Wendland <82680922+CursedRock17@users.noreply.github.com> Update Scalar.hpp Signed-off-by: Lucas Wendland <82680922+CursedRock17@users.noreply.github.com> Update Transform.h Signed-off-by: Lucas Wendland <82680922+CursedRock17@users.noreply.github.com> Update Transform.hpp Signed-off-by: Lucas Wendland <82680922+CursedRock17@users.noreply.github.com> Update Vector3.h Signed-off-by: Lucas Wendland <82680922+CursedRock17@users.noreply.github.com> Update Vector3.hpp Signed-off-by: Lucas Wendland <82680922+CursedRock17@users.noreply.github.com> --- tf2/include/tf2/LinearMath/Matrix3x3.h | 4 +- tf2/include/tf2/LinearMath/Matrix3x3.hpp | 198 +++++++++++----------- tf2/include/tf2/LinearMath/MinMax.h | 4 +- tf2/include/tf2/LinearMath/MinMax.hpp | 28 +-- tf2/include/tf2/LinearMath/QuadWord.h | 4 +- tf2/include/tf2/LinearMath/QuadWord.hpp | 28 +-- tf2/include/tf2/LinearMath/Quaternion.h | 4 +- tf2/include/tf2/LinearMath/Quaternion.hpp | 128 +++++++------- tf2/include/tf2/LinearMath/Scalar.h | 4 +- tf2/include/tf2/LinearMath/Scalar.hpp | 44 ++--- tf2/include/tf2/LinearMath/Transform.h | 4 +- tf2/include/tf2/LinearMath/Transform.hpp | 58 +++---- tf2/include/tf2/LinearMath/Vector3.h | 4 +- tf2/include/tf2/LinearMath/Vector3.hpp | 164 +++++++++--------- tf2/include/tf2/transform_storage.h | 2 - tf2/test/static_cache_test.cpp | 4 +- 16 files changed, 340 insertions(+), 342 deletions(-) diff --git a/tf2/include/tf2/LinearMath/Matrix3x3.h b/tf2/include/tf2/LinearMath/Matrix3x3.h index 6195b8b4c..ad85c5410 100644 --- a/tf2/include/tf2/LinearMath/Matrix3x3.h +++ b/tf2/include/tf2/LinearMath/Matrix3x3.h @@ -3,8 +3,8 @@ Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousph 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, +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. diff --git a/tf2/include/tf2/LinearMath/Matrix3x3.hpp b/tf2/include/tf2/LinearMath/Matrix3x3.hpp index 16d6f435b..ae2eb0a4a 100644 --- a/tf2/include/tf2/LinearMath/Matrix3x3.hpp +++ b/tf2/include/tf2/LinearMath/Matrix3x3.hpp @@ -3,8 +3,8 @@ Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousph 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, +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. @@ -13,19 +13,19 @@ subject to the following restrictions: */ -#ifndef TF2__LINEARMATH__MATRIX3X3_HPP_ +#ifndef TF2__LINEARMATH__MATRIX3X3_HPP_ #define TF2__LINEARMATH__MATRIX3X3_HPP_ #include "Vector3.hpp" #include "Quaternion.hpp" -#include "tf2/visibility_control.hpp" +#include "tf2/visibility_control.h" namespace tf2 { -#define Matrix3x3Data Matrix3x3DoubleData +#define Matrix3x3Data Matrix3x3DoubleData /**@brief The Matrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with Quaternion, Transform and Vector3. @@ -48,7 +48,7 @@ class Matrix3x3 { /* template Matrix3x3(const tf2Scalar& yaw, const tf2Scalar& pitch, const tf2Scalar& roll) - { + { setEulerYPR(yaw, pitch, roll); } */ @@ -57,9 +57,9 @@ class Matrix3x3 { 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, + { + setValue(xx, xy, xz, + yx, yy, yz, zx, zy, zz); } /** @brief Copy constructor */ @@ -81,7 +81,7 @@ class Matrix3x3 { } - /** @brief Get a column of the matrix as a vector + /** @brief Get a column of the matrix as a vector * @param i Column number 0 indexed */ TF2SIMD_FORCE_INLINE Vector3 getColumn(int i) const { @@ -89,7 +89,7 @@ class Matrix3x3 { } - /** @brief Get a row of the matrix as a vector + /** @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 { @@ -97,29 +97,29 @@ class Matrix3x3 { return m_el[i]; } - /** @brief Get a mutable reference to a row of the matrix as a vector + /** @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]; + return m_el[i]; } - /** @brief Get a const reference to a row of the matrix as a vector + /** @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]; + return m_el[i]; } /** @brief Multiply by the target matrix on the right - * @param m Rotation matrix to be applied + * @param m Rotation matrix to be applied * Equivilant to this = this * m */ TF2_PUBLIC - Matrix3x3& operator*=(const Matrix3x3& m); + Matrix3x3& operator*=(const Matrix3x3& m); - /** @brief Set from a carray of tf2Scalars + /** @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) @@ -140,8 +140,8 @@ class Matrix3x3 { * @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, + 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); @@ -150,9 +150,9 @@ class Matrix3x3 { } /** @brief Set the matrix from a quaternion - * @param q The Quaternion to match */ + * @param q The Quaternion to match */ TF2_PUBLIC - void setRotation(const Quaternion& q) + void setRotation(const Quaternion& q) { tf2Scalar d = q.length2(); tf2FullAssert(d != tf2Scalar(0.0)); @@ -170,26 +170,26 @@ class Matrix3x3 { * @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 + * 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; + 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, + cj * sh, sj * ss + cc, sj * cs - sc, -sj, cj * si, cj * ci); } @@ -197,51 +197,51 @@ class Matrix3x3 { * @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) { + 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)); + { + 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), + 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 + /**@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 + void getOpenGLSubMatrix(tf2Scalar *m) const { - m[0] = tf2Scalar(m_el[0].x()); + 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[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[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); + m[11] = tf2Scalar(0.0); } - /**@brief Get the matrix represented as a quaternion + /**@brief Get the matrix represented as a quaternion * @param q The quaternion which will be set */ TF2_PUBLIC void getRotation(Quaternion& q) const @@ -249,7 +249,7 @@ class Matrix3x3 { tf2Scalar trace = m_el[0].x() + m_el[1].y() + m_el[2].z(); tf2Scalar temp[4]; - if (trace > tf2Scalar(0.0)) + if (trace > tf2Scalar(0.0)) { tf2Scalar s = tf2Sqrt(trace + tf2Scalar(1.0)); temp[3]=(s * tf2Scalar(0.5)); @@ -258,13 +258,13 @@ class Matrix3x3 { 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 + } + else { - int i = m_el[0].x() < m_el[1].y() ? + 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; + (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)); @@ -281,7 +281,7 @@ class Matrix3x3 { /**@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 */ + * @param roll around X axis */ TF2_PUBLIC void getEulerYPR(tf2Scalar& yaw, tf2Scalar& pitch, tf2Scalar& roll, unsigned int solution_number = 1) const { @@ -302,7 +302,7 @@ class Matrix3x3 { { 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 @@ -325,43 +325,43 @@ class Matrix3x3 { 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), + 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), + 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), + 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), + 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; + { + yaw = euler_out.yaw; pitch = euler_out.pitch; roll = euler_out.roll; } else - { - yaw = euler_out2.yaw; + { + 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 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*/ + * @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 + /**@brief Create a scaled copy of the matrix * @param s Scaling vector The elements of the vector will scale each column */ TF2_PUBLIC @@ -386,22 +386,22 @@ class Matrix3x3 { Matrix3x3 transpose() const; /**@brief Return the inverse of the matrix */ TF2_PUBLIC - Matrix3x3 inverse() const; + 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 + 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 + 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 + 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(); } @@ -409,12 +409,12 @@ class Matrix3x3 { /**@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. + * 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. + * @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) @@ -453,7 +453,7 @@ class Matrix3x3 { step = 1; } - // compute Jacobi rotation J which leads to a zero for element [p][q] + // 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; @@ -498,7 +498,7 @@ class Matrix3x3 { - /**@brief Calculate the matrix cofactor + /**@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 @@ -506,7 +506,7 @@ class Matrix3x3 { * 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 + 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]; } @@ -529,7 +529,7 @@ class Matrix3x3 { }; -TF2SIMD_FORCE_INLINE Matrix3x3& +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]), @@ -538,14 +538,14 @@ Matrix3x3::operator*=(const Matrix3x3& m) return *this; } -TF2SIMD_FORCE_INLINE tf2Scalar +TF2SIMD_FORCE_INLINE tf2Scalar Matrix3x3::determinant() const -{ +{ return tf2Triple((*this)[0], (*this)[1], (*this)[2]); } -TF2SIMD_FORCE_INLINE Matrix3x3 +TF2SIMD_FORCE_INLINE Matrix3x3 Matrix3x3::absolute() const { return Matrix3x3( @@ -554,23 +554,23 @@ Matrix3x3::absolute() const tf2Fabs(m_el[2].x()), tf2Fabs(m_el[2].y()), tf2Fabs(m_el[2].z())); } -TF2SIMD_FORCE_INLINE Matrix3x3 -Matrix3x3::transpose() const +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 +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 +TF2SIMD_FORCE_INLINE Matrix3x3 Matrix3x3::inverse() const { Vector3 co(cofac(1, 1, 2, 2), cofac(1, 2, 2, 0), cofac(1, 0, 2, 1)); @@ -582,7 +582,7 @@ Matrix3x3::inverse() const co.z() * s, cofac(0, 1, 2, 0) * s, cofac(0, 0, 1, 1) * s); } -TF2SIMD_FORCE_INLINE Matrix3x3 +TF2SIMD_FORCE_INLINE Matrix3x3 Matrix3x3::transposeTimes(const Matrix3x3& m) const { return Matrix3x3( @@ -597,7 +597,7 @@ Matrix3x3::transposeTimes(const Matrix3x3& m) const 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 +TF2SIMD_FORCE_INLINE Matrix3x3 Matrix3x3::timesTranspose(const Matrix3x3& m) const { return Matrix3x3( @@ -607,8 +607,8 @@ Matrix3x3::timesTranspose(const Matrix3x3& m) const } -TF2SIMD_FORCE_INLINE Vector3 -operator*(const Matrix3x3& m, const Vector3& v) +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)); } @@ -620,7 +620,7 @@ operator*(const Vector3& v, const Matrix3x3& m) return Vector3(m.tdotx(v), m.tdoty(v), m.tdotz(v)); } -TF2SIMD_FORCE_INLINE Matrix3x3 +TF2SIMD_FORCE_INLINE Matrix3x3 operator*(const Matrix3x3& m1, const Matrix3x3& m2) { return Matrix3x3( @@ -666,7 +666,7 @@ struct Matrix3x3DoubleData }; - + TF2SIMD_FORCE_INLINE void Matrix3x3::serialize(struct Matrix3x3Data& dataOut) const { @@ -700,4 +700,4 @@ TF2SIMD_FORCE_INLINE void Matrix3x3::deSerializeDouble(const struct Matrix3x3Dou } } -#endif // TF2__LINEARMATH__MATRIX3X3_HPP_ +#endif // TF2__LINEARMATH__MATRIX3X3_HPP_ diff --git a/tf2/include/tf2/LinearMath/MinMax.h b/tf2/include/tf2/LinearMath/MinMax.h index 9f2da6e00..0f6df3ce8 100644 --- a/tf2/include/tf2/LinearMath/MinMax.h +++ b/tf2/include/tf2/LinearMath/MinMax.h @@ -3,8 +3,8 @@ Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousph 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, +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. diff --git a/tf2/include/tf2/LinearMath/MinMax.hpp b/tf2/include/tf2/LinearMath/MinMax.hpp index 6a376382a..6c3746a8a 100644 --- a/tf2/include/tf2/LinearMath/MinMax.hpp +++ b/tf2/include/tf2/LinearMath/MinMax.hpp @@ -3,8 +3,8 @@ Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousph 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, +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. @@ -20,49 +20,49 @@ subject to the following restrictions: #include "Scalar.hpp" template -TF2SIMD_FORCE_INLINE const T& tf2Min(const T& a, const T& b) +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) +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) +TF2SIMD_FORCE_INLINE const T& GEN_clamped(const T& a, const T& lb, const T& ub) { - return a < lb ? lb : (ub < a ? ub : a); + return a < lb ? lb : (ub < a ? ub : a); } template -TF2SIMD_FORCE_INLINE void tf2SetMin(T& a, const T& b) +TF2SIMD_FORCE_INLINE void tf2SetMin(T& a, const T& b) { - if (b < a) + if (b < a) { a = b; } } template -TF2SIMD_FORCE_INLINE void tf2SetMax(T& a, const T& b) +TF2SIMD_FORCE_INLINE void tf2SetMax(T& a, const T& b) { - if (a < b) + if (a < b) { a = b; } } template -TF2SIMD_FORCE_INLINE void GEN_clamp(T& a, const T& lb, const T& ub) +TF2SIMD_FORCE_INLINE void GEN_clamp(T& a, const T& lb, const T& ub) { - if (a < lb) + if (a < lb) { - a = lb; + a = lb; } - else if (ub < a) + else if (ub < a) { a = ub; } diff --git a/tf2/include/tf2/LinearMath/QuadWord.h b/tf2/include/tf2/LinearMath/QuadWord.h index 993fa114f..9327b575b 100644 --- a/tf2/include/tf2/LinearMath/QuadWord.h +++ b/tf2/include/tf2/LinearMath/QuadWord.h @@ -3,8 +3,8 @@ Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousph 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, +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. diff --git a/tf2/include/tf2/LinearMath/QuadWord.hpp b/tf2/include/tf2/LinearMath/QuadWord.hpp index 8e12e80b1..6806a3607 100644 --- a/tf2/include/tf2/LinearMath/QuadWord.hpp +++ b/tf2/include/tf2/LinearMath/QuadWord.hpp @@ -3,8 +3,8 @@ Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousph 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, +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. @@ -18,7 +18,7 @@ subject to the following restrictions: #include "Scalar.hpp" #include "MinMax.hpp" -#include "tf2/visibility_control.hpp" +#include "tf2/visibility_control.h" #if defined (__CELLOS_LV2) && defined (__SPU__) @@ -27,7 +27,7 @@ subject to the following restrictions: namespace tf2 { -/**@brief The QuadWord class is base class for Vector3 and Quaternion. +/**@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 @@ -55,7 +55,7 @@ class QuadWord #endif //__CELLOS_LV2__ __SPU__ public: - + /**@brief Return the x value */ TF2SIMD_FORCE_INLINE const tf2Scalar& getX() const { return m_floats[0]; } @@ -80,7 +80,7 @@ class QuadWord /**@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 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]; } @@ -96,7 +96,7 @@ class QuadWord return !(*this == other); } - /**@brief Set x,y,z and zero w + /**@brief Set x,y,z and zero w * @param x Value of x * @param y Value of y * @param z Value of z @@ -109,14 +109,14 @@ class QuadWord m_floats[3] = 0.f; } -/* void getValue(tf2Scalar *m) const +/* void getValue(tf2Scalar *m) const { m[0] = m_floats[0]; m[1] = m_floats[1]; m[2] = m_floats[2]; } */ -/**@brief Set the values +/**@brief Set the values * @param x Value of x * @param y Value of y * @param z Value of z @@ -134,13 +134,13 @@ class 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) + 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; } @@ -151,13 +151,13 @@ class QuadWord * @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) + 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 + * @param other The other QuadWord to compare with */ TF2SIMD_FORCE_INLINE void setMax(const QuadWord& other) { @@ -167,7 +167,7 @@ class QuadWord 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 + * @param other The other QuadWord to compare with */ TF2SIMD_FORCE_INLINE void setMin(const QuadWord& other) { diff --git a/tf2/include/tf2/LinearMath/Quaternion.h b/tf2/include/tf2/LinearMath/Quaternion.h index 9a2c2f955..a58413f64 100644 --- a/tf2/include/tf2/LinearMath/Quaternion.h +++ b/tf2/include/tf2/LinearMath/Quaternion.h @@ -3,8 +3,8 @@ Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousph 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, +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. diff --git a/tf2/include/tf2/LinearMath/Quaternion.hpp b/tf2/include/tf2/LinearMath/Quaternion.hpp index 6a688c7d9..c4c505b6e 100644 --- a/tf2/include/tf2/LinearMath/Quaternion.hpp +++ b/tf2/include/tf2/LinearMath/Quaternion.hpp @@ -3,8 +3,8 @@ Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousph 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, +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. @@ -20,7 +20,7 @@ subject to the following restrictions: #include "Vector3.hpp" #include "QuadWord.hpp" -#include "tf2/visibility_control.hpp" +#include "tf2/visibility_control.h" namespace tf2 { @@ -36,18 +36,18 @@ class Quaternion : public QuadWord { // 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) + 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); + Quaternion(const Vector3& axis, const tf2Scalar& angle) + { + setRotation(axis, angle); } - /**@brief Set the rotation using axis angle notation + /**@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 @@ -56,7 +56,7 @@ class Quaternion : public QuadWord { 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, + setValue(axis.x() * s, axis.y() * s, axis.z() * s, tf2Cos(angle * tf2Scalar(0.5))); } /**@brief Set the quaternion using Euler angles @@ -66,9 +66,9 @@ class Quaternion : public QuadWord { 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 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); @@ -81,15 +81,15 @@ class Quaternion : public QuadWord { cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw); } /**@brief Set the quaternion using fixed axis RPY - * @param roll Angle around X + * @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 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); @@ -112,7 +112,7 @@ class Quaternion : public QuadWord { /**@brief Sutf2ract out a quaternion * @param q The quaternion to sutf2ract from this one */ TF2_PUBLIC - Quaternion& operator-=(const Quaternion& q) + 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; @@ -128,7 +128,7 @@ class Quaternion : public QuadWord { } /**@brief Multiply this quaternion by q on the right - * @param q The other quaternion + * @param q The other quaternion * Equivilant to this = this * q */ TF2_PUBLIC Quaternion& operator*=(const Quaternion& q) @@ -161,10 +161,10 @@ class Quaternion : public QuadWord { return tf2Sqrt(length2()); } - /**@brief Normalize the quaternion + /**@brief Normalize the quaternion * Such that x^2 + y^2 + z^2 +w^2 = 1 */ TF2_PUBLIC - Quaternion& normalize() + Quaternion& normalize() { return *this /= length(); } @@ -190,7 +190,7 @@ class Quaternion : public QuadWord { /**@brief Inversely scale this quaternion * @param s The scale factor */ TF2_PUBLIC - Quaternion& operator/=(const tf2Scalar& s) + Quaternion& operator/=(const tf2Scalar& s) { tf2Assert(s != tf2Scalar(0.0)); return *this *= tf2Scalar(1.0) / s; @@ -198,14 +198,14 @@ class Quaternion : public QuadWord { /**@brief Return a normalized version of this quaternion */ TF2_PUBLIC - Quaternion normalized() const + Quaternion normalized() const { return *this / length(); - } - /**@brief Return the ***half*** angle between this quaternion and the other + } + /**@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 angle(const Quaternion& q) const { tf2Scalar s = tf2Sqrt(length2() * q.length2()); tf2Assert(s != tf2Scalar(0.0)); @@ -214,18 +214,18 @@ class Quaternion : public QuadWord { /**@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 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 + 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 getAngle() const { tf2Scalar s = tf2Scalar(2.) * tf2Acos(m_floats[3]); return s; @@ -233,7 +233,7 @@ class Quaternion : public QuadWord { /**@brief Return the angle [0, Pi] of rotation represented by this quaternion along the shortest path */ TF2_PUBLIC - tf2Scalar getAngleShortestPath() const + tf2Scalar getAngleShortestPath() const { tf2Scalar s; if (m_floats[3] >= 0) @@ -262,7 +262,7 @@ class Quaternion : public QuadWord { return Quaternion(-m_floats[0], -m_floats[1], -m_floats[2], m_floats[3]); } - /**@brief Return the sum of this quaternion and the other + /**@brief Return the sum of this quaternion and the other * @param q2 The other quaternion */ TF2SIMD_FORCE_INLINE Quaternion operator+(const Quaternion& q2) const @@ -271,7 +271,7 @@ class Quaternion : public QuadWord { 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 + /**@brief Return the difference between this quaternion and the other * @param q2 The other quaternion */ TF2SIMD_FORCE_INLINE Quaternion operator-(const Quaternion& q2) const @@ -280,7 +280,7 @@ class Quaternion : public QuadWord { 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 + /**@brief Return the negative of this quaternion * This simply negates each element */ TF2SIMD_FORCE_INLINE Quaternion operator-() const { @@ -288,7 +288,7 @@ class Quaternion : public QuadWord { 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 + TF2SIMD_FORCE_INLINE Quaternion farthest( const Quaternion& qd) const { Quaternion diff,sum; diff = *this - qd; @@ -299,7 +299,7 @@ class Quaternion : public QuadWord { } /**@todo document this and it's use */ - TF2SIMD_FORCE_INLINE Quaternion nearest( const Quaternion& qd) const + TF2SIMD_FORCE_INLINE Quaternion nearest( const Quaternion& qd) const { Quaternion diff,sum; diff = *this - qd; @@ -311,7 +311,7 @@ class Quaternion : public QuadWord { /**@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 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 @@ -322,7 +322,7 @@ class Quaternion : public QuadWord { { tf2Scalar d = tf2Scalar(1.0) / tf2Sin(theta); tf2Scalar s0 = tf2Sin((tf2Scalar(1.0) - t) * theta); - tf2Scalar s1 = tf2Sin(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, @@ -333,7 +333,7 @@ class Quaternion : public QuadWord { (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 { @@ -350,7 +350,7 @@ class Quaternion : public QuadWord { TF2SIMD_FORCE_INLINE const tf2Scalar& getW() const { return m_floats[3]; } - + }; @@ -369,7 +369,7 @@ 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()); + q1.w() * q2.w() - q1.x() * q2.x() - q1.y() * q2.y() - q1.z() * q2.z()); } TF2SIMD_FORCE_INLINE Quaternion @@ -378,7 +378,7 @@ 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()); + -q.x() * w.x() - q.y() * w.y() - q.z() * w.z()); } TF2SIMD_FORCE_INLINE Quaternion @@ -387,65 +387,65 @@ 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()); + -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); +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(); +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); +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); +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) +inverse(const Quaternion& q) { return q.inverse(); } -/**@brief Return the result of spherical linear interpolation betwen two quaternions +/**@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 + * @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) +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) +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 +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); @@ -464,7 +464,7 @@ shortestArcQuat(const Vector3& v0, const Vector3& v1) // Game Programming Gems 2 return Quaternion(c.getX()*rs,c.getY()*rs,c.getZ()*rs,s * 0.5f); } -TF2SIMD_FORCE_INLINE Quaternion +TF2SIMD_FORCE_INLINE Quaternion shortestArcQuatNormalize2(Vector3& v0,Vector3& v1) { v0.normalize(); diff --git a/tf2/include/tf2/LinearMath/Scalar.h b/tf2/include/tf2/LinearMath/Scalar.h index 43cf12b99..ec3d55ad6 100644 --- a/tf2/include/tf2/LinearMath/Scalar.h +++ b/tf2/include/tf2/LinearMath/Scalar.h @@ -3,8 +3,8 @@ 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, +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. diff --git a/tf2/include/tf2/LinearMath/Scalar.hpp b/tf2/include/tf2/LinearMath/Scalar.hpp index a4f0a5be2..fdc2f9c44 100644 --- a/tf2/include/tf2/LinearMath/Scalar.hpp +++ b/tf2/include/tf2/LinearMath/Scalar.hpp @@ -3,8 +3,8 @@ 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, +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. @@ -43,7 +43,7 @@ subject to the following restrictions: #define ATTRIBUTE_ALIGNED64(a) a #define ATTRIBUTE_ALIGNED128(a) a #else - //#define TF2_HPPAS_ALIGNED_ALLOCATOR + //#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 @@ -57,7 +57,7 @@ subject to the following restrictions: #define TF2_USE_VMX128 #include - #define TF2_HPPAVE_NATIVE_FSEL + #define TF2_HAVE_NATIVE_FSEL #define tf2Fsel(a,b,c) __fsel((a),(b),(c)) #else @@ -79,7 +79,7 @@ subject to the following restrictions: #define tf2Unlikely(_c) _c #else - + #if defined (__CELLOS_LV2__) #define TF2SIMD_FORCE_INLINE inline #define ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16))) @@ -121,7 +121,7 @@ subject to the following restrictions: #define tf2Likely(_c) __builtin_expect((_c), 1) #define tf2Unlikely(_c) __builtin_expect((_c), 0) - + #else //non-windows systems @@ -173,7 +173,7 @@ typedef double tf2Scalar; - + 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); } @@ -191,7 +191,7 @@ TF2SIMD_FORCE_INLINE tf2Scalar tf2Fmod(tf2Scalar x,tf2Scalar y) { return fmod(x, #define TF2SIMD_2_PI tf2Scalar(6.283185307179586232) #define TF2SIMD_PI (TF2SIMD_2_PI * tf2Scalar(0.5)) -#define TF2SIMD_HPPALF_PI (TF2SIMD_2_PI * tf2Scalar(0.25)) +#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) @@ -202,7 +202,7 @@ TF2SIMD_FORCE_INLINE tf2Scalar tf2Fmod(tf2Scalar x,tf2Scalar y) { return fmod(x, #define TF2SIMD_EPSILON DBL_EPSILON #define TF2SIMD_INFINITY DBL_MAX -TF2SIMD_FORCE_INLINE tf2Scalar tf2Atan2Fast(tf2Scalar y, tf2Scalar x) +TF2SIMD_FORCE_INLINE tf2Scalar tf2Atan2Fast(tf2Scalar y, tf2Scalar x) { tf2Scalar coeff_1 = TF2SIMD_PI / 4.0f; tf2Scalar coeff_2 = 3.0f * coeff_1; @@ -235,7 +235,7 @@ TF2SIMD_FORCE_INLINE int tf2IsNegative(tf2Scalar x) { 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_HPPANDLE(name) typedef struct name##__ { int unused; } *name +#define TF2_DECLARE_HANDLE(name) typedef struct name##__ { int unused; } *name #ifndef tf2Fsel TF2SIMD_FORCE_INLINE tf2Scalar tf2Fsel(tf2Scalar a, tf2Scalar b, tf2Scalar c) @@ -260,28 +260,28 @@ TF2SIMD_FORCE_INLINE bool tf2MachineIsLittleEndian() ///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) +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 + // 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)); + 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; + unsigned testEqz = ~testNz; return static_cast((valueIfConditionNonZero & testNz) | (valueIfConditionZero & testEqz)); } TF2SIMD_FORCE_INLINE float tf2Select(unsigned condition, float valueIfConditionNonZero, float valueIfConditionZero) { -#ifdef TF2_HPPAVE_NATIVE_FSEL +#ifdef TF2_HAVE_NATIVE_FSEL return (float)tf2Fsel((tf2Scalar)condition - tf2Scalar(1.0f), valueIfConditionNonZero, valueIfConditionZero); #else - return (condition != 0) ? valueIfConditionNonZero : valueIfConditionZero; + return (condition != 0) ? valueIfConditionNonZero : valueIfConditionZero; #endif } @@ -316,9 +316,9 @@ TF2SIMD_FORCE_INLINE unsigned short tf2SwapEndian(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. +///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) { @@ -334,7 +334,7 @@ TF2SIMD_FORCE_INLINE unsigned int tf2SwapEndianFloat(float d) } // unswap using char pointers -TF2SIMD_FORCE_INLINE float tf2UnswapEndianFloat(unsigned int a) +TF2SIMD_FORCE_INLINE float tf2UnswapEndianFloat(unsigned int a) { float d = 0.0f; unsigned char *src = (unsigned char *)&a; @@ -366,7 +366,7 @@ TF2SIMD_FORCE_INLINE void tf2SwapEndianDouble(double d, unsigned char* dst) } // unswap using char pointers -TF2SIMD_FORCE_INLINE double tf2UnswapEndianDouble(const unsigned char *src) +TF2SIMD_FORCE_INLINE double tf2UnswapEndianDouble(const unsigned char *src) { double d = 0.0; unsigned char *dst = (unsigned char *)&d; @@ -384,7 +384,7 @@ TF2SIMD_FORCE_INLINE double tf2UnswapEndianDouble(const unsigned char *src) } // returns normalized value in range [-TF2SIMD_PI, TF2SIMD_PI] -TF2SIMD_FORCE_INLINE tf2Scalar tf2NormalizeAngle(tf2Scalar angleInRadians) +TF2SIMD_FORCE_INLINE tf2Scalar tf2NormalizeAngle(tf2Scalar angleInRadians) { angleInRadians = tf2Fmod(angleInRadians, TF2SIMD_2_PI); if(angleInRadians < -TF2SIMD_PI) diff --git a/tf2/include/tf2/LinearMath/Transform.h b/tf2/include/tf2/LinearMath/Transform.h index 5a7306398..9e5d22c81 100644 --- a/tf2/include/tf2/LinearMath/Transform.h +++ b/tf2/include/tf2/LinearMath/Transform.h @@ -3,8 +3,8 @@ Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousph 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, +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. diff --git a/tf2/include/tf2/LinearMath/Transform.hpp b/tf2/include/tf2/LinearMath/Transform.hpp index 7adf9450b..0424eb961 100644 --- a/tf2/include/tf2/LinearMath/Transform.hpp +++ b/tf2/include/tf2/LinearMath/Transform.hpp @@ -3,8 +3,8 @@ Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousph 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, +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. @@ -19,7 +19,7 @@ subject to the following restrictions: #include "Matrix3x3.hpp" -#include "tf2/visibility_control.hpp" +#include "tf2/visibility_control.h" namespace tf2 @@ -31,30 +31,30 @@ namespace tf2 /**@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 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))) + 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 b Rotation from Matrix * @param c Translation from Vector default (0,0,0)*/ - explicit TF2SIMD_FORCE_INLINE Transform(const Matrix3x3& b, + explicit TF2SIMD_FORCE_INLINE Transform(const Matrix3x3& b, const Vector3& c = Vector3(tf2Scalar(0), tf2Scalar(0), tf2Scalar(0))) : m_basis(b), m_origin(c) @@ -92,8 +92,8 @@ class Transform { /**@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(), + 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()); } @@ -121,14 +121,14 @@ class Transform { /**@brief Return a quaternion representing the rotation */ TF2_PUBLIC - Quaternion getRotation() const { + Quaternion getRotation() const { Quaternion q; m_basis.getRotation(q); return q; } - - - /**@brief Set from an array + + + /**@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) @@ -140,7 +140,7 @@ class Transform { /**@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 + void getOpenGLMatrix(tf2Scalar *m) const { m_basis.getOpenGLSubMatrix(m); m[12] = m_origin.x(); @@ -151,8 +151,8 @@ class Transform { /**@brief Set the translational element * @param origin The vector to set the translation to */ - TF2SIMD_FORCE_INLINE void setOrigin(const Vector3& origin) - { + TF2SIMD_FORCE_INLINE void setOrigin(const Vector3& origin) + { m_origin = origin; } @@ -161,7 +161,7 @@ class Transform { /**@brief Set the rotational element by Matrix3x3 */ TF2SIMD_FORCE_INLINE void setBasis(const Matrix3x3& basis) - { + { m_basis = basis; } @@ -180,10 +180,10 @@ class Transform { m_origin.setValue(tf2Scalar(0.0), tf2Scalar(0.0), tf2Scalar(0.0)); } - /**@brief Multiply this Transform by another(this = this * another) + /**@brief Multiply this Transform by another(this = this * another) * @param t The other transform */ TF2_PUBLIC - Transform& operator*=(const Transform& t) + Transform& operator*=(const Transform& t) { m_origin += m_basis * t.m_origin; m_basis *= t.m_basis; @@ -193,16 +193,16 @@ class Transform { /**@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 + * @param t The other transform * return this.inverse() * the other */ TF2_PUBLIC - Transform inverseTimes(const Transform& t) const; + Transform inverseTimes(const Transform& t) const; /**@brief Return the product of this transform and the other */ TF2_PUBLIC @@ -241,18 +241,18 @@ Transform::invXform(const Vector3& inVec) const return (m_basis.transpose() * v); } -TF2SIMD_FORCE_INLINE Transform -Transform::inverseTimes(const Transform& t) const +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 +TF2SIMD_FORCE_INLINE Transform Transform::operator*(const Transform& t) const { - return Transform(m_basis * t.m_basis, + return Transform(m_basis * t.m_basis, (*this)(t.m_origin)); } diff --git a/tf2/include/tf2/LinearMath/Vector3.h b/tf2/include/tf2/LinearMath/Vector3.h index 9b8cafbcc..46e09f40b 100644 --- a/tf2/include/tf2/LinearMath/Vector3.h +++ b/tf2/include/tf2/LinearMath/Vector3.h @@ -3,8 +3,8 @@ Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousph 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, +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. diff --git a/tf2/include/tf2/LinearMath/Vector3.hpp b/tf2/include/tf2/LinearMath/Vector3.hpp index 492a2540f..10590a276 100644 --- a/tf2/include/tf2/LinearMath/Vector3.hpp +++ b/tf2/include/tf2/LinearMath/Vector3.hpp @@ -3,8 +3,8 @@ Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousph 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, +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. @@ -20,7 +20,7 @@ subject to the following restrictions: #include "Scalar.hpp" #include "MinMax.hpp" -#include "tf2/visibility_control.hpp" +#include "tf2/visibility_control.h" namespace tf2 { @@ -71,12 +71,12 @@ ATTRIBUTE_ALIGNED16(class) Vector3 /**@brief No initialization constructor */ TF2SIMD_FORCE_INLINE Vector3() {} - - - /**@brief Constructor from scalars + + + /**@brief Constructor from scalars * @param x X value - * @param y Y value - * @param z Z value + * @param y Y value + * @param z Z value */ TF2SIMD_FORCE_INLINE Vector3(const tf2Scalar& x, const tf2Scalar& y, const tf2Scalar& z) { @@ -86,7 +86,7 @@ ATTRIBUTE_ALIGNED16(class) Vector3 m_floats[3] = tf2Scalar(0.); } -/**@brief Add a vector to this one +/**@brief Add a vector to this one * @param The vector to add to this one */ TF2SIMD_FORCE_INLINE Vector3& operator+=(const Vector3& v) { @@ -98,7 +98,7 @@ ATTRIBUTE_ALIGNED16(class) Vector3 /**@brief Sutf2ract a vector from this one * @param The vector to sutf2ract */ - TF2SIMD_FORCE_INLINE Vector3& operator-=(const Vector3& v) + 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; @@ -111,9 +111,9 @@ ATTRIBUTE_ALIGNED16(class) Vector3 return *this; } - /**@brief Inversely scale the vector + /**@brief Inversely scale the vector * @param s Scale factor to divide by */ - TF2SIMD_FORCE_INLINE Vector3& operator/=(const tf2Scalar& s) + TF2SIMD_FORCE_INLINE Vector3& operator/=(const tf2Scalar& s) { tf2FullAssert(s != tf2Scalar(0.0)); return *this *= tf2Scalar(1.0) / s; @@ -146,9 +146,9 @@ ATTRIBUTE_ALIGNED16(class) Vector3 * This is symantically treating the vector like a point */ TF2SIMD_FORCE_INLINE tf2Scalar distance(const Vector3& v) const; - /**@brief Normalize this vector + /**@brief Normalize this vector * x^2 + y^2 + z^2 = 1 */ - TF2SIMD_FORCE_INLINE Vector3& normalize() + TF2SIMD_FORCE_INLINE Vector3& normalize() { return *this /= length(); } @@ -156,28 +156,28 @@ ATTRIBUTE_ALIGNED16(class) Vector3 /**@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 + /**@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 + 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 + TF2SIMD_FORCE_INLINE Vector3 absolute() const { return Vector3( - tf2Fabs(m_floats[0]), - tf2Fabs(m_floats[1]), + tf2Fabs(m_floats[0]), + tf2Fabs(m_floats[1]), tf2Fabs(m_floats[2])); } - /**@brief Return the cross product between this and another vector + /**@brief Return the cross product between this and another vector * @param v The other vector */ TF2SIMD_FORCE_INLINE Vector3 cross(const Vector3& v) const { @@ -189,21 +189,21 @@ ATTRIBUTE_ALIGNED16(class) Vector3 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]) + + 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 + /**@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 + 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 + /**@brief Elementwise multiply this vector by the other * @param v The other vector */ TF2SIMD_FORCE_INLINE Vector3& operator*=(const Vector3& v) { @@ -269,7 +269,7 @@ ATTRIBUTE_ALIGNED16(class) Vector3 /**@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 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]; } @@ -286,7 +286,7 @@ ATTRIBUTE_ALIGNED16(class) Vector3 } /**@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 + * @param other The other Vector3 to compare with */ TF2SIMD_FORCE_INLINE void setMax(const Vector3& other) { @@ -296,7 +296,7 @@ ATTRIBUTE_ALIGNED16(class) Vector3 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 + * @param other The other Vector3 to compare with */ TF2SIMD_FORCE_INLINE void setMin(const Vector3& other) { @@ -328,12 +328,12 @@ ATTRIBUTE_ALIGNED16(class) Vector3 setValue(tf2Scalar(0.),tf2Scalar(0.),tf2Scalar(0.)); } - TF2SIMD_FORCE_INLINE bool isZero() const + 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 + TF2SIMD_FORCE_INLINE bool fuzzyZero() const { return length2() < TF2SIMD_EPSILON; } @@ -353,44 +353,44 @@ ATTRIBUTE_ALIGNED16(class) Vector3 }; /**@brief Return the sum of two vectors (Point symantics)*/ -TF2SIMD_FORCE_INLINE Vector3 -operator+(const Vector3& v1, const Vector3& v2) +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) +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 +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 +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 +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 +TF2SIMD_FORCE_INLINE Vector3 operator*(const tf2Scalar& s, const Vector3& v) -{ - return v * s; +{ + return v * s; } /**@brief Return the vector inversely scaled by s */ @@ -409,40 +409,40 @@ operator/(const Vector3& v1, const Vector3& v2) } /**@brief Return the dot product between two vectors */ -TF2SIMD_FORCE_INLINE tf2Scalar -tf2Dot(const Vector3& v1, const Vector3& v2) -{ - return v1.dot(v2); +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); +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); +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); +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 Vector3 +tf2Cross(const Vector3& v1, const Vector3& v2) +{ + return v1.cross(v2); } TF2SIMD_FORCE_INLINE tf2Scalar @@ -452,10 +452,10 @@ tf2Triple(const Vector3& v1, const Vector3& v2, const Vector3& v3) } /**@brief Return the linear interpolation between two vectors - * @param v1 One vector - * @param v2 The other vector + * @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 +TF2SIMD_FORCE_INLINE Vector3 lerp(const Vector3& v1, const Vector3& v2, const tf2Scalar& t) { return v1.lerp(v2, t); @@ -476,7 +476,7 @@ TF2SIMD_FORCE_INLINE tf2Scalar Vector3::distance(const Vector3& v) const TF2SIMD_FORCE_INLINE Vector3 Vector3::normalized() const { return *this / length(); -} +} TF2SIMD_FORCE_INLINE Vector3 Vector3::rotate( const Vector3& wAxis, const tf2Scalar angle ) const { @@ -498,18 +498,18 @@ class tf2Vector4 : public Vector3 TF2SIMD_FORCE_INLINE tf2Vector4() {} - TF2SIMD_FORCE_INLINE tf2Vector4(const tf2Scalar& x, const tf2Scalar& y, const tf2Scalar& z,const tf2Scalar& w) + 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 + TF2SIMD_FORCE_INLINE tf2Vector4 absolute4() const { return tf2Vector4( - tf2Fabs(m_floats[0]), - tf2Fabs(m_floats[1]), + tf2Fabs(m_floats[0]), + tf2Fabs(m_floats[1]), tf2Fabs(m_floats[2]), tf2Fabs(m_floats[3])); } @@ -543,9 +543,9 @@ class tf2Vector4 : public Vector3 { maxIndex = 3; } - - - + + + return maxIndex; @@ -575,35 +575,35 @@ class tf2Vector4 : public Vector3 { minIndex = 3; } - + return minIndex; } - TF2SIMD_FORCE_INLINE int closestAxis4() const + TF2SIMD_FORCE_INLINE int closestAxis4() const { return absolute4().maxAxis4(); } + + - - - /**@brief Set x,y,z and zero w + /**@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 +/* void getValue(tf2Scalar *m) const { m[0] = m_floats[0]; m[1] = m_floats[1]; m[2] =m_floats[2]; } */ -/**@brief Set the values +/**@brief Set the values * @param x Value of x * @param y Value of y * @param z Value of z diff --git a/tf2/include/tf2/transform_storage.h b/tf2/include/tf2/transform_storage.h index 44f8dc794..1f8d2872a 100644 --- a/tf2/include/tf2/transform_storage.h +++ b/tf2/include/tf2/transform_storage.h @@ -1,4 +1,3 @@ - // Copyright 2010, Willow Garage, Inc. All rights reserved. // // Redistribution and use in source and binary forms, with or without @@ -34,4 +33,3 @@ #include #endif // TF2__TRANSFORM_STORAGE_H_ - diff --git a/tf2/test/static_cache_test.cpp b/tf2/test/static_cache_test.cpp index e03411a1a..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); From d135e276e882e3e3b4e65747c065ecff967f6bab Mon Sep 17 00:00:00 2001 From: CursedRock17 Date: Fri, 8 Nov 2024 01:36:09 -0500 Subject: [PATCH 3/3] Fixing Visibility Controls Signed-off-by: CursedRock17 --- tf2/include/tf2/buffer_core.hpp | 2 +- tf2/include/tf2/buffer_core_interface.hpp | 2 +- tf2/include/tf2/convert.hpp | 2 +- tf2/include/tf2/exceptions.hpp | 2 +- tf2/include/tf2/time.hpp | 2 +- tf2/include/tf2/time_cache.hpp | 2 +- tf2/include/tf2/transform_storage.hpp | 2 +- tf2/include/tf2/utils.hpp | 3 +- tf2/include/tf2/visibility_control.h | 31 ++++++++++- tf2/include/tf2/visibility_control.hpp | 63 ----------------------- 10 files changed, 39 insertions(+), 72 deletions(-) delete mode 100644 tf2/include/tf2/visibility_control.hpp diff --git a/tf2/include/tf2/buffer_core.hpp b/tf2/include/tf2/buffer_core.hpp index 0664ba345..04c6f644e 100644 --- a/tf2/include/tf2/buffer_core.hpp +++ b/tf2/include/tf2/buffer_core.hpp @@ -51,7 +51,7 @@ #include "tf2/buffer_core_interface.hpp" #include "tf2/exceptions.hpp" #include "tf2/transform_storage.hpp" -#include "tf2/visibility_control.hpp" +#include "tf2/visibility_control.h" namespace tf2 { diff --git a/tf2/include/tf2/buffer_core_interface.hpp b/tf2/include/tf2/buffer_core_interface.hpp index 73beb5b68..d9c718a8e 100644 --- a/tf2/include/tf2/buffer_core_interface.hpp +++ b/tf2/include/tf2/buffer_core_interface.hpp @@ -34,7 +34,7 @@ #include "geometry_msgs/msg/transform_stamped.hpp" #include "tf2/time.hpp" -#include "tf2/visibility_control.hpp" +#include "tf2/visibility_control.h" namespace tf2 { diff --git a/tf2/include/tf2/convert.hpp b/tf2/include/tf2/convert.hpp index 89857a2de..b56ac500f 100644 --- a/tf2/include/tf2/convert.hpp +++ b/tf2/include/tf2/convert.hpp @@ -40,7 +40,7 @@ #include "tf2/exceptions.hpp" #include "tf2/impl/convert.hpp" #include "tf2/transform_datatypes.hpp" -#include "tf2/visibility_control.hpp" +#include "tf2/visibility_control.h" namespace tf2 { diff --git a/tf2/include/tf2/exceptions.hpp b/tf2/include/tf2/exceptions.hpp index 11ebcd3d6..690071f5e 100644 --- a/tf2/include/tf2/exceptions.hpp +++ b/tf2/include/tf2/exceptions.hpp @@ -35,7 +35,7 @@ #include #include -#include "tf2/visibility_control.hpp" +#include "tf2/visibility_control.h" namespace tf2 { diff --git a/tf2/include/tf2/time.hpp b/tf2/include/tf2/time.hpp index 5d5bfb1ca..c01aab84c 100644 --- a/tf2/include/tf2/time.hpp +++ b/tf2/include/tf2/time.hpp @@ -33,7 +33,7 @@ #include #include -#include "tf2/visibility_control.hpp" +#include "tf2/visibility_control.h" namespace tf2 { diff --git a/tf2/include/tf2/time_cache.hpp b/tf2/include/tf2/time_cache.hpp index dd17d0fc2..cc7cc5d0a 100644 --- a/tf2/include/tf2/time_cache.hpp +++ b/tf2/include/tf2/time_cache.hpp @@ -37,7 +37,7 @@ #include #include -#include "tf2/visibility_control.hpp" +#include "tf2/visibility_control.h" #include "tf2/transform_storage.hpp" #include "tf2/exceptions.hpp" diff --git a/tf2/include/tf2/transform_storage.hpp b/tf2/include/tf2/transform_storage.hpp index 6e73f1d44..2f285d845 100644 --- a/tf2/include/tf2/transform_storage.hpp +++ b/tf2/include/tf2/transform_storage.hpp @@ -30,10 +30,10 @@ #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" -#include "tf2/visibility_control.hpp" namespace tf2 { diff --git a/tf2/include/tf2/utils.hpp b/tf2/include/tf2/utils.hpp index b4a1f7f46..f275d7759 100644 --- a/tf2/include/tf2/utils.hpp +++ b/tf2/include/tf2/utils.hpp @@ -15,10 +15,11 @@ #ifndef TF2__UTILS_HPP_ #define TF2__UTILS_HPP_ +#include + #include #include #include -#include namespace tf2 { diff --git a/tf2/include/tf2/visibility_control.h b/tf2/include/tf2/visibility_control.h index 9a5a1ec70..cb4611891 100644 --- a/tf2/include/tf2/visibility_control.h +++ b/tf2/include/tf2/visibility_control.h @@ -29,6 +29,35 @@ #ifndef TF2__VISIBILITY_CONTROL_H_ #define TF2__VISIBILITY_CONTROL_H_ -#include +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ + #ifdef __GNUC__ + #define TF2_EXPORT __attribute__ ((dllexport)) + #define TF2_IMPORT __attribute__ ((dllimport)) + #else + #define TF2_EXPORT __declspec(dllexport) + #define TF2_IMPORT __declspec(dllimport) + #endif + #ifdef TF2_BUILDING_DLL + #define TF2_PUBLIC TF2_EXPORT + #else + #define TF2_PUBLIC TF2_IMPORT + #endif + #define TF2_PUBLIC_TYPE TF2_PUBLIC + #define TF2_LOCAL +#else + #define TF2_EXPORT __attribute__ ((visibility("default"))) + #define TF2_IMPORT + #if __GNUC__ >= 4 + #define TF2_PUBLIC __attribute__ ((visibility("default"))) + #define TF2_LOCAL __attribute__ ((visibility("hidden"))) + #else + #define TF2_PUBLIC + #define TF2_LOCAL + #endif + #define TF2_PUBLIC_TYPE +#endif #endif // TF2__VISIBILITY_CONTROL_H_ diff --git a/tf2/include/tf2/visibility_control.hpp b/tf2/include/tf2/visibility_control.hpp deleted file mode 100644 index c011ec959..000000000 --- a/tf2/include/tf2/visibility_control.hpp +++ /dev/null @@ -1,63 +0,0 @@ -// Copyright 2017, 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__VISIBILITY_CONTROL_HPP_ -#define TF2__VISIBILITY_CONTROL_HPP_ - -// This logic was borrowed (then namespaced) from the examples on the gcc wiki: -// https://gcc.gnu.org/wiki/Visibility - -#if defined _WIN32 || defined __CYGWIN__ - #ifdef __GNUC__ - #define TF2_EXPORT __attribute__ ((dllexport)) - #define TF2_IMPORT __attribute__ ((dllimport)) - #else - #define TF2_EXPORT __declspec(dllexport) - #define TF2_IMPORT __declspec(dllimport) - #endif - #ifdef TF2_BUILDING_DLL - #define TF2_PUBLIC TF2_EXPORT - #else - #define TF2_PUBLIC TF2_IMPORT - #endif - #define TF2_PUBLIC_TYPE TF2_PUBLIC - #define TF2_LOCAL -#else - #define TF2_EXPORT __attribute__ ((visibility("default"))) - #define TF2_IMPORT - #if __GNUC__ >= 4 - #define TF2_PUBLIC __attribute__ ((visibility("default"))) - #define TF2_LOCAL __attribute__ ((visibility("hidden"))) - #else - #define TF2_PUBLIC - #define TF2_LOCAL - #endif - #define TF2_PUBLIC_TYPE -#endif - -#endif // TF2__VISIBILITY_CONTROL_HPP_