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()]; }
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::MobilityLinearDamperImpl:: MobilityLinearDamperImpl(const MobilizedBody& mobod, MobilizerUIndex whichU, Real defaultDamping) : m_matter(mobod.getMatterSubsystem()), m_mobodIx(mobod.getMobilizedBodyIndex()), m_whichU(whichU), m_defaultDamping(defaultDamping) { }
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; }
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) { }
Force::MobilityLinearStopImpl::MobilityLinearStopImpl (const MobilizedBody& mobod, MobilizerQIndex whichQ, Real defaultStiffness, Real defaultDissipation, Real defaultQLow, Real defaultQHigh) : m_matter(mobod.getMatterSubsystem()), m_mobodIx(mobod.getMobilizedBodyIndex()), m_whichQ(whichQ), m_defStiffness(defaultStiffness), m_defDissipation(defaultDissipation), m_defQLow(defaultQLow), m_defQHigh(defaultQHigh) { }
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 = ©Matter.Ground(); else { hasArtificialBaseBody = true; // not using the original joint here MobilizedBody::Free free(copyMatter.Ground(), body); copyBody = ©Matter.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::ConstantTorqueImpl::ConstantTorqueImpl(const MobilizedBody& body, const Vec3& torque) : matter(body.getMatterSubsystem()), body(body.getMobilizedBodyIndex()), torque(torque) { }
Force::TwoPointLinearSpringImpl::TwoPointLinearSpringImpl(const MobilizedBody& body1, const Vec3& station1, const MobilizedBody& body2, const Vec3& station2, Real k, Real x0) : matter(body1.getMatterSubsystem()), body1(body1.getMobilizedBodyIndex()), station1(station1), body2(body2.getMobilizedBodyIndex()), station2(station2), k(k), x0(x0) { }
Force::ConstantForceImpl::ConstantForceImpl(const MobilizedBody& body, const Vec3& station, const Vec3& force) : matter(body.getMatterSubsystem()), body(body.getMobilizedBodyIndex()), station(station), force(force) { }
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) { }
Force::TwoPointLinearDamperImpl::TwoPointLinearDamperImpl(const MobilizedBody& body1, const Vec3& station1, const MobilizedBody& body2, const Vec3& station2, Real damping) : matter(body1.getMatterSubsystem()), body1(body1.getMobilizedBodyIndex()), station1(station1), body2(body2.getMobilizedBodyIndex()), station2(station2), damping(damping) { }