/**
 * initialize storages and analyses 
 * 
 * @param s system state before integration
 */
void Manager::initialize(SimTK::State& s, double dt )
{
    // skip initializations for CMC's actuator system
    if( _writeToStorage && _performAnalyses ) { 

        double tReal = s.getTime();
    
        // STORE STARTING CONTROLS
        if (_model->isControlled()){
            _controllerSet->setModel(*_model);
            _controllerSet->storeControls(s, 0);
        }

        // STORE STARTING STATES
        if(hasStateStorage()) {
            // ONLY IF NO STATES WERE PREVIOUSLY STORED
            if(getStateStorage().getSize()==0) {
                SimTK::Vector stateValues = _model->getStateVariableValues(s);
                getStateStorage().store(0,tReal,stateValues.size(), &stateValues[0]);
            }
        }

        // ANALYSES 
        AnalysisSet& analysisSet = _model->updAnalysisSet();
        analysisSet.begin(s);
    }

    return;
}

/** Solve the inverse dynamics system of equations for generalized coordinate forces, Tau. 
    Applied loads are computed by the model from the state.	 */
Vector InverseDynamicsSolver::solve(SimTK::State &s, const SimTK::Vector &udot)
{
	// Default is a statics inverse dynamics analysis, udot = 0;
	Vector knownUdots(getModel().getNumSpeeds(), 0.0);
	
	// If valid accelerations provided then use for inverse dynamics
	if(udot.size() == getModel().getNumSpeeds()){
		knownUdots = udot;
	}
	else if (udot.size() != 0){
		throw Exception("InverseDynamicsSolver::solve with input 'udot' of invalid size.");
	}

	// Realize to dynamics stage so that all model forces are computed
	getModel().getMultibodySystem().realize(s,SimTK::Stage::Dynamics);

	// Get applied mobility (generalized) forces generated by components of the model, like actuators
	const Vector &appliedMobilityForces = getModel().getMultibodySystem().getMobilityForces(s, Stage::Dynamics);
		
	// Get all applied body forces like those from conact
	const Vector_<SpatialVec>& appliedBodyForces = getModel().getMultibodySystem().getRigidBodyForces(s, Stage::Dynamics);
	
	// Perform general inverse dynamics
	return solve(s,	knownUdots, appliedMobilityForces, appliedBodyForces);
}

/**
 * Compute the Joint power.
 */
SimTK::Vector JointInternalPowerProbe::computeProbeInputs(const State& s) const
{
    int nJ = getJointNames().size();
    SimTK::Vector TotalP;

    if (getSumPowersTogether()) {
        TotalP.resize(1);
        TotalP(0) = 0;       // Initialize to zero
    }
    else
        TotalP.resize(nJ);

    // Loop through each joint in the list of joint_names
    for (int i=0; i<nJ; ++i)
    {
        string jointName = getJointNames()[i];
        int k = _model->getJointSet().getIndex(jointName);

        // Get the "Joint" power from the Joint object
        double jointPower = _model->getJointSet().get(k).calcPower(s);
        
        // Append to output vector
        if (getSumPowersTogether())
            TotalP(0) += std::pow(jointPower, getExponent());
        else
            TotalP(i) = std::pow(jointPower, getExponent());
    }

    return TotalP;
}

/**
  This function computes a standard central difference dy/dx. If 
  extrap_endpoints is set to 1, then the derivative at the end points is 
  estimated by linearly extrapolating the dy/dx values beside the end points

  @param x domain vector
  @param y range vector
  @param extrap_endpoints: (false)   Endpoints of the returned vector will be 
  zero, because a central difference
  is undefined at these endpoints
  (true)  Endpoints are computed by linearly 
  extrapolating using a first difference from 
  the neighboring 2 points
  @returns dy/dx computed using central differences
  */
SimTK::Vector calcCentralDifference(const SimTK::Vector& x, 
        const SimTK::Vector& y,                                          
        bool extrap_endpoints){


    SimTK::Vector dy(x.size());
    double dx1,dx2;
    double dy1,dy2;
    int size = x.size();
    for(int i=1; i<x.size()-1; i++){
        dx1 = x(i)-x(i-1);
        dx2 = x(i+1)-x(i);
        dy1 = y(i)-y(i-1);
        dy2 = y(i+1)-y(i);
        dy(i)= 0.5*dy1/dx1 + 0.5*dy2/dx2;
    }

    if(extrap_endpoints == true){
        dy1 = dy(2)-dy(1);
        dx1 = x(2)-x(1);
        dy(0) = dy(1) + (dy1/dx1)*(x(0)-x(1));

        dy2 = dy(size-2)-dy(size-3);
        dx2 = x(size-2)-x(size-3);
        dy(size-1) = dy(size-2) + (dy2/dx2)*(x(size-1)-x(size-2));
    }
    return dy;
}

