const Constraint::LineOnLineContactImpl::VelocityCache&
Constraint::LineOnLineContactImpl::
ensureVelocityCacheRealized(const State& s) const {
if (getMyMatterSubsystemRep().isCacheValueRealized(s, m_velCacheIx))
return getVelocityCache(s);
VelocityCache& vc = updVelocityCache(s);
const SpatialVec& V_AF = getBodyVelocityFromState(s, m_mobod_F);
const SpatialVec& V_AB = getBodyVelocityFromState(s, m_mobod_B);
calcVelocityInfo(s, V_AF, V_AB, vc);
getMyMatterSubsystemRep().markCacheValueRealized(s, m_velCacheIx);
return vc;
}
// This costs about 175 flops if position info has already been calculated,
// otherwise we also pay for ensurePositionCacheRealized().
const Constraint::SphereOnSphereContactImpl::VelocityCache&
Constraint::SphereOnSphereContactImpl::
ensureVelocityCacheRealized(const State& s) const {
if (getMyMatterSubsystemRep().isCacheValueRealized(s, m_velCacheIx))
return getVelocityCache(s);
const PositionCache& pc = ensurePositionCacheRealized(s);
VelocityCache& vc = updVelocityCache(s);
const UnitVec3& Cx_A = pc.X_AC.x();
const UnitVec3& Cy_A = pc.X_AC.y();
const UnitVec3& Cz_A = pc.X_AC.z();
const SpatialVec& V_AF = getBodyVelocityFromState(s, m_mobod_F);
const Vec3& w_AF = V_AF[0];
const Vec3& v_AF = V_AF[1];
const SpatialVec& V_AB = getBodyVelocityFromState(s, m_mobod_B);
const Vec3& w_AB = V_AB[0];
const Vec3& v_AB = V_AB[1];
// These are d/dt_A p_FSf and d/dt_A p_BSb
const Vec3 wX_p_FSf_A = w_AF % pc.p_FSf_A; // 9 flops
const Vec3 wX_p_BSb_A = w_AB % pc.p_BSb_A; // 9 flops
const Vec3 v_ASf = v_AF + wX_p_FSf_A; // 3 flops
const Vec3 v_ASb = v_AB + wX_p_BSb_A; // 3 flops
vc.pd_SfSb_A = v_ASb - v_ASf; // 3 flops
// These are the Coriolis accelerations of Sf and Sb, needed later.
vc.wXwX_p_FSf_A = w_AF % wX_p_FSf_A; // 9 flops
vc.wXwX_p_BSb_A = w_AB % wX_p_BSb_A; // 9 flops
// Calculate the velocity of B's material point (station) at Co,
// measured in the F frame and expressed in A.
const Vec3 pd_FB_A = v_AB - v_AF; // 3 flops
const Vec3 vA_BCo_A = v_AB + w_AB % pc.p_BCo_A; // 12 flops
const Vec3 vA_FCo_A = v_AF + w_AF % pc.p_FCo_A; // 12 flops
vc.vF_BCo_A = vA_BCo_A - vA_FCo_A; // 3 flops
// These are the velocities in the A frame of the *contact point* locations
// measured from F's and B's origins; these are not stations since the
// contact point moves relative to the F and B frame.
const Vec3 pd_FCo_A = w_AF % pc.p_FSf_A + pc.kf*vc.pd_SfSb_A; // 15 flops
const Vec3 pd_BCo_A = pd_FCo_A - pd_FB_A; // 3 flops
vc.wXpd_FCo_A = w_AF % pd_FCo_A; // 9 flops
vc.wXpd_BCo_A = w_AB % pd_BCo_A; // 9 flops
// Calculate d/dt_A Cz.
vc.Czd_A = pc.isSingular
? w_AF % Cz_A // rare
: pc.oor*(vc.pd_SfSb_A - (~vc.pd_SfSb_A*Cz_A)*Cz_A); // 12 flops
// We also need d/dt_A of Cx and Cy, which we'll call Cxd and Cyd. Here's
// how to get those. Since the x-y directions are arbitrary in the plane, we
// can assume that they are not rotating about z, that is, w_FC is in the
// x-y plane. Our strategy will be to work in the F frame here, because we
// know that CzdF = d/dt_F Cz = w_FC % Cz, a vector perpendicular to both
// w_FC and Cz. But that means CzdF is in the x-y plane and since there
// was no z component of w_FC it is just w_FC rotated 90 degrees. Since x
// and y are also 90 degrees apart, we can get the derivatives we need:
// CxdF = -CzdF x Cy
// CydF = CzdF x Cx
// We can then convert those to A-frame derivatives. To get CzdF:
// CzdF = Czd - w_AF % Cz
const Vec3 CzdF_A = vc.Czd_A - w_AF % Cz_A; // 12 flops
const Vec3 CxdF_A = -CzdF_A % Cy_A; // 12 flops
const Vec3 CydF_A = CzdF_A % Cx_A; // 9 flops
vc.Cxd_A = CxdF_A + w_AF % Cx_A; // 12 flops
vc.Cyd_A = CydF_A + w_AF % Cy_A; // 12 flops
getMyMatterSubsystemRep().markCacheValueRealized(s, m_velCacheIx);
return vc;
}
