private static Translation2d intersectionInternal(Pose2d a, Pose2d b)
{
Rotation2d a_r = a.getRotation();
Rotation2d b_r = b.getRotation();
Translation2d a_t = a.getTranslation();
Translation2d b_t = b.getTranslation();
double tan_b = b_r.tan();
double t = ((a_t.x() - b_t.x()) * tan_b + b_t.y() - a_t.y()) / (a_r.sin() - a_r.cos() * tan_b);
if (Double.IsNaN(t))
{
return(new Translation2d(Double.PositiveInfinity, Double.PositiveInfinity));
}
return(a_t.translateBy(a_r.toTranslation().scale(t)));
}
/**
* Logical inverse of the above.
*/
public static Twist2d log(Pose2d transform)
{
double dtheta = transform.getRotation().getRadians();
double half_dtheta = 0.5 * dtheta;
double cos_minus_one = transform.getRotation().cos() - 1.0;
double halftheta_by_tan_of_halfdtheta;
if (Math.Abs(cos_minus_one) < kEps)
{
halftheta_by_tan_of_halfdtheta = 1.0 - 1.0 / 12.0 * dtheta * dtheta;
}
else
{
halftheta_by_tan_of_halfdtheta = -(half_dtheta * transform.getRotation().sin()) / cos_minus_one;
}
Translation2d translation_part = transform.getTranslation()
.rotateBy(new Rotation2d(halftheta_by_tan_of_halfdtheta, -half_dtheta, false));
return(new Twist2d(translation_part.x(), translation_part.y(), dtheta));
}