// 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::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;
}
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;
}
// The default parameters may be overridden by setting a discrete variable in
// the state, and we need a couple of cache entries to hold expensive
// computations. We allocate the state resources here.
void Constraint::LineOnLineContactImpl::
realizeTopologyVirtual(State& state) const {
m_parametersIx = getMyMatterSubsystemRep().
allocateDiscreteVariable(state, Stage::Position,
new Value<Parameters>(Parameters(m_def_X_FEf, m_def_hf,
m_def_X_BEb, m_def_hb)));
m_posCacheIx = getMyMatterSubsystemRep().
allocateLazyCacheEntry(state, Stage::Position,
new Value<PositionCache>());
m_velCacheIx = getMyMatterSubsystemRep().
allocateLazyCacheEntry(state, Stage::Velocity,
new Value<VelocityCache>());
}
// The default plane and sphere parameters may be overridden by setting
// a discrete variable in the state. We allocate the state resources here.
void Constraint::SphereOnSphereContactImpl::
realizeTopologyVirtual(State& state) const {
m_parametersIx = getMyMatterSubsystemRep().
allocateDiscreteVariable(state, Stage::Position,
new Value<Parameters>(Parameters(m_def_p_FSf, m_def_radius_F,
m_def_p_BSb, m_def_radius_B)));
m_posCacheIx = getMyMatterSubsystemRep().
allocateLazyCacheEntry(state, Stage::Position,
new Value<PositionCache>());
m_velCacheIx = getMyMatterSubsystemRep().
allocateLazyCacheEntry(state, Stage::Velocity,
new Value<VelocityCache>());
}
void Constraint::SphereOnPlaneContactImpl::
calcDecorativeGeometryAndAppendVirtual
(const State& s, Stage stage, Array_<DecorativeGeometry>& geom) const
{
// We can't generate the artwork until we know the plane frame and the
// sphere center and radius, which might not be until Position stage.
if ( stage == Stage::Position
&& getMyMatterSubsystemRep().getShowDefaultGeometry())
{
const SimbodyMatterSubsystemRep& matterRep = getMyMatterSubsystemRep();
const Parameters& params = getParameters(s);
const Transform& X_FP = params.m_X_FP;
const Vec3& p_BO = params.m_p_BO;
const Real r = params.m_radius;
// TODO: should be instance-stage data from State rather than
// topological data.
// This makes z axis point along plane normal
const MobilizedBodyIndex planeMBIx =
getMobilizedBodyIndexOfConstrainedBody(m_planeBody_F);
const MobilizedBodyIndex ballMBIx =
getMobilizedBodyIndexOfConstrainedBody(m_ballBody_B);
if (m_planeHalfWidth > 0) {
// On the inboard body, draw a gray transparent rectangle,
// outlined in black lines.
geom.push_back(DecorativeBrick
(Vec3(m_planeHalfWidth,m_planeHalfWidth,r/10))
.setColor(Gray)
.setRepresentation(DecorativeGeometry::DrawSurface)
.setOpacity(Real(0.3))
.setBodyId(planeMBIx)
.setTransform(X_FP));
geom.push_back(DecorativeFrame(m_planeHalfWidth/5)
.setColor(Gray)
.setBodyId(planeMBIx)
.setTransform(X_FP));
}
// On the ball body draw an orange mesh sphere.
geom.push_back(DecorativeSphere(r)
.setColor(Orange)
.setRepresentation(DecorativeGeometry::DrawWireframe)
.setBodyId(ballMBIx)
.setTransform(p_BO));
}
}
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));
}
// The default plane and sphere parameters may be overridden by setting
// a discrete variable in the state. We allocate the state resources here.
void Constraint::SphereOnPlaneContactImpl::
realizeTopologyVirtual(State& state) const {
parametersIx = getMyMatterSubsystemRep().
allocateDiscreteVariable(state, Stage::Position,
new Value<Parameters>
(Parameters(m_def_X_FP, m_def_p_BO, m_def_radius)));
}
void Constraint::SphereOnSphereContactImpl::
calcDecorativeGeometryAndAppendVirtual
(const State& s, Stage stage, Array_<DecorativeGeometry>& geom) const
{
// We can't generate the artwork until we know the spheres' centers and
// radii, which might not be until Position stage.
if ( stage == Stage::Position
&& getMyMatterSubsystemRep().getShowDefaultGeometry())
{
const SimbodyMatterSubsystemRep& matterRep = getMyMatterSubsystemRep();
const Parameters& params = getParameters(s);
const Vec3& p_FSf = params.m_p_FSf;
const Real rf = params.m_radius_F;
const Vec3& p_BSb = params.m_p_BSb;
const Real rb = params.m_radius_B;
const MobilizedBodyIndex mobodFIx =
getMobilizedBodyIndexOfConstrainedBody(m_mobod_F);
const MobilizedBodyIndex mobodBIx =
getMobilizedBodyIndexOfConstrainedBody(m_mobod_B);
// On body F draw a green mesh sphere.
geom.push_back(DecorativeSphere(rf)
.setColor(Green)
.setRepresentation(DecorativeGeometry::DrawWireframe)
.setBodyId(mobodFIx)
.setTransform(p_FSf));
// On the ball body draw an orange mesh sphere.
geom.push_back(DecorativeSphere(rb)
.setColor(Orange)
.setRepresentation(DecorativeGeometry::DrawWireframe)
.setBodyId(mobodBIx)
.setTransform(p_BSb));
}
}
Constraint::LineOnLineContactImpl::VelocityCache&
Constraint::LineOnLineContactImpl::
updVelocityCache(const State& state) const {
return Value<VelocityCache>::updDowncast
(getMyMatterSubsystemRep().updCacheEntry(state,m_velCacheIx));
}
Constraint::LineOnLineContactImpl::PositionCache&
Constraint::LineOnLineContactImpl::
updPositionCache(const State& state) const {
return Value<PositionCache>::updDowncast
(getMyMatterSubsystemRep().updCacheEntry(state,m_posCacheIx));
}
Constraint::LineOnLineContactImpl::Parameters&
Constraint::LineOnLineContactImpl::
updParameters(State& state) const {
return Value<Parameters>::updDowncast
(getMyMatterSubsystemRep().updDiscreteVariable(state,m_parametersIx));
}
// Return the pair of constrained station points, with the first expressed
// in the body 1 frame and the second in the body 2 frame. Note that although
// these are used to define the position error, only the station on body 2
// is used to generate constraint forces; the point of body 1 that is
// coincident with the body 2 point receives the equal and opposite force.
const Constraint::SphereOnPlaneContactImpl::Parameters&
Constraint::SphereOnPlaneContactImpl::
getParameters(const State& state) const {
return Value<Parameters>::downcast
(getMyMatterSubsystemRep().getDiscreteVariable(state,parametersIx));
}
// 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;
}
const Constraint::SphereOnSphereContactImpl::VelocityCache&
Constraint::SphereOnSphereContactImpl::
getVelocityCache(const State& state) const {
return Value<VelocityCache>::downcast
(getMyMatterSubsystemRep().getCacheEntry(state,m_velCacheIx));
}
const Constraint::SphereOnSphereContactImpl::PositionCache&
Constraint::SphereOnSphereContactImpl::
getPositionCache(const State& state) const {
return Value<PositionCache>::downcast
(getMyMatterSubsystemRep().getCacheEntry(state,m_posCacheIx));
}
