text: /* -------------------------------------------------------------------------- *
   *                               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: Derived from IVM code written by Charles Schwieters          *
   *                                                                            *
   * 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.                                             *
   * -------------------------------------------------------------------------- */
  
  namespace SimTK {
  void dummySymbolToAvoidWarningInLengthConstraints() {}
  };
  
  #ifdef NOTDEF
  
  
  /**@file
   *
   * Module for bond-length constraints use a shake-like algorithm.
   * Requires integrator to specify time derivatives to constrain.
   * 
   * setup:
   * 1) determine couplings
   * 2) determine which time derivatives to be applied
   * 
   * execution:
   * 1) need all time derivs of theta passed in
   * 2) iteratively apply constraints from lowest to highest time deriv
   * 
   * Deal with loop bond-length constraints.
   */
  
  #include "simbody/internal/common.h"
  using namespace SimTK;
  
  #include "LengthConstraints.h"
  #include "RigidBodyNode.h"
  #include "SimbodyMatterSubsystemRep.h"
  
  #include "newtonRaphson.h"
  
  #include <iostream>
  using std::ostream;
  using std::cout;
  using std::endl;
  using std::cerr;
  using std::setw;
  
  #include <algorithm>
  
  class LoopWNodes;
  static int compareLevel(const LoopWNodes& l1,    //forward declarations
                          const LoopWNodes& l2);
  
  //static bool sameBranch(const RigidBodyNode* tip,
  //                       const LoopWNodes& l );
  
  class BadNodeDef {};  //exception
  
  
  LoopWNodes::LoopWNodes(const SimbodyMatterSubsystemRep& t, const RBDistanceConstraint& dc)
    : tree(&t), rbDistCons(&dc), flipStations(false), outmostCommonBody(0)
  {
      const RigidBodyNode* dcNode1 = &dc.getStation(1).getNode();
      const RigidBodyNode* dcNode2 = &dc.getStation(2).getNode();
  
      if (dcNode1==dcNode2) {
          cout << "LoopWNodes::LoopWNodes: bad topology:\n\t"
               << "loop stations " << dc.getStation(1)
               << " and  "         << dc.getStation(2)
               << " are now in the same node. Deleting loop.\n";
          SimTK_THROW1(SimTK::Exception::LoopConstraintConstructionFailure, "bad topology");
      }
  
      // Ensure that tips(2) is on the body which is farther from Ground.
      flipStations = (dcNode1->getLevel() > dcNode2->getLevel());
  
      // OK to use tips() at this point.
  
      // Collect up the node path from tips(2) down to the last node on its
      // side of the loop which is at a higher level than tips(1) (may be none).
      const RigidBodyNode* node1 = &tips(1).getNode();
      const RigidBodyNode* node2 = &tips(2).getNode();
      while ( node2->getLevel() > node1->getLevel() ) {
          nodes[1].push_back(node2);
          node2 = node2->getParent();
      }
  
      // We're at the same level on both sides of the loop. Run down both
      // simultaneously until we hit the first common ancestor, collecting
      // up the nodes along the two paths (but not the common ancestor).
      while ( node1 != node2 ) {
          if ( node1->isGroundNode() ) {
              cerr << "LoopWNodes::LoopWNodes: could not find base node.\n\t"
                   << "loop between stations " << tips(1) << " and " 
                   << tips(2) << "\n";
              SimTK_THROW1(SimTK::Exception::LoopConstraintConstructionFailure, 
                           "could not find base node");
          }
          nodes[0].push_back(node1);
          nodes[1].push_back(node2);
          node1 = node1->getParent();
          node2 = node2->getParent();
      }
  
      outmostCommonBody = node1;   // that's the common ancestor; might be Ground
  
      // We want these in base-to-tip order.
      std::reverse(nodes[0].begin(), nodes[0].end());
      std::reverse(nodes[1].begin(), nodes[1].end());
  
      // Make the list of ancestors, starting with the outmostCommonBody unless it
      // is ground. Put in base-to-tip order.
      const RigidBodyNode* next = outmostCommonBody;
      while (next->getLevel() != 0) {
          ancestors.push_back(next);
          next = next->getParent();
      }
      std::reverse(ancestors.begin(), ancestors.end());
  }
  
  
  ostream& 
  operator<<(ostream& o, const LengthSet& s) 
  {
      o << "LengthSet --------------->\n";
      for (int i=0 ; i<(int)s.loops.size() ; i++) {
          o << "Loop " << i << ":\n" << s.loops[i];
      }
      o << "\nNodemap: ";
      for (int i=0; i<(int)s.nodeMap.size(); ++i)
          o << " " << s.nodeMap[i]->getNodeNum()
              << "[" << s.nodeMap[i]->getLevel() << "]";
      o << "\n<-------- end of LengthSet\n";
      return o;
  }
  
  
  ostream& 
  operator<<(ostream& o, const LoopWNodes& w) 
  {
      o << "tip1=" << w.tips(1) << " tip2=" << w.tips(2) 
        << " distance=" << w.getDistance() << endl;
      o << "nodes[0]:";
      for (int i=0; i<(int)w.nodes[0].size(); ++i)
          o << " " << w.nodes[0][i]->getNodeNum() 
                   << "[" << w.nodes[0][i]->getLevel() << "]";
      o << "\nnodes[1]:";
      for (int i=0; i<(int)w.nodes[1].size(); ++i)
          o << " " << w.nodes[1][i]->getNodeNum() 
                   << "[" << w.nodes[1][i]->getLevel() << "]";
      o << "\nOutmost common body: " << w.getOutmostCommonBody()->getNodeNum()
          << "[" << w.getOutmostCommonBody()->getLevel() << "]";
      o << "\nAncestors: ";
      for (int i=0; i<(int)w.ancestors.size(); ++i)
          o << " " << w.ancestors[i]->getNodeNum()
              << "[" << w.ancestors[i]->getLevel() << "]";
      o << endl;
      return o;
  }
  
  LengthConstraints::LengthConstraints
      (const SimbodyMatterSubsystemRep& rbt, int vbose)
    : maxIters( 20 ), maxMin( 20 ), 
      rbTree(rbt), verbose(vbose), posMin("posMin", cout), velMin("velMin", cout)
  {
      posMin.maxIters = maxIters;
      posMin.maxMin   = maxMin;
      velMin.maxIters = maxIters;
      velMin.maxMin   = maxMin;
  }
  
  //
  // compare LoopWNodes by outmost common node level value
  //
  static int
  compareLevel(const LoopWNodes& l1,
               const LoopWNodes& l2) 
  { 
      if ( l1.getOutmostCommonBody()->getLevel() > l2.getOutmostCommonBody()->getLevel() ) 
          return 1;
      else if ( l1.getOutmostCommonBody()->getLevel() < l2.getOutmostCommonBody()->getLevel() )
          return -1;
      else 
          return 0;
  }
  
  // Define this operator so std::sort will work.
  static inline bool operator<(const LoopWNodes& l1, const LoopWNodes& l2) {
      return compareLevel(l1,l2) == -1;
  }
  
  //1) construct: given list of loops 
  //   a) if appropriate issue warning and exit.
  //   b) sort by base node of each loop.
  //   c) find loops which intersect: combine loops and increment
  //    number of length constraints
  //
  // Sherm's constraint coupling hypothesis (060616):
  //
  // For a constraint i:
  //   Kinematic participants K[i] 
  //       = the set of bodies whose mobilities can affect verr[i]
  //   Dynamic participants D[i]
  //       = the set of bodies whose effective inertias can change 
  //         as a result of enforcement of constraint i
  // TODO: A thought on dynamic coupling -- is this the same as computing
  //       effective joint forces? Then only mobilities that can feel a
  //       test constraint force can be considered dynamic participants of
  //       that constraint.
  // 
  // Constraints i & j are directly kinematically coupled if 
  //   K[i] intersect K[j] <> { }
  // Constraints i & j are directly dynamically coupled if
  //   (a) they are directly kinematically coupled, or
  //   (b) D[i] intersect D[j] <> { }
  // TODO: I *think* dynamic coupling is one-way, from outboard to inboard.
  //       That means it defines an evaluation *order*, where modified 
  //       inertias from outboard loops become the articulated body
  //       inertias to use in the inboard loops, rather than requiring simultaneous
  //       solutions for the multipliers.
  //       Kinematic coupling, on the other hand, is symmetric and implies
  //       simultaneous solution.
  //
  // Compute transitive closure:
  //   Constraints i & j are kinematically coupled if
  //   (a) they are directly kinematically coupled, or
  //   (b) constraint i is kinematically coupled to any constraint
  //          k to which j is kinematically coupled
  // That is, build kinematic clusters with completely disjoint constraints.
  //
  // For now, do the same thing with acceleration constraints but this
  // is too strict. TODO
  // 
  //   
  void
  LengthConstraints::construct(const Array_<RBDistanceConstraint*>& iloops)
  {
      //clean up
      pvConstraints.resize(0);
      accConstraints.resize(0);
  
      // if ( !useLengthConstraint )  FIX:!!!
      //   //issue error message?
      //   return;
  
      LoopList loops;
      for (int i=0 ; i < (int)iloops.size() ; i++) {
          try {
              LoopWNodes loop(rbTree, *iloops[i] );
              loops.push_back( loop );
          }
          catch ( BadNodeDef ) {}
      }
  
      //cout << "RAW LOOPS:\n";
      //for (int i=0; i < (int)loops.size(); ++i)
      //    cout << "Loop " << i << ":\n  " << loops[i] << endl;
  
      // sort loops by outmostCommonBody->level
      std::sort(loops.begin(), loops.end()); // uses "<" operator by default; see above
      LoopList accLoops = loops;  //version for acceleration
  
      // Sherm 060616: transitive closure calculation. Remember that 
      // the constraints are sorted by the level of their outmost common body, 
      // starting at ground, so that more-outboard constraints cannot cause
      // us to need to revisit an earlier cluster. However, constraints at
      // the same level can bring together earlier ones at that level.
      // TODO: currently just restarting completely when we add a
      // constraint to a cluster.
  
      for (int i=0 ; i<(int)loops.size() ; i++) {
          pvConstraints.push_back(LengthSet(this));
          pvConstraints.back().addKinematicConstraint(loops[i]);
          bool addedAConstraint;
          do {
              addedAConstraint = false;
              for (int j=i+1 ; j<(int)loops.size() ; j++)
                  if (   (loops[j].nodes[0].size()
                          && pvConstraints[i].contains(loops[j].nodes[0][0]))
                      || (loops[j].nodes[1].size()
                          && pvConstraints[i].contains(loops[j].nodes[1][0])))
                  {
                      //add constraint equation j to cluster i
                      pvConstraints[i].addKinematicConstraint(loops[j]);
                      loops.erase(loops.begin() + j); // STL for &loops[j]
                      addedAConstraint = true;
                      break;
                  }
          } while (addedAConstraint);
      }
  
      for (int i=0 ; i<(int)accLoops.size() ; i++) {
          accConstraints.push_back(LengthSet(this));
          accConstraints.back().addDynamicConstraint(accLoops[i]);
          bool addedAConstraint;
          do {
              addedAConstraint = false;
              for (int j=i+1 ; j<(int)accLoops.size() ; j++)
                  if (   (accLoops[j].nodes[0].size()
                          && accConstraints[i].contains(accLoops[j].nodes[0][0]))
                      || (accLoops[j].nodes[1].size()
                          && accConstraints[i].contains(accLoops[j].nodes[1][0]))
                      || (accLoops[j].ancestors.size()
                          && accConstraints[i].contains(accLoops[j].ancestors[0])))
                  {
                      //add constraint equation j to cluster i
                      accConstraints[i].addDynamicConstraint(accLoops[j]);
                      accLoops.erase(accLoops.begin() + j); // STL for &accLoops[j]
                      addedAConstraint = true;
                      break;
                  }
          } while (addedAConstraint);
      }
  
      if (false && pvConstraints.size()>0) {
          cout << "LengthConstraints::construct: pos/vel length sets found:\n";
          for (int i=0 ; i<(int)pvConstraints.size() ; i++)
              cout << pvConstraints[i] << "\n";
          cout << "LengthConstraints::construct: accel length sets found:\n";
          for (int i=0 ; i<(int)accConstraints.size() ; i++)
              cout << accConstraints[i] << "\n";
      }
  }
  
  void LengthSet::addKinematicConstraint(const LoopWNodes& loop) {
      loops.push_back( LoopWNodes(loop) );
      for (int b=0 ; b<2 ; b++)
          for (int i=0; i<(int)loop.nodes[b].size(); i++)
              if (std::find(nodeMap.begin(),nodeMap.end(),loop.nodes[b][i])
                  ==nodeMap.end())
              {
                  // not found
                  ndofThisSet += loop.nodes[b][i]->getDOF();
                  nodeMap.push_back( loop.nodes[b][i] );
              }
  }
  
  void LengthSet::addDynamicConstraint(const LoopWNodes& loop) {
      loops.push_back( LoopWNodes(loop) );
      for (int b=0 ; b<2 ; b++)
          for (int i=0; i<(int)loop.nodes[b].size(); i++)
              if (std::find(nodeMap.begin(),nodeMap.end(),loop.nodes[b][i])
                  ==nodeMap.end())
              {
                  // not found
                  ndofThisSet += loop.nodes[b][i]->getDOF();
                  nodeMap.push_back( loop.nodes[b][i] );
              }
  
      for (int i=0; i<(int)loop.ancestors.size(); i++)
          if (std::find(nodeMap.begin(),nodeMap.end(),loop.ancestors[i])
              ==nodeMap.end())
          {
              // not found
              ndofThisSet += loop.ancestors[i]->getDOF();
              nodeMap.push_back( loop.ancestors[i] );
          }
  }
  
  bool LengthSet::contains(const RigidBodyNode* node) {
      return std::find(nodeMap.begin(),nodeMap.end(),node) != nodeMap.end();
  }
  
  class CalcPosB {
      State&   s;
      const LengthSet* lengthSet;
  public:
      CalcPosB(State& ss, const LengthSet* lset)
          : s(ss), lengthSet(lset) {}
      Vector operator()(const Vector& pos) const
          { return lengthSet->calcPosB(s,pos); }
  };
  
  class CalcPosZ {
      const State&   s;
      const LengthSet* lengthSet;
  public:
      CalcPosZ(const State& ss, const LengthSet* constraint) 
          : s(ss), lengthSet(constraint) {}
      Vector operator()(const Vector& b) const 
          { return lengthSet->calcPosZ(s, b); }
  };
  
  class CalcVelB {
      State&           s;
      const LengthSet* lengthSet;
  public:
      CalcVelB(State& ss, const LengthSet* constraint)
          : s(ss), lengthSet(constraint) {}
      Vector operator()(const Vector& vel) 
          { return lengthSet->calcVelB(s,vel); }
  };
  
  //
  // Calculate the position constraint violation (zero when constraint met).
  //
  Vector
  LengthSet::calcPosB(State& s, const Vector& pos) const
  {
      setPos(s, pos);
  
      // Although we're not changing the qErr cache entry here, we access
      // it with "upd" because setPos() will have modified the q's and
      // thus invalidated stage Position, so a "get" would fail.
      const Vector& qErr = getRBTree().updQErr(s);
      Vector b((int)loops.size());
      for (int i=0; i<(int)loops.size(); ++i)
          b[i] = loops[i].rbDistCons->getPosErr(qErr);
      return b;
  }
  
  //
  // Calculate the velocity constraint violation (zero when constraint met).
  //
  Vector
  LengthSet::calcVelB(State& s, const Vector& vel) const 
  {
      setVel(s, vel);
  
      // Although we're not changing the uErr cache entry here, we access
      // it with "upd" because setVel() will have modified the u's and
      // thus invalidated stage Velocity, so a "get" would fail.
      const Vector& uErr = getRBTree().updUErr(s);
  
      Vector b((int)loops.size());
      for (int i=0; i<(int)loops.size(); ++i)
          b[i] = loops[i].rbDistCons->getVelErr(uErr);
      return b;
  }
  
  //
  // Given a vector containing violations of position constraints, calculate
  // a state update which should drive those violations to zero (if they
  // were linear).
  //
  // This is a little tricky since the gradient we have is actually the
  // gradient of the *velocity* errors with respect to the generalized speeds,
  // but we want the gradient of the *position* errors with respect to 
  // the generalized coordinates. The theory goes something like this:
  //
  //       d perr    d perr_dot   d verr    d u
  //       ------  = ---------- = ------ * -----
  //        d q      d qdot        d u     d qdot
  //
  // Let P = d perr/dq, A = d verr/du, Q = d qdot/du.
  //
  // We want to find a change deltaq that will elimate the current error b:
  // P deltaq = b. [WRONG:]Instead we solve A * x = b, where x = inv(Q) * deltaq,
  // and then solve deltaq = Q * x. Conveniently Q is our friendly invertible
  // relation between qdot's and u's: qdot = Q*u.
  //
  // TODO: Sherm 080101: the above is incorrect. We have to factor (PQ^-1) and
  // solve (PQ^-1)*deltaq = b for LS deltaq, which is not the same as 
  // solving for LS x in P*x=b and then deltaq=Q*x.
  //
  // TODO: I have oversimplified the above since we are really looking for
  // a least squares solution to an underdetermined system. With the 
  // pseudoinverse A+ available we can write x = A+ * b.
  //
  Vector
  LengthSet::calcPosZ(const State& s, const Vector& b) const
  {
      const Matrix Gt = calcGrad(s);
      const Vector x = calcPseudoInverseA(Gt) * b;
  
      const SBStateDigest digest(s, getRBTree(), Stage::Position);
  
      Vector       zu(getRBTree().getTotalDOF(),0.);
      Vector       zq(getRBTree().getTotalQAlloc(),0.);
  
      // map the vector dir back to the appropriate elements of z
      int indx=0; // sherm 060222: I added this
      for (int i=0 ; i<(int)nodeMap.size() ; i++) {
          const int d    = nodeMap[i]->getDOF();
          const int offs = nodeMap[i]->getUIndex();
          zu(offs,d) = x(indx,d);
          indx += d;
  
          // Make qdot = Q*u.
          nodeMap[i]->calcQDot(digest,zu,zq);  // change u's to qdot's
      }
      assert(indx == ndofThisSet);
  
      return zq;
  }
  
  //
  // Given a vector containing violations of velocity constraints, calculate
  // a state update which would drive those violations to zero (if they
  // are linear and well conditioned).
  //
  // This is simpler than CalcPosZ because we have the right gradient here (it's
  // the same one in both places). TODO: and shouldn't be recalculated!
  //
  class CalcVelZ {
      const State&   s;
      const LengthSet* lengthSet;
      const Matrix Gt;
      const Matrix GInverse;
  public:
      CalcVelZ(const State& ss, const LengthSet* lset)
        : s(ss), lengthSet(lset), Gt(lset->calcGrad(s)),
          GInverse(LengthSet::calcPseudoInverseA(Gt)) 
      {
      }
  
      Vector operator()(const Vector& b) {
          const Vector dir = GInverse * b;
          Vector       z(lengthSet->getRBTree().getTotalDOF(),0.0);
  
          // map the vector dir back to the appropriate elements of z
          int indx=0; // sherm 060222: I added this
          for (int i=0 ; i<(int)lengthSet->nodeMap.size() ; i++) {
              const RigidBodyNode* n = lengthSet->nodeMap[i];
              const int d    = n->getDOF();
              const int offs = n->getUIndex();
              z(offs,d) = dir(indx,d);
              indx += d;
          }
          assert(indx == lengthSet->ndofThisSet);
          return z;
      }
  };
  
  // Project out the position constraint errors from the given state. 
  bool
  LengthConstraints::enforcePositionConstraints(State& s, const Real& requiredTol, const Real& desiredTol) const
  {
      assert(rbTree.getStage(s) >= Stage::Position-1);
      Vector& pos = rbTree.updQ(s);
  
      bool anyChanges = false;
  
      try { 
          for (int i=0 ; i<(int)pvConstraints.size() ; i++) {
              anyChanges = true; // TODO: assuming for now
              posMin.calc(requiredTol, desiredTol, pos,
                          CalcPosB(s, &pvConstraints[i]),
                          CalcPosZ(s, &pvConstraints[i]));
          }
      }
      catch (const SimTK::Exception::NewtonRaphsonFailure& cptn ) {
          cout << "LengthConstraints::enforcePositionConstraints: exception: "
               << cptn.getMessage() << '\n';
      } 
  
      return anyChanges;
  }
  
  // Project out the velocity constraint errors from the given state. 
  bool
  LengthConstraints::enforceVelocityConstraints(State& s, const Real& requiredTol, const Real& desiredTol) const
  {
      assert(rbTree.getStage(s) >= Stage(Stage::Velocity).prev());
      Vector& vel = rbTree.updU(s);
  
      bool anyChanges = false;
  
      try { 
          for (int i=0 ; i<(int)pvConstraints.size() ; i++) {
              anyChanges = true; // TODO: assuming for now
              velMin.calc(requiredTol, desiredTol, vel,
                          CalcVelB(s, &pvConstraints[i]),
                          CalcVelZ(s, &pvConstraints[i]));
          }
      }
      catch (const  SimTK::Exception::NewtonRaphsonFailure& cptn ) {
          cout << "LengthConstraints::enforceVelocityConstraints: exception: "
               << cptn.getMessage() << '\n';
      } 
  
      return anyChanges;
  }
  
  //
  //// on each iteration
  //// for each loop
  //// -first calc all usual properties
  //// -recursively compute phi_ni for each length constraint
  ////   from tip to outmost common body of 
  //// -compute gradient
  //// -update theta using quasi-Newton-Raphson
  //// -compute Cartesian coords
  //// -check for convergence
  //// -repeat
  //
  
  
  void LengthSet::setPos(State& s, const Vector& pos) const
  {
      const SBStateDigest digest(s, getRBTree(), Stage::Position);
      Vector&            q  = getRBTree().updQ(s);
      SBPositionCache&   pc = getRBTree().updPositionCache(s);
      Vector&            qErr = getRBTree().updQErr(s);
  
      for (int i=0 ; i<(int)nodeMap.size() ; i++)
          nodeMap[i]->copyQ(digest, pos, q);
  
      // TODO: sherm this is the wrong place for the stage update!
      s.invalidateAll(Stage::Position);
  
      // sherm TODO: this now computes kinematics for the whole system,
      // but it should only have to update the loop we are interested in.
      // Schwieters had this right before because his equivalent of 'setQ'
      // above also performed the kinematics, while ours just saves the
      // new state variable values and calculates here:
      getRBTree().realizeSubsystemPosition(s);
  
      // TODO: This is redundant after realizePosition(), but I'm leaving
      // it here because this is actually all that need be recalculated for
      // the loop closure iterations.
      for (int i=0; i<(int)loops.size(); ++i)
          loops[i].calcPosInfo(qErr,pc);
  }
  
  // Must have called LengthSet::setPos() already.
  void LengthSet::setVel(State& s, const Vector& vel) const
  {
      const SBStateDigest digest(s, getRBTree(), Stage::Position);
      const SBPositionCache& pc = getRBTree().getPositionCache(s);
      Vector&                u  = getRBTree().updU(s);
      SBVelocityCache&       vc = getRBTree().updVelocityCache(s);
      Vector&                uErr = getRBTree().updUErr(s);
  
      for (int i=0 ; i<(int)nodeMap.size() ; i++)
          nodeMap[i]->copyU(digest, vel, u);
  
      // TODO: sherm this is the wrong place for the stage update!
      s.invalidateAll(Stage::Velocity);
  
      getRBTree().realizeSubsystemVelocity(s);
  
      // TODO: see comment above in setPos
      for (int i=0; i<(int)loops.size(); ++i)
          loops[i].calcVelInfo(pc,uErr,vc);
  }
  
  //
  ////A = df / dtheta_i
  ////  = [ -(q_ni-qn0)x , 1 ] [ phi_na^T Ha , phi_nb^T Hb , ... ]^T
  ////
  //
  
  // Calculate gradient by central difference for testing the analytic
  // version. Presumes that calcEnergy has been called previously with current 
  // value of ipos.
  void 
  LengthSet::fdgradf(State& s,
                     const Vector&  pos,
                     Matrix&        grad) const 
  {
      const SBModelVars& mv = getRBTree().getModelVars(s);
  
      // Gradf gradf(tree);
      // gradf(x,grad); return;
      const double eps = 1e-6;
      const CalcPosB calcB(s, this);
      const Vector b = calcB(pos);
      int grad_indx=0;
      for (int i=0 ; i<(int)nodeMap.size() ; i++) {
          int pos_indx=nodeMap[i]->getQIndex();
          for (int j=0 ; j<nodeMap[i]->getNQInUse(mv) ; j++,pos_indx++,grad_indx++) {
              Vector posp = pos;
              posp(pos_indx) += eps;
              const Vector bp = calcB(posp);
              posp(pos_indx) -= 2.*eps;
              const Vector bpm = calcB(posp);
              for (int k=0 ; k<b.size() ; k++)
                  grad(grad_indx,k) = -(bp(k)-bpm(k)) / (2.*eps);
          }
      }
  }
  
  void 
  LengthSet::testGrad(State& s, const Vector& pos, const Matrix& grad) const
  {
      double tol = 1e-4;
  
      Matrix fdgrad(ndofThisSet,loops.size());
      fdgradf(s,pos,fdgrad);
  
      for (int i=0 ; i<grad.nrow() ; i++)
          for (int j=0 ; j<grad.ncol() ; j++)
              if (fabs(grad(i,j)-fdgrad(i,j)) > fabs(tol))
                  cout << "testGrad: error in gradient: " 
                       << setw(2) << i << ' '
                       << setw(2) << j << ": "
                       << grad(i,j) << ' ' << fdgrad(i,j) << '\n';
      cout.flush();
  }
  
  //
  // unitVec(p+ - p-) * d (p+ - p-) / d (theta_i)
  // d g / d theta for all hingenodes in nodemap
  //
  // sherm: this appears to calculate the transpose of G
  //
  // sherm 060222: OK, this routine doesn't really do what it says
  // when the constraint includes a ball or free joint. It
  // does not use the actual q's, which are quaternions. Instead
  // it is something like d(v+ - v-)/du. If the u's are the 
  // angular velocity across the joint then this is
  // d(p+ - p-)/d qbar where qbar is a 1-2-3 Euler sequence
  // which is 0,0,0 at the current orientation. ("instant
  // coordinates").
  //
  // sherm: 060303 This routine uses the joint transition matrices ~H,
  // which can be thought of as Jacobians ~H = d V_PB_G / d uB, that is
  // partial derivative of the cross-joint relative *spatial* velocity
  // with respect to that joint's generalized speeds uB. This allows
  // analytic computation of d verr / d u where verr is the set of
  // distance constraint velocity errors for the current set of 
  // coupled loops, and u are the generalized speeds for all joints
  // which contribute to any of those loops. Some times this is the
  // gradient we want, but we also want to use this routine to
  // calculate d perr / d q, which we can't get directly. But it
  // is easily finagled into the right gradient; see CalcPosZ above.
  // The trickiest part is that we use nice physical variables for
  // generalized speeds, like angular velocities for ball joints,
  // but we use awkward mathematical constructs like quaternions and
  // Euler angles for q's.
  //
  Matrix
  LengthSet::calcGrad(const State& s) const
  {
      // We're not updating, but need to use upd here because Position stage
      // was invalidated by change to state.
      const SBStateDigest digest(s, getRBTree(), Stage::Position);
      const SBPositionCache& pc = getRBTree().updPositionCache(s);
  
      Matrix grad(ndofThisSet,loops.size(),0.0);
      const Mat33 one(1);  //FIX: should be done once
  
      for (int i=0 ; i<(int)loops.size() ; i++) {
          const LoopWNodes& l = loops[i];
          Array_<SpatialMat> phiT[2];
          for (int b=0 ; b<2 ; b++) {
              phiT[b].resize( l.nodes[b].size() );
              if ( l.nodes[b].size() ) {
                  phiT[b][phiT[b].size()-1] = 1;  // identity
                  for (int j=l.nodes[b].size()-2 ; j>=0 ; j-- ) {
                      const RigidBodyNode* n = l.nodes[b][j+1];
                      phiT[b][j] = phiT[b][j+1] * ~n->getPhi(pc);
                  }
              }
          }
  
          // compute gradient
          //Vec3 uBond = unitVec(l.tipPos(pc,2) - l.tipPos(pc,1));
          const Vec3 uBond = l.tipPos(pc,2) - l.tipPos(pc,1); // p
          Row<2,Mat33> J[2];
          for (int b=1 ; b<=2 ; b++)
              // TODO: get rid of this b-1; make tips 0-based
              J[b-1] = Row<2,Mat33>(-crossMat(l.tipPos(pc,b) -
                                              l.tips(b).getNode().getX_GB(pc).p()),   one);
          int g_indx=0;
          for (int j=0 ; j<(int)nodeMap.size() ; j++) {
              Real elem=0.0;
  
              // We just want to get the index at which nodeMap[j] is found in the
              // Array_ (or -1 if not found) but that's not so easy!
              const Array_<const RigidBodyNode*>& n0 = l.nodes[0];
              const Array_<const RigidBodyNode*>& n1 = l.nodes[1];
              Array_<const RigidBodyNode*>::const_iterator found0 =
                  std::find(n0.begin(),n0.end(),nodeMap[j]);
              Array_<const RigidBodyNode*>::const_iterator found1 =
                  std::find(n1.begin(),n1.end(),nodeMap[j]);
  
              const int l1_indx = (found0==n0.end() ? -1 : found0-n0.begin());
              const int l2_indx = (found1==n1.end() ? -1 : found1-n1.begin());
  
              for (int k=0 ; k < nodeMap[j]->getDOF() ; k++) {
                  const SpatialVec& HtCol = ~nodeMap[j]->getHRow(digest, k);
                  if ( l1_indx >= 0 ) { 
                      elem = -dot(uBond , Vec3(J[0] * phiT[0][l1_indx]*HtCol));
                  } else if ( l2_indx >= 0 ) { 
                      elem =  dot(uBond , Vec3(J[1] * phiT[1][l2_indx]*HtCol));
                  }
                  grad(g_indx++,i) = elem;
              }
          }
          // added by sherm 060222:
          assert(g_indx == ndofThisSet); // ??
      }
      return grad;
  } 
  
  //
  // Calculate generalized inverse which minimizes changes in soln vector.
  // TODO (sherm) This is trying to create a pseudoinverse
  // using normal equations which is numerically bad and can't deal with
  // redundant constraints. Should use an SVD or (faster) QTZ factorization 
  // instead.
  //
  // sherm 060314:
  //                           n
  //                  --------------------
  //                 |                    |
  //     A (mXn) = m |                    |   rank(A) <= m.
  //                 |                    |
  //                  --------------------
  //
  // The nXm pseudo-inverse A+ of a matrix A is A+ = V S+ ~U, where A=U*S*~V
  // is the singular value decomposition (SVD) of A, and S+ is the inverse
  // of the diagonal matrix S with some zeroes thrown it at the end after
  // we run out of rank(A). For an underdetermined system with full row
  // rank, that is, assuming m < n and rank(A)=m, you can do a poor man's
  // calculation of this as A+ = ~A*inv(A*~A). In the overdetermined
  // case we have m > n and rank(A)=n, in which case A+ = inv(~A*A)*~A.
  //
  // We are given gradient G (nuXnc). We're assuming nu > nc and rank(G)=nc, i.e.,
  // no redundant constraints (not a good assumption in general!). We would like
  // to get a least squares solution to ~G x = b where x has dimension nu and
  // b has dimension nc. So if we set A=~G we have the underdetermined system
  // depicted above and want to return A+ = ~A*inv(A*~A) = G*inv(~G*G). Ergo ...
  /*static*/ Matrix
  LengthSet::calcPseudoInverseA(const Matrix& transposeOfA)
  {
      const Matrix A = ~transposeOfA; // now A is dg/dtheta
      const int m = A.nrow(); // see picture above
      const int n = A.ncol();
  
      // Now calculate the pseudo inverse of A.
      Matrix pinvA(n,m,0.);
      if ( A.normSqr() > 1e-10 ) {
          if (m < n)
              pinvA = ~A * (A * ~A).invert(); // normal underdetermined case
          else if (m > n)
              pinvA = (A * ~A).invert() * ~A; // wow, that's a lot of constraints!
          else 
              pinvA = A.invert();             // not likely
      }
      return pinvA;
  }
  
     
  //
  // Given a vector in mobility space, project it along the motion constraints
  // of this LengthSet, by removing its component normal to the constraint
  // manifold. Here we want to obtain a least squares solution x to
  //   A x = A v, A=[ncXnu] is the constraint Jacobian.
  // We solve with pseudo inverse (see calcPseudoInverseA):
  //   Least squares x = A+ * Av.
  // That is the component of v which is normal to the constraint manifold.
  // So we then set v = v - x and return.
  //
  // TODO: projections for integration errors should be done in the
  // error norm. I'm not sure whether that has to be done in this routine
  // or whether caller can scale on the way in and out.
  //
  void
  LengthSet::projectUVecOntoMotionConstraints(const State& s, Vector& v)
  {
      const Matrix transposeOfA = calcGrad(s);
      const Matrix pinvA = calcPseudoInverseA(transposeOfA); // TODO: this should already be available
  
      const Vector rhs  = packedMatTransposeTimesVec(transposeOfA,v); // i.e., rhs = Av
  
      //FIX: using inverse is inefficient
      const Vector x = pinvA * rhs;
  
      // subtract forces due to these constraints
      subtractPackedVecFromVec(v, x); 
  }
  
  Matrix
  LengthSet::calcPseudoInverseAFD(const State& s) const
  {
      Matrix grad(ndofThisSet,loops.size()); // <-- transpose of the actual dg/dtheta
      State sTmp = s;
      Vector pos = getRBTree().getQ(s); // a copy
      fdgradf(sTmp,pos,grad);
  
      const Matrix A = ~grad; // now A is dg/dtheta
      const int m = A.nrow(); // see picture above
      const int n = A.ncol();
  
      // Now calculate the pseudo inverse of A.
      Matrix pinvA(n,m,0.);
      if ( A.normSqr() > 1e-10 ) {
          if (m < n)
              pinvA = ~A * (A * ~A).invert(); // normal underdetermined case
          else if (m > n)
              pinvA = (A * ~A).invert() * ~A; // wow, that's a lot of constraints!
          else 
              pinvA = A.invert();             // not likely
      }
      return pinvA;
  }
  
  //acceleration:
  //  0) after initial acceleration calculation:
  //  1) calculate Y (block diagonal matrix) for all nodes (common body to tip)
  //  2) for each LengthSet, calculate Lagrange multiplier(s) and add in
  //     resulting force- update accelerations
  //     Do this step from tip to base.
  //
  
  bool LengthConstraints::calcConstraintForces(const State& s, const Vector& udotErr,
                                               Vector& multipliers, 
                                               SBAccelerationCache& ac) const 
  {
      if ( accConstraints.size() == 0 )
          return false;
  
      rbTree.calcY(s); // TODO <-- this doesn't belong here!
  
      for (int i=accConstraints.size()-1 ; i>=0 ; i--)
          accConstraints[i].calcConstraintForces(s,udotErr,multipliers,ac);
  
      return true;
  }
  
  void LengthConstraints::addInCorrectionForces(const State& s, const SBAccelerationCache& ac,
                                                SpatialVecList& spatialForces) const 
  {
      for (int i=accConstraints.size()-1 ; i>=0 ; i--)
          accConstraints[i].addInCorrectionForces(s, ac, spatialForces);
  }
  
  void 
  LengthConstraints::projectUVecOntoMotionConstraints(const State& s, Vector& vec)
  {
      if ( pvConstraints.size() == 0 )
          return;
  
      for (int i=pvConstraints.size()-1 ; i>=0 ; i--)
          pvConstraints[i].projectUVecOntoMotionConstraints(s, vec);
  
      for (int i=pvConstraints.size()-1 ; i>=0 ; i--) 
          pvConstraints[i].testProjectedVec(s, vec);
  }
  
  //
  // Calculate the acceleration of atom assuming that the spatial acceleration
  // of the body (node) it's on is available.
  //
  //static Vec3
  //getAccel(const IVMAtom* a)
  //{
  //    const RigidBodyNode* n = a->node;
  //    Vec3 ret( SubVec6(n->getSpatialAcc(),3,3).vector() );
  //    ret += cross( SubVec6(n->getSpatialAcc(),0,3).vector() , a->pos - n->getAtom(0)->pos );
  //    ret += cross( SubVec6(n->getSpatialVel(),0,3).vector() , a->vel - n->getAtom(0)->vel );
  //    return ret;
  //}
  
  //             T     -1 T   
  // Compute   v1 *(J M  J )_mn * v2   where the indices mn are given by 
  // the nodes associated with stations s1 & s2.
  //
  static Real
  computeA(const SBPositionCache& cc, 
           const SBDynamicsCache&      dc,
           const Vec3&    v1,
           const LoopWNodes& loop1, int s1,
           const LoopWNodes& loop2, int s2,
           const Vec3&    v2)
  {
      const RigidBodyNode* n1 = &loop1.tips(s1).getNode();
      const RigidBodyNode* n2 = &loop2.tips(s2).getNode();
  
      const Mat33 one(1);
  
      SpatialRow t1 = ~v1 * Row<2,Mat33>(crossMat(n1->getX_GB(cc).p() - loop1.tipPos(cc,s1)), one);
      SpatialVec t2 = Vec<2,Mat33>(crossMat(loop2.tipPos(cc,s2) - n2->getX_GB(cc).p()), one) * v2;
  
      while ( n1->getLevel() > n2->getLevel() ) {
          t1 = t1 * ~n1->getPsi(dc);
          n1 = n1->getParent();
      }
  
      while ( n2->getLevel() > n1->getLevel() ) {
          t2 = n2->getPsi(dc) * t2;
          n2 = n2->getParent();
      }
  
      while ( n1 != n2 ) {
          if (n1->isGroundNode() || n2->isGroundNode()  ) {
              t1 = 0.;  //not in same branch (or same tree -- sherm)
              t2 = 0.;
              cout << "computeA: cycles wasted calculating missed branch: "
                      << loop1.tips(s1) << " <-> " << loop2.tips(s2) << '\n';
              return 0.;
          }
          t1 = t1 * ~n1->getPsi(dc);
          t2 = n2->getPsi(dc) * t2;
          n1 = n1->getParent();
          n2 = n2->getParent();
      }
  
      // here n1==n2
  
      Real ret = t1 * n1->getY(dc) * t2;
      return ret;
  }
  
  //
  // To be called for LengthSets consecutively from tip to base.
  // Given acceleration errors for each loop contained in this LengthSet,
  // this will calculate a force for every station, and store that force
  // in the runtime block associated with that loop in the AccelerationCache
  // output argument. It is up to the caller to do something with these forces.
  //
  // See Section 2.6 on p. 294 of Schwieters & Clore, 
  // J. Magnetic Resonance 152:288-302. Equation reference below
  // are to that paper. (sherm)
  //
  void
  LengthSet::calcConstraintForces(const State& s, const Vector& udotErr,
                                  Vector& multipliers, SBAccelerationCache& ac) const
  { 
      const SBPositionCache& pc      = getRBTree().getPositionCache(s);
      const SBVelocityCache& vc      = getRBTree().getVelocityCache(s);
      const SBDynamicsCache& dc      = getRBTree().getDynamicsCache(s);
  
      // This is the acceleration error for each loop constraint in this
      // LengthSet. We get a single scalar error per loop, since each
      // contains one distance constraint.
      // See Eq. [53] and the last term of Eq. [66].
      Vector rhs(loops.size());
      for (int i=0; i<(int)loops.size(); ++i)
          rhs[i] = loops[i].rbDistCons->getAccErr(udotErr);
  
      // Here A = Q*(J inv(M) J')*Q' where J is the kinematic Jacobian for
      // the constrained points and Q is the constraint Jacobian. See first 
      // term of Eq. [66].
      Matrix A(loops.size(),loops.size(),0.);
      for (int i=0 ; i<(int)loops.size() ; i++) {
          const Vec3 v1 = loops[i].tipPos(pc,2) - loops[i].tipPos(pc,1);
          for (int bi=1 ; bi<=2 ; bi++)
              for (int bj=1 ; bj<=2 ; bj++) {
                  for (int j=i ; j<(int)loops.size() ; j++) {
                      const Vec3 v2 = loops[j].tipPos(pc,2) - loops[j].tipPos(pc,1);
                      Real  contrib = computeA(pc, dc, v1, loops[i], bi,
                                                       loops[j], bj, v2);
                      A(i,j) += contrib * (bi==bj ? 1 : -1);
                  }
              }
      }
  
      // (sherm) Ouch -- this part is very crude. If this is known to be well 
      // conditioned it should be factored with a symmetric method
      // like Cholesky, otherwise use an SVD (or better, complete orthogonal
      // factorization QTZ) to get appropriate least squares solution.
  
      for (int i=0 ; i<(int)loops.size() ; i++)   //fill lower triangle
          for (int j=0 ; j<i ; j++)
              A(i,j) = A(j,i);
  
      //cout << "Solve A lambda=rhs; A=" << A; 
      //cout << "  rhs = " << rhs << endl;
  
      //FIX: using inverse is inefficient
      const Vector multipliersForThisSet = A.invert() * rhs;
  
      //TODO: need to copy out the right multipliers to the full multipliers array
  
      cout << "  OLD lambda = " << multipliersForThisSet << endl;
  
      // add forces due to these constraints
      for (int i=0 ; i<(int)loops.size() ; i++) {
          const Vec3 frc = multipliersForThisSet[i] * (loops[i].tipPos(pc,2) - loops[i].tipPos(pc,1));
          loops[i].setTipForce(ac, 2, -frc);
          loops[i].setTipForce(ac, 1,  frc);
      }
  }
  
  void LengthSet::addInCorrectionForces(const State& s, const SBAccelerationCache& ac,
                                        SpatialVecList& spatialForces) const 
  {
      const SBPositionCache& pc = getRBTree().getPositionCache(s);
  
      for (int i=0; i<(int)loops.size(); ++i) {
          for (int t=1; t<=2; ++t) {
              const RigidBodyNode& node = loops[i].tipNode(t);
              const Vec3 force = loops[i].tipForce(ac,t);
              const Vec3 moment = cross(loops[i].tipPos(pc,t) - node.getX_GB(pc).p(), force);
              spatialForces[node.getNodeNum()] += SpatialVec(moment, force);
          }
      }
  }
  
  void LengthSet::testAccel(const State& s) const
  {
      const Vector& udotErr = getRBTree().getUDotErr(s);
  
      double testTol=1e-8;
      for (int i=0 ; i<(int)loops.size() ; i++) {
          const Real aerr = fabs(loops[i].rbDistCons->getAccErr(udotErr));
          if (aerr > testTol)
              cout << "LengthSet::testAccel: constraint condition between atoms "
                   << loops[i].tips(1) << " and " << loops[i].tips(2) << " violated.\n"
                   << "\tnorm of violation: " << aerr << '\n';
      }
      cout.flush();
  }
  
  
  // This just computes ~mat*vec but first plucks out the relevant entries
  // in vec to squash it down to the same size as mat.
  Vector 
  LengthSet::packedMatTransposeTimesVec(const Matrix& packedMat, const Vector& vec)
  {
      // sherm 060222: 
      assert(vec.size() == getRBTree().getTotalDOF());
      assert(packedMat.nrow() == ndofThisSet && packedMat.ncol() == ndofThisSet);
  
      Vector packedVec(ndofThisSet);    //build vector same size as mat
      int indx=0;
      for (int j=0 ; j<(int)nodeMap.size() ; j++) {
          const int d    = nodeMap[j]->getDOF();
          const int offs = nodeMap[j]->getUIndex();
          packedVec(indx,d) = vec(offs,d);
          indx += d;
      }
      // sherm 060222: 
      assert(indx == ndofThisSet);
  
      return ~packedMat * packedVec; 
  }
  
  // vec is a full vector in mobility space; packedVec consists of just
  // the mobilities used in this LengthSet.
  void
  LengthSet::subtractPackedVecFromVec(Vector& vec,
                                      const Vector& packedVec)
  {
      // sherm 060222:
      assert(vec.size() == getRBTree().getTotalDOF());
      assert(packedVec.size() == ndofThisSet);
  
      int indx=0;
      for (int j=0 ; j<(int)nodeMap.size() ; j++) {
          const int d    = nodeMap[j]->getDOF();
          const int offs = nodeMap[j]->getUIndex();
          vec(offs,d) -= packedVec(indx,d);
          indx += d;
      }
  
      assert(indx == ndofThisSet);
  }
  
  void
  LengthSet::testProjectedVec(const State& s, 
                              const Vector& vec) const
  {
      // sherm 060222:
      assert(vec.size() == getRBTree().getTotalDOF());
  
      Vector packedVec(ndofThisSet);
      int indx=0;
      for (int j=0 ; j<(int)nodeMap.size() ; j++) {
          const int d    = nodeMap[j]->getDOF();
          const int offs = nodeMap[j]->getUIndex();
          packedVec(indx,d) = vec(offs,d);
          indx += d;
      }
      assert(indx == ndofThisSet);
  
      const Matrix grad = calcGrad(s);
      const Vector test = ~grad * packedVec;
  
      double testTol=1e-8;
      for (int i=0 ; i<(int)loops.size() ; i++) {
          if ( test(i) > testTol )
              cout << "LengthSet::Gradient: constraint condition between atoms "
                   << loops[i].tips(1) << " and " << loops[i].tips(2) << " violated.\n"
                   << "\tnorm of violation: " << test(i) << '\n';
      }
      cout.flush();
  }
  
  void
  LengthConstraints::fixVel0(State& s, Vector& iVel)
  {
      // use molecule grouping
      for (int i=0 ; i<(int)accConstraints.size() ; i++)
          accConstraints[i].fixVel0(s, iVel);
  }
  
  //
  // Correct internal velocities such that the constraint conditions are
  // met under the condition that the disturbance to station velocites is
  // small as possible.
  //
  void
  LengthSet::fixVel0(State& s, Vector& iVel)
  {
      assert(iVel.size() == getRBTree().getTotalDOF());
  
      const SBPositionCache& pc = getRBTree().getPositionCache(s);
      const Vector&          uErr = getRBTree().getUErr(s);
  
      // store internal velocities
      Vector iVel0 = iVel;
  
      // Allocate the vector of body impulses so we don't have to do it
      // inside the loop below. No need to initialize them here, though.
      Vector_<SpatialVec> bodyImpulses(getRBTree().getNumBodies());
  
      //TODO
      // Currently we do not have any constraints involving mobilities (u's)
      // directly, so we don't generate mobility impulses here. But we'll need
      // a set of zero mobility forces to apply below.
      Vector zeroMobilityImpulses(getRBTree().getTotalDOF());
      zeroMobilityImpulses.setToZero();
  
      // verr stores the current velocity errors, which we're assuming are valid.
      Vector verr((int)loops.size());
      for (int i=0; i<(int)loops.size(); ++i)
          verr[i] = loops[i].rbDistCons->getVelErr(uErr);
  
      Matrix mat(loops.size(),loops.size());
      Array_<Vector> deltaIVel(loops.size());
      for (int m=0 ; m<(int)loops.size() ; m++) {
          deltaIVel[m].resize(iVel.size());
  
          // Set all velocities to zero. TODO: this should just be an "ignore velocity"
          // option to realizeVelocity(); it shouldn't actually require putting zeroes everywhere.
          iVel = 0.;
          getRBTree().setU(s, iVel );
          getRBTree().realizeSubsystemVelocity(s);
  
          // sherm: I think the following is a unit "probe" velocity, projected
          // along the separation vector. 
          // That would explain the fact that there are no velocities here!
          const Vec3 probeImpulse = loops[m].tipPos(pc,2)-loops[m].tipPos(pc,1);
          const Vec3 force1 = -probeImpulse;
          const Vec3 force2 =  probeImpulse;
  
          const RigidBodyNode& node1 = loops[m].tipNode(1);
          const RigidBodyNode& node2 = loops[m].tipNode(2);
          const Vec3 moment1 = cross(loops[m].tipPos(pc,1)-node1.getX_GB(pc).p(), force1);
          const Vec3 moment2 = cross(loops[m].tipPos(pc,2)-node2.getX_GB(pc).p(), force2);
  
          // Convert the probe impulses at the stations to spatial impulses at
          // the body origin.
          bodyImpulses.setToZero();
          bodyImpulses[node1.getNodeNum()] += SpatialVec(moment1, force1);
          bodyImpulses[node2.getNodeNum()] += SpatialVec(moment2, force2);
  
          // Apply probe impulse as though it were a force; the resulting "acceleration"
          // is actually the deltaV produced by this impulse, that is, a deltaV which
          // results in a change in velocity along the line between the stations.
  
          // calc deltaVa from k-th constraint condition
          //FIX: make sure that nodeMap is ordered by level
          // get all nodes in given molecule, ordered by level
          getRBTree().realizeZ(s, zeroMobilityImpulses, bodyImpulses);
          getRBTree().realizeTreeAccel(s);
          deltaIVel[m] = getRBTree().getUDot(s);
  
          // Set the new velocities.
          getRBTree().setU(s, deltaIVel[m] );
          getRBTree().realizeSubsystemVelocity(s);
  
          // Calculating partial(velocityError[n])/partial(deltav[m]). Any velocity
          // we see here is due to the deltav, since we started out at zero (uErr
          // was modified by realizeVelocity(), but our reference is still valid).
          for (int n=0; n<(int)loops.size(); ++n)
              mat(n,m) = loops[n].rbDistCons->getVelErr(uErr);
  
          //store results of m-th constraint on deltaVa-n
      }
  
      // Calculate the fraction of each deltaV by which we should change V to simultaneously
      // drive all the velocity errors to zero.
      // TODO: deal with a badly conditioned Jacobian to produce a least squares lambda. 
      const Vector lambda = mat.invert() * verr;
  
      iVel = iVel0;
      for (int m=0 ; m<(int)loops.size() ; m++)
          iVel -= lambda[m] * deltaIVel[m];
      getRBTree().setU(s, iVel);
      getRBTree().realizeSubsystemVelocity(s);
  }
  
  
  std::ostream& operator<<(std::ostream& o, const RBStation& s) {
      o << "station " << s.getPoint() << " on node " << s.getNode().getNodeNum();
      return o;
  }
  
  
  std::ostream& operator<<(std::ostream& o, const RBDirection& d) {
      o << "normal " << d.getUnitVec() << " on node " << d.getNode().getNodeNum();
      return o;
  }
  
      ////////////////////////////
      // RB DISTANCE CONSTRAINT //
      ////////////////////////////
  
  void RBDistanceConstraint::calcStationPosInfo(int i, 
          SBPositionCache& pc) const
  {
      updStation_G(pc,i) = getNode(i).getX_GB(pc).R() * getPoint(i);
      updPos_G(pc,i)     = getNode(i).getX_GB(pc).p() + getStation_G(pc,i);
  }
  
  void RBDistanceConstraint::calcStationVelInfo(int i, 
          const SBPositionCache& pc, 
          SBVelocityCache&       vc) const
  {
      const Vec3& w_G = getNode(i).getSpatialAngVel(vc);
      const Vec3& v_G = getNode(i).getSpatialLinVel(vc);
      updStationVel_G(vc,i) = cross(w_G, getStation_G(pc,i));
      updVel_G(vc,i)        = v_G + getStationVel_G(vc,i);
  }
  
  void RBDistanceConstraint::calcStationAccInfo(int i, 
          const SBPositionCache& pc, 
          const SBVelocityCache& vc,
          SBAccelerationCache&   ac) const
  {
      const Vec3& w_G  = getNode(i).getSpatialAngVel(vc);
      const Vec3& v_G  = getNode(i).getSpatialLinVel(vc);
      const Vec3& aa_G = getNode(i).getSpatialAngAcc(ac);
      const Vec3& a_G  = getNode(i).getSpatialLinAcc(ac);
      updAcc_G(ac,i) = a_G + cross(aa_G, getStation_G(pc,i))
                           + cross(w_G,  getStationVel_G(vc,i)); // i.e., w X (wXr)
  }
  
  
  void RBDistanceConstraint::calcPosInfo(Vector& qErr, SBPositionCache& pc) const
  {
      assert(isValid() && distConstNum >= 0);
      for (int i=1; i<=2; ++i) calcStationPosInfo(i,pc);
  
      const Vec3 p = getPos_G(pc,2) - getPos_G(pc,1);
      updFromTip1ToTip2_G(pc)  = p;
      const Real separation  = getFromTip1ToTip2_G(pc).norm();
      updUnitDirection_G(pc)   = getFromTip1ToTip2_G(pc) / separation;
      //TODO:  |p|-d (should be 0.5(p^2-d^2)
      //updPosErr(cc) = separation - distance; 
      updPosErr(qErr) = 0.5*(p.normSqr() - distance*distance);
  
  }
  
  void RBDistanceConstraint::calcVelInfo(
          const SBPositionCache& pc, 
          Vector&                uErr,
          SBVelocityCache&       vc) const
  {
      assert(isValid() && distConstNum >= 0);
      for (int i=1; i<=2; ++i) calcStationVelInfo(i,pc,vc);
  
      updRelVel_G(vc) = getVel_G(vc,2) - getVel_G(vc,1);
      //TODO: u.v, u=p/|p| (should be p.v)
      //updVelErr(mc)   = ~getUnitDirection_G(pc) * getRelVel_G(vc);
      updVelErr(uErr) = ~getFromTip1ToTip2_G(pc) * getRelVel_G(vc);
  }
  
  void RBDistanceConstraint::calcAccInfo(
          const SBPositionCache& pc, 
          const SBVelocityCache& vc,
          Vector&                udotErr,
          SBAccelerationCache&   ac) const
  {
      assert(isValid() && distConstNum >= 0);
      for (int i=1; i<=2; ++i) calcStationAccInfo(i,pc,vc,ac);
  
      // TODO: v.v + a.p (good), but would have to be
      // u.a + (v-(v.u).u).v/|p| to be compatible with above
      const Vec3 relAcc_G = getAcc_G(ac,2) - getAcc_G(ac,1);
      updAccErr(udotErr) = getRelVel_G(vc).normSqr() + (~relAcc_G * getFromTip1ToTip2_G(pc));
  }
  
  
  #endif
  
  