Moved distance eq. to math package
Removed unused variables
Attempted to fix collision bug