allwpilib
allwpilib copied to clipboard
EllipticalRegionConstraint trajectory constraint rotates the wrong arguments
EllipticalRegionConstraint supports rotated ellipses. It attempts to do this by rotating the ellipse radii. This is incorrect because it resizes the ellipse with the same orientation.
The correct thing to do for a CCW-rotated ellipse is to rotate the test point CW before doing the ellipse equation check. The ellipse radii should stay the same. This is essentially rotating the test point into the ellipse's coordinate frame.