MobilizedBody.h

Go to the documentation of this file.

#ifndef SimTK_SIMBODY_MOBILIZED_BODY_H_
#define SimTK_SIMBODY_MOBILIZED_BODY_H_

/* -------------------------------------------------------------------------- *
 *                      SimTK Core: SimTK Simbody(tm)                         *
 * -------------------------------------------------------------------------- *
 * This is part of the SimTK Core 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.               *
 *                                                                            *
 * Portions copyright (c) 2007-10 Stanford University and the Authors.        *
 * Authors: Michael Sherman                                                   *
 * Contributors: Paul Mitiguy, Peter Eastman                                  *
 *                                                                            *
 * Permission is hereby granted, free of charge, to any person obtaining a    *
 * copy of this software and associated documentation files (the "Software"), *
 * to deal in the Software without restriction, including without limitation  *
 * the rights to use, copy, modify, merge, publish, distribute, sublicense,   *
 * and/or sell copies of the Software, and to permit persons to whom the      *
 * Software is furnished to do so, subject to the following conditions:       *
 *                                                                            *
 * The above copyright notice and this permission notice shall be included in *
 * all copies or substantial portions of the Software.                        *
 *                                                                            *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,   *
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL    *
 * THE AUTHORS, CONTRIBUTORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,    *
 * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR      *
 * OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE  *
 * USE OR OTHER DEALINGS IN THE SOFTWARE.                                     *
 * -------------------------------------------------------------------------- */

#include "SimTKmath.h"
#include "simbody/internal/common.h"
#include "simbody/internal/Body.h"
#include "simbody/internal/Motion.h"

#include <cassert>

namespace SimTK {

class SimbodyMatterSubsystem;
class Motion;
class MobilizedBody;
class MobilizedBodyImpl;

// We only want the template instantiation to occur once. This symbol is 
// defined in the SimTK core compilation unit that instantiates the mobilized 
// body class but should not be defined any other time.
#ifndef SimTK_SIMBODY_DEFINING_MOBILIZED_BODY
    extern template class PIMPLHandle<MobilizedBody, MobilizedBodyImpl, true>;
#endif

typedef MobilizedBody Mobod;


class SimTK_SIMBODY_EXPORT MobilizedBody : public PIMPLHandle<MobilizedBody, MobilizedBodyImpl, true> {
public:

    enum Direction {
        Forward = 0,
        Reverse = 1
    };

    MobilizedBody& setDefaultMotionType(Motion::Level, Motion::Method=Motion::Prescribed);

    void setMotionType(State&, Motion::Level, Motion::Method=Motion::Prescribed) const;

    bool isAccelerationAlwaysZero(const State&) const;
    bool isVelocityAlwaysZero(const State&) const;


        // STATE ACCESS METHODS //



    const Transform& getBodyTransform(const State&) const; // X_GB

    const Rotation& getBodyRotation(const State& s) const {
        return getBodyTransform(s).R();
    }
    const Vec3& getBodyOriginLocation(const State& s) const {
        return getBodyTransform(s).p();
    }

    const Transform& getMobilizerTransform(const State&) const; // X_FM

    const SpatialVec& getBodyVelocity(const State&) const;          // V_GB

    const Vec3& getBodyAngularVelocity(const State& s) const {      // w_GB
        return getBodyVelocity(s)[0]; 
    }
    const Vec3& getBodyOriginVelocity(const State& s) const {       // v_GB
        return getBodyVelocity(s)[1];
    }

    const SpatialVec& getMobilizerVelocity(const State&) const;     // V_FM

    const SpatialVec& getBodyAcceleration(const State& s) const;    // A_GB

    const Vec3& getBodyAngularAcceleration(const State& s) const {  // b_GB
        return getBodyAcceleration(s)[0]; 
    }

    const Vec3& getBodyOriginAcceleration(const State& s) const {   // a_GB
        return getBodyAcceleration(s)[1];
    }

    const SpatialVec& getMobilizerAcceleration(const State&) const { // A_FM
        SimTK_ASSERT_ALWAYS(!"unimplemented method", 
            "MobilizedBody::getMobilizerAcceleration() is not yet implemented -- any volunteers?");
        return *(new SpatialVec());
    }

    const MassProperties& getBodyMassProperties(const State&) const;

    Real getBodyMass(const State& s) const {
        return getBodyMassProperties(s).getMass();
    }

    const Vec3& getBodyMassCenterStation(const State& s) const {
        return getBodyMassProperties(s).getMassCenter();
    }

    const Inertia& getBodyInertiaAboutBodyOrigin(const State& s) const {
        return getBodyMassProperties(s).getInertia();
    }

    const Transform& getInboardFrame (const State&) const;  // X_PF
    const Transform& getOutboardFrame(const State&) const;  // X_BM

    void setInboardFrame (State&, const Transform& X_PF) const;
    void setOutboardFrame(State&, const Transform& X_BM) const;

    // End of State Access - Bodies




    int getNumQ(const State&) const;
    int getNumU(const State&) const;

    QIndex getFirstQIndex(const State&) const;

    UIndex getFirstUIndex(const State&) const;


    Real getOneQ(const State&, int which) const;

    Real getOneU(const State&, int which) const;

    Vector getQAsVector(const State&) const;
    Vector getUAsVector(const State&) const;

    Real getOneQDot   (const State&, int which) const;
    Vector getQDotAsVector(const State&) const;

    Real getOneUDot(const State&, int which) const;
    Real getOneQDotDot(const State&, int which) const;
    Vector getUDotAsVector(const State&) const;
    Vector getQDotDotAsVector(const State&) const;

    Vector getTauAsVector(const State&) const;
    Real getOneTau(const State&, MobilizerUIndex which) const;


    void setOneQ(State&, int which, Real v) const;
    void setOneU(State&, int which, Real v) const;

    void setQFromVector(State& s, const Vector& v) const;
    void setUFromVector(State& s, const Vector& v) const;


    void setQToFitTransform      (State&, const Transform& X_FM) const;
    void setQToFitRotation       (State&, const Rotation&  R_FM) const;
    void setQToFitTranslation    (State&, const Vec3&      p_FM) const;

    void setUToFitVelocity          (State&, const SpatialVec& V_FM) const;
    void setUToFitAngularVelocity   (State&, const Vec3&       w_FM) const;
    void setUToFitLinearVelocity    (State&, const Vec3&       v_FM) const;

    SpatialVec getHCol(const State& s, MobilizerUIndex ux) const;

    SpatialVec getH_FMCol(const State& s, MobilizerUIndex ux) const;

    // End of State Access Methods.

        // BASIC OPERATORS //




    Transform findBodyTransformInAnotherBody(const State& s, 
                                             const MobilizedBody& inBodyA) const
    {
        const Transform& X_GA = inBodyA.getBodyTransform(s);
        const Transform& X_GB = this->getBodyTransform(s);

        return ~X_GA*X_GB; // X_AB=X_AG*X_GB
    }

    Rotation findBodyRotationInAnotherBody(const State& s, 
                                           const MobilizedBody& inBodyA) const
    {
        const Rotation& R_GA = inBodyA.getBodyRotation(s);
        const Rotation& R_GB = this->getBodyRotation(s);

        return ~R_GA*R_GB; // R_AB=R_AG*R_GB
    }

    Vec3 findBodyOriginLocationInAnotherBody(const State& s, 
                                             const MobilizedBody& toBodyA) const
    {   return toBodyA.findStationAtGroundPoint(s,getBodyOriginLocation(s)); }

    SpatialVec findBodyVelocityInAnotherBody(const State& s,
                                             const MobilizedBody& inBodyA) const
    {
        const SpatialVec& V_GB   = this->getBodyVelocity(s);
        const SpatialVec& V_GA   = inBodyA.getBodyVelocity(s);
        const Vec3        w_AB_G = V_GB[0]-V_GA[0]; // angular velocity of B in A, exp in G   ( 3 flops)

        // Angular velocity was easy, but for linear velocity we have to add in an wXr term.
        const Transform&  X_GB       = getBodyTransform(s);
        const Transform&  X_GA       = inBodyA.getBodyTransform(s);
        const Vec3        p_AB_G     = X_GB.p() - X_GA.p(); // vector from OA to OB, exp in G ( 3 flops)
        const Vec3        p_AB_G_dot = V_GB[1]  - V_GA[1];  // d/dt p taken in G              ( 3 flops)

        const Vec3 v_AB_G = p_AB_G_dot - V_GA[0] % p_AB_G;  // d/dt p taken in A, exp in G    (12 flops)

        // We're done, but the answer is expressed in Ground. Reexpress in A and return.
        return ~X_GA.R()*SpatialVec(w_AB_G, v_AB_G);        //                                (30 flops)
    }

    Vec3 findBodyAngularVelocityInAnotherBody(const State& s,
                                       const MobilizedBody& inBodyA) const 
    {
        const Vec3& w_GB   = getBodyAngularVelocity(s);
        const Vec3& w_GA   = inBodyA.getBodyAngularVelocity(s);
        const Vec3  w_AB_G = w_GB-w_GA; // angular velocity of B in A, exp in G ( 3 flops)

        // Now reexpress in A.
        return inBodyA.expressGroundVectorInBodyFrame(s, w_AB_G); //            (15 flops)
    }

    Vec3 findBodyOriginVelocityInAnotherBody(const State& s,
                                      const MobilizedBody& inBodyA) const
    {
        // Doesn't save much to special case this one.
        return findBodyVelocityInAnotherBody(s,inBodyA)[1];
    }

    SpatialVec findBodyAccelerationInAnotherBody(const State& s,
                                                 const MobilizedBody& inBodyA) const
    {
        const Vec3&       p_GB = this->getBodyOriginLocation(s);
        const Transform&  X_GA = inBodyA.getBodyTransform(s);
        const SpatialVec& V_GB = this->getBodyVelocity(s);
        const SpatialVec& V_GA = inBodyA.getBodyVelocity(s);
        const SpatialVec& A_GB = this->getBodyAcceleration(s);
        const SpatialVec& A_GA = inBodyA.getBodyAcceleration(s);
        const Vec3&       p_GA = X_GA.p();
        const Vec3&       w_GA = V_GA[0];
        const Vec3&       w_GB = V_GB[0];
        const Vec3&       b_GA = A_GA[0];
        const Vec3&       b_GB = A_GB[0];

        const Vec3 p_AB_G        = p_GB     - p_GA;         // vector from OA to OB, in G   ( 3 flops)
        const Vec3 p_AB_G_dot    = V_GB[1]  - V_GA[1];      // d/dt p taken in G            ( 3 flops)
        const Vec3 p_AB_G_dotdot = A_GB[1]  - A_GA[1];      // d^2/dt^2 taken in G          ( 3 flops)

        const Vec3 w_AB_G     = w_GB - w_GA;                // relative ang. vel. of B in A, exp. in G (3 flops)
        const Vec3 v_AB_G     = p_AB_G_dot - w_GA % p_AB_G; // d/dt p taken in A, exp in G  (12 flops)

        const Vec3 w_AB_G_dot = b_GB - b_GA;                // d/dt of w_AB_G taken in G    ( 3 flops)
        const Vec3 v_AB_G_dot = p_AB_G_dotdot - (b_GA % p_AB_G + w_GA % p_AB_G_dot); // d/dt v_AB_G taken in G
                                                                                     //     (24 flops)

        // We have the derivative in G; change it to derivative in A by adding in contribution caused
        // by motion of G in A, that is w_AG X w_AB_G. (Note that w_AG=-w_GA.)
        const Vec3 b_AB_G = w_AB_G_dot - w_GA % w_AB_G; // ang. accel. of B in A            (12 flops)
        const Vec3 a_AB_G = v_AB_G_dot - w_GA % v_AB_G; // taken in A, exp. in G            (12 flops)

        return ~X_GA.R() * SpatialVec(b_AB_G, a_AB_G); // taken in A, expressed in A        (30 flops)
    }

