public NavigableRegion(PlanarRegion homeRegion) { this.homeRegion = homeRegion; homeRegion.getTransformToWorld(transformToWorld); }
public static boolean isPointInWorldInsidePlanarRegion(PlanarRegion planarRegion, Point3DReadOnly pointInWorldToCheck, double epsilon) { RigidBodyTransform transformToWorld = new RigidBodyTransform(); planarRegion.getTransformToWorld(transformToWorld); Point2D pointInLocalToCheck = new Point2D(pointInWorldToCheck); pointInLocalToCheck.applyInverseTransform(transformToWorld, false); return isPointInLocalInsidePlanarRegion(planarRegion, pointInLocalToCheck, epsilon); }
public static boolean isPlanarRegionIntersectingWithCircle(Point2DReadOnly circleOriginInWorld, double circleRadius, PlanarRegion query) { RigidBodyTransform transformToWorld = new RigidBodyTransform(); query.getTransformToWorld(transformToWorld); Point2D originInLocal = new Point2D(circleOriginInWorld); originInLocal.applyInverseTransform(transformToWorld, false); return query.getConvexHull().signedDistance(originInLocal) <= circleRadius; }
/** * From the local coordinates of the {@code regionB}, this method computes and return the minimum * z-coordinate among the vertices of {@code regionA}'s concave hull. * * @param regionA the query. Not modified. * @param regionB the reference. Not modified. * @return the height of the lowest vertex of {@code regionA} above {@code regionB}. The returned * value is negative if the lowest vertex is below {@code regionB}. */ public static double computeMinHeightOfRegionAAboveRegionB(PlanarRegion regionA, PlanarRegion regionB) { RigidBodyTransform transformFromBToWorld = new RigidBodyTransform(); regionB.getTransformToWorld(transformFromBToWorld); RigidBodyTransform transformFromAToB = new RigidBodyTransform(); regionA.getTransformToWorld(transformFromAToB); transformFromAToB.preMultiplyInvertOther(transformFromBToWorld); double minZ = Double.POSITIVE_INFINITY; for (int i = 0; i < regionA.getConvexHull().getNumberOfVertices(); i++) { Point3D vertexA3D = new Point3D(regionA.getConcaveHull()[i]); transformFromAToB.transform(vertexA3D); minZ = Math.min(minZ, vertexA3D.getZ()); } return minZ; }
public static boolean isPlanarRegionIntersectingWithCapsule(LineSegment3D capsuleSegmentInWorld, double capsuleRadius, PlanarRegion query) { RigidBodyTransform transformToWorld = new RigidBodyTransform(); query.getTransformToWorld(transformToWorld); Point3D firstEndPointInLocal = new Point3D(capsuleSegmentInWorld.getFirstEndpoint()); Point3D secondEndPointInLocal = new Point3D(capsuleSegmentInWorld.getSecondEndpoint()); firstEndPointInLocal.applyInverseTransform(transformToWorld); secondEndPointInLocal.applyInverseTransform(transformToWorld); Point2D[] concaveHull = query.getConcaveHull(); List<Point3D> convexPolygon3D = new ArrayList<>(); for (int i = 0; i < concaveHull.length; i++) convexPolygon3D.add(new Point3D(concaveHull[i])); double minDistanceToEdge = getDistanceFromLineSegment3DToConvexPolygon(firstEndPointInLocal, secondEndPointInLocal, convexPolygon3D); return minDistanceToEdge <= capsuleRadius; }
private Node createConvexPolygonGraphics(PlanarRegion data) { List<ConvexPolygon2D> convexPolygons = new ArrayList<>(); for (int i = 0; i < data.getNumberOfConvexPolygons(); i++) convexPolygons.add(data.getConvexPolygon(i)); JavaFXMultiColorMeshBuilder meshBuilder = new JavaFXMultiColorMeshBuilder(new TextureColorAdaptivePalette(64)); int regionId = data.getRegionId(); RigidBodyTransform rigidBodyTransform = new RigidBodyTransform(); data.getTransformToWorld(rigidBodyTransform); Color regionColor = OcTreeMeshBuilder.getRegionColor(regionId); for (int i = 0; i < convexPolygons.size(); i++) { ConvexPolygon2D convexPolygon = convexPolygons.get(i); Color color = Color.hsb(regionColor.getHue(), 0.9, 0.5 + 0.5 * ((double) i / (double) convexPolygons.size())); meshBuilder.addPolygon(rigidBodyTransform, convexPolygon, color); } MeshView meshView = new MeshView(meshBuilder.generateMesh()); meshView.setMaterial(meshBuilder.generateMaterial()); return meshView; }
private RigidBodyTransform getWiggleTransformInWorldFrame(RigidBodyTransform wiggleTransformLocalToLocal) { RigidBodyTransform wiggleTransformWorldToWorld = new RigidBodyTransform(); RigidBodyTransform localToWorld = new RigidBodyTransform(); planarRegionToPack.getTransformToWorld(localToWorld); RigidBodyTransform worldToLocal = new RigidBodyTransform(localToWorld); worldToLocal.invert(); wiggleTransformWorldToWorld.set(localToWorld); wiggleTransformWorldToWorld.multiply(wiggleTransformLocalToLocal); wiggleTransformWorldToWorld.multiply(worldToLocal); return wiggleTransformWorldToWorld; }
private Node createConcaveHullGraphics(PlanarRegion data) { int regionId = data.getRegionId(); JavaFXMultiColorMeshBuilder meshBuilder = new JavaFXMultiColorMeshBuilder(new TextureColorAdaptivePalette(16)); RigidBodyTransform transform = new RigidBodyTransform(); data.getTransformToWorld(transform); Point2D[] concaveHullVerticesLocal = data.getConcaveHull(); Color regionColor = OcTreeMeshBuilder.getRegionColor(regionId); List<Point3D> concaveHullVertices = Arrays.stream(concaveHullVerticesLocal).map(Point3D::new).map(p -> { transform.transform(p); return p; }).collect(Collectors.toList()); for (int vertexIndex = 0; vertexIndex < concaveHullVertices.size(); vertexIndex++) { Point3D vertex = concaveHullVertices.get(vertexIndex); Point3D nextVertex = ListWrappingIndexTools.getNext(vertexIndex, concaveHullVertices); Color lineColor = Color.hsb(regionColor.getHue(), regionColor.getSaturation(), regionColor.getBrightness()); meshBuilder.addLine(vertex, nextVertex, 0.0015, lineColor); } MeshView meshView = new MeshView(meshBuilder.generateMesh()); meshView.setMaterial(meshBuilder.generateMaterial()); meshView.setMouseTransparent(true); return meshView; }
private void buildMeshAndMaterial(PlanarRegionsList planarRegionsList) { if (VERBOSE) PrintTools.info(this, "Creating mesh and material for new planar regions."); RigidBodyTransform transformToWorld = new RigidBodyTransform(); List<MeshView> regionMeshViews = new ArrayList<>(); for (int regionIndex = 0; regionIndex < planarRegionsList.getNumberOfPlanarRegions(); regionIndex++) { JavaFXMeshBuilder meshBuilder = new JavaFXMeshBuilder(); PlanarRegion planarRegion = planarRegionsList.getPlanarRegion(regionIndex); int regionId = planarRegion.getRegionId(); planarRegion.getTransformToWorld(transformToWorld); meshBuilder.addMultiLine(transformToWorld, Arrays.asList(planarRegion.getConcaveHull()), VisualizationParameters.CONCAVEHULL_LINE_THICKNESS, true); for (int polygonIndex = 0; polygonIndex < planarRegion.getNumberOfConvexPolygons(); polygonIndex++) { ConvexPolygon2D convexPolygon2d = planarRegion.getConvexPolygon(polygonIndex); meshBuilder.addPolygon(transformToWorld, convexPolygon2d); } MeshView regionMeshView = new MeshView(meshBuilder.generateMesh()); regionMeshView.setMaterial(new PhongMaterial(getRegionColor(regionId))); regionMeshViews.add(regionMeshView); } graphicsToRender.set(regionMeshViews); }
@Test(timeout = 30000) @ContinuousIntegrationTest(estimatedDuration = 0.0) public void testTruncatePlanarRegionIfIntersectingWithPlane() throws Exception { Point3D groundOrigin = new Point3D(); Vector3D groundNormal = new Vector3D(0.0, 0.0, 1.0); Point3D squareOrigin = new Point3D(0.0, 0.0, -0.001); Vector3D squareNormal = new Vector3D(0.0, -1.0, 0.0); AxisAngle squareOrientation = EuclidGeometryTools.axisAngleFromZUpToVector3D(squareNormal); RigidBodyTransform squarePose = new RigidBodyTransform(squareOrientation, squareOrigin); double squareSide = 4.0; Point2D[] concaveHullVertices = {new Point2D(0.0, 0.0), new Point2D(0.0, squareSide), new Point2D(squareSide, squareSide), new Point2D(squareSide, 0.0)}; List<ConvexPolygon2D> convexPolygons = new ArrayList<>(); convexPolygons.add(new ConvexPolygon2D(Vertex2DSupplier.asVertex2DSupplier(concaveHullVertices))); PlanarRegion verticalSquare = new PlanarRegion(squarePose, concaveHullVertices, convexPolygons); Point3D[] expectedVerticesInWorld = Arrays.stream(concaveHullVertices).map(p -> toWorld(p, squarePose)).toArray(Point3D[]::new); expectedVerticesInWorld[0].addZ(0.001); expectedVerticesInWorld[3].addZ(0.001); PlanarRegion truncatedSquare = PlanarRegionTools.truncatePlanarRegionIfIntersectingWithPlane(groundOrigin, groundNormal, verticalSquare, 0.05, null); RigidBodyTransform truncatedTransform = new RigidBodyTransform(); truncatedSquare.getTransformToWorld(truncatedTransform); EuclidCoreTestTools.assertRigidBodyTransformGeometricallyEquals(squarePose, truncatedTransform, EPSILON); Point3D[] actualVerticesInWorld = Arrays.stream(truncatedSquare.getConcaveHull()).map(p -> toWorld(p, squarePose)).toArray(Point3D[]::new); assertEquals(expectedVerticesInWorld.length, actualVerticesInWorld.length); for (int i = 0; i < expectedVerticesInWorld.length; i++) EuclidCoreTestTools.assertPoint3DGeometricallyEquals(expectedVerticesInWorld[i], actualVerticesInWorld[i], EPSILON); }
/** * Transforms footPolygonInRegionFrame from planarRegion frame to the snapped footstep node frame * * @param planarRegion * @param footstepNode * @param snapTransform * @param footPolygonInRegionFrame */ public static void changeFromPlanarRegionToSoleFrame(PlanarRegion planarRegion, FootstepNode footstepNode, RigidBodyTransform snapTransform, ConvexPolygon2D footPolygonInRegionFrame) { RigidBodyTransform regionToWorld = new RigidBodyTransform(); planarRegion.getTransformToWorld(regionToWorld); RigidBodyTransform soleTransform = new RigidBodyTransform(); FootstepNodeTools.getSnappedNodeTransform(footstepNode, snapTransform, soleTransform); RigidBodyTransform regionToSole = new RigidBodyTransform(); regionToSole.setAndInvert(soleTransform); regionToSole.multiply(regionToWorld); footPolygonInRegionFrame.applyTransform(regionToSole, false); }
@Override public void setPlanarRegions(PlanarRegionsList planarRegions) { super.setPlanarRegions(planarRegions); if (!hasPlanarRegions()) return; planarRegionPolytopes.clear(); for (PlanarRegion planarRegion : planarRegions.getPlanarRegionsAsList()) { ConvexPolytope planarRegionPolytope = new ConvexPolytope(); List<? extends Point2DReadOnly> pointsInPlanarRegion = planarRegion.getConvexHull().getVertexBufferView(); planarRegion.getTransformToWorld(tempTransform); for (Point2DReadOnly point : pointsInPlanarRegion) { tempPoint.setToZero(); tempPoint.set(point.getX(), point.getY(), 0.0); tempPoint.applyTransform(tempTransform); planarRegionPolytope.addVertex(tempPoint.getX(), tempPoint.getY(), tempPoint.getZ()); } planarRegionPolytopes.put(planarRegion, planarRegionPolytope); } }
/** * Adds a PlanarRegion transforming from the current coordinate system. * Uses the given appearances in order, one for each Polygon in the PlanarRegion. Then loops on the appearances. * * @param planarRegion */ public void addPlanarRegion(PlanarRegion planarRegion, AppearanceDefinition... appearances) { int numberOfConvexPolygons = planarRegion.getNumberOfConvexPolygons(); RigidBodyTransform transform = new RigidBodyTransform(); planarRegion.getTransformToWorld(transform); transform(transform); for (int i = 0; i < numberOfConvexPolygons; i++) { ConvexPolygon2d convexPolygon = planarRegion.getConvexPolygon(i); MeshDataHolder meshDataHolder = MeshDataGenerator.ExtrudedPolygon(convexPolygon, 0.005); addInstruction(new Graphics3DAddMeshDataInstruction(meshDataHolder, appearances[i % appearances.length])); } transform.invert(); transform(transform); }
/** * Adds a PlanarRegion transforming from the current coordinate system. * Uses the given appearances in order, one for each Polygon in the PlanarRegion. Then loops on the appearances. * * @param planarRegion */ public static void addPlanarRegion(Graphics3DObject graphics3DObject, PlanarRegion planarRegion, AppearanceDefinition... appearances) { int numberOfConvexPolygons = planarRegion.getNumberOfConvexPolygons(); RigidBodyTransform transform = new RigidBodyTransform(); planarRegion.getTransformToWorld(transform); graphics3DObject.transform(transform); for (int i = 0; i < numberOfConvexPolygons; i++) { ConvexPolygon2D convexPolygon = planarRegion.getConvexPolygon(i); MeshDataHolder meshDataHolder = MeshDataGenerator.ExtrudedPolygon(convexPolygon, -0.0001); graphics3DObject.addInstruction(new Graphics3DAddMeshDataInstruction(meshDataHolder, appearances[i % appearances.length])); } transform.invert(); graphics3DObject.transform(transform); }
public static void assertPlanarRegionsEqual(PlanarRegion expected, PlanarRegion actual, double epsilon) { RigidBodyTransform expectedTransform = new RigidBodyTransform(); RigidBodyTransform actualTransform = new RigidBodyTransform(); expected.getTransformToWorld(expectedTransform); actual.getTransformToWorld(actualTransform); EuclidCoreTestTools.assertRigidBodyTransformGeometricallyEquals(expectedTransform, actualTransform, epsilon); assertEquals(expected.getConcaveHullSize(), actual.getConcaveHullSize()); for (int i = 0; i < expected.getConcaveHullSize(); i++) { EuclidCoreTestTools.assertTuple2DEquals(expected.getConcaveHullVertex(i), actual.getConcaveHullVertex(i), epsilon); } assertEquals(expected.getNumberOfConvexPolygons(), actual.getNumberOfConvexPolygons()); for (int i = 0; i < expected.getNumberOfConvexPolygons(); i++) { ConvexPolygon2D expectedConvexPolygon = expected.getConvexPolygon(i); ConvexPolygon2D actualConvexPolygon = actual.getConvexPolygon(i); assertEquals(expectedConvexPolygon.getNumberOfVertices(), actualConvexPolygon.getNumberOfVertices()); assertTrue(expectedConvexPolygon.epsilonEquals(actualConvexPolygon, epsilon)); } } }
public static Cluster createHomeRegionCluster(PlanarRegion homeRegion, NavigableExtrusionDistanceCalculator calculator) { RigidBodyTransform transformToWorld = new RigidBodyTransform(); homeRegion.getTransformToWorld(transformToWorld); Cluster cluster = new Cluster(); cluster.setType(Type.POLYGON); cluster.setTransformToWorld(transformToWorld); cluster.addRawPointsInLocal2D(homeRegion.getConcaveHull()); cluster.setExtrusionSide(ExtrusionSide.INSIDE); double extrusionDistance = calculator.computeExtrusionDistance(homeRegion); ObstacleExtrusionDistanceCalculator nonNavigableCalculator = (p, h) -> extrusionDistance - NAV_TO_NON_NAV_DISTANCE; ObstacleExtrusionDistanceCalculator navigableCalculator = (p, h) -> extrusionDistance; boolean extrudeToTheLeft = cluster.getExtrusionSide() != ExtrusionSide.INSIDE; cluster.addNonNavigableExtrusionsInLocal(extrudePolygon(extrudeToTheLeft, cluster, nonNavigableCalculator)); cluster.addNavigableExtrusionsInLocal(extrudePolygon(extrudeToTheLeft, cluster, navigableCalculator)); cluster.updateBoundingBox(); return cluster; }
/** * 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; }
private Pair<Mesh, Material> generateMeshAndMaterial(PlanarRegionsListMessage newMessage) { meshBuilder.clear(); double lineWidth = 0.01; RigidBodyTransform transformToWorld = new RigidBodyTransform(); PlanarRegionsList planarRegionsList = PlanarRegionMessageConverter.convertToPlanarRegionsList(newMessage); for (int regionIndex = 0; regionIndex < planarRegionsList.getNumberOfPlanarRegions(); regionIndex++) { PlanarRegion planarRegion = planarRegionsList.getPlanarRegion(regionIndex); int regionId = planarRegion.getRegionId(); Color regionColor = getRegionColor(regionId); planarRegion.getTransformToWorld(transformToWorld); meshBuilder.addMultiLine(transformToWorld, planarRegion.getConcaveHull(), lineWidth, regionColor, true); for (int polygonIndex = 0; polygonIndex < planarRegion.getNumberOfConvexPolygons(); polygonIndex++) { ConvexPolygon2D convexPolygon2d = planarRegion.getConvexPolygon(polygonIndex); regionColor = Color.hsb(regionColor.getHue(), 0.9, 0.5 + 0.5 * ((double) polygonIndex / (double) planarRegion.getNumberOfConvexPolygons())); meshBuilder.addPolygon(transformToWorld, convexPolygon2d, regionColor); } } Material material = meshBuilder.generateMaterial(); Mesh mesh = meshBuilder.generateMesh(); return new Pair<>(mesh, material); }
private Pair<Mesh, Material> generateMeshAndMaterial(PlanarRegionsListMessage newMessage) { meshBuilder.clear(); double lineWidth = 0.01; RigidBodyTransform transformToWorld = new RigidBodyTransform(); PlanarRegionsList planarRegionsList = PlanarRegionMessageConverter.convertToPlanarRegionsList(newMessage); for (int regionIndex = 0; regionIndex < planarRegionsList.getNumberOfPlanarRegions(); regionIndex++) { PlanarRegion planarRegion = planarRegionsList.getPlanarRegion(regionIndex); int regionId = planarRegion.getRegionId(); Color regionColor = getRegionColor(regionId); planarRegion.getTransformToWorld(transformToWorld); meshBuilder.addMultiLine(transformToWorld, planarRegion.getConcaveHull(), lineWidth, regionColor, true); for (int polygonIndex = 0; polygonIndex < planarRegion.getNumberOfConvexPolygons(); polygonIndex++) { ConvexPolygon2D convexPolygon2d = planarRegion.getConvexPolygon(polygonIndex); regionColor = Color.hsb(regionColor.getHue(), 0.9, 0.5 + 0.5 * ((double) polygonIndex / (double) planarRegion.getNumberOfConvexPolygons())); meshBuilder.addPolygon(transformToWorld, convexPolygon2d, regionColor); } } Material material = meshBuilder.generateMaterial(); Mesh mesh = meshBuilder.generateMesh(); return new Pair<>(mesh, material); }
private boolean isOnBoundaryOfPlanarRegions(PlanarRegionsList planarRegionsList, FrameConvexPolygon2D footPolygonInWorld) { PoseReferenceFrame planarRegionFrame = new PoseReferenceFrame("PlanarRegionFrame", ReferenceFrame.getWorldFrame()); FrameConvexPolygon2D planarRegionPolygon = new FrameConvexPolygon2D(); ConvexPolygon2D planarRegionsBoundingPolygon = new ConvexPolygon2D(); planarRegionsBoundingPolygon.clear(); for(PlanarRegion region : planarRegionsList.getPlanarRegionsAsList()) { RigidBodyTransform transform = new RigidBodyTransform(); region.getTransformToWorld(transform); planarRegionFrame.setPoseAndUpdate(transform); planarRegionPolygon.set(region.getConvexHull()); planarRegionPolygon.setReferenceFrame(planarRegionFrame); planarRegionPolygon.changeFrameAndProjectToXYPlane(ReferenceFrame.getWorldFrame()); planarRegionsBoundingPolygon.addVertices(planarRegionPolygon); } planarRegionsBoundingPolygon.update(); for (int i = 0; i < footPolygonInWorld.getNumberOfVertices(); i++) { if (!planarRegionsBoundingPolygon.isPointInside(footPolygonInWorld.getVertex(i))) { return true; } } return false; }