Cached Vector2D's for collision detection better, had missed some out
Added todo, some reformating