test/convex.cpp
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2019, LAAS-CNRS
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 Open Source Robotics Foundation 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 BOOST_TEST_MODULE FCL_GEOMETRIC_SHAPES
38 #include <boost/test/included/unit_test.hpp>
39 
40 #include <hpp/fcl/shape/convex.h>
41 #include <hpp/fcl/collision.h>
42 #include <hpp/fcl/distance.h>
43 
44 #include "utility.h"
45 
46 using namespace hpp::fcl;
47 
48 Convex<Quadrilateral> buildBox(FCL_REAL l, FCL_REAL w, FCL_REAL d) {
49  Vec3f* pts = new Vec3f[8];
50  pts[0] = Vec3f(l, w, d);
51  pts[1] = Vec3f(l, w, -d);
52  pts[2] = Vec3f(l, -w, d);
53  pts[3] = Vec3f(l, -w, -d);
54  pts[4] = Vec3f(-l, w, d);
55  pts[5] = Vec3f(-l, w, -d);
56  pts[6] = Vec3f(-l, -w, d);
57  pts[7] = Vec3f(-l, -w, -d);
58 
59  Quadrilateral* polygons = new Quadrilateral[6];
60  polygons[0].set(0, 2, 3, 1); // x+ side
61  polygons[1].set(2, 6, 7, 3); // y- side
62  polygons[2].set(4, 5, 7, 6); // x- side
63  polygons[3].set(0, 1, 5, 4); // y+ side
64  polygons[4].set(1, 3, 7, 5); // z- side
65  polygons[5].set(0, 2, 6, 4); // z+ side
66 
67  return Convex<Quadrilateral>(true,
68  pts, // points
69  8, // num points
70  polygons,
71  6 // number of polygons
72  );
73 }
74 
75 BOOST_AUTO_TEST_CASE(convex) {
76  FCL_REAL l = 1, w = 1, d = 1;
77  Convex<Quadrilateral> box(buildBox(l, w, d));
78 
79  // Check neighbors
80  for (int i = 0; i < 8; ++i) {
81  BOOST_CHECK_EQUAL(box.neighbors[i].count(), 3);
82  }
83  BOOST_CHECK_EQUAL(box.neighbors[0][0], 1);
84  BOOST_CHECK_EQUAL(box.neighbors[0][1], 2);
85  BOOST_CHECK_EQUAL(box.neighbors[0][2], 4);
86 
87  BOOST_CHECK_EQUAL(box.neighbors[1][0], 0);
88  BOOST_CHECK_EQUAL(box.neighbors[1][1], 3);
89  BOOST_CHECK_EQUAL(box.neighbors[1][2], 5);
90 
91  BOOST_CHECK_EQUAL(box.neighbors[2][0], 0);
92  BOOST_CHECK_EQUAL(box.neighbors[2][1], 3);
93  BOOST_CHECK_EQUAL(box.neighbors[2][2], 6);
94 
95  BOOST_CHECK_EQUAL(box.neighbors[3][0], 1);
96  BOOST_CHECK_EQUAL(box.neighbors[3][1], 2);
97  BOOST_CHECK_EQUAL(box.neighbors[3][2], 7);
98 
99  BOOST_CHECK_EQUAL(box.neighbors[4][0], 0);
100  BOOST_CHECK_EQUAL(box.neighbors[4][1], 5);
101  BOOST_CHECK_EQUAL(box.neighbors[4][2], 6);
102 
103  BOOST_CHECK_EQUAL(box.neighbors[5][0], 1);
104  BOOST_CHECK_EQUAL(box.neighbors[5][1], 4);
105  BOOST_CHECK_EQUAL(box.neighbors[5][2], 7);
106 
107  BOOST_CHECK_EQUAL(box.neighbors[6][0], 2);
108  BOOST_CHECK_EQUAL(box.neighbors[6][1], 4);
109  BOOST_CHECK_EQUAL(box.neighbors[6][2], 7);
110 
111  BOOST_CHECK_EQUAL(box.neighbors[7][0], 3);
112  BOOST_CHECK_EQUAL(box.neighbors[7][1], 5);
113  BOOST_CHECK_EQUAL(box.neighbors[7][2], 6);
114 }
115 
116 template <typename Sa, typename Sb>
117 void compareShapeIntersection(const Sa& sa, const Sb& sb,
118  const Transform3f& tf1, const Transform3f& tf2,
119  FCL_REAL tol = 1e-9) {
120  CollisionRequest request(CONTACT | DISTANCE_LOWER_BOUND, 1);
121  CollisionResult resA, resB;
122 
123  collide(&sa, tf1, &sa, tf2, request, resA);
124  collide(&sb, tf1, &sb, tf2, request, resB);
125 
126  BOOST_CHECK_EQUAL(resA.isCollision(), resB.isCollision());
127  BOOST_CHECK_EQUAL(resA.numContacts(), resB.numContacts());
128 
129  if (resA.isCollision() && resB.isCollision()) {
130  Contact cA = resA.getContact(0), cB = resB.getContact(0);
131 
132  BOOST_TEST_MESSAGE(tf1 << '\n'
133  << cA.pos.format(pyfmt) << '\n'
134  << '\n'
135  << tf2 << '\n'
136  << cB.pos.format(pyfmt) << '\n');
137  // Only warnings because there are still some bugs.
138  BOOST_WARN_SMALL((cA.pos - cB.pos).squaredNorm(), tol);
139  BOOST_WARN_SMALL((cA.normal - cB.normal).squaredNorm(), tol);
140  } else {
141  BOOST_CHECK_CLOSE(resA.distance_lower_bound, resB.distance_lower_bound,
142  tol); // distances should be same
143  }
144 }
145 
146 template <typename Sa, typename Sb>
147 void compareShapeDistance(const Sa& sa, const Sb& sb, const Transform3f& tf1,
148  const Transform3f& tf2, FCL_REAL tol = 1e-9) {
149  DistanceRequest request(true);
150  DistanceResult resA, resB;
151 
152  distance(&sa, tf1, &sa, tf2, request, resA);
153  distance(&sb, tf1, &sb, tf2, request, resB);
154 
155  BOOST_TEST_MESSAGE(tf1 << '\n'
156  << resA.normal.format(pyfmt) << '\n'
157  << resA.nearest_points[0].format(pyfmt) << '\n'
158  << resA.nearest_points[1].format(pyfmt) << '\n'
159  << '\n'
160  << tf2 << '\n'
161  << resB.normal.format(pyfmt) << '\n'
162  << resB.nearest_points[0].format(pyfmt) << '\n'
163  << resB.nearest_points[1].format(pyfmt) << '\n');
164  // TODO in one case, there is a mismatch between the distances and I cannot
165  // say which one is correct. To visualize the case, use script
166  // test/geometric_shapes.py
167  BOOST_WARN_CLOSE(resA.min_distance, resB.min_distance, tol);
168  // BOOST_CHECK_CLOSE(resA.min_distance, resB.min_distance, tol);
169 
170  // Only warnings because there are still some bugs.
171  BOOST_WARN_SMALL((resA.normal - resA.normal).squaredNorm(), tol);
172  BOOST_WARN_SMALL(
173  (resA.nearest_points[0] - resB.nearest_points[0]).squaredNorm(), tol);
174  BOOST_WARN_SMALL(
175  (resA.nearest_points[1] - resB.nearest_points[1]).squaredNorm(), tol);
176 }
177 
178 BOOST_AUTO_TEST_CASE(compare_convex_box) {
179  FCL_REAL extents[6] = {0, 0, 0, 10, 10, 10};
180  FCL_REAL l = 1, w = 1, d = 1, eps = 1e-4;
181  Box box(l * 2, w * 2, d * 2);
182  Convex<Quadrilateral> convex_box(buildBox(l, w, d));
183 
184  Transform3f tf1;
185  Transform3f tf2;
186 
187  tf2.setTranslation(Vec3f(3, 0, 0));
188  compareShapeIntersection(box, convex_box, tf1, tf2, eps);
189  compareShapeDistance(box, convex_box, tf1, tf2, eps);
190 
191  tf2.setTranslation(Vec3f(0, 0, 0));
192  compareShapeIntersection(box, convex_box, tf1, tf2, eps);
193  compareShapeDistance(box, convex_box, tf1, tf2, eps);
194 
195  for (int i = 0; i < 1000; ++i) {
196  generateRandomTransform(extents, tf2);
197  compareShapeIntersection(box, convex_box, tf1, tf2, eps);
198  compareShapeDistance(box, convex_box, tf1, tf2, eps);
199  }
200 }
201 
202 #ifdef HPP_FCL_HAS_QHULL
203 BOOST_AUTO_TEST_CASE(convex_hull_throw) {
204  std::vector<Vec3f> points({
205  Vec3f(1, 1, 1),
206  Vec3f(0, 0, 0),
207  Vec3f(1, 0, 0),
208  });
209 
210  BOOST_CHECK_THROW(ConvexBase::convexHull(points.data(), 0, false, NULL),
211  std::invalid_argument);
212  BOOST_CHECK_THROW(ConvexBase::convexHull(points.data(), 1, false, NULL),
213  std::invalid_argument);
214  BOOST_CHECK_THROW(ConvexBase::convexHull(points.data(), 2, false, NULL),
215  std::invalid_argument);
216  BOOST_CHECK_THROW(ConvexBase::convexHull(points.data(), 3, false, NULL),
217  std::invalid_argument);
218 }
219 
220 BOOST_AUTO_TEST_CASE(convex_hull_quad) {
221  std::vector<Vec3f> points({
222  Vec3f(1, 1, 1),
223  Vec3f(0, 0, 0),
224  Vec3f(1, 0, 0),
225  Vec3f(0, 0, 1),
226  });
227 
228  ConvexBase* convexHull = ConvexBase::convexHull(
229  points.data(), (unsigned int)points.size(), false, NULL);
230 
231  BOOST_REQUIRE_EQUAL(convexHull->num_points, 4);
232  BOOST_CHECK_EQUAL(convexHull->neighbors[0].count(), 3);
233  BOOST_CHECK_EQUAL(convexHull->neighbors[1].count(), 3);
234  BOOST_CHECK_EQUAL(convexHull->neighbors[2].count(), 3);
235  delete convexHull;
236 }
237 
238 BOOST_AUTO_TEST_CASE(convex_hull_box_like) {
239  std::vector<Vec3f> points({
240  Vec3f(1, 1, 1),
241  Vec3f(1, 1, -1),
242  Vec3f(1, -1, 1),
243  Vec3f(1, -1, -1),
244  Vec3f(-1, 1, 1),
245  Vec3f(-1, 1, -1),
246  Vec3f(-1, -1, 1),
247  Vec3f(-1, -1, -1),
248  Vec3f(0, 0, 0),
249  Vec3f(0, 0, 0.99),
250  });
251 
252  ConvexBase* convexHull = ConvexBase::convexHull(
253  points.data(), (unsigned int)points.size(), false, NULL);
254 
255  BOOST_REQUIRE_EQUAL(8, convexHull->num_points);
256  for (int i = 0; i < 8; ++i) {
257  BOOST_CHECK(convexHull->points[i].cwiseAbs() == Vec3f(1, 1, 1));
258  BOOST_CHECK_EQUAL(convexHull->neighbors[i].count(), 3);
259  }
260  delete convexHull;
261 
262  convexHull = ConvexBase::convexHull(points.data(),
263  (unsigned int)points.size(), true, NULL);
264  Convex<Triangle>* convex_tri = dynamic_cast<Convex<Triangle>*>(convexHull);
265  BOOST_CHECK(convex_tri != NULL);
266 
267  BOOST_REQUIRE_EQUAL(8, convexHull->num_points);
268  for (int i = 0; i < 8; ++i) {
269  BOOST_CHECK(convexHull->points[i].cwiseAbs() == Vec3f(1, 1, 1));
270  BOOST_CHECK(convexHull->neighbors[i].count() >= 3);
271  BOOST_CHECK(convexHull->neighbors[i].count() <= 6);
272  }
273  delete convexHull;
274 }
275 #endif
hpp::fcl::pyfmt
const Eigen::IOFormat pyfmt
Definition: utility.cpp:84
compareShapeIntersection
void compareShapeIntersection(const Sa &sa, const Sb &sb, const Transform3f &tf1, const Transform3f &tf2, FCL_REAL tol=1e-9)
Definition: test/convex.cpp:117
hpp::fcl::Vec3f
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:66
hpp::fcl::Quadrilateral::set
void set(index_type p0, index_type p1, index_type p2, index_type p3)
Set the vertex indices of the quadrilateral.
Definition: data_types.h:145
collision_manager.box
box
Definition: collision_manager.py:11
hpp::fcl::DistanceResult::nearest_points
Vec3f nearest_points[2]
nearest points
Definition: collision_data.h:427
hpp::fcl::ConvexBase::convexHull
static ConvexBase * convexHull(const Vec3f *points, unsigned int num_points, bool keepTriangles, const char *qhullCommand=NULL)
Build a convex hull based on Qhull library and store the vertices and optionally the triangles.
Definition: src/shape/convex.cpp:46
gjk.tf1
tuple tf1
Definition: test/scripts/gjk.py:27
hpp::fcl::CONTACT
@ CONTACT
Definition: collision_data.h:229
hpp::fcl::CollisionResult::getContact
const Contact & getContact(size_t i) const
get the i-th contact calculated
Definition: collision_data.h:345
hpp::fcl::CollisionResult::numContacts
size_t numContacts() const
number of contacts found
Definition: collision_data.h:342
utility.h
eps
const FCL_REAL eps
Definition: obb.cpp:93
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
compareShapeDistance
void compareShapeDistance(const Sa &sa, const Sb &sb, const Transform3f &tf1, const Transform3f &tf2, FCL_REAL tol=1e-9)
Definition: test/convex.cpp:147
extents
FCL_REAL extents[6]
Definition: test/geometric_shapes.cpp:51
hpp::fcl::Convex
Convex polytope.
Definition: shape/convex.h:50
hpp::fcl::collide
HPP_FCL_DLLAPI std::size_t collide(const CollisionObject *o1, const CollisionObject *o2, const CollisionRequest &request, CollisionResult &result)
Main collision interface: given two collision objects, and the requirements for contacts,...
Definition: src/collision.cpp:63
distance.h
hpp::fcl::FCL_REAL
double FCL_REAL
Definition: data_types.h:65
hpp::fcl::DistanceResult::normal
Vec3f normal
In case both objects are in collision, store the normal.
Definition: collision_data.h:430
hpp::fcl::Quadrilateral
Quadrilateral with 4 indices for points.
Definition: data_types.h:134
hpp::fcl::Contact::normal
Vec3f normal
contact normal, pointing from o1 to o2
Definition: collision_data.h:74
hpp::fcl::CollisionRequest
request to the collision algorithm
Definition: collision_data.h:235
buildBox
Convex< Quadrilateral > buildBox(FCL_REAL l, FCL_REAL w, FCL_REAL d)
Definition: test/convex.cpp:48
gjk.tf2
tuple tf2
Definition: test/scripts/gjk.py:36
hpp::fcl::CollisionResult
collision result
Definition: collision_data.h:302
BOOST_AUTO_TEST_CASE
BOOST_AUTO_TEST_CASE(convex)
Definition: test/convex.cpp:75
hpp::fcl::CollisionResult::isCollision
bool isCollision() const
return binary collision result
Definition: collision_data.h:339
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
Definition: broadphase_bruteforce.h:45
hpp::fcl::Transform3f
Simple transform class used locally by InterpMotion.
Definition: transform.h:54
hpp::fcl::ConvexBase
Base for convex polytope.
Definition: shape/geometric_shapes.h:581
hpp::fcl::DistanceRequest
request to the distance computation
Definition: collision_data.h:392
hpp::fcl::DistanceResult
distance result
Definition: collision_data.h:420
convex.h
hpp::fcl::generateRandomTransform
void generateRandomTransform(FCL_REAL extents[6], Transform3f &transform)
Generate one random transform whose translation is constrained by extents and rotation without constr...
Definition: utility.cpp:208
hpp::fcl::DISTANCE_LOWER_BOUND
@ DISTANCE_LOWER_BOUND
Definition: collision_data.h:230
hpp::fcl::Contact::pos
Vec3f pos
contact position, in world space
Definition: collision_data.h:77
hpp::fcl::CollisionResult::distance_lower_bound
FCL_REAL distance_lower_bound
Definition: collision_data.h:312
hpp::fcl::Contact
Contact information returned by collision.
Definition: collision_data.h:54
collision.h
hpp::fcl::Box
Center at zero point, axis aligned box.
Definition: shape/geometric_shapes.h:125

hpp-fcl
Author(s):
autogenerated on Fri Jan 26 2024 03:46:13