@Override
public void compute(double time)
{
currentTime.set(time);
double t1 = leaveTime.getDoubleValue();
double t2 = trajectoryTime.getDoubleValue() - approachTime.getDoubleValue();
double tf = trajectoryTime.getDoubleValue();
xyPolynomial.compute(MathTools.clipToMinMax(time, t1, t2));
currentDistortionPose.interpolate(initialDistortionPose, finalDistortionPose, xyPolynomial.getPosition());
distortedPlanePose.setAndMatchFrame(currentDistortionPose);
distortedPlane.update();
changeFrame(distortedPlane, false);
boolean shouldBeZero = currentTime.getDoubleValue() >= t2 || currentTime.getDoubleValue() < t1;
double alphaDot = shouldBeZero ? 0.0 : xyPolynomial.getVelocity();
double alphaDDot = shouldBeZero ? 0.0 : xyPolynomial.getAcceleration();
currentPosition.interpolate(initialPosition, finalPosition, xyPolynomial.getPosition());
currentVelocity.subAndScale(alphaDot, finalPosition, initialPosition);
currentAcceleration.subAndScale(alphaDDot, finalPosition, initialPosition);
zPolynomial.compute(MathTools.clipToMinMax(time, 0.0, tf));
currentPosition.setZ(zPolynomial.getPosition());
currentVelocity.setZ(zPolynomial.getVelocity());
currentAcceleration.setZ(zPolynomial.getAcceleration());
changeFrame(currentTrajectoryFrame, false);
}