/**
* Do twist interpolation of this pose assuming constant curvature.
*/
public Pose2d interpolate(Pose2d other, double x)
{
if (x <= 0)
{
return(new Pose2d(this));
}
else if (x >= 1)
{
return(new Pose2d(other));
}
Twist2d twist = Pose2d.log(inverse().transformBy(other));
return(transformBy(Pose2d.exp(twist.scaled(x))));
}
public double distance(Pose2d other)
{
return(Pose2d.log(inverse().transformBy(other)).norm());
}