Collision working

Figured out the issue, it was stupid. They were checking themselves and determining if the same block was where it currently is. 
Mostly works, kind of buggy. Take a look at it see what you guys think.
Sorry about all the updates, still trying to figure this out.