.. _program_listing_file__tmp_ws_src_hpp-fcl_include_hpp_fcl_math_transform.h:

Program Listing for File transform.h
====================================

|exhale_lsh| :ref:`Return to documentation for file <file__tmp_ws_src_hpp-fcl_include_hpp_fcl_math_transform.h>` (``/tmp/ws/src/hpp-fcl/include/hpp/fcl/math/transform.h``)

.. |exhale_lsh| unicode:: U+021B0 .. UPWARDS ARROW WITH TIP LEFTWARDS

.. code-block:: cpp

   /*
    * Software License Agreement (BSD License)
    *
    *  Copyright (c) 2011-2014, Willow Garage, Inc.
    *  Copyright (c) 2014-2015, Open Source Robotics Foundation
    *  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 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 OWNER 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 HPP_FCL_TRANSFORM_H
   #define HPP_FCL_TRANSFORM_H
   
   #include <hpp/fcl/data_types.h>
   
   namespace hpp {
   namespace fcl {
   
   typedef Eigen::Quaternion<FCL_REAL> Quaternion3f;
   
   static inline std::ostream& operator<<(std::ostream& o, const Quaternion3f& q) {
     o << "(" << q.w() << " " << q.x() << " " << q.y() << " " << q.z() << ")";
     return o;
   }
   
   class HPP_FCL_DLLAPI Transform3f {
     Matrix3f R;
   
     Vec3f T;
   
    public:
     Transform3f() {
       setIdentity();  // set matrix_set true
     }
   
     static Transform3f Identity() { return Transform3f(); }
   
     template <typename Matrixx3Like, typename Vector3Like>
     Transform3f(const Eigen::MatrixBase<Matrixx3Like>& R_,
                 const Eigen::MatrixBase<Vector3Like>& T_)
         : R(R_), T(T_) {}
   
     template <typename Vector3Like>
     Transform3f(const Quaternion3f& q_, const Eigen::MatrixBase<Vector3Like>& T_)
         : R(q_.toRotationMatrix()), T(T_) {}
   
     Transform3f(const Matrix3f& R_) : R(R_), T(Vec3f::Zero()) {}
   
     Transform3f(const Quaternion3f& q_) : R(q_), T(Vec3f::Zero()) {}
   
     Transform3f(const Vec3f& T_) : R(Matrix3f::Identity()), T(T_) {}
   
     Transform3f(const Transform3f& tf) : R(tf.R), T(tf.T) {}
   
     Transform3f& operator=(const Transform3f& tf) {
       R = tf.R;
       T = tf.T;
       return *this;
     }
   
     inline const Vec3f& getTranslation() const { return T; }
   
     inline const Vec3f& translation() const { return T; }
   
     inline Vec3f& translation() { return T; }
   
     inline const Matrix3f& getRotation() const { return R; }
   
     inline const Matrix3f& rotation() const { return R; }
   
     inline Matrix3f& rotation() { return R; }
   
     inline Quaternion3f getQuatRotation() const { return Quaternion3f(R); }
   
     template <typename Matrix3Like, typename Vector3Like>
     inline void setTransform(const Eigen::MatrixBase<Matrix3Like>& R_,
                              const Eigen::MatrixBase<Vector3Like>& T_) {
       R.noalias() = R_;
       T.noalias() = T_;
     }
   
     inline void setTransform(const Quaternion3f& q_, const Vec3f& T_) {
       R = q_.toRotationMatrix();
       T = T_;
     }
   
     template <typename Derived>
     inline void setRotation(const Eigen::MatrixBase<Derived>& R_) {
       R.noalias() = R_;
     }
   
     template <typename Derived>
     inline void setTranslation(const Eigen::MatrixBase<Derived>& T_) {
       T.noalias() = T_;
     }
   
     inline void setQuatRotation(const Quaternion3f& q_) {
       R = q_.toRotationMatrix();
     }
   
     template <typename Derived>
     inline Vec3f transform(const Eigen::MatrixBase<Derived>& v) const {
       return R * v + T;
     }
   
     inline Transform3f& inverseInPlace() {
       R.transposeInPlace();
       T = -R * T;
       return *this;
     }
   
     inline Transform3f inverse() {
       return Transform3f(R.transpose(), -R.transpose() * T);
     }
   
     inline Transform3f inverseTimes(const Transform3f& other) const {
       return Transform3f(R.transpose() * other.R, R.transpose() * (other.T - T));
     }
   
     inline const Transform3f& operator*=(const Transform3f& other) {
       T += R * other.T;
       R *= other.R;
       return *this;
     }
   
     inline Transform3f operator*(const Transform3f& other) const {
       return Transform3f(R * other.R, R * other.T + T);
     }
   
     inline bool isIdentity(
         const FCL_REAL& prec =
             Eigen::NumTraits<FCL_REAL>::dummy_precision()) const {
       return R.isIdentity(prec) && T.isZero(prec);
     }
   
     inline void setIdentity() {
       R.setIdentity();
       T.setZero();
     }
   
     bool operator==(const Transform3f& other) const {
       return R == other.R && (T == other.getTranslation());
     }
   
     bool operator!=(const Transform3f& other) const { return !(*this == other); }
   
     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
   };
   
   template <typename Derived>
   inline Quaternion3f fromAxisAngle(const Eigen::MatrixBase<Derived>& axis,
                                     FCL_REAL angle) {
     return Quaternion3f(Eigen::AngleAxis<FCL_REAL>(angle, axis));
   }
   
   }  // namespace fcl
   }  // namespace hpp
   
   #endif