Cartesian Converter

[Tyler-Lentz]

🚀 Feature

While our telemetry and competition waypoint information is given in Lat/Lon/Alt, it makes the pathing much easier to convert everything to XYZ first and then do our calculations.

Goal: create a class which can be instantiated based on a central (0,0) point, and then use it to convert (lat,lon,alt) <--> (x,y,z) where units in the xyz plane are 1 meter.

This should be done in include/pathing/cartesian.hpp and src/pathing/cartesian.cpp

Reference: https://github.com/tritonuas/obc/blob/main/pathing/rtpp/cartesian.py

Documentation

Create a wiki page explaining the math and how to use the class. This will help your understanding and the understanding of people who come after

Testing

To test this, there are several behaviors that should be verified

1. Make sure the 0,0 point always gets converted to 0,0
2. Make sure the distances between points is consistent: if two lat/lon/alt points are known to be 100m apart, they should still be 100 units apart after conversion
3. Make sure (x,y,z) -> (lat,lon.alt) reconverts back to the same original point, and also the same if you start with (lat,lon,alt)
4. Do the math by hand and then verify the code correctly matches the math (Also, potentially use another online calculator as a reference if they are doing the same transformation instead of doing it by hand). Essentially, find known values and verify they are converting right.