test_fcl_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 #include <gtest/gtest.h>
38 
39 #include <cmath>
40 #include <limits>
44 
45 template <typename S>
46 void test_distance_capsule_box(fcl::GJKSolverType solver_type, S solver_tolerance, S test_tolerance)
47 {
48  using CollisionGeometryPtr_t = std::shared_ptr<fcl::CollisionGeometry<S>>;
49 
50  // Capsule of radius 2 and of height 4
51  CollisionGeometryPtr_t capsuleGeometry (new fcl::Capsule<S> (2., 4.));
52  // Box of size 1 by 2 by 4
53  CollisionGeometryPtr_t boxGeometry (new fcl::Box<S> (1., 2., 4.));
54 
55  // Enable computation of nearest points
56  fcl::DistanceRequest<S> distanceRequest (true);
57  fcl::DistanceResult<S> distanceResult;
58 
59  distanceRequest.gjk_solver_type = solver_type;
60  distanceRequest.distance_tolerance = solver_tolerance;
61 
64  fcl::CollisionObject<S> capsule (capsuleGeometry, tf1);
65  fcl::CollisionObject<S> box (boxGeometry, tf2);
66 
67  // test distance
68  fcl::distance (&capsule, &box, distanceRequest, distanceResult);
69  // Nearest point on capsule
70  fcl::Vector3<S> o1 (distanceResult.nearest_points [0]);
71  // Nearest point on box
72  fcl::Vector3<S> o2 (distanceResult.nearest_points [1]);
73  EXPECT_NEAR (distanceResult.min_distance, 0.5, test_tolerance);
74  EXPECT_NEAR (o1 [0], 1.0, test_tolerance);
75  EXPECT_NEAR (o1 [1], 0.0, test_tolerance);
76  EXPECT_NEAR (o2 [0], 0.5, test_tolerance);
77  EXPECT_NEAR (o2 [1], 0.0, test_tolerance);
78 
79  // Move capsule above box
80  tf1 = fcl::Translation3<S>(fcl::Vector3<S> (0., 0., 8.));
81  capsule.setTransform (tf1);
82 
83  // test distance
84  distanceResult.clear ();
85  fcl::distance (&capsule, &box, distanceRequest, distanceResult);
86  o1 = distanceResult.nearest_points [0];
87  o2 = distanceResult.nearest_points [1];
88 
89  EXPECT_NEAR (distanceResult.min_distance, 2.0, test_tolerance);
90  EXPECT_NEAR (o1 [0], 0.0, test_tolerance);
91  EXPECT_NEAR (o1 [1], 0.0, test_tolerance);
92  EXPECT_NEAR (o1 [2], 4.0, test_tolerance);
93 
94  EXPECT_NEAR (o2 [0], 0.0, test_tolerance);
95  EXPECT_NEAR (o2 [1], 0.0, test_tolerance);
96  EXPECT_NEAR (o2 [2], 2.0, test_tolerance);
97 
98  // Rotate capsule around y axis by pi/2 and move it behind box
99  tf1.translation() = fcl::Vector3<S>(-10., 0., 0.);
100  tf1.linear() = fcl::Quaternion<S>(sqrt(2)/2, 0, sqrt(2)/2, 0).toRotationMatrix();
101  capsule.setTransform (tf1);
102 
103  // test distance
104  distanceResult.clear ();
105  fcl::distance (&capsule, &box, distanceRequest, distanceResult);
106  o1 = distanceResult.nearest_points [0];
107  o2 = distanceResult.nearest_points [1];
108 
109  EXPECT_NEAR (distanceResult.min_distance, 5.5, test_tolerance);
110  EXPECT_NEAR (o1 [0], -6.0, test_tolerance);
111  EXPECT_NEAR (o1 [1], 0.0, test_tolerance);
112  EXPECT_NEAR (o1 [2], 0.0, test_tolerance);
113  EXPECT_NEAR (o2 [0], -0.5, test_tolerance);
114  EXPECT_NEAR (o2 [1], 0.0, test_tolerance);
115  EXPECT_NEAR (o2 [2], 0.0, test_tolerance);
116 }
117 
118 GTEST_TEST(FCL_GEOMETRIC_SHAPES, distance_capsule_box_ccd)
119 {
120  test_distance_capsule_box<double>(fcl::GJKSolverType::GST_LIBCCD, 1e-8, 4e-4);
121 }
122 
123 GTEST_TEST(FCL_GEOMETRIC_SHAPES, distance_capsule_box_indep)
124 {
125  test_distance_capsule_box<double>(fcl::GJKSolverType::GST_INDEP, 1e-8, 1e-4);
126 }
127 
128 //==============================================================================
129 int main(int argc, char* argv[])
130 {
131  ::testing::InitGoogleTest(&argc, argv);
132  return RUN_ALL_TESTS();
133 }
fcl::DistanceRequest
request to the distance computation
Definition: distance_request.h:52
fcl::CollisionObject::setTransform
void setTransform(const Matrix3< S > &R, const Vector3< S > &T)
set object's transform
Definition: collision_object-inl.h:198
fcl::Transform3
Eigen::Transform< S, 3, Eigen::Isometry > Transform3
Definition: types.h:91
fcl::GJKSolverType
GJKSolverType
Type of narrow phase GJK solver.
Definition: gjk_solver_type.h:45
collision_object.h
GTEST_TEST
GTEST_TEST(FCL_GEOMETRIC_SHAPES, distance_capsule_box_ccd)
Definition: test_fcl_capsule_box_1.cpp:118
fcl::distance
S distance(const Eigen::MatrixBase< DerivedA > &R0, const Eigen::MatrixBase< DerivedB > &T0, const kIOS< S > &b1, const kIOS< S > &b2, Vector3< S > *P, Vector3< S > *Q)
Approximate distance between two kIOS bounding volumes.
Definition: kIOS-inl.h:266
fcl::Quaternion
Eigen::Quaternion< S > Quaternion
Definition: types.h:88
fcl::DistanceResult::nearest_points
Vector3< S > nearest_points[2]
Nearest points in the world coordinates.
Definition: distance_result.h:69
fcl::DistanceResult
distance result
Definition: distance_request.h:48
fcl::Vector3
Eigen::Matrix< S, 3, 1 > Vector3
Definition: types.h:70
distance.h
EXPECT_NEAR
#define EXPECT_NEAR(a, b, prec)
fcl::DistanceRequest::gjk_solver_type
GJKSolverType gjk_solver_type
narrow phase solver type
Definition: distance_request.h:102
fcl::GST_LIBCCD
@ GST_LIBCCD
Definition: gjk_solver_type.h:45
fcl::Translation3
Eigen::Translation< S, 3 > Translation3
Definition: types.h:94
fcl::DistanceResult::min_distance
S min_distance
Minimum distance between two objects.
Definition: distance_result.h:64
main
int main(int argc, char *argv[])
Definition: test_fcl_capsule_box_1.cpp:129
fcl::Box
Center at zero point, axis aligned box.
Definition: box.h:51
fcl::DistanceRequest::distance_tolerance
S distance_tolerance
the threshold used in GJK algorithm to stop distance iteration
Definition: distance_request.h:99
test_distance_capsule_box
void test_distance_capsule_box(fcl::GJKSolverType solver_type, S solver_tolerance, S test_tolerance)
Definition: test_fcl_capsule_box_1.cpp:46
fcl::GST_INDEP
@ GST_INDEP
Definition: gjk_solver_type.h:45
fcl::CollisionObject
the object for collision or distance computation, contains the geometry and the transform information
Definition: collision_object.h:51
fcl::Capsule< S >
fcl::DistanceResult::clear
void clear()
clear the result
Definition: distance_result-inl.h:127
collision.h


fcl
Author(s):
autogenerated on Tue Dec 5 2023 03:40:49