// This costs about 154 flops.
const Constraint::SphereOnSphereContactImpl::PositionCache&
Constraint::SphereOnSphereContactImpl::
ensurePositionCacheRealized(const State& s) const {
if (getMyMatterSubsystemRep().isCacheValueRealized(s, m_posCacheIx))
return getPositionCache(s);
const Parameters& params = getParameters(s);
const Vec3& p_FSf = params.m_p_FSf;
const Vec3& p_BSb = params.m_p_BSb;
const Real rf = params.m_radius_F;
const Real rb = params.m_radius_B;
PositionCache& pc = updPositionCache(s);
pc.kf = rf/(rf+rb); // we'll put Co at Sf + kf*p_SfSb, ~10 flops
const Transform& X_AF = getBodyTransformFromState(s, m_mobod_F);
const Transform& X_AB = getBodyTransformFromState(s, m_mobod_B);
const Vec3& p_AF = X_AF.p();
const Vec3& p_AB = X_AB.p();
pc.p_FSf_A = X_AF.R() * p_FSf; // exp. in A, 15 flops
const Vec3 p_ASf = X_AF.p() + pc.p_FSf_A; // meas. from Ao, 3 flops
pc.p_BSb_A = X_AB.R() * p_BSb; // exp. in A, 15 flops
const Vec3 p_ASb = X_AB.p() + pc.p_BSb_A; // meas. from Ao, 3 flops
pc.p_SfSb_A = p_ASb - p_ASf; // vec from Sf to Sb, exp. in A, 3 flops
pc.r = pc.p_SfSb_A.norm(); // ~20 flops
pc.oor = 1/pc.r; // ~10 flops (might be Infinity)
// Assume non-singular.
UnitVec3 Cz_A(pc.p_SfSb_A * pc.oor, true); // 3 flops
pc.isSingular = false;
if (pc.r < TinyReal) {
pc.isSingular = true;
Cz_A = X_AF.z(); // arbitrary
}
// Now compute the contact frame C, in A. It would be somewhat more elegant
// to compute it in F, so that the x-y axis directions would depend only
// on the relative pose between the two bodies. However, that is more
// expensive than working in A and since the x-y axes are arbitrary and
// ephemeral anyway doing it faster seems like the way to go (sherm 140502).
// Place the contact point along the center-to-center line.
const Vec3 p_ACo(p_ASf + pc.kf*pc.p_SfSb_A); // 6 flops
pc.X_AC.updP() = p_ACo;
pc.X_AC.updR().setRotationFromOneAxis(Cz_A, ZAxis); // ~60 flops
// We'll need contact point Co measured from F and from B.
pc.p_FCo_A = p_ACo - p_AF; // 3 flops
pc.p_BCo_A = p_ACo - p_AB; // 3 flops
getMyMatterSubsystemRep().markCacheValueRealized(s, m_posCacheIx);
return pc;
}
const Constraint::LineOnLineContactImpl::PositionCache&
Constraint::LineOnLineContactImpl::
ensurePositionCacheRealized(const State& s) const {
if (getMyMatterSubsystemRep().isCacheValueRealized(s, m_posCacheIx))
return getPositionCache(s);
PositionCache& pc = updPositionCache(s);
const Transform& X_AF = getBodyTransformFromState(s, m_mobod_F);
const Transform& X_AB = getBodyTransformFromState(s, m_mobod_B);
calcPositionInfo(s, X_AF, X_AB, pc);
getMyMatterSubsystemRep().markCacheValueRealized(s, m_posCacheIx);
return pc;
}
