Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	test/shape_inflation.cpp	Lines:	78	81	96.3 %
Date:	2024-02-09 12:57:42	Branches:	154	330	46.7 %


/*
 *  Software License Agreement (BSD License)
 *
 *  Copyright (c) 2022, INRIA.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of Willow Garage, Inc. nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 */

#define BOOST_TEST_MODULE FCL_SECURITY_MARGIN
#include <boost/test/included/unit_test.hpp>

#include <cmath>
#include <iostream>
#include <hpp/fcl/distance.h>
#include <hpp/fcl/math/transform.h>
#include <hpp/fcl/collision.h>
#include <hpp/fcl/collision_object.h>
#include <hpp/fcl/shape/geometric_shapes.h>
#include <hpp/fcl/shape/geometric_shapes_utility.h>

#include "utility.h"

using namespace hpp::fcl;
using hpp::fcl::CollisionGeometryPtr_t;
using hpp::fcl::CollisionObject;
using hpp::fcl::CollisionRequest;
using hpp::fcl::CollisionResult;
using hpp::fcl::DistanceRequest;
using hpp::fcl::DistanceResult;
using hpp::fcl::Transform3f;
using hpp::fcl::Vec3f;

#define MATH_SQUARED(x) (x * x)

template <typename Shape>
bool isApprox(const Shape &s1, const Shape &s2, const FCL_REAL tol);

bool isApprox(const FCL_REAL &v1, const FCL_REAL &v2, const FCL_REAL tol) {
  typedef Eigen::Matrix<FCL_REAL, 1, 1> ScalarMatrix;
  ScalarMatrix m1;
  m1 << v1;
  ScalarMatrix m2;
  m2 << v2;
  return m1.isApprox(m2, tol);
}

bool isApprox(const Box &s1, const Box &s2, const FCL_REAL tol) {
  return s1.halfSide.isApprox(s2.halfSide, tol);
}

bool isApprox(const Sphere &s1, const Sphere &s2, const FCL_REAL tol) {
  return isApprox(s1.radius, s2.radius, tol);
}

bool isApprox(const Ellipsoid &s1, const Ellipsoid &s2, const FCL_REAL tol) {
  return s1.radii.isApprox(s2.radii, tol);
}

bool isApprox(const Capsule &s1, const Capsule &s2, const FCL_REAL tol) {
  return isApprox(s1.radius, s2.radius, tol) &&
         isApprox(s1.halfLength, s2.halfLength, tol);
}

bool isApprox(const Cylinder &s1, const Cylinder &s2, const FCL_REAL tol) {
  return isApprox(s1.radius, s2.radius, tol) &&
         isApprox(s1.halfLength, s2.halfLength, tol);
}

bool isApprox(const Cone &s1, const Cone &s2, const FCL_REAL tol) {
  return isApprox(s1.radius, s2.radius, tol) &&
         isApprox(s1.halfLength, s2.halfLength, tol);
}

bool isApprox(const TriangleP &s1, const TriangleP &s2, const FCL_REAL tol) {
  return s1.a.isApprox(s2.a, tol) && s1.b.isApprox(s2.b, tol) &&
         s1.c.isApprox(s2.c, tol);
}

bool isApprox(const Halfspace &s1, const Halfspace &s2, const FCL_REAL tol) {
  return isApprox(s1.d, s2.d, tol) && s1.n.isApprox(s2.n, tol);
}

template <typename Shape>
void test(const Shape &original_shape, const FCL_REAL inflation,
          const FCL_REAL tol = 1e-8) {
  // Zero inflation
  {
    const FCL_REAL inflation = 0.;
    const auto &inflation_result = original_shape.inflated(inflation);
    const Transform3f &shift = inflation_result.second;
    const Shape &inflated_shape = inflation_result.first;

    BOOST_CHECK(isApprox(original_shape, inflated_shape, tol));
    BOOST_CHECK(shift.isIdentity(tol));
  }

  // Positive inflation
  {
    const auto &inflation_result = original_shape.inflated(inflation);
    const Shape &inflated_shape = inflation_result.first;
    const Transform3f &inflation_shift = inflation_result.second;

    BOOST_CHECK(!isApprox(original_shape, inflated_shape, tol));

    const auto &deflation_result = inflated_shape.inflated(-inflation);
    const Shape &deflated_shape = deflation_result.first;
    const Transform3f &deflation_shift = deflation_result.second;

    BOOST_CHECK(isApprox(original_shape, deflated_shape, tol));
    BOOST_CHECK((inflation_shift * deflation_shift).isIdentity(tol));
  }

  // Negative inflation
  {
    const auto &inflation_result = original_shape.inflated(-inflation);
    const Shape &inflated_shape = inflation_result.first;
    const Transform3f &inflation_shift = inflation_result.second;

    BOOST_CHECK(!isApprox(original_shape, inflated_shape, tol));

    const auto &deflation_result = inflated_shape.inflated(+inflation);
    const Shape &deflated_shape = deflation_result.first;
    const Transform3f &deflation_shift = deflation_result.second;

    BOOST_CHECK(isApprox(original_shape, deflated_shape, tol));
    BOOST_CHECK((inflation_shift * deflation_shift).isIdentity(tol));
  }
}

