The code is ok, but could be a bit better. It is the most called code in the entire pathing stack (based on benchmarking in 2024) - [ ] Remove straight section points (always put ends of the turning sections) - Quad/plane may not reach top speed if points are too close apart (some ardupilot behiavior) - will need to enforce line checking for inbounds instead of point checking - [ ] cleanup code in general - [ ] the code sorts multiple times, that's wasted compute - [-] ~use boost for lin alg~ after further review, it probably would not make that big of a difference.
The code is ok, but could be a bit better. It is the most called code in the entire pathing stack (based on benchmarking in 2024)
use boost for lin algafter further review, it probably would not make that big of a difference.