Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	include/hpp/fcl/internal/tools.h	Lines:	78	99	78.8 %
Date:	2024-02-09 12:57:42	Branches:	76	120	63.3 %


/*
 * 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 Joseph Mirabel */

#ifndef HPP_FCL_INTERNAL_TOOLS_H
#define HPP_FCL_INTERNAL_TOOLS_H

#include <hpp/fcl/fwd.hh>

#include <cmath>
#include <iostream>
#include <limits>

#include <hpp/fcl/data_types.h>

namespace hpp {
namespace fcl {

template <typename Derived>
static inline typename Derived::Scalar triple(
    const Eigen::MatrixBase<Derived>& x, const Eigen::MatrixBase<Derived>& y,
    const Eigen::MatrixBase<Derived>& z) {
  return x.derived().dot(y.derived().cross(z.derived()));
}

template <typename Derived1, typename Derived2, typename Derived3>
void generateCoordinateSystem(const Eigen::MatrixBase<Derived1>& _w,
                              const Eigen::MatrixBase<Derived2>& _u,
                              const Eigen::MatrixBase<Derived3>& _v) {
  typedef typename Derived1::Scalar T;

  Eigen::MatrixBase<Derived1>& w = const_cast<Eigen::MatrixBase<Derived1>&>(_w);
  Eigen::MatrixBase<Derived2>& u = const_cast<Eigen::MatrixBase<Derived2>&>(_u);
  Eigen::MatrixBase<Derived3>& v = const_cast<Eigen::MatrixBase<Derived3>&>(_v);

  T inv_length;
  if (std::abs(w[0]) >= std::abs(w[1])) {
    inv_length = (T)1.0 / sqrt(w[0] * w[0] + w[2] * w[2]);
    u[0] = -w[2] * inv_length;
    u[1] = (T)0;
    u[2] = w[0] * inv_length;
    v[0] = w[1] * u[2];
    v[1] = w[2] * u[0] - w[0] * u[2];
    v[2] = -w[1] * u[0];
  } else {
    inv_length = (T)1.0 / sqrt(w[1] * w[1] + w[2] * w[2]);
    u[0] = (T)0;
    u[1] = w[2] * inv_length;
    u[2] = -w[1] * inv_length;
    v[0] = w[1] * u[2] - w[2] * u[1];
    v[1] = -w[0] * u[2];
    v[2] = w[0] * u[1];
  }
}

/* ----- Start Matrices ------ */
template <typename Derived, typename OtherDerived>
void relativeTransform(const Eigen::MatrixBase<Derived>& R1,
                       const Eigen::MatrixBase<OtherDerived>& t1,
                       const Eigen::MatrixBase<Derived>& R2,
                       const Eigen::MatrixBase<OtherDerived>& t2,
                       const Eigen::MatrixBase<Derived>& R,
                       const Eigen::MatrixBase<OtherDerived>& t) {
  const_cast<Eigen::MatrixBase<Derived>&>(R) = R1.transpose() * R2;
  const_cast<Eigen::MatrixBase<OtherDerived>&>(t) = R1.transpose() * (t2 - t1);
}

/// @brief compute the eigen vector and eigen vector of a matrix. dout is the
/// eigen values, vout is the eigen vectors
template <typename Derived, typename Vector>
void eigen(const Eigen::MatrixBase<Derived>& m,
           typename Derived::Scalar dout[3], Vector* vout) {
  typedef typename Derived::Scalar Scalar;
  Derived R(m.derived());
  int n = 3;
  int j, iq, ip, i;
  Scalar tresh, theta, tau, t, sm, s, h, g, c;
  int nrot;
  HPP_FCL_UNUSED_VARIABLE(nrot);
  Scalar b[3];
  Scalar z[3];
  Scalar v[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}};
  Scalar d[3];

  for (ip = 0; ip < n; ++ip) {
    b[ip] = d[ip] = R(ip, ip);
    z[ip] = 0;
  }

  nrot = 0;

