GCC Code Coverage Report

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			/** \author Jia Pan */
37
38			#ifndef COAL_RSS_H
39			#define COAL_RSS_H
40
41			#include "coal/data_types.h"
42
43			#include <boost/math/constants/constants.hpp>
44
45			namespace coal {
46
47			struct CollisionRequest;
48
49			/// @addtogroup Bounding_Volume
50			/// @{
51
52			/// @brief A class for rectangle sphere-swept bounding volume
53			struct COAL_DLLAPI RSS {
54			EIGEN_MAKE_ALIGNED_OPERATOR_NEW
55
56			/// @brief Orientation of RSS. axis[i] is the ith column of the orientation
57			/// matrix for the RSS; it is also the i-th principle direction of the RSS. We
58			/// assume that axis[0] corresponds to the axis with the longest length,
59			/// axis[1] corresponds to the shorter one and axis[2] corresponds to the
60			/// shortest one.
61			Matrix3s axes;
62
63			/// @brief Origin of the rectangle in RSS
64			Vec3s Tr;
65
66			/// @brief Side lengths of rectangle
67			Scalar length[2];
68
69			/// @brief Radius of sphere summed with rectangle to form RSS
70			Scalar radius;
71
72			/// @brief Default constructor with default values
73	2/4 ✓ Branch 2 taken 2795376 times. ✗ Branch 3 not taken. ✓ Branch 6 taken 2795376 times. ✗ Branch 7 not taken.	2795376	RSS() : axes(Matrix3s::Zero()), Tr(Vec3s::Zero()), radius(-1) {
74		2795376	length[0] = 0;
75		2795376	length[1] = 0;
76		2795376	}
77
78			/// @brief Equality operator
79		812109	bool operator==(const RSS& other) const {
80	1/2 ✓ Branch 2 taken 812109 times. ✗ Branch 3 not taken.	1624218	return axes == other.axes && Tr == other.Tr &&
81	3/6 ✓ Branch 0 taken 812109 times. ✗ Branch 1 not taken. ✓ Branch 2 taken 812109 times. ✗ Branch 3 not taken. ✓ Branch 4 taken 812109 times. ✗ Branch 5 not taken.	2436327	length[0] == other.length[0] && length[1] == other.length[1] &&
82	1/2 ✓ Branch 0 taken 812109 times. ✗ Branch 1 not taken.	1624218	radius == other.radius;
83			}
84
85			/// @brief Difference operator
86			bool operator!=(const RSS& other) const { return !(*this == other); }
87
88			/// @brief Check whether the RSS contains a point
89			bool contain(const Vec3s& p) const;
90
91			/// @brief Check collision between two RSS
92			bool overlap(const RSS& other) const;
93
94			/// Not implemented
95		15645	bool overlap(const RSS& other, const CollisionRequest&,
96			Scalar& sqrDistLowerBound) const {
97		15645	sqrDistLowerBound = std::numeric_limits<Scalar>::quiet_NaN();
98		15645	return overlap(other);
99			}
100
101			/// @brief the distance between two RSS; P and Q, if not NULL, return the
102			/// nearest points
103			Scalar distance(const RSS& other, Vec3s* P = NULL, Vec3s* Q = NULL) const;
104
105			/// @brief A simple way to merge the RSS and a point, not compact.
106			/// @todo This function may have some bug.
107			RSS& operator+=(const Vec3s& p);
108
109			/// @brief Merge the RSS and another RSS
110			inline RSS& operator+=(const RSS& other) {
111			this = this + other;
112			return *this;
113			}
114
115			/// @brief Return the merged RSS of current RSS and the other one
116			RSS operator+(const RSS& other) const;
117
118			/// @brief Size of the RSS (used in BV_Splitter to order two RSSs)
119		31542	inline Scalar size() const {
120		31542	return (std::sqrt(length[0] * length[0] + length[1] * length[1]) +
121		31542	2 * radius);
122			}
123
124			/// @brief The RSS center
125		98004	inline const Vec3s& center() const { return Tr; }
126
127			/// @brief Width of the RSS
128		✗	inline Scalar width() const { return length[0] + 2 * radius; }
129
130			/// @brief Height of the RSS
131		✗	inline Scalar height() const { return length[1] + 2 * radius; }
132
133			/// @brief Depth of the RSS
134		✗	inline Scalar depth() const { return 2 * radius; }
135
136			/// @brief Volume of the RSS
137			inline Scalar volume() const {
138			return (length[0] * length[1] * 2 * radius +
139			4 * boost::math::constants::pi<Scalar>() * radius * radius *
140			radius);
141			}
142
143			/// @brief Check collision between two RSS and return the overlap part.
144			/// For RSS, we return nothing, as the overlap part of two RSSs usually is not
145			/// a RSS.
146			bool overlap(const RSS& other, RSS& /overlap_part/) const {
147			return overlap(other);
148			}
149			};
150
151			/// @brief distance between two RSS bounding volumes
152			/// P and Q (optional return values) are the closest points in the rectangles,
153			/// not the RSS. But the direction P - Q is the correct direction for cloest
154			/// points Notice that P and Q are both in the local frame of the first RSS (not
155			/// global frame and not even the local frame of object 1)
156			COAL_DLLAPI Scalar distance(const Matrix3s& R0, const Vec3s& T0, const RSS& b1,
157			const RSS& b2, Vec3s* P = NULL, Vec3s* Q = NULL);
158
159			/// @brief Check collision between two RSSs, b1 is in configuration (R0, T0) and
160			/// b2 is in identity.
161			COAL_DLLAPI bool overlap(const Matrix3s& R0, const Vec3s& T0, const RSS& b1,
162			const RSS& b2);
163
164			/// @brief Check collision between two RSSs, b1 is in configuration (R0, T0) and
165			/// b2 is in identity.
166			COAL_DLLAPI bool overlap(const Matrix3s& R0, const Vec3s& T0, const RSS& b1,
167			const RSS& b2, const CollisionRequest& request,
168			Scalar& sqrDistLowerBound);
169
170			} // namespace coal
171
172			#endif
173