GCC Code Coverage Report

Directory: ./
File: include/coal/internal/tools.h
Date: 2025-06-03 17:15:40
Exec Total Coverage
Lines: 76 98 77.6%
Functions: 2 4 50.0%
Branches: 77 122 63.1%
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 COAL_INTERNAL_TOOLS_H
39 #define COAL_INTERNAL_TOOLS_H
40
41 #include "coal/fwd.hh"
42
43 #include <cmath>
44 #include <iostream>
45 #include <limits>
46
47 #include "coal/data_types.h"
48
49 namespace coal {
50
51 template <typename Derived>
52 88901 static inline typename Derived::Scalar triple(
53 const Eigen::MatrixBase<Derived>& x, const Eigen::MatrixBase<Derived>& y,
54 const Eigen::MatrixBase<Derived>& z) {
55 88901 return x.derived().dot(y.derived().cross(z.derived()));
56 }
57
58 template <typename Derived1, typename Derived2, typename Derived3>
59 ✗ void generateCoordinateSystem(const Eigen::MatrixBase<Derived1>& _w,
60 const Eigen::MatrixBase<Derived2>& _u,
61 const Eigen::MatrixBase<Derived3>& _v) {
62 typedef typename Derived1::Scalar T;
63
64 ✗ Eigen::MatrixBase<Derived1>& w = const_cast<Eigen::MatrixBase<Derived1>&>(_w);
65 ✗ Eigen::MatrixBase<Derived2>& u = const_cast<Eigen::MatrixBase<Derived2>&>(_u);
66 ✗ Eigen::MatrixBase<Derived3>& v = const_cast<Eigen::MatrixBase<Derived3>&>(_v);
67
68 T inv_length;
69 ✗ if (std::abs(w[0]) >= std::abs(w[1])) {
70 ✗ inv_length = (T)1.0 / sqrt(w[0] * w[0] + w[2] * w[2]);
71 ✗ u[0] = -w[2] * inv_length;
72 ✗ u[1] = (T)0;
73 ✗ u[2] = w[0] * inv_length;
74 ✗ v[0] = w[1] * u[2];
75 ✗ v[1] = w[2] * u[0] - w[0] * u[2];
76 ✗ v[2] = -w[1] * u[0];
77 } else {
78 ✗ inv_length = (T)1.0 / sqrt(w[1] * w[1] + w[2] * w[2]);
79 ✗ u[0] = (T)0;
80 ✗ u[1] = w[2] * inv_length;
81 ✗ u[2] = -w[1] * inv_length;
82 ✗ v[0] = w[1] * u[2] - w[2] * u[1];
83 ✗ v[1] = -w[0] * u[2];
84 ✗ v[2] = w[0] * u[1];
85 }
86 ✗ }
87
88 /* ----- Start Matrices ------ */
89 template <typename Derived, typename OtherDerived>
90 7158 void relativeTransform(const Eigen::MatrixBase<Derived>& R1,
91 const Eigen::MatrixBase<OtherDerived>& t1,
92 const Eigen::MatrixBase<Derived>& R2,
93 const Eigen::MatrixBase<OtherDerived>& t2,
94 const Eigen::MatrixBase<Derived>& R,
95 const Eigen::MatrixBase<OtherDerived>& t) {
96
3/6
✓ Branch 1 taken 7158 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7158 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7158 times.
✗ Branch 8 not taken.
 7158 const_cast<Eigen::MatrixBase<Derived>&>(R) = R1.transpose() * R2;
97
4/8
✓ Branch 1 taken 7158 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7158 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7158 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 7158 times.
✗ Branch 11 not taken.
 7158 const_cast<Eigen::MatrixBase<OtherDerived>&>(t) = R1.transpose() * (t2 - t1);
98 7158 }
99
100 /// @brief compute the eigen vector and eigen vector of a matrix. dout is the
101 /// eigen values, vout is the eigen vectors
102 template <typename Derived, typename Vector>
103 1789350 void eigen(const Eigen::MatrixBase<Derived>& m,
104 typename Derived::Scalar dout[3], Vector* vout) {
105 typedef typename Derived::Scalar S;
106
1/2
✓ Branch 2 taken 1789350 times.
✗ Branch 3 not taken.
 1789350 Derived R(m.derived());
107 1789350 int n = 3;
108 int j, iq, ip, i;
109 S tresh, theta, tau, t, sm, s, h, g, c;
110
111 S b[3];
112 S z[3];
113 1789350 S v[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}};
114 S d[3];
115
116
2/2
✓ Branch 0 taken 5368050 times.
✓ Branch 1 taken 1789350 times.
 7157400 for (ip = 0; ip < n; ++ip) {
117
1/2
✓ Branch 1 taken 5368050 times.
✗ Branch 2 not taken.
 5368050 b[ip] = d[ip] = R(ip, ip);
118 5368050 z[ip] = 0;
119 }
120
121
1/2
✓ Branch 0 taken 10490342 times.
✗ Branch 1 not taken.
 10490342 for (i = 0; i < 50; ++i) {
122 10490342 sm = 0;
123
2/2
✓ Branch 0 taken 31471026 times.
✓ Branch 1 taken 10490342 times.
 41961368 for (ip = 0; ip < n; ++ip)
124
3/4
✓ Branch 1 taken 31471026 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 31471026 times.
✓ Branch 5 taken 31471026 times.
 62942052 for (iq = ip + 1; iq < n; ++iq) sm += std::abs(R(ip, iq));
125
2/2
✓ Branch 0 taken 1789350 times.
✓ Branch 1 taken 8700992 times.
 10490342 if (sm == 0.0) {
126
3/6
✓ Branch 1 taken 1789350 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1789350 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1789350 times.
✗ Branch 8 not taken.
 1789350 vout[0] << v[0][0], v[0][1], v[0][2];
127
3/6
✓ Branch 1 taken 1789350 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1789350 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1789350 times.
✗ Branch 8 not taken.
 1789350 vout[1] << v[1][0], v[1][1], v[1][2];
128
3/6
✓ Branch 1 taken 1789350 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1789350 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1789350 times.
✗ Branch 8 not taken.
 1789350 vout[2] << v[2][0], v[2][1], v[2][2];
129 1789350 dout[0] = d[0];
130 1789350 dout[1] = d[1];
131 1789350 dout[2] = d[2];
132 1789350 return;
133 }
134
135
2/2
✓ Branch 0 taken 5199615 times.
✓ Branch 1 taken 3501377 times.
 8700992 if (i < 3)
136 5199615 tresh = Scalar(0.2 * sm / (n * n));
137 else
138 3501377 tresh = 0.0;
139
140
2/2
✓ Branch 0 taken 26102976 times.
✓ Branch 1 taken 8700992 times.
 34803968 for (ip = 0; ip < n; ++ip) {
141
2/2
✓ Branch 0 taken 26102976 times.
✓ Branch 1 taken 26102976 times.
 52205952 for (iq = ip + 1; iq < n; ++iq) {
142
1/2
✓ Branch 1 taken 26102976 times.
✗ Branch 2 not taken.
 26102976 g = Scalar(100) * std::abs(R(ip, iq));
143
8/8
✓ Branch 0 taken 5381508 times.
✓ Branch 1 taken 20721468 times.
✓ Branch 4 taken 5158926 times.
✓ Branch 5 taken 222582 times.
✓ Branch 6 taken 5040765 times.
✓ Branch 7 taken 118161 times.
✓ Branch 8 taken 5040765 times.
✓ Branch 9 taken 21062211 times.
 31261902 if (i > 3 && std::abs(d[ip]) + g == std::abs(d[ip]) &&
144 5158926 std::abs(d[iq]) + g == std::abs(d[iq]))
145
1/2
✓ Branch 1 taken 5040765 times.
✗ Branch 2 not taken.
 5040765 R(ip, iq) = 0.0;
146
3/4
✓ Branch 1 taken 21062211 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 14746163 times.
✓ Branch 5 taken 6316048 times.
 21062211 else if (std::abs(R(ip, iq)) > tresh) {
147 14746163 h = d[iq] - d[ip];
148
2/2
✓ Branch 2 taken 3572590 times.
✓ Branch 3 taken 11173573 times.
 14746163 if (std::abs(h) + g == std::abs(h))
149
1/2
✓ Branch 1 taken 3572590 times.
✗ Branch 2 not taken.
 3572590 t = (R(ip, iq)) / h;
150 else {
151
1/2
✓ Branch 1 taken 11173573 times.
✗ Branch 2 not taken.
 11173573 theta = Scalar(0.5) * h / (R(ip, iq));
152 11173573 t = 1 / (std::abs(theta) + std::sqrt(1 + theta * theta));
153
2/2
✓ Branch 0 taken 5316587 times.
✓ Branch 1 taken 5856986 times.
 11173573 if (theta < 0.0) t = -t;
154 }
155 14746163 c = 1 / std::sqrt(1 + t * t);
156 14746163 s = t * c;
157 14746163 tau = s / (1 + c);
158
1/2
✓ Branch 1 taken 14746163 times.
✗ Branch 2 not taken.
 14746163 h = t * R(ip, iq);
159 14746163 z[ip] -= h;
160 14746163 z[iq] += h;
161 14746163 d[ip] -= h;
162 14746163 d[iq] += h;
163
1/2
✓ Branch 1 taken 14746163 times.
✗ Branch 2 not taken.
 14746163 R(ip, iq) = 0.0;
164
2/2
✓ Branch 0 taken 4686448 times.
✓ Branch 1 taken 14746163 times.
 19432611 for (j = 0; j < ip; ++j) {
165
1/2
✓ Branch 1 taken 4686448 times.
✗ Branch 2 not taken.
 4686448 g = R(j, ip);
166
1/2
✓ Branch 1 taken 4686448 times.
✗ Branch 2 not taken.
 4686448 h = R(j, iq);
167
1/2
✓ Branch 1 taken 4686448 times.
✗ Branch 2 not taken.
 4686448 R(j, ip) = g - s * (h + g * tau);
168
1/2
✓ Branch 1 taken 4686448 times.
✗ Branch 2 not taken.
 4686448 R(j, iq) = h + s * (g - h * tau);
169 }
170
2/2
✓ Branch 0 taken 4780366 times.
✓ Branch 1 taken 14746163 times.
 19526529 for (j = ip + 1; j < iq; ++j) {
171
1/2
✓ Branch 1 taken 4780366 times.
✗ Branch 2 not taken.
 4780366 g = R(ip, j);
172
1/2
✓ Branch 1 taken 4780366 times.
✗ Branch 2 not taken.
 4780366 h = R(j, iq);
173
1/2
✓ Branch 1 taken 4780366 times.
✗ Branch 2 not taken.
 4780366 R(ip, j) = g - s * (h + g * tau);
174
1/2
✓ Branch 1 taken 4780366 times.
✗ Branch 2 not taken.
 4780366 R(j, iq) = h + s * (g - h * tau);
175 }
176
2/2
✓ Branch 0 taken 5279349 times.
✓ Branch 1 taken 14746163 times.
 20025512 for (j = iq + 1; j < n; ++j) {
177
1/2
✓ Branch 1 taken 5279349 times.
✗ Branch 2 not taken.
 5279349 g = R(ip, j);
178
1/2
✓ Branch 1 taken 5279349 times.
✗ Branch 2 not taken.
 5279349 h = R(iq, j);
179
1/2
✓ Branch 1 taken 5279349 times.
✗ Branch 2 not taken.
 5279349 R(ip, j) = g - s * (h + g * tau);
180
1/2
✓ Branch 1 taken 5279349 times.
✗ Branch 2 not taken.
 5279349 R(iq, j) = h + s * (g - h * tau);
181 }
182
2/2
✓ Branch 0 taken 44238489 times.
✓ Branch 1 taken 14746163 times.
 58984652 for (j = 0; j < n; ++j) {
183 44238489 g = v[j][ip];
184 44238489 h = v[j][iq];
185 44238489 v[j][ip] = g - s * (h + g * tau);
186 44238489 v[j][iq] = h + s * (g - h * tau);
187 }
188 }
189 }
190 }
191
2/2
✓ Branch 0 taken 26102976 times.
✓ Branch 1 taken 8700992 times.
 34803968 for (ip = 0; ip < n; ++ip) {
192 26102976 b[ip] += z[ip];
193 26102976 d[ip] = b[ip];
194 26102976 z[ip] = 0.0;
195 }
196 }
197
198 ✗ std::cerr << "eigen: too many iterations in Jacobi transform." << std::endl;
199
200 ✗ return;
201 }
202
203 template <typename Derived, typename OtherDerived>
204 437 bool isEqual(const Eigen::MatrixBase<Derived>& lhs,
205 const Eigen::MatrixBase<OtherDerived>& rhs,
206 const Scalar tol = std::numeric_limits<Scalar>::epsilon() * 100) {
207 437 return ((lhs - rhs).array().abs() < tol).all();
208 }
209
210 } // namespace coal
211
212 #endif
213