void Constraint::LineOnLineContactImpl::
calcDecorativeGeometryAndAppendVirtual
(const State& s, Stage stage, Array_<DecorativeGeometry>& geom) const
{
// We can't generate the artwork until we know the lines' placements,
// which might not be until Position stage.
if ( stage != Stage::Position
|| !getMyMatterSubsystemRep().getShowDefaultGeometry())
return;
const Parameters& params = getParameters(s);
const Real hf = params.hf;
const Real hb = params.hb;
const PositionCache& pc = ensurePositionCacheRealized(s);
const Transform& X_AC = pc.X_AC;
const MobilizedBody& mobod_A = getAncestorMobilizedBody();
const Transform& X_GA = mobod_A.getBodyTransform(s);
const Rotation& R_GA = X_GA.R();
const Transform X_GC = X_GA * X_AC;
// Convert interesting stuff from A to G.
const UnitVec3 df_G = R_GA * X_AC.x();
const UnitVec3 db_G = R_GA * pc.db_A;
const Vec3 p_GQf = X_GA * pc.p_AQf;
const Vec3 p_GQb = X_GA * pc.p_AQb;
const Vec3 half_Lf = hf * df_G;
const Vec3 half_Lb = hb * db_G;
const MobilizedBody& bodyF = getMobilizedBodyFromConstrainedBody(m_mobod_F);
const MobilizedBody& bodyB = getMobilizedBodyFromConstrainedBody(m_mobod_B);
const Transform& X_GF = bodyF.getBodyTransform(s);
const Transform& X_GB = bodyB.getBodyTransform(s);
const Transform X_GEf = X_GF * params.X_FEf;
const Transform X_GEb = X_GB * params.X_BEb;
// On body F draw a green line segment around the orange closest point.
geom.push_back(DecorativeLine(p_GQf-half_Lf, p_GQf+half_Lf)
.setColor(Green));
geom.push_back(DecorativeFrame().setTransform(X_GEf)
.setColor(Green*.9).setLineThickness(1).setScale(0.5)); // F color
geom.push_back(DecorativePoint(p_GQf)
.setColor(Orange).setLineThickness(2)); // B color
// On body B draw an orange line segment around the green closest point.
geom.push_back(DecorativeLine(p_GQb-half_Lb, p_GQb+half_Lb)
.setColor(Orange));
geom.push_back(DecorativeFrame().setTransform(X_GEb)
.setColor(Orange*.9).setLineThickness(1).setScale(0.5)); // B color
geom.push_back(DecorativePoint(p_GQb)
.setColor(Green).setLineThickness(2)); // F color
// Show the contact frame in red.
geom.push_back(DecorativeFrame().setTransform(X_GC)
.setColor(Red));
}
// This costs about 340 flops if position info has already been calculated,
// otherwise we also pay for ensurePositionCacheRealized().
void Constraint::LineOnLineContactImpl::
calcVelocityInfo(const State& state,
const SpatialVec& V_AF, const SpatialVec& V_AB,
VelocityCache& vc) const
{
const PositionCache& pc = ensurePositionCacheRealized(state);
if (pc.edgesAreParallel)
return;
const Vec3& w_AF = V_AF[0]; // handy abbreviations
const Vec3& v_AF = V_AF[1];
const Vec3& w_AB = V_AB[0];
const Vec3& v_AB = V_AB[1];
// These are d/dt_A p_FPf and d/dt_A p_BPb
const Vec3 wX_p_FPf_A = w_AF % pc.p_FPf_A; // 9 flops
const Vec3 wX_p_BPb_A = w_AB % pc.p_BPb_A; // 9
const Vec3 v_APf = v_AF + wX_p_FPf_A; // 3
const Vec3 v_APb = v_AB + wX_p_BPb_A; // 3
vc.dp_PfPb_A = v_APb - v_APf; // 3
vc.ddf_A = w_AF % pc.df_A; // 9 flops
vc.ddb_A = w_AB % pc.db_A; // 9
vc.dw_A = pc.sense*(vc.ddf_A % pc.db_A + pc.df_A % vc.ddb_A);//24
vc.dn_A = pc.oos * (vc.dw_A - (~pc.n_A*vc.dw_A)*pc.n_A); // 14
// Calculate the velocity of B's material point (station) at Co,
// measured in the F frame and expressed in A.
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
vc.vF_BCo_A = vA_BCo_A - vA_FCo_A; // 3
// We have s=||w||, oos=1/s. We want doos = d/dt oos.
vc.doos = -square(pc.oos) * dot(pc.n_A, vc.dw_A); // 8 flops
const Vec3 nXdb = pc.n_A % pc.db_A; // 9 flops
const Vec3 nXdf = pc.n_A % pc.df_A; // 9
const Vec3 d_nXdb = vc.dn_A % pc.db_A + pc.n_A % vc.ddb_A; // 21
const Vec3 d_nXdf = vc.dn_A % pc.df_A + pc.n_A % vc.ddf_A; // 21
const Real dtf = -pc.sense * ( // 20
pc.oos * (~vc.dp_PfPb_A*nXdb + ~pc.p_PfPb_A*d_nXdb)
+ vc.doos * (~pc.p_PfPb_A*nXdb) );
const Real dtb = -pc.sense * ( // 20
pc.oos * (~vc.dp_PfPb_A*nXdf + ~pc.p_PfPb_A*d_nXdf)
+ vc.doos * (~pc.p_PfPb_A*nXdf) );
const Vec3 dQf = v_APf + dtf * pc.df_A + pc.tf * vc.ddf_A; // 12 flops
const Vec3 dQb = v_APb + dtb * pc.db_A + pc.tb * vc.ddb_A; // 12
const Vec3 dCo = 0.5*(dQf + dQb); // 6
const Vec3 dp_FCo = dCo - v_AF; // 3
const Vec3 dp_BCo = dCo - v_AB; // 3
vc.wXdp_FCo_A = w_AF % dp_FCo; // 9 flops
vc.wXdp_BCo_A = w_AB % dp_BCo; // 9
vc.ddfXddb2 = 2.*(vc.ddf_A % vc.ddb_A); // 12
vc.wXddf_A = w_AF % vc.ddf_A; // 9
vc.wXddb_A = w_AB % vc.ddb_A; // 9
// These are the Coriolis accelerations of Pf and Pb, needed later.
vc.wXwX_p_FPf_A = w_AF % wX_p_FPf_A; // 9 flops
vc.wXwX_p_BPb_A = w_AB % wX_p_BPb_A; // 9
// Record derivative of the contact frame.
// We have Cx=df, Cz=n, Cy=n x df. Want derivatives in A.
vc.dCx_A = vc.ddf_A;
vc.dCz_A = vc.dn_A;
vc.dCy_A = d_nXdf;
vc.dCo_A = dCo;
}
// 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;
}