    Vec3 findBodyAngularAccelerationInAnotherBody(const State& s,
                                                  const MobilizedBody& inBodyA) const
    {
        const Rotation& R_GA = inBodyA.getBodyRotation(s);
        const Vec3&     w_GA = inBodyA.getBodyAngularVelocity(s);
        const Vec3&     w_GB = this->getBodyAngularVelocity(s);
        const Vec3&     b_GA = inBodyA.getBodyAngularAcceleration(s);
        const Vec3&     b_GB = this->getBodyAngularAcceleration(s);

        const Vec3 w_AB_G     = w_GB - w_GA; // relative ang. vel. of B in A, exp. in G (3 flops)
        const Vec3 w_AB_G_dot = b_GB - b_GA; // d/dt of w_AB_G taken in G    ( 3 flops)

        // We have the derivative in G; change it to derivative in A by adding 
        // in contribution caused by motion of G in A, that is w_AG X w_AB_G. 
        // (Note that w_AG=-w_GA.)
        const Vec3 b_AB_G = w_AB_G_dot - w_GA % w_AB_G; // ang. accel. of B in A            (12 flops)

        return ~R_GA * b_AB_G; // taken in A, expressed in A                                (15 flops)
    }

    Vec3 findBodyOriginAccelerationInAnotherBody(const State& s, 
                                          const MobilizedBody& inBodyA) const
    {
        // Not much to be saved by trying to optimize this since the linear part
        // is the most expensive to calculate.
        return findBodyAccelerationInAnotherBody(s,inBodyA)[1];
    }

    Vec3 findStationLocationInGround(const State& s, const Vec3& stationOnB) const {
        return getBodyTransform(s) * stationOnB;
    }


    Vec3 findStationLocationInAnotherBody(const State& s, const Vec3& stationOnB, 
                               const MobilizedBody& toBodyA) const
    {
        return toBodyA.findStationAtGroundPoint(s, findStationLocationInGround(s,stationOnB));
    }

    Vec3 findStationVelocityInGround(const State& s, const Vec3& stationOnB) const {
        const Vec3& w = getBodyAngularVelocity(s); // in G
        const Vec3& v = getBodyOriginVelocity(s);  // in G
        const Vec3  r = expressVectorInGroundFrame(s,stationOnB); // 15 flops
        return v + w % r;                                         // 12 flops
    }


    Vec3 findStationVelocityInAnotherBody(const State& s, 
                                          const Vec3&          stationOnBodyB, // p_BS
                                          const MobilizedBody& inBodyA) const
    {
        const SpatialVec V_AB   = findBodyVelocityInAnotherBody(s,inBodyA); // (51 flops)
         // OB->S rexpressed in A but not shifted to OA
        const Vec3       p_BS_A = expressVectorInAnotherBodyFrame(s, stationOnBodyB, inBodyA);
                                                                            // (30 flops)
        return V_AB[1] + (V_AB[0] % p_BS_A);                                // (12 flops)
    }

      
    Vec3 findStationAccelerationInGround(const State& s, const Vec3& stationOnB) const {
        const Vec3& w = getBodyAngularVelocity(s);     // in G
        const Vec3& b = getBodyAngularAcceleration(s); // in G
        const Vec3& a = getBodyOriginAcceleration(s);  // in G

        const Vec3  r = expressVectorInGroundFrame(s,stationOnB); // 15 flops
        return a + b % r + w % (w % r);                           // 33 flops
    }

    Vec3 findStationAccelerationInAnotherBody(const State& s,
                                              const Vec3&          stationOnBodyB, 
                                              const MobilizedBody& inBodyA) const 
    {
        const Vec3       w_AB = findBodyAngularVelocityInAnotherBody(s,inBodyA);  // ( 18 flops)
        const SpatialVec A_AB = findBodyAccelerationInAnotherBody(s,inBodyA);     // (105 flops)
         // OB->S rexpressed in A but not shifted to OA
        const Vec3       p_BS_A = expressVectorInAnotherBodyFrame(s, stationOnBodyB, inBodyA);
                                                                                  // ( 30 flops)

        return A_AB[1] + (A_AB[0] % p_BS_A) + w_AB % (w_AB % p_BS_A);             // ( 33 flops)
    }

    void findStationLocationAndVelocityInGround(const State& s, const Vec3& locationOnB,
                                                Vec3& locationOnGround, Vec3& velocityInGround) const
    {
        const Vec3& p_GB   = getBodyOriginLocation(s);
        const Vec3  p_BS_G = expressVectorInGroundFrame(s,locationOnB); // 15 flops
        locationOnGround = p_GB + p_BS_G;                               //  3 flops

        const Vec3& w_GB = getBodyAngularVelocity(s);
        const Vec3& v_GB = getBodyOriginVelocity(s);
        velocityInGround = v_GB + w_GB % p_BS_G;                        // 12 flops
    }


    void findStationLocationVelocityAndAccelerationInGround
       (const State& s, const Vec3& locationOnB,
        Vec3& locationOnGround, Vec3& velocityInGround, Vec3& accelerationInGround) const
    {
        const Rotation&  R_GB = getBodyRotation(s);
        const Vec3&      p_GB = getBodyOriginLocation(s);

        const Vec3 r = R_GB*locationOnB; // re-express station vector p_BS in G (15 flops)
        locationOnGround  = p_GB + r;    // 3 flops

        const Vec3& w = getBodyAngularVelocity(s);      // in G
        const Vec3& v = getBodyOriginVelocity(s);       // in G
        const Vec3& b = getBodyAngularAcceleration(s);  // in G
        const Vec3& a = getBodyOriginAcceleration(s);   // in G

        const Vec3 wXr = w % r; // "whipping" velocity w X r due to angular velocity (9 flops)
        velocityInGround     = v + wXr;                 // v + w X r (3 flops)
        accelerationInGround = a + b % r + w % wXr;     // 24 flops
    }

    Vec3 findMassCenterLocationInGround(const State& s) const {
        return findStationLocationInGround(s,getBodyMassCenterStation(s));
    }

    Vec3 findMassCenterLocationInAnotherBody(const State& s, const MobilizedBody& toBodyA) const {
        return findStationLocationInAnotherBody(s,getBodyMassCenterStation(s),toBodyA);
    }

    Vec3 findStationAtGroundPoint(const State& s, const Vec3& locationInG) const {
        return ~getBodyTransform(s) * locationInG;
    }

    Vec3 findStationAtAnotherBodyStation(const State& s, const MobilizedBody& fromBodyA, 
                                const Vec3& stationOnA) const {
        return fromBodyA.findStationLocationInAnotherBody(s, stationOnA, *this);
    }

    Vec3 findStationAtAnotherBodyOrigin(const State& s, const MobilizedBody& fromBodyA) const {
        return findStationAtGroundPoint(s,fromBodyA.getBodyOriginLocation(s));
    }

    Vec3 findStationAtAnotherBodyMassCenter(const State& s, const MobilizedBody& fromBodyA) const {
        return fromBodyA.findStationLocationInAnotherBody(s,getBodyMassCenterStation(s),*this);
    }

    Transform findFrameTransformInGround
       (const State& s, const Transform& frameOnB) const {
        return getBodyTransform(s) * frameOnB;
    }

    SpatialVec findFrameVelocityInGround
       (const State& s, const Transform& frameOnB) const {
        return SpatialVec(getBodyAngularVelocity(s),
                          findStationVelocityInGround(s,frameOnB.p()));
    }

    SpatialVec findFrameAccelerationInGround
       (const State& s, const Transform& frameOnB) const {
        return SpatialVec(getBodyAngularAcceleration(s),
                          findStationAccelerationInGround(s,frameOnB.p()));
    }

    Vec3 expressVectorInGroundFrame(const State& s, const Vec3& vectorInB) const {
        return getBodyRotation(s)*vectorInB;
    }

    Vec3 expressGroundVectorInBodyFrame(const State& s, const Vec3& vectorInG) const {
        return ~getBodyRotation(s)*vectorInG;
    }

    Vec3 expressVectorInAnotherBodyFrame(const State& s, const Vec3& vectorInB,
                                         const MobilizedBody& inBodyA) const
    {
        return inBodyA.expressGroundVectorInBodyFrame(s, expressVectorInGroundFrame(s,vectorInB));
    }

    MassProperties expressMassPropertiesInGroundFrame(const State& s) const {
        const MassProperties& M_OB_B = getBodyMassProperties(s);
        const Rotation&       R_GB   = getBodyRotation(s);
        return M_OB_B.reexpress(~R_GB);
    }

    MassProperties expressMassPropertiesInAnotherBodyFrame
       (const State& s, const MobilizedBody& inBodyA) const 
    {
        const MassProperties& M_OB_B = getBodyMassProperties(s);
        const Rotation        R_AB   = findBodyRotationInAnotherBody(s,inBodyA);
        return M_OB_B.reexpress(~R_AB);
    }

    // End of Basic Operators.




    SpatialMat calcBodySpatialInertiaMatrixInGround(const State& s) const
    {
        if (isGround())
            return SpatialMat(Mat33(Infinity)); // sets diagonals to Inf

        const MassProperties& mp   = getBodyMassProperties(s);
        const Rotation&       R_GB = getBodyRotation(s);
         // re-express in Ground without shifting, convert to spatial mat.
        return mp.reexpress(~R_GB).toSpatialMat();
    }

    Inertia calcBodyCentralInertia(const State& s, 
                                   MobilizedBodyIndex objectBodyB) const
    {
        return getBodyMassProperties(s).calcCentralInertia();
    }

    Inertia calcBodyInertiaAboutAnotherBodyStation(const State& s,
                                                   const MobilizedBody& inBodyA, 
                                                   const Vec3&          aboutLocationOnBodyA) const
    {
        // get B's mass props MB, measured about OB, exp. in B
        const MassProperties& MB_OB_B = getBodyMassProperties(s);

        // Calculate the vector from the body B origin (current "about" point) to the new "about" point PA,
        // expressed in B.
        const Vec3 r_OB_PA = findStationAtAnotherBodyStation(s, inBodyA, aboutLocationOnBodyA);

        // Now shift the "about" point for body B's inertia IB to PA, but still expressed in B.
        const Inertia IB_PA_B = MB_OB_B.calcShiftedInertia(r_OB_PA);
        
        // Finally reexpress the inertia in the A frame.
        const Rotation R_BA    = inBodyA.findBodyRotationInAnotherBody(s, *this);
        const Inertia  IB_PA_A = IB_PA_B.reexpress(R_BA);
        return IB_PA_A;
    }