/*Detailed Computational Costs
                Comparisons     Div     Mult     Additions   Assignments
    splineGuess log(n,2)                2        3           1
    (n currently set to 100)

    Newton Iter
        f                               21       20          13 
        df                              20       19          11
        update  4               1                3           6    
        total   4               1       41       42          30
    \endverbatim

    To evaluate u to SimTK::Eps*100 this typically involves 2 Newton 
    iterations, yielding a total cost of

    \verbatim
                Comparisons     Div     Mult    Additions   Assignments
    eval U      7+8=15          2       82      42          60
*/
double SegmentedQuinticBezierToolkit::calcU(double ax, const SimTK::Vector& bezierPtsX, 
                                 const SimTK::Spline& splineUX, double tol, 
                                 int maxIter)
{
    //Check to make sure that ax is in the curve domain
    double minX = 1e100;
    double maxX = -1e100;
    for(int i=0; i<bezierPtsX.nrow(); i++){
        if(bezierPtsX(i) > maxX)
            maxX = bezierPtsX(i);
        if(bezierPtsX(i) < minX)
            minX = bezierPtsX(i);
    }

    SimTK_ERRCHK_ALWAYS( ax >= minX && ax <= maxX, 
        "SegmentedQuinticBezierToolkit::calcU", 
        "Error: input ax was not in the domain of the Bezier curve specified \n"
        "by the control points in bezierPtsX.");

    double u = splineUX.calcValue(ax);
    u = clampU(u);
    double f = 0;


    f = calcQuinticBezierCurveVal(u,bezierPtsX)-ax;
    double df= 0;
    double du= 0;
    int iter = 0;
    bool pathologic = false;
    
    //Newton iterate to the desired tolerance
    while(abs(f) > tol && iter < maxIter && pathologic == false){
       //Take a Newton step
        df = calcQuinticBezierCurveDerivU(u,bezierPtsX,1);
        if(abs(df) > 0){
            du = -f/df;
            u  = u + du;
            u  = clampU(u); 
            f = calcQuinticBezierCurveVal(u,bezierPtsX)-ax;
        }else{
            pathologic = true;
        }
        iter++;
    }

    //Check for convergence
    SimTK_ERRCHK2_ALWAYS( (f <= tol), 
        "SegmentedQuinticBezierToolkit::calcU", 
        "Error: desired tolerance of %f on U not met by the Newton iteration."
        " A tolerance of %f was reached.",tol, f);

    SimTK_ERRCHK_ALWAYS( (pathologic==false), 
        "SegmentedQuinticBezierToolkit::calcU", 
        "Error: Newton iteration went pathologic: df = 0 to machine precision.");
      
    
    //Return the value
    return u;
}

//==============================================================================
// Common Functions 
//==============================================================================
int initTestStates(SimTK::Vector &qi, SimTK::Vector &ui)
{
    using namespace SimTK;

    Random::Uniform randomAngle(-Pi/4, Pi/4);
    Random::Uniform randomSpeed(-1.0, 1.0);

    // Provide initial states as random angles and speeds for OpenSim and 
    // Simbody models
    for(int i = 0; i<qi.size(); i++)
        qi[i] = randomAngle.getValue();

    for(int i = 0; i<ui.size(); i++)
        ui[i] = randomSpeed.getValue();
    
    return qi.size();
}

/* Calculate the equivalent spatial force, FB_G, acting on the body connected by this joint at 
   its location B, expressed in ground.  */
SimTK::SpatialVec Joint::calcEquivalentSpatialForce(const SimTK::State &s, const SimTK::Vector &mobilityForces) const
{
	// The number of mobilities for the entire system.
	int nm = _model->getMatterSubsystem().getNumMobilities();

	if(nm != mobilityForces.size()){
		throw Exception("Joint::calcEquivalentSpatialForce(): input mobilityForces does not match model's mobilities");
	}

	const SimTK::MobilizedBodyIndex &mbx = getChildBody().getIndex();
	// build a unique list of underlying MobilizedBodies that are involved
	// with this Joint in addition to and not including that of the child body

	std::set<SimTK::MobilizedBodyIndex> mbds;

	const CoordinateSet& coordinateSet = get_CoordinateSet();

	for(int i=0; i<coordinateSet.getSize(); ++i){
		const MobilizedBodyIndex& coordsMbx = coordinateSet[i].getBodyIndex();
		if (coordsMbx != mbx){
			mbds.insert(coordsMbx);
		}
	}
	
	SimTK::SpatialVec FB_G = calcEquivalentSpatialForceForMobilizedBody(s, mbx, mobilityForces);
	SimTK::SpatialVec FBx_G;

	std::set<SimTK::MobilizedBodyIndex>::const_iterator it = mbds.begin();

	const SimTK::MobilizedBody &G = getModel().getMatterSubsystem().getGround();
	const SimTK::MobilizedBody &B = getModel().getMatterSubsystem().getMobilizedBody(mbx);
	SimTK::Vec3 r_BG =
		B.expressVectorInAnotherBodyFrame(s, B.getOutboardFrame(s).p(), G);

	while(it != mbds.end()){
		FBx_G = calcEquivalentSpatialForceForMobilizedBody(s, *it, mobilityForces);

		const SimTK::MobilizedBody &b = 
			getModel().getMatterSubsystem().getMobilizedBody(*it);

		
		SimTK::Vec3 r_bG = 
			b.expressVectorInAnotherBodyFrame(s, b.getOutboardFrame(s).p(), G);

		// Torques add and include term due to offset in forces
		FB_G += FBx_G; // shiftForceFromTo(FBx_G, r_bG, r_BG);
		++it;
	}
	
	return FB_G;
}

