#ifndef SimTK_SIMBODY_RIGID_BODY_NODE_SPEC_H_
#define SimTK_SIMBODY_RIGID_BODY_NODE_SPEC_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) 2005-12 Stanford University and the Authors.        *
 * Authors: Michael Sherman                                                   *
 * Contributors:                                                              *
 *    Charles Schwieters (NIH): wrote the public domain IVM code from which   *
 *                              this was derived.                             *
 *    Peter Eastman: wrote the Euler Angle<->Quaternion conversion            *
 *                                                                            *
 * 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.                                             *
 * -------------------------------------------------------------------------- */

/**@file
 * This file contains just the templatized class RigidBodyNodeSpec<dof> which
 * is used as the base class for most of the RigidBodyNodes (that is, the
 * implementations of Mobilizers). The only exceptions are nodes whose 
 * mobilizers provide no degrees of freedom -- Ground and Weld.
 *
 * This file contains all the multibody mechanics method declarations that 
 * involve a single body and its mobilizer (inboard joint), that is, one node 
 * in the multibody tree. These methods constitute the inner loops of the 
 * multibody calculations, and much suffering is undergone here to make them 
 * run fast. In particular most calculations are templatized by the number of 
 * mobilities, so that compile-time sizes are known for everything.
 *
 * Most methods here expect to be called in a particular order during traversal 
 * of the tree -- either base to tip or tip to base.
 */

#include "SimbodyMatterSubsystemRep.h"
#include "RigidBodyNode.h"

/**
 * This still-abstract class is a skeleton implementation of a built-in 
 * mobilizer, with the number of mobilities (dofs, u's) specified as a template 
 * parameter. That way all the code that is common except for the dimensionality
 * of the mobilizer can be written once, and the compiler generates specific 
 * implementations for each of the six possible dimensionalities (1-6 
 * mobilities). Each implementation works only on fixed size Vec<> and Mat<> 
 * types, so can use very high speed inline operators.
 */
