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 COAL_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 "coal/distance.h"
48 #include "coal/math/transform.h"
49 #include "coal/collision.h"
50 #include "coal/collision_object.h"
51 #include "coal/shape/geometric_shapes.h"
52 
53 #include "utility.h"
54 
55 using coal::CollisionGeometryPtr_t;
56 using coal::CollisionObject;
57 using coal::DistanceRequest;
58 using coal::DistanceResult;
59 using coal::Transform3s;
60 using coal::Vec3s;
61 
62 BOOST_AUTO_TEST_CASE(distance_box_box_1) {
63  CollisionGeometryPtr_t s1(new coal::Box(6, 10, 2));
64  CollisionGeometryPtr_t s2(new coal::Box(2, 2, 2));
65 
66  Transform3s tf1;
67  Transform3s tf2(Vec3s(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, true, 0, 0);
74  DistanceResult distanceResult;
75 
76  coal::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 Vec3s& p1 = distanceResult.nearest_points[0];
91  const Vec3s& 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 coal::Box(6, 10, 2));
105  CollisionGeometryPtr_t s2(new coal::Box(2, 2, 2));
106  static double pi = M_PI;
107  Transform3s tf1;
108  Transform3s tf2(coal::makeQuat(cos(pi / 8), sin(pi / 8) / sqrt(3),
109  sin(pi / 8) / sqrt(3), sin(pi / 8) / sqrt(3)),
110  Vec3s(0, 0, 10));
111 
112  CollisionObject o1(s1, tf1);
113  CollisionObject o2(s2, tf2);
114 
115  // Enable computation of nearest points
116  DistanceRequest distanceRequest(true, true, 0, 0);
117  DistanceResult distanceResult;
118 
119  coal::distance(&o1, &o2, distanceRequest, distanceResult);
120 
121  std::cerr << "Applied transformations on two boxes" << std::endl;
122  std::cerr << " T1 = " << tf1.getTranslation() << std::endl
123  << " R1 = " << tf1.getRotation() << std::endl
124  << " T2 = " << tf2.getTranslation() << std::endl
125  << " R2 = " << tf2.getRotation() << std::endl;
126  std::cerr << "Closest points: p1 = " << distanceResult.nearest_points[0]
127  << ", p2 = " << distanceResult.nearest_points[1]
128  << ", distance = " << distanceResult.min_distance << std::endl;
129 
130  const Vec3s& p1 = distanceResult.nearest_points[0];
131  const Vec3s& p2 = distanceResult.nearest_points[1];
132  double distance = -1.62123444 + 10 - 1;
133  BOOST_CHECK_CLOSE(distanceResult.min_distance, distance, 1e-4);
134 
135  BOOST_CHECK_CLOSE(p1[0], 0.60947571, 1e-4);
136  BOOST_CHECK_CLOSE(p1[1], 0.01175873, 1e-4);
137  BOOST_CHECK_CLOSE(p1[2], 1, 1e-6);
138  BOOST_CHECK_CLOSE(p2[0], 0.60947571, 1e-4);
139  BOOST_CHECK_CLOSE(p2[1], 0.01175873, 1e-4);
140  BOOST_CHECK_CLOSE(p2[2], -1.62123444 + 10, 1e-4);
141 }
142 
143 BOOST_AUTO_TEST_CASE(distance_box_box_3) {
144  CollisionGeometryPtr_t s1(new coal::Box(1, 1, 1));
145  CollisionGeometryPtr_t s2(new coal::Box(1, 1, 1));
146  static double pi = M_PI;
147  Transform3s tf1(coal::makeQuat(cos(pi / 8), 0, 0, sin(pi / 8)),
148  Vec3s(-2, 1, .5));
149  Transform3s tf2(coal::makeQuat(cos(pi / 8), 0, sin(pi / 8), 0),
150  Vec3s(2, .5, .5));
151 
152  CollisionObject o1(s1, tf1);
153  CollisionObject o2(s2, tf2);
154 
155  // Enable computation of nearest points
156  DistanceRequest distanceRequest(true, 0, 0);
157  DistanceResult distanceResult;
158 
159  coal::distance(&o1, &o2, distanceRequest, distanceResult);
160 
161  std::cerr << "Applied transformations on two boxes" << std::endl;
162  std::cerr << " T1 = " << tf1.getTranslation() << std::endl
163  << " R1 = " << tf1.getRotation() << std::endl
164  << " T2 = " << tf2.getTranslation() << std::endl
165  << " R2 = " << tf2.getRotation() << std::endl;
166  std::cerr << "Closest points: p1 = " << distanceResult.nearest_points[0]
167  << ", p2 = " << distanceResult.nearest_points[1]
168  << ", distance = " << distanceResult.min_distance << std::endl;
169 
170  const Vec3s& p1 = distanceResult.nearest_points[0];
171  const Vec3s& p2 = distanceResult.nearest_points[1];
172  double distance = 4 - sqrt(2);
173  BOOST_CHECK_CLOSE(distanceResult.min_distance, distance, 1e-4);
174 
175  const Vec3s p1Ref(sqrt(2) / 2 - 2, 1, .5);
176  const Vec3s p2Ref(2 - sqrt(2) / 2, 1, .5);
177  BOOST_CHECK_CLOSE(p1[0], p1Ref[0], 1e-4);
178  BOOST_CHECK_CLOSE(p1[1], p1Ref[1], 1e-4);
179  BOOST_CHECK_CLOSE(p1[2], p1Ref[2], 1e-4);
180  BOOST_CHECK_CLOSE(p2[0], p2Ref[0], 1e-4);
181  BOOST_CHECK_CLOSE(p2[1], p2Ref[1], 1e-4);
182  BOOST_CHECK_CLOSE(p2[2], p2Ref[2], 1e-4);
183 
184  // Apply the same global transform to both objects and recompute
185  Transform3s tf3(coal::makeQuat(0.435952844074, -0.718287018243,
186  0.310622451066, 0.444435113443),
187  Vec3s(4, 5, 6));
188  tf1 = tf3 * tf1;
189  tf2 = tf3 * tf2;
190  o1 = CollisionObject(s1, tf1);
191  o2 = CollisionObject(s2, tf2);
192 
193  distanceResult.clear();
194  coal::distance(&o1, &o2, distanceRequest, distanceResult);
195 
196  std::cerr << "Applied transformations on two boxes" << std::endl;
197  std::cerr << " T1 = " << tf1.getTranslation() << std::endl
198  << " R1 = " << tf1.getRotation() << std::endl
199  << " T2 = " << tf2.getTranslation() << std::endl
200  << " R2 = " << tf2.getRotation() << std::endl;
201  std::cerr << "Closest points: p1 = " << distanceResult.nearest_points[0]
202  << ", p2 = " << distanceResult.nearest_points[1]
203  << ", distance = " << distanceResult.min_distance << std::endl;
204 
205  BOOST_CHECK_CLOSE(distanceResult.min_distance, distance, 1e-4);
206 
207  const Vec3s p1Moved = tf3.transform(p1Ref);
208  const Vec3s p2Moved = tf3.transform(p2Ref);
209  BOOST_CHECK_CLOSE(p1[0], p1Moved[0], 1e-4);
210  BOOST_CHECK_CLOSE(p1[1], p1Moved[1], 1e-4);
211  BOOST_CHECK_CLOSE(p1[2], p1Moved[2], 1e-4);
212  BOOST_CHECK_CLOSE(p2[0], p2Moved[0], 1e-4);
213  BOOST_CHECK_CLOSE(p2[1], p2Moved[1], 1e-4);
214  BOOST_CHECK_CLOSE(p2[2], p2Moved[2], 1e-4);
215 }
216 
217 BOOST_AUTO_TEST_CASE(distance_box_box_4) {
218  coal::Box s1(1, 1, 1);
219  coal::Box s2(1, 1, 1);
220 
221  // Enable computation of nearest points
222  DistanceRequest distanceRequest(true, true, 0, 0);
223  DistanceResult distanceResult;
224  double distance;
225 
226  Transform3s tf1(Vec3s(2, 0, 0));
227  Transform3s tf2;
228  coal::distance(&s1, tf1, &s2, tf2, distanceRequest, distanceResult);
229 
230  distance = 1.;
231  BOOST_CHECK_CLOSE(distanceResult.min_distance, distance, 1e-4);
232 
233  tf1.setTranslation(Vec3s(1.01, 0, 0));
234  distanceResult.clear();
235  coal::distance(&s1, tf1, &s2, tf2, distanceRequest, distanceResult);
236 
237  distance = 0.01;
238  BOOST_CHECK_CLOSE(distanceResult.min_distance, distance, 2e-3);
239 
240  tf1.setTranslation(Vec3s(0.99, 0, 0));
241  distanceResult.clear();
242  coal::distance(&s1, tf1, &s2, tf2, distanceRequest, distanceResult);
243 
244  distance = -0.01;
245  BOOST_CHECK_CLOSE(distanceResult.min_distance, distance, 2e-3);
246 
247  tf1.setTranslation(Vec3s(0, 0, 0));
248  distanceResult.clear();
249  coal::distance(&s1, tf1, &s2, tf2, distanceRequest, distanceResult);
250 
251  distance = -1;
252  BOOST_CHECK_CLOSE(distanceResult.min_distance, distance, 2e-3);
253 }
collision.h
coal::Vec3s
Eigen::Matrix< CoalScalar, 3, 1 > Vec3s
Definition: coal/data_types.h:77
coal::Transform3s::transform
Vec3s transform(const Eigen::MatrixBase< Derived > &v) const
transform a given vector by the transform
Definition: coal/math/transform.h:152
gjk.tf1
tuple tf1
Definition: test/scripts/gjk.py:27
distance
double distance(const std::vector< Transform3s > &tf, const BVHModel< BV > &m1, const BVHModel< BV > &m2, bool verbose)
Definition: benchmark.cpp:93
utility.h
coal::distance
COAL_DLLAPI CoalScalar distance(const Matrix3s &R0, const Vec3s &T0, const kIOS &b1, const kIOS &b2, Vec3s *P=NULL, Vec3s *Q=NULL)
Approximate distance between two kIOS bounding volumes.
Definition: kIOS.cpp:180
collision_object.h
coal::Box
Center at zero point, axis aligned box.
Definition: coal/shape/geometric_shapes.h:166
coal::Transform3s
Simple transform class used locally by InterpMotion.
Definition: coal/math/transform.h:55
coal::DistanceResult::min_distance
CoalScalar min_distance
minimum distance between two objects. If two objects are in collision and DistanceRequest::enable_sig...
Definition: coal/collision_data.h:1058
transform.h
coal::DistanceRequest
request to the distance computation
Definition: coal/collision_data.h:985
distance.h
coal::DistanceResult
distance result
Definition: coal/collision_data.h:1051
coal::CollisionObject
the object for collision or distance computation, contains the geometry and the transform information
Definition: coal/collision_object.h:214
gjk.tf2
tuple tf2
Definition: test/scripts/gjk.py:36
octree.p1
tuple p1
Definition: octree.py:54
coal::makeQuat
Quatf makeQuat(CoalScalar w, CoalScalar x, CoalScalar y, CoalScalar z)
Definition: utility.cpp:370
BOOST_AUTO_TEST_CASE
BOOST_AUTO_TEST_CASE(distance_box_box_1)
Definition: box_box_distance.cpp:62
geometric_shapes.h
pi
constexpr CoalScalar pi
Definition: collision_node_asserts.cpp:10
coal::DistanceResult::clear
void clear()
clear the result
Definition: coal/collision_data.h:1139
coal::DistanceResult::nearest_points
std::array< Vec3s, 2 > nearest_points
nearest points. See CollisionResult::nearest_points.
Definition: coal/collision_data.h:1065
coal::CollisionGeometryPtr_t
shared_ptr< CollisionGeometry > CollisionGeometryPtr_t
Definition: include/coal/fwd.hh:134

hpp-fcl
Author(s):
autogenerated on Sat Nov 23 2024 03:44:57