/** * Tests if the two given planes are coincident: * <ul> * <li> {@code planeNormal1} and {@code planeNormal2} are collinear given the tolerance {@code angleEpsilon}. * <li> the distance of {@code pointOnPlane2} from the first plane is less than {@code distanceEpsilon}. * </ul> * <p> * Edge cases: * <ul> * <li> if the length of either normal is below {@code 1.0E-7}, this method fails and returns {@code false}. * </ul> * </p> * * @param pointOnPlane1 a point on the first plane. Not modified. * @param planeNormal1 the normal of the first plane. Not modifed. * @param pointOnPlane2 a point on the second plane. Not modified. * @param planeNormal2 the normal of the second plane. Not modified. * @param angleEpsilon tolerance on the angle in radians to determine if the plane normals are collinear. * @param distanceEpsilon tolerance on the distance to determine if {@code pointOnPlane2} belongs to the first plane. * @return {@code true} if the two planes are coincident, {@code false} otherwise. */ public static boolean arePlanesCoincident(Point3d pointOnPlane1, Vector3d planeNormal1, Point3d pointOnPlane2, Vector3d planeNormal2, double angleEpsilon, double distanceEpsilon) { if (!areVectorsCollinear(planeNormal1, planeNormal2, angleEpsilon)) return false; else return distanceFromPointToPlane(pointOnPlane2, pointOnPlane1, planeNormal1) < distanceEpsilon; }
/** * Computes the minimum distance between a given point and a plane. * * @param point the 3D query. Not modified. * @param pointOnPlane a point located on the plane. Not modified. * @param planeNormal the normal of the plane. Not modified. * @return the distance between the point and the plane. * @throws ReferenceFrameMismatchException if the arguments are not expressed in the same reference frame. */ public static double distanceFromPointToPlane(FramePoint point, FramePoint pointOnPlane, FrameVector planeNormal) { point.checkReferenceFrameMatch(pointOnPlane); point.checkReferenceFrameMatch(planeNormal); return distanceFromPointToPlane(point.getPoint(), pointOnPlane.getPoint(), planeNormal.getVector()); }
FrameVector3D planeNormal = new FrameVector3D(pointOnPlane.getReferenceFrame(), 0, 0, 1); FramePoint3D point = new FramePoint3D(ReferenceFrame.getWorldFrame(), 0, 0, 3); double actual = GeometryTools.distanceFromPointToPlane(point, pointOnPlane, planeNormal); double expected = 3.0; assertEquals("FAILED: Distance from point to plane", expected, actual, EPSILON); planeNormal = new FrameVector3D(pointOnPlane.getReferenceFrame(), 0, 0, 1); point = new FramePoint3D(ReferenceFrame.getWorldFrame(), 3, 3, -3); actual = GeometryTools.distanceFromPointToPlane(point, pointOnPlane, planeNormal); expected = 3.0; assertEquals("FAILED: Distance from point to plane", expected, actual, EPSILON); planeNormal = new FrameVector3D(pointOnPlane.getReferenceFrame(), 0, 0, 1); point = new FramePoint3D(ReferenceFrame.getWorldFrame(), 0, 0, -3); actual = GeometryTools.distanceFromPointToPlane(point, pointOnPlane, planeNormal); expected = 3.0; assertEquals("FAILED: Distance from point to plane", expected, actual, EPSILON); planeNormal = new FrameVector3D(pointOnPlane.getReferenceFrame(), 0, 0, 1); point = new FramePoint3D(ReferenceFrame.getWorldFrame(), 0, 0, -3); actual = GeometryTools.distanceFromPointToPlane(point, pointOnPlane, planeNormal); expected = 6.0; assertEquals("FAILED: Distance from point to plane", expected, actual, EPSILON); planeNormal = new FrameVector3D(pointOnPlane.getReferenceFrame(), 1, 0, 0); point = new FramePoint3D(ReferenceFrame.getWorldFrame(), 3, 0, 0); actual = GeometryTools.distanceFromPointToPlane(point, pointOnPlane, planeNormal); expected = 3.0; assertEquals("FAILED: Distance from point to plane", expected, actual, EPSILON);