template<int dof, bool noR_FM=false, bool noX_MB=false, bool noR_PF=false>
class RigidBodyNodeSpec : public RigidBodyNode {
public:

RigidBodyNodeSpec(const MassProperties& mProps_B,
                  const Transform&      X_PF,
                  const Transform&      X_BM,
                  UIndex&               nextUSlot,
                  USquaredIndex&        nextUSqSlot,
                  QIndex&               nextQSlot,
                  QDotHandling          qdotHandling,
                  QuaternionUse         quaternionUse,
                  bool                  isReversed)
:   RigidBodyNode(mProps_B, X_PF, X_BM, 
                  qdotHandling, quaternionUse, isReversed)
{
    // don't call any virtual methods in here!
    uIndex   = nextUSlot;
    uSqIndex = nextUSqSlot;
    qIndex   = nextQSlot;
}

void updateSlots(UIndex& nextUSlot, USquaredIndex& nextUSqSlot, QIndex& nextQSlot) {
    // OK to call virtual method here.
    nextUSlot   += getDOF();
    nextUSqSlot += getDOF()*getDOF();
    nextQSlot   += getMaxNQ();
}

virtual ~RigidBodyNodeSpec() {}

// This is the type of the joint transition matrix H, but our definition
// of H is transposed from Jain's or Schwieters'. That is, what we're 
// calling H here is what Jain calls H* and Schwieters calls H^T. So
// our H matrix is 6 x nu, or more usefully it is 2 rows of Vec3's.
// The first row define how u's contribute to angular velocities; the
// second defines how u's contribute to linear velocities.
typedef Mat<2,dof,Vec3> HType;

// Provide default implementations for setQToFitTransformImpl() and setQToFitVelocityImpl() 
// which are implemented using the rotational and translational quantity routines. These assume
// that the rotational and translational coordinates are independent, with rotation handled
// first and then left alone. If a mobilizer type needs to deal with rotation and
// translation simultaneously, it should provide a specific implementation for these two routines.
// *Each* mobilizer must implement setQToFit{Rotation,Translation} and 
// setUToFit{AngularVelocity,LinearVelocity}; there are no defaults.

virtual void setQToFitTransformImpl(const SBStateDigest& sbs, const Transform& X_FM, Vector& q) const {
    setQToFitRotationImpl   (sbs,X_FM.R(),q);
    setQToFitTranslationImpl(sbs,X_FM.p(),q);
}

virtual void setUToFitVelocityImpl(const SBStateDigest& sbs, const Vector& q, const SpatialVec& V_FM, Vector& u) const {
    setUToFitAngularVelocityImpl(sbs,q,V_FM[0],u);
    setUToFitLinearVelocityImpl (sbs,q,V_FM[1],u);
}

// The following routines calculate joint-specific position kinematic
// quantities. They may assume that *all* position kinematics (not just
// joint-specific) has been done for the parent, and that the position
// state variables q are available. Each routine may also assume that the
// previous routines have been called, in the order below.
// The routines are structured as operators -- they use the State but
// do not change anything in it, including the cache. Instead they are
// passed arguments to write their results into. In practice, these
// arguments will typically be in the State cache (see below).


// This mandatory routine calculates the joint transition matrix H_FM, giving
// the change of velocity induced by the generalized speeds u for this 
// mobilizer, expressed in the mobilizer's inboard "fixed" frame F (attached to
// this body's parent). 
// This method constitutes the *definition* of the generalized speeds for
// a particular joint.
// This routine can depend on X_FM having already
// been calculated and available in the PositionCache but must NOT depend
// on any quantities involving Ground or other bodies.
// Note: this calculates the H matrix *as defined*; we might reverse
// the frames in use. So we call this H_F0M0 while the possibly reversed
// version is H_FM. Be sure to use X_F0M0 in your calculation for H_F0M0
// if you need access to the cross-mobilizer transform.
// CAUTION: our definition of H is transposed from Jain and Schwieters.
virtual void calcAcrossJointVelocityJacobian(
    const SBStateDigest& sbs,
    HType&               H_F0M0) const=0;

// This mandatory routine calculates the time derivative taken in F of
// the matrix H_FM (call it HDot_FM). This is zero if the generalized
// speeds are all defined in terms of the F frame, which is often the case.
// This routine can depend on X_FM and H_FM being available already in
// the state PositionCache, and V_FM being already in the state VelocityCache.
// However, it must NOT depend on any quantities involving Ground or other bodies.
// Note: this calculates the HDot matrix *as defined*; we might reverse
// the frames in use. So we call this HDot_F0M0 while the possibly reversed
// version is HDot_FM. Be sure to use X_F0M0 and V_F0M0 in your calculation
// of HDot_F0M0 if you need access to the cross-mobilizer transform and/or velocity.
// CAUTION: our definition of H is transposed from Jain and Schwieters.
virtual void calcAcrossJointVelocityJacobianDot(
    const SBStateDigest& sbs,
    HType&               HDot_F0M0) const = 0;

// We allow a mobilizer to be defined in reverse when that is more
// convenient. That is, the H matrix can be defined by giving H_F0M0=H_MF and 
// HDot_F0M0=HDot_MF instead of H_FM and HDot_FM. In that case the 
// following methods are called instead of the above; the default 
// implementation postprocesses the output from the above methods, but a 
// mobilizer can override it if it knows how to get the job done faster.
virtual void calcReverseMobilizerH_FM(
    const SBStateDigest& sbs,
    HType&               H_FM) const;

virtual void calcReverseMobilizerHDot_FM(
    const SBStateDigest& sbs,
    HType&               HDot_FM) const;

// This routine is NOT joint specific, but cannot be called until the across-joint
// transform X_FM has been calculated and is available in the State cache.
void calcBodyTransforms(
    const SBTreePositionCache&  pc, 
    Transform&                  X_PB, 
    Transform&                  X_GB) const 
{
    const Transform& X_MB = getX_MB();   // fixed
    const Transform& X_PF = getX_PF();   // fixed
    const Transform& X_FM = getX_FM(pc); // just calculated
    const Transform& X_GP = getX_GP(pc); // already calculated

    const Transform X_FB = (noX_MB ? X_FM : X_FM*X_MB); // 63 flops
    X_PB = (noR_PF ? Transform(X_FB.R(), X_PF.p()+X_FB.p()) : X_PF*X_FB); // 63 flops
    X_GB = X_GP * X_PB;        //  63 flops
}

// Same for all mobilizers. The return matrix here is precisely the 
// one used by Jain and Schwieters, but transposed.
void calcParentToChildVelocityJacobianInGround(
    const SBModelVars&          mv,
    const SBTreePositionCache&  pc, 
    HType&                      H_PB_G) const;

// Same for all mobilizers. This is the time derivative of 
// the matrix H_PB_G above, with the derivative taken in the 
// Ground frame.
void calcParentToChildVelocityJacobianInGroundDot(
    const SBModelVars&          mv,
    const SBTreePositionCache&  pc, 
    const SBTreeVelocityCache&  vc, 
    HType&                      HDot_PB_G) const;

void realizeModel(SBStateDigest& sbs) const 
{
}

void realizeInstance(const SBStateDigest& sbs) const
{
}

// Set a new configuration and calculate the consequent kinematics.
// Must call base-to-tip.
void realizePosition(const SBStateDigest& sbs) const 
{
    const SBModelVars&      mv   = sbs.getModelVars();
    const SBModelCache&     mc   = sbs.getModelCache();
    const SBInstanceCache&  ic   = sbs.getInstanceCache();
    const Vector&           allQ = sbs.getQ();
    SBTreePositionCache&    pc   = sbs.updTreePositionCache();
    Vector&                 allQErr = sbs.updQErr();

    // Mobilizer specific.
    
    const SBModelPerMobodInfo& mbInfo = getModelInfo(mc);

    // First perform precalculations on these new q's, such as stashing away
    // sines and cosines of angles. We'll put all the results into the State's
    // position cache for later use. If there are local constraints on the q's
    // (currently that means quaternion normalization constraints), then we
    // calculate those here and put the result in the appropriate slot of qerr.

    const int nq=mbInfo.nQInUse, nqpool=mbInfo.nQPoolInUse,
              nqerr=(mbInfo.hasQuaternionInUse ? 1 : 0);
    const Real* q0    = nq     ? &allQ[mbInfo.firstQIndex]                : 0;
    Real*       qpool0= nqpool ? &pc.mobilizerQCache[mbInfo.startInQPool] : 0;
    Real*       qerr0 = nqerr  ? &allQErr[ic.firstQuaternionQErrSlot
                                          + mbInfo.quaternionPoolIndex]     
                               : 0;
    performQPrecalculations(sbs, q0, nq, qpool0, nqpool, qerr0, nqerr);

    // Now that we've done the necessary precalculations, calculate the cross-
    // mobilizer transform X_FM without recalculating anything. For reversed 
    // mobilizers we have to do our own reversal here because mobilizers don't
    // handle that themselves.

    if (isReversed()) {
        Transform X_MF;
        calcX_FM(sbs, q0, nq, qpool0, nqpool, X_MF);
        updX_FM(pc) = ~X_MF;
    } else 
        calcX_FM(sbs, q0, nq, qpool0, nqpool, updX_FM(pc));

    // With X_FM in the cache, and X_GP for the parent already calculated (we're doing
    // an outward pass), we can calculate X_PB and X_GB now.
    calcBodyTransforms(pc, updX_PB(pc), updX_GB(pc));

    // Here we do allow the mobilizer to calculate the reversed H matrix, but the
    // default implementation of the reversed method just calls the forward method
    // and then reverses it. For some mobilizers that is unreasonably expensive.
    // REMINDER: our H matrix definition is transposed from Jain and Schwieters.
    if (isReversed()) calcReverseMobilizerH_FM       (sbs, updH_FM(pc));
    else              calcAcrossJointVelocityJacobian(sbs, updH_FM(pc));

    // Here we're using the cross-mobilizer hinge matrix H_FM that we just 
    // calculated to compute H(==H_PB_G), the equivalent hinge matrix between 
    // the parent's body frame P and child's body frame B, but expressed in
    // Ground. (F is fixed on P and M is fixed on B.)
    calcParentToChildVelocityJacobianInGround(mv,pc, updH(pc));

    // Mobilizer independent.
    calcJointIndependentKinematicsPos(pc);
}

// Set new velocities for the current configuration, and calculate
// all the velocity-dependent terms. Must call base-to-tip.
// This routine may assume that *all* position 
// kinematics (not just joint-specific) has been done for this node,
// that all velocity kinematics has been done for the parent, and
// that the velocity state variables (u) are available. The
// quantities that must be computed are:
//   V_FM   relative velocity of B's M frame in P's F frame, 
//             expressed in F (note: this is also V_PM_F since
//             F is fixed on P). (This is H_FM*u.)
//   V_PB_G  relative velocity of B in P (==H*u), expr. in G
//   HDot_FM time derivative of hinge matrix H_FM
//   HDot    time derivative of H (==H_PB_G) hinge matrix relating body frames
//   VD_PB_G acceleration remainder term HDot*u, expr. in G
// The code is the same for all joints, although parametrized by ndof.
void realizeVelocity(const SBStateDigest& sbs) const 
{
    const SBModelVars&          mv = sbs.getModelVars();
    const SBTreePositionCache&  pc = sbs.getTreePositionCache();
    SBTreeVelocityCache&        vc = sbs.updTreeVelocityCache();
    const Vector&               allU = sbs.getU();
    const Vec<dof>&             u = fromU(allU);

    // Mobilizer specific.
    calcQDot(sbs, &allU[uIndex], &sbs.updQDot()[qIndex]);

    updV_FM(vc)    = getH_FM(pc) * u;   // 6*dof flops
    updV_PB_G(vc)  = getH(pc)    * u;   // 6*dof flops

    // REMINDER: our H matrix definition is transposed from Jain and Schwieters.
    if (isReversed()) calcReverseMobilizerHDot_FM       (sbs, updHDot_FM(vc));
    else              calcAcrossJointVelocityJacobianDot(sbs, updHDot_FM(vc));

    // Here we're using the cross-mobilizer hinge matrix derivative HDot_FM 
    // that we just calculated to compute HDot(==HDot_PB_G), the derivative
    // of H (==H_PB_G) between the parent's body frame P and child's body 
    // frame B. The derivative is taken in the Ground frame and the result
    // is expressed in Ground. (F is fixed on P and M is fixed on B.)
    calcParentToChildVelocityJacobianInGroundDot(mv,pc,vc, updHDot(vc));
    updVD_PB_G(vc) = getHDot(vc) * u;   // 6*dof flops

    // Mobilizer independent.
    calcJointIndependentKinematicsVel(pc,vc);
}

// Articulated body inertias have been calculated; here we're 
void realizeDynamics(const SBArticulatedBodyInertiaCache&   abc,
                     const SBStateDigest&                   sbs) const 
{
    const SBTreePositionCache&  pc = sbs.getTreePositionCache();
    const SBTreeVelocityCache&  vc = sbs.getTreeVelocityCache();
    SBDynamicsCache&            dc = sbs.updDynamicsCache();

    // Mobilizer-specific.
    // None.

    // Mobilizer independent.
    calcJointIndependentDynamicsVel(pc,abc,vc,dc);
}

// There is no realizeAcceleration().

void realizeReport(const SBStateDigest& sbs) const
{
}

// Use base class implementation of calcCompositeBodyInertiasInward() since
// that is independent of mobilities.

// This is a dynamics-stage calculation and must be called tip-to-base (inward).
void realizeArticulatedBodyInertiasInward(
    const SBInstanceCache&          ic,
    const SBTreePositionCache&      pc,
    SBArticulatedBodyInertiaCache&  abc) const;

// calcJointIndependentDynamicsVel() must be called after ArticulatedBodyInertias.

// This dynamics-stage calculation is needed for handling constraints. It
// must be called base-to-tip (outward);
void realizeYOutward(
    const SBInstanceCache&                  ic,
    const SBTreePositionCache&              pc,
    const SBArticulatedBodyInertiaCache&    abc,
    SBDynamicsCache&                        dc) const;

// These routines give each node a chance to set appropriate defaults in a piece
// of the state corresponding to a particular stage. Default implementations here
// assume non-ball joint; override if necessary.
virtual void setMobilizerDefaultModelValues   (const SBTopologyCache&, SBModelVars&)  const {}
virtual void setMobilizerDefaultInstanceValues(const SBModelVars&, SBInstanceVars&) const {}
virtual void setMobilizerDefaultTimeValues    (const SBModelVars&, SBTimeVars&)    const {}

virtual void setMobilizerDefaultPositionValues(const SBModelVars& s, Vector& q) const 
{
    toQ(q) = 0;
}
virtual void setMobilizerDefaultVelocityValues(const SBModelVars&, Vector& u) const 
{
    toU(u) = 0;
}
virtual void setMobilizerDefaultDynamicsValues(const SBModelVars&, SBDynamicsVars&) const {}
virtual void setMobilizerDefaultAccelerationValues(const SBModelVars&, 
                                                   SBDynamicsVars& v) const {}

int          getDOF()            const {return dof;}
virtual int  getMaxNQ()          const {
    assert(quaternionUse == QuaternionIsNeverUsed);
    return dof; // maxNQ can be larger than dof if there's a quaternion
}

// Default method must be overridden if there is a chance that NQ
// might not be the same as NU.
virtual int  getNQInUse(const SBModelVars&) const {
    assert(quaternionUse == QuaternionIsNeverUsed); 
    return dof; // DOF <= NQ <= maxNQ
}

// Currently NU is always just the Mobilizer's DOFs (the template argument)
// Later we may want to offer modeling options to lock joints, or perhaps
// break them.
virtual int  getNUInUse(const SBModelVars&) const {
    return dof;
}

// Default method must be overridden if this mobilizer might use a
// quaternion under some conditions.
virtual bool isUsingQuaternion(const SBStateDigest&, MobilizerQIndex& startOfQuaternion) const {
    assert(quaternionUse == QuaternionIsNeverUsed);
    startOfQuaternion.invalidate();
    return false;
}

// You must override the next few methods if qdot might not be the same
// as u under *any* circumstance.

// This method should calculate qdot=N*u, where N=N(q) is the kinematic
// coupling matrix. State digest should be at Stage::Position.
virtual void calcQDot(const SBStateDigest&, 
                      const Real* u, Real* qdot) const 
{
    assert(qdotHandling == QDotIsAlwaysTheSameAsU);
    Vec<dof>::updAs(qdot) = Vec<dof>::getAs(u); // default says qdot=u
}

// This method should calculate u=N^-1*qdot, where N=N(q).
// State digest should be at Stage::Position.
virtual void calcLocalUFromLocalQDot(const SBStateDigest&, 
                                     const Real* qdot, Real* u) const 
{
    assert(qdotHandling == QDotIsAlwaysTheSameAsU);
    Vec<dof>::updAs(u) = Vec<dof>::getAs(qdot); // default says u=qdot
}

// This method should calculate qdotdot=N*udot + NDot*u, where N=N(q),
// NDot=NDot(q,u). State digest should be at Stage::Velocity.
virtual void calcQDotDot(const SBStateDigest&, 
                         const Real* udot, Real* qdotdot) const
{
    assert(qdotHandling == QDotIsAlwaysTheSameAsU);
    Vec<dof>::updAs(qdotdot) = Vec<dof>::getAs(udot); // default: qdotdot=udot
}

// This method should calculate udot=N^-1*(qdotdot-NDot*u), where N=N(q),
// NDot=NDot(q,u). State digest should be at Stage::Velocity.
virtual void calcLocalUDotFromLocalQDotDot(const SBStateDigest&, 
                                           const Real* qdotdot, Real* udot) const
{
    assert(qdotHandling == QDotIsAlwaysTheSameAsU);
    Vec<dof>::updAs(udot) = Vec<dof>::getAs(qdotdot); // default: udot=qdotdot
}




// State digest should be at Stage::Position for calculating N (the matrix that
// maps generalized speeds u to coordinate derivatives qdot, qdot=Nu). The 
// default implementation assumes nq==nu and the nqXnu block of N corresponding 
// to this mobilizer is identity. Then either operation (regardless of side) 
// just copies nu numbers from in to out.
//
// THIS MUST BE OVERRIDDEN by any mobilizer for which nq != nu, or qdot != u.
virtual void multiplyByN(const SBStateDigest&, bool matrixOnRight,  
                         const Real* in, Real* out) const
{
    assert(qdotHandling == QDotIsAlwaysTheSameAsU);
    Vec<dof>::updAs(out) = Vec<dof>::getAs(in);
}

virtual void multiplyByNInv(const SBStateDigest&, bool matrixOnRight, 
                            const Real* in, Real* out) const
{
    assert(qdotHandling == QDotIsAlwaysTheSameAsU);
    Vec<dof>::updAs(out) = Vec<dof>::getAs(in);
}

// State digest should be at Stage::Velocity for calculating NDot (the matrix 
// that is used in mapping generalized accelerations udot to coordinate 2nd 
// derivatives qdotdot, qdotdot=N udot + NDot u. The default implementation 
// assumes nq==nu and the nqXnu block of N corresponding to this mobilizer is 
// identity, so NDot is an nuXnu block of zeroes. Then either operation 
// (regardless of side) just copies nu zeros to out.
//
// THIS MUST BE OVERRIDDEN by any mobilizer for which nq != nu, or qdot != u.
virtual void multiplyByNDot(const SBStateDigest&, bool matrixOnRight, 
                            const Real* in, Real* out) const
{
    assert(qdotHandling == QDotIsAlwaysTheSameAsU);
    Vec<dof>::updAs(out) = 0;
}


// No default implementations here for:
// calcMobilizerTransformFromQ
// calcMobilizerVelocityFromU
// calcMobilizerAccelerationFromUDot
// calcParentToChildTransformFromQ
// calcParentToChildVelocityFromU
// calcParentToChildAccelerationFromUDot


virtual void setVelFromSVel(
    const SBTreePositionCache&  pc, 
    const SBTreeVelocityCache&  vc,
    const SpatialVec&           sVel, 
    Vector&                     u) const;

// Return true if any change is made to the q vector.
virtual bool enforceQuaternionConstraints(
    const SBStateDigest&    sbs,
    Vector&                 q,
    Vector&                 qErrest) const 
{
    assert(quaternionUse == QuaternionIsNeverUsed);
    return false;
}

void convertToEulerAngles(const Vector& inputQ, Vector& outputQ) const {
    // The default implementation just copies Q.  Subclasses may override this.
    assert(quaternionUse == QuaternionIsNeverUsed);
    toQ(outputQ) = fromQ(inputQ);
}

void convertToQuaternions(const Vector& inputQ, Vector& outputQ) const {
    // The default implementation just copies Q.  Subclasses may override this.
    assert(quaternionUse == QuaternionIsNeverUsed);
    toQ(outputQ) = fromQ(inputQ);
}

// Get a column of H_PB_G, which is what Jain calls H* and Schwieters calls H^T.
const SpatialVec& getHCol(const SBTreePositionCache& pc, int j) const {
    return getH(pc)(j);
}

// Get a column of H_FM the local cross-mobilizer hinge matrix expressed in the
// parent (inboard) mobilizer frame F.
const SpatialVec& getH_FMCol(const SBTreePositionCache& pc, int j) const {
    return getH_FM(pc)(j);
}

// Access to body-oriented state and cache entries is the same for all nodes,
// and joint oriented access is almost the same but parametrized by dof. There is a special
// case for quaternions because they use an extra state variable, and although we don't
// have to we make special scalar routines available for 1-dof joints. Note that all State access
// routines are inline, not virtual, so the cost is just an indirection and an index.
//
// TODO: these inner-loop methods probably shouldn't be indexing a Vector, which requires
// several indirections. Instead, the top-level caller should find the Real* data contained in the
// Vector and then pass that to the RigidBodyNode routines which will call these ones.

// General joint-dependent select-my-goodies-from-the-pool routines.
const Vec<dof>&     fromQ  (const Real* q)   const {return Vec<dof>::getAs(&q[qIndex]);}
Vec<dof>&           toQ    (      Real* q)   const {return Vec<dof>::updAs(&q[qIndex]);}
const Vec<dof>&     fromU  (const Real* u)   const {return Vec<dof>::getAs(&u[uIndex]);}
Vec<dof>&           toU    (      Real* u)   const {return Vec<dof>::updAs(&u[uIndex]);}
const Mat<dof,dof>& fromUSq(const Real* uSq) const {return Mat<dof,dof>::getAs(&uSq[uSqIndex]);}
Mat<dof,dof>&       toUSq  (      Real* uSq) const {return Mat<dof,dof>::updAs(&uSq[uSqIndex]);}

// Same, but specialized for the common case where dof=1 so everything is scalar.
const Real& from1Q  (const Real* q)   const {return q[qIndex];}
Real&       to1Q    (      Real* q)   const {return q[qIndex];}
const Real& from1U  (const Real* u)   const {return u[uIndex];}
Real&       to1U    (      Real* u)   const {return u[uIndex];}
const Real& from1USq(const Real* uSq) const {return uSq[uSqIndex];}
Real&       to1USq  (      Real* uSq) const {return uSq[uSqIndex];}

// Same, specialized for quaternions. We're assuming that the quaternion is stored
// in the *first* four coordinates, if there are more than four altogether.
const Vec4& fromQuat(const Real* q) const {return Vec4::getAs(&q[qIndex]);}
Vec4&       toQuat  (      Real* q) const {return Vec4::updAs(&q[qIndex]);}

// Extract a Vec3 from a Q-like or U-like object, beginning at an offset from the qIndex or uIndex.
const Vec3& fromQVec3(const Real* q, int offs) const {return Vec3::getAs(&q[qIndex+offs]);}
Vec3&       toQVec3  (      Real* q, int offs) const {return Vec3::updAs(&q[qIndex+offs]);}
const Vec3& fromUVec3(const Real* u, int offs) const {return Vec3::getAs(&u[uIndex+offs]);}
Vec3&       toUVec3  (      Real* u, int offs) const {return Vec3::updAs(&u[uIndex+offs]);}

// These provide an identical interface for when q,u are given as Vectors rather than Reals.

const Vec<dof>&     fromQ  (const Vector& q)   const {return fromQ(&q[0]);} // convert to array of Real
Vec<dof>&           toQ    (      Vector& q)   const {return toQ  (&q[0]);}
const Vec<dof>&     fromU  (const Vector& u)   const {return fromU(&u[0]);}
Vec<dof>&           toU    (      Vector& u)   const {return toU  (&u[0]);}
const Mat<dof,dof>& fromUSq(const Vector& uSq) const {return fromUSq(&uSq[0]);}
Mat<dof,dof>&       toUSq  (      Vector& uSq) const {return toUSq  (&uSq[0]);}

const Real& from1Q  (const Vector& q)   const {return from1Q  (&q[0]);}
Real&       to1Q    (      Vector& q)   const {return to1Q    (&q[0]);}
const Real& from1U  (const Vector& u)   const {return from1U  (&u[0]);}
Real&       to1U    (      Vector& u)   const {return to1U    (&u[0]);}
const Real& from1USq(const Vector& uSq) const {return from1USq(&uSq[0]);}
Real&       to1USq  (      Vector& uSq) const {return to1USq  (&uSq[0]);}

// Same, specialized for quaternions. We're assuming that the quaternion comes first in the coordinates.
const Vec4& fromQuat(const Vector& q) const {return fromQuat(&q[0]);}
Vec4&       toQuat  (      Vector& q) const {return toQuat  (&q[0]);}

// Extract a Vec3 from a Q-like or U-like object, beginning at an offset from the qIndex or uIndex.
const Vec3& fromQVec3(const Vector& q, int offs) const {return fromQVec3(&q[0], offs);}
Vec3&       toQVec3  (      Vector& q, int offs) const {return toQVec3  (&q[0], offs);}
const Vec3& fromUVec3(const Vector& u, int offs) const {return fromUVec3(&u[0], offs);}
Vec3&       toUVec3  (      Vector& u, int offs) const {return toUVec3  (&u[0], offs);}

// Applications of the above extraction routines to particular interesting items in the State. Note
// that you can't use these for quaternions since they extract "dof" items.

// Cache entries (cache is mutable in a const State)

