Added initial code

Code based from multi_agent to be adapted into a simulation environment,
includes collision detection and resolution, and the ability to add
agents and obstacles