/** * Given a 2D point expressed in the plane local frame, computes whether the point is in this region. * @param point2dInLocal query expressed in local coordinates. * @return true if the point is inside this region, false otherwise. */ public boolean isPointInside(Point2d point2dInLocal) { return isPointInside(point2dInLocal.x, point2dInLocal.y); }
assertTrue(planarRegion.isPointInside(point2d)); point2d.set(2.0, 0.0); assertTrue(planarRegion.isPointInside(point2d)); point2d.set(0.0, 2.0); assertTrue(planarRegion.isPointInside(point2d)); point2d.set(2.0, 2.0); assertFalse(planarRegion.isPointInside(point2d)); assertTrue(planarRegion.isPointInside(point3d, maximumOrthogonalDistance)); point3d.set(2.0, 0.0, 0.0); assertTrue(planarRegion.isPointInside(point3d, maximumOrthogonalDistance)); point3d.set(0.0, 2.0, 0.0); assertTrue(planarRegion.isPointInside(point3d, maximumOrthogonalDistance)); point3d.set(2.0, 2.0, 0.0); assertFalse(planarRegion.isPointInside(point3d, maximumOrthogonalDistance)); assertTrue(planarRegion.isPointInside(point3d, maximumOrthogonalDistance)); point3d.set(2.0, 0.0, -0.5 * maximumOrthogonalDistance); assertTrue(planarRegion.isPointInside(point3d, maximumOrthogonalDistance)); point3d.set(0.0, 2.0, -0.5 * maximumOrthogonalDistance); assertTrue(planarRegion.isPointInside(point3d, maximumOrthogonalDistance)); point3d.set(0.0, 0.0, -1.5 * maximumOrthogonalDistance); assertFalse(planarRegion.isPointInside(point3d, maximumOrthogonalDistance)); point3d.set(2.0, 0.0, -1.5 * maximumOrthogonalDistance); assertFalse(planarRegion.isPointInside(point3d, maximumOrthogonalDistance)); point3d.set(0.0, 2.0, -1.5 * maximumOrthogonalDistance); assertFalse(planarRegion.isPointInside(point3d, maximumOrthogonalDistance)); assertTrue(planarRegion.isPointInside(point3d, maximumOrthogonalDistance));
/** * Find all the planar regions that contain the given point. * @param point the query coordinates. * @param maximumOrthogonalDistance tolerance expressed as maximum orthogonal distance from the region. * @return the list of planar regions containing the query. Returns null when no region contains the query. */ public List<PlanarRegion> findPlanarRegionsContainingPoint(Point3d point, double maximumOrthogonalDistance) { List<PlanarRegion> containers = null; for (int i = 0; i < regions.size(); i++) { PlanarRegion candidateRegion = regions.get(i); if (candidateRegion.isPointInside(point, maximumOrthogonalDistance)) { if (containers == null) containers = new ArrayList<>(); containers.add(candidateRegion); } } return containers; }
assertTrue(planarRegion.isPointInside(point2d)); point2d.setIncludingFrame(localFrame, 2.0, 0.0); assertTrue(planarRegion.isPointInside(point2d)); point2d.setIncludingFrame(localFrame, 0.0, 2.0); assertTrue(planarRegion.isPointInside(point2d)); point2d.setIncludingFrame(localFrame, 2.0, 2.0); assertFalse(planarRegion.isPointInside(point2d)); assertTrue(planarRegion.isPointInside(point3d, maximumOrthogonalDistance)); point3d.setIncludingFrame(localFrame, 2.0, 0.0, 0.0); point3d.changeFrame(worldFrame); assertTrue(planarRegion.isPointInside(point3d, maximumOrthogonalDistance)); point3d.setIncludingFrame(localFrame, 0.0, 2.0, 0.0); point3d.changeFrame(worldFrame); assertTrue(planarRegion.isPointInside(point3d, maximumOrthogonalDistance)); point3d.setIncludingFrame(localFrame, 2.0, 2.0, 0.0); point3d.changeFrame(worldFrame); assertFalse(planarRegion.isPointInside(point3d, maximumOrthogonalDistance)); assertTrue(planarRegion.isPointInside(point3d, maximumOrthogonalDistance)); point3d.setIncludingFrame(localFrame, 2.0, 0.0, -0.5 * maximumOrthogonalDistance); point3d.changeFrame(worldFrame); assertTrue(planarRegion.isPointInside(point3d, maximumOrthogonalDistance)); point3d.setIncludingFrame(localFrame, 0.0, 2.0, -0.5 * maximumOrthogonalDistance); point3d.changeFrame(worldFrame); assertTrue(planarRegion.isPointInside(point3d, maximumOrthogonalDistance)); point3d.setIncludingFrame(localFrame, 0.0, 0.0, -1.5 * maximumOrthogonalDistance); point3d.changeFrame(worldFrame); assertFalse(planarRegion.isPointInside(point3d, maximumOrthogonalDistance));
/** * Find all the planar regions that contain the given point. * * @param point the query coordinates. * @param maximumOrthogonalDistance tolerance expressed as maximum orthogonal distance from the * region. * @return the list of planar regions containing the query. Returns null when no region contains * the query. */ public List<PlanarRegion> findPlanarRegionsContainingPoint(Point3DReadOnly point, double maximumOrthogonalDistance) { List<PlanarRegion> containers = null; for (int i = 0; i < regions.size(); i++) { PlanarRegion candidateRegion = regions.get(i); if (candidateRegion.isPointInside(point, maximumOrthogonalDistance)) { if (containers == null) containers = new ArrayList<>(); containers.add(candidateRegion); } } return containers; }
/** * Given a 2D point expressed in the plane local frame, computes whether the point is in this * region. * * @param point2dInLocal query expressed in local coordinates. * @return true if the point is inside this region, false otherwise. */ public boolean isPointInside(Point2DReadOnly point2dInLocal) { return isPointInside(point2dInLocal.getX(), point2dInLocal.getY()); }
/** * Checks to see if a given point is on the plane or above it by the specified distance. * * @param point3dInWorld the point to check * @return True if the point is on the plane or no more than distanceFromPlane above it. */ public boolean isPointInWorld2DInside(Point3DReadOnly point3dInWorld) { Point3D localPoint = new Point3D(); fromWorldToLocalTransform.transform(point3dInWorld, localPoint); boolean isInsideXY = isPointInside(localPoint.getX(), localPoint.getY()); return isInsideXY; }
/** * Given a 3D point in world coordinates, computes whether the point is in this region. * @param point3dInWorld query expressed in world coordinates. * @param maximumOrthogonalDistance tolerance expressed as maximum orthogonal distance from the region. * @return true if the point is inside this region, false otherwise. */ public boolean isPointInside(Point3d point3dInWorld, double maximumOrthogonalDistance) { Point3d localPoint = new Point3d(); fromWorldToLocalTransform.transform(point3dInWorld, localPoint); if (!MathTools.isInsideBoundsInclusive(localPoint.getZ(), maximumOrthogonalDistance)) return false; else return isPointInside(localPoint.getX(), localPoint.getY()); }
/** * Given a 3D point in world coordinates, computes whether the point is in this region. * * @param point3dInWorld query expressed in world coordinates. * @param maximumOrthogonalDistance tolerance expressed as maximum orthogonal distance from the * region. * @return true if the point is inside this region, false otherwise. */ public boolean isPointInside(Point3DReadOnly point3dInWorld, double maximumOrthogonalDistance) { Point3D localPoint = new Point3D(); fromWorldToLocalTransform.transform(point3dInWorld, localPoint); if (!MathTools.intervalContains(localPoint.getZ(), maximumOrthogonalDistance)) return false; else return isPointInside(localPoint.getX(), localPoint.getY()); }
/** * Checks to see if a given point is on the plane or below it by the specified distance. * * @param point3dInWorld the point to check * @param distanceFromPlane The distance below the plane that the point is allowed to be * @return True if the point is on the plane or no more than distanceFromPlane below it. */ public boolean isPointOnOrSlightlyBelow(Point3d point3dInWorld, double distanceFromPlane) { MathTools.checkIfPositive(distanceFromPlane); Point3d localPoint = new Point3d(); fromWorldToLocalTransform.transform(point3dInWorld, localPoint); boolean onOrBelow = localPoint.getZ() <= 0.0; boolean withinDistance = localPoint.getZ() > (distanceFromPlane * -1.0); boolean isInsideXY = isPointInside(localPoint.getX(), localPoint.getY()); return onOrBelow && withinDistance && isInsideXY; }
/** * Checks to see if a given point is on the plane or above it by the specified distance. * * @param point3dInWorld the point to check * @param distanceFromPlane The distance above the plane that the point is allowed to be * @return True if the point is on the plane or no more than distanceFromPlane above it. */ public boolean isPointOnOrSlightlyAbove(Point3d point3dInWorld, double distanceFromPlane) { MathTools.checkIfPositive(distanceFromPlane); Point3d localPoint = new Point3d(); fromWorldToLocalTransform.transform(point3dInWorld, localPoint); boolean onOrAbove = localPoint.getZ() >= 0.0; boolean withinDistance = localPoint.getZ() < distanceFromPlane; boolean isInsideXY = isPointInside(localPoint.getX(), localPoint.getY()); return onOrAbove && withinDistance && isInsideXY; }
/** * Checks to see if a given point is on the plane or above it by the specified distance. * * @param point3dInWorld the point to check * @param distanceFromPlane The distance above the plane that the point is allowed to be * @return True if the point is on the plane or no more than distanceFromPlane above it. */ public boolean isPointOnOrSlightlyAbove(Point3DReadOnly point3dInWorld, double distanceFromPlane) { MathTools.checkPositive(distanceFromPlane); Point3D localPoint = new Point3D(); fromWorldToLocalTransform.transform(point3dInWorld, localPoint); boolean onOrAbove = localPoint.getZ() >= 0.0; boolean withinDistance = localPoint.getZ() < distanceFromPlane; boolean isInsideXY = isPointInside(localPoint.getX(), localPoint.getY()); return onOrAbove && withinDistance && isInsideXY; }
/** * Checks to see if a given point is on the plane or below it by the specified distance. * * @param point3dInWorld the point to check * @param distanceFromPlane The distance below the plane that the point is allowed to be * @return True if the point is on the plane or no more than distanceFromPlane below it. */ public boolean isPointOnOrSlightlyBelow(Point3DReadOnly point3dInWorld, double distanceFromPlane) { MathTools.checkPositive(distanceFromPlane); Point3D localPoint = new Point3D(); fromWorldToLocalTransform.transform(point3dInWorld, localPoint); boolean onOrBelow = localPoint.getZ() <= 0.0; boolean withinDistance = localPoint.getZ() > (distanceFromPlane * -1.0); boolean isInsideXY = isPointInside(localPoint.getX(), localPoint.getY()); return onOrBelow && withinDistance && isInsideXY; }
/** * Computes if the point is in the region projected onto the world xy-plane. * @param x x-coordinate of the query. * @param y y-coordinate of the query. * @return true if the point is inside this region, false otherwise. */ public boolean isPointInsideByProjectionOntoXYPlane(double x, double y) { Point3d localPoint = new Point3d(); localPoint.setX(x); localPoint.setY(y); localPoint.setZ(getPlaneZGivenXY(x, y)); fromWorldToLocalTransform.transform(localPoint); return isPointInside(localPoint.getX(), localPoint.getY()); }
/** * Computes if the point is in the region projected onto the world xy-plane. * * @param x x-coordinate of the query. * @param y y-coordinate of the query. * @return true if the point is inside this region, false otherwise. */ public boolean isPointInsideByProjectionOntoXYPlane(double x, double y) { Point3D localPoint = new Point3D(); localPoint.setX(x); localPoint.setY(y); localPoint.setZ(getPlaneZGivenXY(x, y)); fromWorldToLocalTransform.transform(localPoint); return isPointInside(localPoint.getX(), localPoint.getY()); }
assertTrue(planarRegion.isPointInside(new Point3D(0.0, 0.0, 0.0), 1e-7)); assertTrue(planarRegion.isPointInside(new Point3D(0.0, 0.0, 0.0), 1e-7)); assertFalse(planeZGivenXYIsNaN); assertTrue(valueMatchesComputed); assertTrue(planarRegion.isPointInside(new Point3D(0.0, 0.0, 0.0), 1e-7)); assertTrue(planeZGivenXYIsNaN); assertFalse(valueMatchesComputed); assertTrue(planarRegion.isPointInside(new Point3D(0.0, 0.0, 0.0), 1e-7)); assertTrue(planarRegion.isPointInside(new Point3D(0.0, 0.0, 0.5), 1e-7));
/** * Will return the intersection point between a line and a single planar region. If the line does * not intersect the region this method will return null. */ public static Point3D intersectRegionWithLine(PlanarRegion region, Line3D projectionLineInWorld) { RigidBodyTransform regionToWorld = new RigidBodyTransform(); region.getTransformToWorld(regionToWorld); Vector3DReadOnly planeNormal = new Vector3D(0.0, 0.0, 1.0); Point3DReadOnly pointOnPlane = new Point3D(region.getConvexPolygon(0).getVertex(0)); Point3DBasics pointOnLineInLocal = new Point3D(projectionLineInWorld.getPoint()); Vector3DBasics directionOfLineInLocal = new Vector3D(projectionLineInWorld.getDirection()); pointOnLineInLocal.applyInverseTransform(regionToWorld); directionOfLineInLocal.applyInverseTransform(regionToWorld); Point3D intersectionWithPlaneInLocal = EuclidGeometryTools.intersectionBetweenLine3DAndPlane3D(pointOnPlane, planeNormal, pointOnLineInLocal, directionOfLineInLocal); if (intersectionWithPlaneInLocal == null) { return null; } if (region.isPointInside(intersectionWithPlaneInLocal.getX(), intersectionWithPlaneInLocal.getY())) { intersectionWithPlaneInLocal.applyTransform(regionToWorld); return intersectionWithPlaneInLocal; } return null; }