Obstacle avoidance algorithm c++