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