SimbodyMatterSubsystem.h

Go to the documentation of this file.

#ifndef SimTK_SIMBODY_MATTER_SUBSYSTEM_H_
#define SimTK_SIMBODY_MATTER_SUBSYSTEM_H_
 
/* -------------------------------------------------------------------------- *
 *                               Simbody(tm)                                  *
 * -------------------------------------------------------------------------- *
 * This is part of the SimTK biosimulation toolkit originating from           *
 * Simbios, the NIH National Center for Physics-Based Simulation of           *
 * Biological Structures at Stanford, funded under the NIH Roadmap for        *
 * Medical Research, grant U54 GM072970. See https://simtk.org/home/simbody.  *
 *                                                                            *
 * Portions copyright (c) 2006-15 Stanford University and the Authors.        *
 * Authors: Michael Sherman                                                   *
 * Contributors: Paul Mitiguy                                                 *
 *                                                                            *
 * Licensed under the Apache License, Version 2.0 (the "License"); you may    *
 * not use this file except in compliance with the License. You may obtain a  *
 * copy of the License at http://www.apache.org/licenses/LICENSE-2.0.         *
 *                                                                            *
 * Unless required by applicable law or agreed to in writing, software        *
 * distributed under the License is distributed on an "AS IS" BASIS,          *
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.   *
 * See the License for the specific language governing permissions and        *
 * limitations under the License.                                             *
 * -------------------------------------------------------------------------- */
 
#include "SimTKcommon.h"
#include "simbody/internal/common.h"
#include "simbody/internal/MobilizedBody.h"
 
#include <cassert>
#include <vector>
#include <iostream>
 
class SimbodyMatterSubsystemRep;
 
