Cleaned up Pathfinding, small fixes, started work on a physics unit