double Dmax,
double Amax,
double Vmax,
final double epsilon) {
// try all of the different path types (each attempt is O(1), so no real performace hit here)
Path res = new DALD(curTime, curPos, curSpeed, destPos, destSpeed, Dmax, Amax, Vmax);
if (res.selfValidate()) {
return res;
}
res = new ALD(curTime, curPos, curSpeed, destPos, destSpeed, Dmax, Amax, Vmax);
if (res.selfValidate()) {
return res;
}
res = new SCurve(curTime, curPos, curSpeed, destPos, destSpeed, Dmax, Amax, Vmax);
if (res.selfValidate()) {
return res;
}
res = new DSCurve(curTime, curPos, curSpeed, destPos, destSpeed, Dmax, Amax, Vmax);
if (res.selfValidate()) {
return res;
}
res = new DLD(curTime, curPos, curSpeed, destPos, destSpeed, Dmax, Amax, Vmax);
if (res.selfValidate()) {
return res;
}
// if no path could be found, then give up and return a zero-feedforward path
return new Path() {