Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	test/math.cpp	Lines:	70	70	100.0 %
Date:	2024-02-09 12:57:42	Branches:	393	786	50.0 %


/*
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2011-2014, Willow Garage, Inc.
 *  Copyright (c) 2014-2015, Open Source Robotics Foundation
 *  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 Open Source Robotics Foundation 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 _USE_MATH_DEFINES
#include <cmath>

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

#include <hpp/fcl/data_types.h>
#include <hpp/fcl/math/transform.h>

#include <hpp/fcl/internal/intersect.h>
#include <hpp/fcl/internal/tools.h>

using namespace hpp::fcl;

BOOST_AUTO_TEST_CASE(vec_test_eigen_vec64) {
  Vec3f v1(1.0, 2.0, 3.0);
  BOOST_CHECK(v1[0] == 1.0);
  BOOST_CHECK(v1[1] == 2.0);
  BOOST_CHECK(v1[2] == 3.0);

  Vec3f v2 = v1;
  Vec3f v3(3.3, 4.3, 5.3);
  v1 += v3;
  BOOST_CHECK(isEqual(v1, v2 + v3));
  v1 -= v3;
  BOOST_CHECK(isEqual(v1, v2));
  v1 -= v3;
  BOOST_CHECK(isEqual(v1, v2 - v3));
  v1 += v3;

  v1.array() *= v3.array();
  BOOST_CHECK(isEqual(v1, v2.cwiseProduct(v3)));
  v1.array() /= v3.array();
  BOOST_CHECK(isEqual(v1, v2));
  v1.array() /= v3.array();
  BOOST_CHECK(isEqual(v1, v2.cwiseQuotient(v3)));
  v1.array() *= v3.array();

  v1 *= 2.0;
  BOOST_CHECK(isEqual(v1, v2 * 2.0));
  v1 /= 2.0;
  BOOST_CHECK(isEqual(v1, v2));
  v1 /= 2.0;
  BOOST_CHECK(isEqual(v1, v2 / 2.0));
  v1 *= 2.0;

  v1.array() += 2.0;
  BOOST_CHECK(isEqual(v1, (v2.array() + 2.0).matrix()));
  v1.array() -= 2.0;
  BOOST_CHECK(isEqual(v1, v2));
  v1.array() -= 2.0;
  BOOST_CHECK(isEqual(v1, (v2.array() - 2.0).matrix()));
  v1.array() += 2.0;

  BOOST_CHECK(isEqual((-Vec3f(1.0, 2.0, 3.0)), Vec3f(-1.0, -2.0, -3.0)));

  v1 = Vec3f(1.0, 2.0, 3.0);
  v2 = Vec3f(3.0, 4.0, 5.0);
  BOOST_CHECK(isEqual((v1.cross(v2)), Vec3f(-2.0, 4.0, -2.0)));
  BOOST_CHECK(std::abs(v1.dot(v2) - 26) < 1e-5);

  v1 = Vec3f(3.0, 4.0, 5.0);
  BOOST_CHECK(std::abs(v1.squaredNorm() - 50) < 1e-5);
  BOOST_CHECK(std::abs(v1.norm() - sqrt(50)) < 1e-5);
  BOOST_CHECK(isEqual(v1.normalized(), v1 / v1.norm()));

  v1 = Vec3f(1.0, 2.0, 3.0);
  v2 = Vec3f(3.0, 4.0, 5.0);
  BOOST_CHECK(isEqual(v1.cross(v2), Vec3f(-2.0, 4.0, -2.0)));
  BOOST_CHECK(v1.dot(v2) == 26);
}

Vec3f rotate(Vec3f input, FCL_REAL w, Vec3f vec) {
  return 2 * vec.dot(input) * vec + (w * w - vec.dot(vec)) * input +
         2 * w * vec.cross(input);
}

BOOST_AUTO_TEST_CASE(quaternion) {
  Quaternion3f q1(Quaternion3f::Identity()), q2, q3;
  q2 = fromAxisAngle(Vec3f(0, 0, 1), M_PI / 2);
  q3 = q2.inverse();

  Vec3f v(1, -1, 0);

  BOOST_CHECK(isEqual(v, q1 * v));
  BOOST_CHECK(isEqual(Vec3f(1, 1, 0), q2 * v));
  BOOST_CHECK(
      isEqual(rotate(v, q3.w(), Vec3f(q3.x(), q3.y(), q3.z())), q3 * v));
}

BOOST_AUTO_TEST_CASE(transform) {
  Quaternion3f q = fromAxisAngle(Vec3f(0, 0, 1), M_PI / 2);
  Vec3f T(0, 1, 2);
  Transform3f tf(q, T);

  Vec3f v(1, -1, 0);

  BOOST_CHECK(isEqual(q * v + T, q * v + T));

  Vec3f rv(q * v);
  // typename Transform3f::transform_return_type<Vec3f>::type output =
  // tf * v;
  // std::cout << rv << std::endl;
  // std::cout << output.lhs() << std::endl;
  BOOST_CHECK(isEqual(rv + T, tf.transform(v)));
}


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			/*
2			* Software License Agreement (BSD License)
3			*
4			* Copyright (c) 2011-2014, Willow Garage, Inc.
5			* Copyright (c) 2014-2015, Open Source Robotics Foundation
6			* All rights reserved.
7			*
8			* Redistribution and use in source and binary forms, with or without
9			* modification, are permitted provided that the following conditions
10			* are met:
11			*
12			* * Redistributions of source code must retain the above copyright
13			* notice, this list of conditions and the following disclaimer.
14			* * Redistributions in binary form must reproduce the above
15			* copyright notice, this list of conditions and the following
16			* disclaimer in the documentation and/or other materials provided
17			* with the distribution.
18			* * Neither the name of Open Source Robotics Foundation nor the names of its
19			* contributors may be used to endorse or promote products derived
20			* from this software without specific prior written permission.
21			*
22			* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23			* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24			* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25			* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26			* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27			* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28			* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29			* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30			* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31			* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32			* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33			* POSSIBILITY OF SUCH DAMAGE.
34			*/
35
36			#define _USE_MATH_DEFINES
37			#include <cmath>
38
39			#define BOOST_TEST_MODULE FCL_MATH
40			#include <boost/test/included/unit_test.hpp>
41
42			#include <hpp/fcl/data_types.h>
43			#include <hpp/fcl/math/transform.h>
44
45			#include <hpp/fcl/internal/intersect.h>
46			#include <hpp/fcl/internal/tools.h>
47
48			using namespace hpp::fcl;
49
50	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE(vec_test_eigen_vec64) {
51	✓✗	2	Vec3f v1(1.0, 2.0, 3.0);
52	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(v1[0] == 1.0);
53	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(v1[1] == 2.0);
54	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(v1[2] == 3.0);
55
56	✓✗	2	Vec3f v2 = v1;
57	✓✗	2	Vec3f v3(3.3, 4.3, 5.3);
58	✓✗	2	v1 += v3;
59	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	2	BOOST_CHECK(isEqual(v1, v2 + v3));
60	✓✗	2	v1 -= v3;
61	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(isEqual(v1, v2));
62	✓✗	2	v1 -= v3;
63	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	2	BOOST_CHECK(isEqual(v1, v2 - v3));
64	✓✗	2	v1 += v3;
65
66	✓✗✓✗ ✓✗	2	v1.array() *= v3.array();
67	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	2	BOOST_CHECK(isEqual(v1, v2.cwiseProduct(v3)));
68	✓✗✓✗ ✓✗	2	v1.array() /= v3.array();
69	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(isEqual(v1, v2));
70	✓✗✓✗ ✓✗	2	v1.array() /= v3.array();
71	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	2	BOOST_CHECK(isEqual(v1, v2.cwiseQuotient(v3)));
72	✓✗✓✗ ✓✗	2	v1.array() *= v3.array();
73
74	✓✗	2	v1 *= 2.0;
75	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	2	BOOST_CHECK(isEqual(v1, v2 * 2.0));
76	✓✗	2	v1 /= 2.0;
77	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(isEqual(v1, v2));
78	✓✗	2	v1 /= 2.0;
79	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	2	BOOST_CHECK(isEqual(v1, v2 / 2.0));
80	✓✗	2	v1 *= 2.0;
81
82	✓✗✓✗	2	v1.array() += 2.0;
83	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	2	BOOST_CHECK(isEqual(v1, (v2.array() + 2.0).matrix()));
84	✓✗✓✗	2	v1.array() -= 2.0;
85	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(isEqual(v1, v2));
86	✓✗✓✗	2	v1.array() -= 2.0;
87	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	2	BOOST_CHECK(isEqual(v1, (v2.array() - 2.0).matrix()));
88	✓✗✓✗	2	v1.array() += 2.0;
89
90	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	2	BOOST_CHECK(isEqual((-Vec3f(1.0, 2.0, 3.0)), Vec3f(-1.0, -2.0, -3.0)));
91
92	✓✗	2	v1 = Vec3f(1.0, 2.0, 3.0);
93	✓✗	2	v2 = Vec3f(3.0, 4.0, 5.0);
94	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(isEqual((v1.cross(v2)), Vec3f(-2.0, 4.0, -2.0)));
95	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(std::abs(v1.dot(v2) - 26) < 1e-5);
96
97	✓✗	2	v1 = Vec3f(3.0, 4.0, 5.0);
98	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(std::abs(v1.squaredNorm() - 50) < 1e-5);
99	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(std::abs(v1.norm() - sqrt(50)) < 1e-5);
100	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	2	BOOST_CHECK(isEqual(v1.normalized(), v1 / v1.norm()));
101
102	✓✗	2	v1 = Vec3f(1.0, 2.0, 3.0);
103	✓✗	2	v2 = Vec3f(3.0, 4.0, 5.0);
104	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(isEqual(v1.cross(v2), Vec3f(-2.0, 4.0, -2.0)));
105	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(v1.dot(v2) == 26);
106		2	}
107
108		1	Vec3f rotate(Vec3f input, FCL_REAL w, Vec3f vec) {
109	✓✗✓✗ ✓✗✓✗ ✓✗✓✗	1	return 2 * vec.dot(input) * vec + (w * w - vec.dot(vec)) * input +
110	✓✗✓✗	3	2 * w * vec.cross(input);
111			}
112
113	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE(quaternion) {
114	✓✗✓✗ ✓✗	2	Quaternion3f q1(Quaternion3f::Identity()), q2, q3;
115	✓✗✓✗ ✓✗	2	q2 = fromAxisAngle(Vec3f(0, 0, 1), M_PI / 2);
116	✓✗✓✗	2	q3 = q2.inverse();
117
118	✓✗	2	Vec3f v(1, -1, 0);
119
120	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	2	BOOST_CHECK(isEqual(v, q1 * v));
121	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(isEqual(Vec3f(1, 1, 0), q2 * v));
122	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(
123			isEqual(rotate(v, q3.w(), Vec3f(q3.x(), q3.y(), q3.z())), q3 * v));
124		2	}
125
126	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE(transform) {
127	✓✗✓✗	2	Quaternion3f q = fromAxisAngle(Vec3f(0, 0, 1), M_PI / 2);
128	✓✗	2	Vec3f T(0, 1, 2);
129	✓✗	2	Transform3f tf(q, T);
130
131	✓✗	2	Vec3f v(1, -1, 0);
132
133	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(isEqual(q * v + T, q * v + T));
134
135	✓✗	2	Vec3f rv(q * v);
136			// typename Transform3f::transform_return_type<Vec3f>::type output =
137			// tf * v;
138			// std::cout << rv << std::endl;
139			// std::cout << output.lhs() << std::endl;
140	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(isEqual(rv + T, tf.transform(v)));
141		2	}