hpp-fcl: include/hpp/fcl/internal/tools.h Source File

hpp-fcl 1.7.3 HPP fork of FCL -- The Flexible Collision Library

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