void Cubic_spline::findCutoffs()
{
//Find the cutoff lengths for each function.
rcut.Resize(nfunctions);
int totfunc=0;
for(int i=0; i < nsplines; i++) {
int nrep=0;
switch(symmetry(i)) {
case sym_S:
nrep=1;
break;
case sym_P:
case sym_P_siesta:
nrep=3;
break;
case sym_5D:
case sym_D_siesta:
nrep=5;
break;
case sym_6D:
nrep=6;
break;
case sym_7F:
case sym_7F_crystal://CRYSTAL F orbital
case sym_F_siesta:
nrep=7;
break;
case sym_10F:
nrep=10;
break;
case sym_9G:
nrep=9;
break;
case sym_15G:
nrep=15;
break;
default:
error("I don't have this symmetry:", symmetry(i));
}
doublevar cutoff=splines(i).findCutoff();
for(int j=0; j< nrep; j++) {
rcut(totfunc++)=cutoff;
}
}
threshold=0;
for(int i=0; i< rcut.GetDim(0); i++) {
if(threshold < rcut(i)) threshold=rcut(i);
}
for(int i=0; i< nsplines; i++) {
splines(i).pad(threshold);
}
}
//-------------------------------------------------------------------------
void Cubic_spline::raw_input(ifstream & input)
{
splines.Resize(nsplines);
for(int s=0; s< nsplines; s++) {
splines(s).raw_input(input);
}
}
//-------------------------------------------------------------------------
int Cubic_spline::readspline(vector <string> & words) {
unsigned int pos=0;
vector <vector <string> > splinefits;
vector <string> onesection;
while(readsection(words, pos, onesection, "SPLINE")) {
splinefits.push_back(onesection);
}
if(splinefits.size()==0) {
return 0;
}
nsplines=splinefits.size();
splines.Resize(nsplines);
symmetry.Resize(nsplines);
for(int i=0; i< nsplines; i++) {
symmetry(i)=symmetry_lookup(splinefits[i][0]);
//splinefits[i].erase(splinefits[i].begin());
}
assign_indiv_symmetries();
//cout << "created " << nfunc() << " total functions " << endl;
double max_support=0.0;
for(int i=0; i< nsplines; i++) {
int n=splinefits[i].size();
double supp=atof(splinefits[i][n-2].c_str());
if(supp > max_support) max_support=supp;
}
//cout << "maximum support " << max_support <<endl;
for(int s=0; s< nsplines; s++) {
splines(s).readspline(splinefits[s], enforce_cusp, cusp/double(symmetry_lvalue(symmetry(s))+1));
if(requested_cutoff > 0) {
splines(s).enforceCutoff(requested_cutoff);
}
}
findCutoffs();
return nfunc();
}
int Cubic_spline::writeinput(string & indent, ostream & os)
{
os << indent << atomname << endl;
os << indent << "AOSPLINE \n";
os << indent << "NORMTYPE " << norm_type << endl;
if(renormalize==false) {
os << indent << "NORENORMALIZE\n";
}
if(zero_derivative) {
os << indent << "ZERO_DERIVATIVE\n";
}
if(customspacing!=0.02)
os << indent << "SPACING "<<customspacing<<endl;
if(requested_cutoff > 0 ) {
os << indent << "CUTOFF " << requested_cutoff << endl;
}
if(enforce_cusp) {
os << indent << "CUSP " << cusp << endl;
os << indent << "CUSP_MATCHING " << cusp_matching << endl;
}
if(exponent.size() > 0) {
os << indent << "GAMESS { \n";
for(int funcNum=0; funcNum<nsplines; funcNum++) {
os << indent << symmetry_lookup(symmetry(funcNum)) << endl;
os << indent << exponent[funcNum].size() << endl;
for(unsigned int i=0; i < exponent[funcNum].size(); i++) {
os << indent << " " << i << " " << exponent[funcNum][i]
<< " " << coefficient[funcNum][i] << endl;
}
}
os << indent << " } \n";
}
else {
for(int funcNum=0; funcNum < nsplines; funcNum++) {
os << indent << "SPLINE { \n";
os << indent << symmetry_lookup(symmetry(funcNum)) << endl;
splines(funcNum).writeinput(indent, os);
os << indent << " }\n";
}
}
return 1;
}
/*!
This takes a vector of words
presumably gotten from an input file and parses them into a basis function.
It takes something very similar to GAMESS input, but it does not allow you
to have a normalization constant.(ie, all function headers should be like
S 1, not S 1 1.0) It will normalize it anyway.
*/
int Cubic_spline::readbasis(vector <string> & words,unsigned int & pos,
Array1 <double> & parms){
doublevar spacing=parms(0);
int n= (int) parms(1);
double yp1=parms(2);
double ypn=parms(3);
int symmtype=0;
if(parms.GetDim(0) > 4)
symmtype=(int) parms(4);
assert(symmtype==0 || symmtype==1);
string symm;
int ngauss;
vector <symmetry_type> symmetry_temp;
for(unsigned int p=pos; p<words.size(); p++) {
symm=words[p];
ngauss=atoi(words[++p].c_str());
symmetry_temp.push_back(symmetry_lookup(symm));
vector <doublevar> exp(ngauss);
vector <doublevar> c(ngauss);
if(p+ngauss*3 >= words.size()) {
error("Unexpected end of GAMESS section. Count is wrong.");
}
for(int i=0; i<ngauss; i++) {
p++;
exp[i]=atof(words[++p].c_str());
c[i]=atof(words[++p].c_str());
}
exponent.push_back(exp);
coefficient.push_back(c);
}
nsplines=exponent.size();
Array1 <doublevar> y(n);
Array1 <doublevar> x(n);
splines.Resize(nsplines);
symmetry.Resize(nsplines);
for(int i=0; i< nsplines; i++) {
symmetry(i)=symmetry_temp[i];
}
//cout << "nfunc " << nfunc() << endl;
const double normtol=1e-5; //tolerance for the normalization of the basis fn
for(int funcNum=0; funcNum<nsplines; funcNum++) {
//calculate interpolation points
for(int j=0; j<n; j++) {
x(j)=j*spacing;
}
y=0;
for(unsigned int i=0; i < exponent[funcNum].size(); i++) {
doublevar norm;
if(norm_type=="GAMESSNORM") {
doublevar fac=sqrt(2.*exponent[funcNum][i]/pi);
doublevar feg=4.*exponent[funcNum][i];
doublevar feg2=feg*feg;
switch(symmetry(funcNum)) {
case sym_S:
norm=sqrt(2.*feg*fac);
break;
case sym_P:
norm=sqrt(2.*feg2*fac/3.);
break;
case sym_5D:
case sym_6D:
norm=sqrt(2.*feg*feg2*fac/15.);
break;
case sym_7F:
case sym_7F_crystal://CRYSTAL F orbital
case sym_10F:
norm=sqrt(2.*feg2*feg2*fac/105.);
break;
case sym_9G:
case sym_15G:
norm=sqrt(2.*feg2*feg2*feg*fac/945.); //Lubos-done
break;
/* general formula here with n principal quantum numbers
norm=sqrt[(2 ((4exponent)**n) sqrt(2exponent/pi))/(2n-1)!!]
*/
default:
norm=0;
error("Unknown symmetry in Cubic_spline::readbasis! Shouldn't be here!");
}
}
else if(norm_type=="CRYSTAL") {
switch(symmetry(funcNum))
{
case sym_S:
norm=1;
break;
case sym_P:
norm=1/sqrt(3.0);
break;
case sym_5D:
case sym_6D:
norm=1/sqrt(5.0);
break;
case sym_7F:
case sym_7F_crystal://CRYSTAL F orbital
case sym_10F:
norm=1/sqrt(7.0);
break;
case sym_9G:
case sym_15G:
norm=1/sqrt(9.0);
break;
default:
norm=0;
error("unknown symmetry in cubic_spline: ", symmetry(funcNum));
}
}
else if(norm_type=="NONE") {
norm=1;
}
else {
norm=0;
error("Didn't understand NORMTYPE ", norm_type);
}
for(int j=0; j<n; j++)
{
y(j)+=norm*coefficient[funcNum][i]
*exp(-exponent[funcNum][i]*x(j)*x(j));
}
}
//Renormalize to one
if(renormalize) {
//check normalization
doublevar sum=0;
for(int j=0; j<n; j++) {
int p=0;
switch(symmetry(funcNum)) {
case sym_S:
p=1;
break;
case sym_P:
p=2;
break;
case sym_5D:
case sym_6D:
p=3;
break;
case sym_7F:
case sym_7F_crystal://CRYSTAL F orbital
case sym_10F:
p=4;
break;
case sym_9G:
case sym_15G:
p=5;
break;
default:
error("unknown symmetry when checking the normalization");
}
//doublevar power=pow(x(j), 2*(symmetry(funcNum)+1) );
doublevar power=pow(x(j), 2*p );
sum+=spacing*power*y(j)*y(j);
}
//cout << "normalization : " << sqrt(sum) << endl;
//renormalize if needed
if(fabs(sum-1) > normtol){
//debug_write(cout, "Normalizing basis function, sum: ", sum, "\n");
for(int j=0; j<n; j++){
y(j)=y(j)/sqrt(sum);
}
}
}
//Force the function to go to zero if the user requested a cutoff
if(requested_cutoff > 0) {
const double smooth=1.2;
const double cutmax=requested_cutoff-1e-6;
const double cutmin=cutmax-smooth;
for(int j=0; j< n; j++) {
if(x(j) > cutmin) {
if(x(j) > cutmax) { //if we're beyond the cutoff completely
y(j)=0;
}
else { //if we're in the smooth cutoff region
double zz=(x(j)-cutmin)/smooth;
y(j) *= (1-zz*zz*zz*(6*zz*zz-15*zz+10));
}
}
}
}
//--Estimate the derivatives on the boundaries
if ( zero_derivative ) {
yp1=0.0;
} else {
yp1=(y(1)-y(0))/spacing;
}
ypn=(y(n-1) - y(n-2) ) /spacing;
if(enforce_cusp) {
double der=cusp/double(symmetry_lvalue(symmetry(funcNum))+1);
yp1=der;
// KMR: using an STO near the atom should be a separate decision from
// passing the first derivative to the spliner
if(match_sto) { //inspired from J. Chem. Phys. 130, 114107 (2009)
//Here, we match the value, first derivative, and second derivative to the
//given smooth function at some correction radius rc. We can then safely
//replace the function from [0:rc] with the Slater function.
//cout << "enforcing cusp" << endl;
double rc=cusp_matching;
int closest=rc/spacing;
rc=x(closest);
//cout << "rc " << rc << endl;
double curve=(y(closest+1)+y(closest-1)-2*y(closest))/(spacing*spacing);
double deriv=(y(closest+1)-y(closest-1))/(2*spacing);
double f=y(closest);
double b=(deriv*der*der-curve*der)/(curve*der*rc*rc+curve*rc*2-deriv*der*der*rc*rc-deriv*4*der*rc-2*deriv);
double a=curve/(exp(der*rc)*(der*der*(1+b*rc*rc)+4*der*b*rc+2*b));
double c=f-a*exp(der*rc)*(1+b*rc*rc);
//cout << "a " << a << " b " << b << " c " << c << endl;
for(int j=0; j <= closest; j++) {
y(j)=a*exp(der*x(j))*(1+b*x(j)*x(j))+c;
}
}
}
/*
for(int j=0; j< n; j++) {
cout << "jjjjjjjj " << x(j) << " " << y(j) << endl;
}
cout << "jjjjjjj " << endl;
*/
splines(funcNum).splinefit(x,y,yp1, ypn);
}
nfunctions=nfunc();
assign_indiv_symmetries();
findCutoffs();
return nfunctions;
}
//-------------------------------------------------------------------------
int Cubic_spline::read(
vector <string> & words,
unsigned int & pos
)
{
Array1 <double> basisparms(4);
atomname=words[0];
basisparms(0)=.02; //spacing
basisparms(1)=2500; //number of points
basisparms(2)=1e31;
basisparms(3)=1e31;
if(!readvalue(words, pos=0, norm_type, "NORMTYPE")) {
norm_type="GAMESSNORM";
}
if(haskeyword(words, pos=0, "NORENORMALIZE")) {
renormalize=false;
}
else {
renormalize=true;
}
doublevar cutmax;
if(readvalue(words, pos=0, cutmax, "CUTOFF")) {
requested_cutoff=cutmax;
}
else {
requested_cutoff=-1;
}
enforce_cusp=false;
if(readvalue(words, pos=0, cusp, "CUSP")) enforce_cusp=true;
match_sto=false;
if(readvalue(words, pos=0, cusp_matching, "CUSP_MATCHING")) match_sto=true;
zero_derivative=false;
if(haskeyword(words, pos=0, "ZERO_DERIVATIVE")) zero_derivative=true;
customspacing=basisparms(0);
if(readvalue(words, pos=0, customspacing, "SPACING")) {
basisparms(0)=customspacing;
basisparms(1)=50.0/customspacing;
}
vector <string> basisspec;
string read_file; //read positions from a file
if(readsection(words,pos=0, basisspec, "GAMESS"))
{
unsigned int newpos=0;
return readbasis(basisspec, newpos, basisparms);
}
else {
pos=0;
if(readspline(words)) {
return 1;
}
else {
error("couldn't find proper basis section in AOSPLINE. "
"Try GAMESS { .. }.");
return 0;
}
}
//We assume that all splines use the same interval,
//which saves a few floating divisions, so check to make
//sure the assumption is good.
for(int i=0; i< splines.GetDim(0); i++) {
if(!splines(0).match(splines(i)))
error("spline ", i, " doesn't match the first one ");
}
}
Array<SmoothLinearSpline> Trajectory<JointAngles>::generateLinearSplines() throw (DataException<int> )
{
// n = number of segments
// n+1 = number of input waypoints
// n+3 = number of waypoints (inc. pre-post pseudo)
// This is a bit different from the cubic as we don't use this in a combo interpolater.
Array<SmoothLinearSpline> splines(dimension());
unsigned int n = waypoints.size() - 1; // n = number of segments
Array<double> waypoint_times(n + 3), values(n + 3); // Including pre and post points
/******************************************
** Check rates are set for head/tail
*******************************************/
// Note that we may yet change values[1] and values[n+1] when fixing the pseudo points below
// Set them all to zero (default option, i.e. starting at rest).
if (!waypoints[0].rates_configured())
{
waypoints[0].rates() = WayPoint<JointAngles>::JointDataArray::Constant(waypoints[0].dimension(), 0.0);
}
if (!waypoints[n].rates_configured())
{
waypoints[n].rates() = WayPoint<JointAngles>::JointDataArray::Constant(waypoints[n].dimension(), 0.0);
}
/******************************************
** Make the pre pseudo point.
*******************************************/
/*
* Pull back the first waypoint, find intersection of initial tangent line and the nominal rate line.
* Why? This lets us ensure the first segment is of the specified velocity, and the second segment will
* fall the correct between nominal rates required to get to w_1.
*
* x_
* ^ \__
* / \
* o ------ o
* /
* /
* x
*
* - diagonal lines : nominal rate slopes
* - x : possible pseudo waypoints (at intersection of initial velocity line and nominal rate line)
*
* Basic process:
*
* - pullback a bit
* - get intersections as above
* - find minimum time intersection time (t_first_duration)
* - synchronise a pseudo waypoint at that time on the initial tangents for each joint (only 1 of these will be an actual intersection point)
* - check acceleration constraints of quintics for this waypoint at each joint
* - if fail, pullback again, if success, done!
*/
std::vector<LinearFunction> initial_tangents; // these are the lines from potential pseudo point to w_1, our eventual pseudo point must lie on it.
std::vector<double> initial_angles, initial_rates;
for (unsigned int j = 0; j < dimension(); ++j)
{
initial_angles.push_back(waypoints[0].angles()[j]);
initial_rates.push_back(waypoints[0].rates()[j]);
initial_tangents.push_back(LinearFunction::PointSlopeForm(0, initial_angles[j], initial_rates[j]));
}
std::vector<CartesianPoint2d, Eigen::aligned_allocator<CartesianPoint2d> > pseudo_points(dimension());
std::vector<LinearFunction> nominal_rate_lines(dimension());
WayPoint<JointAngles> pre_pseudo_waypoint(dimension());
double t_pullback = 0.001; // start with 1ms
double t_pullback_max_ = waypoints[0].duration();
double t_pre_intersect_duration = 0.0; // duration from the pulled back wp0 to the intersection point
bool acceleration_constraint_broken = true;
while (acceleration_constraint_broken && (t_pullback <= t_pullback_max_))
{
// establish t_pre_intersect_duration as the minimum time to the intersection of initial tangent
// and nominal rate line
t_pre_intersect_duration = t_pullback_max_ / 2;
for (unsigned int j = 0; j < dimension(); ++j)
{
LinearFunction nominal_rate_line = LinearFunction::PointSlopeForm(
t_pullback, initial_angles[j], -1 * ecl::psign(initial_rates[j]) * waypoints[0].nominalRates()[j]);
CartesianPoint2d pseudo_point = LinearFunction::Intersection(initial_tangents[j], nominal_rate_line);
if (pseudo_point.x() < t_pre_intersect_duration)
{
t_pre_intersect_duration = pseudo_point.x();
}
nominal_rate_lines[j] = nominal_rate_line;
}
acceleration_constraint_broken = false;
for (unsigned int j = 0; j < dimension(); ++j)
{
CartesianPoint2d pseudo_point(t_pre_intersect_duration, initial_tangents[j](t_pre_intersect_duration));
// from the pseudo way point to the next way point (wp1)
double pseudo_point_duration = t_pullback + waypoints[0].duration() - t_pre_intersect_duration;
// Will test 5 positions, which cover all of t_pre_intersect_duration
// and half of pseudo_point_duration
for (unsigned int i = 1; i <= 5; ++i)
{
double t_l = t_pre_intersect_duration - i * t_pre_intersect_duration / 5.0;
double t_r = t_pre_intersect_duration + i * pseudo_point_duration / 10.0;
LinearFunction segment = LinearFunction::Interpolation(pseudo_point.x(), pseudo_point.y(),
t_pullback + waypoints[0].duration(),
waypoints[1].angles()[j]);
double y_0 = initial_tangents[j](t_l);
double y_0_dot = initial_tangents[j].derivative(t_l);
double y = segment(t_r);
double y_dot = segment.derivative(t_r); // slope
QuinticPolynomial quintic = QuinticPolynomial::Interpolation(t_l, y_0, y_0_dot, 0.0, t_r, y, y_dot, 0.0);
if ((fabs(CubicPolynomial::Maximum(t_l, t_r, quintic.derivative().derivative())) < fabs(max_accelerations[j]))
&& (fabs(CubicPolynomial::Minimum(t_l, t_r, quintic.derivative().derivative())) < fabs(max_accelerations[j])))
{
acceleration_constraint_broken = false;
break; // we're good, get out and continue through all the joints
}
if (i == 5)
{
#ifdef DEBUG_LINEAR_INTERPOLATION
std::cout << "Linear Interpolation: pre psuedo point failed with acceleration checks [" << t_pre_intersect_duration << "][" << t_pullback << "]" << std::endl;
#endif
acceleration_constraint_broken = true;
}
}
}
t_pullback = t_pullback * 2; // takes 11 runs to get from 1ms to 1s; adjust, if this is too much
}
if (acceleration_constraint_broken)
{
throw DataException<int>(LOC, ConstructorError, "Max acceleration bound broken by pre pseudo point.", 0);
}
pre_pseudo_waypoint.duration(t_pullback + waypoints[0].duration() - t_pre_intersect_duration);
for (unsigned int j = 0; j < dimension(); ++j)
{
pre_pseudo_waypoint.angles()[j] = initial_tangents[j](t_pre_intersect_duration);
}
#ifdef DEBUG_LINEAR_INTERPOLATION
std::cout << "Linear Interpolation: pre psuedo point done!" << std::endl;
std::cout << " : angles " << pre_pseudo_waypoint.angles() << std::endl;
std::cout << " : pre pseudo duration " << t_pre_intersect_duration << std::endl;
std::cout << " : modified first waypoint duration " << pre_pseudo_waypoint.duration() << std::endl;
#endif
/******************************************
** Make the post pseudo point.
*******************************************/
std::vector<double> final_angles, final_rates;
for (unsigned int j = 0; j < dimension(); ++j)
{
final_angles.push_back(waypoints[n].angles()[j]);
final_rates.push_back(waypoints[n].rates()[j]);
nominal_rate_lines[j] = LinearFunction::PointSlopeForm(
0, final_angles[j], -1 * ecl::psign(final_rates[j]) * waypoints[n - 1].nominalRates()[j]);
}
std::vector<LinearFunction> final_tangents(dimension());
WayPoint<JointAngles> post_pseudo_waypoint(dimension());
double t_pullforward = 0.001; // start with 1ms
double t_pullforward_max_ = waypoints[n - 1].duration();
double t_post_intersect_duration = 0.0; // duration from the pulled back wp0 to the intersection point
acceleration_constraint_broken = true;
while (acceleration_constraint_broken && (t_pullforward <= t_pullforward_max_))
{
// establish t_final as the maximum time to the intersection of final tangent and nominal rate line
t_post_intersect_duration = 0.0;
for (unsigned int j = 0; j < dimension(); ++j)
{
LinearFunction final_tangent = LinearFunction::PointSlopeForm(t_pullforward, final_angles[j], final_rates[j]);
final_tangents[j] = final_tangent;
CartesianPoint2d pseudo_point = LinearFunction::Intersection(final_tangents[j], nominal_rate_lines[j]);
if (pseudo_point.x() > t_post_intersect_duration)
{
t_post_intersect_duration = pseudo_point.x();
}
}
// check acceleration constraints
acceleration_constraint_broken = false;
for (unsigned int j = 0; j < dimension(); ++j)
{
CartesianPoint2d pseudo_point(t_post_intersect_duration, final_tangents[j](t_post_intersect_duration));
double pseudo_point_duration = t_pullforward - t_post_intersect_duration;
for (unsigned int i = 1; i <= 5; ++i)
{
double t_l = t_post_intersect_duration - i * (waypoints[n - 1].duration() + t_post_intersect_duration) / 10.0;
double t_r = t_post_intersect_duration + i * pseudo_point_duration / 5.0;
LinearFunction segment = LinearFunction::Interpolation(-waypoints[n - 1].duration(),
waypoints[n - 1].angles()[j], pseudo_point.x(),
pseudo_point.y());
double y_0 = segment(t_l);
double y_0_dot = segment.derivative(t_l);
double y = final_tangents[j](t_r);
double y_dot = final_tangents[j].derivative(t_r);
QuinticPolynomial quintic = QuinticPolynomial::Interpolation(t_l, y_0, y_0_dot, 0.0, t_r, y, y_dot, 0.0);
if ((fabs(CubicPolynomial::Maximum(t_l, t_r, quintic.derivative().derivative())) < fabs(max_accelerations[j]))
&& (fabs(CubicPolynomial::Minimum(t_l, t_r, quintic.derivative().derivative())) < fabs(max_accelerations[j])))
{
acceleration_constraint_broken = false;
break; // we're good, get out and continue through all the joints
}
if (i == 5)
{
#ifdef DEBUG_LINEAR_INTERPOLATION
std::cout << "Linear Interpolation: post psuedo point failed with acceleration checks [" << t_post_intersect_duration << "][" << t_pullforward << "]" << std::endl;
#endif
acceleration_constraint_broken = true;
}
}
}
t_pullforward = t_pullforward * 2; // takes 12 runs to get from 1ms to 1s, adjust, if this is too much
}
if (acceleration_constraint_broken)
{
throw DataException<int>(LOC, ConstructorError, "Max acceleration bound broken by pre pseudo point.", 0);
}
post_pseudo_waypoint.duration(t_pullforward - t_post_intersect_duration);
for (unsigned int j = 0; j < dimension(); ++j)
{
post_pseudo_waypoint.angles()[j] = final_tangents[j](t_post_intersect_duration);
}
#ifdef DEBUG_LINEAR_INTERPOLATION
std::cout << "Linear Interpolation: post psuedo point done!" << std::endl;
std::cout << " : angles " << post_pseudo_waypoint.angles() << std::endl;
std::cout << " : modified last point duration " << waypoints[n-1].duration() + t_post_intersect_duration << std::endl;
std::cout << " : post pseudo duration " << post_pseudo_waypoint.duration() << std::endl;
#endif
/*********************
** Waypoint Times
**********************/
// n+3 points (w_0...w_n + pre and post pseudos)
// n+3 waypoint_times
waypoint_times[0] = 0.0;
waypoint_times[1] = t_pre_intersect_duration;
waypoint_times[2] = t_pre_intersect_duration + pre_pseudo_waypoint.duration();
for (unsigned int i = 2; i < n; ++i)
{
waypoint_times[i + 1] = waypoint_times[i] + waypoints[i - 1].duration();
}
waypoint_times[n + 1] = waypoint_times[n] + waypoints[n - 1].duration() + t_post_intersect_duration;
waypoint_times[n + 2] = waypoint_times[n + 1] + post_pseudo_waypoint.duration();
#ifdef DEBUG_LINEAR_INTERPOLATION
std::cout << "Linear Interpolation: waypoint time estimates before interpolation: " << waypoint_times << std::endl;
#endif
for (unsigned int j = 0; j < dimension(); ++j)
{
/******************************************
** Set Values
*******************************************/
values[0] = waypoints[0].angles()[j];
values[1] = pre_pseudo_waypoint.angles()[j];
for (unsigned int i = 2; i <= n; ++i)
{
values[i] = waypoints[i - 1].angles()[j];
}
values[n + 1] = post_pseudo_waypoint.angles()[j];
values[n + 2] = waypoints[n].angles()[j];
#ifdef DEBUG_LINEAR_INTERPOLATION
std::cout << "Linear Interpolation: values[" << j << "]: " << values << std::endl;
#endif
/******************************************
** Generate Spline
*******************************************/
try
{
splines[j] = SmoothLinearSpline::Interpolation(waypoint_times, values, max_accelerations[j]);
}
catch (DataException<int> &e)
{
throw DataException<int>(LOC, e);
}
}
#ifdef DEBUG_LINEAR_INTERPOLATION
for ( unsigned int j = 0; j < dimension(); ++j)
{
std::cout << "Linear Interpolation: discretised domain [" << j << "]: " << splines[j].domain() << std::endl;
}
#endif
return splines;
}
