box_box_distance.cpp
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2014-2016, CNRS-LAAS.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage, Inc. nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  */
34 
37 #define _USE_MATH_DEFINES
38 #include <cmath>
39 
40 #define BOOST_TEST_MODULE FCL_BOX_BOX
41 #include <boost/test/included/unit_test.hpp>
42 
43 #define CHECK_CLOSE_TO_0(x, eps) BOOST_CHECK_CLOSE((x + 1.0), (1.0), (eps))
44 
45 #include <cmath>
46 #include <iostream>
47 #include <hpp/fcl/distance.h>
48 #include <hpp/fcl/math/transform.h>
49 #include <hpp/fcl/collision.h>
50 #include <hpp/fcl/collision_object.h>
51 #include <hpp/fcl/shape/geometric_shapes.h>
52 
53 #include "utility.h"
54 
55 using hpp::fcl::CollisionGeometryPtr_t;
56 using hpp::fcl::CollisionObject;
57 using hpp::fcl::DistanceRequest;
58 using hpp::fcl::DistanceResult;
59 using hpp::fcl::Transform3f;
60 using hpp::fcl::Vec3f;
61 
62 BOOST_AUTO_TEST_CASE(distance_box_box_1) {
63  CollisionGeometryPtr_t s1(new hpp::fcl::Box(6, 10, 2));
64  CollisionGeometryPtr_t s2(new hpp::fcl::Box(2, 2, 2));
65 
66  Transform3f tf1;
67  Transform3f tf2(Vec3f(25, 20, 5));
68 
69  CollisionObject o1(s1, tf1);
70  CollisionObject o2(s2, tf2);
71 
72  // Enable computation of nearest points
73  DistanceRequest distanceRequest(true, 0, 0);
74  DistanceResult distanceResult;
75 
76  hpp::fcl::distance(&o1, &o2, distanceRequest, distanceResult);
77 
78  std::cerr << "Applied transformations on two boxes" << std::endl;
79  std::cerr << " T1 = " << tf1.getTranslation() << std::endl
80  << " R1 = " << tf1.getRotation() << std::endl
81  << " T2 = " << tf2.getTranslation() << std::endl
82  << " R2 = " << tf2.getRotation() << std::endl;
83  std::cerr << "Closest points: p1 = " << distanceResult.nearest_points[0]
84  << ", p2 = " << distanceResult.nearest_points[1]
85  << ", distance = " << distanceResult.min_distance << std::endl;
86  double dx = 25 - 3 - 1;
87  double dy = 20 - 5 - 1;
88  double dz = 5 - 1 - 1;
89 
90  const Vec3f& p1 = distanceResult.nearest_points[0];
91  const Vec3f& p2 = distanceResult.nearest_points[1];
92  BOOST_CHECK_CLOSE(distanceResult.min_distance,
93  sqrt(dx * dx + dy * dy + dz * dz), 1e-4);
94 
95  BOOST_CHECK_CLOSE(p1[0], 3, 1e-6);
96  BOOST_CHECK_CLOSE(p1[1], 5, 1e-6);
97  BOOST_CHECK_CLOSE(p1[2], 1, 1e-6);
98  BOOST_CHECK_CLOSE(p2[0], 24, 1e-6);
99  BOOST_CHECK_CLOSE(p2[1], 19, 1e-6);
100  BOOST_CHECK_CLOSE(p2[2], 4, 1e-6);
101 }
102 
103 BOOST_AUTO_TEST_CASE(distance_box_box_2) {
104  CollisionGeometryPtr_t s1(new hpp::fcl::Box(6, 10, 2));
105  CollisionGeometryPtr_t s2(new hpp::fcl::Box(2, 2, 2));
106  static double pi = M_PI;
107  Transform3f tf1;
108  Transform3f tf2(
109  hpp::fcl::makeQuat(cos(pi / 8), sin(pi / 8) / sqrt(3),
110  sin(pi / 8) / sqrt(3), sin(pi / 8) / sqrt(3)),
111  Vec3f(0, 0, 10));
112 
113  CollisionObject o1(s1, tf1);
114  CollisionObject o2(s2, tf2);
115 
116  // Enable computation of nearest points
117  DistanceRequest distanceRequest(true, 0, 0);
118  DistanceResult distanceResult;
119 
120  hpp::fcl::distance(&o1, &o2, distanceRequest, distanceResult);
121 
122  std::cerr << "Applied transformations on two boxes" << std::endl;
123  std::cerr << " T1 = " << tf1.getTranslation() << std::endl
124  << " R1 = " << tf1.getRotation() << std::endl
125  << " T2 = " << tf2.getTranslation() << std::endl
126  << " R2 = " << tf2.getRotation() << std::endl;
127  std::cerr << "Closest points: p1 = " << distanceResult.nearest_points[0]
128  << ", p2 = " << distanceResult.nearest_points[1]
129  << ", distance = " << distanceResult.min_distance << std::endl;
130 
131  const Vec3f& p1 = distanceResult.nearest_points[0];
132  const Vec3f& p2 = distanceResult.nearest_points[1];
133  double distance = -1.62123444 + 10 - 1;
134  BOOST_CHECK_CLOSE(distanceResult.min_distance, distance, 1e-4);
135 
136  BOOST_CHECK_CLOSE(p1[0], 0.60947571, 1e-4);
137  BOOST_CHECK_CLOSE(p1[1], 0.01175873, 1e-4);
138  BOOST_CHECK_CLOSE(p1[2], 1, 1e-6);
139  BOOST_CHECK_CLOSE(p2[0], 0.60947571, 1e-4);
140  BOOST_CHECK_CLOSE(p2[1], 0.01175873, 1e-4);
141  BOOST_CHECK_CLOSE(p2[2], -1.62123444 + 10, 1e-4);
142 }
143 
144 BOOST_AUTO_TEST_CASE(distance_box_box_3) {
145  CollisionGeometryPtr_t s1(new hpp::fcl::Box(1, 1, 1));
146  CollisionGeometryPtr_t s2(new hpp::fcl::Box(1, 1, 1));
147  static double pi = M_PI;
148  Transform3f tf1(hpp::fcl::makeQuat(cos(pi / 8), 0, 0, sin(pi / 8)),
149  Vec3f(-2, 1, .5));
150  Transform3f tf2(hpp::fcl::makeQuat(cos(pi / 8), 0, sin(pi / 8), 0),
151  Vec3f(2, .5, .5));
152 
153  CollisionObject o1(s1, tf1);
154  CollisionObject o2(s2, tf2);
155 
156  // Enable computation of nearest points
157  DistanceRequest distanceRequest(true, 0, 0);
158  DistanceResult distanceResult;
159 
160  hpp::fcl::distance(&o1, &o2, distanceRequest, distanceResult);
161 
162  std::cerr << "Applied transformations on two boxes" << std::endl;
163  std::cerr << " T1 = " << tf1.getTranslation() << std::endl
164  << " R1 = " << tf1.getRotation() << std::endl
165  << " T2 = " << tf2.getTranslation() << std::endl
166  << " R2 = " << tf2.getRotation() << std::endl;
167  std::cerr << "Closest points: p1 = " << distanceResult.nearest_points[0]
168  << ", p2 = " << distanceResult.nearest_points[1]
169  << ", distance = " << distanceResult.min_distance << std::endl;
170 
171  const Vec3f& p1 = distanceResult.nearest_points[0];
172  const Vec3f& p2 = distanceResult.nearest_points[1];
173  double distance = 4 - sqrt(2);
174  BOOST_CHECK_CLOSE(distanceResult.min_distance, distance, 1e-4);
175 
176  const Vec3f p1Ref(sqrt(2) / 2 - 2, 1, .5);
177  const Vec3f p2Ref(2 - sqrt(2) / 2, 1, .5);
178  BOOST_CHECK_CLOSE(p1[0], p1Ref[0], 1e-4);
179  BOOST_CHECK_CLOSE(p1[1], p1Ref[1], 1e-4);
180  BOOST_CHECK_CLOSE(p1[2], p1Ref[2], 1e-4);
181  BOOST_CHECK_CLOSE(p2[0], p2Ref[0], 1e-4);
182  BOOST_CHECK_CLOSE(p2[1], p2Ref[1], 1e-4);
183  BOOST_CHECK_CLOSE(p2[2], p2Ref[2], 1e-4);
184 
185  // Apply the same global transform to both objects and recompute
186  Transform3f tf3(hpp::fcl::makeQuat(0.435952844074, -0.718287018243,
187  0.310622451066, 0.444435113443),
188  Vec3f(4, 5, 6));
189  tf1 = tf3 * tf1;
190  tf2 = tf3 * tf2;
191  o1 = CollisionObject(s1, tf1);
192  o2 = CollisionObject(s2, tf2);
193 
194  distanceResult.clear();
195  hpp::fcl::distance(&o1, &o2, distanceRequest, distanceResult);
196 
197  std::cerr << "Applied transformations on two boxes" << std::endl;
198  std::cerr << " T1 = " << tf1.getTranslation() << std::endl
199  << " R1 = " << tf1.getRotation() << std::endl
200  << " T2 = " << tf2.getTranslation() << std::endl
201  << " R2 = " << tf2.getRotation() << std::endl;
202  std::cerr << "Closest points: p1 = " << distanceResult.nearest_points[0]
203  << ", p2 = " << distanceResult.nearest_points[1]
204  << ", distance = " << distanceResult.min_distance << std::endl;
205 
206  BOOST_CHECK_CLOSE(distanceResult.min_distance, distance, 1e-4);
207 
208  const Vec3f p1Moved = tf3.transform(p1Ref);
209  const Vec3f p2Moved = tf3.transform(p2Ref);
210  BOOST_CHECK_CLOSE(p1[0], p1Moved[0], 1e-4);
211  BOOST_CHECK_CLOSE(p1[1], p1Moved[1], 1e-4);
212  BOOST_CHECK_CLOSE(p1[2], p1Moved[2], 1e-4);
213  BOOST_CHECK_CLOSE(p2[0], p2Moved[0], 1e-4);
214  BOOST_CHECK_CLOSE(p2[1], p2Moved[1], 1e-4);
215  BOOST_CHECK_CLOSE(p2[2], p2Moved[2], 1e-4);
216 }
217 
218 BOOST_AUTO_TEST_CASE(distance_box_box_4) {
219  hpp::fcl::Box s1(1, 1, 1);
220  hpp::fcl::Box s2(1, 1, 1);
221 
222  // Enable computation of nearest points
223  DistanceRequest distanceRequest(true, 0, 0);
224  DistanceResult distanceResult;
225  double distance;
226 
227  Transform3f tf1(Vec3f(2, 0, 0));
228  Transform3f tf2;
229  hpp::fcl::distance(&s1, tf1, &s2, tf2, distanceRequest, distanceResult);
230 
231  distance = 1.;
232  BOOST_CHECK_CLOSE(distanceResult.min_distance, distance, 1e-4);
233 
234  tf1.setTranslation(Vec3f(1.01, 0, 0));
235  distanceResult.clear();
236  hpp::fcl::distance(&s1, tf1, &s2, tf2, distanceRequest, distanceResult);
237 
238  distance = 0.01;
239  BOOST_CHECK_CLOSE(distanceResult.min_distance, distance, 2e-3);
240 
241  tf1.setTranslation(Vec3f(0.99, 0, 0));
242  distanceResult.clear();
243  hpp::fcl::distance(&s1, tf1, &s2, tf2, distanceRequest, distanceResult);
244 
245  distance = -0.01;
246  BOOST_CHECK_CLOSE(distanceResult.min_distance, distance, 2e-3);
247 
248  tf1.setTranslation(Vec3f(0, 0, 0));
249  distanceResult.clear();
250  hpp::fcl::distance(&s1, tf1, &s2, tf2, distanceRequest, distanceResult);
251 
252  distance = -1;
253  BOOST_CHECK_CLOSE(distanceResult.min_distance, distance, 2e-3);
254 }
hpp::fcl::Transform3f
Simple transform class used locally by InterpMotion.
Definition: transform.h:54
hpp::fcl::DistanceRequest
request to the distance computation
Definition: collision_data.h:392
octree.p1
tuple p1
Definition: octree.py:55
hpp::fcl::Transform3f::setTranslation
void setTranslation(const Eigen::MatrixBase< Derived > &T_)
set transform from translation
Definition: transform.h:142
BOOST_AUTO_TEST_CASE
BOOST_AUTO_TEST_CASE(distance_box_box_1)
Definition: box_box_distance.cpp:62
hpp::fcl::DistanceResult::nearest_points
Vec3f nearest_points[2]
nearest points
Definition: collision_data.h:427
gjk.tf2
tuple tf2
Definition: test/scripts/gjk.py:37
geometric_shapes.h
M_PI
#define M_PI
hpp::fcl::DistanceResult::clear
void clear()
clear the result
Definition: collision_data.h:504
hpp::fcl::makeQuat
Quaternion3f makeQuat(FCL_REAL w, FCL_REAL x, FCL_REAL y, FCL_REAL z)
Definition: utility.cpp:368
hpp::fcl::distance
HPP_FCL_DLLAPI FCL_REAL distance(const Matrix3f &R0, const Vec3f &T0, const kIOS &b1, const kIOS &b2, Vec3f *P=NULL, Vec3f *Q=NULL)
Approximate distance between two kIOS bounding volumes.
Definition: kIOS.cpp:181
hpp::fcl::Transform3f::transform
Vec3f transform(const Eigen::MatrixBase< Derived > &v) const
transform a given vector by the transform
Definition: transform.h:153
hpp::fcl::DistanceResult::min_distance
FCL_REAL min_distance
minimum distance between two objects. if two objects are in collision, min_distance <= 0...
Definition: collision_data.h:424
hpp::fcl::Box
Center at zero point, axis aligned box.
Definition: shape/geometric_shapes.h:125
hpp::fcl::Transform3f::getTranslation
const Vec3f & getTranslation() const
get translation
Definition: transform.h:100
hpp::fcl::CollisionGeometryPtr_t
shared_ptr< CollisionGeometry > CollisionGeometryPtr_t
Definition: include/hpp/fcl/fwd.hh:96
collision_object.h
hpp::fcl::Vec3f
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:66
hpp::fcl::CollisionObject
the object for collision or distance computation, contains the geometry and the transform information...
Definition: collision_object.h:215
hpp::fcl::Transform3f::getRotation
const Matrix3f & getRotation() const
get rotation
Definition: transform.h:109
distance
double distance(const std::vector< Transform3f > &tf, const BVHModel< BV > &m1, const BVHModel< BV > &m2, bool verbose)
Definition: benchmark.cpp:93
gjk.tf1
tuple tf1
Definition: test/scripts/gjk.py:28

hpp-fcl
Author(s):
autogenerated on Fri Jun 2 2023 02:39:00