    SpatialVec calcBodyMomentumAboutBodyOriginInGround(const State& s) {
        const MassProperties M_OB_G = expressMassPropertiesInGroundFrame(s);
        const SpatialVec&    V_GB   = getBodyVelocity(s);
        return M_OB_G.toSpatialMat() * V_GB;
    }

    SpatialVec calcBodyMomentumAboutBodyMassCenterInGround(const State& s) const {
        const MassProperties& M_OB_B = getBodyMassProperties(s);
        const Rotation&       R_GB   = getBodyRotation(s);

        // Given a central inertia matrix I, angular velocity w, and mass center velocity v,
        // the central angular momentum is Iw and linear momentum is mv.
        const Inertia I_CB_B = M_OB_B.calcCentralInertia();
        const Inertia I_CB_G = I_CB_B.reexpress(~R_GB);
        const Real    mb     = M_OB_B.getMass();
        const Vec3&   w_GB   = getBodyAngularVelocity(s);
        Vec3          v_G_CB = findStationVelocityInGround(s, M_OB_B.getMassCenter());

        return SpatialVec( I_CB_G*w_GB, mb*v_G_CB );
    }

    Real calcStationToStationDistance(const State& s,
                                      const Vec3&          locationOnBodyB,
                                      const MobilizedBody& bodyA,
                                      const Vec3&          locationOnBodyA) const
    {
        if (isSameMobilizedBody(bodyA))
            return (locationOnBodyA-locationOnBodyB).norm();

        const Vec3 r_OG_PB = this->findStationLocationInGround(s,locationOnBodyB);
        const Vec3 r_OG_PA = bodyA.findStationLocationInGround(s,locationOnBodyA);
        return (r_OG_PA - r_OG_PB).norm();
    }

    Real calcStationToStationDistanceTimeDerivative(const State& s,
                                                    const Vec3&          locationOnBodyB,
                                                    const MobilizedBody& bodyA,
                                                    const Vec3&          locationOnBodyA) const
    {
        if (isSameMobilizedBody(bodyA))
            return 0;

        Vec3 rB, rA, vB, vA;
        this->findStationLocationAndVelocityInGround(s,locationOnBodyB,rB,vB);
        bodyA.findStationLocationAndVelocityInGround(s,locationOnBodyA,rA,vA);
        const Vec3 r = rA-rB, v = vA-vB;
        const Real d = r.norm();

        // When the points are coincident, the rate of change of distance is just their relative speed.
        // Otherwise, it is the speed along the direction of separation. 
        if (d==0) return v.norm();
        else return dot(v, r/d);
    }


    Real calcStationToStationDistance2ndTimeDerivative(const State& s,
                                                       const Vec3&          locationOnBodyB,
                                                       const MobilizedBody& bodyA,
                                                       const Vec3&          locationOnBodyA) const
    {
        if (isSameMobilizedBody(bodyA))
            return 0;

        Vec3 rB, rA, vB, vA, aB, aA;
        this->findStationLocationVelocityAndAccelerationInGround(s,locationOnBodyB,rB,vB,aB);
        bodyA.findStationLocationVelocityAndAccelerationInGround(s,locationOnBodyA,rA,vA,aA);

        const Vec3 r = rA-rB, v = vA-vB, a = aA-aB;
        const Real d = r.norm();
        
        // This method is the time derivative of calcFixedPointToPointDistanceTimeDerivative(), so it
        // must follow the same two cases. That is, when the points are coincident the change in 
        // separation rate is the time derivative of the speed |v|, otherwise it is the time
        // derivative of the speed along the separation vector.

        if (d==0) {
            // Return d/dt |v|. This has two cases: if |v| is zero, the rate of change of speed is
            // just the points' relative acceleration magnitude. Otherwise, it is the acceleration
            // in the direction of the current relative velocity vector.
            const Real s = v.norm(); // speed
            if (s==0) return a.norm();
            else return dot(a, v/s);
        }

        // Points are separated.
        const Vec3 u = r/d;             // u is the separation direction (a unit vector from B to A) 
        const Vec3 vp = v - dot(v,u)*u; // velocity perpendicular to separation direction
        return dot(a,u) + dot(vp,v)/d;
    }


    Vec3 calcBodyMovingPointVelocityInBody(const State& s,
                                           const Vec3& locationOnBodyB, 
                                           const Vec3& velocityOnBodyB,
                                           const MobilizedBody& inBodyA) const
    {
        SimTK_ASSERT_ALWAYS(!"unimplemented method", 
            "MobilizedBody::calcBodyMovingPointVelocityInBody() is not yet implemented -- any volunteers?");
        return Vec3::getNaN();
    }


    Vec3 calcBodyMovingPointAccelerationInBody(const State& s, 
                                               const Vec3&          locationOnBodyB, 
                                               const Vec3&          velocityOnBodyB, 
                                               const Vec3&          accelerationOnBodyB,
                                               const MobilizedBody& inBodyA) const
    {
        SimTK_ASSERT_ALWAYS(!"unimplemented method", 
            "MobilizedBody::calcBodyMovingPointAccelerationInBody() is not yet implemented -- any volunteers?");
        return Vec3::getNaN();
    }

    Real calcMovingPointToPointDistanceTimeDerivative(const State& s,
                                                      const Vec3&          locationOnBodyB,
                                                      const Vec3&          velocityOnBodyB,
                                                      const MobilizedBody& bodyA,
                                                      const Vec3&          locationOnBodyA,
                                                      const Vec3&          velocityOnBodyA) const
    {
        SimTK_ASSERT_ALWAYS(!"unimplemented method", 
            "MobilizedBody::calcMovingPointToPointDistanceTimeDerivative() is not yet implemented -- any volunteers?");
        return NaN;
    }

    Real calcMovingPointToPointDistance2ndTimeDerivative(const State& s,
                                                         const Vec3&          locationOnBodyB,
                                                         const Vec3&          velocityOnBodyB,
                                                         const Vec3&          accelerationOnBodyB,
                                                         const MobilizedBody& bodyA,
                                                         const Vec3&          locationOnBodyA,
                                                         const Vec3&          velocityOnBodyA,
                                                         const Vec3&          accelerationOnBodyA) const
    {
        SimTK_ASSERT_ALWAYS(!"unimplemented method", 
            "MobilizedBody::calcMovingPointToPointDistance2ndTimeDerivative() is not yet implemented -- any volunteers?");
        return NaN;
    }


    // End of High Level Operators.


            // CONSTRUCTION METHODS //

    MobilizedBody();

    explicit MobilizedBody(MobilizedBodyImpl* r);



    MobilizedBody& addBodyDecoration(const Transform& X_BD, const DecorativeGeometry& g) {
        (void)updBody().addDecoration(X_BD,g);
        return *this;
    }

    MobilizedBody& addOutboardDecoration(const Transform& X_MD,  const DecorativeGeometry&);

    MobilizedBody& addInboardDecoration (const Transform& X_FD, const DecorativeGeometry&);

    const Body& getBody() const;
    Body& updBody();

    MobilizedBody& setBody(const Body&);

    MobilizedBody& setDefaultMassProperties(const MassProperties& m) {
        updBody().setDefaultRigidBodyMassProperties(m); // might not be allowed
        return *this;
    }

    const MassProperties& getDefaultMassProperties() const {
        return getBody().getDefaultRigidBodyMassProperties(); // every body type can do this
    }

    void adoptMotion(Motion& ownerHandle);

    void clearMotion();

    bool hasMotion() const;

    const Motion& getMotion() const;

    MobilizedBody& setDefaultInboardFrame (const Transform& X_PF);
    MobilizedBody& setDefaultOutboardFrame(const Transform& X_BM);

    const Transform& getDefaultInboardFrame()  const; // X_PF
    const Transform& getDefaultOutboardFrame() const; // X_BM

    operator MobilizedBodyIndex() const {return getMobilizedBodyIndex();}

    MobilizedBodyIndex     getMobilizedBodyIndex()  const;

    const MobilizedBody&   getParentMobilizedBody() const;

    const MobilizedBody&   getBaseMobilizedBody()   const;

    const SimbodyMatterSubsystem& getMatterSubsystem() const;
    SimbodyMatterSubsystem&       updMatterSubsystem();

    bool isInSubsystem() const;

    bool isInSameSubsystem(const MobilizedBody&) const;

    bool isSameMobilizedBody(const MobilizedBody& mBody) const;

    bool isGround() const;

    int getLevelInMultibodyTree() const;
    
    MobilizedBody& cloneForNewParent(MobilizedBody& parent) const;


        // Utility operators //

    Real  getOneFromQPartition(const State&, int which, const Vector& qlike) const;

    Real& updOneFromQPartition(const State&, int which, Vector& qlike) const;

    Real  getOneFromUPartition(const State&, int which, const Vector& ulike) const;

    Real& updOneFromUPartition(const State&, int which, Vector& ulike) const;

    void applyOneMobilityForce(const State& s, int which, Real f, 
                               Vector& mobilityForces) const
    {
        updOneFromUPartition(s,which,mobilityForces) += f;
    }

    void applyBodyForce(const State& s, const SpatialVec& spatialForceInG, 
                        Vector_<SpatialVec>& bodyForcesInG) const;

    void applyBodyTorque(const State& s, const Vec3& torqueInG, 
                         Vector_<SpatialVec>& bodyForcesInG) const;

    void applyForceToBodyPoint(const State& s, const Vec3& pointInB, const Vec3& forceInG,
                               Vector_<SpatialVec>& bodyForcesInG) const;

    // End of Construction and Misc Methods.

        // BUILT IN MOBILIZER DECLARATIONS //

    // These are the built-in MobilizedBody types. Types on the same line are
    // synonymous. Each of these has a known number of coordinates and speeds 
    // (at least a default number) so
    // can define routines which return and accept specific-size arguments, e.g.
    // Real (for 1-dof mobilizer) and Vec5 (for 5-dof mobilizer). Here is the
    // conventional interface that each built-in should provide. The base type
    // provides similar routines but using variable-sized or "one at a time"
    // arguments. (Vec<1> here will actually be a Real; assume the built-in
    // MobilizedBody class is "BuiltIn")
    //
    //    BuiltIn&       setDefaultQ(const Vec<nq>&);
    //    const Vec<nq>& getDefaultQ() const;
    //
    //    const Vec<nq>& getQ[Dot[Dot]](const State&) const;
    //    const Vec<nu>& getU[Dot](const State&) const;
    //
    //    void setQ(State&, const Vec<nq>&) const;
    //    void setU(State&, const Vec<nu>&) const;
    //
    //    const Vec<nq>& getMyPartQ(const State&, const Vector& qlike) const;
    //    const Vec<nu>& getMyPartU(const State&, const Vector& ulike) const;
    //   
    //    Vec<nq>& updMyPartQ(const State&, Vector& qlike) const;
    //    Vec<nu>& updMyPartU(const State&, Vector& ulike) const;
    //      


