Fixed a small bug calculating P0 and N from a plane equation