public static Translation2d fromPolar(Rotation2d direction, double magnitude)
{
return(new Translation2d(direction.cos() * magnitude, direction.sin() * magnitude));
}
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)));
}
/**
* We can also rotate Translation2d's. See:
* https://en.wikipedia.org/wiki/Rotation_matrix
*
* @param rotation The rotation to apply.
* @return This translation rotated by rotation.
*/
public Translation2d rotateBy(Rotation2d rotation)
{
return(new Translation2d(x_ * rotation.cos() - y_ * rotation.sin(), x_ * rotation.sin() + y_ * rotation.cos()));
}