GCC Code Coverage Report

Directory:	./
File:	include/coal/math/transform.h
Date:	2025-06-03 17:15:40

	Exec	Total	Coverage
Lines:	85	101	84.2%
Functions:	50	56	89.3%
Branches:	60	130	46.2%

  
      Line
      Branch
      Exec
      Source
    
      /*
    
       * 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.
    
       */
    
      /** \author Jia Pan */
    
      #ifndef COAL_TRANSFORM_H
    
      #define COAL_TRANSFORM_H
    
      #include "coal/fwd.hh"
    
      #include "coal/data_types.h"
    
      namespace coal {
    
      COAL_DEPRECATED typedef Eigen::Quaternion<Scalar> Quaternion3f;
    
      typedef Eigen::Quaternion<Scalar> Quats;
    
      ✗
      static inline std::ostream& operator<<(std::ostream& o, const Quats& q) {
    
      ✗
        o << "(" << q.w() << " " << q.x() << " " << q.y() << " " << q.z() << ")";
    
      ✗
        return o;
    
      }
    
      /// @brief Simple transform class used locally by InterpMotion
    
      class COAL_DLLAPI Transform3s {
    
        /// @brief Matrix cache
    
        Matrix3s R;
    
        /// @brief Translation vector
    
        Vec3s T;
    
       public:
    
        /// @brief Default transform is no movement
    
      317557
        Transform3s() {
    
      317557
          setIdentity();  // set matrix_set true
    
      317557
        }
    
      1700
        static Transform3s Identity() { return Transform3s(); }
    
        /// @brief Construct transform from rotation and translation
    
        template <typename Matrixx3Like, typename Vector3Like>
    
      21067
        Transform3s(const Eigen::MatrixBase<Matrixx3Like>& R_,
    
                    const Eigen::MatrixBase<Vector3Like>& T_)
    
      21067
            : R(R_), T(T_) {}
    
        /// @brief Construct transform from rotation and translation
    
        template <typename Vector3Like>
    
      333
        Transform3s(const Quats& q_, const Eigen::MatrixBase<Vector3Like>& T_)
    
      333
            : R(q_.toRotationMatrix()), T(T_) {}
    
        /// @brief Construct transform from rotation
    
        2/4✓ Branch 2 taken 102 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 102 times.
✗ Branch 6 not taken.

      102
        Transform3s(const Matrix3s& R_) : R(R_), T(Vec3s::Zero()) {}
    
        /// @brief Construct transform from rotation
    
        2/4✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.

      2
        Transform3s(const Quats& q_) : R(q_), T(Vec3s::Zero()) {}
    
        /// @brief Construct transform from translation
    
        2/4✓ Branch 1 taken 7075 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7075 times.
✗ Branch 5 not taken.

      7075
        Transform3s(const Vec3s& T_) : R(Matrix3s::Identity()), T(T_) {}
    
        /// @brief Construct transform from other transform
    
      59700
        Transform3s(const Transform3s& tf) : R(tf.R), T(tf.T) {}
    
        /// @brief operator =
    
      66087
        Transform3s& operator=(const Transform3s& tf) {
    
      66087
          R = tf.R;
    
      66087
          T = tf.T;
    
      66087
          return *this;
    
        }
    
        /// @brief get translation
    
      2769047
        inline const Vec3s& getTranslation() const { return T; }
    
        /// @brief get translation
    
      149
        inline const Vec3s& translation() const { return T; }
    
        /// @brief get translation
    
      233
        inline Vec3s& translation() { return T; }
    
        /// @brief get rotation
    
      4435679
        inline const Matrix3s& getRotation() const { return R; }
    
        /// @brief get rotation
    
      348
        inline const Matrix3s& rotation() const { return R; }
    
        /// @brief get rotation
    
      248
        inline Matrix3s& rotation() { return R; }
    
        /// @brief get quaternion
    
      1
        inline Quats getQuatRotation() const { return Quats(R); }
    
        /// @brief set transform from rotation and translation
    
        template <typename Matrix3Like, typename Vector3Like>
    
      62021
        inline void setTransform(const Eigen::MatrixBase<Matrix3Like>& R_,
    
                                 const Eigen::MatrixBase<Vector3Like>& T_) {
    
        2/4✓ Branch 1 taken 62021 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 62021 times.
✗ Branch 5 not taken.

      62021
          R.noalias() = R_;
    
        2/4✓ Branch 1 taken 62021 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 62021 times.
✗ Branch 5 not taken.

      62021
          T.noalias() = T_;
    
      62021
        }
    
        /// @brief set transform from rotation and translation
    
      ✗
        inline void setTransform(const Quats& q_, const Vec3s& T_) {
    
      ✗
          R = q_.toRotationMatrix();
    
      ✗
          T = T_;
    
      ✗
        }
    
        /// @brief set transform from rotation
    
        template <typename Derived>
    
      6002
        inline void setRotation(const Eigen::MatrixBase<Derived>& R_) {
    
        2/4✓ Branch 1 taken 6002 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6002 times.
✗ Branch 5 not taken.

      6002
          R.noalias() = R_;
    
      6002
        }
    
        /// @brief set transform from translation
    
        template <typename Derived>
    
      26225
        inline void setTranslation(const Eigen::MatrixBase<Derived>& T_) {
    
        2/4✓ Branch 1 taken 16199 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 16199 times.
✗ Branch 5 not taken.

      26225
          T.noalias() = T_;
    
      26225
        }
    
        /// @brief set transform from rotation
    
        1/2✓ Branch 1 taken 891 times.
✗ Branch 2 not taken.

      891
        inline void setQuatRotation(const Quats& q_) { R = q_.toRotationMatrix(); }
    
        /// @brief transform a given vector by the transform
    
        template <typename Derived>
    
      202156102
        inline Vec3s transform(const Eigen::MatrixBase<Derived>& v) const {
    
        3/6✓ Branch 1 taken 177952893 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 177952893 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 177952893 times.
✗ Branch 8 not taken.

      402728080
          return R * v + T;
    
        }
    
        /// @brief transform a given vector by the inverse of the transform
    
        template <typename Derived>
    
      203
        inline Vec3s inverseTransform(const Eigen::MatrixBase<Derived>& v) const {
    
        4/8✓ Branch 1 taken 195 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 195 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 195 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 195 times.
✗ Branch 11 not taken.

      406
          return R.transpose() * (v - T);
    
        }
    
        /// @brief inverse transform
    
      ✗
        inline Transform3s& inverseInPlace() {
    
      ✗
          R.transposeInPlace();
    
      ✗
          T = -R * T;
    
      ✗
          return *this;
    
        }
    
        /// @brief inverse transform
    
      164
        inline Transform3s inverse() {
    
        5/10✓ Branch 1 taken 164 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 164 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 164 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 164 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 164 times.
✗ Branch 14 not taken.

      164
          return Transform3s(R.transpose(), -R.transpose() * T);
    
        }
    
        /// @brief inverse the transform and multiply with another
    
      3051
        inline Transform3s inverseTimes(const Transform3s& other) const {
    
        6/12✓ Branch 1 taken 3051 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3051 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3051 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 3051 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 3051 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 3051 times.
✗ Branch 17 not taken.

      3051
          return Transform3s(R.transpose() * other.R, R.transpose() * (other.T - T));
    
        }
    
        /// @brief multiply with another transform
    
      ✗
        inline const Transform3s& operator*=(const Transform3s& other) {
    
      ✗
          T += R * other.T;
    
      ✗
          R *= other.R;
    
      ✗
          return *this;
    
        }
    
        /// @brief multiply with another transform
    
      6866
        inline Transform3s operator*(const Transform3s& other) const {
    
        4/8✓ Branch 1 taken 6866 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6866 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6866 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 6866 times.
✗ Branch 11 not taken.

      6866
          return Transform3s(R * other.R, R * other.T + T);
    
        }
    
        /// @brief check whether the transform is identity
    
      371
        inline bool isIdentity(
    
            const Scalar& prec = Eigen::NumTraits<Scalar>::dummy_precision()) const {
    
        3/4✓ Branch 1 taken 280 times.
✓ Branch 2 taken 91 times.
✓ Branch 4 taken 280 times.
✗ Branch 5 not taken.

      371
          return R.isIdentity(prec) && T.isZero(prec);
    
        }
    
        /// @brief set the transform to be identity transform
    
      323471
        inline void setIdentity() {
    
      323471
          R.setIdentity();
    
      323471
          T.setZero();
    
      323471
        }
    
        /// @brief return a random transform
    
      100
        static Transform3s Random() {
    
      100
          Transform3s tf = Transform3s();
    
      100
          tf.setRandom();
    
      100
          return tf;
    
        }
    
        /// @brief set the transform to a random transform
    
        inline void setRandom();
    
      48
        bool operator==(const Transform3s& other) const {
    
        2/4✓ Branch 2 taken 48 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 48 times.
✗ Branch 7 not taken.

      48
          return (R == other.getRotation()) && (T == other.getTranslation());
    
        }
    
      ✗
        bool operator!=(const Transform3s& other) const { return !(*this == other); }
    
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    
      };
    
      template <typename Derived>
    
      2
      inline Quats fromAxisAngle(const Eigen::MatrixBase<Derived>& axis,
    
                                 Scalar angle) {
    
        2/4✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.

      2
        return Quats(Eigen::AngleAxis<Scalar>(angle, axis));
    
      }
    
      /// @brief Uniformly random quaternion sphere.
    
      /// Code taken from Pinocchio (https://github.com/stack-of-tasks/pinocchio).
    
      100
      inline Quats uniformRandomQuaternion() {
    
        // Rotational part
    
      100
        const Scalar u1 = (Scalar)rand() / Scalar(RAND_MAX);
    
      100
        const Scalar u2 = (Scalar)rand() / Scalar(RAND_MAX);
    
      100
        const Scalar u3 = (Scalar)rand() / Scalar(RAND_MAX);
    
      100
        const Scalar mult1 = std::sqrt(Scalar(1.0) - u1);
    
      100
        const Scalar mult2 = std::sqrt(u1);
    
        static const Scalar PI_value = static_cast<Scalar>(EIGEN_PI);
    
      100
        Scalar s2 = std::sin(2 * PI_value * u2);
    
      100
        Scalar c2 = std::cos(2 * PI_value * u2);
    
      100
        Scalar s3 = std::sin(2 * PI_value * u3);
    
      100
        Scalar c3 = std::cos(2 * PI_value * u3);
    
      100
        Quats q;
    
      100
        q.w() = mult1 * s2;
    
      100
        q.x() = mult1 * c2;
    
      100
        q.y() = mult2 * s3;
    
      100
        q.z() = mult2 * c3;
    
      100
        return q;
    
      }
    
      100
      inline void Transform3s::setRandom() {
    
        1/2✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.

      100
        const Quats q = uniformRandomQuaternion();
    
        1/2✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.

      100
        this->rotation() = q.matrix();
    
        1/2✓ Branch 2 taken 100 times.
✗ Branch 3 not taken.

      100
        this->translation().setRandom();
    
      100
      }
    
      /// @brief Construct othonormal basis from vector.
    
      /// The z-axis is the normalized input vector.
    
      42
      inline Matrix3s constructOrthonormalBasisFromVector(const Vec3s& vec) {
    
        2/4✓ Branch 1 taken 42 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 42 times.
✗ Branch 5 not taken.

      42
        Matrix3s basis = Matrix3s::Zero();
    
        3/6✓ Branch 1 taken 42 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 42 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 42 times.
✗ Branch 8 not taken.

      42
        basis.col(2) = vec.normalized();
    
        4/8✓ Branch 1 taken 42 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 42 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 42 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 42 times.
✗ Branch 11 not taken.

      42
        basis.col(1) = -vec.unitOrthogonal();
    
        4/8✓ Branch 1 taken 42 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 42 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 42 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 42 times.
✗ Branch 11 not taken.

      42
        basis.col(0) = basis.col(1).cross(vec);
    
      42
        return basis;
    
      }
    
      }  // namespace coal
    
      #endif

