fix calculation of distance from plane to origin

fix calculation of distance from plane to origin