GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
Line | Branch | Exec | Source |
1 |
/* |
||
2 |
* Software License Agreement (BSD License) |
||
3 |
* Copyright (c) 2015-2021, CNRS-LAAS INRIA |
||
4 |
* All rights reserved. |
||
5 |
* |
||
6 |
* Redistribution and use in source and binary forms, with or without |
||
7 |
* modification, are permitted provided that the following conditions |
||
8 |
* are met: |
||
9 |
* |
||
10 |
* * Redistributions of source code must retain the above copyright |
||
11 |
* notice, this list of conditions and the following disclaimer. |
||
12 |
* * Redistributions in binary form must reproduce the above |
||
13 |
* copyright notice, this list of conditions and the following |
||
14 |
* disclaimer in the documentation and/or other materials provided |
||
15 |
* with the distribution. |
||
16 |
* * Neither the name of Open Source Robotics Foundation nor the names of its |
||
17 |
* contributors may be used to endorse or promote products derived |
||
18 |
* from this software without specific prior written permission. |
||
19 |
* |
||
20 |
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
||
21 |
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT |
||
22 |
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS |
||
23 |
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE |
||
24 |
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, |
||
25 |
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, |
||
26 |
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; |
||
27 |
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER |
||
28 |
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT |
||
29 |
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN |
||
30 |
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
||
31 |
* POSSIBILITY OF SUCH DAMAGE. |
||
32 |
*/ |
||
33 |
|||
34 |
#include <cmath> |
||
35 |
#include <limits> |
||
36 |
#include <hpp/fcl/math/transform.h> |
||
37 |
#include <hpp/fcl/shape/geometric_shapes.h> |
||
38 |
#include <hpp/fcl/internal/shape_shape_func.h> |
||
39 |
|||
40 |
// Note that partial specialization of template functions is not allowed. |
||
41 |
// Therefore, two implementations with the default narrow phase solvers are |
||
42 |
// provided. If another narrow phase solver were to be used, the default |
||
43 |
// template implementation would be called, unless the function is also |
||
44 |
// specialized for this new type. |
||
45 |
// |
||
46 |
// One solution would be to make narrow phase solvers derive from an abstract |
||
47 |
// class and specialize the template for this abstract class. |
||
48 |
namespace hpp { |
||
49 |
namespace fcl { |
||
50 |
struct GJKSolver; |
||
51 |
|||
52 |
/// Clamp num / denom in [0, 1] |
||
53 |
2 |
FCL_REAL clamp(const FCL_REAL& num, const FCL_REAL& denom) { |
|
54 |
✗✓ | 2 |
assert(denom >= 0.); |
55 |
✗✓ | 2 |
if (num <= 0.) |
56 |
return 0.; |
||
57 |
✓✗ | 2 |
else if (num >= denom) |
58 |
2 |
return 1.; |
|
59 |
else |
||
60 |
return num / denom; |
||
61 |
} |
||
62 |
|||
63 |
/// Clamp s=s_n/s_d in [0, 1] and stores a + s * d in a_sd |
||
64 |
3005 |
void clamped_linear(Vec3f& a_sd, const Vec3f& a, const FCL_REAL& s_n, |
|
65 |
const FCL_REAL& s_d, const Vec3f& d) { |
||
66 |
✗✓ | 3005 |
assert(s_d >= 0.); |
67 |
✓✓ | 3005 |
if (s_n <= 0.) |
68 |
28 |
a_sd = a; |
|
69 |
✓✓ | 2977 |
else if (s_n >= s_d) |
70 |
✓✗ | 1041 |
a_sd = a + d; |
71 |
else |
||
72 |
✓✗✓✗ ✓✗ |
1936 |
a_sd = a + s_n / s_d * d; |
73 |
3005 |
} |
|
74 |
|||
75 |
// Compute the distance between C1 and C2 by computing the distance |
||
76 |
// between the two segments supporting the capsules. |
||
77 |
// Match algorithm of Real-Time Collision Detection, Christer Ericson - Closest |
||
78 |
// Point of Two Line Segments |
||
79 |
template <> |
||
80 |
5012 |
FCL_REAL ShapeShapeDistance<Capsule, Capsule>( |
|
81 |
const CollisionGeometry* o1, const Transform3f& tf1, |
||
82 |
const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, |
||
83 |
const DistanceRequest& request, DistanceResult& result) { |
||
84 |
5012 |
const Capsule* capsule1 = static_cast<const Capsule*>(o1); |
|
85 |
5012 |
const Capsule* capsule2 = static_cast<const Capsule*>(o2); |
|
86 |
|||
87 |
5012 |
FCL_REAL EPSILON = std::numeric_limits<FCL_REAL>::epsilon() * 100; |
|
88 |
|||
89 |
// We assume that capsules are centered at the origin. |
||
90 |
5012 |
const fcl::Vec3f& c1 = tf1.getTranslation(); |
|
91 |
5012 |
const fcl::Vec3f& c2 = tf2.getTranslation(); |
|
92 |
// We assume that capsules are oriented along z-axis. |
||
93 |
5012 |
FCL_REAL halfLength1 = capsule1->halfLength; |
|
94 |
5012 |
FCL_REAL halfLength2 = capsule2->halfLength; |
|
95 |
5012 |
FCL_REAL radius1 = capsule1->radius; |
|
96 |
5012 |
FCL_REAL radius2 = capsule2->radius; |
|
97 |
// direction of capsules |
||
98 |
// ||d1|| = 2 * halfLength1 |
||
99 |
✓✗✓✗ ✓✗ |
5012 |
const fcl::Vec3f d1 = 2 * halfLength1 * tf1.getRotation().col(2); |
100 |
✓✗✓✗ ✓✗ |
5012 |
const fcl::Vec3f d2 = 2 * halfLength2 * tf2.getRotation().col(2); |
101 |
|||
102 |
// Starting point of the segments |
||
103 |
// p1 + d1 is the end point of the segment |
||
104 |
✓✗✓✗ ✓✗ |
5012 |
const fcl::Vec3f p1 = c1 - d1 / 2; |
105 |
✓✗✓✗ ✓✗ |
5012 |
const fcl::Vec3f p2 = c2 - d2 / 2; |
106 |
✓✗✓✗ |
5012 |
const fcl::Vec3f r = p1 - p2; |
107 |
✓✗ | 5012 |
FCL_REAL a = d1.dot(d1); |
108 |
✓✗ | 5012 |
FCL_REAL b = d1.dot(d2); |
109 |
✓✗ | 5012 |
FCL_REAL c = d1.dot(r); |
110 |
✓✗ | 5012 |
FCL_REAL e = d2.dot(d2); |
111 |
✓✗ | 5012 |
FCL_REAL f = d2.dot(r); |
112 |
// S1 is parametrized by the equation p1 + s * d1 |
||
113 |
// S2 is parametrized by the equation p2 + t * d2 |
||
114 |
|||
115 |
✓✗✓✗ |
5012 |
Vec3f w1, w2; |
116 |
✓✓ | 5012 |
if (a <= EPSILON) { |
117 |
✓✗ | 1000 |
w1 = p1; |
118 |
✓✗ | 1000 |
if (e <= EPSILON) |
119 |
// Check if the segments degenerate into points |
||
120 |
✓✗ | 1000 |
w2 = p2; |
121 |
else |
||
122 |
// First segment is degenerated |
||
123 |
clamped_linear(w2, p2, f, e, d2); |
||
124 |
✗✓ | 4012 |
} else if (e <= EPSILON) { |
125 |
// Second segment is degenerated |
||
126 |
clamped_linear(w1, p1, -c, a, d1); |
||
127 |
w2 = p2; |
||
128 |
} else { |
||
129 |
// Always non-negative, equal 0 if the segments are colinear |
||
130 |
4012 |
FCL_REAL denom = fmax(a * e - b * b, 0); |
|
131 |
|||
132 |
FCL_REAL s; |
||
133 |
FCL_REAL t; |
||
134 |
✓✓ | 4012 |
if (denom > EPSILON) { |
135 |
✓✗ | 2 |
s = clamp((b * f - c * e), denom); |
136 |
2 |
t = b * s + f; |
|
137 |
} else { |
||
138 |
4010 |
s = 0.; |
|
139 |
4010 |
t = f; |
|
140 |
} |
||
141 |
|||
142 |
✓✓ | 4012 |
if (t <= 0.0) { |
143 |
✓✗ | 2985 |
w2 = p2; |
144 |
✓✗ | 2985 |
clamped_linear(w1, p1, -c, a, d1); |
145 |
✓✓ | 1027 |
} else if (t >= e) { |
146 |
✓✗ | 20 |
clamped_linear(w1, p1, (b - c), a, d1); |
147 |
✓✗✓✗ |
20 |
w2 = p2 + d2; |
148 |
} else { |
||
149 |
✓✗✓✗ ✓✗ |
1007 |
w1 = p1 + s * d1; |
150 |
✓✗✓✗ ✓✗ |
1007 |
w2 = p2 + t / e * d2; |
151 |
} |
||
152 |
} |
||
153 |
|||
154 |
// witness points achieving the distance between the two segments |
||
155 |
✓✗✓✗ |
5012 |
FCL_REAL distance = (w1 - w2).norm(); |
156 |
✓✗✓✗ ✓✗ |
5012 |
const Vec3f normal = (w1 - w2) / distance; |
157 |
✓✗ | 5012 |
result.normal = normal; |
158 |
|||
159 |
// capsule spcecific distance computation |
||
160 |
5012 |
distance = distance - (radius1 + radius2); |
|
161 |
5012 |
result.min_distance = distance; |
|
162 |
|||
163 |
// witness points for the capsules |
||
164 |
✓✓ | 5012 |
if (request.enable_nearest_points) { |
165 |
✓✗✓✗ ✓✗ |
9 |
result.nearest_points[0] = w1 - radius1 * normal; |
166 |
✓✗✓✗ ✓✗ |
9 |
result.nearest_points[1] = w2 + radius2 * normal; |
167 |
} |
||
168 |
|||
169 |
5012 |
return distance; |
|
170 |
} |
||
171 |
|||
172 |
} // namespace fcl |
||
173 |
|||
174 |
} // namespace hpp |
Generated by: GCOVR (Version 4.2) |