  for (i = 0; i < 50; ++i) {
    sm = 0;
    for (ip = 0; ip < n; ++ip)
      for (iq = ip + 1; iq < n; ++iq) sm += std::abs(R(ip, iq));
    if (sm == 0.0) {
      vout[0] << v[0][0], v[0][1], v[0][2];
      vout[1] << v[1][0], v[1][1], v[1][2];
      vout[2] << v[2][0], v[2][1], v[2][2];
      dout[0] = d[0];
      dout[1] = d[1];
      dout[2] = d[2];
      return;
    }

    if (i < 3)
      tresh = 0.2 * sm / (n * n);
    else
      tresh = 0.0;

    for (ip = 0; ip < n; ++ip) {
      for (iq = ip + 1; iq < n; ++iq) {
        g = 100.0 * std::abs(R(ip, iq));
        if (i > 3 && std::abs(d[ip]) + g == std::abs(d[ip]) &&
            std::abs(d[iq]) + g == std::abs(d[iq]))
          R(ip, iq) = 0.0;
        else if (std::abs(R(ip, iq)) > tresh) {
          h = d[iq] - d[ip];
          if (std::abs(h) + g == std::abs(h))
            t = (R(ip, iq)) / h;
          else {
            theta = 0.5 * h / (R(ip, iq));
            t = 1.0 / (std::abs(theta) + std::sqrt(1.0 + theta * theta));
            if (theta < 0.0) t = -t;
          }
          c = 1.0 / std::sqrt(1 + t * t);
          s = t * c;
          tau = s / (1.0 + c);
          h = t * R(ip, iq);
          z[ip] -= h;
          z[iq] += h;
          d[ip] -= h;
          d[iq] += h;
          R(ip, iq) = 0.0;
          for (j = 0; j < ip; ++j) {
            g = R(j, ip);
            h = R(j, iq);
            R(j, ip) = g - s * (h + g * tau);
            R(j, iq) = h + s * (g - h * tau);
          }
          for (j = ip + 1; j < iq; ++j) {
            g = R(ip, j);
            h = R(j, iq);
            R(ip, j) = g - s * (h + g * tau);
            R(j, iq) = h + s * (g - h * tau);
          }
          for (j = iq + 1; j < n; ++j) {
            g = R(ip, j);
            h = R(iq, j);
            R(ip, j) = g - s * (h + g * tau);
            R(iq, j) = h + s * (g - h * tau);
          }
          for (j = 0; j < n; ++j) {
            g = v[j][ip];
            h = v[j][iq];
            v[j][ip] = g - s * (h + g * tau);
            v[j][iq] = h + s * (g - h * tau);
          }
          nrot++;
        }
      }
    }
    for (ip = 0; ip < n; ++ip) {
      b[ip] += z[ip];
      d[ip] = b[ip];
      z[ip] = 0.0;
    }
  }

  std::cerr << "eigen: too many iterations in Jacobi transform." << std::endl;

  return;
}

template <typename Derived, typename OtherDerived>
bool isEqual(const Eigen::MatrixBase<Derived>& lhs,
             const Eigen::MatrixBase<OtherDerived>& rhs,
             const FCL_REAL tol = std::numeric_limits<FCL_REAL>::epsilon() *
                                  100) {
  return ((lhs - rhs).array().abs() < tol).all();
}

}  // namespace fcl
}  // namespace hpp

#endif


