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));
}
void Force::TwoPointLinearSpringImpl::
calcDecorativeGeometryAndAppend(const State& state, Stage stage,
Array_<DecorativeGeometry>& geom) const {
if (stage==Stage::Position && matter.getShowDefaultGeometry()) {
const Transform& X_GB1 = matter.getMobilizedBody(body1)
.getBodyTransform(state);
const Transform& X_GB2 = matter.getMobilizedBody(body2)
.getBodyTransform(state);
const Vec3 s1_G = X_GB1.R() * station1;
const Vec3 s2_G = X_GB2.R() * station2;
const Vec3 p1_G = X_GB1.p() + s1_G; // measured from ground origin
const Vec3 p2_G = X_GB2.p() + s2_G;
geom.push_back(DecorativeLine(p1_G,p2_G).setColor(Orange));
}
}
