capsule_box_1.cpp
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2014-2016, CNRS-LAAS and AIST
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 CNRS-LAAS and AIST 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 #define CHECK_CLOSE_TO_0(x, eps) BOOST_CHECK_CLOSE((x + 1.0), (1.0), (eps))
41 
42 #include <cmath>
43 #include <hpp/fcl/distance.h>
44 #include <hpp/fcl/math/transform.h>
45 #include <hpp/fcl/collision.h>
48 
49 #include "utility.h"
50 
51 BOOST_AUTO_TEST_CASE(distance_capsule_box) {
53  // Capsule of radius 2 and of height 4
54  CollisionGeometryPtr_t capsuleGeometry(new hpp::fcl::Capsule(2., 4.));
55  // Box of size 1 by 2 by 4
56  CollisionGeometryPtr_t boxGeometry(new hpp::fcl::Box(1., 2., 4.));
57 
58  // Enable computation of nearest points
59  hpp::fcl::DistanceRequest distanceRequest(true, 0, 0);
60  hpp::fcl::DistanceResult distanceResult;
61 
64  hpp::fcl::CollisionObject capsule(capsuleGeometry, tf1);
65  hpp::fcl::CollisionObject box(boxGeometry, tf2);
66 
67  // test distance
68  hpp::fcl::distance(&capsule, &box, distanceRequest, distanceResult);
69  // Nearest point on capsule
70  hpp::fcl::Vec3f o1(distanceResult.nearest_points[0]);
71  // Nearest point on box
72  hpp::fcl::Vec3f o2(distanceResult.nearest_points[1]);
73  BOOST_CHECK_CLOSE(distanceResult.min_distance, 0.5, 1e-1);
74  BOOST_CHECK_CLOSE(o1[0], 1.0, 1e-1);
75  CHECK_CLOSE_TO_0(o1[1], 1e-1);
76  BOOST_CHECK_CLOSE(o2[0], 0.5, 1e-1);
77  CHECK_CLOSE_TO_0(o2[1], 1e-1);
78 
79  // Move capsule above box
81  capsule.setTransform(tf1);
82 
83  // test distance
84  distanceResult.clear();
85  hpp::fcl::distance(&capsule, &box, distanceRequest, distanceResult);
86  o1 = distanceResult.nearest_points[0];
87  o2 = distanceResult.nearest_points[1];
88 
89  BOOST_CHECK_CLOSE(distanceResult.min_distance, 2.0, 1e-1);
90  CHECK_CLOSE_TO_0(o1[0], 1e-1);
91  CHECK_CLOSE_TO_0(o1[1], 1e-1);
92  BOOST_CHECK_CLOSE(o1[2], 4.0, 1e-1);
93 
94  CHECK_CLOSE_TO_0(o2[0], 1e-1);
95  CHECK_CLOSE_TO_0(o2[1], 1e-1);
96  BOOST_CHECK_CLOSE(o2[2], 2.0, 1e-1);
97 
98  // Rotate capsule around y axis by pi/2 and move it behind box
99  tf1.setTranslation(hpp::fcl::Vec3f(-10., 0., 0.));
100  tf1.setQuatRotation(hpp::fcl::makeQuat(sqrt(2) / 2, 0, sqrt(2) / 2, 0));
101  capsule.setTransform(tf1);
102 
103  // test distance
104  distanceResult.clear();
105  hpp::fcl::distance(&capsule, &box, distanceRequest, distanceResult);
106  o1 = distanceResult.nearest_points[0];
107  o2 = distanceResult.nearest_points[1];
108 
109  BOOST_CHECK_CLOSE(distanceResult.min_distance, 5.5, 1e-1);
110  BOOST_CHECK_CLOSE(o1[0], -6, 1e-2);
111  CHECK_CLOSE_TO_0(o1[1], 1e-1);
112  CHECK_CLOSE_TO_0(o1[2], 1e-1);
113  BOOST_CHECK_CLOSE(o2[0], -0.5, 1e-2);
114  CHECK_CLOSE_TO_0(o2[1], 1e-1);
115  CHECK_CLOSE_TO_0(o2[2], 1e-1);
116 }
hpp::fcl::Vec3f
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:66
collision_object.h
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
gjk.tf1
tuple tf1
Definition: test/scripts/gjk.py:27
utility.h
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::CollisionGeometryPtr_t
shared_ptr< CollisionGeometry > CollisionGeometryPtr_t
Definition: include/hpp/fcl/fwd.hh:96
hpp::fcl::DistanceResult::clear
void clear()
clear the result
Definition: collision_data.h:504
distance.h
hpp::fcl::makeQuat
Quaternion3f makeQuat(FCL_REAL w, FCL_REAL x, FCL_REAL y, FCL_REAL z)
Definition: utility.cpp:368
hpp::fcl::CollisionObject
the object for collision or distance computation, contains the geometry and the transform information
Definition: collision_object.h:215
gjk.tf2
tuple tf2
Definition: test/scripts/gjk.py:36
hpp::fcl::Capsule
Capsule It is where is the distance between the point x and the capsule segment AB,...
Definition: shape/geometric_shapes.h:333
BOOST_AUTO_TEST_CASE
BOOST_AUTO_TEST_CASE(distance_capsule_box)
Definition: capsule_box_1.cpp:51
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::Transform3f
Simple transform class used locally by InterpMotion.
Definition: transform.h:54
hpp::fcl::CollisionObject::setTransform
void setTransform(const Matrix3f &R, const Vec3f &T)
set object's transform
Definition: collision_object.h:292
hpp::fcl::DistanceRequest
request to the distance computation
Definition: collision_data.h:392
hpp::fcl::DistanceResult
distance result
Definition: collision_data.h:420
transform.h
CHECK_CLOSE_TO_0
#define CHECK_CLOSE_TO_0(x, eps)
Definition: capsule_box_1.cpp:40
geometric_shapes.h
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 Aug 2 2024 02:45:13