/*
Cost: n comparisons, for a quintic Bezier curve with n-spline sections
Comp Div Mult Add Assignments
Cost 3*n+2 1*n 3
*/
int SegmentedQuinticBezierToolkit::calcIndex(double x,
const SimTK::Matrix& bezierPtsX)
{
int idx = 0;
bool flag_found = false;
for(int i=0; i<bezierPtsX.ncol(); i++){
if( x >= bezierPtsX(0,i) && x < bezierPtsX(5,i) ){
idx = i;
i = bezierPtsX.ncol();
flag_found = true;
}
}
//Check if the value x is identically the last point
if(flag_found == false && x == bezierPtsX(5,bezierPtsX.ncol()-1)){
idx = bezierPtsX.ncol()-1;
flag_found = true;
}
SimTK_ERRCHK_ALWAYS( (flag_found == true),
"SegmentedQuinticBezierToolkit::calcIndex",
"Error: A value of x was used that is not within the Bezier curve set.");
return idx;
}
/**
This function will print cvs file of the matrix
data
@params data: A matrix of data
@params filename: The name of the file to print
*/
void printMatrixToFile( const SimTK::Matrix& data, const std::string& filename)
{
ofstream datafile;
datafile.open(filename.c_str());
for(int i = 0; i < data.nrow(); i++){
for(int j = 0; j < data.ncol(); j++){
if(j<data.ncol()-1)
datafile << data(i,j) << ",";
else
datafile << data(i,j) << "\n";
}
}
datafile.close();
}
/**
* This function will print cvs file of the column vector col0 and the matrix data
*
* @params col0: A vector that must have the same number of rows as the data matrix
* This column vector is printed as the first column
* @params data: A matrix of data
* @params filename: The name of the file to print
*/
void printMatrixToFile(SimTK::Vector col0,SimTK::Matrix data, string filename){
ofstream datafile;
datafile.open(filename.c_str());
for(int i = 0; i < data.nrow(); i++){
datafile << col0(i) << ",";
for(int j = 0; j < data.ncol(); j++){
if(j<data.ncol()-1)
datafile << data(i,j) << ",";
else
datafile << data(i,j) << "\n";
}
}
datafile.close();
}
/**
* This function will print cvs file of the column vector col0 and the matrix data
*
* @params col0: A vector that must have the same number of rows as the data matrix
* This column vector is printed as the first column
* @params data: A matrix of data
* @params filename: The name of the file to print
*/
void SegmentedQuinticBezierToolkit::
printMatrixToFile(const SimTK::Vector& col0,
const SimTK::Matrix& data, std::string& filename)
{
ofstream datafile;
datafile.open(filename.c_str());
for(int i = 0; i < data.nrow(); i++){
datafile << col0(i) << ",";
for(int j = 0; j < data.ncol(); j++){
if(j<data.ncol()-1)
datafile << data(i,j) << ",";
else
datafile << data(i,j) << "\n";
}
}
datafile.close();
}
void SegmentedQuinticBezierToolkit::
printBezierSplineFitCurves(const SimTK::Function_<double>& curveFit,
SimTK::Matrix& ctrlPts, SimTK::Vector& xVal, SimTK::Vector& yVal,
std::string& filename)
{
std::string caller = "printBezierSplineFitCurves";
int nbezier = int(ctrlPts.ncol()/2.0);
int rows = NUM_SAMPLE_PTS*nbezier - (nbezier-1);
SimTK::Vector y1Val(rows);
SimTK::Vector y2Val(rows);
SimTK::Vector ySVal(rows);
SimTK::Vector y1SVal(rows);
SimTK::Vector y2SVal(rows);
SimTK::Matrix printMatrix(rows,6);
SimTK::Vector tmp(1);
std::vector<int> deriv1(1);
std::vector<int> deriv2(2);
deriv1[0] = 0;
deriv2[0] = 0;
deriv2[1] = 0;
double u = 0;
int oidx = 0;
int offset = 0;
for(int j=0; j < nbezier ; j++)
{
if(j > 0){
offset = 1;
}
for(int i=0; i<NUM_SAMPLE_PTS-offset; i++)
{
oidx = i + j*NUM_SAMPLE_PTS - offset*(j-1);
u = ( (double)(i+offset) )/( (double)(NUM_SAMPLE_PTS-1) );
y1Val(oidx) = calcQuinticBezierCurveDerivDYDX(u,
ctrlPts(2*j),ctrlPts(2*j+1),1);
y2Val(oidx) = calcQuinticBezierCurveDerivDYDX(u,
ctrlPts(2*j),ctrlPts(2*j+1),2);
tmp(0) = xVal(oidx);
ySVal(oidx) = curveFit.calcValue( tmp );
y1SVal(oidx) = curveFit.calcDerivative(deriv1,tmp);
y2SVal(oidx) = curveFit.calcDerivative(deriv2,tmp);
printMatrix(oidx,0) = yVal(oidx);
printMatrix(oidx,1) = y1Val(oidx);
printMatrix(oidx,2) = y2Val(oidx);
printMatrix(oidx,3) = ySVal(oidx);
printMatrix(oidx,4) = y1SVal(oidx);
printMatrix(oidx,5) = y2SVal(oidx);
}
}
printMatrixToFile(xVal,printMatrix,filename);
}
/**
@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;
}