    class Pin;              typedef Pin             Torsion;
    class Universal;
    class Cylinder;

    class Weld;
    class Slider;           typedef Slider          Prismatic;
    class Translation2D;    typedef Translation2D   Cartesian2D, CartesianCoords2D;
    class Translation;      typedef Translation     Cartesian, CartesianCoords;
    class BendStretch;      typedef BendStretch     PolarCoords;
    class TorsionStretch;   typedef TorsionStretch  ConicalCoords2D;
    class SphericalCoords;
    class CylindricalCoords;
    class LineOrientation;


    class Planar;
    class Gimbal;
    class Ball;             typedef Ball            Orientation, Spherical;
    class Free;
    class FreeLine;
    class Screw;
    class Ellipsoid;
    class Custom;
    class Ground;
    class FunctionBased;
    
    class PinImpl;
    class SliderImpl;
    class UniversalImpl;
    class CylinderImpl;
    class BendStretchImpl;
    class TorsionStretchImpl;
    class PlanarImpl;
    class GimbalImpl;
    class BallImpl;
    class TranslationImpl;
    class SphericalCoordsImpl;
    class FreeImpl;
    class LineOrientationImpl;
    class FreeLineImpl;
    class WeldImpl;
    class ScrewImpl;
    class EllipsoidImpl;
    class CustomImpl;
    class GroundImpl;
    class FunctionBasedImpl;

};

class SimTK_SIMBODY_EXPORT MobilizedBody::Pin : public MobilizedBody {
public:
        // SPECIALIZED INTERFACE FOR PIN MOBILIZER

    // "Angle" is just a nicer name for a pin joint's lone generalized coordinate q.
    Pin& setDefaultAngle(Real angleInRadians) {return setDefaultQ(angleInRadians);}
    Real getDefaultAngle() const              {return getDefaultQ();}

        // Friendly, mobilizer-specific access to generalized coordinates and speeds.

    void setAngle(State& s, Real angleInRadians) {setQ(s, angleInRadians);}
    Real getAngle(const State& s) const {return getQ(s);}

    void setRate(State& s, Real rateInRadiansPerTime) {setU(s, rateInRadiansPerTime);}
    Real getRate(const State& s) const {return getU(s);}

    // Mobility forces are "u-like", that is, one per dof.
    Real getAppliedPinTorque(const State& s, const Vector& mobilityForces) const {
        return getMyPartU(s,mobilityForces);
    }
    void applyPinTorque(const State& s, Real torque, Vector& mobilityForces) const {
        updMyPartU(s,mobilityForces) += torque;
    }

        // STANDARDIZED MOBILIZED BODY INTERFACE

        // required constructors
    explicit Pin(Direction=Forward);
    Pin(MobilizedBody& parent, const Body&, Direction=Forward);
    Pin(MobilizedBody& parent, const Transform& inbFrame,
        const Body&,           const Transform& outbFrame,
        Direction=Forward);

        // access to generalized coordinates q and generalized speeds u
    Pin& setDefaultQ(Real);
    Real getDefaultQ() const;

    Real getQ(const State&) const;
    Real getQDot(const State&) const;
    Real getQDotDot(const State&) const;
    Real getU(const State&) const;
    Real getUDot(const State&) const;

    void setQ(State&, Real) const;
    void setU(State&, Real) const;

    Real getMyPartQ(const State&, const Vector& qlike) const;
    Real getMyPartU(const State&, const Vector& ulike) const;
   
    Real& updMyPartQ(const State&, Vector& qlike) const;
    Real& updMyPartU(const State&, Vector& ulike) const;

        // specialize return type for convenience
    Pin& addBodyDecoration(const Transform& X_BD, const DecorativeGeometry& g)
      { (void)MobilizedBody::addBodyDecoration(X_BD,g); return *this; }
    Pin& addOutboardDecoration(const Transform& X_MD,  const DecorativeGeometry& g)
      { (void)MobilizedBody::addOutboardDecoration(X_MD,g); return *this; }
    Pin& addInboardDecoration (const Transform& X_FD, const DecorativeGeometry& g)
      { (void)MobilizedBody::addInboardDecoration(X_FD,g); return *this; }
    Pin& setDefaultInboardFrame(const Transform& X_PF)
      { (void)MobilizedBody::setDefaultInboardFrame(X_PF); return *this; }
    Pin& setDefaultOutboardFrame(const Transform& X_BM)
      { (void)MobilizedBody::setDefaultOutboardFrame(X_BM); return *this; }
    SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(Pin, PinImpl, MobilizedBody);
};

class SimTK_SIMBODY_EXPORT MobilizedBody::Slider : public MobilizedBody {
public:
        // SPECIALIZED INTERFACE FOR SLIDER MOBILIZER

    // "Length" is just a nicer name for a sliding joint's lone generalized coordinate q.
    Slider& setDefaultLength(Real length) {return setDefaultQ(length);}
    Real getDefaultLength() const         {return getDefaultQ();}

        // Friendly, mobilizer-specific access to generalized coordinates and speeds.

    void setLength(State& s, Real length) {setQ(s, length);}
    Real getLength(const State& s) const {return getQ(s);}

    void setRate(State& s, Real rateInLengthPerTime) {setU(s, rateInLengthPerTime);}
    Real getRate(const State& s) const {return getU(s);}

    // Mobility forces are "u-like", that is, one per dof.
    Real getAppliedForce(const State& s, const Vector& mobilityForces) const {
        return getMyPartU(s,mobilityForces);
    }
    void applyForce(const State& s, Real force, Vector& mobilityForces) const {
        updMyPartU(s,mobilityForces) += force;
    }

        // STANDARDIZED MOBILIZED BODY INTERFACE

        // required constructors
    explicit Slider(Direction=Forward);
    Slider(MobilizedBody& parent, const Body&, Direction=Forward);
    Slider(MobilizedBody& parent, const Transform& inbFrame,
           const Body&,           const Transform& outbFrame, Direction=Forward);

        // access to generalized coordinates q and generalized speeds u
    Slider& setDefaultQ(Real);
    Real getDefaultQ() const;

    Real getQ(const State&) const;
    Real getQDot(const State&) const;
    Real getQDotDot(const State&) const;
    Real getU(const State&) const;
    Real getUDot(const State&) const;

    void setQ(State&, Real) const;
    void setU(State&, Real) const;

    Real getMyPartQ(const State&, const Vector& qlike) const;
    Real getMyPartU(const State&, const Vector& ulike) const;
   
    Real& updMyPartQ(const State&, Vector& qlike) const;
    Real& updMyPartU(const State&, Vector& ulike) const;

        // specialize return type for convenience
    Slider& addBodyDecoration(const Transform& X_BD, const DecorativeGeometry& g)
      { (void)MobilizedBody::addBodyDecoration(X_BD,g); return *this; }
    Slider& addOutboardDecoration(const Transform& X_MD,  const DecorativeGeometry& g)
      { (void)MobilizedBody::addOutboardDecoration(X_MD,g); return *this; }
    Slider& addInboardDecoration (const Transform& X_FD, const DecorativeGeometry& g)
      { (void)MobilizedBody::addInboardDecoration(X_FD,g); return *this; }
    Slider& setDefaultInboardFrame(const Transform& X_PF)
      { (void)MobilizedBody::setDefaultInboardFrame(X_PF); return *this; }
    Slider& setDefaultOutboardFrame(const Transform& X_BM)
      { (void)MobilizedBody::setDefaultOutboardFrame(X_BM); return *this; }
    SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(Slider, SliderImpl, MobilizedBody);
};

class SimTK_SIMBODY_EXPORT MobilizedBody::Screw : public MobilizedBody {
public:
    explicit Screw(Real pitch, Direction=Forward);

    Screw(MobilizedBody& parent, const Body&, Real pitch, Direction=Forward);

    Screw(MobilizedBody& parent, const Transform& inbFrame,
         const Body&,           const Transform& outbFrame,
         Real pitch, Direction=Forward);

    Screw& addBodyDecoration(const Transform& X_BD, const DecorativeGeometry& g) {
        (void)MobilizedBody::addBodyDecoration(X_BD,g); return *this;
    }
    Screw& addOutboardDecoration(const Transform& X_MD,  const DecorativeGeometry& g) {
        (void)MobilizedBody::addOutboardDecoration(X_MD,g); return *this;
    }
    Screw& addInboardDecoration (const Transform& X_FD, const DecorativeGeometry& g) {
        (void)MobilizedBody::addInboardDecoration(X_FD,g); return *this;
    }

    Screw& setDefaultInboardFrame(const Transform& X_PF) {
        (void)MobilizedBody::setDefaultInboardFrame(X_PF); return *this;
    }

    Screw& setDefaultOutboardFrame(const Transform& X_BM) {
        (void)MobilizedBody::setDefaultOutboardFrame(X_BM); return *this;
    }

    Screw& setDefaultPitch(Real pitch);
    Real   getDefaultPitch() const;

    Screw& setDefaultQ(Real);
    Real   getDefaultQ() const;

    Real getQ(const State&) const;
    Real getQDot(const State&) const;
    Real getQDotDot(const State&) const;
    Real getU(const State&) const;
    Real getUDot(const State&) const;

    void setQ(State&, Real) const;
    void setU(State&, Real) const;

    Real getMyPartQ(const State&, const Vector& qlike) const;
    Real getMyPartU(const State&, const Vector& ulike) const;
   
    Real& updMyPartQ(const State&, Vector& qlike) const;
    Real& updMyPartU(const State&, Vector& ulike) const;
    SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(Screw, ScrewImpl, MobilizedBody);
};

class SimTK_SIMBODY_EXPORT MobilizedBody::Universal : public MobilizedBody {
public:
    explicit Universal(Direction=Forward);

    Universal(MobilizedBody& parent, const Body&, Direction=Forward);

    Universal(MobilizedBody& parent, const Transform& inbFrame,
              const Body&,           const Transform& outbFrame, Direction=Forward);

    Universal& addBodyDecoration(const Transform& X_BD, const DecorativeGeometry& g) {
        (void)MobilizedBody::addBodyDecoration(X_BD,g); return *this;
    }
    Universal& addOutboardDecoration(const Transform& X_MD,  const DecorativeGeometry& g) {
        (void)MobilizedBody::addOutboardDecoration(X_MD,g); return *this;
    }
    Universal& addInboardDecoration (const Transform& X_FD, const DecorativeGeometry& g) {
        (void)MobilizedBody::addInboardDecoration(X_FD,g); return *this;
    }

    Universal& setDefaultInboardFrame(const Transform& X_PF) {
        (void)MobilizedBody::setDefaultInboardFrame(X_PF); return *this;
    }

    Universal& setDefaultOutboardFrame(const Transform& X_BM) {
        (void)MobilizedBody::setDefaultOutboardFrame(X_BM); return *this;
    }
    SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(Universal, UniversalImpl, MobilizedBody);
};

class SimTK_SIMBODY_EXPORT MobilizedBody::Cylinder : public MobilizedBody {
public:
    explicit Cylinder(Direction=Forward);

