1286 lines
39 KiB
C++
1286 lines
39 KiB
C++
/* -------------------------------------------------------------------------- *
|
|
* OpenSim: SegmentedQuinticBezierToolkit.cpp *
|
|
* -------------------------------------------------------------------------- *
|
|
* The OpenSim API is a toolkit for musculoskeletal modeling and simulation. *
|
|
* See http://opensim.stanford.edu and the NOTICE file for more information. *
|
|
* OpenSim is developed at Stanford University and supported by the US *
|
|
* National Institutes of Health (U54 GM072970, R24 HD065690) and by DARPA *
|
|
* through the Warrior Web program. *
|
|
* *
|
|
* Copyright (c) 2005-2012 Stanford University and the Authors *
|
|
* Author(s): Matthew Millard *
|
|
* *
|
|
* Licensed under the Apache License, Version 2.0 (the "License"); you may *
|
|
* not use this file except in compliance with the License. You may obtain a *
|
|
* copy of the License at http://www.apache.org/licenses/LICENSE-2.0. *
|
|
* *
|
|
* Unless required by applicable law or agreed to in writing, software *
|
|
* distributed under the License is distributed on an "AS IS" BASIS, *
|
|
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. *
|
|
* See the License for the specific language governing permissions and *
|
|
* limitations under the License. *
|
|
* -------------------------------------------------------------------------- */
|
|
/*
|
|
Update:
|
|
This is a port of the original code so that it will work with
|
|
the multibody code RBDL written by Martin Felis.
|
|
|
|
Author:
|
|
Matthew Millard
|
|
|
|
Date:
|
|
Nov 2015
|
|
|
|
*/
|
|
|
|
//=============================================================================
|
|
// INCLUDES
|
|
//=============================================================================
|
|
#include "SegmentedQuinticBezierToolkit.h"
|
|
#include <cstdio>
|
|
#include <iostream>
|
|
#include <fstream>
|
|
#include <cmath>
|
|
#include <sstream>
|
|
|
|
//#include <cassert>
|
|
using namespace RigidBodyDynamics::Addons::Geometry;
|
|
|
|
//=============================================================================
|
|
// STATICS
|
|
//=============================================================================
|
|
//using namespace SimTK;
|
|
//using namespace OpenSim;
|
|
using namespace std;
|
|
using namespace RigidBodyDynamics::Math;
|
|
static double UTOL_DESIRED = std::numeric_limits<double>::epsilon()*1e2;
|
|
static double UTOL_INITIALSOLN = std::numeric_limits<double>::epsilon()*1e11;
|
|
static int MAXITER_INITIALSOLN = 12;
|
|
|
|
static int NUM_SAMPLE_PTS = 100; //The number of knot points to use to sample
|
|
//each Bezier corner section
|
|
|
|
double SegmentedQuinticBezierToolkit::scaleCurviness(double curviness)
|
|
{
|
|
double c = 0.1 + 0.8*curviness;
|
|
return c;
|
|
}
|
|
|
|
/**
|
|
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 VectorNd& col0,
|
|
const MatrixNd& data,
|
|
std::string& filename)
|
|
{
|
|
|
|
ofstream datafile;
|
|
datafile.open(filename.c_str());
|
|
|
|
for(int i = 0; i < data.rows(); i++){
|
|
datafile << col0[i] << ",";
|
|
for(int j = 0; j < data.cols(); j++){
|
|
if(j<data.cols()-1)
|
|
datafile << data(i,j) << ",";
|
|
else
|
|
datafile << data(i,j) << "\n";
|
|
}
|
|
}
|
|
datafile.close();
|
|
}
|
|
|
|
void SegmentedQuinticBezierToolkit::
|
|
printBezierSplineFitCurves( const Function_<double>& curveFit,
|
|
MatrixNd& ctrlPts,
|
|
VectorNd& xVal,
|
|
VectorNd& yVal,
|
|
std::string& filename)
|
|
{
|
|
std::string caller = "printBezierSplineFitCurves";
|
|
int nbezier = int(ctrlPts.cols()/2.0);
|
|
int rows = NUM_SAMPLE_PTS*nbezier - (nbezier-1);
|
|
|
|
VectorNd y1Val(rows);
|
|
VectorNd y2Val(rows);
|
|
|
|
VectorNd ySVal(rows);
|
|
VectorNd y1SVal(rows);
|
|
VectorNd y2SVal(rows);
|
|
|
|
MatrixNd printMatrix(rows,6);
|
|
|
|
VectorNd 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.col(2*j),ctrlPts.col(2*j+1),1);
|
|
y2Val[oidx] = calcQuinticBezierCurveDerivDYDX(u,
|
|
ctrlPts.col(2*j),ctrlPts.col(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);
|
|
|
|
}
|
|
|
|
//=============================================================================
|
|
// Bezier Corner Element Fitting Function
|
|
//=============================================================================
|
|
|
|
/*Detailed Computational Costs
|
|
Divisions Multiplication Additions Assignments
|
|
1 13 9 23
|
|
*/
|
|
MatrixNd SegmentedQuinticBezierToolkit::
|
|
calcQuinticBezierCornerControlPoints(
|
|
double x0, double y0, double dydx0,
|
|
double x1, double y1, double dydx1, double curviness)
|
|
{
|
|
MatrixNd xyPts(6,2);
|
|
|
|
/*
|
|
SimTK_ERRCHK_ALWAYS( (curviness>=0 && curviness <= 1) ,
|
|
"SegmentedQuinticBezierToolkit::calcQuinticBezierCornerControlPoints",
|
|
"Error: double argument curviness must be between 0.0 and 1.0.");
|
|
*/
|
|
if( !(curviness>=0 && curviness <= 1) ){
|
|
|
|
cerr << "SegmentedQuinticBezierToolkit::"
|
|
<<"calcQuinticBezierCornerControlPoints"
|
|
<<"Error: double argument curviness must be between 0.0 and 1.0."
|
|
<<"curviness : " << curviness << " "
|
|
<< endl;
|
|
assert (0);
|
|
abort();
|
|
}
|
|
|
|
|
|
//1. Calculate the location where the two lines intersect
|
|
// (x-x0)*dydx0 + y0 = (x-x1)*dydx1 + y1
|
|
// x*(dydx0-dydx1) = y1-y0-x1*dydx1+x0*dydx0
|
|
// x = (y1-y0-x1*dydx1+x0*dydx0)/(dydx0-dydx1);
|
|
|
|
double xC = 0.;
|
|
double yC = 0.;
|
|
double epsilon = std::numeric_limits<double>::epsilon();
|
|
double rootEPS = sqrt(epsilon);
|
|
if(abs(dydx0-dydx1) > rootEPS){
|
|
xC = (y1-y0-x1*dydx1+x0*dydx0)/(dydx0-dydx1);
|
|
}else{
|
|
xC = (x1+x0)/2.0;
|
|
}
|
|
|
|
yC = (xC-x1)*dydx1 + y1;
|
|
//Check to make sure that the inputs are consistent with a corner, and will
|
|
//not produce an 's' shaped section. To check this we compute the sides of
|
|
//a triangle that is formed by the two points that the user entered, and
|
|
//also the intersection of the 2 lines the user entered. If the distance
|
|
//between the two points the user entered is larger than the distance from
|
|
//either point to the intersection loctaion, this function will generate a
|
|
//'C' shaped curve. If this is not true, an 'S' shaped curve will result,
|
|
//and this function should not be used.
|
|
|
|
double xCx0 = (xC-x0);
|
|
double yCy0 = (yC-y0);
|
|
double xCx1 = (xC-x1);
|
|
double yCy1 = (yC-y1);
|
|
double x0x1 = (x1-x0);
|
|
double y0y1 = (y1-y0);
|
|
|
|
|
|
|
|
double a = xCx0*xCx0 + yCy0*yCy0;
|
|
double b = xCx1*xCx1 + yCy1*yCy1;
|
|
double c = x0x1*x0x1 + y0y1*y0y1;
|
|
|
|
//This error message needs to be better.
|
|
/*
|
|
SimTK_ERRCHK_ALWAYS( ((c > a) && (c > b)),
|
|
"SegmentedQuinticBezierToolkit::calcQuinticBezierCornerControlPoints",
|
|
"The intersection point for the two lines defined by the input"
|
|
"parameters must be consistent with a C shaped corner.");
|
|
*/
|
|
|
|
|
|
|
|
if( !((c > a) && (c > b)) ){
|
|
cerr << "SegmentedQuinticBezierToolkit"
|
|
<< "::calcQuinticBezierCornerControlPoints:"
|
|
<< "The line segments at the end of the curve sections "
|
|
<< "do not intersect within the domain "
|
|
<< "("<< x0 << "," << x1 << ") of the curve. "
|
|
<< "and so there is a chance that curve will not"
|
|
<< " be monotonic. There are 2 ways to fix this problem: "
|
|
<< endl
|
|
<< "1. Add an intermediate point,"
|
|
<< endl
|
|
<< " 2. Space the domain points more widely "
|
|
<< endl
|
|
<< "Details: "
|
|
<< endl << " a: " << a
|
|
<< endl << " b: " << b
|
|
<< endl << " c: " << c << endl;
|
|
assert (0);
|
|
abort();
|
|
|
|
}
|
|
|
|
/*
|
|
Value of the 2nd derivative at the end points.
|
|
This is not exposed to the user for now, as rarely is possible
|
|
or even easy to know what these values should be. Internally
|
|
I'm using this here because we get curves with nicer 1st
|
|
derivatives than if we take the easy option to get a second
|
|
derivative of zero (by setting the middle control points equal
|
|
to their neighbors.
|
|
*/
|
|
|
|
double d2ydx20 = 0;
|
|
double d2ydx21 = 0;
|
|
|
|
//Start point
|
|
xyPts(0,0) = x0;
|
|
xyPts(0,1) = y0;
|
|
//End point
|
|
xyPts(5,0) = x1;
|
|
xyPts(5,1) = y1;
|
|
|
|
|
|
/*
|
|
//Original code - leads to 2 localized corners
|
|
xyPts(1,0) = x0 + curviness*(xC-xyPts(0,0));
|
|
xyPts(1,1) = y0 + curviness*(yC-xyPts(0,1));
|
|
//xyPts(2,0) = xyPts(1,0);
|
|
//xyPts(2,1) = xyPts(1,1);
|
|
|
|
//Second two midpoints
|
|
xyPts(3,0) = xyPts(5,0) + curviness*(xC-xyPts(5,0));
|
|
xyPts(3,1) = xyPts(5,1) + curviness*(yC-xyPts(5,1));
|
|
xyPts(4,0) = xyPts(3,0);
|
|
xyPts(4,1) = xyPts(3,1);
|
|
*/
|
|
|
|
//Set the 1st and 4th control points (nearest to the end points)
|
|
//to get the correct first derivative
|
|
xyPts(1,0) = x0 + curviness*(xC-xyPts(0,0));
|
|
xyPts(1,1) = y0 + curviness*(yC-xyPts(0,1));
|
|
|
|
xyPts(4,0) = xyPts(5,0) + curviness*(xC-xyPts(5,0));
|
|
xyPts(4,1) = xyPts(5,1) + curviness*(yC-xyPts(5,1));
|
|
|
|
//Now go and update the middle points to get the desired 2nd
|
|
//derivative at the ends. Note that even if d2ydx2 = 0 the
|
|
//resulting curve using this method has a much smoother 1st
|
|
//derivative than if the middle control points are set to be
|
|
//equal to the 1st and 4th control points.
|
|
|
|
double dxdu0 = 5.0*(xyPts(1,0) - xyPts(0,0));
|
|
xyPts(2,0) = xyPts(1,0) + 0.5*(xC-xyPts(1,0)) ;
|
|
double d2xdu20 = 20.0*(xyPts(2,0) - 2.0*xyPts(1,0) + xyPts(0,0));
|
|
double d2ydu20 = (dxdu0*dxdu0*(d2ydx20) + d2xdu20*(dydx0)) ;
|
|
xyPts(2,1) = d2ydu20*(1.0/20.0) + 2.0*xyPts(1,1) - xyPts(0,1) ;
|
|
|
|
double dxdu1 = 5.0*(xyPts(5,0) - xyPts(4,0));
|
|
xyPts(3,0) = xyPts(4,0) + 0.5*(xC-xyPts(4,0));
|
|
double d2xdu21 = 20.0*(xyPts(3,0) - 2.0*xyPts(4,0) + xyPts(5,0) );
|
|
double d2ydu21 = (dxdu1*dxdu1*(d2ydx21) + d2xdu21*(dydx1));
|
|
xyPts(3,1) = d2ydu21*(1.0/20.0) + 2.0*xyPts(4,1) - xyPts(5,1);
|
|
|
|
return xyPts;
|
|
}
|
|
|
|
//=============================================================================
|
|
// BASIC QUINTIC BEZIER EVALUATION FUNCTIONS
|
|
//=============================================================================
|
|
|
|
/*
|
|
Multiplications Additions Assignments
|
|
21 20 13
|
|
*/
|
|
double SegmentedQuinticBezierToolkit::
|
|
calcQuinticBezierCurveVal(double u1, const VectorNd& 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);
|
|
*/
|
|
if(!(u1 >= 0 && u1 <= 1)){
|
|
cerr << "SegmentedQuinticBezierToolkit::calcQuinticBezierCurveVal"
|
|
<< "Error: double argument u must be between 0.0 and 1.0"
|
|
<< "but " << u1 <<" was entered.";
|
|
assert (0);
|
|
abort();
|
|
}
|
|
|
|
/*
|
|
SimTK_ERRCHK_ALWAYS( (pts.size() == 6) ,
|
|
"SegmentedQuinticBezierToolkit::calcQuinticBezierCurveVal",
|
|
"Error: vector argument pts must have a length of 6.");
|
|
*/
|
|
if(!(pts.size() == 6) ){
|
|
cerr << "SegmentedQuinticBezierToolkit::calcQuinticBezierCurveVal:"
|
|
<< "Error: vector argument pts must have a length of 6.";
|
|
assert (0);
|
|
abort();
|
|
}
|
|
|
|
double u2 = u1*u1;
|
|
double u3 = u2*u1;
|
|
double u4 = u3*u1;
|
|
double u5 = u4*u1;
|
|
|
|
//v0 = 1;
|
|
double v1 = (1-u1);
|
|
double v2 = v1*v1;
|
|
double v3 = v2*v1;
|
|
double v4 = v3*v1;
|
|
double v5 = v4*v1;
|
|
|
|
val = pts[0] *v5*1.0
|
|
+ pts[1]*u1*v4*5.0
|
|
+ pts[2]*u2*v3*10.0
|
|
+ pts[3]*u3*v2*10.0
|
|
+ pts[4]*u4*v1*5.0
|
|
+ pts[5]*u5 *1.0;
|
|
|
|
|
|
return val;
|
|
}
|
|
|
|
/*
|
|
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 VectorNd& xpts,
|
|
const VectorNd& ypts,
|
|
int order)
|
|
{
|
|
double val = NAN;//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.");
|
|
*/
|
|
if( !(u>=0 && u <= 1) ){
|
|
cerr << "SegmentedQuinticBezierToolkit::calcQuinticBezierCurveDerivU:"
|
|
<< "Error: double argument u must be between 0.0 and 1.0."
|
|
<< endl;
|
|
assert(0);
|
|
abort();
|
|
|
|
}
|
|
|
|
if( !(xpts.size()==6) ){
|
|
cerr << "SegmentedQuinticBezierToolkit::calcQuinticBezierCurveDerivU:"
|
|
<< "Error: vector argument xpts must have a length of 6."
|
|
<< endl;
|
|
assert(0);
|
|
abort();
|
|
|
|
}
|
|
if( !(ypts.size()==6) ){
|
|
cerr << "SegmentedQuinticBezierToolkit::calcQuinticBezierCurveDerivU:"
|
|
<< "Error: vector argument ypts must have a length of 6."
|
|
<< endl;
|
|
assert(0);
|
|
abort();
|
|
}
|
|
|
|
if( !(order >= 1) ){
|
|
cerr << "SegmentedQuinticBezierToolkit::calcQuinticBezierCurveDerivU:"
|
|
<< "Error: order must be greater than."
|
|
<< endl;
|
|
assert(0);
|
|
abort();
|
|
}
|
|
|
|
if( !(order <= 6) ){
|
|
cerr << "SegmentedQuinticBezierToolkit::calcQuinticBezierCurveDerivU:"
|
|
<< "Error: order must be less than, or equal to 6."
|
|
<< endl;
|
|
assert(0);
|
|
abort();
|
|
}
|
|
//std::string localCaller = caller;
|
|
//localCaller.append(".calcQuinticBezierCurveDerivDYDX");
|
|
//Compute the derivative d^n y/ dx^n
|
|
switch(order){
|
|
case 1: //Calculate dy/dx
|
|
{
|
|
double dxdu =calcQuinticBezierCurveDerivU(u,xpts,1);
|
|
double dydu =calcQuinticBezierCurveDerivU(u,ypts,1);
|
|
double dydx = dydu/dxdu;
|
|
|
|
val = dydx;
|
|
//Question:
|
|
//how is a divide by zero treated? Is SimTK::INF returned?
|
|
}
|
|
break;
|
|
case 2: //Calculate d^2y/dx^2
|
|
{
|
|
double dxdu =calcQuinticBezierCurveDerivU(u,xpts,1);
|
|
double dydu =calcQuinticBezierCurveDerivU(u,ypts,1);
|
|
double d2xdu2=calcQuinticBezierCurveDerivU(u,xpts,2);
|
|
double d2ydu2=calcQuinticBezierCurveDerivU(u,ypts,2);
|
|
|
|
//Optimized code from Maple -
|
|
//see MuscleCurveCodeOpt_20120210 for details
|
|
double t1 = 0.1e1 / dxdu;
|
|
double t3 = dxdu*dxdu;//dxdu ^ 2;
|
|
double d2ydx2 = (d2ydu2 * t1 - dydu / t3 * d2xdu2) * t1;
|
|
|
|
val = d2ydx2;
|
|
|
|
}
|
|
break;
|
|
case 3: //Calculate d^3y/dx^3
|
|
{
|
|
double dxdu =calcQuinticBezierCurveDerivU(u,xpts,1);
|
|
double dydu =calcQuinticBezierCurveDerivU(u,ypts,1);
|
|
double d2xdu2=calcQuinticBezierCurveDerivU(u,xpts,2);
|
|
double d2ydu2=calcQuinticBezierCurveDerivU(u,ypts,2);
|
|
double d3xdu3=calcQuinticBezierCurveDerivU(u,xpts,3);
|
|
double d3ydu3=calcQuinticBezierCurveDerivU(u,ypts,3);
|
|
|
|
double t1 = 1 / dxdu;
|
|
double t3 = dxdu*dxdu;//(dxdu ^ 2);
|
|
double t4 = 1 / t3;
|
|
double t11 = d2xdu2*d2xdu2;//(d2xdu2 ^ 2);
|
|
double t14 = (dydu * t4);
|
|
double d3ydx3 = ((d3ydu3 * t1 - 2 * d2ydu2 * t4 * d2xdu2
|
|
+ 2 * dydu / t3 / dxdu * t11 - t14 * d3xdu3) * t1
|
|
- (d2ydu2 * t1 - t14 * d2xdu2) * t4 * d2xdu2) * t1;
|
|
|
|
val = d3ydx3;
|
|
}
|
|
break;
|
|
case 4: //Calculate d^4y/dx^4
|
|
{
|
|
double dxdu =calcQuinticBezierCurveDerivU(u,xpts,1);
|
|
double dydu =calcQuinticBezierCurveDerivU(u,ypts,1);
|
|
double d2xdu2=calcQuinticBezierCurveDerivU(u,xpts,2);
|
|
double d2ydu2=calcQuinticBezierCurveDerivU(u,ypts,2);
|
|
double d3xdu3=calcQuinticBezierCurveDerivU(u,xpts,3);
|
|
double d3ydu3=calcQuinticBezierCurveDerivU(u,ypts,3);
|
|
double d4xdu4=calcQuinticBezierCurveDerivU(u,xpts,4);
|
|
double d4ydu4=calcQuinticBezierCurveDerivU(u,ypts,4);
|
|
|
|
double t1 = 1 / dxdu;
|
|
double t3 = dxdu*dxdu;//dxdu ^ 2;
|
|
double t4 = 1 / t3;
|
|
double t9 = (0.1e1 / t3 / dxdu);
|
|
double t11 = d2xdu2*d2xdu2;//(d2xdu2 ^ 2);
|
|
double t14 = (d2ydu2 * t4);
|
|
double t17 = t3*t3;//(t3 ^ 2);
|
|
double t23 = (dydu * t9);
|
|
double t27 = (dydu * t4);
|
|
double t37 = d3ydu3 * t1 - 2 * t14 * d2xdu2 + 2 * t23 * t11
|
|
- t27 * d3xdu3;
|
|
double t43 = d2ydu2 * t1 - t27 * d2xdu2;
|
|
double t47 = t43 * t4;
|
|
double d4ydx4 = (((d4ydu4 * t1 - 3 * d3ydu3 * t4 * d2xdu2
|
|
+ 6 * d2ydu2 * t9 * t11 - 3 * t14 * d3xdu3
|
|
- 6 * dydu / t17 * t11 * d2xdu2
|
|
+ 6 * t23 * d2xdu2 * d3xdu3
|
|
- t27 * d4xdu4) * t1 - 2 * t37 * t4 * d2xdu2
|
|
+ 2 * t43 * t9 * t11 - t47 * d3xdu3) * t1
|
|
- (t37 * t1 - t47 * d2xdu2) * t4 * d2xdu2) * t1;
|
|
|
|
|
|
val = d4ydx4;
|
|
|
|
}
|
|
break;
|
|
case 5:
|
|
{
|
|
double dxdu =calcQuinticBezierCurveDerivU(u,xpts,1);
|
|
double dydu =calcQuinticBezierCurveDerivU(u,ypts,1);
|
|
double d2xdu2=calcQuinticBezierCurveDerivU(u,xpts,2);
|
|
double d2ydu2=calcQuinticBezierCurveDerivU(u,ypts,2);
|
|
double d3xdu3=calcQuinticBezierCurveDerivU(u,xpts,3);
|
|
double d3ydu3=calcQuinticBezierCurveDerivU(u,ypts,3);
|
|
double d4xdu4=calcQuinticBezierCurveDerivU(u,xpts,4);
|
|
double d4ydu4=calcQuinticBezierCurveDerivU(u,ypts,4);
|
|
double d5xdu5=calcQuinticBezierCurveDerivU(u,xpts,5);
|
|
double d5ydu5=calcQuinticBezierCurveDerivU(u,ypts,5);
|
|
|
|
double t1 = 1 / dxdu;
|
|
double t3 = dxdu*dxdu;//dxdu ^ 2;
|
|
double t4 = 1 / t3;
|
|
double t9 = (0.1e1 / t3 / dxdu);
|
|
double t11 = d2xdu2*d2xdu2;//(d2xdu2 ^ 2);
|
|
double t14 = (d3ydu3 * t4);
|
|
double t17 = t3*t3;//(t3 ^ 2);
|
|
double t18 = 1 / t17;
|
|
double t20 = (t11 * d2xdu2);
|
|
double t23 = (d2ydu2 * t9);
|
|
double t24 = (d2xdu2 * d3xdu3);
|
|
double t27 = (d2ydu2 * t4);
|
|
double t33 = t11*t11;//(t11 ^ 2);
|
|
double t36 = (dydu * t18);
|
|
double t40 = (dydu * t9);
|
|
double t41 = d3xdu3*d3xdu3;//(d3xdu3 ^ 2);
|
|
double t47 = (dydu * t4);
|
|
double t49 = d5ydu5 * t1 - 4 * d4ydu4 * t4 * d2xdu2
|
|
+ 12 * d3ydu3 * t9 * t11 - 6 * t14 * d3xdu3
|
|
- 24 * d2ydu2 * t18 * t20 + 24 * t23 * t24
|
|
- 4 * t27 * d4xdu4 + 24 * dydu / t17 / dxdu * t33
|
|
- 36 * t36 * t11 * d3xdu3 + 6 * t40 * t41
|
|
+ 8 * t40 * d2xdu2 * d4xdu4 - t47 * d5xdu5;
|
|
double t63 = d4ydu4 * t1 - 3 * t14 * d2xdu2 + 6 * t23 * t11
|
|
- 3 * t27 * d3xdu3 - 6 * t36 * t20
|
|
+ 6 * t40 * t24 - t47 * d4xdu4;
|
|
double t73 = d3ydu3 * t1 - 2 * t27 * d2xdu2 + 2 * t40 * t11
|
|
- t47 * d3xdu3;
|
|
double t77 = t73 * t4;
|
|
double t82 = d2ydu2 * t1 - t47 * d2xdu2;
|
|
double t86 = t82 * t9;
|
|
double t89 = t82 * t4;
|
|
double t99 = t63 * t1 - 2 * t77 * d2xdu2 + 2 * t86 * t11
|
|
- t89 * d3xdu3;
|
|
double t105 = t73 * t1 - t89 * d2xdu2;
|
|
double t109 = t105 * t4;
|
|
double d5ydx5 = (((t49 * t1 - 3 * t63 * t4 * d2xdu2
|
|
+ 6 * t73 * t9 * t11 - 3 * t77 * d3xdu3
|
|
- 6 * t82 * t18 * t20
|
|
+ 6 * t86 * t24 - t89 * d4xdu4) * t1
|
|
- 2 * t99 * t4 * d2xdu2
|
|
+ 2 * t105 * t9 * t11 - t109 * d3xdu3) * t1
|
|
- (t99 * t1 - t109 * d2xdu2) * t4 * d2xdu2) * t1;
|
|
|
|
|
|
val = d5ydx5;
|
|
|
|
}
|
|
break;
|
|
case 6:
|
|
{
|
|
double dxdu =calcQuinticBezierCurveDerivU(u,xpts,1);
|
|
double dydu =calcQuinticBezierCurveDerivU(u,ypts,1);
|
|
double d2xdu2=calcQuinticBezierCurveDerivU(u,xpts,2);
|
|
double d2ydu2=calcQuinticBezierCurveDerivU(u,ypts,2);
|
|
double d3xdu3=calcQuinticBezierCurveDerivU(u,xpts,3);
|
|
double d3ydu3=calcQuinticBezierCurveDerivU(u,ypts,3);
|
|
double d4xdu4=calcQuinticBezierCurveDerivU(u,xpts,4);
|
|
double d4ydu4=calcQuinticBezierCurveDerivU(u,ypts,4);
|
|
double d5xdu5=calcQuinticBezierCurveDerivU(u,xpts,5);
|
|
double d5ydu5=calcQuinticBezierCurveDerivU(u,ypts,5);
|
|
double d6xdu6=calcQuinticBezierCurveDerivU(u,xpts,6);
|
|
double d6ydu6=calcQuinticBezierCurveDerivU(u,ypts,6);
|
|
|
|
double t1 = dxdu*dxdu;//(dxdu ^ 2);
|
|
double t3 = (0.1e1 / t1 / dxdu);
|
|
double t5 = d2xdu2*d2xdu2;//(d2xdu2 ^ 2);
|
|
double t8 = t1*t1;//(t1 ^ 2);
|
|
double t9 = 1 / t8;
|
|
double t11 = (t5 * d2xdu2);
|
|
double t14 = (d3ydu3 * t3);
|
|
double t15 = (d2xdu2 * d3xdu3);
|
|
double t19 = (0.1e1 / t8 / dxdu);
|
|
double t21 = t5*t5;//(t5 ^ 2);
|
|
double t24 = (d2ydu2 * t9);
|
|
double t25 = (t5 * d3xdu3);
|
|
double t28 = (d2ydu2 * t3);
|
|
double t29 = d3xdu3*d3xdu3;//(d3xdu3 ^ 2);
|
|
double t32 = (d2xdu2 * d4xdu4);
|
|
double t41 = (dydu * t19);
|
|
double t45 = (dydu * t9);
|
|
double t49 = (dydu * t3);
|
|
double t56 = 1 / dxdu;
|
|
double t61 = 1 / t1;
|
|
double t62 = (dydu * t61);
|
|
double t67 = (d4ydu4 * t61);
|
|
double t70 = (d2ydu2 * t61);
|
|
double t73 = (d3ydu3 * t61);
|
|
double t76 = 20 * d4ydu4 * t3 * t5 - 60 * d3ydu3 * t9 * t11
|
|
+ 60 * t14 * t15 + 120 * d2ydu2 * t19 * t21
|
|
- 180 * t24 * t25
|
|
+ 30 * t28 * t29 + 40 * t28 * t32
|
|
- 120 * dydu / t8 / t1 * t21 * d2xdu2
|
|
+ 240 * t41 *t11*d3xdu3
|
|
- 60 * t45 * t5 * d4xdu4 + 20 * t49 * d3xdu3 * d4xdu4
|
|
+ 10 * t49 * d2xdu2 * d5xdu5 + d6ydu6 * t56
|
|
- 90 * t45 * d2xdu2 * t29 - t62 * d6xdu6
|
|
- 5 * d5ydu5 * t61 * d2xdu2 - 10 * t67 * d3xdu3
|
|
- 5 * t70 * d5xdu5 - 10 * t73 * d4xdu4;
|
|
|
|
double t100 = d5ydu5 * t56 - 4 * t67 * d2xdu2 + 12 * t14 * t5
|
|
- 6 * t73 * d3xdu3 - 24 * t24 * t11 + 24 * t28 * t15
|
|
- 4 * t70 * d4xdu4 + 24 * t41 * t21 - 36 * t45 * t25
|
|
+ 6 * t49 * t29 + 8 * t49 * t32 - t62 * d5xdu5;
|
|
|
|
double t116 = d4ydu4 * t56 - 3 * t73 * d2xdu2 + 6 * t28 * t5
|
|
- 3 * t70 * d3xdu3 - 6 * t45 * t11 + 6 * t49 * t15
|
|
- t62 * d4xdu4;
|
|
|
|
double t120 = t116 * t61;
|
|
double t129 = d3ydu3 * t56 - 2 * t70 * d2xdu2 + 2 * t49 * t5
|
|
- t62 * d3xdu3;
|
|
double t133 = t129 * t3;
|
|
double t136 = t129 * t61;
|
|
double t141 = d2ydu2 * t56 - t62 * d2xdu2;
|
|
double t145 = t141 * t9;
|
|
double t148 = t141 * t3;
|
|
double t153 = t141 * t61;
|
|
double t155 = t76 * t56 - 4 * t100 * t61 * d2xdu2
|
|
+ 12 * t116 * t3 * t5 - 6 * t120 * d3xdu3
|
|
- 24 * t129 * t9 * t11 + 24 * t133 * t15
|
|
- 4 * t136 * d4xdu4
|
|
+ 24 * t141 * t19 * t21 - 36 * t145 * t25 + 6 * t148 * t29
|
|
+ 8 * t148 * t32 - t153 * d5xdu5;
|
|
|
|
double t169 = t100 * t56 - 3 * t120 * d2xdu2 + 6 * t133 * t5
|
|
- 3 * t136 * d3xdu3 - 6 * t145 * t11 + 6 * t148 * t15
|
|
- t153 * d4xdu4;
|
|
|
|
double t179 = t116 * t56 - 2 * t136 * d2xdu2 + 2 * t148 * t5
|
|
- t153 * d3xdu3;
|
|
|
|
double t183 = t179 * t61;
|
|
double t188 = t129 * t56 - t153 * d2xdu2;
|
|
double t192 = t188 * t3;
|
|
double t195 = t188 * t61;
|
|
double t205 = t169 * t56 - 2 * t183 * d2xdu2 + 2 * t192 * t5
|
|
- t195 * d3xdu3;
|
|
double t211 = t179 * t56 - t195 * d2xdu2;
|
|
double t215 = t211 * t61;
|
|
double d6ydx6 = (((t155 * t56 - 3 * t169 * t61 * d2xdu2
|
|
+ 6 * t179 * t3 * t5 - 3 * t183 * d3xdu3
|
|
- 6 * t188 * t9 *t11
|
|
+ 6 * t192 * t15 - t195 * d4xdu4) * t56
|
|
- 2 * t205 * t61 * d2xdu2
|
|
+ 2 * t211*t3*t5-t215*d3xdu3)*t56
|
|
- (t205 * t56 - t215 * d2xdu2) * t61 * d2xdu2) * t56;
|
|
|
|
|
|
val = d6ydx6;
|
|
}
|
|
break;
|
|
default:
|
|
val = NAN; //SimTK::NaN;
|
|
}
|
|
return val;
|
|
}
|
|
|
|
/* 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 VectorNd& 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");
|
|
*/
|
|
if( !(u>=0 && u <= 1) ){
|
|
cerr << "SegmentedQuinticBezierToolkit::calcQuinticBezierCurveDerivU:"
|
|
<< "Error: double argument u must be between 0.0 and 1.0."
|
|
<< endl;
|
|
assert(0);
|
|
abort();
|
|
}
|
|
if( !(pts.size()==6) ){
|
|
cerr << "SegmentedQuinticBezierToolkit::calcQuinticBezierCurveDerivU:"
|
|
<< "Error: vector argument pts must have a length of 6."
|
|
<< endl;
|
|
assert(0);
|
|
abort();
|
|
}
|
|
|
|
if( !(order >= 1) ){
|
|
cerr << "SegmentedQuinticBezierToolkit::calcQuinticBezierCurveDerivU:"
|
|
<< "Error: order must be greater than, or equal to 1"
|
|
<< endl;
|
|
assert(0);
|
|
abort();
|
|
}
|
|
|
|
//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;
|
|
}
|
|
break;
|
|
case 5:
|
|
{
|
|
val = p0 * (-0.120e3)
|
|
+ p1 * 0.600e3
|
|
+ p2 * (-0.1200e4)
|
|
+ p3 * 0.1200e4
|
|
+ p4 * (-0.600e3)
|
|
+ p5 * 0.120e3;
|
|
}
|
|
break;
|
|
default:
|
|
val=0;
|
|
|
|
}
|
|
|
|
return val;
|
|
|
|
|
|
}
|
|
|
|
double SegmentedQuinticBezierToolkit::clampU(double u){
|
|
double uC = u;
|
|
if(u<0.0){
|
|
uC=0;
|
|
}
|
|
if(u>1.0){
|
|
uC=1;
|
|
}
|
|
return uC;
|
|
}
|
|
|
|
/*Detailed Computational Costs
|
|
Comparisons Div Mult Additions Assignments
|
|
Geuss calculation 1 1 1
|
|
|
|
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 VectorNd& bezierPtsX,
|
|
double tol,
|
|
int maxIter)
|
|
{
|
|
//Check to make sure that ax is in the curve domain
|
|
double minX = std::numeric_limits<double>::max();
|
|
double maxX = -minX;
|
|
for(int i=0; i<bezierPtsX.rows(); 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.");
|
|
*/
|
|
if( !(ax >= minX && ax <= maxX) ){
|
|
cerr << "SegmentedQuinticBezierToolkit::calcU:"
|
|
<< "Error: input ax was not in the domain of the "
|
|
<< "Bezier curve specified by the control points in bezierPtsX."
|
|
<< endl;
|
|
assert(0);
|
|
abort();
|
|
}
|
|
|
|
double u = ax/(maxX-minX);
|
|
double f = 0;
|
|
|
|
u = clampU(u);
|
|
f = calcQuinticBezierCurveVal(u,bezierPtsX)-ax;
|
|
|
|
//Use the bisection method to get a good initial
|
|
//start for the Newton method. This is necessary
|
|
//as these curves, though C2, can be so nonlinear
|
|
//that the Newton method oscillates unless it is
|
|
//close to the initial solution.
|
|
if(abs(f) > tol){
|
|
double uL = 0;
|
|
double uR = 1;
|
|
|
|
double fL = calcQuinticBezierCurveVal(uL,bezierPtsX)-ax;
|
|
double fR = calcQuinticBezierCurveVal(uR,bezierPtsX)-ax;
|
|
|
|
int iterBisection = 0;
|
|
|
|
while(iterBisection < MAXITER_INITIALSOLN
|
|
&& min(abs(fL),abs(fR)) > UTOL_INITIALSOLN ){
|
|
u = 0.5*(uL+uR);
|
|
f = calcQuinticBezierCurveVal(u,bezierPtsX)-ax;
|
|
|
|
if(signbit(f) == signbit(fL)){
|
|
fL = f;
|
|
uL = u;
|
|
}else{
|
|
fR = f;
|
|
uR = u;
|
|
}
|
|
iterBisection++;
|
|
}
|
|
|
|
if(abs(fL) < abs(fR)){
|
|
u = uL;
|
|
f = fL;
|
|
}else{
|
|
u = uR;
|
|
f = fR;
|
|
}
|
|
}
|
|
|
|
|
|
|
|
double df = 0;
|
|
double du = 0;
|
|
int iter = 0;
|
|
bool pathologic = false;
|
|
double fprev = f;
|
|
double stepLength = 1.0;
|
|
double perturbation01 = 0.0;
|
|
//Take Newton steps to the desired tolerance
|
|
while((abs(f) > min(tol,UTOL_DESIRED))
|
|
&& (iter < maxIter)
|
|
&& (pathologic == false) ){
|
|
//Take a Newton step
|
|
df = calcQuinticBezierCurveDerivU(u,bezierPtsX,1);
|
|
|
|
if(abs(df) > 0){
|
|
du = -f/df;
|
|
u = u + stepLength*du;
|
|
u = clampU(u);
|
|
fprev = f;
|
|
f = calcQuinticBezierCurveVal(u,bezierPtsX)-ax;
|
|
}else{
|
|
//This should never happen. If we are unluky enough to get here
|
|
//purturb the current solution and continue until we run out of
|
|
//iterations.
|
|
perturbation01 = double(rand()%100)/100.0;
|
|
u = u + perturbation01*0.1;
|
|
u = clampU(u);
|
|
}
|
|
|
|
iter++;
|
|
}
|
|
|
|
//Check for convergence
|
|
if( abs(f) > tol ){
|
|
std::stringstream errMsg;
|
|
errMsg.precision(17);
|
|
errMsg << "SegmentedQuinticBezierToolkit::calcU:" << endl
|
|
<< "Error: desired tolerance of " << tol << endl
|
|
<< " on U not met by the Newton iteration." << endl
|
|
<< " A tolerance of " << f << " was reached." << endl
|
|
<< " Curve range x(u): " << minX << "-" << maxX << endl
|
|
<< " Desired x: " << ax << " closest u " << u << endl
|
|
<< " Bezier points " << endl << bezierPtsX << endl;
|
|
cerr << errMsg.str();
|
|
assert(0);
|
|
abort();
|
|
}
|
|
|
|
return u;
|
|
}
|
|
/*
|
|
|
|
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 MatrixNd& bezierPtsX)
|
|
{
|
|
int idx = 0;
|
|
bool flag_found = false;
|
|
|
|
for(int i=0; i<bezierPtsX.cols(); i++){
|
|
if( x >= bezierPtsX(0,i) && x < bezierPtsX(5,i) ){
|
|
idx = i;
|
|
i = bezierPtsX.cols();
|
|
flag_found = true;
|
|
}
|
|
}
|
|
|
|
//Check if the value x is identically the last point
|
|
if(flag_found == false && x == bezierPtsX(5,bezierPtsX.cols()-1)){
|
|
idx = bezierPtsX.cols()-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.");
|
|
*/
|
|
if( !(flag_found == true)){
|
|
cerr << "SegmentedQuinticBezierToolkit::calcIndex"
|
|
<< "Error: A value of x was used that is not"
|
|
<< " within the Bezier curve set." << endl;
|
|
assert(0);
|
|
abort();
|
|
}
|
|
|
|
|
|
|
|
|
|
return idx;
|
|
}
|
|
|
|
int SegmentedQuinticBezierToolkit::calcIndex(double x,
|
|
const std::vector<VectorNd>& bezierPtsX)
|
|
{
|
|
int idx = 0;
|
|
bool flag_found = false;
|
|
|
|
int n = bezierPtsX.size();
|
|
for(int i=0; i<n; i++){
|
|
if( x >= bezierPtsX[i][0] && x < bezierPtsX[i][5] ){
|
|
idx = i;
|
|
flag_found = true;
|
|
break;
|
|
}
|
|
}
|
|
|
|
//Check if the value x is identically the last point
|
|
if(!flag_found && x == bezierPtsX[n-1][5]){
|
|
idx = n-1;
|
|
flag_found = true;
|
|
}
|
|
|
|
if(!(flag_found == true)){
|
|
cerr << "SegmentedQuinticBezierToolkit::calcIndex "
|
|
<< "Error: A value of x was used that is not "
|
|
<< "within the Bezier curve set."
|
|
<< endl;
|
|
assert(0);
|
|
abort();
|
|
}
|
|
|
|
return idx;
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
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
|
|
|
|
*/
|
|
/*
|
|
MatrixNd SegmentedQuinticBezierToolkit::calcNumIntBezierYfcnX(
|
|
const VectorNd& vX,
|
|
double ic0, double intAcc,
|
|
double uTol, int uMaxIter,
|
|
const MatrixNd& mX, const MatrixNd& mY,
|
|
const SimTK::Array_<SimTK::Spline>& aSplineUX,
|
|
bool flag_intLeftToRight,
|
|
const std::string& caller)
|
|
{
|
|
MatrixNd 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;
|
|
}
|
|
*/
|