/**
@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);
intXY(vX.size()-idx-1,1) = (double)state.getZ()[0];
}
idx++;
}
//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;
}
}
return flag_continuous;//contErr;//
}