    Cylinder(MobilizedBody& parent, const Body&, Direction=Forward);

    Cylinder(MobilizedBody& parent, const Transform& inbFrame,
             const Body&,           const Transform& outbFrame, Direction=Forward);

    Cylinder& addBodyDecoration(const Transform& X_BD, const DecorativeGeometry& g) {
        (void)MobilizedBody::addBodyDecoration(X_BD,g); return *this;
    }
    Cylinder& addOutboardDecoration(const Transform& X_MD,  const DecorativeGeometry& g) {
        (void)MobilizedBody::addOutboardDecoration(X_MD,g); return *this;
    }
    Cylinder& addInboardDecoration (const Transform& X_FD, const DecorativeGeometry& g) {
        (void)MobilizedBody::addInboardDecoration(X_FD,g); return *this;
    }

    Cylinder& setDefaultInboardFrame(const Transform& X_PF) {
        (void)MobilizedBody::setDefaultInboardFrame(X_PF); return *this;
    }

    Cylinder& setDefaultOutboardFrame(const Transform& X_BM) {
        (void)MobilizedBody::setDefaultOutboardFrame(X_BM); return *this;
    }
    SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(Cylinder, CylinderImpl, MobilizedBody);
};

class SimTK_SIMBODY_EXPORT MobilizedBody::BendStretch : public MobilizedBody {
public:
    explicit BendStretch(Direction=Forward);

    BendStretch(MobilizedBody& parent, const Body&, Direction=Forward);

    BendStretch(MobilizedBody& parent, const Transform& inbFrame,
                const Body&,           const Transform& outbFrame, Direction=Forward);

    BendStretch& addBodyDecoration(const Transform& X_BD, const DecorativeGeometry& g) {
        (void)MobilizedBody::addBodyDecoration(X_BD,g); return *this;
    }
    BendStretch& addOutboardDecoration(const Transform& X_MD,  const DecorativeGeometry& g) {
        (void)MobilizedBody::addOutboardDecoration(X_MD,g); return *this;
    }
    BendStretch& addInboardDecoration (const Transform& X_FD, const DecorativeGeometry& g) {
        (void)MobilizedBody::addInboardDecoration(X_FD,g); return *this;
    }

    BendStretch& setDefaultInboardFrame(const Transform& X_PF) {
        (void)MobilizedBody::setDefaultInboardFrame(X_PF); return *this;
    }

    BendStretch& setDefaultOutboardFrame(const Transform& X_BM) {
        (void)MobilizedBody::setDefaultOutboardFrame(X_BM); return *this;
    }
    SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(BendStretch, BendStretchImpl, MobilizedBody);
};

class SimTK_SIMBODY_EXPORT MobilizedBody::Planar : public MobilizedBody {
public:
    explicit Planar(Direction=Forward);

    Planar(MobilizedBody& parent, const Body&, Direction=Forward);

    Planar(MobilizedBody& parent, const Transform& inbFrame,
           const Body&,           const Transform& outbFrame,
           Direction=Forward);

    Planar& addBodyDecoration(const Transform& X_BD, const DecorativeGeometry& g) {
        (void)MobilizedBody::addBodyDecoration(X_BD,g); return *this;
    }
    Planar& addOutboardDecoration(const Transform& X_MD,  const DecorativeGeometry& g) {
        (void)MobilizedBody::addOutboardDecoration(X_MD,g); return *this;
    }
    Planar& addInboardDecoration (const Transform& X_FD, const DecorativeGeometry& g) {
        (void)MobilizedBody::addInboardDecoration(X_FD,g); return *this;
    }

    Planar& setDefaultInboardFrame(const Transform& X_PF) {
        (void)MobilizedBody::setDefaultInboardFrame(X_PF); return *this;
    }

    Planar& setDefaultOutboardFrame(const Transform& X_BM) {
        (void)MobilizedBody::setDefaultOutboardFrame(X_BM); return *this;
    }

    // Friendly, mobilizer-specific access to coordinates and speeds.
    Planar& setDefaultAngle(Real a) {
        Vec3 q = getDefaultQ(); q[0] = a; setDefaultQ(q);
        return *this;
    }
    Planar& setDefaultTranslation(const Vec2& r) {
        Vec3 q = getDefaultQ(); q.updSubVec<2>(1) = r; setDefaultQ(q);
        return *this;
    }

    Real getDefaultAngle() const {return getDefaultQ()[0];}
    const Vec2& getDefaultTranslation() const {return getDefaultQ().getSubVec<2>(1);}

    void setAngle      (State& s, Real        a) {setOneQ(s,0,a);}
    void setTranslation(State& s, const Vec2& r) {setOneQ(s,1,r[0]); setOneQ(s,2,r[1]);}

    Real getAngle(const State& s) const {return getQ(s)[0];}
    const Vec2& getTranslation(const State& s) const {return getQ(s).getSubVec<2>(1);}

    // Generic default state Topology methods.
    const Vec3& getDefaultQ() const;
    Planar& setDefaultQ(const Vec3& q);

    const Vec3& getQ(const State&) const;
    const Vec3& getQDot(const State&) const;
    const Vec3& getQDotDot(const State&) const;
    const Vec3& getU(const State&) const;
    const Vec3& getUDot(const State&) const;

    void setQ(State&, const Vec3&) const;
    void setU(State&, const Vec3&) const;

    const Vec3& getMyPartQ(const State&, const Vector& qlike) const;
    const Vec3& getMyPartU(const State&, const Vector& ulike) const;
   
    Vec3& updMyPartQ(const State&, Vector& qlike) const;
    Vec3& updMyPartU(const State&, Vector& ulike) const;
    SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(Planar, PlanarImpl, MobilizedBody);
};

class SimTK_SIMBODY_EXPORT MobilizedBody::SphericalCoords : public MobilizedBody {
public:
    explicit SphericalCoords(Direction=Forward);

    SphericalCoords(MobilizedBody& parent, const Body&, Direction=Forward);

    SphericalCoords(MobilizedBody& parent, const Transform& inbFrame,
                    const Body&,           const Transform& outbFrame,
                    Direction=Forward);

    SphericalCoords(MobilizedBody& parent,      const Transform& inbFrame,
                    const Body&,                const Transform& outbFrame,
                    Real azimuthOffset,         bool azimuthNegated,
                    Real zenithOffset,          bool zenithNegated,
                    CoordinateAxis radialAxis,  bool radialNegated,
                    Direction=Forward);

    SphericalCoords& addBodyDecoration(const Transform& X_BD, const DecorativeGeometry& g) {
        (void)MobilizedBody::addBodyDecoration(X_BD,g); return *this;
    }
    SphericalCoords& addOutboardDecoration(const Transform& X_MD,  const DecorativeGeometry& g) {
        (void)MobilizedBody::addOutboardDecoration(X_MD,g); return *this;
    }
    SphericalCoords& addInboardDecoration (const Transform& X_FD, const DecorativeGeometry& g) {
        (void)MobilizedBody::addInboardDecoration(X_FD,g); return *this;
    }

    SphericalCoords& setDefaultInboardFrame(const Transform& X_PF) {
        (void)MobilizedBody::setDefaultInboardFrame(X_PF); return *this;
    }

    SphericalCoords& setDefaultOutboardFrame(const Transform& X_BM) {
        (void)MobilizedBody::setDefaultOutboardFrame(X_BM); return *this;
    }

    // Friendly, mobilizer-specific access to coordinates and speeds.
    SphericalCoords& setDefaultAngles(const Vec2& a) {
        Vec3 q = getDefaultQ(); q.updSubVec<2>(0) = a; setDefaultQ(q);
        return *this;
    }
    SphericalCoords& setDefaultRadius(Real r) {
        Vec3 q = getDefaultQ(); q[2] = r; setDefaultQ(q);
        return *this;
    }
    SphericalCoords& setRadialAxis(CoordinateAxis);
    SphericalCoords& setNegateAzimuth(bool);
    SphericalCoords& setNegateZenith(bool);
    SphericalCoords& setNegateRadial(bool);

    const Vec2&    getDefaultAngles()      const {return getDefaultQ().getSubVec<2>(0);}
    Real           getDefaultTranslation() const {return getDefaultQ()[2];}
    
    CoordinateAxis getRadialAxis()    const;
    bool           isAzimuthNegated() const;
    bool           isZenithNegated()  const;
    bool           isRadialNegated()  const;

    void setAngles(State& s, const Vec2& a) {setOneQ(s,0,a[0]); setOneQ(s,1,a[1]);}
    void setRadius(State& s, Real        r) {setOneQ(s,2,r);}

    const Vec2& getAngles(const State& s) const {return getQ(s).getSubVec<2>(0);}
    Real        getRadius(const State& s) const {return getQ(s)[2];}

    // Generic default state Topology methods.
    const Vec3& getDefaultQ() const;
    SphericalCoords& setDefaultQ(const Vec3& q);

    const Vec3& getQ(const State&) const;
    const Vec3& getQDot(const State&) const;
    const Vec3& getQDotDot(const State&) const;
    const Vec3& getU(const State&) const;
    const Vec3& getUDot(const State&) const;

    void setQ(State&, const Vec3&) const;
    void setU(State&, const Vec3&) const;

    const Vec3& getMyPartQ(const State&, const Vector& qlike) const;
    const Vec3& getMyPartU(const State&, const Vector& ulike) const;
   
    Vec3& updMyPartQ(const State&, Vector& qlike) const;
    Vec3& updMyPartU(const State&, Vector& ulike) const;

    SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(SphericalCoords, SphericalCoordsImpl, MobilizedBody);
};

class SimTK_SIMBODY_EXPORT MobilizedBody::Gimbal : public MobilizedBody {
public:
    explicit Gimbal(Direction=Forward);

    Gimbal(MobilizedBody& parent, const Body&, Direction=Forward);

    Gimbal(MobilizedBody& parent, const Transform& inbFrame,
           const Body&,           const Transform& outbFrame, Direction=Forward);

    Gimbal& addBodyDecoration(const Transform& X_BD, const DecorativeGeometry& g) {
        (void)MobilizedBody::addBodyDecoration(X_BD,g); return *this;
    }
    Gimbal& addOutboardDecoration(const Transform& X_MD,  const DecorativeGeometry& g) {
        (void)MobilizedBody::addOutboardDecoration(X_MD,g); return *this;
    }
    Gimbal& addInboardDecoration (const Transform& X_FD, const DecorativeGeometry& g) {
        (void)MobilizedBody::addInboardDecoration(X_FD,g); return *this;
    }

    Gimbal& setDefaultInboardFrame(const Transform& X_PF) {
        (void)MobilizedBody::setDefaultInboardFrame(X_PF); return *this;
    }

    Gimbal& setDefaultOutboardFrame(const Transform& X_BM) {
        (void)MobilizedBody::setDefaultOutboardFrame(X_BM); return *this;
    }

