Add visualization and collision detection

General code refactoring