38 #ifndef FCL_NARROWPHASE_DETAIL_CAPSULECAPSULE_INL_H
39 #define FCL_NARROWPHASE_DETAIL_CAPSULECAPSULE_INL_H
85 const auto kEpsSquared =
kEps *
kEps;
91 S a = p_P1Q1.dot(p_P1Q1);
92 S e = p_P2Q2.dot(p_P2Q2);
93 S f = p_P2Q2.dot(p_P2P1);
96 if (a <= kEpsSquared && e <= kEpsSquared) {
101 return (*p_FC1 - *p_FC2).squaredNorm();
103 if (a <= kEpsSquared) {
107 *t =
clamp(f / e, (S)0.0, (S)1.0);
109 const S c = p_P1Q1.dot(p_P2P1);
110 if (e <= kEpsSquared) {
114 *s =
clamp(-c / a, (S)0.0, (S)1.0);
117 const S b = p_P1Q1.dot(p_P2Q2);
121 const S denom =
max(S(0), a*e-b*b);
125 if (denom > kEpsSquared) {
126 *s =
clamp((b*f - c*e) / denom, (S)0.0, (S)1.0);
132 *t = (b*(*s) + f) / e;
139 *s =
clamp(-c / a, (S)0.0, (S)1.0);
140 }
else if (*t > 1.0) {
142 *s =
clamp((b - c) / a, (S)0.0, (S)1.0);
146 *p_FC1 = p_FP1 + p_P1Q1 * (*s);
147 *p_FC2 = p_FP2 + p_P2Q2 * (*t);
148 return (*p_FC1 - *p_FC2).squaredNorm();
153 template <
typename S>
155 return X_AB.matrix().template block<3, 1>(0, 2);
159 template <
typename S>
164 assert(dist !=
nullptr);
165 assert(p_FW1 !=
nullptr);
166 assert(p_FW2 !=
nullptr);
169 const Vector3<S> p_FC1o = X_FC1.translation();
170 const Vector3<S> p_FC2o = X_FC2.translation();
180 const S half_length = c.
lz / 2;
182 return half_length * Cz_F;
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;
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;
205 &s, &t, &p_FN1, &p_FN2);
207 const S segment_dist = sqrt(squared_dist);
213 if (segment_dist > eps) {
214 vhat_C1C2_F = (p_FN2 - p_FN1) / segment_dist;
227 if (abs(C1z_F.dot(C2z_F)) < 1 - eps) {
229 vhat_C1C2_F = C1z_F.cross(C2z_F).normalized();
232 vhat_C1C2_F = X_FC1.matrix().template block<3, 1>(0, 0);
235 *p_FW1 = p_FN1 + vhat_C1C2_F * s1.
radius;
236 *p_FW2 = p_FN2 - vhat_C1C2_F * s2.
radius;