namespace SimTK {
 
class MobilizedBody;
class MultibodySystem;
class Constraint;
 
class UnilateralContact;
class StateLimitedFriction;
 
class SimTK_SIMBODY_EXPORT SimbodyMatterSubsystem : public Subsystem {
public:
 
//==============================================================================
explicit SimbodyMatterSubsystem(MultibodySystem&);
SimbodyMatterSubsystem();
~SimbodyMatterSubsystem() {} // invokes ~Subsystem()
 
const MobilizedBody& getMobilizedBody(MobilizedBodyIndex) const;
 
MobilizedBody& updMobilizedBody(MobilizedBodyIndex);
 
const MobilizedBody::Ground& getGround() const;
MobilizedBody::Ground& updGround();
MobilizedBody::Ground& Ground() {return updGround();}
 
const Constraint& getConstraint(ConstraintIndex) const;
 
Constraint& updConstraint(ConstraintIndex);
 
void setShowDefaultGeometry(bool show);
bool getShowDefaultGeometry() const;
 
int getNumBodies() const;
 
int getNumConstraints() const;
 
int getNumMobilities() const;
 
int getTotalQAlloc() const;
 
MobilizedBodyIndex adoptMobilizedBody(MobilizedBodyIndex  parent, 
                                      MobilizedBody&      child);
 
ConstraintIndex adoptConstraint(Constraint&);
 
UnilateralContactIndex adoptUnilateralContact(UnilateralContact*);
int getNumUnilateralContacts() const;
const UnilateralContact& getUnilateralContact(UnilateralContactIndex) const;
UnilateralContact& updUnilateralContact(UnilateralContactIndex);
StateLimitedFrictionIndex adoptStateLimitedFriction(StateLimitedFriction*);
int getNumStateLimitedFrictions() const;
const StateLimitedFriction&
getStateLimitedFriction(StateLimitedFrictionIndex) const;
StateLimitedFriction& 
updStateLimitedFriction(StateLimitedFrictionIndex);
 
SimbodyMatterSubsystem(const SimbodyMatterSubsystem& ss) : Subsystem(ss) {}
SimbodyMatterSubsystem& operator=(const SimbodyMatterSubsystem& ss) 
{   Subsystem::operator=(ss); return *this; }
 
 
//==============================================================================
void setUseEulerAngles(State& state, bool useEulerAngles) const;
 
bool getUseEulerAngles(const State& state) const;
 
int  getNumQuaternionsInUse(const State& state) const;
 
bool isUsingQuaternion(const State& state, MobilizedBodyIndex mobodIx) const;
 
QuaternionPoolIndex getQuaternionPoolIndex(const State& state, 
                                           MobilizedBodyIndex mobodIx) const;
 
void setConstraintIsDisabled(State&          state, 
                             ConstraintIndex constraintIx, 
                             bool            shouldDisableConstraint) const;
 
bool isConstraintDisabled(const State&, ConstraintIndex constraint) const;
 
void convertToEulerAngles(const State& inputState, State& outputState) const;
 
void convertToQuaternions(const State& inputState, State& outputState) const; 
 
void normalizeQuaternions(State& state) const;
 
//==============================================================================
Real calcSystemMass(const State& s) const;
 
Vec3 calcSystemMassCenterLocationInGround(const State& s) const;
 
MassProperties calcSystemMassPropertiesInGround(const State& s) const;
 
Inertia calcSystemCentralInertiaInGround(const State& s) const;
 
Vec3 calcSystemMassCenterVelocityInGround(const State& s) const;
 
Vec3 calcSystemMassCenterAccelerationInGround(const State& s) const;
 
SpatialVec calcSystemMomentumAboutGroundOrigin(const State& s) const;
 
SpatialVec calcSystemCentralMomentum(const State& s) const;
 
Real calcKineticEnergy(const State& state) const;
//==============================================================================
void multiplyBySystemJacobian( const State&         state,
                               const Vector&        u,
                               Vector_<SpatialVec>& Ju) const;
 
void calcBiasForSystemJacobian(const State&         state,
                               Vector_<SpatialVec>& JDotu) const;
 
 
void calcBiasForSystemJacobian(const State&         state,
                               Vector&              JDotu) const;
 
void multiplyBySystemJacobianTranspose( const State&                state,
                                        const Vector_<SpatialVec>&  F_G,
                                        Vector&                     f) const;
 
 
void calcSystemJacobian(const State&            state,
                        Matrix_<SpatialVec>&    J_G) const; // nb X nu
 
void calcSystemJacobian(const State&            state,
                        Matrix&                 J_G) const; // 6 nb X nu
 
 
void multiplyByStationJacobian(const State&                      state,
                               const Array_<MobilizedBodyIndex>& onBodyB,
                               const Array_<Vec3>&               stationPInB,
                               const Vector&                     u,
                               Vector_<Vec3>&                    JSu) const;
 
Vec3 multiplyByStationJacobian(const State&         state,
                               MobilizedBodyIndex   onBodyB,
                               const Vec3&          stationPInB,
                               const Vector&        u) const
{
    ArrayViewConst_<MobilizedBodyIndex> bodies(&onBodyB, &onBodyB+1);
    ArrayViewConst_<Vec3>               stations(&stationPInB, &stationPInB+1);
    Vector_<Vec3>                       JSu(1);
    multiplyByStationJacobian(state, bodies, stations, u, JSu);
    return JSu[0];
}
 
 
void multiplyByStationJacobianTranspose
   (const State&                        state,
    const Array_<MobilizedBodyIndex>&   onBodyB,
    const Array_<Vec3>&                 stationPInB,
    const Vector_<Vec3>&                f_GP,
    Vector&                             f) const;
 
void multiplyByStationJacobianTranspose
   (const State&                        state,
    MobilizedBodyIndex                  onBodyB,
    const Vec3&                         stationPInB,
    const Vec3&                         f_GP,
    Vector&                             f) const
{
    ArrayViewConst_<MobilizedBodyIndex> bodies(&onBodyB, &onBodyB+1);
    ArrayViewConst_<Vec3>               stations(&stationPInB, &stationPInB+1);
    Vector_<Vec3>                       forces(1, f_GP);
    multiplyByStationJacobianTranspose(state, bodies, stations, forces, f);
}
 
void calcStationJacobian(const State&                        state,
                         const Array_<MobilizedBodyIndex>&   onBodyB,
                         const Array_<Vec3>&                 stationPInB,
                         Matrix_<Vec3>&                      JS) const;
 
void calcStationJacobian(const State&       state,
                         MobilizedBodyIndex onBodyB,
                         const Vec3&        stationPInB,
                         RowVector_<Vec3>&  JS) const
{
    ArrayViewConst_<MobilizedBodyIndex> bodies(&onBodyB, &onBodyB+1);
    ArrayViewConst_<Vec3>               stations(&stationPInB, &stationPInB+1);
    calcStationJacobian(state, bodies, stations, JS);
}
 
void calcStationJacobian(const State&                        state,
                         const Array_<MobilizedBodyIndex>&   onBodyB,
                         const Array_<Vec3>&                 stationPInB,
                         Matrix&                             JS) const;
 
void calcStationJacobian(const State&       state,
                         MobilizedBodyIndex onBodyB,
                         const Vec3&        stationPInB,
                         Matrix&            JS) const
{
    ArrayViewConst_<MobilizedBodyIndex> bodies(&onBodyB, &onBodyB+1);
    ArrayViewConst_<Vec3>               stations(&stationPInB, &stationPInB+1);
    calcStationJacobian(state, bodies, stations, JS);
}
 
 
void calcBiasForStationJacobian(const State&                      state,
                                const Array_<MobilizedBodyIndex>& onBodyB,
                                const Array_<Vec3>&               stationPInB,
                                Vector_<Vec3>&                    JSDotu) const;
 
void calcBiasForStationJacobian(const State&                      state,
                                const Array_<MobilizedBodyIndex>& onBodyB,
                                const Array_<Vec3>&               stationPInB,
                                Vector&                           JSDotu) const;
 
Vec3 calcBiasForStationJacobian(const State&         state,
                                MobilizedBodyIndex   onBodyB,
                                const Vec3&          stationPInB) const
{
    ArrayViewConst_<MobilizedBodyIndex> bodies(&onBodyB, &onBodyB+1);
    ArrayViewConst_<Vec3>               stations(&stationPInB, &stationPInB+1);
    Vector_<Vec3>                       JSDotu(1);
    calcBiasForStationJacobian(state, bodies, stations, JSDotu);
    return JSDotu[0];
}
 
 
void multiplyByFrameJacobian
   (const State&                        state,
    const Array_<MobilizedBodyIndex>&   onBodyB,
    const Array_<Vec3>&                 originAoInB,
    const Vector&                       u,
    Vector_<SpatialVec>&                JFu) const;
 
SpatialVec multiplyByFrameJacobian( const State&         state,
                                    MobilizedBodyIndex   onBodyB,
                                    const Vec3&          originAoInB,
                                    const Vector&        u) const
{
    ArrayViewConst_<MobilizedBodyIndex> bodies(&onBodyB, &onBodyB+1);
    ArrayViewConst_<Vec3>               stations(&originAoInB, &originAoInB+1);
    Vector_<SpatialVec>                 JFu(1);
    multiplyByFrameJacobian(state, bodies, stations, u, JFu);
    return JFu[0];
}
 
 
void multiplyByFrameJacobianTranspose
   (const State&                        state,
    const Array_<MobilizedBodyIndex>&   onBodyB,
    const Array_<Vec3>&                 originAoInB,
    const Vector_<SpatialVec>&          F_GAo,
    Vector&                             f) const;
 
void multiplyByFrameJacobianTranspose(  const State&        state,
                                        MobilizedBodyIndex  onBodyB,
                                        const Vec3&         originAoInB,
                                        const SpatialVec&   F_GAo,
                                        Vector&             f) const
{
    ArrayViewConst_<MobilizedBodyIndex> bodies(&onBodyB, &onBodyB+1);
    ArrayViewConst_<Vec3>               stations(&originAoInB, &originAoInB+1);
    const Vector_<SpatialVec>           forces(1, F_GAo);
    multiplyByFrameJacobianTranspose(state, bodies, stations, forces, f);
}
 
 
 
void calcFrameJacobian(const State&                         state,
                       const Array_<MobilizedBodyIndex>&    onBodyB,
                       const Array_<Vec3>&                  originAoInB,
                       Matrix_<SpatialVec>&                 JF) const;
 
void calcFrameJacobian(const State&             state,
                       MobilizedBodyIndex       onBodyB,
                       const Vec3&              originAoInB,
                       RowVector_<SpatialVec>&  JF) const
{
    ArrayViewConst_<MobilizedBodyIndex> bodies(&onBodyB, &onBodyB+1);
    ArrayViewConst_<Vec3>               stations(&originAoInB, &originAoInB+1);
    calcFrameJacobian(state, bodies, stations, JF);
}
 
void calcFrameJacobian(const State&                         state,
                       const Array_<MobilizedBodyIndex>&    onBodyB,
                       const Array_<Vec3>&                  originAoInB,
                       Matrix&                              JF) const;
 
void calcFrameJacobian(const State&             state,
                       MobilizedBodyIndex       onBodyB,
                       const Vec3&              originAoInB,
                       Matrix&                  JF) const 
{
    ArrayViewConst_<MobilizedBodyIndex> bodies(&onBodyB, &onBodyB+1);
    ArrayViewConst_<Vec3>               stations(&originAoInB, &originAoInB+1);
    calcFrameJacobian(state, bodies, stations, JF);
}
 
void calcBiasForFrameJacobian
   (const State&                        state,
    const Array_<MobilizedBodyIndex>&   onBodyB,
    const Array_<Vec3>&                 originAoInB,
    Vector_<SpatialVec>&                JFDotu) const;
 
void calcBiasForFrameJacobian(const State&                      state,
                              const Array_<MobilizedBodyIndex>& onBodyB,
                              const Array_<Vec3>&               originAoInB,
                              Vector&                           JFDotu) const;
 
SpatialVec calcBiasForFrameJacobian(const State&         state,
                                    MobilizedBodyIndex   onBodyB,
                                    const Vec3&          originAoInB) const
{
    ArrayViewConst_<MobilizedBodyIndex> bodies(&onBodyB, &onBodyB+1);
    ArrayViewConst_<Vec3>               stations(&originAoInB, &originAoInB+1);
    Vector_<SpatialVec>                 JFDotu(1);
    calcBiasForFrameJacobian(state, bodies, stations, JFDotu);
    return JFDotu[0];
}
 
//==============================================================================
void multiplyByPositionJacobianWrtInboardFramePositions(
        const State&                      state,
        const Vector_<Vec3>&              dp_PF,
        Vector_<Vec3>&                    dp_GB) const;
 
void multiplyByPositionJacobianWrtInboardFramePositionsTranspose(
        const State&                      state,
        const Vector_<Vec3>&              g_GB,
        Vector_<Vec3>&                    g_PF) const;
 
void multiplyByPositionJacobianWrtOutboardFramePositions(
        const State&                      state,
        const Vector_<Vec3>&              dp_BM,
        Vector_<Vec3>&                    dp_GB) const;
 
void multiplyByPositionJacobianWrtOutboardFramePositionsTranspose(
        const State&                      state,
        const Vector_<Vec3>&              g_GB,
        Vector_<Vec3>&                    g_BM) const;
 
//==============================================================================
void multiplyByM(const State& state, const Vector& a, Vector& Ma) const;
 
void multiplyByMInv(const State&    state,
                    const Vector&   v,
                    Vector&         MinvV) const;
 
void calcM(const State&, Matrix& M) const;
 
void calcMInv(const State&, Matrix& MInv) const;
 
void calcProjectedMInv(const State&   s,
                       Matrix&        GMInvGt) const;
 
void solveForConstraintImpulses(const State&     state,
                                const Vector&    deltaV,
                                Vector&          impulse) const;
 
 
void multiplyByG(const State&  state,
                 const Vector& ulike,
                 Vector&       Gulike) const {
    Vector bias;
    calcBiasForMultiplyByG(state, bias);
    multiplyByG(state, ulike, bias, Gulike);
}
 
 
void multiplyByG(const State&  state,
                 const Vector& ulike,
                 const Vector& bias,
                 Vector&       Gulike) const;
 
void calcBiasForMultiplyByG(const State& state,
                            Vector&      bias) const;
 
void calcG(const State& state, Matrix& G) const;
 
 
void calcBiasForAccelerationConstraints(const State& state,
                                        Vector&      bias) const;
 
void calcConstraintAccelerationErrors(const State&  state,
                                      const Vector& knownUDot,
                                      Vector&       pvaerr) const;
 
void multiplyByGTranspose(const State&  state,
                          const Vector& lambda,
                          Vector&       f) const;
    
void calcGTranspose(const State&, Matrix& Gt) const;
 
 
void multiplyByPq(const State&  state,
                  const Vector& qlike,
                  Vector&       PqXqlike) const {
    Vector biasp;
    calcBiasForMultiplyByPq(state, biasp);
    multiplyByPq(state, qlike, biasp, PqXqlike);
}
 
 
void multiplyByPq(const State&  state,
                 const Vector&  qlike,
                 const Vector&  biasp,
                 Vector&        PqXqlike) const;
 
void calcBiasForMultiplyByPq(const State& state,
                             Vector&      biasp) const;
 
void calcPq(const State& state, Matrix& Pq) const;
 
 
void multiplyByPqTranspose(const State&  state,
                           const Vector& lambdap,
                           Vector&       f) const;
 
void calcPqTranspose(const State& state, Matrix& Pqt) const;
 
void calcP(const State& state, Matrix& P) const;
 
void calcPt(const State& state, Matrix& Pt) const;
 
void multiplyByPV(const State&  state,
                  const Vector& ulike,
                  Vector&       PVulike) const {
    Vector biaspv;
    calcBiasForMultiplyByPV(state, biaspv);
    multiplyByPV(state, ulike, biaspv, PVulike);
}
 
void multiplyByPV(const State&   state,
                  const Vector&  ulike,
                  const Vector&  biaspv,
                  Vector&        PVulike) const;
 
void calcBiasForMultiplyByPV(const State& state,
                             Vector&      bias) const;
 
void calcPV(const State& state, Matrix& PV) const;
 
void multiplyByPVTranspose(const State&  state,
                           const Vector& lambdapv,
                           Vector&       fpv) const;
 
void calcPVTranspose(const State&, Matrix& PVt) const;
 
void multiplyByN(const State& s, bool transpose, 
                 const Vector& in, Vector& out) const;
 
void multiplyByNInv(const State& s, bool transpose, 
                    const Vector& in, Vector& out) const;
 
void multiplyByNDot(const State& s, bool transpose, 
                    const Vector& in, Vector& out) const;
 
//==============================================================================
void calcAcceleration
   (const State&               state,
    const Vector&              appliedMobilityForces,
    const Vector_<SpatialVec>& appliedBodyForces,
    Vector&                    udot,    // output only; no prescribed motions
    Vector_<SpatialVec>&       A_GB) const;
 
void calcAccelerationIgnoringConstraints
   (const State&                state,
    const Vector&               appliedMobilityForces,
    const Vector_<SpatialVec>&  appliedBodyForces,
    Vector&                     udot,    
    Vector_<SpatialVec>&        A_GB) const;
 
 
 
void calcResidualForceIgnoringConstraints
   (const State&               state,
    const Vector&              appliedMobilityForces,
    const Vector_<SpatialVec>& appliedBodyForces,
    const Vector&              knownUdot,
    Vector&                    residualMobilityForces) const;
 
 
void calcResidualForce
   (const State&               state,
    const Vector&              appliedMobilityForces,
    const Vector_<SpatialVec>& appliedBodyForces,
    const Vector&              knownUdot,
    const Vector&              knownLambda,
    Vector&                    residualMobilityForces) const;
 
 
void calcCompositeBodyInertias(const State&    state,
    Array_<SpatialInertia,MobilizedBodyIndex>& R) const;
 
 
 
void calcBodyAccelerationFromUDot(const State&         state,
                                  const Vector&        knownUDot,
                                  Vector_<SpatialVec>& A_GB) const;
 
void calcConstraintForcesFromMultipliers
  (const State&         state, 
   const Vector&        multipliers,
   Vector_<SpatialVec>& bodyForcesInG,
   Vector&              mobilityForces) const;
 
void calcMobilizerReactionForces
   (const State&         state, 
    Vector_<SpatialVec>& forcesAtMInG) const;
 
const Vector& getMotionMultipliers(const State& state) const;
 
Vector calcMotionErrors(const State& state, const Stage& stage) const;
 
void findMotionForces
   (const State&         state,
    Vector&              mobilityForces) const;
 
const Vector& getConstraintMultipliers(const State& state) const;
 
void findConstraintForces
  (const State&         state, 
   Vector_<SpatialVec>& bodyForcesInG,
   Vector&              mobilityForces) const;
 
Real calcMotionPower(const State& state) const;
 
Real calcConstraintPower(const State& state) const;
 
void calcTreeEquivalentMobilityForces(const State&, 
    const Vector_<SpatialVec>& bodyForces,
    Vector&                    mobilityForces) const;
 
 
void calcQDot(const State& s,
    const Vector& u,
    Vector&       qdot) const;
 
void calcQDotDot(const State& s,
    const Vector& udot,
    Vector&       qdotdot) const;
 
void addInStationForce(const State&         state, 
                       MobilizedBodyIndex   bodyB, 
                       const Vec3&          stationOnB, 
                       const Vec3&          forceInG, 
                       Vector_<SpatialVec>& bodyForcesInG) const;
 
void addInBodyTorque(const State&           state, 
                     MobilizedBodyIndex     mobodIx, 
                     const Vec3&            torqueInG, 
                     Vector_<SpatialVec>&   bodyForcesInG) const;
 
void addInMobilityForce(const State&        state,
                        MobilizedBodyIndex  mobodIx, 
                        MobilizerUIndex     which, 
                        Real                f,
                        Vector&             mobilityForces) const;
//==============================================================================
void realizePositionKinematics(const State& state) const;
 
void realizeVelocityKinematics(const State&) const;
 
void realizeCompositeBodyInertias(const State&) const;
 
void realizeArticulatedBodyInertias(const State&) const;
 
void realizeArticulatedBodyVelocity(const State&) const;
 
 
    // INSTANCE STAGE responses and operators //
 
const Array_<QIndex>& getFreeQIndex(const State& state) const;
 
const Array_<UIndex>& getFreeUIndex(const State& state) const;
 
const Array_<UIndex>& getFreeUDotIndex(const State& state) const;
 
const Array_<UIndex>& getKnownUDotIndex(const State& state) const;
 
void packFreeQ
   (const State& s, const Vector& allQ, Vector& packedFreeQ) const;
 
void unpackFreeQ
   (const State& s, const Vector& packedFreeQ, Vector& unpackedFreeQ) const;
 
void packFreeU
   (const State& s, const Vector& allU, Vector& packedFreeU) const;
 
void unpackFreeU
   (const State& s, const Vector& packedFreeU, Vector& unpackedFreeU) const;
 
 
    // POSITION STAGE responses //
 
const SpatialInertia& 
getCompositeBodyInertia(const State& state, MobilizedBodyIndex mbx) const;
 
const ArticulatedInertia& 
getArticulatedBodyInertia(const State& state, MobilizedBodyIndex mbx) const;
 
 
    // VELOCITY STAGE responses //
 
const SpatialVec& 
getGyroscopicForce(const State& state, MobilizedBodyIndex mbx) const;
 
const SpatialVec& 
getMobilizerCoriolisAcceleration(const State&       state, 
                                 MobilizedBodyIndex mbx) const;
 
const SpatialVec& 
getTotalCoriolisAcceleration(const State& state, MobilizedBodyIndex mbx) const;
 
 
const SpatialVec& 
getTotalCentrifugalForces(const State& state, MobilizedBodyIndex mbx) const;
//==============================================================================
void calcMobilizerReactionForcesUsingFreebodyMethod
   (const State&         state, 
    Vector_<SpatialVec>& forcesAtMInG) const;
 
void invalidatePositionKinematics(const State& state) const;
 
bool isPositionKinematicsRealized(const State&) const;
 
void invalidateVelocityKinematics(const State& state) const;
 
bool isVelocityKinematicsRealized(const State&) const;
 
void invalidateCompositeBodyInertias(const State& state) const;
 
bool isCompositeBodyInertiasRealized(const State&) const;
 
void invalidateArticulatedBodyInertias(const State& state) const;
 
bool isArticulatedBodyInertiasRealized(const State&) const;
 
void invalidateArticulatedBodyVelocity(const State& state) const;
 
bool isArticulatedBodyVelocityRealized(const State&) const;
//==============================================================================
 
int getNumParticles() const;
 
// The generalized coordinates for a particle are always the three measure numbers
// (x,y,z) of the particle's Ground-relative Cartesian location vector. The generalized
// speeds are always the three corresponding measure numbers of the particle's
// Ground-relative Cartesian velocity. The generalized applied forces are
// always the three measure numbers of a Ground-relative force vector.
const Vector_<Vec3>& getAllParticleLocations    (const State&) const;
const Vector_<Vec3>& getAllParticleVelocities   (const State&) const;
 
const Vec3& getParticleLocation(const State& s, ParticleIndex p) const {
    return getAllParticleLocations(s)[p];
}
const Vec3& getParticleVelocity(const State& s, ParticleIndex p) const {
    return getAllParticleVelocities(s)[p];
}
 
Vector& updAllParticleMasses(State& s) const;
 
void setAllParticleMasses(State& s, const Vector& masses) const {
    updAllParticleMasses(s) = masses;
}
 
// Note that particle generalized coordinates, speeds, and applied forces
// are defined to be the particle Cartesian locations, velocities, and
// applied force vectors, so can be set directly at Stage::Model or higher.
 
// These are the only routines that must be provided by the concrete MatterSubsystem.
Vector_<Vec3>& updAllParticleLocations(State&)     const;
Vector_<Vec3>& updAllParticleVelocities(State&)    const;
 
// The following inline routines are provided by the generic MatterSubsystem 
// class for convenience.
 
Vec3& updParticleLocation(State& s, ParticleIndex p) const {
    return updAllParticleLocations(s)[p];
}
Vec3& updParticleVelocity(State& s, ParticleIndex p) const {
    return updAllParticleVelocities(s)[p];
}
 
void setParticleLocation(State& s, ParticleIndex p, const Vec3& r) const {
    updAllParticleLocations(s)[p] = r;
}
void setParticleVelocity(State& s, ParticleIndex p, const Vec3& v) const {
    updAllParticleVelocities(s)[p] = v;
}
 
void setAllParticleLocations(State& s, const Vector_<Vec3>& r) const {
    updAllParticleLocations(s) = r;
}
void setAllParticleVelocities(State& s, const Vector_<Vec3>& v) const {
    updAllParticleVelocities(s) = v;
}
 
const Vector& getAllParticleMasses(const State&) const;
 
const Vector_<Vec3>& getAllParticleAccelerations(const State&) const;
 
const Vec3& getParticleAcceleration(const State& s, ParticleIndex p) const {
    return getAllParticleAccelerations(s)[p];
}
//==============================================================================
// Internally this is now called getArticulatedBodyCentrifugalForces().
DEPRECATED_14("do you really need this? see getTotalCentrifugalForces() instead")
const SpatialVec& 
getMobilizerCentrifugalForces(const State& state, MobilizedBodyIndex mbx) const;
//==============================================================================
//     Bookkeeping methods and internal types -- hide from Doxygen
public:
class Subtree; // used for working with a connected subgraph of the MobilizedBody tree
class SubtreeResults;
 
 
SimTK_PIMPL_DOWNCAST(SimbodyMatterSubsystem, Subsystem);
const SimbodyMatterSubsystemRep& getRep() const;
SimbodyMatterSubsystemRep&       updRep();
private:
};
 
SimTK_SIMBODY_EXPORT std::ostream& 
operator<<(std::ostream&, const SimbodyMatterSubsystem&);
 
 
} // namespace SimTK
 
#endif // SimTK_SIMBODY_MATTER_SUBSYSTEM_H_