/**
* @param p0 The starting pose of the spline
* @param p1 The ending pose of the spline
*/
public QuinticSpline(Pose2d p0, Pose2d p1)
{
double scale = 1.2 * p0.getTranslation().distance(p1.getTranslation());
x0 = p0.getTranslation().x();
x1 = p1.getTranslation().x();
dx0 = p0.getRotation().cos() * scale;
dx1 = p1.getRotation().cos() * scale;
ddx0 = 0;
ddx1 = 0;
y0 = p0.getTranslation().y();
y1 = p1.getTranslation().y();
dy0 = p0.getRotation().sin() * scale;
dy1 = p1.getRotation().sin() * scale;
ddy0 = 0;
ddy1 = 0;
computeCoefficients();
}