Directory: | ./ |
---|---|
File: | include/coal/BV/kIOS.h |
Date: | 2025-04-01 09:23:31 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 15 | 26 | 57.7% |
Branches: | 14 | 40 | 35.0% |
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_KIOS_H | ||
39 | #define COAL_KIOS_H | ||
40 | |||
41 | #include "coal/BV/OBB.h" | ||
42 | |||
43 | namespace coal { | ||
44 | |||
45 | struct CollisionRequest; | ||
46 | |||
47 | /// @addtogroup Bounding_Volume | ||
48 | /// @{ | ||
49 | |||
50 | /// @brief A class describing the kIOS collision structure, which is a set of | ||
51 | /// spheres. | ||
52 | class COAL_DLLAPI kIOS { | ||
53 | /// @brief One sphere in kIOS | ||
54 | struct COAL_DLLAPI kIOS_Sphere { | ||
55 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW | ||
56 | |||
57 | Vec3s o; | ||
58 | Scalar r; | ||
59 | |||
60 | ✗ | bool operator==(const kIOS_Sphere& other) const { | |
61 | ✗ | return o == other.o && r == other.r; | |
62 | } | ||
63 | |||
64 | ✗ | bool operator!=(const kIOS_Sphere& other) const { | |
65 | ✗ | return !(*this == other); | |
66 | } | ||
67 | }; | ||
68 | |||
69 | /// @brief generate one sphere enclosing two spheres | ||
70 | 19611 | static kIOS_Sphere encloseSphere(const kIOS_Sphere& s0, | |
71 | const kIOS_Sphere& s1) { | ||
72 |
2/4✓ Branch 1 taken 19611 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 19611 times.
✗ Branch 5 not taken.
|
19611 | Vec3s d = s1.o - s0.o; |
73 |
1/2✓ Branch 1 taken 19611 times.
✗ Branch 2 not taken.
|
19611 | Scalar dist2 = d.squaredNorm(); |
74 | 19611 | Scalar diff_r = s1.r - s0.r; | |
75 | |||
76 | /** The sphere with the larger radius encloses the other */ | ||
77 |
2/2✓ Branch 0 taken 2533 times.
✓ Branch 1 taken 17078 times.
|
19611 | if (diff_r * diff_r >= dist2) { |
78 |
2/2✓ Branch 0 taken 1224 times.
✓ Branch 1 taken 1309 times.
|
2533 | if (s1.r > s0.r) |
79 |
1/2✓ Branch 1 taken 1224 times.
✗ Branch 2 not taken.
|
1224 | return s1; |
80 | else | ||
81 |
1/2✓ Branch 1 taken 1309 times.
✗ Branch 2 not taken.
|
1309 | return s0; |
82 | } else /** spheres partially overlapping or disjoint */ | ||
83 | { | ||
84 | 17078 | float dist = (float)std::sqrt(dist2); | |
85 |
1/2✓ Branch 1 taken 17078 times.
✗ Branch 2 not taken.
|
17078 | kIOS_Sphere s; |
86 | 17078 | s.r = dist + s0.r + s1.r; | |
87 |
1/2✓ Branch 0 taken 17078 times.
✗ Branch 1 not taken.
|
17078 | if (dist > 0) |
88 |
3/6✓ Branch 1 taken 17078 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 17078 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 17078 times.
✗ Branch 8 not taken.
|
17078 | s.o = s0.o + d * ((s.r - s0.r) / dist); |
89 | else | ||
90 | ✗ | s.o = s0.o; | |
91 | 17078 | return s; | |
92 | } | ||
93 | } | ||
94 | |||
95 | public: | ||
96 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW | ||
97 | |||
98 | /// @brief Equality operator | ||
99 | ✗ | bool operator==(const kIOS& other) const { | |
100 | ✗ | bool res = obb == other.obb && num_spheres == other.num_spheres; | |
101 | ✗ | if (!res) return false; | |
102 | |||
103 | ✗ | for (size_t k = 0; k < num_spheres; ++k) { | |
104 | ✗ | if (spheres[k] != other.spheres[k]) return false; | |
105 | } | ||
106 | |||
107 | ✗ | return true; | |
108 | } | ||
109 | |||
110 | /// @brief Difference operator | ||
111 | bool operator!=(const kIOS& other) const { return !(*this == other); } | ||
112 | |||
113 | static constexpr size_t max_num_spheres = 5; | ||
114 | |||
115 | /// @brief The (at most) five spheres for intersection | ||
116 | kIOS_Sphere spheres[max_num_spheres]; | ||
117 | |||
118 | /// @brief The number of spheres, no larger than 5 | ||
119 | unsigned int num_spheres; | ||
120 | |||
121 | /// @ OBB related with kIOS | ||
122 | OBB obb; | ||
123 | |||
124 | /// @brief Check whether the kIOS contains a point | ||
125 | bool contain(const Vec3s& p) const; | ||
126 | |||
127 | /// @brief Check collision between two kIOS | ||
128 | bool overlap(const kIOS& other) const; | ||
129 | |||
130 | /// @brief Check collision between two kIOS | ||
131 | bool overlap(const kIOS& other, const CollisionRequest&, | ||
132 | Scalar& sqrDistLowerBound) const; | ||
133 | |||
134 | /// @brief The distance between two kIOS | ||
135 | Scalar distance(const kIOS& other, Vec3s* P = NULL, Vec3s* Q = NULL) const; | ||
136 | |||
137 | /// @brief A simple way to merge the kIOS and a point | ||
138 | kIOS& operator+=(const Vec3s& p); | ||
139 | |||
140 | /// @brief Merge the kIOS and another kIOS | ||
141 | kIOS& operator+=(const kIOS& other) { | ||
142 | *this = *this + other; | ||
143 | return *this; | ||
144 | } | ||
145 | |||
146 | /// @brief Return the merged kIOS of current kIOS and the other one | ||
147 | kIOS operator+(const kIOS& other) const; | ||
148 | |||
149 | /// @brief size of the kIOS (used in BV_Splitter to order two kIOSs) | ||
150 | Scalar size() const; | ||
151 | |||
152 | /// @brief Center of the kIOS | ||
153 | 42248 | const Vec3s& center() const { return spheres[0].o; } | |
154 | |||
155 | /// @brief Width of the kIOS | ||
156 | Scalar width() const; | ||
157 | |||
158 | /// @brief Height of the kIOS | ||
159 | Scalar height() const; | ||
160 | |||
161 | /// @brief Depth of the kIOS | ||
162 | Scalar depth() const; | ||
163 | |||
164 | /// @brief Volume of the kIOS | ||
165 | Scalar volume() const; | ||
166 | }; | ||
167 | |||
168 | /// @brief Translate the kIOS BV | ||
169 | COAL_DLLAPI kIOS translate(const kIOS& bv, const Vec3s& t); | ||
170 | |||
171 | /// @brief Check collision between two kIOSs, b1 is in configuration (R0, T0) | ||
172 | /// and b2 is in identity. | ||
173 | /// @todo Not efficient | ||
174 | COAL_DLLAPI bool overlap(const Matrix3s& R0, const Vec3s& T0, const kIOS& b1, | ||
175 | const kIOS& b2); | ||
176 | |||
177 | /// @brief Check collision between two kIOSs, b1 is in configuration (R0, T0) | ||
178 | /// and b2 is in identity. | ||
179 | /// @todo Not efficient | ||
180 | COAL_DLLAPI bool overlap(const Matrix3s& R0, const Vec3s& T0, const kIOS& b1, | ||
181 | const kIOS& b2, const CollisionRequest& request, | ||
182 | Scalar& sqrDistLowerBound); | ||
183 | |||
184 | /// @brief Approximate distance between two kIOS bounding volumes | ||
185 | /// @todo P and Q is not returned, need implementation | ||
186 | COAL_DLLAPI Scalar distance(const Matrix3s& R0, const Vec3s& T0, const kIOS& b1, | ||
187 | const kIOS& b2, Vec3s* P = NULL, Vec3s* Q = NULL); | ||
188 | |||
189 | } // namespace coal | ||
190 | |||
191 | #endif | ||
192 |