/* 
Multiplications     Additions   Assignments
21                  20          13
*/
double  SegmentedQuinticBezierToolkit::
    calcQuinticBezierCurveVal(double u, const SimTK::Vector& pts)
{
    double val = -1;


    SimTK_ERRCHK1_ALWAYS( (u>=0 && u <= 1) , 
        "SegmentedQuinticBezierToolkit::calcQuinticBezierCurveVal", 
        "Error: double argument u must be between 0.0 and 1.0"
        "but %f was entered.",u);

    

    SimTK_ERRCHK_ALWAYS( (pts.size() == 6) , 
        "SegmentedQuinticBezierToolkit::calcQuinticBezierCurveVal", 
        "Error: vector argument pts must have a length of 6.");

    //Compute the Bezier point
    double p0 = pts(0);
    double p1 = pts(1);
    double p2 = pts(2);
    double p3 = pts(3);
    double p4 = pts(4);
    double p5 = pts(5);

    double u5 = 1;
    double u4 = u;
    double u3 = u4*u;
    double u2 = u3*u;
    double u1 = u2*u;
    double u0 = u1*u;

    //See lines 1-6 of MuscleCurveCodeOpt_20120210
    double t2 = u1 * 0.5e1;
    double t3 = u2 * 0.10e2;
    double t4 = u3 * 0.10e2;
    double t5 = u4 * 0.5e1;
    double t9 = u0 * 0.5e1;
    double t10 = u1 * 0.20e2;
    double t11 = u2 * 0.30e2;
    double t15 = u0 * 0.10e2;
    val = p0 * (u0 * (-0.1e1) + t2 - t3 + t4 - t5 + u5 * 0.1e1) 
        + p1 * (t9 - t10 + t11 + u3 * (-0.20e2) + t5) 
        + p2 * (-t15 + u1 * 0.30e2 - t11 + t4) 
        + p3 * (t15 - t10 + t3) 
        + p4 * (-t9 + t2) + p5 * u0 * 0.1e1;


    return val;
}

void compareSimulationStates(SimTK::Vector q_sb, SimTK::Vector u_sb,
    SimTK::Vector q_osim, SimTK::Vector u_osim,
    string errorMessagePrefix = "")
{
    using namespace SimTK;
    
    Vector q_err = q_sb;
    Vector u_err = u_sb;

    int nq = q_osim.size();
    if(q_sb.size() > nq){ // we have an unused quaternion slot in Simbody

        q_sb.dump("Simbody q's:");
        q_osim.dump("OpenSim q's:");
        //This is a hack knowing the free and ball joint tests have the
        // quaternion q's first and that the q's are packed as qqqq or aaa*
        // for a ball and qqqqxyz or aaaxyz* for a free joint
        int quat_ind = ((nq > 6) ? 6 : 3);
        int j = 0;
        for(int i=0; i< q_sb.size(); i++){
            if(i != quat_ind){
                q_err[j] = q_sb[i] - q_osim[j];
                j++;
            }
        }
    }
    else{
        if(nq > q_sb.size()){ // OpenSim using constrains
            q_err[0] = q_sb[0] - q_osim[0];
            q_err[1] = q_sb[1] - q_osim[1];
            u_err[0] = u_sb[0] - u_osim[0];
            u_err[1] = u_sb[1] - u_osim[1];
        }
        else{
            q_err = q_sb - q_osim;
            u_err = u_sb - u_osim;
        }
    }

    //cout<<"\nSimbody - OpenSim:"<<endl;
    //q_err.dump("Diff q's:");
    //u_err.dump("Diff u's:");

    stringstream errorMessage1, errorMessage2;
    errorMessage1 << "testConstraints compareSimulationStates failed q_err.norm = " << q_err.norm();
    errorMessage2 << "testConstraints compareSimulationStates failed u_err.norm = " << u_err.norm();
    ASSERT(q_err.norm() <= 10*integ_accuracy, __FILE__, __LINE__, errorMessagePrefix + errorMessage1.str());
    ASSERT(u_err.norm() <= 20*integ_accuracy, __FILE__, __LINE__, errorMessagePrefix + errorMessage2.str());
}