    // This is just a nicer name for the generalized coordinate.
    Gimbal& setDefaultRotation(const Rotation& R_FM) {
        return setDefaultQ(R_FM.convertRotationToBodyFixedXYZ());
    }
    Rotation getDefaultRotation() const {
        const Vec3& q = getDefaultQ();
        return Rotation(BodyRotationSequence,
            q[0], XAxis, q[1], YAxis, q[2], ZAxis);
    }

    // This is used only for visualization.
    Gimbal& setDefaultRadius(Real r);
    Real getDefaultRadius() const;

    // Generic default state Topology methods.
    const Vec3& getDefaultQ() const; // X,Y,Z body-fixed Euler angles
    Gimbal& setDefaultQ(const Vec3& q);

    const Vec3& getQ(const State&) const;
    const Vec3& getQDot(const State&) const;
    const Vec3& getQDotDot(const State&) const;
    const Vec3& getU(const State&) const;
    const Vec3& getUDot(const State&) const;

    void setQ(State&, const Vec3&) const;
    void setU(State&, const Vec3&) const;

    const Vec3& getMyPartQ(const State&, const Vector& qlike) const;
    const Vec3& getMyPartU(const State&, const Vector& ulike) const;
   
    Vec3& updMyPartQ(const State&, Vector& qlike) const;
    Vec3& updMyPartU(const State&, Vector& ulike) const;
    SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(Gimbal, GimbalImpl, MobilizedBody);
};

class SimTK_SIMBODY_EXPORT MobilizedBody::Ball : public MobilizedBody {
public:
    explicit Ball(Direction=Forward);

    Ball(MobilizedBody& parent, const Body&, Direction=Forward);

    Ball(MobilizedBody& parent, const Transform& inbFrame,
         const Body&,           const Transform& outbFrame, Direction=Forward);

    Ball& addBodyDecoration(const Transform& X_BD, const DecorativeGeometry& g) {
        (void)MobilizedBody::addBodyDecoration(X_BD,g); return *this;
    }
    Ball& addOutboardDecoration(const Transform& X_MD,  const DecorativeGeometry& g) {
        (void)MobilizedBody::addOutboardDecoration(X_MD,g); return *this;
    }
    Ball& addInboardDecoration (const Transform& X_FD, const DecorativeGeometry& g) {
        (void)MobilizedBody::addInboardDecoration(X_FD,g); return *this;
    }

    Ball& setDefaultInboardFrame(const Transform& X_PF) {
        (void)MobilizedBody::setDefaultInboardFrame(X_PF); return *this;
    }

    Ball& setDefaultOutboardFrame(const Transform& X_BM) {
        (void)MobilizedBody::setDefaultOutboardFrame(X_BM); return *this;
    }

    // This is just a nicer name for the generalized coordinate.
    Ball& setDefaultRotation(const Rotation& R_FM) {
        return setDefaultQ(R_FM.convertRotationToQuaternion());
    }
    Rotation getDefaultRotation() const {return Rotation(getDefaultQ());}

    // This is used only for visualization.
    Ball& setDefaultRadius(Real r);
    Real getDefaultRadius() const;

    // Generic default state Topology methods.
    const Quaternion& getDefaultQ() const;
    Ball& setDefaultQ(const Quaternion& q);

    const Vec4& getQ(const State&) const;
    const Vec4& getQDot(const State&) const;
    const Vec4& getQDotDot(const State&) const;
    const Vec3& getU(const State&) const;
    const Vec3& getUDot(const State&) const;

    void setQ(State&, const Vec4&) const;
    void setU(State&, const Vec3&) const;

    const Vec4& getMyPartQ(const State&, const Vector& qlike) const;
    const Vec3& getMyPartU(const State&, const Vector& ulike) const;
   
    Vec4& updMyPartQ(const State&, Vector& qlike) const;
    Vec3& updMyPartU(const State&, Vector& ulike) const;
    SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(Ball, BallImpl, MobilizedBody);
};

class SimTK_SIMBODY_EXPORT MobilizedBody::Ellipsoid : public MobilizedBody {
public:
    explicit Ellipsoid(Direction=Forward); // not very useful until radii are set, but has some defaults

    Ellipsoid(MobilizedBody& parent, const Body&, Direction=Forward);

    Ellipsoid(MobilizedBody& parent, const Transform& inbFrame,
              const Body&,           const Transform& outbFrame,
              Direction=Forward);

    Ellipsoid(MobilizedBody& parent, const Transform& inbFrame,
              const Body&,           const Transform& outbFrame,
              const Vec3& radii,
              Direction=Forward);

    Ellipsoid& addBodyDecoration(const Transform& X_BD, const DecorativeGeometry& g) {
        (void)MobilizedBody::addBodyDecoration(X_BD,g); return *this;
    }
    Ellipsoid& addOutboardDecoration(const Transform& X_MD,  const DecorativeGeometry& g) {
        (void)MobilizedBody::addOutboardDecoration(X_MD,g); return *this;
    }
    Ellipsoid& addInboardDecoration (const Transform& X_FD, const DecorativeGeometry& g) {
        (void)MobilizedBody::addInboardDecoration(X_FD,g); return *this;
    }

    Ellipsoid& setDefaultInboardFrame(const Transform& X_PF) {
        (void)MobilizedBody::setDefaultInboardFrame(X_PF); return *this;
    }

    Ellipsoid& setDefaultOutboardFrame(const Transform& X_BM) {
        (void)MobilizedBody::setDefaultOutboardFrame(X_BM); return *this;
    }

    // This is just a nicer name for the generalized coordinate.
    Ellipsoid& setDefaultRotation(const Rotation& R_FM) {
        return setDefaultQ(R_FM.convertRotationToQuaternion());
    }
    Rotation getDefaultRotation() const {return Rotation(getDefaultQ());}

    Ellipsoid& setDefaultRadii(const Vec3& r);
    const Vec3& getDefaultRadii() const;

    // Generic default state Topology methods.
    const Quaternion& getDefaultQ() const;
    Quaternion& updDefaultQ();
    Ellipsoid& setDefaultQ(const Quaternion& q) {updDefaultQ()=q; return *this;}

    const Vec4& getQ(const State&) const;
    const Vec4& getQDot(const State&) const;
    const Vec4& getQDotDot(const State&) const;
    const Vec3& getU(const State&) const;
    const Vec3& getUDot(const State&) const;

    void setQ(State&, const Vec4&) const;
    void setU(State&, const Vec3&) const;

    const Vec4& getMyPartQ(const State&, const Vector& qlike) const;
    const Vec3& getMyPartU(const State&, const Vector& ulike) const;
   
    Vec4& updMyPartQ(const State&, Vector& qlike) const;
    Vec3& updMyPartU(const State&, Vector& ulike) const;

    SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(Ellipsoid, EllipsoidImpl, MobilizedBody);
};

class SimTK_SIMBODY_EXPORT MobilizedBody::Translation : public MobilizedBody {
public:
    explicit Translation(Direction=Forward);

    Translation(MobilizedBody& parent, const Body&, Direction=Forward);

    Translation(MobilizedBody& parent, const Transform& inbFrame,
                const Body&,           const Transform& outbFrame, Direction=Forward);

    Translation& addBodyDecoration(const Transform& X_BD, const DecorativeGeometry& g) {
        (void)MobilizedBody::addBodyDecoration(X_BD,g); return *this;
    }
    Translation& addOutboardDecoration(const Transform& X_MD,  const DecorativeGeometry& g) {
        (void)MobilizedBody::addOutboardDecoration(X_MD,g); return *this;
    }
    Translation& addInboardDecoration (const Transform& X_FD, const DecorativeGeometry& g) {
        (void)MobilizedBody::addInboardDecoration(X_FD,g); return *this;
    }

    Translation& setDefaultInboardFrame(const Transform& X_PF) {
        (void)MobilizedBody::setDefaultInboardFrame(X_PF); return *this;
    }

    Translation& setDefaultOutboardFrame(const Transform& X_BM) {
        (void)MobilizedBody::setDefaultOutboardFrame(X_BM); return *this;
    }

    // This is just a nicer name for setting this mobilizer's generalized coordinates,
    // which together constitute the vector from the F frame's origin to the M
    // frame's origin, expressed in F.

    // Set the topological default values for the initial q's.
    Translation& setDefaultTranslation(const Vec3& p_FM) {
        return setDefaultQ(p_FM);
    }

    // Get the topological default values for the initial q's.
    const Vec3& getDefaultTranslation() const {
        return getDefaultQ();
    }

    // Set the current value of q's in the given State. Note that this is
    // the *cross-mobilizer* translation, not location in the Ground frame.
    void setMobilizerTranslation(State& s, const Vec3& p_FM) const {
        setQ(s,p_FM);
    }

    // Get the current value of the q's for this mobilizer from the given State.
    const Vec3& getMobilizerTranslation(const State& s) const {
        return getQ(s);
    }


    // Set the current value of u's in the given State. Note that this is
    // the *cross-mobilizer* velocity v_FM, not velocity in the Ground frame.
    void setMobilizerVelocity(State& s, const Vec3& v_FM) const {
        setU(s,v_FM);
    }

    // Get the current value of the u's for this mobilizer from the given State.
    const Vec3& getMobilizerVelocity(const State& s) const {
        return getU(s);
    }

    // Get the value of the udot's for this mobilizer from the given State.
    const Vec3& getMobilizerAcceleration(const State& s) const {
        return getUDot(s);
    }

    // Generic default state Topology methods.
    const Vec3& getDefaultQ() const;
    Translation& setDefaultQ(const Vec3& q);

    const Vec3& getQ(const State&) const;
    const Vec3& getQDot(const State&) const;
    const Vec3& getQDotDot(const State&) const;
    const Vec3& getU(const State&) const;
    const Vec3& getUDot(const State&) const;

    void setQ(State&, const Vec3&) const;
    void setU(State&, const Vec3&) const;

    const Vec3& getMyPartQ(const State&, const Vector& qlike) const;
    const Vec3& getMyPartU(const State&, const Vector& ulike) const;
   
    Vec3& updMyPartQ(const State&, Vector& qlike) const;
    Vec3& updMyPartU(const State&, Vector& ulike) const;
    SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(Translation, TranslationImpl, MobilizedBody);
};

class SimTK_SIMBODY_EXPORT MobilizedBody::Free : public MobilizedBody {
public:
    explicit Free(Direction=Forward);

    Free(MobilizedBody& parent, const Body&, Direction=Forward);

    Free(MobilizedBody& parent, const Transform& inbFrame,
         const Body&,           const Transform& outbFrame,
         Direction=Forward);

    Free& addBodyDecoration(const Transform& X_BD, const DecorativeGeometry& g) {
        (void)MobilizedBody::addBodyDecoration(X_BD,g); return *this;
    }
    Free& addOutboardDecoration(const Transform& X_MD,  const DecorativeGeometry& g) {
        (void)MobilizedBody::addOutboardDecoration(X_MD,g); return *this;
    }
    Free& addInboardDecoration (const Transform& X_FD, const DecorativeGeometry& g) {
        (void)MobilizedBody::addInboardDecoration(X_FD,g); return *this;
    }

