Constraint::SphereOnPlaneContact::SphereOnPlaneContact (MobilizedBody& planeBody, const Transform& defaultPlaneFrame, MobilizedBody& sphereBody, const Vec3& defaultSphereCenter, Real defaultSphereRadius, bool enforceRolling) : Constraint(new SphereOnPlaneContactImpl(enforceRolling)) { SimTK_APIARGCHECK_ALWAYS( planeBody.isInSubsystem() && sphereBody.isInSubsystem(), "Constraint::SphereOnPlaneContact","SphereOnPlaneContact", "Both bodies must already be in a SimbodyMatterSubsystem."); SimTK_APIARGCHECK_ALWAYS(planeBody.isInSameSubsystem(sphereBody), "Constraint::SphereOnPlaneContact","SphereOnPlaneContact", "The two bodies to be connected must be in the same " "SimbodyMatterSubsystem."); SimTK_APIARGCHECK1_ALWAYS(defaultSphereRadius > 0, "Constraint::SphereOnPlaneContact","SphereOnPlaneContact", "The sphere radius must be greater than zero but was %g.", defaultSphereRadius); planeBody.updMatterSubsystem().adoptConstraint(*this); updImpl().m_planeBody_F = updImpl().addConstrainedBody(planeBody); updImpl().m_ballBody_B = updImpl().addConstrainedBody(sphereBody); updImpl().m_def_X_FP = defaultPlaneFrame; updImpl().m_def_p_BO = defaultSphereCenter; updImpl().m_def_radius = defaultSphereRadius; }
Constraint::LineOnLineContact::LineOnLineContact (MobilizedBody& mobod_F, const Transform& defaultEdgeFrameF, Real defaultHalfLengthF, MobilizedBody& mobod_B, const Transform& defaultEdgeFrameB, Real defaultHalfLengthB, bool enforceRolling) : Constraint(new LineOnLineContactImpl(enforceRolling)) { SimTK_APIARGCHECK_ALWAYS(mobod_F.isInSubsystem() && mobod_B.isInSubsystem(), "Constraint::LineOnLineContact","LineOnLineContact", "Both mobilized bodies must already be in a SimbodyMatterSubsystem."); SimTK_APIARGCHECK_ALWAYS(mobod_F.isInSameSubsystem(mobod_B), "Constraint::LineOnLineContact","LineOnLineContact", "The two mobilized bodies to be connected must be in the same " "SimbodyMatterSubsystem."); mobod_F.updMatterSubsystem().adoptConstraint(*this); updImpl().m_mobod_F = updImpl().addConstrainedBody(mobod_F); updImpl().m_mobod_B = updImpl().addConstrainedBody(mobod_B); updImpl().m_def_X_FEf = defaultEdgeFrameF; updImpl().m_def_hf = defaultHalfLengthF; updImpl().m_def_X_BEb = defaultEdgeFrameB; updImpl().m_def_hb = defaultHalfLengthB; }
Constraint::SphereOnSphereContact::SphereOnSphereContact (MobilizedBody& mobod_F, const Vec3& defaultCenter_F, Real defaultRadius_F, MobilizedBody& mobod_B, const Vec3& defaultCenter_B, Real defaultRadius_B, bool enforceRolling) : Constraint(new SphereOnSphereContactImpl(enforceRolling)) { SimTK_APIARGCHECK_ALWAYS(mobod_F.isInSubsystem() && mobod_B.isInSubsystem(), "Constraint::SphereOnSphereContact","SphereOnSphereContact", "Both mobilized bodies must already be in a SimbodyMatterSubsystem."); SimTK_APIARGCHECK_ALWAYS(mobod_F.isInSameSubsystem(mobod_B), "Constraint::SphereOnSphereContact","SphereOnSphereContact", "The two mobilized bodies to be connected must be in the same " "SimbodyMatterSubsystem."); SimTK_APIARGCHECK2_ALWAYS(defaultRadius_F > 0 && defaultRadius_B > 0, "Constraint::SphereOnSphereContact","SphereOnSphereContact", "The sphere radii must be greater than zero; they were %g and %g.", defaultRadius_F, defaultRadius_B); mobod_F.updMatterSubsystem().adoptConstraint(*this); updImpl().m_mobod_F = updImpl().addConstrainedBody(mobod_F); updImpl().m_mobod_B = updImpl().addConstrainedBody(mobod_B); updImpl().m_def_p_FSf = defaultCenter_F; updImpl().m_def_radius_F = defaultRadius_F; updImpl().m_def_p_BSb = defaultCenter_B; updImpl().m_def_radius_B = defaultRadius_B; }
MyConstraintImplementation(MobilizedBody& mobilizer, Real speed) : Implementation(mobilizer.updMatterSubsystem(), 0,1,0), theMobilizer(), whichMobility(), prescribedSpeed(NaN) { theMobilizer = addConstrainedMobilizer(mobilizer); whichMobility = MobilizerUIndex(0); prescribedSpeed = speed; }
// Constructor takes two bodies and the desired separation distance // between their body frame origins. Tell the base class that this // constraint generates 1 holonomic (position level), 0 nonholonomic // (velocity level), and 0 acceleration-only constraint equations. ExampleConstraint(MobilizedBody& b1, MobilizedBody& b2, Real distance) : Implementation(b1.updMatterSubsystem(), 1, 0, 0), distance(distance) { body1 = addConstrainedBody(b1); body2 = addConstrainedBody(b2); }