    // Position


// CAUTION: our H definition is transposed from Jain and Schwieters.
const HType& getH_FM(const SBTreePositionCache& pc) const
  { return HType::getAs(&pc.storageForH_FM[2*uIndex]); }
HType&       updH_FM(SBTreePositionCache& pc) const
  { return HType::updAs(&pc.storageForH_FM[2*uIndex]); }

// "H" here should really be H_PB_G, that is, cross joint transition
// matrix relating parent and body frames, but expressed in Ground.
// CAUTION: our H definition is transposed from Jain and Schwieters.
const HType& getH(const SBTreePositionCache& pc) const
  { return HType::getAs(&pc.storageForH[2*uIndex]); }
HType&       updH(SBTreePositionCache& pc) const
  { return HType::updAs(&pc.storageForH[2*uIndex]); }

    // Velocity

// CAUTION: our H definition is transposed from Jain and Schwieters.
const HType& getHDot_FM(const SBTreeVelocityCache& vc) const
  { return HType::getAs(&vc.storageForHDot_FM[2*uIndex]); }
HType&       updHDot_FM(SBTreeVelocityCache& vc) const
  { return HType::updAs(&vc.storageForHDot_FM[2*uIndex]); }

// CAUTION: our H definition is transposed from Jain and Schwieters.
const HType& getHDot(const SBTreeVelocityCache& vc) const
  { return HType::getAs(&vc.storageForHDot[2*uIndex]); }
HType&       updHDot(SBTreeVelocityCache& vc) const
  { return HType::updAs(&vc.storageForHDot[2*uIndex]); }

