I started working a bit on the path following again. Naming things better and moving stuff around. I also added a simple profiler to the stuff too as seen on above picture. The case shows 25 agents moving in a quite simple level.
The bad news is that the performance is bad! The good news is that it's ok.
There are two times tracked in the above picture. The red-ish line shows how much time is spent in the brute force RVO sampling, and the lime-ish line shows the time taken by the total navigation loop calculation.
My goal is to get this case to around 0.5 ms, and I think it is doable.