//=============================================================================
// HELPER
//=============================================================================
void AnalyzeTool::run(SimTK::State& s, Model &aModel, int iInitial, int iFinal, const Storage &aStatesStore, bool aSolveForEquilibrium)
{
    AnalysisSet& analysisSet = aModel.updAnalysisSet();

    for(int i=0;i<analysisSet.getSize();i++) {
        analysisSet.get(i).setStatesStore(aStatesStore);
    }

    // TODO: some sort of filtering or something to make derivatives smoother?
    GCVSplineSet statesSplineSet(5,&aStatesStore);

    // PERFORM THE ANALYSES
    double tPrev=0.0,t=0.0,dt=0.0;
    int ny = s.getNY();
    Array<double> dydt(0.0,ny);
    Array<double> yFromStorage(0.0,ny);

    const Array<string>& labels =  aStatesStore.getColumnLabels();
    int numOpenSimStates = labels.getSize()-1;

    SimTK::Vector stateData;
    stateData.resize(numOpenSimStates);

    for(int i=iInitial;i<=iFinal;i++) {
        tPrev = t;
        aStatesStore.getTime(i,s.updTime()); // time
        t = s.getTime();
        aModel.setAllControllersEnabled(true);

        aStatesStore.getData(i,numOpenSimStates,&stateData[0]); // states
        // Get data into local Vector and assign to State using common utility
        // to handle internal (non-OpenSim) states that may exist
        Array<std::string> stateNames = aStatesStore.getColumnLabels();
        for (int j=0; j<stateData.size(); ++j){
            // storage labels included time at index 0 so +1 to skip
            aModel.setStateVariableValue(s, stateNames[j+1], stateData[j]);
        }
       
        // Adjust configuration to match constraints and other goals
        aModel.assemble(s);

        // equilibrateMuscles before realization as it may affect forces
        if(aSolveForEquilibrium){
            try{// might not be able to equilibrate if model is in
                // a non-physical pose. For example, a pose where the 
                // muscle length is shorter than the tendon slack-length.
                // the muscle will throw an Exception in this case.
                aModel.equilibrateMuscles(s);
            }
            catch (const std::exception& e) {
                cout << "WARNING- AnalyzeTool::run() unable to equilibrate muscles ";
                cout << "at time = " << t <<"." << endl;
                cout << "Reason: " << e.what() << endl;
            }
        }
        // Make sure model is at least ready to provide kinematics
        aModel.getMultibodySystem().realize(s, SimTK::Stage::Velocity);

        if(i==iInitial) {
            analysisSet.begin(s);
        } else if(i==iFinal) {
            analysisSet.end(s);
        // Step
        } else {
            analysisSet.step(s,i);
        }
    }
}

/**
  @param timeV A nx1 time vector to integrate along, which must monotonic 
  and increasing
  @param xM    A nxm matrix of row vectors, each corresponding to the row vector
  that should be applied to yF at time t
  @param yF    A function
  @param ic    The initial condition for the integral
  @param intAcc The accuracy of the integral
  @returns an nx2 matrix, time in column 0, integral of y in column 1
  */
SimTK::Matrix calcFunctionTimeIntegral( 
        const SimTK::Vector& timeV,
        const SimTK::Matrix& xM, 
        const MuscleFirstOrderActivationDynamicModel& yF,
        double ic, 
        int dim, 
        double startTime, 
        double endTime,
        double intAcc)
{
    SimTK::Matrix intXY(timeV.size(),2);

    //Populate FunctionData
    FunctionData fdata(yF);
    fdata.m_ic      = ic;
    fdata.m_intDim  = dim;
    fdata.m_tmpXV      = SimTK::Vector(xM.ncol());
    fdata.m_tmpXV = 0;

    SimTK::Array_< SimTK::Spline_<double> > splinedInput(xM.ncol());

    //Now spline xM over time
    for(int i=0; i<xM.ncol(); i++){        

        splinedInput[i] = SimTK::SplineFitter<Real>::
            fitForSmoothingParameter(1,timeV,xM(i),0).getSpline();
    }
    fdata.m_splinedInput = splinedInput;
    //FunctionData is now completely built


    //Set up system
    //double startTime = timeV(0);
    //double endTime   = timeV(timeV.nelt()-1);   
    MySystem sys(fdata);
    State initState = sys.realizeTopology();
    initState.setTime(startTime);


    RungeKuttaMersonIntegrator integ(sys);
    integ.setAccuracy(intAcc);
    integ.setFinalTime(endTime);
    integ.setReturnEveryInternalStep(false);
    integ.initialize(initState);

    int idx = 0;    
    double nextTimeInterval = 0;
    Integrator::SuccessfulStepStatus status;

    while (idx < timeV.nelt()) {      
        nextTimeInterval = timeV(idx);

        status=integ.stepTo(nextTimeInterval);

        // Use this for variable step output.
        //status = integ.stepTo(Infinity);

        if (status == Integrator::EndOfSimulation)
            break;

        const State& state = integ.getState();

        intXY(idx,0) = nextTimeInterval;
        intXY(idx,1) = (double)state.getZ()[0];                        

        idx++;

    }

    //cout << "Integrated Solution"<<endl;
    //cout << intXY << endl;


    //intXY.resizeKeep(idx,2);
    return intXY;
}

