[WIP] Partial fixes to vectorized navigation tests
Some changes are proposed to make NavStatePools useful to replace NavigationState** as function arguments, specially those ones with const qualifiers.
Vectorized navigation tests still broken. The testVectorSafety test fails due to deprecated function SafetyToInMinimize().
NavigationBenchmark is also broken, probably due to incomplete setup of geometry.