consider use of atan2 to compute angle between vectors

[godmar]

A small note on something I observed here.

In 2D, you can compute the angle between 2 vectors numerically robustly by taking the atan2 of the cross and dot product, as in:

  private static double getAngleBetweenVectorsAtan2(Translation2d u, Translation2d v) {
    double dot_product = u.toVector().dot(v.toVector());

    return Math.atan2(cross(u, v), dot_product);
  }
  public static double cross(Translation2d a, Translation2d b) {
    return a.getX() * b.getY() - a.getY() * b.getX();
  }