/**
  This function tests numerically for continuity of a curve. The test is 
  performed by taking a point on the curve, and then two points (called the 
  shoulder points) to the left and right of the point in question. The value
  of the functions derivative is evaluated at each of the shoulder points and
  used to linearly extrapolate from the shoulder points back to the original 
  point. If the original point and the linear extrapolations of each of the 
  shoulder points agree within tol, then the curve is assumed to be 
  continuous.


  @param xV       Values to test for continuity, note that the first and last
  points cannot be tested

  @param yV       Function values at the test points

  @param dydxV    Function derivative values evaluated at xV

  @param d2ydx2V  Function 2nd derivative values (or estimates) evaluated at
  xV

  @param minTol   The minimum error allowed - this prevents the second order
  error term from going to zero

  @param taylorErrorMult  This scales the error tolerance. The default error
  tolerance is the 2nd-order Taylor series
  term.
  */
bool isFunctionContinuous(const SimTK::Vector& xV, const SimTK::Vector& yV,
        const SimTK::Vector& dydxV, const SimTK::Vector& d2ydx2V, 
        double minTol, double taylorErrorMult)
{
    bool flag_continuous = true;
    //double contErr = 0;

    double xL = 0;      // left shoulder point
    double xR = 0;      // right shoulder point
    double yL = 0;      // left shoulder point function value
    double yR = 0;      // right shoulder point function value
    double dydxL = 0;   // left shoulder point derivative value
    double dydxR = 0;   // right shoulder point derivative value

    double xVal = 0;    //x value to test
    double yVal = 0;    //Y(x) value to test

    double yValEL = 0;  //Extrapolation to yVal from the left
    double yValER = 0;  //Extrapolation to yVal from the right

    double errL = 0;
    double errR = 0;

    double errLMX = 0;
    double errRMX = 0;


    for(int i =1; i < xV.nelt()-1; i++){
        xVal = xV(i);
        yVal = yV(i);

        xL = xV(i-1);
        xR = xV(i+1);

        yL = yV(i-1);
        yR = yV(i+1);

        dydxL = dydxV(i-1);
        dydxR = dydxV(i+1);


        yValEL = yL + dydxL*(xVal-xL);
        yValER = yR - dydxR*(xR-xVal);

        errL = abs(yValEL-yVal);
        errR = abs(yValER-yVal);

        errLMX = abs(d2ydx2V(i-1)*0.5*(xVal-xL)*(xVal-xL));
        errRMX = abs(d2ydx2V(i+1)*0.5*(xR-xVal)*(xR-xVal));

        errLMX*=taylorErrorMult;
        errRMX*=taylorErrorMult;

        if(errLMX < minTol)
            errLMX = minTol;

        if(errRMX < minTol)
            errRMX = minTol; // to accommodate numerical
        //error in errL

        if(errL > errLMX || errR > errRMX){
            /*if(errL > errR){
              if(errL > contErr)
              contErr = errL;

              }else{
              if(errR > contErr)
              contErr = errR;
              }*/

            flag_continuous = false;
        }
//.........这里部分代码省略.........

/* Calculate the equivalent spatial force, FB_G, acting on a mobilized body specified 
   by index acting at its mobilizer frame B, expressed in ground.  */
SimTK::SpatialVec Joint::calcEquivalentSpatialForceForMobilizedBody(const SimTK::State &s, 
	const SimTK::MobilizedBodyIndex mbx, const SimTK::Vector &mobilityForces) const
{
	// Get the mobilized body
	const SimTK::MobilizedBody mbd    = getModel().getMatterSubsystem().getMobilizedBody(mbx);
	const SimTK::UIndex        ustart = mbd.getFirstUIndex(s);
	const int                  nu     = mbd.getNumU(s);

	const SimTK::MobilizedBody ground = getModel().getMatterSubsystem().getGround();

    if (nu == 0) // No mobility forces (weld joint?).
        return SimTK::SpatialVec(SimTK::Vec3(0), SimTK::Vec3(0));

	// Construct the H (joint jacobian, joint transition) matrrix for this mobilizer
	SimTK::Matrix transposeH_PB_w(nu, 3);
	SimTK::Matrix transposeH_PB_v(nu, 3);
	// from individual columns
	SimTK::SpatialVec Hcol;
	
	// To obtain the joint Jacobian, H_PB (H_FM in Simbody) need to be realized to at least position
	_model->getMultibodySystem().realize(s, SimTK::Stage::Position);

	SimTK::Vector f(nu, 0.0);
	for(int i =0; i<nu; ++i){
		f[i] = mobilityForces[ustart + i];
		// Get the H matrix for this Joint by constructing it from the operator H*u
		Hcol = mbd.getH_FMCol(s, SimTK::MobilizerUIndex(i));
		const SimTK::Vector hcolw(Hcol[0]);
		const SimTK::Vector hcolv(Hcol[1]);

		transposeH_PB_w[i] = ~hcolw;
		transposeH_PB_v[i] = ~hcolv;
	}

	// Spatial force and torque vectors
	SimTK::Vector Fv(3, 0.0), Fw(3, 0.0);

	// Solve the pseudoinverse problem of Fv = pinv(~H_PB_G_v)*f;
	SimTK::FactorQTZ pinvForce(transposeH_PB_v);

	//if rank = 0, body force cannot contribute to the mobility force
	if(pinvForce.getRank() > 0)
		pinvForce.solve(f, Fv);
	
	// Now solve the pseudoinverse for torque for any unaccounted f: Fw = pinv(~H_PB_G_w)*(f - ~H_PB_G_v*Fv);
	SimTK::FactorQTZ pinvTorq(transposeH_PB_w);

	//if rank = 0, body torque cannot contribute to the mobility force
	if(pinvTorq.getRank() > 0)
		pinvTorq.solve(f, Fw);
	
	// Now we have two solution with either the body force Fv or body torque accounting for some or all of f
	SimTK::Vector fv =  transposeH_PB_v*Fv;
	SimTK::Vector fw =  transposeH_PB_w*Fw; 

	// which to choose? Choose the more effective as fx.norm/Fx.norm
	if(fv.norm() > SimTK::SignificantReal){ // if body force can contributes at all
		// if body torque can contribute too and it is more effective
		if(fw.norm() > SimTK::SignificantReal){
			if (fw.norm()/Fw.norm() > fv.norm()/Fv.norm() ){ 
				// account for f using torque, Fw, so compute Fv with remainder
				pinvForce.solve(f-fw, Fv);		
			}else{
				// account for f using force, Fv, first and Fw from remainder
				pinvTorq.solve(f-fv, Fw);
			}
		}
		// else no torque contribution and Fw should be zero
	}
	// no force contribution but have a torque
	else if(fw.norm() > SimTK::SignificantReal){
		// just Fw
	}
	else{
		// should be the case where gen force is zero.
		assert(f.norm() < SimTK::SignificantReal);
	}

	// The spatial forces above are expresseded in the joint frame of the parent
	// Transform from parent joint frame, P into the parent body frame, Po
	const SimTK::Rotation R_PPo = (mbd.getInboardFrame(s).R());

	// Re-express forces in ground, first by describing force in the parent, Po, 
	// frame instead of joint frame
	SimTK::Vec3 vecFw = R_PPo*SimTK::Vec3::getAs(&Fw[0]);
	SimTK::Vec3 vecFv = R_PPo*SimTK::Vec3::getAs(&Fv[0]);

	//Force Acting on joint frame, B,  in child body expressed in Parent body, Po
	SimTK::SpatialVec FB_Po(vecFw, vecFv);

	const MobilizedBody parent = mbd.getParentMobilizedBody();
	// to apply spatial forces on bodies they must be expressed in ground
	vecFw = parent.expressVectorInAnotherBodyFrame(s, FB_Po[0], ground);
	vecFv = parent.expressVectorInAnotherBodyFrame(s, FB_Po[1], ground);

	// Package resulting torque and force as a spatial vec
	SimTK::SpatialVec FB_G(vecFw, vecFv);

//.........这里部分代码省略.........

/**
* This function tests the accuracy of the natural cubic spline sp. The accuracy of the
* spline is tested in the following manner:
*
*   a.  Spline must pass through the knots given
*       -Error between spline and input data at the knots (should be zero)
*   b.  The first derivatives are continuous at the knot points
*       -Error between the value of the first derivative at the knot point, and
*        what a linear extrapolation would predict just to the left and right of 
*        the knot point. (should be zero, within a tolerance affected by the step size in xD)
*   c.  The second derivatives are continuous at the knots points
*       -Error between the value of the numerically calculated derivative at the knot point, and
*        what a linear extrapolation would predict just to the left and right of 
*        the knot point. (should be zero, within a tolerance affected by the step size in xD)
*   d.  The second derivative is zero at the end points.
*       -Numerically calculated extrapolation of the 2nd derivative should be zero 
*        at the end points within some tolerance
*
*
*
*
*/
SimTK::Vector testNCSpline(SimTK::Function* sp, SimTK::Vector xK, SimTK::Vector yK, SimTK::Vector xM,SimTK::Vector xD,string name, bool print){
    int size = xK.size();
    int sizeD= xD.size();
    int sizeDK = xD.size()/(xK.size()-1);
    double deltaD = (xK(xK.size()-1)-xK(0))/xD.size();

    SimTK::Matrix ysp_K(size,2),ysp_M(size-1,2),ysp_D(sizeD,4);
    SimTK::Vector errVec(4);
    errVec = 1;
    ysp_K = 0;
    ysp_M = 0;
    ysp_D = 0;

    vector<int> derOrder(1);
    derOrder[0] = 0;
    
    

    ///////////////////////////////////////////
    //1. Evaluate the spline at the knots, the mid points and then a dense sample
    ///////////////////////////////////////////
        SimTK::Vector tmpV1(1);
        double xVal=0;
        for(int i=0;i<size;i++){
            xVal = xK(i);
            tmpV1(0)=xK(i);
            ysp_K(i,0) = sp->calcValue(tmpV1);
            ysp_K(i,1) = sp->calcDerivative(derOrder,tmpV1);
        }           
        for(int i=0;i<size-1;i++){
            xVal = xM(i);
            tmpV1(0) = xM(i);
            ysp_M(i,0) = sp->calcValue(tmpV1);
            ysp_M(i,1) = sp->calcDerivative(derOrder,tmpV1);
        }
        for(int i=0;i<sizeD;i++){
            xVal = xD(i);
            tmpV1(0) = xD(i);
            ysp_D(i,0) = sp->calcValue(tmpV1);
            ysp_D(i,1) = sp->calcDerivative(derOrder,tmpV1);
        }

    //////////////////////////////////////
    //2.    Compute the second derivative of the spline (using central differences), and linearly 
    //      interpolate to get the end points. The end points should go to exactly zero because the 
    //      second derivative is linear in a cubic spline, as is the linear extrapolation
    //
    //      Also compute the 3rd derivative using the same method. The 3rd derivative is required in the
    //      test to determine if the second derivative is continuous at the knots or not.
    //////////////////////////////////////

        ysp_D(2)    = getCentralDifference(xD, ysp_D(1),    true);
        ysp_D(3)    = getCentralDifference(xD, ysp_D(2),    true);

    //////////////////////////////////////
    //3. Now check to see if the splines meet the conditions of a natural cubic spline:
    //////////////////////////////////////

        SimTK::Vector tmpK(size,size),tmpM(size-1,size-1);

        //* a.  Spline passes through all knot points given
                    tmpK = yK-ysp_K(0);
                errVec(0) = tmpK.norm();
            
        //  b. The first derivative is continuous at the knot points. Apply a continuity test
        //      to the data points that define the second derivative
        //
        //      Continuity test:    a linear extrapolation of first derivative of the curve in interest
        //                          on either side of the point in interest should equal the point in interest;

            double ykL,ykR,y0L,dydxL,y0R,dydxR = 0;
            for(int i=1; i<size-1; i++){
                    y0L = ysp_D(i*sizeDK-1,1);
                    y0R = ysp_D(i*sizeDK+1,1);
                    dydxL = ysp_D(i*sizeDK-1,2); //Found using central differences
                    dydxR = ysp_D(i*sizeDK+1,2); //Found using central differences
                    ykL = y0L + dydxL*deltaD;
                    ykR = y0R - dydxR*deltaD;
//.........这里部分代码省略.........

/*
Detailed Computational Costs
        dy/dx       Divisions   Multiplications Additions   Assignments
            dy/du               20              19          11
            dx/du               20              19          11
            dy/dx   1        
            total   1           40              38          22

        d2y/dx2     Divisions   Multiplications Additions   Assignments
            dy/du               20              19          11
            dx/du               20              19          11
            d2y/du2             17              17          9
            d2x/du2             17              17          9
            d2y/dx2 2           4               1           3    
            total   2           78              73          23

        d3y/dx3     Divisions   Multiplications Additions   Assignments
            dy/du               20              19          11
            dx/du               20              19          11
            d2y/du2             17              17          9
            d2x/du2             17              17          9
            d3y/du3             14              14          6
            d3x/du3             14              14          6

            d3y/dx3 4           16              5           6
            total   4           118             105         58

        d4y/dx4     Divisions   Multiplications Additions   Assignments
            dy/du               20              19          11
            dx/du               20              19          11
            d2y/du2             17              17          9
            d2x/du2             17              17          9
            d3y/du3             14              14          6
            d3x/du3             14              14          6
            d4y/du4             11              11          3
            d4x/du4             11              11          3

            d4y/dx4 5           44              15          13
            total   5           168             137         71

        d5y/dx5     Divisions   Multiplications Additions   Assignments
            dy/du               20              19          11
            dx/du               20              19          11
            d2y/du2             17              17          9
            d2x/du2             17              17          9
            d3y/du3             14              14          6
            d3x/du3             14              14          6
            d4y/du4             11              11          3
            d4x/du4             11              11          3
            d5y/du5             6               6           1
            d5x/du5             6               6           1 

            d5y/dx5 7           100             36          28
            total   7           236             170         88  

        d6y/dx6
            dy/du               20              19          11
            dx/du               20              19          11
            d2y/du2             17              17          9
            d2x/du2             17              17          9
            d3y/du3             14              14          6
            d3x/du3             14              14          6
            d4y/du4             11              11          3
            d4x/du4             11              11          3
            d5y/du5             6               6           1
            d5x/du5             6               6           1 

            d6y/dx6 9           198             75          46
            total   9           334             209         106

*/
double SegmentedQuinticBezierToolkit::calcQuinticBezierCurveDerivDYDX(double u,
                const SimTK::Vector& xpts, const SimTK::Vector& ypts, int order)
{
    double val = SimTK::NaN;
   
    //Bounds checking on the input
    SimTK_ERRCHK_ALWAYS( (u>=0 && u <= 1) , 
        "SegmentedQuinticBezierToolkit::calcQuinticBezierCurveDerivU", 
        "Error: double argument u must be between 0.0 and 1.0.");

    SimTK_ERRCHK_ALWAYS( (xpts.size()==6) , 
        "SegmentedQuinticBezierToolkit::calcQuinticBezierCurveDerivU", 
        "Error: vector argument xpts \nmust have a length of 6.");

    SimTK_ERRCHK_ALWAYS( (ypts.size()==6) , 
        "SegmentedQuinticBezierToolkit::calcQuinticBezierCurveDerivU", 
        "Error: vector argument ypts \nmust have a length of 6.");

    SimTK_ERRCHK_ALWAYS( (order >= 1),
        "SegmentedQuinticBezierToolkit::calcQuinticBezierCurveDerivU", 
        "Error: order must be greater than.");

    SimTK_ERRCHK_ALWAYS( (order <= 6),
        "SegmentedQuinticBezierToolkit::calcQuinticBezierCurveDerivU", 
        "Error: order must be less than, or equal to 6.");

    //std::string localCaller = caller;
    //localCaller.append(".calcQuinticBezierCurveDerivDYDX");
    //Compute the derivative d^n y/ dx^n
//.........这里部分代码省略.........

/* Computational Cost Details
        Divisions   Multiplications Additions   Assignments
dx/du               20              19          11
d2x/du2             17              17          9
d3y/du3             14              14          6

*/
double SegmentedQuinticBezierToolkit::calcQuinticBezierCurveDerivU(double u,
                 const SimTK::Vector& pts,int order)
{
    double val = -1;

    SimTK_ERRCHK_ALWAYS( (u>=0 && u <= 1) , 
        "SegmentedQuinticBezierToolkit::calcQuinticBezierCurveDerivU", 
        "Error: double argument u must be between 0.0 and 1.0.");

    SimTK_ERRCHK_ALWAYS( (pts.size()==6) , 
        "SegmentedQuinticBezierToolkit::calcQuinticBezierCurveDerivU", 
        "Error: vector argument pts \nmust have a length of 6.");

    SimTK_ERRCHK_ALWAYS( (order >= 1),
        "SegmentedQuinticBezierToolkit::calcQuinticBezierCurveDerivU", 
        "Error: order must be greater than, or equal to 1");

    //Compute the Bezier point
    double p0 = pts(0);
    double p1 = pts(1);
    double p2 = pts(2);
    double p3 = pts(3);
    double p4 = pts(4);
    double p5 = pts(5);

    switch(order){
        case 1: 
            {  
                double t1 = u*u;//u ^ 2;
                double t2 = t1*t1;//t1 ^ 2;
                double t4 = t1 * u;
                double t5 = t4 * 0.20e2;
                double t6 = t1 * 0.30e2;
                double t7 = u * 0.20e2;
                double t10 = t2 * 0.25e2;
                double t11 = t4 * 0.80e2;
                double t12 = t1 * 0.90e2;
                double t16 = t2 * 0.50e2;
                val = p0 * (t2 * (-0.5e1) + t5 - t6 + t7 - 0.5e1) 
                    + p1 * (t10 - t11 + t12 + u * (-0.40e2) + 0.5e1) 
                    + p2 * (-t16 + t4 * 0.120e3 - t12 + t7) 
                    + p3 * (t16 - t11 + t6) 
                    + p4 * (-t10 + t5) 
                    + p5 * t2 * 0.5e1;

            }
            break;
        case 2:    
            {
                double t1 = u*u;//u ^ 2;
                double t2 = t1 * u;
                double t4 = t1 * 0.60e2;
                double t5 = u * 0.60e2;
                double t8 = t2 * 0.100e3;
                double t9 = t1 * 0.240e3;
                double t10 = u * 0.180e3;
                double t13 = t2 * 0.200e3;
                val = p0 * (t2 * (-0.20e2) + t4 - t5 + 0.20e2) 
                    + p1 * (t8 - t9 + t10 - 0.40e2) 
                    + p2 * (-t13 + t1 * 0.360e3 - t10 + 0.20e2) 
                    + p3 * (t13 - t9 + t5) 
                    + p4 * (-t8 + t4)
                    + p5 * t2 * 0.20e2;

            }
            break;      
        case 3:   
            {
                double t1 = u*u;//u ^ 2;
                double t3 = u * 0.120e3;
                double t6 = t1 * 0.300e3;
                double t7 = u * 0.480e3;
                double t10 = t1 * 0.600e3;
                val = p0 * (t1 * (-0.60e2) + t3 - 0.60e2) 
                    + p1 * (t6 - t7 + 0.180e3) 
                    + p2 * (-t10 + u * 0.720e3 - 0.180e3) 
                    + p3 * (t10 - t7 + 0.60e2) 
                    + p4 * (-t6 + t3) 
                    + p5 * t1 * 0.60e2;

            }
            break;
        case 4:       
            {
                double t4 = u * 0.600e3;
                double t7 = u * 0.1200e4;
                val = p0 * (u * (-0.120e3) + 0.120e3) 
                    + p1 * (t4 - 0.480e3) 
                    + p2 * (-t7 + 0.720e3) 
                    + p3 * (t7 - 0.480e3) 
                    + p4 * (-t4 + 0.120e3) 
                    + p5 * u * 0.120e3;
            }
//.........这里部分代码省略.........

 SimTK::Real calcValue(const SimTK::Vector& x) const {
     assert(x.size() == argumentSize);
     return value;
 }