    Free& setDefaultInboardFrame(const Transform& X_PF) {
        (void)MobilizedBody::setDefaultInboardFrame(X_PF); return *this;
    }

    Free& setDefaultOutboardFrame(const Transform& X_BM) {
        (void)MobilizedBody::setDefaultOutboardFrame(X_BM); return *this;
    }

    // Leaves rotation unchanged.
    Free& setDefaultTranslation(const Vec3&);

    // Leaves translation unchanged. The internal representation is a quaternion
    // so we guarantee that the stored value is numerically identical to the
    // supplied one.
    Free& setDefaultQuaternion(const Quaternion&);

    // Leaves translation unchanged. The Rotation matrix will be converted to
    // a quaternion for storage.
    Free& setDefaultRotation(const Rotation&);
    // Sets both translation and rotation. The Rotation part of the Transform 
    // will be converted to a quaternion for storage.
    Free& setDefaultTransform(const Transform&);

    // These return references to the stored default values.
    const Vec3& getDefaultTranslation() const;
    const Quaternion& getDefaultQuaternion() const;

    // These next two are derived from the stored values.
    Rotation getDefaultRotation() const {
        return Rotation(getDefaultQuaternion());
    }
    Transform getDefaultTransform() const {
        return Transform(Rotation(getDefaultQuaternion()), getDefaultTranslation());
    }

    // Generic default state Topology methods.

    // Returns (Vec4,Vec3) where the Vec4 is a normalized quaternion.
    const Vec7& getDefaultQ() const;

    // Interprets the supplied q as (Vec4,Vec3) where the Vec4 is a possibly
    // unnormalized quaternion. The quaternion will be normalized before it is
    // stored here, so you may not get back exactly the value supplied here if
    // you call getDefaultQ().
    Free& setDefaultQ(const Vec7& q);

    // Note that there is no guarantee that the quaternion part of the returned Q is normalized.
    const Vec7& getQ(const State&) const;
    const Vec7& getQDot(const State&) const;
    const Vec7& getQDotDot(const State&) const;

    const Vec6& getU(const State&) const;
    const Vec6& getUDot(const State&) const;

    // The Q's in the state are set exactly as supplied without normalization.
    void setQ(State&, const Vec7&) const;
    void setU(State&, const Vec6&) const;

    const Vec7& getMyPartQ(const State&, const Vector& qlike) const;
    const Vec6& getMyPartU(const State&, const Vector& ulike) const;
   
    Vec7& updMyPartQ(const State&, Vector& qlike) const;
    Vec6& updMyPartU(const State&, Vector& ulike) const;
    SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(Free, FreeImpl, MobilizedBody);
};


// These are special "ball" and "free" joints designed to allow arbitrary orientations
// for "linear" bodies, such as a CO2 molecule consisting only of point masses arranged
// along a straight line. Such bodies have no inertia about the line and cause singularities
// in the equations of motion if attached to Orientation or Free mobilizers. Instead, use the
// LineOrientation and LineFree moblizers, making sure that the inertialess direction is
// along the outboard body's z axis (that is, Mz). These mobilizers introduce only two
// mobilities (generalized speeds u), being incapable of representing non-zero angular
// velocity of M in F about Mz. The generalized speeds are in fact the wx and wy 
// components of w_FM_M, that is, the x and y components of the angular velocity of M
// in F *expressed in M*. However, at least three generalized coordinates (q's)
// are required to represent the orientation. By default we use four quaternions for
// unconditional stability. Alternatively, you can request a 1-2-3 body fixed 
// Euler angle sequence (that is, about x, then new y, then new z) which will
// suffer a singularity when the y rotation is 90 degrees since that aligns the
// first rotation axis (x) with the last (z) which is the inertialess direction.

class SimTK_SIMBODY_EXPORT MobilizedBody::LineOrientation : public MobilizedBody {
public:
    explicit LineOrientation(Direction=Forward);

    LineOrientation(MobilizedBody& parent, const Body&, Direction=Forward);

    LineOrientation(MobilizedBody& parent, const Transform& inbFrame,
                    const Body&,           const Transform& outbFrame, Direction=Forward);

    LineOrientation& addBodyDecoration(const Transform& X_BD, const DecorativeGeometry& g) {
        (void)MobilizedBody::addBodyDecoration(X_BD,g); return *this;
    }
    LineOrientation& addOutboardDecoration(const Transform& X_MD,  const DecorativeGeometry& g) {
        (void)MobilizedBody::addOutboardDecoration(X_MD,g); return *this;
    }
    LineOrientation& addInboardDecoration (const Transform& X_FD, const DecorativeGeometry& g) {
        (void)MobilizedBody::addInboardDecoration(X_FD,g); return *this;
    }

    LineOrientation& setDefaultInboardFrame(const Transform& X_PF) {
        (void)MobilizedBody::setDefaultInboardFrame(X_PF); return *this;
    }

    LineOrientation& setDefaultOutboardFrame(const Transform& X_BM) {
        (void)MobilizedBody::setDefaultOutboardFrame(X_BM); return *this;
    }

    // This is just a nicer name for the generalized coordinate.
    LineOrientation& setDefaultRotation(const Rotation& R_FM) {
        return setDefaultQ(R_FM.convertRotationToQuaternion());
    }
    Rotation getDefaultRotation() const {return Rotation(getDefaultQ());}

    // Generic default state Topology methods.
    const Quaternion& getDefaultQ() const;
    LineOrientation& setDefaultQ(const Quaternion& q);

    const Vec4& getQ(const State&) const;
    const Vec4& getQDot(const State&) const;
    const Vec4& getQDotDot(const State&) const;
    const Vec2& getU(const State&) const;
    const Vec2& getUDot(const State&) const;

    void setQ(State&, const Vec4&) const;
    void setU(State&, const Vec2&) const;

    const Vec4& getMyPartQ(const State&, const Vector& qlike) const;
    const Vec2& getMyPartU(const State&, const Vector& ulike) const;
   
    Vec4& updMyPartQ(const State&, Vector& qlike) const;
    Vec2& updMyPartU(const State&, Vector& ulike) const;
    SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(LineOrientation, LineOrientationImpl, MobilizedBody);
};

class SimTK_SIMBODY_EXPORT MobilizedBody::FreeLine : public MobilizedBody {
public:
    explicit FreeLine(Direction=Forward);

    FreeLine(MobilizedBody& parent, const Body&, Direction=Forward);

    FreeLine(MobilizedBody& parent, const Transform& inbFrame,
             const Body&,           const Transform& outbFrame, Direction=Forward);

    FreeLine& addBodyDecoration(const Transform& X_BD, const DecorativeGeometry& g) {
        (void)MobilizedBody::addBodyDecoration(X_BD,g); return *this;
    }
    FreeLine& addOutboardDecoration(const Transform& X_MD,  const DecorativeGeometry& g) {
        (void)MobilizedBody::addOutboardDecoration(X_MD,g); return *this;
    }
    FreeLine& addInboardDecoration (const Transform& X_FD, const DecorativeGeometry& g) {
        (void)MobilizedBody::addInboardDecoration(X_FD,g); return *this;
    }

    FreeLine& setDefaultInboardFrame(const Transform& X_PF) {
        (void)MobilizedBody::setDefaultInboardFrame(X_PF); return *this;
    }

    FreeLine& setDefaultOutboardFrame(const Transform& X_BM) {
        (void)MobilizedBody::setDefaultOutboardFrame(X_BM); return *this;
    }
    SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(FreeLine, FreeLineImpl, MobilizedBody);
};

class SimTK_SIMBODY_EXPORT MobilizedBody::Weld : public MobilizedBody {
public:
    Weld();

    Weld(MobilizedBody& parent, const Body&);

    Weld(MobilizedBody& parent, const Transform& inbFrame,
         const Body&,           const Transform& outbFrame);

    Weld& addBodyDecoration(const Transform& X_BD, const DecorativeGeometry& g) {
        (void)MobilizedBody::addBodyDecoration(X_BD,g); return *this;
    }
    Weld& addOutboardDecoration(const Transform& X_MD,  const DecorativeGeometry& g) {
        (void)MobilizedBody::addOutboardDecoration(X_MD,g); return *this;
    }
    Weld& addInboardDecoration (const Transform& X_FD, const DecorativeGeometry& g) {
        (void)MobilizedBody::addInboardDecoration(X_FD,g); return *this;
    }

    Weld& setDefaultInboardFrame(const Transform& X_PF) {
        (void)MobilizedBody::setDefaultInboardFrame(X_PF); return *this;
    }

    Weld& setDefaultOutboardFrame(const Transform& X_BM) {
        (void)MobilizedBody::setDefaultOutboardFrame(X_BM); return *this;
    }
    SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(Weld, WeldImpl, MobilizedBody);
};


class SimTK_SIMBODY_EXPORT MobilizedBody::Ground : public MobilizedBody {
public:
    Ground();
    Ground& addBodyDecoration(const Transform& X_BD, const DecorativeGeometry& g) {
        (void)MobilizedBody::addBodyDecoration(X_BD,g); return *this;
    }
    SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(Ground, GroundImpl, MobilizedBody);
};


class SimTK_SIMBODY_EXPORT MobilizedBody::Custom : public MobilizedBody {
public:
    class Implementation;
    class ImplementationImpl;

    /* Create a Custom MobilizedBody.
     * 
     * @param parent         the MobilizedBody's parent body
     * @param implementation the object which implements the custom mobilized body.  The MobilizedBody::Custom takes over
     *                       ownership of the implementation object, and deletes it when the MobilizedBody itself
     *                       is deleted.
     * @param body           describes this MobilizedBody's physical properties
     * @param direction      whether you want the coordinates defined as though parent & child were swapped
     */
    Custom(MobilizedBody& parent, Implementation* implementation, const Body& body, Direction direction=Forward);
    /* Create a Custom MobilizedBody.
     * 
     * @param parent         the MobilizedBody's parent body
     * @param implementation the object which implements the custom mobilized body.  The MobilizedBody::Custom takes over
     *                       ownership of the implementation object, and deletes it when the MobilizedBody itself
     *                       is deleted.
     * @param inbFrame       the MobilizedBody's inboard reference frame
     * @param body           describes this MobilizedBody's physical properties
     * @param outbFrame      the MobilizedBody's outboard reference frame
     * @param direction      whether you want the coordinates defined as though parent & child were swapped
     */
    Custom(MobilizedBody& parent, Implementation* implementation, 
           const Transform& inbFrame, const Body& body, const Transform& outbFrame,
           Direction direction=Forward);
    SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(Custom, CustomImpl, MobilizedBody);
protected:
    const Implementation& getImplementation() const;
    Implementation&       updImplementation();

