C++ MobilizedBody类代码示例

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::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::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;
}

Force::MobilityLinearDamperImpl::
MobilityLinearDamperImpl(const MobilizedBody&   mobod, 
                         MobilizerUIndex        whichU,
                         Real                   defaultDamping) 
:   m_matter(mobod.getMatterSubsystem()), 
    m_mobodIx(mobod.getMobilizedBodyIndex()), m_whichU(whichU), 
    m_defaultDamping(defaultDamping) 
{
}

Force::MobilityConstantForceImpl::MobilityConstantForceImpl
   (const MobilizedBody&    mobod, 
    MobilizerUIndex         whichU,
    Real                    defaultForce) 
:   m_matter(mobod.getMatterSubsystem()), 
    m_mobodIx(mobod.getMobilizedBodyIndex()), m_whichU(whichU), 
    m_defaultForce(defaultForce) 
{
}

Force::MobilityLinearSpringImpl::
MobilityLinearSpringImpl(const MobilizedBody&    mobod, 
                         MobilizerQIndex         whichQ,
                         Real                    defaultStiffness, 
                         Real                    defaultQZero) 
:   m_matter(mobod.getMatterSubsystem()), 
    m_mobodIx(mobod.getMobilizedBodyIndex()), m_whichQ(whichQ), 
    m_defaultStiffness(defaultStiffness), m_defaultQZero(defaultQZero)
{
}

//==============================================================================
//                            GET REACTION PAIR
//==============================================================================
static ReactionPair getReactionPair(const State&         state,
                                    const MobilizedBody& mobod) 
{
    SpatialVec p = mobod.findMobilizerReactionOnParentAtFInGround(state);
    SpatialVec c = mobod.findMobilizerReactionOnBodyAtMInGround(state);
    const Rotation& R_GC = mobod.getBodyRotation(state);
    const Rotation& R_GP = mobod.getParentMobilizedBody().getBodyRotation(state);
    ReactionPair pair;
    pair.reactionOnChildInChild   = ~R_GC*c; // from Ground to Child
    pair.reactionOnParentInParent = ~R_GP*p; // from Ground to Parent
    return pair;
}

SpatialVec Force::DiscreteForces::
getOneBodyForce(const State& state, const MobilizedBody& mobod) const {
    const Vector_<SpatialVec>& bodyForces = getImpl().getAllBodyForces(state);
    if (bodyForces.size() == 0) {return SpatialVec(Vec3(0),Vec3(0));}

    return bodyForces[mobod.getMobilizedBodyIndex()];
}

 MyConstraintImplementation(MobilizedBody& mobilizer, Real speed)
   : Implementation(mobilizer.updMatterSubsystem(), 0,1,0),  
     theMobilizer(), whichMobility(), prescribedSpeed(NaN)
 {
     theMobilizer = addConstrainedMobilizer(mobilizer);
     whichMobility = MobilizerUIndex(0);
     prescribedSpeed = speed;
 }

Real Force::DiscreteForces::
getOneMobilityForce(const State& state, const MobilizedBody& mobod,
                    MobilizerUIndex whichU) const {
    const Vector& mobForces = getImpl().getAllMobilityForces(state);
    if (mobForces.size() == 0) {return 0;}

    return mobod.getOneFromUPartition(state, whichU, mobForces);
}

void Force::DiscreteForces::
setOneBodyForce(State& state, const MobilizedBody& mobod,
                const SpatialVec& spatialForceInG) const {
    Vector_<SpatialVec>& bodyForces = getImpl().updAllBodyForces(state);
    if (bodyForces.size() == 0) {
        bodyForces.resize(getImpl().m_matter.getNumBodies());
        bodyForces.setToZero();
    }
    bodyForces[mobod.getMobilizedBodyIndex()] = spatialForceInG;
}

void Force::DiscreteForces::
addForceToBodyPoint(State& state, const MobilizedBody& mobod,
                    const Vec3& pointInB, const Vec3& forceInG) const {
    Vector_<SpatialVec>& bodyForces = getImpl().updAllBodyForces(state);
    if (bodyForces.size() == 0) {
        bodyForces.resize(getImpl().m_matter.getNumBodies());
        bodyForces.setToZero();
    }
    mobod.applyForceToBodyPoint(state, pointInB, forceInG, bodyForces);
}

void Force::DiscreteForces::
setOneMobilityForce(State& state, const MobilizedBody& mobod,
                    MobilizerUIndex whichU, Real f) const {
    Vector& mobForces = getImpl().updAllMobilityForces(state);
    if (mobForces.size() == 0) {
        mobForces.resize(state.getNU());
        mobForces.setToZero();
    }
    // Don't use "apply" here because that would add in the force.
    mobod.updOneFromUPartition(state, whichU, mobForces) = f;
}

void ObservedPointFitter::
createClonedSystem(const MultibodySystem& original, MultibodySystem& copy, 
                   const Array_<MobilizedBodyIndex>& originalBodyIxs, 
                   Array_<MobilizedBodyIndex>& copyBodyIxs,
                   bool& hasArtificialBaseBody) 
{
    const SimbodyMatterSubsystem& originalMatter = original.getMatterSubsystem();
    SimbodyMatterSubsystem copyMatter(copy);
    Body::Rigid body = Body::Rigid(MassProperties(1, Vec3(0), Inertia(1)));
    body.addDecoration(Transform(), DecorativeSphere(Real(.1)));
    std::map<MobilizedBodyIndex, MobilizedBodyIndex> idMap;
    hasArtificialBaseBody = false;
    for (int i = 0; i < (int)originalBodyIxs.size(); ++i) {
        const MobilizedBody& originalBody = originalMatter.getMobilizedBody(originalBodyIxs[i]);
        MobilizedBody* copyBody;
        if (i == 0) {
            if (originalBody.isGround())
                copyBody = &copyMatter.Ground();
            else {
                hasArtificialBaseBody = true; // not using the original joint here
                MobilizedBody::Free free(copyMatter.Ground(), body);
                copyBody = &copyMatter.updMobilizedBody(free.getMobilizedBodyIndex());
            }
        }
        else {
            MobilizedBody& parent = copyMatter.updMobilizedBody(idMap[originalBody.getParentMobilizedBody().getMobilizedBodyIndex()]);
            copyBody = &originalBody.cloneForNewParent(parent);
        }
        copyBodyIxs.push_back(copyBody->getMobilizedBodyIndex());
        idMap[originalBodyIxs[i]] = copyBody->getMobilizedBodyIndex();
    }
    copy.realizeTopology();
    State& s = copy.updDefaultState();
    copyMatter.setUseEulerAngles(s, true);
    copy.realizeModel(s);
}

Force::TwoPointConstantForceImpl::TwoPointConstantForceImpl(const MobilizedBody& body1, const Vec3& station1,
        const MobilizedBody& body2, const Vec3& station2, Real force) : matter(body1.getMatterSubsystem()),
        body1(body1.getMobilizedBodyIndex()), station1(station1),
        body2(body2.getMobilizedBodyIndex()), station2(station2), force(force) {
}