void pf_fit_hermite_cubic(Waypoint a, Waypoint b, Spline *s) {
pf_fit_hermite_pre(a, b, s);
double a0_delta = tan(bound_radians(a.angle - (*s).angle_offset));
double a1_delta = tan(bound_radians(b.angle - (*s).angle_offset));
(*s).a = 0; (*s).b = 0;
(*s).c = (a0_delta + a1_delta) / ((*s).knot_distance * (*s).knot_distance);
(*s).d = -(2 * a0_delta + a1_delta) / (*s).knot_distance;
(*s).e = a0_delta;
}
void pf_fit_hermite_quintic(Waypoint a, Waypoint b, Spline *s) {
pf_fit_hermite_pre(a, b, s);
double a0_delta = tan(bound_radians(a.angle - (*s).angle_offset));
double a1_delta = tan(bound_radians(b.angle - (*s).angle_offset));
double d = (*s).knot_distance;
(*s).a = -(3 * (a0_delta + a1_delta)) / (d*d*d*d);
(*s).b = (8 * a0_delta + 7 * a1_delta) / (d*d*d);
(*s).c = -(6 * a0_delta + 4 * a1_delta) / (d*d);
(*s).d = 0; (*s).e = a0_delta;
}
double spline_angle(Spline s, double percentage) {
return bound_radians(atan(spline_deriv(s, percentage)) + s.angle_offset);
}
