test_fcl_math.cpp
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-2016, Open Source Robotics Foundation
6  * Copyright (c) 2016, Toyota Research Institute
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of Open Source Robotics Foundation nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  */
36 
37 #include <gtest/gtest.h>
38 
39 #include "fcl/broadphase/detail/morton.h"
40 #include "fcl/config.h"
41 #include "fcl/math/bv/AABB.h"
42 #include "fcl/math/bv/RSS.h"
43 #include "fcl/math/bv/utility.h"
44 #include "fcl/math/constants.h"
45 
46 using namespace fcl;
47 
48 template <typename S>
49 void test_vec_test_basic_vector()
50 {
51  Vector3<S> v1(1.0, 2.0, 3.0);
52  EXPECT_TRUE(v1[0] == (S)1.0);
53  EXPECT_TRUE(v1[1] == (S)2.0);
54  EXPECT_TRUE(v1[2] == (S)3.0);
55 
56  Vector3<S> v2 = v1;
57  Vector3<S> v3(3.3, 4.3, 5.3);
58  v1 += v3;
59  EXPECT_TRUE(v1.isApprox(v2 + v3));
60  v1 -= v3;
61  EXPECT_TRUE(v1.isApprox(v2));
62  v1 -= v3;
63  EXPECT_TRUE(v1.isApprox(v2 - v3));
64  v1 += v3;
65 
66  v1.array() *= v3.array();
67  EXPECT_TRUE(v1.array().isApprox(v2.array() * v3.array()));
68  v1.array() /= v3.array();
69  EXPECT_TRUE(v1.isApprox(v2));
70  v1.array() /= v3.array();
71  EXPECT_TRUE(v1.array().isApprox(v2.array() / v3.array()));
72  v1.array() *= v3.array();
73 
74  v1 *= 2.0;
75  EXPECT_TRUE(v1.isApprox(v2 * 2.0));
76  v1 /= 2.0;
77  EXPECT_TRUE(v1.isApprox(v2));
78  v1 /= 2.0;
79  EXPECT_TRUE(v1.isApprox(v2 / 2.0));
80  v1 *= 2.0;
81 
82  v1.array() += 2.0;
83  EXPECT_TRUE(v1.array().isApprox(v2.array() + 2.0));
84  v1.array() -= 2.0;
85  EXPECT_TRUE(v1.isApprox(v2));
86  v1.array() -= 2.0;
87  EXPECT_TRUE(v1.array().isApprox(v2.array() - 2.0));
88  v1.array() += 2.0;
89 
90  EXPECT_TRUE((-Vector3<S>(1.0, 2.0, 3.0)) == (Vector3<S>(-1.0, -2.0, -3.0)));
91 
92  v1 = Vector3<S>(1.0, 2.0, 3.0);
93  v2 = Vector3<S>(3.0, 4.0, 5.0);
94  EXPECT_TRUE((v1.cross(v2)).isApprox(Vector3<S>(-2.0, 4.0, -2.0)));
95  EXPECT_TRUE(std::abs(v1.dot(v2) - 26) < 1e-5);
96 
97  v1 = Vector3<S>(3.0, 4.0, 5.0);
98  EXPECT_TRUE(std::abs(v1.squaredNorm() - 50.0) < 1e-5);
99  EXPECT_TRUE(std::abs(v1.norm() - sqrt(50.0)) < 1e-5);
100  EXPECT_TRUE(v1.normalized().isApprox(v1 / v1.norm()));
101 
102  v1 = Vector3<S>(1.0, 2.0, 3.0);
103  v2 = Vector3<S>(3.0, 4.0, 5.0);
104  EXPECT_TRUE((v1.cross(v2)).isApprox(Vector3<S>(-2.0, 4.0, -2.0)));
105  EXPECT_TRUE(v1.dot(v2) == 26);
106 }
107 
108 GTEST_TEST(FCL_MATH, vec_test_basic_vector3)
109 {
110 // test_vec_test_basic_vector<float>();
111  test_vec_test_basic_vector<double>();
112 }
113 
114 template <typename S>
115 void test_morton()
116 {
117  AABB<S> bbox(Vector3<S>(0, 0, 0), Vector3<S>(1000, 1000, 1000));
118  detail::morton_functor<S, std::bitset<30>> F1(bbox);
119  detail::morton_functor<S, std::bitset<60>> F2(bbox);
120  detail::morton_functor<S, uint64> F3(bbox); // 60 bits
121  detail::morton_functor<S, uint32> F4(bbox); // 30 bits
122 
123  Vector3<S> p(254, 873, 674);
124 
125  EXPECT_TRUE(F1(p).to_ulong() == F4(p));
126  EXPECT_TRUE(F2(p).to_ullong() == F3(p));
127 }
128 
129 GTEST_TEST(FCL_MATH, morton)
130 {
131 // test_morton<float>();
132  test_morton<double>();
133 }
134 
135 // Test fitting an RSS to various number of points
136 template <typename S>
137 void test_rss_fit()
138 {
139  const S epsilon = constants<S>::eps_78();
140  RSS<S> rss;
141  Vector3<S> pn[8];
142  pn[0] << 0, 0, 0;
143  pn[1] << 1, 0, 0;
144  // Check fitting 1 point
145  fit(pn, 1, rss);
146  EXPECT_EQ(rss.center(), pn[0]);
147  EXPECT_EQ(rss.width(), 0.0);
148  EXPECT_EQ(rss.height(), 0.0);
149  EXPECT_EQ(rss.depth(), 0.0);
150  EXPECT_TRUE(rss.axis.isApprox(Matrix3<S>::Identity()));
151  // Check fitting 2 points for each axis
152  fit(pn, 2, rss);
153  EXPECT_TRUE(rss.center().isApprox(Vector3<S>(0.5, 0, 0)));
154  EXPECT_EQ(rss.width(), 1.0);
155  EXPECT_EQ(rss.height(), 0.0);
156  EXPECT_EQ(rss.depth(), 0.0);
157  EXPECT_TRUE(rss.axis.col(0).isApprox(Vector3<S>(1, 0, 0)) ||
158  rss.axis.col(0).isApprox(Vector3<S>(-1, 0, 0)));
159  pn[1] << 0, 1, 0;
160  fit(pn, 2, rss);
161  EXPECT_TRUE(rss.center().isApprox(Vector3<S>(0, 0.5, 0)));
162  EXPECT_EQ(rss.width(), 1.0);
163  EXPECT_EQ(rss.height(), 0.0);
164  EXPECT_EQ(rss.depth(), 0.0);
165  EXPECT_TRUE(rss.axis.col(0).isApprox(Vector3<S>(0, 1, 0)) ||
166  rss.axis.col(0).isApprox(Vector3<S>(0, -1, 0)));
167  pn[1] << 0, 0, 1;
168  fit(pn, 2, rss);
169  EXPECT_TRUE(rss.center().isApprox(Vector3<S>(0, 0, 0.5)));
170  EXPECT_EQ(rss.width(), 1.0);
171  EXPECT_EQ(rss.height(), 0.0);
172  EXPECT_EQ(rss.depth(), 0.0);
173  EXPECT_TRUE(rss.axis.col(0).isApprox(Vector3<S>(0, 0, 1)) ||
174  rss.axis.col(0).isApprox(Vector3<S>(0, 0, -1)));
175  // Check fitting 2 in opposite order
176  pn[0] << 0, 0, 1;
177  pn[1] << 0, 0, 0;
178  fit(pn, 2, rss);
179  EXPECT_TRUE(rss.center().isApprox(Vector3<S>(0, 0, 0.5)));
180  EXPECT_EQ(rss.width(), 1.0);
181  EXPECT_EQ(rss.height(), 0.0);
182  EXPECT_EQ(rss.depth(), 0.0);
183  EXPECT_TRUE(rss.axis.col(0).isApprox(Vector3<S>(0, 0, 1)) ||
184  rss.axis.col(0).isApprox(Vector3<S>(0, 0, -1)));
185  // Check fitting 2 not along an axis
186  pn[0] << -1, 1, 0;
187  pn[1] << 0, 0, 0;
188  fit(pn, 2, rss);
189  EXPECT_TRUE(rss.center().isApprox(Vector3<S>(-0.5, 0.5, 0)));
190  EXPECT_NEAR(rss.width(), sqrt(2.0), epsilon);
191  EXPECT_EQ(rss.height(), 0.0);
192  EXPECT_EQ(rss.depth(), 0.0);
193  EXPECT_TRUE(
194  rss.axis.col(0).isApprox(Vector3<S>(-sqrt(2.0)/2.0, sqrt(2.0)/2.0, 0)) ||
195  rss.axis.col(0).isApprox(Vector3<S>(sqrt(2.0)/2.0, -sqrt(2.0)/2.0, 0)));
196  // Check fitting 3
197  pn[0] << 0, 0, 0;
198  pn[1] << 1, 0, 0;
199  pn[2] << 0, 1, 0;
200  fit(pn, 3, rss);
201  Vector3<S> c3(0.25, 0.25, 0);
202  EXPECT_TRUE(c3.isApprox(rss.center()));
203  EXPECT_NEAR(rss.width(), sqrt(2.0), epsilon);
204  EXPECT_NEAR(rss.height(), sqrt(2.0) / 2.0, epsilon);
205  EXPECT_EQ(rss.depth(), 0.0);
206  // Check fitting 8
207  pn[3] << 0, 0, 1;
208  pn[4] << 1, 0, 0;
209  pn[5] << 1, 0, 1;
210  pn[6] << 1, 1, 0;
211  pn[7] << 1, 1, 1;
212  fit(pn, 8, rss);
213  EXPECT_GE(rss.depth(), 0.5);
214 }
215 
216 // Test convert RSS to other bounding volumes
217 template <typename S>
218 void test_rss_conversion()
219 {
220  const S epsilon = constants<S>::eps_78();
221  RSS<S> rss;
222  Vector3<S> pn[8];
223  AABB<S> aabb;
224  OBB<S> obb;
225 
226  pn[0] << 0, 0, 0;
227  pn[1] << 1, 0, 0;
228  pn[2] << 0, 1, 0;
229  pn[3] << 0, 0, 1;
230  pn[4] << 1, 0, 0;
231  pn[5] << 1, 0, 1;
232  pn[6] << 1, 1, 0;
233  pn[7] << 1, 1, 1;
234  fit(pn, 8, rss);
235 
236  convertBV(rss, Transform3<S>::Identity(), aabb);
237  EXPECT_GE(aabb.width(), rss.width());
238  EXPECT_GE(aabb.height(), rss.height());
239  EXPECT_GE(aabb.depth(), rss.depth());
240  EXPECT_TRUE(aabb.center().isApprox(rss.center()));
241  convertBV(aabb, Transform3<S>::Identity(), rss);
242  // The resulting RSS must be bigger than the AABB for it to contain it
243  EXPECT_GE(rss.width() - rss.depth() + epsilon, aabb.width());
244  EXPECT_GE(rss.height() - rss.depth() + epsilon, aabb.height());
245  EXPECT_GE(rss.depth(), aabb.depth());
246  EXPECT_TRUE(aabb.center().isApprox(rss.center()));
247  convertBV(rss, Transform3<S>::Identity(), obb);
248  EXPECT_GE(obb.width(), rss.width());
249  EXPECT_GE(obb.height(), rss.height());
250  EXPECT_GE(obb.depth(), rss.depth());
251  EXPECT_TRUE(obb.center().isApprox(rss.center()));
252 }
253 
254 // Test RSS to RSS distance function
255 template <typename S>
256 void test_rss_distance()
257 {
258  const S epsilon = constants<S>::eps_78();
259  Vector3<S> p0[3];
260  Vector3<S> p1[3];
261  RSS<S> rss;
262  RSS<S> rss2;
263  // Test RSS to RSS distance for correctness
264  p0[0] << 1, 1, 1;
265  p0[1] << 1, 1, -1;
266  p0[2] << 0, 1, -1;
267  p1[0] << 1, -1, 1;
268  p1[1] << 1, -1, -1;
269  p1[2] << 0, -1, -1;
270  fit(p0, 3, rss);
271  fit(p1, 3, rss2);
272  EXPECT_NEAR(rss.distance(rss2), 2.0, epsilon);
273  rss.To << 0, 0, 0.5;
274  rss.axis = Matrix3<S>::Identity();
275  rss.l[0] = 1;
276  rss.l[1] = 1;
277  rss.r = 0.5;
278  rss2.To << -1, -1, 2.5;
279  rss2.axis = Matrix3<S>::Identity();
280  rss2.l[0] = 1;
281  rss2.l[1] = 1;
282  rss2.r = 0.5;
283  EXPECT_NEAR(rss.distance(rss2), 1.0, epsilon);
284  rss2.axis << -1, 0, 0,
285  0, -1, 0,
286  0, 0, -1;
287  EXPECT_NEAR(rss.distance(rss2), sqrt(6) - 1.0, epsilon);
288  rss2.To << 0, 0, 2.5;
289  EXPECT_NEAR(rss.distance(rss2), 1.0, epsilon);
290 }
291 
292 // Test setting RSS position
293 template <typename S>
294 void test_rss_position()
295 {
296  // Verify setting To via the center works correctly.
297  RSS<S> rss;
298  rss.l[0] = 1;
299  rss.l[1] = 1;
300  rss.r = 0.5;
301  rss.axis = Matrix3<S>::Identity();
302  rss.setToFromCenter(Vector3<S>(0.5, 0.5, 0.5));
303  EXPECT_TRUE(rss.To.isApprox(Vector3<S>(0.0, 0.0, 0.5)));
304  rss.axis *= -1.0;
305  rss.setToFromCenter(Vector3<S>(-0.5, -0.5, 2.5));
306  EXPECT_TRUE(rss.To.isApprox(Vector3<S>(0.0, 0.0, 2.5)));
307 }
308 
309 GTEST_TEST(FCL_MATH, rss_fit)
310 {
311  test_rss_fit<double>();
312 }
313 
314 GTEST_TEST(FCL_MATH, rss_conversion)
315 {
316  test_rss_conversion<double>();
317 }
318 
319 GTEST_TEST(FCL_MATH, rss_distance)
320 {
321  test_rss_distance<double>();
322 }
323 
324 GTEST_TEST(FCL_MATH, rss_position)
325 {
326  test_rss_position<double>();
327 }
328 
329 // TODO test overlap
330 // TODO test contain
331 // TODO test operator+
332 // TODO test size
333 
334 //==============================================================================
335 int main(int argc, char* argv[])
336 {
337  ::testing::InitGoogleTest(&argc, argv);
338  return RUN_ALL_TESTS();
339 }
fcl::AABB::width
S width() const
Width of the AABB.
Definition: AABB-inl.h:188
fcl::convertBV
FCL_EXPORT void convertBV(const BV1 &bv1, const Transform3< typename BV1::S > &tf1, BV2 &bv2)
Convert a bounding volume of type BV1 in configuration tf1 to bounding volume of type BV2 in identity...
Definition: math/bv/utility-inl.h:973
fcl::OBB::depth
S depth() const
Depth of the OBB.
Definition: OBB-inl.h:192
fcl
Main namespace.
Definition: broadphase_bruteforce-inl.h:45
fcl::constants::eps_78
static Real eps_78()
Returns ε^(7/8) for the precision of the underlying scalar type.
Definition: constants.h:164
fcl::RSS::axis
Matrix3< S > axis
Frame T&#39;s orientation in frame F.
Definition: RSS.h:70
fcl::RSS::To
Vector3< S > To
Origin of frame T in frame F.
Definition: RSS.h:76
fcl::Transform3
Eigen::Transform< S, 3, Eigen::Isometry > Transform3
Definition: types.h:91
fcl::OBB::center
const Vector3< S > center() const
Center of the OBB.
Definition: OBB-inl.h:213
epsilon
S epsilon()
Definition: test_fcl_simple.cpp:56
fcl::OBB::width
S width() const
Width of the OBB.
Definition: OBB-inl.h:178
fcl::Matrix3
Eigen::Matrix< S, 3, 3 > Matrix3
Definition: types.h:85
EXPECT_NEAR
#define EXPECT_NEAR(a, b, prec)
fcl::AABB::center
Vector3< S > center() const
Center of the AABB.
Definition: AABB-inl.h:230
fcl::Vector3
Eigen::Matrix< S, 3, 1 > Vector3
Definition: types.h:70
aabb
SaPCollisionManager< S >::SaPAABB * aabb
back pointer to SAP interval
Definition: broadphase_SaP.h:184
GTEST_TEST
GTEST_TEST(FCL_MATH, vec_test_basic_vector3)
Definition: test_fcl_math.cpp:108
fcl::RSS::setToFromCenter
void setToFromCenter(const Vector3< S > &p_FoCenter_F)
Set To of the RSS from the center coordinates in frame F.
Definition: RSS-inl.h:424
fcl::OBB::height
S height() const
Height of the OBB.
Definition: OBB-inl.h:185
EXPECT_EQ
#define EXPECT_EQ(a, b)
test_morton
void test_morton()
Definition: test_fcl_math.cpp:115
test_rss_position
void test_rss_position()
Definition: test_fcl_math.cpp:294
fcl::RSS::l
S l[2]
Side lengths of rectangle in frame T.
Definition: RSS.h:79
fcl::AABB::depth
S depth() const
Depth of the AABB.
Definition: AABB-inl.h:202
main
int main(int argc, char *argv[])
Definition: test_fcl_math.cpp:335
fcl::RSS::depth
S depth() const
Depth of the RSS.
Definition: RSS-inl.h:395
test_vec_test_basic_vector
void test_vec_test_basic_vector()
Definition: test_fcl_math.cpp:49
fcl::RSS::width
S width() const
Width of the RSS.
Definition: RSS-inl.h:381
test_rss_distance
void test_rss_distance()
Definition: test_fcl_math.cpp:256
test_rss_fit
void test_rss_fit()
Definition: test_fcl_math.cpp:137
fcl::RSS::r
S r
Radius of sphere summed with rectangle to form RSS.
Definition: RSS.h:82
fcl::AABB::height
S height() const
Height of the AABB.
Definition: AABB-inl.h:195
fcl::RSS::center
const Vector3< S > center() const
The RSS center C, measured and expressed in frame F: p_FoC_F.
Definition: RSS-inl.h:416
fcl::fit
FCL_EXPORT void fit(const Vector3< typename BV::S > *const ps, int n, BV &bv)
Compute a bounding volume that fits a set of n points.
Definition: math/bv/utility-inl.h:671
EXPECT_TRUE
#define EXPECT_TRUE(args)
fcl::RSS::height
S height() const
Height of the RSS.
Definition: RSS-inl.h:388
test_rss_conversion
void test_rss_conversion()
Definition: test_fcl_math.cpp:218
fcl::RSS::distance
S distance(const RSS< S > &other, Vector3< S > *P=nullptr, Vector3< S > *Q=nullptr) const
the distance between two RSS; P and Q, if not nullptr, return the nearest points
Definition: RSS-inl.h:433

fcl_catkin
Author(s):
autogenerated on Thu Mar 23 2023 03:00:19