CS485 Potential Field / Rigid Body Robot Planner in C++