C++ Vector::size方法代码示例

/**
  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;
}

/** 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);
}

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());
}

/**
 * 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;
}

//==============================================================================
// 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;
}

/*
            Comp        Div     Mult    Additions   Assignments
calcIdx     3*3+2=11                    1*3=3       3
calcU       15          2       82      42          60
calcQuinticBezierCurveVal
                                21      20          13
Total       26          2       103     65          76
\endverbatim

Ignoring the costs associated with the integrator itself, and assuming
that the integrator evaluates the function 6 times per integrated point,
the cost of evaluating the integral at each point in vX is:

\verbatim
                Comp        Div     Mult    Additions      Assignments
RK45 on 1pt  6*(26          2       103      65              76)
Total           156         12      618      390            456
\endverbatim

Typically the integral is evaluated 100 times per section in order to 
build an accurate spline-fit of the integrated function. Once again,
ignoring the overhead of the integrator, the function evaluations alone
for the current example would be

\verbatim
RK45 on 100pts per section, over 3 sections
            Comp        Div     Mult        Additions      Assignments        
        3*100*(156         12      618         390            456
Total       46,800      3600    185,400     117,000        136,000

*/
SimTK::Matrix SegmentedQuinticBezierToolkit::calcNumIntBezierYfcnX(
                            const SimTK::Vector& vX, 
                            double ic0, double intAcc, 
                            double uTol, int uMaxIter,
                            const SimTK::Matrix& mX, const SimTK::Matrix& mY,
                            const SimTK::Array_<SimTK::Spline>& aSplineUX,
                            bool flag_intLeftToRight,
                            const std::string& caller)
{
    SimTK::Matrix intXY(vX.size(),2);
    BezierData bdata;
        bdata._mX             = mX;
        bdata._mY             = mY;
        bdata._initalValue    = ic0;
        bdata._aArraySplineUX = aSplineUX;
        bdata._uMaxIter       = uMaxIter;
        bdata._uTol           = uTol;
        bdata._flag_intLeftToRight = flag_intLeftToRight;
        bdata._name           = caller;

    //These aren't really times, but I'm perpetuating the SimTK language
    //so that I don't make a mistake
    double startTime = vX(0);
    double endTime   = vX(vX.size()-1);
    
    if(flag_intLeftToRight){
        bdata._startValue = startTime;
    }else{
        bdata._startValue = endTime;
    }

    MySystem sys(bdata);
    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 < vX.nelt()) {      
        if(idx < vX.nelt()){
            if(flag_intLeftToRight){
                nextTimeInterval = vX(idx);
            }else{
                nextTimeInterval = endTime-vX(vX.size()-idx-1);
            }
        }
        status=integ.stepTo(nextTimeInterval);

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

        if (status == Integrator::EndOfSimulation)
            break;
        
        const State& state = integ.getState();

        if(flag_intLeftToRight){
            intXY(idx,0) = nextTimeInterval;
            intXY(idx,1) = (double)state.getZ()[0];                        
        }else{
            intXY(vX.size()-idx-1,0) = vX(vX.size()-idx-1);
//.........这里部分代码省略.........

/* 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;
            }
//.........这里部分代码省略.........

/*
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
//.........这里部分代码省略.........

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

/**
  @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 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;
//.........这里部分代码省略.........

//=============================================================================
// 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);
        }
    }
}

//=============================================================================
// UTILITY
//=============================================================================
//_____________________________________________________________________________
bool MuscleOptimizer::processModel(Model* inputModel, Model* referenceModel, const std::string& aPathToSubject)
{

    if (!get_apply()) return false;

    try
    {
        OpenSim::Array<std::string> musclesInput, musclesReference;
        referenceModel->getMuscles().getNames(musclesReference);
        inputModel->getMuscles().getNames(musclesInput);

        int nEnabledMuscles = 0;
        // Check that all enabled input model's muscles can be found in the reference model
        for (int im = 0; im < musclesInput.getSize(); ++im)
        {
            if (isEnabledMuscle(musclesInput[im]))
            {
                ++nEnabledMuscles; // also, count the total number of enabled muscles, so that we can provide this info to the user to check progress
                if (musclesReference.findIndex(musclesInput[im]) < 0)
                {
                    cout << "Muscle optimizer: ERROR- Muscle " << musclesInput[im] << " could not be found in reference model! Aborting." << std::endl;
                    return false;
                }
            }
        }

        int curMuscleOrdinal = 0;
        for (int im = 0; im < musclesInput.getSize(); ++im) {
            std::string currentMuscleName = musclesInput[im];
            if (isEnabledMuscle(currentMuscleName))
            {
                std::cout << "Optimizing muscle " << ++curMuscleOrdinal << "/" << nEnabledMuscles << ": " << currentMuscleName << ";";
                // Reset models' poses
                SimTK::State& referenceInitialState = referenceModel->initSystem();
                SimTK::State& inputInitialState = inputModel->initSystem();

                MuscleOptimizer::CoordinateCombinations coordCombinations = sampleROMsForMuscle(*inputModel, inputInitialState, currentMuscleName, get_n_evaluation_points());
                if (coordCombinations.size()>0)
                {
                    std::cout << "using coordinates [ ";
                    for (auto& cit : coordCombinations)
                        std::cout << cit.first << " ";
                    std::cout << "], total no. of combinations " << coordCombinations.at(0).second.size() << endl;
                }
                else
                {
                    std::cout << "   No coordinates for " << currentMuscleName << ", skipping optimization" << std::endl;
                    continue;
                }
                if (coordCombinations.size() > 0 && coordCombinations.at(0).second.size() < get_n_evaluation_points() / 2) //just a check that the sampling did not fail
                    std::cout << "WARNING! no. of coordinate combinations is less than half the number of eval points" << endl;
                std::vector<TemplateMuscleInfo> templateQuantities = sampleTemplateQuantities(*referenceModel, referenceInitialState, currentMuscleName, coordCombinations);


                SimTK::Vector targetMTUlength = sampleMTULength(*inputModel, inputInitialState, currentMuscleName, coordCombinations);

                if (targetMTUlength.size() != templateQuantities.size()) {
                    std::cout << "   Could not sample target MTU lengths on the same poses as reference model" << std::endl;
                    std::cout << "   There might be some inconsistencies between joints/coordinates definitions in the two models" << std::endl;
                    continue;
                }

                SimTK::Matrix A(templateQuantities.size(), 2);
                for (size_t i = 0; i < templateQuantities.size(); ++i)
                {
                    A(i, 0) = templateQuantities.at(i).normalizedFiberLength*cos(templateQuantities.at(i).pennationAngle);
                    A(i, 1) = templateQuantities.at(i).normalizedTendonLength;
                }

                SimTK::Vector x;
                SimTK::FactorQTZ qtz(A);
                qtz.solve(targetMTUlength, x);


                if (x[0] <= 0 || x[1] <= 0)
                {
                    if (x[0] <= 0)
                        std::cout << " Negative estimate for optimal fiber length of muscle " << currentMuscleName << std::endl;

                    if (x[1] <= 0)
                        std::cout << " Negative estimate for tendon slack length of muscle " << currentMuscleName << std::endl;

                    if (max(A.col(1)) - min(A.col(1)) < 0.0001)
                        std::cout << " Tendon length not changing throughout range of motion" << std::endl;

                    SimTK::Vector templateMTUlength = sampleMTULength(*referenceModel, referenceInitialState, currentMuscleName, coordCombinations);

                    SimTK::Vector Lfib_targ(targetMTUlength.size());
                    for (int i = 0; i < Lfib_targ.size(); ++i)
                    {
                        double Lten_fraction = A(i, 1)*referenceModel->getMuscles().get(currentMuscleName).getTendonSlackLength() / templateMTUlength(i);
                        Lfib_targ(i) = (1 - Lten_fraction)*targetMTUlength(i);
                    }
                    std::cout << " Fallback: optimize optimal fiber length assuming same proportion between fiber and tendon as in reference muscle" << std::endl;
                    // first round - estimate Lopt, assuming that the same proportion between fiber and tendon in kept as in reference muscle
                    SimTK::Matrix A_1(A.col(0));
//.........这里部分代码省略.........