Fixed and tested pathfinding for 3D.

The path calculations work fine. Finding the exact player location is
still an issue. Also, the player should get a different hitbox (One arm
leaning on a block fully supports Teddy).