Tidy
Added contact point controller
Optimisation
Tidy