void MonotonicInterpolate(const vector<Vector>& pts,const vector<Real>& times,
GeneralizedCubicBezierSpline& path,
CSpace* space,GeodesicManifold* manifold)
{
MonotonicInterpolate(pts,times,path.segments,space,manifold);
Assert(path.segments.size()+1==times.size());
path.durations.resize(path.segments.size());
for(size_t i=0;i<path.segments.size();i++) path.durations[i] = times[i+1]-times[i];
}
void MonotonicInterpolate(const vector<Vector>& pts,
GeneralizedCubicBezierSpline& path,
CSpace* space,GeodesicManifold* manifold,
Real coxDeBoorParameter)
{
if(coxDeBoorParameter == 0) {
//uniform
MonotonicInterpolate(pts,path.segments,space,manifold);
path.durations.resize(path.segments.size());
fill(path.durations.begin(),path.durations.end(),1);
}
else {
vector<Real> times(pts.size());
times[0] = 0;
path.durations.resize(pts.size()-1);
for(size_t i=0;i+1<pts.size();i++) {
path.durations[i] = Pow(pts[i].distance(pts[i+1]),coxDeBoorParameter);
times[i+1] = times[i] + path.durations[i];
}
MonotonicInterpolate(pts,times,path.segments,space,manifold);
}
}
bool MultiSmoothInterpolate(SmoothConstrainedInterpolator& interp,const vector<Vector>& pts,GeneralizedCubicBezierSpline& path)
{
path.segments.resize(0);
path.durations.resize(0);
Vector temp;
GeneralizedCubicBezierSpline cpath;
vector<GeneralizedCubicBezierCurve> pathSegs;
MonotonicInterpolate(pts,pathSegs,interp.space);
//SplineInterpolate(pts,pathSegs,interp.space);
Assert(pathSegs.size()+1==pts.size());
vector<Vector> derivs(pts.size());
pathSegs[0].Deriv(0,derivs[0]);
for(size_t i=0;i+1<pts.size();i++)
pathSegs[i].Deriv(1,derivs[i+1]);
//project tangent points
for(size_t i=0;i<pts.size();i++)
interp.ProjectVelocity(pts[i],derivs[i]);
/*
//condition tangent points?
for(size_t i=0;i+1<pts.size();i++)
pathSegs[i].SetNaturalTangents(derivs[i],derivs[i+1]);
//TODO: global solve?
for(int iters=0;iters<10;iters++) {
for(size_t i=0;i+1<pts.size();i++) {
ConditionMiddleTangent(pathSegs[i],pathSegs[i+1]);
pathSegs[i].Deriv(1,derivs[i+1]);
}
}
*/
for(size_t i=0;i+1<pts.size();i++) {
cpath.segments.resize(0);
cpath.durations.resize(0);
Assert(i < pathSegs.size());
Assert(pathSegs.size()+1==pts.size());
//bool res=interp.Make(pts[i],di,pts[i+1],dn,cpath);
bool res=interp.Make(pathSegs[i].x0,derivs[i],pathSegs[i].x3,derivs[i+1],cpath);
if(!res) {
printf("Could not make path between point %d and %d\n",i,i+1);
return false;
}
//cout<<"Actual initial velocity: "<<3.0*(cpath.front().x1-cpath.front().x0)/cdurations.front()<<", terminal velocity: "<<3.0*(cpath.back().x3-cpath.back().x2)/cdurations.back()<<endl;
path.Concat(cpath);
}
return true;
}
