Clean up the codes, and fixed collision step

- Codes are cleaned up
- one mistake was fixed in collision step
- however the results viewed in Paraview don't look correct (not stable
after a while)
- suggestion from the tutor: there maybe mistakes in boundary handling