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 |
#include <hpp/fcl/BV/kIOS.h> |
||
39 |
#include <hpp/fcl/BVH/BVH_utility.h> |
||
40 |
#include <hpp/fcl/math/transform.h> |
||
41 |
|||
42 |
#include <iostream> |
||
43 |
#include <limits> |
||
44 |
|||
45 |
namespace hpp { |
||
46 |
namespace fcl { |
||
47 |
|||
48 |
bool kIOS::overlap(const kIOS& other) const { |
||
49 |
for (unsigned int i = 0; i < num_spheres; ++i) { |
||
50 |
for (unsigned int j = 0; j < other.num_spheres; ++j) { |
||
51 |
FCL_REAL o_dist = (spheres[i].o - other.spheres[j].o).squaredNorm(); |
||
52 |
FCL_REAL sum_r = spheres[i].r + other.spheres[j].r; |
||
53 |
if (o_dist > sum_r * sum_r) return false; |
||
54 |
} |
||
55 |
} |
||
56 |
|||
57 |
return obb.overlap(other.obb); |
||
58 |
} |
||
59 |
|||
60 |
2974 |
bool kIOS::overlap(const kIOS& other, const CollisionRequest& request, |
|
61 |
FCL_REAL& sqrDistLowerBound) const { |
||
62 |
✓✓ | 12856 |
for (unsigned int i = 0; i < num_spheres; ++i) { |
63 |
✓✓ | 52474 |
for (unsigned int j = 0; j < other.num_spheres; ++j) { |
64 |
✓✗ | 42592 |
FCL_REAL o_dist = (spheres[i].o - other.spheres[j].o).squaredNorm(); |
65 |
42592 |
FCL_REAL sum_r = spheres[i].r + other.spheres[j].r; |
|
66 |
✓✓ | 42592 |
if (o_dist > sum_r * sum_r) { |
67 |
211 |
o_dist = sqrt(o_dist) - sum_r; |
|
68 |
211 |
sqrDistLowerBound = o_dist * o_dist; |
|
69 |
211 |
return false; |
|
70 |
} |
||
71 |
} |
||
72 |
} |
||
73 |
|||
74 |
2763 |
return obb.overlap(other.obb, request, sqrDistLowerBound); |
|
75 |
} |
||
76 |
|||
77 |
bool kIOS::contain(const Vec3f& p) const { |
||
78 |
for (unsigned int i = 0; i < num_spheres; ++i) { |
||
79 |
FCL_REAL r = spheres[i].r; |
||
80 |
if ((spheres[i].o - p).squaredNorm() > r * r) return false; |
||
81 |
} |
||
82 |
|||
83 |
return true; |
||
84 |
} |
||
85 |
|||
86 |
kIOS& kIOS::operator+=(const Vec3f& p) { |
||
87 |
for (unsigned int i = 0; i < num_spheres; ++i) { |
||
88 |
FCL_REAL r = spheres[i].r; |
||
89 |
FCL_REAL new_r_sqr = (p - spheres[i].o).squaredNorm(); |
||
90 |
if (new_r_sqr > r * r) { |
||
91 |
spheres[i].r = sqrt(new_r_sqr); |
||
92 |
} |
||
93 |
} |
||
94 |
|||
95 |
obb += p; |
||
96 |
return *this; |
||
97 |
} |
||
98 |
|||
99 |
6537 |
kIOS kIOS::operator+(const kIOS& other) const { |
|
100 |
6537 |
kIOS result; |
|
101 |
6537 |
unsigned int new_num_spheres = std::min(num_spheres, other.num_spheres); |
|
102 |
✓✓ | 26148 |
for (unsigned int i = 0; i < new_num_spheres; ++i) { |
103 |
19611 |
result.spheres[i] = encloseSphere(spheres[i], other.spheres[i]); |
|
104 |
} |
||
105 |
|||
106 |
6537 |
result.num_spheres = new_num_spheres; |
|
107 |
|||
108 |
6537 |
result.obb = obb + other.obb; |
|
109 |
|||
110 |
6537 |
return result; |
|
111 |
} |
||
112 |
|||
113 |
FCL_REAL kIOS::width() const { return obb.width(); } |
||
114 |
|||
115 |
FCL_REAL kIOS::height() const { return obb.height(); } |
||
116 |
|||
117 |
FCL_REAL kIOS::depth() const { return obb.depth(); } |
||
118 |
|||
119 |
7592 |
FCL_REAL kIOS::volume() const { return obb.volume(); } |
|
120 |
|||
121 |
7592 |
FCL_REAL kIOS::size() const { return volume(); } |
|
122 |
|||
123 |
3106 |
FCL_REAL kIOS::distance(const kIOS& other, Vec3f* P, Vec3f* Q) const { |
|
124 |
3106 |
FCL_REAL d_max = 0; |
|
125 |
3106 |
long id_a = -1, id_b = -1; |
|
126 |
✓✓ | 18038 |
for (unsigned int i = 0; i < num_spheres; ++i) { |
127 |
✓✓ | 79164 |
for (unsigned int j = 0; j < other.num_spheres; ++j) { |
128 |
✓✗ | 64232 |
FCL_REAL d = (spheres[i].o - other.spheres[j].o).norm() - |
129 |
64232 |
(spheres[i].r + other.spheres[j].r); |
|
130 |
✓✓ | 64232 |
if (d_max < d) { |
131 |
2271 |
d_max = d; |
|
132 |
✗✓✗✗ |
2271 |
if (P && Q) { |
133 |
id_a = (long)i; |
||
134 |
id_b = (long)j; |
||
135 |
} |
||
136 |
} |
||
137 |
} |
||
138 |
} |
||
139 |
|||
140 |
✗✓✗✗ |
3106 |
if (P && Q) { |
141 |
if (id_a != -1 && id_b != -1) { |
||
142 |
const Vec3f v = spheres[id_a].o - spheres[id_b].o; |
||
143 |
FCL_REAL len_v = v.norm(); |
||
144 |
*P = spheres[id_a].o - v * (spheres[id_a].r / len_v); |
||
145 |
*Q = spheres[id_b].o + v * (spheres[id_b].r / len_v); |
||
146 |
} |
||
147 |
} |
||
148 |
|||
149 |
3106 |
return d_max; |
|
150 |
} |
||
151 |
|||
152 |
bool overlap(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1, |
||
153 |
const kIOS& b2) { |
||
154 |
kIOS b2_temp = b2; |
||
155 |
for (unsigned int i = 0; i < b2_temp.num_spheres; ++i) { |
||
156 |
b2_temp.spheres[i].o.noalias() = |
||
157 |
R0.transpose() * (b2_temp.spheres[i].o - T0); |
||
158 |
} |
||
159 |
|||
160 |
b2_temp.obb.To.noalias() = R0.transpose() * (b2_temp.obb.To - T0); |
||
161 |
b2_temp.obb.axes.applyOnTheLeft(R0.transpose()); |
||
162 |
|||
163 |
return b1.overlap(b2_temp); |
||
164 |
} |
||
165 |
|||
166 |
1648 |
bool overlap(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1, |
|
167 |
const kIOS& b2, const CollisionRequest& request, |
||
168 |
FCL_REAL& sqrDistLowerBound) { |
||
169 |
✓✗ | 1648 |
kIOS b2_temp = b2; |
170 |
✓✓ | 9368 |
for (unsigned int i = 0; i < b2_temp.num_spheres; ++i) { |
171 |
✓✗ | 7720 |
b2_temp.spheres[i].o.noalias() = |
172 |
✓✗✓✗ ✓✗✓✗ |
15440 |
R0.transpose() * (b2_temp.spheres[i].o - T0); |
173 |
} |
||
174 |
|||
175 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
1648 |
b2_temp.obb.To.noalias() = R0.transpose() * (b2_temp.obb.To - T0); |
176 |
✓✗✓✗ |
1648 |
b2_temp.obb.axes.applyOnTheLeft(R0.transpose()); |
177 |
|||
178 |
✓✗ | 3296 |
return b1.overlap(b2_temp, request, sqrDistLowerBound); |
179 |
} |
||
180 |
|||
181 |
1292 |
FCL_REAL distance(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1, |
|
182 |
const kIOS& b2, Vec3f* P, Vec3f* Q) { |
||
183 |
✓✗ | 1292 |
kIOS b2_temp = b2; |
184 |
✓✓ | 6398 |
for (unsigned int i = 0; i < b2_temp.num_spheres; ++i) { |
185 |
✓✗✓✗ ✓✗ |
5106 |
b2_temp.spheres[i].o = R0 * b2_temp.spheres[i].o + T0; |
186 |
} |
||
187 |
|||
188 |
✓✗ | 2584 |
return b1.distance(b2_temp, P, Q); |
189 |
} |
||
190 |
|||
191 |
kIOS translate(const kIOS& bv, const Vec3f& t) { |
||
192 |
kIOS res(bv); |
||
193 |
for (size_t i = 0; i < res.num_spheres; ++i) { |
||
194 |
res.spheres[i].o += t; |
||
195 |
} |
||
196 |
|||
197 |
translate(res.obb, t); |
||
198 |
return res; |
||
199 |
} |
||
200 |
|||
201 |
} // namespace fcl |
||
202 |
|||
203 |
} // namespace hpp |
Generated by: GCOVR (Version 4.2) |