Line	Branch	Exec	Source
1			/*
2			* Software License Agreement (BSD License)
3			*
4			* Copyright (c) 2011-2014, Willow Garage, Inc.
5			* Copyright (c) 2014-2015, Open Source Robotics Foundation
6			* All rights reserved.
7			*
8			* Redistribution and use in source and binary forms, with or without
9			* modification, are permitted provided that the following conditions
10			* are met:
11			*
12			* * Redistributions of source code must retain the above copyright
13			* notice, this list of conditions and the following disclaimer.
14			* * Redistributions in binary form must reproduce the above
15			* copyright notice, this list of conditions and the following
16			* disclaimer in the documentation and/or other materials provided
17			* with the distribution.
18			* * Neither the name of Open Source Robotics Foundation nor the names of its
19			* contributors may be used to endorse or promote products derived
20			* from this software without specific prior written permission.
21			*
22			* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23			* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24			* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25			* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26			* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27			* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28			* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29			* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30			* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31			* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32			* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33			* POSSIBILITY OF SUCH DAMAGE.
34			*/
35
36			/** \author Jia Pan */
37
38			#ifndef COAL_TRANSFORM_H
39			#define COAL_TRANSFORM_H
40
41			#include "coal/fwd.hh"
42			#include "coal/data_types.h"
43
44			namespace coal {
45
46			COAL_DEPRECATED typedef Eigen::Quaternion<Scalar> Quaternion3f;
47			typedef Eigen::Quaternion<Scalar> Quats;
48
49		✗	static inline std::ostream& operator<<(std::ostream& o, const Quats& q) {
50		✗	o << "(" << q.w() << " " << q.x() << " " << q.y() << " " << q.z() << ")";
51		✗	return o;
52			}
53
54			/// @brief Simple transform class used locally by InterpMotion
55			class COAL_DLLAPI Transform3s {
56			/// @brief Matrix cache
57			Matrix3s R;
58
59			/// @brief Translation vector
60			Vec3s T;
61
62			public:
63			/// @brief Default transform is no movement
64		317557	Transform3s() {
65		317557	setIdentity(); // set matrix_set true
66		317557	}
67
68		1700	static Transform3s Identity() { return Transform3s(); }
69
70			/// @brief Construct transform from rotation and translation
71			template <typename Matrixx3Like, typename Vector3Like>
72		21067	Transform3s(const Eigen::MatrixBase<Matrixx3Like>& R_,
73			const Eigen::MatrixBase<Vector3Like>& T_)
74		21067	: R(R_), T(T_) {}
75
76			/// @brief Construct transform from rotation and translation
77			template <typename Vector3Like>
78		333	Transform3s(const Quats& q_, const Eigen::MatrixBase<Vector3Like>& T_)
79		333	: R(q_.toRotationMatrix()), T(T_) {}
80
81			/// @brief Construct transform from rotation
82	2/4 ✓ Branch 2 taken 102 times. ✗ Branch 3 not taken. ✓ Branch 5 taken 102 times. ✗ Branch 6 not taken.	102	Transform3s(const Matrix3s& R_) : R(R_), T(Vec3s::Zero()) {}
83
84			/// @brief Construct transform from rotation
85	2/4 ✓ Branch 2 taken 2 times. ✗ Branch 3 not taken. ✓ Branch 5 taken 2 times. ✗ Branch 6 not taken.	2	Transform3s(const Quats& q_) : R(q_), T(Vec3s::Zero()) {}
86
87			/// @brief Construct transform from translation
88	2/4 ✓ Branch 1 taken 7075 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 7075 times. ✗ Branch 5 not taken.	7075	Transform3s(const Vec3s& T_) : R(Matrix3s::Identity()), T(T_) {}
89
90			/// @brief Construct transform from other transform
91		59700	Transform3s(const Transform3s& tf) : R(tf.R), T(tf.T) {}
92
93			/// @brief operator =
94		66087	Transform3s& operator=(const Transform3s& tf) {
95		66087	R = tf.R;
96		66087	T = tf.T;
97		66087	return *this;
98			}
99
100			/// @brief get translation
101		2769047	inline const Vec3s& getTranslation() const { return T; }
102
103			/// @brief get translation
104		149	inline const Vec3s& translation() const { return T; }
105
106			/// @brief get translation
107		233	inline Vec3s& translation() { return T; }
108
109			/// @brief get rotation
110		4435679	inline const Matrix3s& getRotation() const { return R; }
111
112			/// @brief get rotation
113		348	inline const Matrix3s& rotation() const { return R; }
114
115			/// @brief get rotation
116		248	inline Matrix3s& rotation() { return R; }
117
118			/// @brief get quaternion
119		1	inline Quats getQuatRotation() const { return Quats(R); }
120
121			/// @brief set transform from rotation and translation
122			template <typename Matrix3Like, typename Vector3Like>
123		62021	inline void setTransform(const Eigen::MatrixBase<Matrix3Like>& R_,
124			const Eigen::MatrixBase<Vector3Like>& T_) {
125	2/4 ✓ Branch 1 taken 62021 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 62021 times. ✗ Branch 5 not taken.	62021	R.noalias() = R_;
126	2/4 ✓ Branch 1 taken 62021 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 62021 times. ✗ Branch 5 not taken.	62021	T.noalias() = T_;
127		62021	}
128
129			/// @brief set transform from rotation and translation
130		✗	inline void setTransform(const Quats& q_, const Vec3s& T_) {
131		✗	R = q_.toRotationMatrix();
132		✗	T = T_;
133		✗	}
134
135			/// @brief set transform from rotation
136			template <typename Derived>
137		6002	inline void setRotation(const Eigen::MatrixBase<Derived>& R_) {
138	2/4 ✓ Branch 1 taken 6002 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 6002 times. ✗ Branch 5 not taken.	6002	R.noalias() = R_;
139		6002	}
140
141			/// @brief set transform from translation
142			template <typename Derived>
143		26225	inline void setTranslation(const Eigen::MatrixBase<Derived>& T_) {
144	2/4 ✓ Branch 1 taken 16199 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 16199 times. ✗ Branch 5 not taken.	26225	T.noalias() = T_;
145		26225	}
146
147			/// @brief set transform from rotation
148	1/2 ✓ Branch 1 taken 891 times. ✗ Branch 2 not taken.	891	inline void setQuatRotation(const Quats& q_) { R = q_.toRotationMatrix(); }
149
150			/// @brief transform a given vector by the transform
151			template <typename Derived>
152		202156102	inline Vec3s transform(const Eigen::MatrixBase<Derived>& v) const {
153	3/6 ✓ Branch 1 taken 177952893 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 177952893 times. ✗ Branch 5 not taken. ✓ Branch 7 taken 177952893 times. ✗ Branch 8 not taken.	402728080	return R * v + T;
154			}
155
156			/// @brief transform a given vector by the inverse of the transform
157			template <typename Derived>
158		203	inline Vec3s inverseTransform(const Eigen::MatrixBase<Derived>& v) const {
159	4/8 ✓ Branch 1 taken 195 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 195 times. ✗ Branch 5 not taken. ✓ Branch 7 taken 195 times. ✗ Branch 8 not taken. ✓ Branch 10 taken 195 times. ✗ Branch 11 not taken.	406	return R.transpose() * (v - T);
160			}
161
162			/// @brief inverse transform
163		✗	inline Transform3s& inverseInPlace() {
164		✗	R.transposeInPlace();
165		✗	T = -R * T;
166		✗	return *this;
167			}
168
169			/// @brief inverse transform
170		164	inline Transform3s inverse() {
171	5/10 ✓ Branch 1 taken 164 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 164 times. ✗ Branch 5 not taken. ✓ Branch 7 taken 164 times. ✗ Branch 8 not taken. ✓ Branch 10 taken 164 times. ✗ Branch 11 not taken. ✓ Branch 13 taken 164 times. ✗ Branch 14 not taken.	164	return Transform3s(R.transpose(), -R.transpose() * T);
172			}
173
174			/// @brief inverse the transform and multiply with another
175		3051	inline Transform3s inverseTimes(const Transform3s& other) const {
176	6/12 ✓ Branch 1 taken 3051 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 3051 times. ✗ Branch 5 not taken. ✓ Branch 7 taken 3051 times. ✗ Branch 8 not taken. ✓ Branch 10 taken 3051 times. ✗ Branch 11 not taken. ✓ Branch 13 taken 3051 times. ✗ Branch 14 not taken. ✓ Branch 16 taken 3051 times. ✗ Branch 17 not taken.	3051	return Transform3s(R.transpose() * other.R, R.transpose() * (other.T - T));
177			}
178
179			/// @brief multiply with another transform
180		✗	inline const Transform3s& operator*=(const Transform3s& other) {
181		✗	T += R * other.T;
182		✗	R *= other.R;
183		✗	return *this;
184			}
185
186			/// @brief multiply with another transform
187		6866	inline Transform3s operator*(const Transform3s& other) const {
188	4/8 ✓ Branch 1 taken 6866 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 6866 times. ✗ Branch 5 not taken. ✓ Branch 7 taken 6866 times. ✗ Branch 8 not taken. ✓ Branch 10 taken 6866 times. ✗ Branch 11 not taken.	6866	return Transform3s(R * other.R, R * other.T + T);
189			}
190
191			/// @brief check whether the transform is identity
192		371	inline bool isIdentity(
193			const Scalar& prec = Eigen::NumTraits<Scalar>::dummy_precision()) const {
194	3/4 ✓ Branch 1 taken 280 times. ✓ Branch 2 taken 91 times. ✓ Branch 4 taken 280 times. ✗ Branch 5 not taken.	371	return R.isIdentity(prec) && T.isZero(prec);
195			}
196
197			/// @brief set the transform to be identity transform
198		323471	inline void setIdentity() {
199		323471	R.setIdentity();
200		323471	T.setZero();
201		323471	}
202
203			/// @brief return a random transform
204		100	static Transform3s Random() {
205		100	Transform3s tf = Transform3s();
206		100	tf.setRandom();
207		100	return tf;
208			}
209
210			/// @brief set the transform to a random transform
211			inline void setRandom();
212
213		48	bool operator==(const Transform3s& other) const {
214	2/4 ✓ Branch 2 taken 48 times. ✗ Branch 3 not taken. ✓ Branch 6 taken 48 times. ✗ Branch 7 not taken.	48	return (R == other.getRotation()) && (T == other.getTranslation());
215			}
216
217		✗	bool operator!=(const Transform3s& other) const { return !(*this == other); }
218
219			EIGEN_MAKE_ALIGNED_OPERATOR_NEW
220			};
221
222			template <typename Derived>
223		2	inline Quats fromAxisAngle(const Eigen::MatrixBase<Derived>& axis,
224			Scalar angle) {
225	2/4 ✓ Branch 1 taken 2 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 2 times. ✗ Branch 5 not taken.	2	return Quats(Eigen::AngleAxis<Scalar>(angle, axis));
226			}
227
228			/// @brief Uniformly random quaternion sphere.
229			/// Code taken from Pinocchio (https://github.com/stack-of-tasks/pinocchio).
230		100	inline Quats uniformRandomQuaternion() {
231			// Rotational part
232		100	const Scalar u1 = (Scalar)rand() / Scalar(RAND_MAX);
233		100	const Scalar u2 = (Scalar)rand() / Scalar(RAND_MAX);
234		100	const Scalar u3 = (Scalar)rand() / Scalar(RAND_MAX);
235
236		100	const Scalar mult1 = std::sqrt(Scalar(1.0) - u1);
237		100	const Scalar mult2 = std::sqrt(u1);
238
239			static const Scalar PI_value = static_cast<Scalar>(EIGEN_PI);
240		100	Scalar s2 = std::sin(2 * PI_value * u2);
241		100	Scalar c2 = std::cos(2 * PI_value * u2);
242		100	Scalar s3 = std::sin(2 * PI_value * u3);
243		100	Scalar c3 = std::cos(2 * PI_value * u3);
244
245		100	Quats q;
246		100	q.w() = mult1 * s2;
247		100	q.x() = mult1 * c2;
248		100	q.y() = mult2 * s3;
249		100	q.z() = mult2 * c3;
250		100	return q;
251			}
252
253		100	inline void Transform3s::setRandom() {
254	1/2 ✓ Branch 1 taken 100 times. ✗ Branch 2 not taken.	100	const Quats q = uniformRandomQuaternion();
255	1/2 ✓ Branch 1 taken 100 times. ✗ Branch 2 not taken.	100	this->rotation() = q.matrix();
256	1/2 ✓ Branch 2 taken 100 times. ✗ Branch 3 not taken.	100	this->translation().setRandom();
257		100	}
258
259			/// @brief Construct othonormal basis from vector.
260			/// The z-axis is the normalized input vector.
261		42	inline Matrix3s constructOrthonormalBasisFromVector(const Vec3s& vec) {
262	2/4 ✓ Branch 1 taken 42 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 42 times. ✗ Branch 5 not taken.	42	Matrix3s basis = Matrix3s::Zero();
263	3/6 ✓ Branch 1 taken 42 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 42 times. ✗ Branch 5 not taken. ✓ Branch 7 taken 42 times. ✗ Branch 8 not taken.	42	basis.col(2) = vec.normalized();
264	4/8 ✓ Branch 1 taken 42 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 42 times. ✗ Branch 5 not taken. ✓ Branch 7 taken 42 times. ✗ Branch 8 not taken. ✓ Branch 10 taken 42 times. ✗ Branch 11 not taken.	42	basis.col(1) = -vec.unitOrthogonal();
265	4/8 ✓ Branch 1 taken 42 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 42 times. ✗ Branch 5 not taken. ✓ Branch 7 taken 42 times. ✗ Branch 8 not taken. ✓ Branch 10 taken 42 times. ✗ Branch 11 not taken.	42	basis.col(0) = basis.col(1).cross(vec);
266		42	return basis;
267			}
268
269			} // namespace coal
270
271			#endif
272