    // Dynamics

// These are calculated with articulated body inertias.
const Mat<dof,dof>& getD(const SBArticulatedBodyInertiaCache& abc) const 
{return fromUSq(abc.storageForD);}
Mat<dof,dof>&       updD(SBArticulatedBodyInertiaCache&       abc) const 
{return toUSq  (abc.storageForD);}

const Mat<dof,dof>& getDI(const SBArticulatedBodyInertiaCache& abc) const 
{return fromUSq(abc.storageForDI);}
Mat<dof,dof>&       updDI(SBArticulatedBodyInertiaCache&       abc) const 
{return toUSq  (abc.storageForDI);}

const Mat<2,dof,Vec3>& getG(const SBArticulatedBodyInertiaCache& abc) const
  { return Mat<2,dof,Vec3>::getAs(&abc.storageForG[2*uIndex]); }
Mat<2,dof,Vec3>&       updG(SBArticulatedBodyInertiaCache&       abc) const
  { return Mat<2,dof,Vec3>::updAs(&abc.storageForG[2*uIndex]); }

const Vec<dof>& getTau(const SBInstanceCache& ic, const Real* tau) const {
    const PresForcePoolIndex tauIx = ic.getMobodInstanceInfo(nodeNum).firstPresForce;
    assert(tauIx.isValid());
    return Vec<dof>::getAs(&tau[tauIx]);
}
Vec<dof>& updTau(const SBInstanceCache& ic, Real* tau) const {
    const PresForcePoolIndex tauIx = ic.getMobodInstanceInfo(nodeNum).firstPresForce;
    assert(tauIx.isValid());
    return Vec<dof>::updAs(&tau[tauIx]);
}