template <typename Shape>
void test_throw(const Shape &shape, const FCL_REAL inflation) {
  BOOST_REQUIRE_THROW(shape.inflated(inflation), std::invalid_argument);
}

template <typename Shape>
void test_no_throw(const Shape &shape, const FCL_REAL inflation) {
  BOOST_REQUIRE_NO_THROW(shape.inflated(inflation));
}

BOOST_AUTO_TEST_CASE(test_inflate) {
  const hpp::fcl::Sphere sphere(1);
  test(sphere, 0.01, 1e-8);
  test_throw(sphere, -1.1);
  test_no_throw(sphere, 1.);

  const hpp::fcl::Box box(1, 1, 1);
  test(box, 0.01, 1e-8);
  test_throw(box, -0.6);

  const hpp::fcl::Ellipsoid ellipsoid(1, 2, 3);
  test(ellipsoid, 0.01, 1e-8);
  test_throw(ellipsoid, -1.1);

  const hpp::fcl::Capsule capsule(1., 2.);
  test(capsule, 0.01, 1e-8);
  test_throw(capsule, -1.1);

  const hpp::fcl::Cylinder cylinder(1., 2.);
  test(cylinder, 0.01, 1e-8);
  test_throw(cylinder, -1.1);

  const hpp::fcl::Cone cone(1., 4.);
  test(cone, 0.01, 1e-8);
  test_throw(cone, -1.1);

  const hpp::fcl::Halfspace halfspace(Vec3f::UnitZ(), 0.);
  test(halfspace, 0.01, 1e-8);

  //  const hpp::fcl::TriangleP triangle(Vec3f::UnitX(), Vec3f::UnitY(),
  //                                     Vec3f::UnitZ());
  //  test(triangle, 0.01, 1e-8);
}


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			/*
2			* Software License Agreement (BSD License)
3			*
4			* Copyright (c) 2022, INRIA.
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 Willow Garage, Inc. 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
35			#define BOOST_TEST_MODULE FCL_SECURITY_MARGIN
36			#include <boost/test/included/unit_test.hpp>
37
38			#include <cmath>
39			#include <iostream>
40			#include <hpp/fcl/distance.h>
41			#include <hpp/fcl/math/transform.h>
42			#include <hpp/fcl/collision.h>
43			#include <hpp/fcl/collision_object.h>
44			#include <hpp/fcl/shape/geometric_shapes.h>
45			#include <hpp/fcl/shape/geometric_shapes_utility.h>
46
47			#include "utility.h"
48
49			using namespace hpp::fcl;
50			using hpp::fcl::CollisionGeometryPtr_t;
51			using hpp::fcl::CollisionObject;
52			using hpp::fcl::CollisionRequest;
53			using hpp::fcl::CollisionResult;
54			using hpp::fcl::DistanceRequest;
55			using hpp::fcl::DistanceResult;
56			using hpp::fcl::Transform3f;
57			using hpp::fcl::Vec3f;
58
59			#define MATH_SQUARED(x) (x * x)
60
61			template <typename Shape>
62			bool isApprox(const Shape &s1, const Shape &s2, const FCL_REAL tol);
63
64		34	bool isApprox(const FCL_REAL &v1, const FCL_REAL &v2, const FCL_REAL tol) {
65			typedef Eigen::Matrix<FCL_REAL, 1, 1> ScalarMatrix;
66	✓✗	34	ScalarMatrix m1;
67	✓✗	34	m1 << v1;
68	✓✗	34	ScalarMatrix m2;
69	✓✗	34	m2 << v2;
70	✓✗	68	return m1.isApprox(m2, tol);
71			}
72
73		5	bool isApprox(const Box &s1, const Box &s2, const FCL_REAL tol) {
74		5	return s1.halfSide.isApprox(s2.halfSide, tol);
75			}
76
77		5	bool isApprox(const Sphere &s1, const Sphere &s2, const FCL_REAL tol) {
78		5	return isApprox(s1.radius, s2.radius, tol);
79			}
80
81		5	bool isApprox(const Ellipsoid &s1, const Ellipsoid &s2, const FCL_REAL tol) {
82		5	return s1.radii.isApprox(s2.radii, tol);
83			}
84
85		5	bool isApprox(const Capsule &s1, const Capsule &s2, const FCL_REAL tol) {
86	✓✓	8	return isApprox(s1.radius, s2.radius, tol) &&
87	✓✗	8	isApprox(s1.halfLength, s2.halfLength, tol);
88			}
89
90		5	bool isApprox(const Cylinder &s1, const Cylinder &s2, const FCL_REAL tol) {
91	✓✓	8	return isApprox(s1.radius, s2.radius, tol) &&
92	✓✗	8	isApprox(s1.halfLength, s2.halfLength, tol);
93			}
94
95		5	bool isApprox(const Cone &s1, const Cone &s2, const FCL_REAL tol) {
96	✓✓	8	return isApprox(s1.radius, s2.radius, tol) &&
97	✓✗	8	isApprox(s1.halfLength, s2.halfLength, tol);
98			}
99
100			bool isApprox(const TriangleP &s1, const TriangleP &s2, const FCL_REAL tol) {
101			return s1.a.isApprox(s2.a, tol) && s1.b.isApprox(s2.b, tol) &&
102			s1.c.isApprox(s2.c, tol);
103			}
104
105		5	bool isApprox(const Halfspace &s1, const Halfspace &s2, const FCL_REAL tol) {
106	✓✓✓✗	5	return isApprox(s1.d, s2.d, tol) && s1.n.isApprox(s2.n, tol);
107			}
108
109			template <typename Shape>
110		14	void test(const Shape &original_shape, const FCL_REAL inflation,
111			const FCL_REAL tol = 1e-8) {
112			// Zero inflation
113			{
114		14	const FCL_REAL inflation = 0.;
115	✓✗	28	const auto &inflation_result = original_shape.inflated(inflation);
116		14	const Transform3f &shift = inflation_result.second;
117		14	const Shape &inflated_shape = inflation_result.first;
118
119	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	14	BOOST_CHECK(isApprox(original_shape, inflated_shape, tol));
120	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	14	BOOST_CHECK(shift.isIdentity(tol));
121			}
122
123			// Positive inflation
124			{
125	✓✗	28	const auto &inflation_result = original_shape.inflated(inflation);
126		14	const Shape &inflated_shape = inflation_result.first;
127		14	const Transform3f &inflation_shift = inflation_result.second;
128
129	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	14	BOOST_CHECK(!isApprox(original_shape, inflated_shape, tol));
130
131	✓✗	28	const auto &deflation_result = inflated_shape.inflated(-inflation);
132		14	const Shape &deflated_shape = deflation_result.first;
133		14	const Transform3f &deflation_shift = deflation_result.second;
134
135	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	14	BOOST_CHECK(isApprox(original_shape, deflated_shape, tol));
136	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	14	BOOST_CHECK((inflation_shift * deflation_shift).isIdentity(tol));
137			}
138
139			// Negative inflation
140			{
141	✓✗	28	const auto &inflation_result = original_shape.inflated(-inflation);
142		14	const Shape &inflated_shape = inflation_result.first;
143		14	const Transform3f &inflation_shift = inflation_result.second;
144
145	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	14	BOOST_CHECK(!isApprox(original_shape, inflated_shape, tol));
146
147	✓✗	28	const auto &deflation_result = inflated_shape.inflated(+inflation);
148		14	const Shape &deflated_shape = deflation_result.first;
149		14	const Transform3f &deflation_shift = deflation_result.second;
150
151	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	14	BOOST_CHECK(isApprox(original_shape, deflated_shape, tol));
152	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	14	BOOST_CHECK((inflation_shift * deflation_shift).isIdentity(tol));
153			}
154		14	}
155
156			template <typename Shape>
157		12	void test_throw(const Shape &shape, const FCL_REAL inflation) {
158	✓✗✓✗ ✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✓ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	36	BOOST_REQUIRE_THROW(shape.inflated(inflation), std::invalid_argument);
159		12	}
160
161			template <typename Shape>
162		1	void test_no_throw(const Shape &shape, const FCL_REAL inflation) {
163	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓✗✓ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗	1	BOOST_REQUIRE_NO_THROW(shape.inflated(inflation));
164		1	}
165
166	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE(test_inflate) {
167	✓✗	4	const hpp::fcl::Sphere sphere(1);
168	✓✗	2	test(sphere, 0.01, 1e-8);
169	✓✗	2	test_throw(sphere, -1.1);
170	✓✗	2	test_no_throw(sphere, 1.);
171
172	✓✗	4	const hpp::fcl::Box box(1, 1, 1);
173	✓✗	2	test(box, 0.01, 1e-8);
174	✓✗	2	test_throw(box, -0.6);
175
176	✓✗	4	const hpp::fcl::Ellipsoid ellipsoid(1, 2, 3);
177	✓✗	2	test(ellipsoid, 0.01, 1e-8);
178	✓✗	2	test_throw(ellipsoid, -1.1);
179
180	✓✗	4	const hpp::fcl::Capsule capsule(1., 2.);
181	✓✗	2	test(capsule, 0.01, 1e-8);
182	✓✗	2	test_throw(capsule, -1.1);
183
184	✓✗	4	const hpp::fcl::Cylinder cylinder(1., 2.);
185	✓✗	2	test(cylinder, 0.01, 1e-8);
186	✓✗	2	test_throw(cylinder, -1.1);
187
188	✓✗	4	const hpp::fcl::Cone cone(1., 4.);
189	✓✗	2	test(cone, 0.01, 1e-8);
190	✓✗	2	test_throw(cone, -1.1);
191
192	✓✗✓✗ ✓✗	4	const hpp::fcl::Halfspace halfspace(Vec3f::UnitZ(), 0.);
193	✓✗	2	test(halfspace, 0.01, 1e-8);
194
195			// const hpp::fcl::TriangleP triangle(Vec3f::UnitX(), Vec3f::UnitY(),
196			// Vec3f::UnitZ());
197			// test(triangle, 0.01, 1e-8);
198		2	}