aldebaran/libalmath

Switch branches/tags
Nothing to show
Fetching contributors…
Cannot retrieve contributors at this time
739 lines (677 sloc) 23.7 KB
 /* * Copyright (c) 2012 Aldebaran Robotics. All rights reserved. * Use of this source code is governed by a BSD-style license that can be * found in the COPYING file. */ #pragma once #ifndef _LIBALMATH_ALMATH_TYPES_ALTRANSFORM_H_ #define _LIBALMATH_ALMATH_TYPES_ALTRANSFORM_H_ #include #include namespace AL { namespace Math { /// /// A homogenous transformation matrix. /// /// /// more information /// \ingroup Types struct ALMATH_API Transform { /** \cond PRIVATE */ float r1_c1, r1_c2, r1_c3, r1_c4; float r2_c1, r2_c2, r2_c3, r2_c4; float r3_c1, r3_c2, r3_c3, r3_c4; /** \endcond */ /// /// Create a Transform initialized to identity. /** * * \f$\left[\begin{array}{cccc} * r_1c_1 & r_1c_2 & r_1c_3 & r_1c_4 \\ * r_2c_1 & r_2c_2 & r_2c_3 & r_2c_4 \\ * r_3c_1 & r_3c_2 & r_3c_3 & r_3c_4 \\ * 0.0 & 0.0 & 0.0 & 1.0 * \end{array}\right] = * \left[\begin{array}{cccc} * 1.0 & 0.0 & 0.0 & 0.0 \\ * 0.0 & 1.0 & 0.0 & 0.0 \\ * 0.0 & 0.0 & 1.0 & 0.0 \\ * 0.0 & 0.0 & 0.0 & 1.0 * \end{array}\right]\f$ */ /// Transform(); /// /// Create a Transform with an std::vector. /// /// /// An std::vector of size 12 or 16 for respectively: /// /** * * \f$\left[\begin{array}{cccc} * r_1c_1 & r_1c_2 & r_1c_3 & r_1c_4 \\ * r_2c_1 & r_2c_2 & r_2c_3 & r_2c_4 \\ * r_3c_1 & r_3c_2 & r_3c_3 & r_3c_4 \\ * 0.0 & 0.0 & 0.0 & 1.0 * \end{array}\right] = * \left[\begin{array}{cccc} * pFloats[00] & pFloats[01] & pFloats[02] & pFloats[03] \\ * pFloats[04] & pFloats[05] & pFloats[06] & pFloats[07] \\ * pFloats[08] & pFloats[09] & pFloats[10] & pFloats[11] \\ * 0.0 & 0.0 & 0.0 & 1.0 * \end{array}\right]\f$ */ explicit Transform(const std::vector& pFloats); /// /// Create a Transform initialized with explicit value for translation /// part. Rotation part is set to identity. /** * * \f$\left[\begin{array}{cccc} * r_1c_1 & r_1c_2 & r_1c_3 & r_1c_4 \\ * r_2c_1 & r_2c_2 & r_2c_3 & r_2c_4 \\ * r_3c_1 & r_3c_2 & r_3c_3 & r_3c_4 \\ * 0.0 & 0.0 & 0.0 & 1.0 * \end{array}\right] = * \left[\begin{array}{cccc} * 1.0 & 0.0 & 0.0 & pPosX \\ * 0.0 & 1.0 & 0.0 & pPosY \\ * 0.0 & 0.0 & 1.0 & pPosZ \\ * 0.0 & 0.0 & 0.0 & 1.0 * \end{array}\right]\f$ */ /// /// the float value for translation x /// the float value for translation y /// the float value for translation z Transform( const float& pPosX, const float& pPosY, const float& pPosZ); /// /// Overloading of operator *= for Transform. /// /// the second Transform Transform& operator*= (const Transform& pT2); /// /// Overloading of operator * for Transform. /// /// the second Transform Transform operator* (const Transform& pT2) const; /// /// Overloading of operator == for Transform. /// /// the second Transform bool operator==(const Transform& pT2) const; /// /// Overloading of operator != for Transform. /// /// the second Transform bool operator!=(const Transform& pT2) const; /// /// Check if the actual Transform is near the one /// given in argument. /// /// /// the second Transform /// an optionnal epsilon distance: Default: 0.0001 /// /// true if the distance between the two Transform is less than pEpsilon /// bool isNear( const Transform& pT2, const float& pEpsilon=0.0001f) const; /// /// Check if the rotation part is correct. /// The condition checks are: /// \f$R^t * R = I\f$ /// and /// determinant(R) = 1.0 /// /// /// an optionnal epsilon distance. Default: 0.0001 /// /// true if the Transform is correct /// bool isTransform( const float& pEpsilon=0.0001f) const; /// /// Normalize data, if needed, to have transform properties. /// /// void normalizeTransform(void); /// /// Compute the norm translation part of the actual Transform: /// /// \f$\sqrt{pT.r_1c_4^2+pT.r_2c_4^2+pT.r_3c_4^2}\f$ /// /// /// the float norm of the Transform /// float norm() const; /// /// Compute the determinant of rotation part of the actual Transform: /// /** * \f$pT.r_1c_1*pT.r_2c_2*pT.r_3c_3 + pT.r_1c_2*pT.r_2c_3*pT.r_3c_1 + * pT.r_1c_3*pT.r_2c_1*pT.r_3c_2 - pT.r_1c_1*pT.r_2c_3*pT.r_3c_2 - * pT.r_1c_2*pT.r_2c_1*pT.r_3c_3 - pT.r_1c_3*pT.r_2c_2*pT.r_3c_1\f$ */ /// /// /// the float determinant of rotation Transform part /// float determinant() const; /// /// Compute the transform inverse of the actual Transform: /// /** * \f$pT = \left[\begin{array}{cc}R & r \\ * 0_{31} & 1 \end{array}\right]\f$ */ /// /** \f$pTOut = \left[\begin{array}{cc} * R^t & (-R^t*r) \\ * 0_{31} & 1 * \end{array}\right]\f$ */ /// /// /// /// the Transform inverse /// Transform inverse() const; /// /// Create a Transform initialized with explicit rotation around x axis. /// /** \f$pT = \left[\begin{array}{cccc} * 1.0 & 0.0 & 0.0 & 0.0 \\ * 0.0 & cos(pRotX) & -sin(pRotX) & 0.0 \\ * 0.0 & sin(pRotX) & cos(pRotX) & 0.0 \\ * 0.0 & 0.0 & 0.0 & 1.0 \end{array}\right]\f$ */ /// /// the float value for angle rotation in radian around x axis static Transform fromRotX(const float pRotX); /// /// Create a Transform initialized with explicit rotation around y axis. /// /** * \f$pT = \left[\begin{array}{cccc} * cos(pRotY) & 0.0 & sin(pRotY) & 0.0 \\ * 0.0 & 1.0 & 0.0 & 0.0 \\ * -sin(pRotY) & 0.0 & cos(pRotY) & 0.0 \\ * 0.0 & 0.0 & 0.0 & 1.0 * \end{array}\right]\f$ */ /// /// the float value for angle rotation in radian around y axis static Transform fromRotY(const float pRotY); /// /// Create a Transform initialized with explicit rotation around z axis. /// /** * \f$pT = \left[\begin{array}{cccc} * cos(pRotZ) & -sin(pRotZ) & 0.0 & 0.0 \\ * sin(pRotZ) & cos(pRotZ) & 0.0 & 0.0 \\ * 0.0 & 0.0 & 1.0 & 0.0 \\ * 0.0 & 0.0 & 0.0 & 1.0 * \end{array}\right]\f$ */ /// /// the float value for angle rotation in radian around z axis static Transform fromRotZ(const float pRotZ); /// /// Create a Transform initialize with euler angle. /// /// H = fromRotZ(pWZ)*fromRotY(pWY)*fromRotX(pWX) /// /// /// the float value for euler angle x in radian /// the float value for euler angle y in radian /// the float value for euler angle z in radian static Transform from3DRotation( const float& pWX, const float& pWY, const float& pWZ); /// /// Create a Transform initialize with explicit value for translation part. /// /** * \f$pT = \left[\begin{array}{cccc} * 1.0 & 0.0 & 0.0 & pX \\ * 0.0 & 1.0 & 0.0 & pY \\ * 0.0 & 0.0 & 1.0 & pZ \\ * 0.0 & 0.0 & 0.0 & 1.0 * \end{array}\right]\f$ */ /// /// the float value for translation axis x in meter (r1_c4) /// the float value for translation axis y in meter (r2_c4) /// the float value for translation axis z in meter (r3_c4) static Transform fromPosition( const float pX, const float pY, const float pZ); /// /// Create a Transform initialize with explicit value for translation part and euler angle. /// /// H = fromRotZ(pWZ)*fromRotY(pWY)*fromRotX(pWX) /// /// then /// /// H.r1_c4 = pX /// /// H.r2_c4 = pY /// /// H.r3_c4 = pZ /// /// /// the float value for translation axis x in meter (r1_c4) /// the float value for translation axis y in meter (r2_c4) /// the float value for translation axis z in meter (r3_c4) /// the float value for euler angle x in radian /// the float value for euler angle y in radian /// the float value for euler angle z in radian static Transform fromPosition( const float& pX, const float& pY, const float& pZ, const float& pWX, const float& pWY, const float& pWZ); /// /// Compute the Transform between the actual /// Transform and the one given in argument: /// /// result: inverse(pT1)*pT2 /// /// /// the second transform Transform diff(const Transform& pT2) const; /// /// Compute the squared distance between the actual /// Transform and the one given in argument (translation part): /// /// \f$(pT1.r_1c_4-pT2.r_1c_4)^2+(pT1.r_2c_4-pT2.r_2c_4)^2+(pT1.r_3c_4-pT2.r_3c_4)^2\f$ /// /// the second Transform /// /// the float squared distance between the two Transform: translation part /// float distanceSquared(const Transform& pT2) const; /// /// Compute the distance between the actual /// Transform and the one given in argument: /// /// \f$\sqrt{(pT1.r_1c_4-pT2.r_1c_4)^2+(pT1.r_2c_4-pT2.r_2c_4)^2+(pT1.r_3c_4-pT2.r_3c_4)^2}\f$ /// /// the second Transform /// /// the float distance between the two Transform /// float distance(const Transform& pT2) const; /// /// Return the Transform as a vector of float: /// /** \f$\begin{array}{cccc} * [r_1c_1, & r_1c_2, & r_1c_3, & r_1c_4, \\ * r_2c_1, & r_2c_2, & r_2c_3, & r_2c_4, \\ * r_3c_1, & r_3c_2, & r_3c_3, & r_3c_4, \\ * 0.0, & 0.0, & 0.0, & 1.0] * \end{array}\f$ */ /// void toVector(std::vector& pReturnVector) const; std::vector toVector(void) const; /// /// Write the Transform in the vector and update the iterator: /// /** \f$\begin{array}{cccc} * [r_1c_1, & r_1c_2, & r_1c_3, & r_1c_4, \\ * r_2c_1, & r_2c_2, & r_2c_3, & r_2c_4, \\ * r_3c_1, & r_3c_2, & r_3c_3, & r_3c_4, \\ * 0.0, & 0.0, & 0.0, & 1.0] * \end{array}\f$ */ /// It is assumed the vector has enough space. /// void writeToVector(std::vector::iterator& pIt) const; }; // end struct /// /// pTOut = pT*pTOut /// /// /// the first constant Transform /// the second modified Transform /// \ingroup Types ALMATH_API void transformPreMultiply( const Transform& pT, Transform& pTOut); /// /// Compute the norm translation part of the actual Transform: /// /// \f$\sqrt{pT.r_1c_4^2+pT.r_2c_4^2+pT.r_3c_4^2}\f$ /// /// the given Transform /// /// the float norm of the given Transform /// /// \ingroup Types ALMATH_API float norm(const Transform& pT); /// /// Normalize data, if needed, to have transform properties. /// /// /// the given Transform /// \ingroup Types ALMATH_API void normalizeTransform(Transform& pT); /// /// DEPRECATED: Use toVector function. /// Copy the Transform in a vector of float: /// /** \f$\begin{array}{cccc} * [r_1c_1, & r_1c_2, & r_1c_3, & r_1c_4, \\ * r_2c_1, & r_2c_2, & r_2c_3, & r_2c_4, \\ * r_3c_1, & r_3c_2, & r_3c_3, & r_3c_4, \\ * 0.0, & 0.0, & 0.0, & 1.0] * \end{array}\f$ */ /// /// /// the given Transform /// the vector of float update to given transform value /// \ingroup Types ALMATH_API void transformToFloatVector( const Transform& pT, std::vector& pTOut); /// /// DEPRECATED: Use toVector function. /// Return the Transform in a vector of float: /// /** * \f$\begin{array}{cccc} * [r_1c_1, & r_1c_2, & r_1c_3, & r_1c_4, \\ * r_2c_1, & r_2c_2, & r_2c_3, & r_2c_4, \\ * r_3c_1, & r_3c_2, & r_3c_3, & r_3c_4, \\ * 0.0, & 0.0, & 0.0, & 1.0] * \end{array}\f$ */ /// /// /// the given Transform /// /// the vector of float update to given transform value /// /// \ingroup Types ALMATH_API std::vector transformToFloatVector( const Transform& pT); /// /// Compute the determinant of rotation part of the given Transform: /// /** \f$pT.r_1c_1*pT.r_2c_2*pT.r_3c_3 + pT.r_1c_2*pT.r_2c_3*pT.r_3c_1 + * pT.r_1c_3*pT.r_2c_1 * pT.r_3c_2 - pT.r_1c_1*pT.r_2c_3*pT.r_3c_2 - * pT.r_1c_2*pT.r_2c_1*pT.r_3c_3 - pT.r_1c_3*pT.r_2c_2*pT.r_3c_1\f$ */ /// /// the given Transform /// /// the float determinant of rotation Transform part /// /// \ingroup Types ALMATH_API float determinant(const Transform& pT); /// /// Compute the determinant of rotation part of the given vector of floats: /// /** \f$pT[0]*pT[5]*pT[10] + pT[1]*pT[6]*pT[8] + * pT[2]*pT[4]*pT[9] - pT[0]*pT[6]*pT[9] - * pT[1]*pT[4]*pT[10] - pT[2]*pT[5]*pT[8]\f$ */ /// /// the given vector of floats /// /// the float determinant of rotation Transform part /// /// \ingroup Types ALMATH_API float determinant(const std::vector& pFloats); /// /// Return the transform inverse of the given Transform: /// /** \f$pT = \left[\begin{array}{cc} * R & r \\ * 0_{31} & 1 * \end{array}\right]\f$ */ /// /** \f$pTOut = \left[\begin{array}{cc} * R^t & (-R^t*r) \\ * 0_{31} & 1 * \end{array}\right]\f$ */ /// /// /// the given Transform /// the inverse of the given Transform /// \ingroup Types ALMATH_API void transformInverse( const Transform& pT, Transform& pTOut); /// /// Return the transform inverse of the given Transform: /// /** \f$pT = \left[\begin{array}{cc} * R & r \\ * 0_{31} & 1 * \end{array}\right]\f$ */ /// /** \f$pTOut = \left[\begin{array}{cc} * R^t & (-R^t*r) \\ * 0_{31} & 1 * \end{array}\right]\f$ */ /// /// /// the given Transform /// /// the Transform inverse /// /// \ingroup Types ALMATH_API Transform transformInverse(const Transform& pT); /// /// Create a Transform initialize with explicit rotation around x axis: /// /** \f$pTOut = \left[\begin{array}{cccc} * 1.0 & 0.0 & 0.0 & 0.0 \\ * 0.0 & cos(pRotX) & -sin(pRotX) & 0.0 \\ * 0.0 & sin(pRotX) & cos(pRotX) & 0.0 \\ * 0.0 & 0.0 & 0.0 & 1.0 * \end{array}\right]\f$ */ /// /// /// the float value for angle rotation in radian around x axis /// /// the Transform /// /// \ingroup Types ALMATH_API Transform transformFromRotX(const float pRotX); /// /// Create a Transform initialize with explicit rotation around y axis: /// /** \f$pTOut = \left[\begin{array}{cccc} * cos(pRotY) & 0.0 & sin(pRotY) & 0.0 \\ * 0.0 & 1.0 & 0.0 & 0.0 \\ * -sin(pRotY) & 0.0 & cos(pRotY) & 0.0 \\ * 0.0 & 0.0 & 0.0 & 1.0 * \end{array}\right]\f$ */ /// /// /// the float value for angle rotation in radian around y axis /// /// the Transform /// /// \ingroup Types ALMATH_API Transform transformFromRotY(const float pRotY); /// /// Create a Transform initialize with explicit rotation around z axis: /// /** \f$pTOut = \left[\begin{array}{cccc} * cos(pRotZ) & -sin(pRotZ) & 0.0 & 0.0 \\ * sin(pRotZ) & cos(pRotZ) & 0.0 & 0.0 \\ * 0.0 & 0.0 & 1.0 & 0.0 \\ * 0.0 & 0.0 & 0.0 & 1.0 * \end{array}\right]\f$ */ /// /// /// the float value for angle rotation in radian around z axis /// /// the Transform /// /// \ingroup Types ALMATH_API Transform transformFromRotZ(const float pRotZ); /// /// Create a Transform initialize with euler angle: /// H = fromRotZ(pWZ)*fromRotY(pWY)*fromRotX(pWX) /// /// /// the float value for euler angle x in radian /// the float value for euler angle y in radian /// the float value for euler angle z in radian /// /// the Transform /// /// \ingroup Types ALMATH_API Transform transformFrom3DRotation( const float& pWX, const float& pWY, const float& pWZ); /// /// Create a Transform initialize with explicit value for translation part: /// /** \f$pTOut = \left[\begin{array}{cccc} * 1.0 & 0.0 & 0.0 & pX \\ * 0.0 & 1.0 & 0.0 & pY \\ * 0.0 & 0.0 & 1.0 & pZ \\ * 0.0 & 0.0 & 0.0 & 1.0 * \end{array}\right]\f$ */ /// /// /// the float value for translation axis x in meter (r1_c4) /// the float value for translation axis y in meter (r2_c4) /// the float value for translation axis z in meter (r3_c4) /// /// the Transform /// /// \ingroup Types ALMATH_API Transform transformFromPosition( const float& pX, const float& pY, const float& pZ); /// /// Create a Transform initialize with explicit value for translation part and euler angle: /// /// H = fromRotZ(pWZ)*fromRotY(pWY)*fromRotX(pWX) /// /// H.r1_c4 = pX /// /// H.r2_c4 = pY /// /// H.r3_c4 = pZ /// /// /// the float value for translation axis x in meter (r1_c4) /// the float value for translation axis y in meter (r2_c4) /// the float value for translation axis z in meter (r3_c4) /// the float value for euler angle x in radian /// the float value for euler angle y in radian /// the float value for euler angle z in radian /// /// the Transform /// /// \ingroup Types ALMATH_API Transform transformFromPosition( const float& pX, const float& pY, const float& pZ, const float& pWX, const float& pWY, const float& pWZ); /// /// Inverse the given Transform in place: /// /// /// the given Transform /// \ingroup Types ALMATH_API void transformInvertInPlace(Transform& pT); /// /// Alternative name for inverse: return the transform inverse of the given Transform: /// /** \f$pT = \left[\begin{array}{cc} * R & r \\ * 0_{31} & 1 * \end{array}\right]\f$ */ /// /** \f$pTOut = \left[\begin{array}{cc} * R^t & (-R^t*r) \\ * 0_{31} & 1 * \end{array}\right]\f$ */ /// /// /// the given Transform /// \ingroup Types ALMATH_API Transform pinv(const Transform& pT); /// /// Compute the Transform between the actual Transform and the one give in argument result: /// /// inverse(pT1)*pT2 /// /// /// the first transform /// the second transform /// /// the Transform /// /// \ingroup Types ALMATH_API Transform transformDiff( const Transform& pT1, const Transform& pT2); /// /// Compute the squared distance between the actual /// Transform and the one give in argument (translation part): /// /// \f$(pT1.r_1c_4-pT2.r_1c_4)^2 +(pT1.r_2c_4-pT2.r_2c_4)^2+(pT1.r_3c_4-pT2.r_3c_4)^2\f$ /// /// the first Transform /// the second Transform /// /// the float squared distance between the two Transform: translation part /// /// \ingroup Types ALMATH_API float transformDistanceSquared( const Transform& pT1, const Transform& pT2); /// /// Compute the distance between the actual /// Transform and the one give in argument: /// /// \f$\sqrt{(pT1.r_1c_4-pT2.r_1c_4)^2+(pT1.r_2c_4-pT2.r_2c_4)^2+(pT1.r_3c_4-pT2.r_3c_4)^2}\f$ /// /// the first Transform /// the second Transform /// /// the float distance between the two Transform /// /// \ingroup Types ALMATH_API float transformDistance( const Transform& pT1, const Transform& pT2); } // end namespace Math } // end namespace AL #endif // _LIBALMATH_ALMATH_TYPES_ALTRANSFORM_H_