capsule_capsule-inl.h
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2011-2014, Willow Garage, Inc.
5  * Copyright (c) 2014-2016, 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 
38 #ifndef FCL_NARROWPHASE_DETAIL_CAPSULECAPSULE_INL_H
39 #define FCL_NARROWPHASE_DETAIL_CAPSULECAPSULE_INL_H
40 
42 
43 namespace fcl
44 {
45 
46 namespace detail
47 {
48 
49 //==============================================================================
50 extern template
51 double clamp(double n, double min, double max);
52 
53 //==============================================================================
54 extern template double closestPtSegmentSegment(const Vector3d& p_FP1,
55  const Vector3d& p_FQ1,
56  const Vector3d& p_FP2,
57  const Vector3d& p_FQ2, double* s,
58  double* t, Vector3d* p_FC1,
59  Vector3d* p_FC2);
60 
61 //==============================================================================
62 extern template
64  const Capsule<double>& s1, const Transform3<double>& tf1,
65  const Capsule<double>& s2, const Transform3<double>& tf2,
66  double* dist, Vector3d* p1_res, Vector3d* p2_res);
67 
68 //==============================================================================
69 template <typename S>
70 S clamp(S n, S min, S max)
71 {
72  if (n < min) return min;
73  if (n > max) return max;
74  return n;
75 }
76 
77 //==============================================================================
78 template <typename S>
79 S closestPtSegmentSegment(const Vector3<S>& p_FP1, const Vector3<S>& p_FQ1,
80  const Vector3<S>& p_FP2, const Vector3<S>& p_FQ2,
81  S* s, S* t, Vector3<S>* p_FC1, Vector3<S>* p_FC2) {
82  // TODO(SeanCurtis-TRI): Document the match underlying this function -- the
83  // variables: a, b, c, e, and f are otherwise overly inscrutable.
84  const auto kEps = constants<S>::eps_78();
85  const auto kEpsSquared = kEps * kEps;
86 
87  Vector3<S> p_P1Q1 = p_FQ1 - p_FP1; // Segment 1's displacement vector: D1.
88  Vector3<S> p_P2Q2 = p_FQ2 - p_FP2; // Segment 2's displacement vector: D2.
89  Vector3<S> p_P2P1 = p_FP1 - p_FP2;
90 
91  S a = p_P1Q1.dot(p_P1Q1); // Squared length of segment S1, always nonnegative.
92  S e = p_P2Q2.dot(p_P2Q2); // Squared length of segment S2, always nonnegative.
93  S f = p_P2Q2.dot(p_P2P1);
94 
95  // Check if either or both segments degenerate into points.
96  if (a <= kEpsSquared && e <= kEpsSquared) {
97  // Both segments degenerate into points.
98  *s = *t = 0.0;
99  *p_FC1 = p_FP1;
100  *p_FC2 = p_FP2;
101  return (*p_FC1 - *p_FC2).squaredNorm();
102  }
103  if (a <= kEpsSquared) {
104  // First segment degenerates into a point.
105  *s = 0.0;
106  // t = (b * s + f) / e, s = 0 --> t = f / e.
107  *t = clamp(f / e, (S)0.0, (S)1.0);
108  } else {
109  const S c = p_P1Q1.dot(p_P2P1);
110  if (e <= kEpsSquared) {
111  // Second segment degenerates into a point.
112  *t = 0.0;
113  // s = (b*t - c) / a, t = 0 --> s = -c / a.
114  *s = clamp(-c / a, (S)0.0, (S)1.0);
115  } else {
116  // The general non-degenerate case starts here.
117  const S b = p_P1Q1.dot(p_P2Q2);
118  // Mathematically, ae - b² ≥ 0, but we need to protect ourselves from
119  // possible rounding error near zero that _might_ produce -epsilon.
120  using std::max;
121  const S denom = max(S(0), a*e-b*b);
122 
123  // If segments are not parallel, compute closest point on L1 to L2 and
124  // clamp to segment S1. Else pick arbitrary s (here 0).
125  if (denom > kEpsSquared) {
126  *s = clamp((b*f - c*e) / denom, (S)0.0, (S)1.0);
127  } else {
128  *s = 0.0;
129  }
130  // Compute point on L2 closest to S1(s) using
131  // t = Dot((P1 + D1*s) - P2, D2) / Dot(D2,D2) = (b*s + f) / e
132  *t = (b*(*s) + f) / e;
133 
134  // If t in [0,1] done. Else clamp t, recompute s for the new value
135  // of t using s = Dot((P2 + D2*t) - P1, D1) / Dot(D1,D1) = (t*b - c) / a
136  // and clamp s to [0, 1].
137  if(*t < 0.0) {
138  *t = 0.0;
139  *s = clamp(-c / a, (S)0.0, (S)1.0);
140  } else if (*t > 1.0) {
141  *t = 1.0;
142  *s = clamp((b - c) / a, (S)0.0, (S)1.0);
143  }
144  }
145  }
146  *p_FC1 = p_FP1 + p_P1Q1 * (*s);
147  *p_FC2 = p_FP2 + p_P2Q2 * (*t);
148  return (*p_FC1 - *p_FC2).squaredNorm();
149 }
150 
151 // Given a transform relating frames A and B, returns Bz_A, the z-axis of frame
152 // B, expressed in frame A.
153 template <typename S>
155  return X_AB.matrix().template block<3, 1>(0, 2);
156 }
157 
158 //==============================================================================
159 template <typename S>
160 bool capsuleCapsuleDistance(const Capsule<S>& s1, const Transform3<S>& X_FC1,
161  const Capsule<S>& s2, const Transform3<S>& X_FC2,
162  S* dist, Vector3<S>* p_FW1, Vector3<S>* p_FW2)
163 {
164  assert(dist != nullptr);
165  assert(p_FW1 != nullptr);
166  assert(p_FW2 != nullptr);
167 
168  // Origin of the capsules' frames relative to F's origin.
169  const Vector3<S> p_FC1o = X_FC1.translation();
170  const Vector3<S> p_FC2o = X_FC2.translation();
171 
172  // A capsule is defined centered on the origin of its canonical frame C
173  // and with the central line segment aligned with Cz. So, the two end points
174  // of the capsule's center line segment are at `z+ = lz / 2 * Cz` and
175  // `z- = -lz / 2 * Cz`, respectively. Cz_F is simply the third column of the
176  // rotation matrix, R_FC. This "half arm" is the position vector from the
177  // canonical frame's origin to the z+ point: p_CoZ+_F in frame F.
178  auto calc_half_arm = [](const Capsule<S>& c,
179  const Transform3<S>& X_FC) -> Vector3<S> {
180  const S half_length = c.lz / 2;
181  const Vector3<S> Cz_F = z_axis(X_FC);
182  return half_length * Cz_F;
183  };
184 
185  const Vector3<S> half_arm_1_F = calc_half_arm(s1, X_FC1);
186  const Vector3<S> p_FC1a = p_FC1o + half_arm_1_F;
187  const Vector3<S> p_FC1b = p_FC1o - half_arm_1_F;
188 
189  const Vector3<S> half_arm_2_F = calc_half_arm(s2, X_FC2);
190  const Vector3<S> p_FC2a = p_FC2o + half_arm_2_F;
191  const Vector3<S> p_FC2b = p_FC2o - half_arm_2_F;
192 
193  // s and t correspond to the length of each line segment; should be s1.lz and
194  // s2.lz, respectively.
195  S s, t;
196  // The points on the line segments closest to each other.
197  Vector3<S> p_FN1, p_FN2;
198  // TODO(SeanCurtis-TRI): This query isn't well tailored for this problem.
199  // By construction, we know the unit-length vectors for the two segments (and
200  // their lengths), but closestPtSegmentSegment() infers the segment direction
201  // from the end point. Furthermore, it returns the values for s and t,
202  // neither of which is required by this function. The API should be
203  // streamlined so there is less waste.
204  const S squared_dist = closestPtSegmentSegment(p_FC1a, p_FC1b, p_FC2a, p_FC2b,
205  &s, &t, &p_FN1, &p_FN2);
206 
207  const S segment_dist = sqrt(squared_dist);
208  *dist = segment_dist - s1.radius - s2.radius;
209  Vector3<S> vhat_C1C2_F;
210  const auto eps = constants<S>::eps_78();
211  // We can only use the vector between the center-line nearest points to find
212  // the witness points if they are sufficiently far apart.
213  if (segment_dist > eps) {
214  vhat_C1C2_F = (p_FN2 - p_FN1) / segment_dist;
215  } else {
216  // The points are too close. The center lines intersect. We have two cases:
217  // 1. They intersect at a single point (non-parallel center lines).
218  // - The center lines span a plane and the witness points should lie
219  // above and below that plane the corresponding radius amount.
220  // 2. They intersect at multiple points (parallel, overlapping center
221  // lies).
222  // - Any direction on the plane perpendicular to the common center line
223  // will suffice. We arbitrarily pick the Cx direction.
224  const Vector3<S>& C1z_F = z_axis(X_FC1);
225  const Vector3<S>& C2z_F = z_axis(X_FC2);
226  using std::abs;
227  if (abs(C1z_F.dot(C2z_F)) < 1 - eps) {
228  // Vectors are sufficiently perpendicular to use cross product.
229  vhat_C1C2_F = C1z_F.cross(C2z_F).normalized();
230  } else {
231  // Vectors are parallel, simply use Cx_F as the vector.
232  vhat_C1C2_F = X_FC1.matrix().template block<3, 1>(0, 0);
233  }
234  }
235  *p_FW1 = p_FN1 + vhat_C1C2_F * s1.radius;
236  *p_FW2 = p_FN2 - vhat_C1C2_F * s2.radius;
237 
238  return true;
239 }
240 
241 } // namespace detail
242 } // namespace fcl
243 
244 #endif
kEps
static const double kEps
Definition: test_gjk_libccd-inl_gjk_doSimplex2.cpp:128
fcl::Transform3
Eigen::Transform< S, 3, Eigen::Isometry > Transform3
Definition: types.h:91
fcl::detail::clamp
template double clamp(double n, double min, double max)
fcl::Capsule::radius
S radius
Radius of capsule.
Definition: capsule.h:61
fcl::constants::eps_78
static Real eps_78()
Returns ε^(7/8) for the precision of the underlying scalar type.
Definition: constants.h:164
max
static T max(T x, T y)
Definition: svm.cpp:52
fcl::Capsule::lz
S lz
Length along z axis.
Definition: capsule.h:64
fcl::Vector3
Eigen::Matrix< S, 3, 1 > Vector3
Definition: types.h:70
fcl::detail::closestPtSegmentSegment
template double closestPtSegmentSegment(const Vector3d &p_FP1, const Vector3d &p_FQ1, const Vector3d &p_FP2, const Vector3d &p_FQ2, double *s, double *t, Vector3d *p_FC1, Vector3d *p_FC2)
fcl::detail::z_axis
Vector3< S > z_axis(const Transform3< S > &X_AB)
Definition: capsule_capsule-inl.h:154
Vector3d
fcl::Vector3d Vector3d
Definition: test_broadphase_dynamic_AABB_tree.cpp:48
fcl::detail::capsuleCapsuleDistance
template bool capsuleCapsuleDistance(const Capsule< double > &s1, const Transform3< double > &tf1, const Capsule< double > &s2, const Transform3< double > &tf2, double *dist, Vector3d *p1_res, Vector3d *p2_res)
fcl::Capsule< double >
template class FCL_EXPORT Capsule< double >
min
static T min(T x, T y)
Definition: svm.cpp:49
fcl
Main namespace.
Definition: broadphase_bruteforce-inl.h:45
fcl::Capsule< S >
capsule_capsule.h


fcl
Author(s):
autogenerated on Tue Dec 5 2023 03:40:48