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::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
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
hpp::fcl::Transform3f::setTranslation
void setTranslation(const Eigen::MatrixBase< Derived > &T_)
set transform from translation
Definition: transform.h:142
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
hpp::fcl::Quadrilateral
Quadrilateral with 4 indices for points.
Definition: data_types.h:134
hpp::fcl::CollisionResult::numContacts
size_t numContacts() const
number of contacts found
Definition: collision_data.h:342
eps
const FCL_REAL eps
Definition: obb.cpp:93
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
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
hpp::fcl::CollisionRequest
request to the collision algorithm
Definition: collision_data.h:235
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::FCL_REAL
double FCL_REAL
Definition: data_types.h:65
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::DistanceResult::normal
Vec3f normal
In case both objects are in collision, store the normal.
Definition: collision_data.h:430
extents
FCL_REAL extents[6]
Definition: test/geometric_shapes.cpp:51
buildBox
Convex< Quadrilateral > buildBox(FCL_REAL l, FCL_REAL w, FCL_REAL d)
Definition: test/convex.cpp:48
hpp::fcl::Contact::pos
Vec3f pos
contact position, in world space
Definition: collision_data.h:77
hpp::fcl::ConvexBase
Base for convex polytope.
Definition: shape/geometric_shapes.h:581
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, including num of max contacts, whether perform exhaustive collision (i.e., returning returning all the contact points), whether return detailed contact information (i.e., normal, contact point, depth; otherwise only contact primitive id is returned), this function performs the collision between them. Return value is the number of contacts generated between the two objects.
Definition: src/collision.cpp:63
BOOST_AUTO_TEST_CASE
BOOST_AUTO_TEST_CASE(convex)
Definition: test/convex.cpp:75
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
hpp::fcl::CollisionResult::getContact
const Contact & getContact(size_t i) const
get the i-th contact calculated
Definition: collision_data.h:345
collision_manager.box
box
Definition: collision_manager.py:11
hpp::fcl::CollisionResult::isCollision
bool isCollision() const
return binary collision result
Definition: collision_data.h:339
hpp::fcl::Vec3f
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:66
hpp::fcl::Convex
Convex polytope.
Definition: shape/convex.h:50
hpp::fcl::Contact
Contact information returned by collision.
Definition: collision_data.h:54
hpp::fcl::ConvexBase::neighbors
Neighbors * neighbors
Definition: shape/geometric_shapes.h:655
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::Contact::normal
Vec3f normal
contact normal, pointing from o1 to o2
Definition: collision_data.h:74
gjk.tf1
tuple tf1
Definition: test/scripts/gjk.py:28
hpp::fcl::pyfmt
const Eigen::IOFormat pyfmt
Definition: utility.cpp:84

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