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) {
}