    Custom() {}
};

// We only want the template instantiation to occur once. This symbol is defined in the SimTK core
// compilation unit that defines the MobilizedBody class but should not be defined any other time.
// BE SURE TO DEAL WITH THIS IN MobilizedBody_Instantiation.cpp.
#ifndef SimTK_SIMBODY_DEFINING_MOBILIZED_BODY
    extern template class PIMPLHandle<MobilizedBody::Custom::Implementation, MobilizedBody::Custom::ImplementationImpl>;
#endif


class SimTK_SIMBODY_EXPORT MobilizedBody::Custom::Implementation 
  : public PIMPLHandle<Implementation,ImplementationImpl> 
{
public:
    // No default constructor because you have to supply at least the SimbodyMatterSubsystem
    // to which this MobilizedBody belongs.

    virtual ~Implementation();

    virtual Implementation* clone() const = 0;

    Implementation(SimbodyMatterSubsystem&, int nu, int nq, int nAngles=0);

    Vector getQ(const State& s) const;
    
    Vector getU(const State& s) const;

    Vector getQDot(const State& s) const;

    Vector getUDot(const State& s) const;
    
    Vector getQDotDot(const State& s) const;

    Transform getMobilizerTransform(const State& s) const;

    SpatialVec getMobilizerVelocity(const State& s) const;

    bool getUseEulerAngles(const State& s) const;

    void invalidateTopologyCache() const;



    virtual Transform calcMobilizerTransformFromQ(const State& s, int nq, const Real* q) const = 0;


    virtual SpatialVec multiplyByHMatrix(const State& s, int nu, const Real* u) const = 0;

    virtual void multiplyByHTranspose(const State& s, const SpatialVec& F, int nu, Real* f) const = 0;

    virtual SpatialVec multiplyByHDotMatrix(const State& s, int nu, const Real* u) const = 0;

    virtual void multiplyByHDotTranspose(const State& s, const SpatialVec& F, int nu, Real* f) const = 0;

    virtual void multiplyByN(const State& s, bool transposeMatrix, 
                             int nIn, const Real* in, int nOut, Real* out) const;

    virtual void multiplyByNInv(const State& s, bool transposeMatrix, 
                                int nIn, const Real* in, int nOut, Real* out) const;

    virtual void multiplyByNDot(const State& s, bool transposeMatrix, 
                                int nIn, const Real* in, int nOut, Real* out) const;

        // Methods for setting Mobilizer initial conditions. Note -- I've stripped this
        // down to the two basic routines but the built-ins have 8 so that you can 
        // specify only rotations or translations. I'm not sure that's needed here and
        // I suppose you could add more routines later if needed.
        // Eventually it might be nice to provide default implementation here that would
        // use a root finder to attempt to solve these initial condition problems. For
        // most joints there is a much more direct way to do it, and sometimes there
        // are behavioral choices to make, which is why it is nice to have mobilizer-specific
        // overrides for these.

    virtual void setQToFitTransform(const State&, const Transform& X_FM, int nq, Real* q) const;

    virtual void setUToFitVelocity(const State&, const SpatialVec& V_FM, int nu, Real* u) const;

    virtual void calcDecorativeGeometryAndAppend
       (const State& s, Stage stage, Array_<DecorativeGeometry>& geom) const
    {
    }











    virtual void realizeTopology(State&) const { }

    virtual void realizeModel(State&) const { }

    virtual void realizeInstance(const State&) const { }

    virtual void realizeTime(const State&) const { }

    virtual void realizePosition(const State&) const { }

    virtual void realizeVelocity(const State&) const { }

    virtual void realizeDynamics(const State&) const { }

    virtual void realizeAcceleration(const State&) const { }

    virtual void realizeReport(const State&) const { }

    friend class MobilizedBody::CustomImpl;
};

class SimTK_SIMBODY_EXPORT MobilizedBody::FunctionBased : public MobilizedBody::Custom {
public:
    /* Create a FunctionBased MobilizedBody.
     * 
     * @param parent         the MobilizedBody's parent body
     * @param body           describes this MobilizedBody's physical properties
     * @param nmobilities    the number of generalized coordinates belonging to this MobilizedBody
     * @param functions      the Functions describing how the body moves based on its generalized coordinates.
     *                       This must be of length 6.  The elements correspond to, in order, x rotation, y rotation, z rotation,
     *                       x translation, y translation, and z translation.  The MobilizedBody takes over ownership of the functions,
     *                       and automatically deletes them when the MobilizedBody is deleted.
     * @param coordIndices   the indices of the generalized coordinates that are inputs to each function.  For example, if coordIndices[2] = {0, 1},
     *                       that means that functions[2] takes two input arguments, and q[0] and q[1] respectively should be passed as those arguments.
     * @param direction      whether you want the coordinates defined as though parent & child were swapped
     */
    FunctionBased(MobilizedBody& parent, const Body& body, 
                  int nmobilities, const Array_<const Function*>& functions,
                  const Array_<Array_<int> >& coordIndices,
                  Direction direction=Forward);

    FunctionBased(MobilizedBody& parent, const Body& body, 
                  int nmobilities, const std::vector<const Function*>& functions,
                  const std::vector<std::vector<int> >& coordIndices,
                  Direction direction=Forward) 
    {
        Array_< Array_<int> > coordCopy(coordIndices); // sorry, must copy
        // Use the above constructor.
        new(this) FunctionBased(parent,body,nmobilities,
                                ArrayViewConst_<const Function*>(functions), 
                                coordCopy, direction);
    }

    /* Create a FunctionBased MobilizedBody.
     * 
     * @param parent         the MobilizedBody's parent body
     * @param inbFrame       the default inboard frame
     * @param body           describes this MobilizedBody's physical properties
     * @param outbFrame      the default outboard frame
     * @param nmobilities    the number of generalized coordinates belonging to this MobilizedBody
     * @param functions      the Functions describing how the body moves based on its generalized coordinates.
     *                       This must be of length 6.  The elements correspond to, in order, x rotation, y rotation, z rotation,
     *                       x translation, y translation, and z translation.  The MobilizedBody takes over ownership of the functions,
     *                       and automatically deletes them when the MobilizedBody is deleted.
     * @param coordIndices   the indices of the generalized coordinates that are inputs to each function.  For example, if coordIndices[2] = {0, 1},
     *                       that means that functions[2] takes two input arguments, and q[0] and q[1] respectively should be passed as those arguments.
     * @param direction      whether you want the coordinates defined as though parent & child were swapped
     */
    FunctionBased(MobilizedBody& parent, const Transform& inbFrame, 
                  const Body& body, const Transform& outbFrame, 
                  int nmobilities, const Array_<const Function*>& functions,
                  const Array_<Array_<int> >& coordIndices,
                  Direction direction=Forward);

    FunctionBased(MobilizedBody& parent, const Transform& inbFrame, 
                  const Body& body, const Transform& outbFrame, 
                  int nmobilities, const std::vector<const Function*>& functions,
                  const std::vector<std::vector<int> >& coordIndices,
                  Direction direction=Forward)
    {
        Array_< Array_<int> > coordCopy(coordIndices); // sorry, must copy
        // Use the above constructor.
        new(this) FunctionBased(parent,inbFrame,body,outbFrame,
                                nmobilities, ArrayViewConst_<const Function*>(functions), 
                                coordCopy, direction);
    }

    /* Create a FunctionBased MobilizedBody.
     * 
     * @param parent         the MobilizedBody's parent body
     * @param body           describes this MobilizedBody's physical properties
     * @param nmobilities    the number of generalized coordinates belonging to this MobilizedBody
     * @param functions      the Functions describing how the body moves based on its generalized coordinates.
     *                       This must be of length 6.  The elements correspond to, in order, x rotation, y rotation, z rotation,
     *                       x translation, y translation, and z translation.  The MobilizedBody takes over ownership of the functions,
     *                       and automatically deletes them when the MobilizedBody is deleted.
     * @param coordIndices   the indices of the generalized coordinates that are inputs to each function.  For example, if coordIndices[2] = {0, 1},
     *                       that means that functions[2] takes two input arguments, and q[0] and q[1] respectively should be passed as those arguments.
     * @param axes           the axes directions (as Vec3's) for each spatial coordinate, which each function describes, and is therefore length 6.
     *                       First 3 and last 3 axes must be linearly independent, otherwise there will be redundant speeds for the same motion.
     * @param direction      whether you want the coordinates defined as though parent & child were swapped
     */
    FunctionBased(MobilizedBody& parent, const Body& body, 
                  int nmobilities, const Array_<const Function*>& functions,
                  const Array_<Array_<int> >& coordIndices, const Array_<Vec3>& axes,
                  Direction direction=Forward);

    FunctionBased(MobilizedBody& parent, const Body& body, 
                  int nmobilities, const std::vector<const Function*>& functions,
                  const std::vector<std::vector<int> >& coordIndices, const std::vector<Vec3>& axes,
                  Direction direction=Forward)
    {
        Array_< Array_<int> > coordCopy(coordIndices); // sorry, must copy
        // Use the above constructor.
        new(this) FunctionBased(parent,body,
                                nmobilities, ArrayViewConst_<const Function*>(functions), 
                                coordCopy, ArrayViewConst_<Vec3>(axes), 
                                direction);
    }

    /* Create a FunctionBased MobilizedBody.
     * 
     * @param parent         the MobilizedBody's parent body
     * @param inbFrame       the default inboard frame
     * @param body           describes this MobilizedBody's physical properties
     * @param outbFrame      the default outboard frame
     * @param nmobilities    the number of generalized coordinates belonging to this MobilizedBody
     * @param functions      the Functions describing how the body moves based on its generalized coordinates.
     *                       This must be of length 6.  The elements correspond to, in order, x rotation, y rotation, z rotation,
     *                       x translation, y translation, and z translation.  The MobilizedBody takes over ownership of the functions,
     *                       and automatically deletes them when the MobilizedBody is deleted.
     * @param coordIndices   the indices of the generalized coordinates that are inputs to each function.  For example, if coordIndices[2] = {0, 1},
     *                       that means that functions[2] takes two input arguments, and q[0] and q[1] respectively should be passed as those arguments.
     * @param axes           the axes directions (as Vec3's) for each spatial coordinate, which each function describes, and is therefore length 6.
     *                       First 3 and last 3 axes must be linearly independent, otherwise there will be redundant speeds for the same motion.
     * @param direction      whether you want the coordinates defined as though parent & child were swapped
     */
    FunctionBased(MobilizedBody& parent, const Transform& inbFrame, 
                  const Body& body, const Transform& outbFrame, 
                  int nmobilities, const Array_<const Function*>& functions,
                  const Array_<Array_<int> >& coordIndices, const Array_<Vec3>& axes,
                  Direction direction=Forward);

    FunctionBased(MobilizedBody& parent, const Transform& inbFrame, 
                  const Body& body, const Transform& outbFrame,
                  int nmobilities, const std::vector<const Function*>& functions,
                  const std::vector<std::vector<int> >& coordIndices, const std::vector<Vec3>& axes,
                  Direction direction=Forward)
    {
        Array_< Array_<int> > coordCopy(coordIndices); // sorry, must copy
        // Use the above constructor.
        new(this) FunctionBased(parent,inbFrame,body,outbFrame,
                                nmobilities, ArrayViewConst_<const Function*>(functions), 
                                coordCopy, ArrayViewConst_<Vec3>(axes), 
                                direction);
    }
private:
    FunctionBased() {}
};

} // namespace SimTK

#endif // SimTK_SIMBODY_MOBILIZED_BODY_H_