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

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

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

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