This was a cool optimization I implemented in our robot code this year. You can find the exact implementation in our code here.
An example of this being used can be seen in our simulation demo in our 2023 Season Documentation Thread.
6 Likes