GCC Code Coverage Report

Directory: ./
File: src/distance/capsule_capsule.cpp
Date: 2025-04-01 09:23:31
Exec Total Coverage
Lines: 61 64 95.3%
Branches: 70 130 53.8%
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 "coal/math/transform.h"
35 #include "coal/shape/geometric_shapes.h"
36 #include "coal/internal/shape_shape_func.h"
37
38 #include "coal/tracy.hh"
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 coal {
49 struct GJKSolver;
50
51 namespace internal {
52 /// Clamp num / denom in [0, 1]
53 834 Scalar clamp(const Scalar& num, const Scalar& denom) {
54
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 834 times.
 834 assert(denom >= 0.);
55
2/2
✓ Branch 0 taken 366 times.
✓ Branch 1 taken 468 times.
 834 if (num <= 0.)
56 366 return 0.;
57
2/2
✓ Branch 0 taken 338 times.
✓ Branch 1 taken 130 times.
 468 else if (num >= denom)
58 338 return 1.;
59 else
60 130 return num / denom;
61 }
62
63 /// Clamp s=s_n/s_d in [0, 1] and stores a + s * d in a_sd
64 3653 void clamped_linear(Vec3s& a_sd, const Vec3s& a, const Scalar& s_n,
65 const Scalar& s_d, const Vec3s& d) {
66
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 3653 times.
 3653 assert(s_d >= 0.);
67
2/2
✓ Branch 0 taken 292 times.
✓ Branch 1 taken 3361 times.
 3653 if (s_n <= 0.)
68 292 a_sd = a;
69
2/2
✓ Branch 0 taken 1309 times.
✓ Branch 1 taken 2052 times.
 3361 else if (s_n >= s_d)
70
1/2
✓ Branch 2 taken 1309 times.
✗ Branch 3 not taken.
 1309 a_sd = a + d;
71 else
72
3/6
✓ Branch 1 taken 2052 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2052 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2052 times.
✗ Branch 8 not taken.
 2052 a_sd = a + s_n / s_d * d;
73 3653 }
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 /// @param wp1, wp2: witness points on the capsules
80 /// @param normal: normal pointing from capsule1 to capsule2
81 template <>
82 5844 Scalar ShapeShapeDistance<Capsule, Capsule>(
83 const CollisionGeometry* o1, const Transform3s& tf1,
84 const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*,
85 const bool, Vec3s& wp1, Vec3s& wp2, Vec3s& normal) {
86 COAL_TRACY_ZONE_SCOPED_N(
87 "coal::internal::ShapeShapeDistance<Capsule, Capsule>");
88 5844 const Capsule* capsule1 = static_cast<const Capsule*>(o1);
89 5844 const Capsule* capsule2 = static_cast<const Capsule*>(o2);
90
91 5844 Scalar EPSILON = std::numeric_limits<Scalar>::epsilon() * 100;
92
93 // We assume that capsules are centered at the origin.
94 5844 const coal::Vec3s& c1 = tf1.getTranslation();
95 5844 const coal::Vec3s& c2 = tf2.getTranslation();
96 // We assume that capsules are oriented along z-axis.
97 5844 Scalar halfLength1 = capsule1->halfLength;
98 5844 Scalar halfLength2 = capsule2->halfLength;
99 5844 Scalar radius1 = (capsule1->radius + capsule1->getSweptSphereRadius());
100 5844 Scalar radius2 = (capsule2->radius + capsule2->getSweptSphereRadius());
101 // direction of capsules
102 // ||d1|| = 2 * halfLength1
103
3/6
✓ Branch 2 taken 5844 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 5844 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 5844 times.
✗ Branch 9 not taken.
 5844 const coal::Vec3s d1 = 2 * halfLength1 * tf1.getRotation().col(2);
104
3/6
✓ Branch 2 taken 5844 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 5844 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 5844 times.
✗ Branch 9 not taken.
 5844 const coal::Vec3s d2 = 2 * halfLength2 * tf2.getRotation().col(2);
105
106 // Starting point of the segments
107 // p1 + d1 is the end point of the segment
108
3/6
✓ Branch 1 taken 5844 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5844 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5844 times.
✗ Branch 8 not taken.
 5844 const coal::Vec3s p1 = c1 - d1 / 2;
109
3/6
✓ Branch 1 taken 5844 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5844 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5844 times.
✗ Branch 8 not taken.
 5844 const coal::Vec3s p2 = c2 - d2 / 2;
110
2/4
✓ Branch 1 taken 5844 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5844 times.
✗ Branch 5 not taken.
 5844 const coal::Vec3s r = p1 - p2;
111
1/2
✓ Branch 1 taken 5844 times.
✗ Branch 2 not taken.
 5844 Scalar a = d1.dot(d1);
112
1/2
✓ Branch 1 taken 5844 times.
✗ Branch 2 not taken.
 5844 Scalar b = d1.dot(d2);
113
1/2
✓ Branch 1 taken 5844 times.
✗ Branch 2 not taken.
 5844 Scalar c = d1.dot(r);
114
1/2
✓ Branch 1 taken 5844 times.
✗ Branch 2 not taken.
 5844 Scalar e = d2.dot(d2);
115
1/2
✓ Branch 1 taken 5844 times.
✗ Branch 2 not taken.
 5844 Scalar f = d2.dot(r);
116 // S1 is parametrized by the equation p1 + s * d1
117 // S2 is parametrized by the equation p2 + t * d2
118
119
2/4
✓ Branch 1 taken 5844 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5844 times.
✗ Branch 5 not taken.
 5844 Vec3s w1, w2;
120
2/2
✓ Branch 0 taken 1000 times.
✓ Branch 1 taken 4844 times.
 5844 if (a <= EPSILON) {
121
1/2
✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
 1000 w1 = p1;
122
1/2
✓ Branch 0 taken 1000 times.
✗ Branch 1 not taken.
 1000 if (e <= EPSILON)
123 // Check if the segments degenerate into points
124
1/2
✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
 1000 w2 = p2;
125 else
126 // First segment is degenerated
127 ✗ clamped_linear(w2, p2, f, e, d2);
128
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 4844 times.
 4844 } else if (e <= EPSILON) {
129 // Second segment is degenerated
130 ✗ clamped_linear(w1, p1, -c, a, d1);
131 ✗ w2 = p2;
132 } else {
133 // Always non-negative, equal 0 if the segments are colinear
134 4844 Scalar denom = Scalar(fmax(a * e - b * b, 0));
135
136 Scalar s;
137 Scalar t;
138
2/2
✓ Branch 0 taken 834 times.
✓ Branch 1 taken 4010 times.
 4844 if (denom > EPSILON) {
139
1/2
✓ Branch 1 taken 834 times.
✗ Branch 2 not taken.
 834 s = clamp((b * f - c * e), denom);
140 834 t = b * s + f;
141 } else {
142 4010 s = 0.;
143 4010 t = f;
144 }
145
146
2/2
✓ Branch 0 taken 3261 times.
✓ Branch 1 taken 1583 times.
 4844 if (t <= 0.0) {
147
1/2
✓ Branch 1 taken 3261 times.
✗ Branch 2 not taken.
 3261 w2 = p2;
148
1/2
✓ Branch 1 taken 3261 times.
✗ Branch 2 not taken.
 3261 clamped_linear(w1, p1, -c, a, d1);
149
2/2
✓ Branch 0 taken 392 times.
✓ Branch 1 taken 1191 times.
 1583 } else if (t >= e) {
150
1/2
✓ Branch 1 taken 392 times.
✗ Branch 2 not taken.
 392 clamped_linear(w1, p1, (b - c), a, d1);
151
2/4
✓ Branch 1 taken 392 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 392 times.
✗ Branch 5 not taken.
 392 w2 = p2 + d2;
152 } else {
153
3/6
✓ Branch 1 taken 1191 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1191 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1191 times.
✗ Branch 8 not taken.
 1191 w1 = p1 + s * d1;
154
3/6
✓ Branch 1 taken 1191 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1191 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1191 times.
✗ Branch 8 not taken.
 1191 w2 = p2 + t / e * d2;
155 }
156 }
157
158 // witness points achieving the distance between the two segments
159
2/4
✓ Branch 1 taken 5844 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5844 times.
✗ Branch 5 not taken.
 5844 Scalar distance = (w1 - w2).norm();
160
161 // capsule spcecific distance computation
162 5844 distance = distance - (radius1 + radius2);
163
164 // Normal points from o1 to o2
165
3/6
✓ Branch 1 taken 5844 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5844 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5844 times.
✗ Branch 8 not taken.
 5844 normal = (w2 - w1).normalized();
166
3/6
✓ Branch 1 taken 5844 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5844 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5844 times.
✗ Branch 8 not taken.
 5844 wp1 = w1 + radius1 * normal;
167
3/6
✓ Branch 1 taken 5844 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5844 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5844 times.
✗ Branch 8 not taken.
 5844 wp2 = w2 - radius2 * normal;
168
169 5844 return distance;
170 }
171 } // namespace internal
172
173 } // namespace coal
174