NavX MXP Continuous Angle to Calculate Derivative

[Jared Russell]

This year on 254 we switched to using a simple Rotation2d class for all angles (because, among other reasons, dealing with angle rollover as you did here is easy to screw up). Internally, this class stores the sine and cosine of an angle explicitly. This has some nice properties:

• We use static utility methods to create a Rotation2d from a vector (x,y -> cos,sin after normalization), absolute angle in radians, or absolute angle in degrees.
• We have accessors that return the absolute angle in degrees or radians (wrapped to -Pi to Pi).
• We have "Inverse()" and "RotateBy(Rotation2d other)" methods for creating a new Rotation2d:
  

  Rotation2d Inverse() {
    return new Rotation2d(cos_angle, -sin_angle);
  }
  
  Rotation2d RotateBy(Rotation2d other) {
    return new Rotation2d(cos_angle * other.cos_angle - sin_angle * other.sin_angle,
                  cos_angle * other.sin_angle + sin_angle * other.cos_angle);
  }

• If you want to know the shortest rotation from A to B, you can do something like:
  

  double shortest_distance_degrees = A.Inverse().RotateBy(B).ToDegrees();

  This will always give you a solution on [-180, 180].
• Cheap access to the sine, cosine, and tangent (sin/cos) of the angle because they are already cached.

[JamesTerm]

Thanks Ether... learn something new everyday!

The way Microsoft's library present's this function is like so:
IEEERemainder = dividend - (divisor * Math.Round(dividend / divisor))

It's great to see such a compatible use-case for this function as written in this thread... this function has also helped improve some bugs in my simulations as well.

[JamesTerm]

Here is a c++ equivalent within standard libraries:
http://en.cppreference.com/w/cpp/numeric/math/remainder