snape / RVO2 C++ 988 289 Optimal Reciprocal Collision Avoidance (C++) collision-avoidance cpp motion-planning robotics