/**
* Return true if this pose is (nearly) colinear with the another.
*/
public bool isColinear(Pose2d other)
{
if (!getRotation().isParallel(other.getRotation()))
{
return(false);
}
Twist2d twist = log(inverse().transformBy(other));
return(epsilonEquals(twist.dy, 0.0) && epsilonEquals(twist.dtheta, 0.0));
}
/**
* 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))));
}
/**
* Obtain a new Pose2d from a (constant curvature) velocity. See:
* https://github.com/strasdat/Sophus/blob/master/sophus/se2.hpp
*/
public static Pose2d exp(Twist2d delta)
{
double sin_theta = Math.Sin(delta.dtheta);
double cos_theta = Math.Cos(delta.dtheta);
double s, c;
if (Math.Abs(delta.dtheta) < kEps)
{
s = 1.0 - 1.0 / 6.0 * delta.dtheta * delta.dtheta;
c = .5 * delta.dtheta;
}
else
{
s = sin_theta / delta.dtheta;
c = (1.0 - cos_theta) / delta.dtheta;
}
return(new Pose2d(new Translation2d(delta.dx * s - delta.dy * c, delta.dx * c + delta.dy * s),
new Rotation2d(cos_theta, sin_theta, false)));
}