    // Acceleration

const Vec<dof>&   getEpsilon (const SBTreeAccelerationCache& ac) const {return fromU (ac.epsilon);}
Vec<dof>&         updEpsilon (SBTreeAccelerationCache&       ac) const {return toU   (ac.epsilon);}
const Real&       get1Epsilon(const SBTreeAccelerationCache& ac) const {return from1U(ac.epsilon);}
Real&             upd1Epsilon(SBTreeAccelerationCache&       ac) const {return to1U  (ac.epsilon);}


void multiplyBySystemJacobian(
    const SBTreePositionCache&  pc,
    const Real*                 v,
    SpatialVec*                 Jv) const;

void multiplyBySystemJacobianTranspose(
    const SBTreePositionCache&  pc, 
    SpatialVec*                 zTmp,
    const SpatialVec*           X, 
    Real*                       JtX) const;

void calcEquivalentJointForces(
    const SBTreePositionCache&  pc,
    const SBDynamicsCache&      dc,
    const SpatialVec*           bodyForces,
    SpatialVec*                 allZ,
    Real*                       jointForces) const;

void calcUDotPass1Inward(
    const SBInstanceCache&      ic,
    const SBTreePositionCache&  pc,
    const SBArticulatedBodyInertiaCache&,
    const SBDynamicsCache&      dc,
    const Real*                 jointForces,
    const SpatialVec*           bodyForces,
    const Real*                 allUDot,
    SpatialVec*                 allZ,
    SpatialVec*                 allGepsilon,
    Real*                       allEpsilon) const;

void calcUDotPass2Outward(
    const SBInstanceCache&      ic,
    const SBTreePositionCache&  pc,
    const SBArticulatedBodyInertiaCache&,
    const SBTreeVelocityCache&  vc,
    const SBDynamicsCache&      dc,
    const Real*                 epsilonTmp,
    SpatialVec*                 allA_GB,
    Real*                       allUDot,
    Real*                       allTau) const;

void multiplyByMInvPass1Inward(
    const SBInstanceCache&      ic,
    const SBTreePositionCache&  pc,
    const SBArticulatedBodyInertiaCache&,
    const Real*                 f,
    SpatialVec*                 allZ,
    SpatialVec*                 allGepsilon,
    Real*                       allEpsilon) const OVERRIDE_11;

void multiplyByMInvPass2Outward(
    const SBInstanceCache&      ic,
    const SBTreePositionCache&  pc,
    const SBArticulatedBodyInertiaCache&,
    const Real*                 epsilonTmp,
    SpatialVec*                 allA_GB,
    Real*                       allUDot) const OVERRIDE_11;

// Also serves as pass 1 for inverse dynamics.
void calcBodyAccelerationsFromUdotOutward(
    const SBTreePositionCache&  pc,
    const SBTreeVelocityCache&  vc,
    const Real*                 allUDot,
    SpatialVec*                 allA_GB) const;

void calcInverseDynamicsPass2Inward(
    const SBTreePositionCache&  pc,
    const SBTreeVelocityCache&  vc,
    const SpatialVec*           allA_GB,
    const Real*                 jointForces,
    const SpatialVec*           bodyForces,
    SpatialVec*                 allFTmp,
    Real*                       allTau) const; 

void multiplyByMPass1Outward(
    const SBTreePositionCache&  pc,
    const Real*                 allUDot,
    SpatialVec*                 allA_GB) const;
void multiplyByMPass2Inward(
    const SBTreePositionCache&  pc,
    const SpatialVec*           allA_GB,
    SpatialVec*                 allFTmp,
    Real*                       allTau) const;

};

#endif // SimTK_SIMBODY_RIGID_BODY_NODE_SPEC_H_