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 COAL_GEOMETRIC_SHAPES
38 #include <boost/test/included/unit_test.hpp>
39 
40 #include "coal/shape/convex.h"
41 #include "coal/collision.h"
42 #include "coal/distance.h"
43 
44 #include "utility.h"
45 
46 using namespace coal;
47 
48 BOOST_AUTO_TEST_CASE(convex) {
49  CoalScalar l = 1, w = 1, d = 1;
50  Convex<Quadrilateral> box(buildBox(l, w, d));
51 
52  // Check neighbors
53  for (size_t i = 0; i < 8; ++i) {
54  BOOST_CHECK_EQUAL((*box.neighbors)[i].count(), 3);
55  }
56  BOOST_CHECK_EQUAL((*box.neighbors)[0][0], 1);
57  BOOST_CHECK_EQUAL((*box.neighbors)[0][1], 2);
58  BOOST_CHECK_EQUAL((*box.neighbors)[0][2], 4);
59 
60  BOOST_CHECK_EQUAL((*box.neighbors)[1][0], 0);
61  BOOST_CHECK_EQUAL((*box.neighbors)[1][1], 3);
62  BOOST_CHECK_EQUAL((*box.neighbors)[1][2], 5);
63 
64  BOOST_CHECK_EQUAL((*box.neighbors)[2][0], 0);
65  BOOST_CHECK_EQUAL((*box.neighbors)[2][1], 3);
66  BOOST_CHECK_EQUAL((*box.neighbors)[2][2], 6);
67 
68  BOOST_CHECK_EQUAL((*box.neighbors)[3][0], 1);
69  BOOST_CHECK_EQUAL((*box.neighbors)[3][1], 2);
70  BOOST_CHECK_EQUAL((*box.neighbors)[3][2], 7);
71 
72  BOOST_CHECK_EQUAL((*box.neighbors)[4][0], 0);
73  BOOST_CHECK_EQUAL((*box.neighbors)[4][1], 5);
74  BOOST_CHECK_EQUAL((*box.neighbors)[4][2], 6);
75 
76  BOOST_CHECK_EQUAL((*box.neighbors)[5][0], 1);
77  BOOST_CHECK_EQUAL((*box.neighbors)[5][1], 4);
78  BOOST_CHECK_EQUAL((*box.neighbors)[5][2], 7);
79 
80  BOOST_CHECK_EQUAL((*box.neighbors)[6][0], 2);
81  BOOST_CHECK_EQUAL((*box.neighbors)[6][1], 4);
82  BOOST_CHECK_EQUAL((*box.neighbors)[6][2], 7);
83 
84  BOOST_CHECK_EQUAL((*box.neighbors)[7][0], 3);
85  BOOST_CHECK_EQUAL((*box.neighbors)[7][1], 5);
86  BOOST_CHECK_EQUAL((*box.neighbors)[7][2], 6);
87 }
88 
89 template <typename Sa, typename Sb>
90 void compareShapeIntersection(const Sa& sa, const Sb& sb,
91  const Transform3s& tf1, const Transform3s& tf2,
92  CoalScalar tol = 1e-9) {
93  CollisionRequest request(CONTACT | DISTANCE_LOWER_BOUND, 1);
94  CollisionResult resA, resB;
95 
96  collide(&sa, tf1, &sa, tf2, request, resA);
97  collide(&sb, tf1, &sb, tf2, request, resB);
98 
99  BOOST_CHECK_EQUAL(resA.isCollision(), resB.isCollision());
100  BOOST_CHECK_EQUAL(resA.numContacts(), resB.numContacts());
101 
102  if (resA.isCollision() && resB.isCollision()) {
103  Contact cA = resA.getContact(0), cB = resB.getContact(0);
104 
105  BOOST_TEST_MESSAGE(tf1 << '\n'
106  << cA.pos.format(pyfmt) << '\n'
107  << '\n'
108  << tf2 << '\n'
109  << cB.pos.format(pyfmt) << '\n');
110  // Only warnings because there are still some bugs.
111  BOOST_WARN_SMALL((cA.pos - cB.pos).squaredNorm(), tol);
112  BOOST_WARN_SMALL((cA.normal - cB.normal).squaredNorm(), tol);
113  } else {
114  BOOST_CHECK_CLOSE(resA.distance_lower_bound, resB.distance_lower_bound,
115  tol); // distances should be same
116  }
117 }
118 
119 template <typename Sa, typename Sb>
120 void compareShapeDistance(const Sa& sa, const Sb& sb, const Transform3s& tf1,
121  const Transform3s& tf2, CoalScalar tol = 1e-9) {
122  DistanceRequest request(true);
123  DistanceResult resA, resB;
124 
125  distance(&sa, tf1, &sa, tf2, request, resA);
126  distance(&sb, tf1, &sb, tf2, request, resB);
127 
128  BOOST_TEST_MESSAGE(tf1 << '\n'
129  << resA.normal.format(pyfmt) << '\n'
130  << resA.nearest_points[0].format(pyfmt) << '\n'
131  << resA.nearest_points[1].format(pyfmt) << '\n'
132  << '\n'
133  << tf2 << '\n'
134  << resB.normal.format(pyfmt) << '\n'
135  << resB.nearest_points[0].format(pyfmt) << '\n'
136  << resB.nearest_points[1].format(pyfmt) << '\n');
137  // TODO in one case, there is a mismatch between the distances and I cannot
138  // say which one is correct. To visualize the case, use script
139  // test/geometric_shapes.py
140  BOOST_WARN_CLOSE(resA.min_distance, resB.min_distance, tol);
141  // BOOST_CHECK_CLOSE(resA.min_distance, resB.min_distance, tol);
142 
143  // Only warnings because there are still some bugs.
144  BOOST_WARN_SMALL((resA.normal - resA.normal).squaredNorm(), tol);
145  BOOST_WARN_SMALL(
146  (resA.nearest_points[0] - resB.nearest_points[0]).squaredNorm(), tol);
147  BOOST_WARN_SMALL(
148  (resA.nearest_points[1] - resB.nearest_points[1]).squaredNorm(), tol);
149 }
150 
151 BOOST_AUTO_TEST_CASE(compare_convex_box) {
152  CoalScalar extents[6] = {0, 0, 0, 10, 10, 10};
153  CoalScalar l = 1, w = 1, d = 1, eps = 1e-4;
154  Box box(l * 2, w * 2, d * 2);
155  Convex<Quadrilateral> convex_box(buildBox(l, w, d));
156 
157  Transform3s tf1;
158  Transform3s tf2;
159 
160  tf2.setTranslation(Vec3s(3, 0, 0));
161  compareShapeIntersection(box, convex_box, tf1, tf2, eps);
162  compareShapeDistance(box, convex_box, tf1, tf2, eps);
163 
164  tf2.setTranslation(Vec3s(0, 0, 0));
165  compareShapeIntersection(box, convex_box, tf1, tf2, eps);
166  compareShapeDistance(box, convex_box, tf1, tf2, eps);
167 
168  for (int i = 0; i < 1000; ++i) {
169  generateRandomTransform(extents, tf2);
170  compareShapeIntersection(box, convex_box, tf1, tf2, eps);
171  compareShapeDistance(box, convex_box, tf1, tf2, eps);
172  }
173 }
174 
175 #ifdef COAL_HAS_QHULL
176 BOOST_AUTO_TEST_CASE(convex_hull_throw) {
177  std::shared_ptr<std::vector<Vec3s>> points(
178  new std::vector<Vec3s>({Vec3s(1, 1, 1), Vec3s(0, 0, 0), Vec3s(1, 0, 0)}));
179 
180  BOOST_CHECK_THROW(ConvexBase::convexHull(points, 0, false, NULL),
181  std::invalid_argument);
182  BOOST_CHECK_THROW(ConvexBase::convexHull(points, 1, false, NULL),
183  std::invalid_argument);
184  BOOST_CHECK_THROW(ConvexBase::convexHull(points, 2, false, NULL),
185  std::invalid_argument);
186  BOOST_CHECK_THROW(ConvexBase::convexHull(points, 3, false, NULL),
187  std::invalid_argument);
188 }
189 
190 BOOST_AUTO_TEST_CASE(convex_hull_quad) {
191  std::shared_ptr<std::vector<Vec3s>> points(new std::vector<Vec3s>({
192  Vec3s(1, 1, 1),
193  Vec3s(0, 0, 0),
194  Vec3s(1, 0, 0),
195  Vec3s(0, 0, 1),
196  }));
197 
198  ConvexBase* convexHull = ConvexBase::convexHull(points, 4, false, NULL);
199 
200  BOOST_REQUIRE_EQUAL(convexHull->num_points, 4);
201  BOOST_CHECK_EQUAL((*(convexHull->neighbors))[0].count(), 3);
202  BOOST_CHECK_EQUAL((*(convexHull->neighbors))[1].count(), 3);
203  BOOST_CHECK_EQUAL((*(convexHull->neighbors))[2].count(), 3);
204  delete convexHull;
205 }
206 
207 BOOST_AUTO_TEST_CASE(convex_hull_box_like) {
208  std::shared_ptr<std::vector<Vec3s>> points(new std::vector<Vec3s>({
209  Vec3s(1, 1, 1),
210  Vec3s(1, 1, -1),
211  Vec3s(1, -1, 1),
212  Vec3s(1, -1, -1),
213  Vec3s(-1, 1, 1),
214  Vec3s(-1, 1, -1),
215  Vec3s(-1, -1, 1),
216  Vec3s(-1, -1, -1),
217  Vec3s(0, 0, 0),
218  Vec3s(0, 0, 0.99),
219  }));
220 
221  ConvexBase* convexHull = ConvexBase::convexHull(points, 9, false, NULL);
222 
223  BOOST_REQUIRE_EQUAL(8, convexHull->num_points);
224  {
225  const std::vector<Vec3s>& cvxhull_points = *(convexHull->points);
226  for (size_t i = 0; i < 8; ++i) {
227  BOOST_CHECK(cvxhull_points[i].cwiseAbs() == Vec3s(1, 1, 1));
228  BOOST_CHECK_EQUAL((*(convexHull->neighbors))[i].count(), 3);
229  }
230  }
231  delete convexHull;
232 
233  convexHull = ConvexBase::convexHull(points, 9, true, NULL);
234  Convex<Triangle>* convex_tri = dynamic_cast<Convex<Triangle>*>(convexHull);
235  BOOST_CHECK(convex_tri != NULL);
236 
237  BOOST_REQUIRE_EQUAL(8, convexHull->num_points);
238  {
239  const std::vector<Vec3s>& cvxhull_points = *(convexHull->points);
240  for (size_t i = 0; i < 8; ++i) {
241  BOOST_CHECK(cvxhull_points[i].cwiseAbs() == Vec3s(1, 1, 1));
242  BOOST_CHECK((*(convexHull->neighbors))[i].count() >= 3);
243  BOOST_CHECK((*(convexHull->neighbors))[i].count() <= 6);
244  }
245  }
246  delete convexHull;
247 }
248 
249 BOOST_AUTO_TEST_CASE(convex_copy_constructor) {
250  Convex<Triangle>* convexHullTriCopy;
251  {
252  std::shared_ptr<std::vector<Vec3s>> points(new std::vector<Vec3s>({
253  Vec3s(1, 1, 1),
254  Vec3s(1, 1, -1),
255  Vec3s(1, -1, 1),
256  Vec3s(1, -1, -1),
257  Vec3s(-1, 1, 1),
258  Vec3s(-1, 1, -1),
259  Vec3s(-1, -1, 1),
260  Vec3s(-1, -1, -1),
261  Vec3s(0, 0, 0),
262  }));
263 
264  Convex<Triangle>* convexHullTri = dynamic_cast<Convex<Triangle>*>(
265  ConvexBase::convexHull(points, 9, true, NULL));
266  convexHullTriCopy = new Convex<Triangle>(*convexHullTri);
267  BOOST_CHECK(*convexHullTri == *convexHullTriCopy);
268  }
269  Convex<Triangle>* convexHullTriCopyOfCopy =
270  new Convex<Triangle>(*convexHullTriCopy);
271  BOOST_CHECK(*convexHullTriCopyOfCopy == *convexHullTriCopy);
272 }
273 
274 BOOST_AUTO_TEST_CASE(convex_clone) {
275  std::shared_ptr<std::vector<Vec3s>> points(new std::vector<Vec3s>({
276  Vec3s(1, 1, 1),
277  Vec3s(1, 1, -1),
278  Vec3s(1, -1, 1),
279  Vec3s(1, -1, -1),
280  Vec3s(-1, 1, 1),
281  Vec3s(-1, 1, -1),
282  Vec3s(-1, -1, 1),
283  Vec3s(-1, -1, -1),
284  Vec3s(0, 0, 0),
285  }));
286 
287  Convex<Triangle>* convexHullTri = dynamic_cast<Convex<Triangle>*>(
288  ConvexBase::convexHull(points, 9, true, NULL));
289  Convex<Triangle>* convexHullTriCopy;
290  convexHullTriCopy = convexHullTri->clone();
291  BOOST_CHECK(*convexHullTri == *convexHullTriCopy);
292 }
293 
294 #endif
coal::Contact::pos
Vec3s pos
contact position, in world space
Definition: coal/collision_data.h:102
collision.h
coal::CONTACT
@ CONTACT
Definition: coal/collision_data.h:305
coal::Vec3s
Eigen::Matrix< CoalScalar, 3, 1 > Vec3s
Definition: coal/data_types.h:77
coal::ConvexBase::num_points
unsigned int num_points
Definition: coal/shape/geometric_shapes.h:720
coal::generateRandomTransform
void generateRandomTransform(CoalScalar extents[6], Transform3s &transform)
Generate one random transform whose translation is constrained by extents and rotation without constr...
Definition: utility.cpp:210
collision_manager.box
box
Definition: collision_manager.py:11
coal::ConvexBase::neighbors
std::shared_ptr< std::vector< Neighbors > > neighbors
Neighbors of each vertex. It is an array of size num_points. For each vertex, it contains the number ...
Definition: coal/shape/geometric_shapes.h:732
coal::CollisionResult::isCollision
bool isCollision() const
return binary collision result
Definition: coal/collision_data.h:443
coal::Contact::normal
Vec3s normal
contact normal, pointing from o1 to o2. The normal defined as the normalized separation vector: norma...
Definition: coal/collision_data.h:88
gjk.tf1
tuple tf1
Definition: test/scripts/gjk.py:27
coal
Main namespace.
Definition: coal/broadphase/broadphase_bruteforce.h:44
coal::DistanceResult::normal
Vec3s normal
normal.
Definition: coal/collision_data.h:1061
utility.h
coal::buildBox
Convex< Quadrilateral > buildBox(CoalScalar l, CoalScalar w, CoalScalar d)
Constructs a box with halfsides (l, w, d), centered around 0. The box is 2*l wide on the x-axis,...
Definition: utility.cpp:460
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
eps
const CoalScalar eps
Definition: obb.cpp:93
coal::Convex::clone
virtual Convex< PolygonT > * clone() const
Clone (deep copy)
convex.h
coal::CollisionResult::distance_lower_bound
CoalScalar distance_lower_bound
Definition: coal/collision_data.h:401
coal::Box
Center at zero point, axis aligned box.
Definition: coal/shape/geometric_shapes.h:166
coal::DISTANCE_LOWER_BOUND
@ DISTANCE_LOWER_BOUND
Definition: coal/collision_data.h:306
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
coal::DistanceRequest
request to the distance computation
Definition: coal/collision_data.h:985
d
d
coal::ConvexBase::convexHull
static ConvexBase * convexHull(std::shared_ptr< std::vector< Vec3s >> &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:44
coal::CollisionResult
collision result
Definition: coal/collision_data.h:390
coal::CollisionRequest
request to the collision algorithm
Definition: coal/collision_data.h:311
distance.h
coal::DistanceResult
distance result
Definition: coal/collision_data.h:1051
gjk.tf2
tuple tf2
Definition: test/scripts/gjk.py:36
coal::Convex
Convex polytope.
Definition: coal/serialization/collision_object.h:51
BOOST_AUTO_TEST_CASE
BOOST_AUTO_TEST_CASE(convex)
Definition: test/convex.cpp:48
coal::Contact
Contact information returned by collision.
Definition: coal/collision_data.h:58
extents
CoalScalar extents[6]
Definition: test/geometric_shapes.cpp:53
coal::pyfmt
const Eigen::IOFormat pyfmt
Definition: utility.cpp:85
compareShapeDistance
void compareShapeDistance(const Sa &sa, const Sb &sb, const Transform3s &tf1, const Transform3s &tf2, CoalScalar tol=1e-9)
Definition: test/convex.cpp:120
coal::CollisionResult::getContact
const Contact & getContact(size_t i) const
get the i-th contact calculated
Definition: coal/collision_data.h:449
coal::collide
COAL_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:61
coal::ConvexBase::points
std::shared_ptr< std::vector< Vec3s > > points
An array of the points of the polygon.
Definition: coal/shape/geometric_shapes.h:719
coal::CoalScalar
double CoalScalar
Definition: coal/data_types.h:76
compareShapeIntersection
void compareShapeIntersection(const Sa &sa, const Sb &sb, const Transform3s &tf1, const Transform3s &tf2, CoalScalar tol=1e-9)
Definition: test/convex.cpp:90
coal::DistanceResult::nearest_points
std::array< Vec3s, 2 > nearest_points
nearest points. See CollisionResult::nearest_points.
Definition: coal/collision_data.h:1065
coal::ConvexBase
Base for convex polytope.
Definition: coal/shape/geometric_shapes.h:645
coal::CollisionResult::numContacts
size_t numContacts() const
number of contacts found
Definition: coal/collision_data.h:446

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