Cleaned up a lot of code and added comments. AH-290

Broke out repetitive code in method. Renamed variables with better names.
Wrote comments in the collision test.