Generated by: GCOVR (Version 4.2)

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 Joseph Mirabel */
37
38			#ifndef HPP_FCL_INTERNAL_TOOLS_H
39			#define HPP_FCL_INTERNAL_TOOLS_H
40
41			#include <hpp/fcl/fwd.hh>
42
43			#include <cmath>
44			#include <iostream>
45			#include <limits>
46
47			#include <hpp/fcl/data_types.h>
48
49			namespace hpp {
50			namespace fcl {
51
52			template <typename Derived>
53		84698	static inline typename Derived::Scalar triple(
54			const Eigen::MatrixBase<Derived>& x, const Eigen::MatrixBase<Derived>& y,
55			const Eigen::MatrixBase<Derived>& z) {
56	✓✗	84698	return x.derived().dot(y.derived().cross(z.derived()));
57			}
58
59			template <typename Derived1, typename Derived2, typename Derived3>
60			void generateCoordinateSystem(const Eigen::MatrixBase<Derived1>& _w,
61			const Eigen::MatrixBase<Derived2>& _u,
62			const Eigen::MatrixBase<Derived3>& _v) {
63			typedef typename Derived1::Scalar T;
64
65			Eigen::MatrixBase<Derived1>& w = const_cast<Eigen::MatrixBase<Derived1>&>(_w);
66			Eigen::MatrixBase<Derived2>& u = const_cast<Eigen::MatrixBase<Derived2>&>(_u);
67			Eigen::MatrixBase<Derived3>& v = const_cast<Eigen::MatrixBase<Derived3>&>(_v);
68
69			T inv_length;
70			if (std::abs(w[0]) >= std::abs(w[1])) {
71			inv_length = (T)1.0 / sqrt(w[0] * w[0] + w[2] * w[2]);
72			u[0] = -w[2] * inv_length;
73			u[1] = (T)0;
74			u[2] = w[0] * inv_length;
75			v[0] = w[1] * u[2];
76			v[1] = w[2] * u[0] - w[0] * u[2];
77			v[2] = -w[1] * u[0];
78			} else {
79			inv_length = (T)1.0 / sqrt(w[1] * w[1] + w[2] * w[2]);
80			u[0] = (T)0;
81			u[1] = w[2] * inv_length;
82			u[2] = -w[1] * inv_length;
83			v[0] = w[1] * u[2] - w[2] * u[1];
84			v[1] = -w[0] * u[2];
85			v[2] = w[0] * u[1];
86			}
87			}
88
89			/* ----- Start Matrices ------ */
90			template <typename Derived, typename OtherDerived>
91		6449	void relativeTransform(const Eigen::MatrixBase<Derived>& R1,
92			const Eigen::MatrixBase<OtherDerived>& t1,
93			const Eigen::MatrixBase<Derived>& R2,
94			const Eigen::MatrixBase<OtherDerived>& t2,
95			const Eigen::MatrixBase<Derived>& R,
96			const Eigen::MatrixBase<OtherDerived>& t) {
97	✓✗✓✗	6449	const_cast<Eigen::MatrixBase<Derived>&>(R) = R1.transpose() * R2;
98	✓✗✓✗ ✓✗	6449	const_cast<Eigen::MatrixBase<OtherDerived>&>(t) = R1.transpose() * (t2 - t1);
99		6449	}
100
101			/// @brief compute the eigen vector and eigen vector of a matrix. dout is the
102			/// eigen values, vout is the eigen vectors
103			template <typename Derived, typename Vector>
104		1777280	void eigen(const Eigen::MatrixBase<Derived>& m,
105			typename Derived::Scalar dout[3], Vector* vout) {
106			typedef typename Derived::Scalar Scalar;
107	✓✗	1777280	Derived R(m.derived());
108		1777280	int n = 3;
109			int j, iq, ip, i;
110			Scalar tresh, theta, tau, t, sm, s, h, g, c;
111			int nrot;
112			HPP_FCL_UNUSED_VARIABLE(nrot);
113			Scalar b[3];
114			Scalar z[3];
115		1777280	Scalar v[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}};
116			Scalar d[3];
117
118	✓✓	7109120	for (ip = 0; ip < n; ++ip) {
119	✓✗	5331840	b[ip] = d[ip] = R(ip, ip);
120		5331840	z[ip] = 0;
121			}
122
123		1777280	nrot = 0;
124
125	✓✗	10417613	for (i = 0; i < 50; ++i) {
126		10417613	sm = 0;
127	✓✓	41670452	for (ip = 0; ip < n; ++ip)
128	✓✓✓✗	62505678	for (iq = ip + 1; iq < n; ++iq) sm += std::abs(R(ip, iq));
129	✓✓	10417613	if (sm == 0.0) {
130	✓✗✓✗ ✓✗	1777280	vout[0] << v[0][0], v[0][1], v[0][2];
131	✓✗✓✗ ✓✗	1777280	vout[1] << v[1][0], v[1][1], v[1][2];
132	✓✗✓✗ ✓✗	1777280	vout[2] << v[2][0], v[2][1], v[2][2];
133		1777280	dout[0] = d[0];
134		1777280	dout[1] = d[1];
135		1777280	dout[2] = d[2];
136		1777280	return;
137			}
138
139	✓✓	8640333	if (i < 3)
140		5164049	tresh = 0.2 * sm / (n * n);
141			else
142		3476284	tresh = 0.0;
143
144	✓✓	34561332	for (ip = 0; ip < n; ++ip) {
145	✓✓	51841998	for (iq = ip + 1; iq < n; ++iq) {
146	✓✗	25920999	g = 100.0 * std::abs(R(ip, iq));
147	✓✓✓✓ ✓✓✓✓	31043591	if (i > 3 && std::abs(d[ip]) + g == std::abs(d[ip]) &&
148		5122592	std::abs(d[iq]) + g == std::abs(d[iq]))
149	✓✗	5006633	R(ip, iq) = 0.0;
150	✓✗✓✓	20914366	else if (std::abs(R(ip, iq)) > tresh) {
151		14638069	h = d[iq] - d[ip];
152	✓✓	14638069	if (std::abs(h) + g == std::abs(h))
153	✓✗	3556933	t = (R(ip, iq)) / h;
154			else {
155	✓✗	11081136	theta = 0.5 * h / (R(ip, iq));
156		11081136	t = 1.0 / (std::abs(theta) + std::sqrt(1.0 + theta * theta));
157	✓✓	11081136	if (theta < 0.0) t = -t;
158			}
159		14638069	c = 1.0 / std::sqrt(1 + t * t);
160		14638069	s = t * c;
161		14638069	tau = s / (1.0 + c);
162	✓✗	14638069	h = t * R(ip, iq);
163		14638069	z[ip] -= h;
164		14638069	z[iq] += h;
165		14638069	d[ip] -= h;
166		14638069	d[iq] += h;
167	✓✗	14638069	R(ip, iq) = 0.0;
168	✓✓	19290676	for (j = 0; j < ip; ++j) {
169	✓✗	4652607	g = R(j, ip);
170	✓✗	4652607	h = R(j, iq);
171	✓✗	4652607	R(j, ip) = g - s * (h + g * tau);
172	✓✗	4652607	R(j, iq) = h + s * (g - h * tau);
173			}
174	✓✓	19382204	for (j = ip + 1; j < iq; ++j) {
175	✓✗	4744135	g = R(ip, j);
176	✓✗	4744135	h = R(j, iq);
177	✓✗	4744135	R(ip, j) = g - s * (h + g * tau);
178	✓✗	4744135	R(j, iq) = h + s * (g - h * tau);
179			}
180	✓✓	19879396	for (j = iq + 1; j < n; ++j) {
181	✓✗	5241327	g = R(ip, j);
182	✓✗	5241327	h = R(iq, j);
183	✓✗	5241327	R(ip, j) = g - s * (h + g * tau);
184	✓✗	5241327	R(iq, j) = h + s * (g - h * tau);
185			}
186	✓✓	58552276	for (j = 0; j < n; ++j) {
187		43914207	g = v[j][ip];
188		43914207	h = v[j][iq];
189		43914207	v[j][ip] = g - s * (h + g * tau);
190		43914207	v[j][iq] = h + s * (g - h * tau);
191			}
192		14638069	nrot++;
193			}
194			}
195			}
196	✓✓	34561332	for (ip = 0; ip < n; ++ip) {
197		25920999	b[ip] += z[ip];
198		25920999	d[ip] = b[ip];
199		25920999	z[ip] = 0.0;
200			}
201			}
202
203			std::cerr << "eigen: too many iterations in Jacobi transform." << std::endl;
204
205			return;
206			}
207
208			template <typename Derived, typename OtherDerived>
209		367	bool isEqual(const Eigen::MatrixBase<Derived>& lhs,
210			const Eigen::MatrixBase<OtherDerived>& rhs,
211			const FCL_REAL tol = std::numeric_limits<FCL_REAL>::epsilon() *
212			100) {
213		367	return ((lhs - rhs).array().abs() < tol).all();
214			}
215
216			} // namespace fcl
217			} // namespace hpp
218
219			#endif