private static void writePlanarRegionVertices(Path folderPath, PlanarRegion region, int regionIndex) throws IOException { File regionFile = new File(folderPath.toFile(), "region" + region.getRegionId() + "_" + regionIndex); FileWriter fileWriter = new FileWriter(regionFile); for (Point2D vertex : region.getConcaveHull()) { fileWriter.write(vertex.getX() + ", " + vertex.getY() + "\n"); } for (int polygonIndex = 0; polygonIndex < region.getNumberOfConvexPolygons(); polygonIndex++) { ConvexPolygon2D convexPolygon = region.getConvexPolygon(polygonIndex); for (int vertexIndex = 0; vertexIndex < convexPolygon.getNumberOfVertices(); vertexIndex++) { Point2DReadOnly vertex = convexPolygon.getVertex(vertexIndex); fileWriter.write(vertex.getX() + ", " + vertex.getY() + "\n"); } } fileWriter.close(); }
public void addPolygons(ArrayList<ConvexPolygon2d> polygons) { PlanarRegion planarRegion = new PlanarRegion(transformGenerator.getRigidBodyTransformCopy(), polygons); planarRegion.setRegionId(id++); planarRegionsList.addPlanarRegion(planarRegion); }
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)); } } }
@Override public double heightAt(double x, double y, double z) { if (planarRegion.isPointInsideByProjectionOntoXYPlane(x, y)) { return planarRegion.getPlaneZGivenXY(x, y); } else { return 0.0; } }
/** * Returns the last convex polygon representing a portion of this region. Special case: returns * null when this region is empty. The polygon is expressed in the region local coordinates. */ public ConvexPolygon2D getLastConvexPolygon() { if (isEmpty()) return null; else return getConvexPolygon(getNumberOfConvexPolygons() - 1); }
/** * Returns the last convex polygon representing a portion of this region and removes it from this * planar region. Special case: returns null when this region is empty. The polygon is expressed * in the region local coordinates. */ public ConvexPolygon2D pollLastConvexPolygon() { if (isEmpty()) return null; else return pollConvexPolygon(getNumberOfConvexPolygons() - 1); }
regionA.getTransformToWorld(transformA); boolean foundMatchingRegion = false; regionB.getTransformToWorld(transformB); if (transformA.epsilonEquals(transformB, 0.01)) ConvexPolygon2D newHull = new ConvexPolygon2D(regionA.getConvexHull(), regionB.getConvexHull()); PlanarRegion combinedRegion = new PlanarRegion(transformA, newHull); combinedRegion.setRegionId(regionA.getRegionId()); ret.addPlanarRegion(combinedRegion); foundMatchingRegion = true; PlanarRegion region = new PlanarRegion(transformA, new ConvexPolygon2D(regionA.getConvexHull())); region.setRegionId(regionA.getRegionId()); ret.addPlanarRegion(region);
planarRegionToTruncate.getPointInRegion(pointOnRegion); planarRegionToTruncate.getNormal(regionNormal); planarRegionToTruncate.getTransformToWorld(transformFromRegionToWorld); pointOnPlaneInRegionFrame.applyInverseTransform(transformFromRegionToWorld); planeNormalInRegionFrame.applyInverseTransform(transformFromRegionToWorld); Point2DReadOnly vertex2D = planarRegionToTruncate.getConcaveHullVertex(planarRegionToTruncate.getConcaveHullSize() - 1); Point3D vertex3D = new Point3D(vertex2D); double previousSignedDistance = signedDistanceFromPoint3DToPlane3D(vertex3D, pointOnPlaneInRegionFrame, planeNormalInRegionFrame); double epsilonDistance = 1.0e-10; for (int i = 0; i < planarRegionToTruncate.getConcaveHullSize(); i++) vertex2D = planarRegionToTruncate.getConcaveHullVertex(i); vertex3D = new Point3D(vertex2D); PlanarRegion truncatedRegion = new PlanarRegion(transformFromRegionToWorld, concaveHullVertices, truncatedConvexPolygons); truncatedRegion.setRegionId(planarRegionToTruncate.getRegionId()); if (filter == null || filter.isPlanarRegionRelevant(truncatedRegion)) return truncatedRegion;
public static PlanarRegionMessage convertToPlanarRegionMessage(PlanarRegion planarRegion) int regionId = planarRegion.getRegionId(); Point3f regionOrigin = new Point3f(); Vector3f regionNormal = new Vector3f(); planarRegion.getPointInRegion(regionOrigin); planarRegion.getNormal(regionNormal); for (int hullIndex = 0; hullIndex < planarRegion.getNumberOfConcaveHulls(); hullIndex++) Point2d[] hullVertices = planarRegion.getConcaveHull(hullIndex); Point2f[] messageHullVertices = new Point2f[hullVertices.length]; for (int polygonIndex = 0; polygonIndex < planarRegion.getNumberOfConvexPolygons(); polygonIndex++) ConvexPolygon2d convexPolygon = planarRegion.getConvexPolygon(polygonIndex); Point2f[] vertices = new Point2f[convexPolygon.getNumberOfVertices()]; for (int vertexIndex = 0; vertexIndex < convexPolygon.getNumberOfVertices(); vertexIndex++)
PlanarRegion planarRegion = new PlanarRegion(regionTransform, regionConvexPolygons); BoundingBox3D boundingBox3dInWorld = planarRegion.getBoundingBox3dInWorld(); RigidBodyTransform transformToWorld = new RigidBodyTransform(); planarRegion.getTransformToWorld(transformToWorld);
public static double computePlanarRegionArea(PlanarRegion planarRegion) { double area = 0.0; for (int i = 0; i < planarRegion.getNumberOfConvexPolygons(); i++) { area += planarRegion.getConvexPolygon(i).getArea(); } return area; }
private static void writePlanarRegionsData(Path folderPath, PlanarRegionsList planarRegionData) throws IOException { File header = new File(folderPath.toFile(), "header.txt"); FileWriter fileWriter = new FileWriter(header); Map<Integer, MutableInt> regionIdToIndex = new HashMap<>(); for (PlanarRegion region : planarRegionData.getPlanarRegionsAsList()) { Point3D origin = new Point3D(); Vector3D normal = new Vector3D(); region.getPointInRegion(origin); region.getNormal(normal); int numberOfConvexPolygons = region.getNumberOfConvexPolygons(); int[] convexPolygonsSizes = new int[numberOfConvexPolygons]; for (int i = 0; i < numberOfConvexPolygons; i++) convexPolygonsSizes[i] = region.getConvexPolygon(i).getNumberOfVertices(); int regionId = region.getRegionId(); MutableInt regionIndex = regionIdToIndex.getOrDefault(regionId, new MutableInt(0)); regionIdToIndex.put(regionId, regionIndex); regionIndex.increment(); fileWriter.write("regionId: " + Integer.toString(regionId)); fileWriter.write(", index: " + Integer.toString(regionIndex.getValue().intValue())); fileWriter.write(", origin: " + origin.getX() + ", " + origin.getY() + ", " + origin.getZ()); fileWriter.write(", normal: " + normal.getX() + ", " + normal.getY() + ", " + normal.getZ()); fileWriter.write(", concave hull size: " + region.getConcaveHullSize()); fileWriter.write(", number of convex polygons: " + numberOfConvexPolygons + ", " + Arrays.toString(convexPolygonsSizes)); fileWriter.write("\n"); writePlanarRegionVertices(folderPath, region, regionIndex.intValue()); } fileWriter.close(); }
/** * 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); }
/** * @return a full depth copy of this region. The copy can be entirely modified without * interfering with this region. */ public PlanarRegion copy() { RigidBodyTransform transformToWorldCopy = new RigidBodyTransform(fromLocalToWorldTransform); Point2D[] concaveHullCopy = new Point2D[concaveHullsVertices.length]; for (int i = 0; i < concaveHullsVertices.length; i++) concaveHullCopy[i] = new Point2D(concaveHullsVertices[i]); List<ConvexPolygon2D> convexPolygonsCopy = new ArrayList<>(); for (int i = 0; i < getNumberOfConvexPolygons(); i++) convexPolygonsCopy.add(new ConvexPolygon2D(convexPolygons.get(i))); PlanarRegion planarRegion = new PlanarRegion(transformToWorldCopy, concaveHullCopy, convexPolygonsCopy); planarRegion.setRegionId(regionId); return planarRegion; }
private static void checkPlanarRegionsEqual(int regionId, PlanarRegion planarRegionA, PlanarRegion planarRegionB) { Point3D centerA = new Point3D(); Point3D centerB = new Point3D(); planarRegionA.getPointInRegion(centerA); planarRegionB.getPointInRegion(centerB); Vector3D normalA = new Vector3D(); Vector3D normalB = new Vector3D(); planarRegionA.getNormal(normalA); planarRegionB.getNormal(normalB); EuclidCoreTestTools.assertPoint3DGeometricallyEquals("Center of regions " + regionId + " are not equal.", centerA, centerB, epsilon); EuclidCoreTestTools.assertVector3DGeometricallyEquals("Normal of regions " + regionId + " are not equal.", normalA, normalB, epsilon); assertEquals("Number of convex polygons of " + regionId + " not equal. ", planarRegionA.getNumberOfConvexPolygons(), planarRegionB.getNumberOfConvexPolygons()); for (int i = 0; i < planarRegionA.getNumberOfConvexPolygons(); i++) { assertTrue("Convex polygon " + i + " of planar region " + regionId + " is not equal", planarRegionA.getConvexPolygon(i).epsilonEquals(planarRegionB.getConvexPolygon(i), epsilon)); } }
@ContinuousIntegrationTest(estimatedDuration = 5.0) @Test(timeout = 30000) public void testHeightAt() throws Exception { Random random = new Random(1776L); for (int i = 0; i < 100000; i++) { PlanarRegion planarRegion = PlanarRegion.generatePlanarRegionFromRandomPolygonsWithRandomTransform(random, random.nextInt(10), RandomNumbers.nextDouble(random, 0.0, 30.0), random.nextInt(10)); BoundingBox3D boundingBox3dInWorld = planarRegion.getBoundingBox3dInWorld(); double randomXCoord = RandomNumbers.nextDouble(random, boundingBox3dInWorld.getMinX() - 10.0, boundingBox3dInWorld.getMaxX() + 10.0); double randomYCoord = RandomNumbers.nextDouble(random, boundingBox3dInWorld.getMinY() - 10.0, boundingBox3dInWorld.getMaxY() + 10.0); double randomZCoord = RandomNumbers.nextDouble(random, boundingBox3dInWorld.getMinZ() - 10.0, boundingBox3dInWorld.getMaxZ() + 10.0); PlanarRegionTerrainObject terrainObject = new PlanarRegionTerrainObject(planarRegion, DEFAULT_ALLOWABLE_PENETRATION_THICKNESS); double planarRegionZAtXY = planarRegion.getPlaneZGivenXY(randomXCoord, randomYCoord); if (planarRegion.isPointInsideByProjectionOntoXYPlane(randomXCoord, randomYCoord)) { assertEquals(planarRegionZAtXY, terrainObject.heightAt(randomXCoord, randomYCoord, randomZCoord), 1e-10); } else { assertNotEquals(planarRegionZAtXY, terrainObject.heightAt(randomXCoord, randomYCoord, randomZCoord), 1e-10); } } }
@ContinuousIntegrationTest(estimatedDuration = 0.4) @Test(timeout = 30000) public void testIsPointOnOrSlightlyBelow() { Random random = new Random(1776L); RigidBodyTransform transformToWorld = new RigidBodyTransform(); Point3D regionTranslation = new Point3D(); Point3D pointAbove = new Point3D(); Point3D pointBelow = new Point3D(); Vector3D regionNormal = new Vector3D(); for (int i = 0; i < 10000; i++) { PlanarRegion planarRegion = PlanarRegion.generatePlanarRegionFromRandomPolygonsWithRandomTransform(random, 1, 10.0, 5); planarRegion.getTransformToWorld(transformToWorld); Point2DReadOnly centroid = planarRegion.getLastConvexPolygon().getCentroid(); regionTranslation.set(centroid.getX(), centroid.getY(), 0.0); transformToWorld.transform(regionTranslation); regionTranslation.setZ(planarRegion.getPlaneZGivenXY(regionTranslation.getX(), regionTranslation.getY())); planarRegion.getNormal(regionNormal); regionNormal.normalize(); regionNormal.scale(1e-6); pointAbove.add(regionTranslation, regionNormal); pointBelow.sub(regionTranslation, regionNormal); assertTrue(planarRegion.isPointOnOrSlightlyBelow(pointBelow, 1e-5)); assertFalse(planarRegion.isPointOnOrSlightlyBelow(pointAbove, 1e-5)); } }
if (planarRegionToProcess.isEmpty()) polygonToProcess = planarRegionToProcess.pollLastConvexPolygon(); planarRegionToProcess.getTransformToWorld(regionTransform); currentRegionPose.setPose(regionTransform); currentRegionId.set(planarRegionToProcess.getRegionId()); vertexIndexOffset++; polygonToProcess = planarRegionToProcess.getLastConvexPolygon(); polygonToProcess = planarRegionToProcess.pollLastConvexPolygon(); isDonePackingPolygons = false; if (planarRegionToProcess.isEmpty()) planarRegionsListToProcess.pollLastPlanarRegion(); isPlanarRegionsListComplete.set(planarRegionsListToProcess.isEmpty());
Plane3D plane = planarRegion.getPlane(); Point3D closestPoint = plane.orthogonalProjectionCopy(walkerPosition3D); planarRegion.transformFromWorldToLocal(walkerShapeLocal); walkerPosition3D.set(walkerShapeLocal.getPosition()); Point2D walkerPosition2D = new Point2D(walkerPosition3D); if (planarRegion.getNumberOfConvexPolygons() == 0) List<Point2DReadOnly> concaveHullVertices = new ArrayList<>(Arrays.asList(planarRegion.getConcaveHull())); double depthThreshold = 0.05; List<ConvexPolygon2D> convexPolygons = new ArrayList<>(); for (int i = 0; i < planarRegion.getNumberOfConvexPolygons(); i++) ConvexPolygon2D convexPolygon = planarRegion.getConvexPolygon(i); Point2DBasics closestPoint2D = convexPolygon.orthogonalProjectionCopy(walkerPosition2D); if (closestPoint2D == null) planarRegion.transformFromLocalToWorld(intersectionWorld); errorMessages += fail(datasetName, "Body path is going through a region at: " + intersectionWorld); collisionsToPack.add(intersectionWorld);
@ContinuousIntegrationTest(estimatedDuration = 0.6) @Test(timeout = 30000) public void testIsPointOn() { Random random = new Random(1776L); RigidBodyTransform transformToWorld = new RigidBodyTransform(); Point3D regionTranslation = new Point3D(); Point3D pointAbove = new Point3D(); Point3D pointBelow = new Point3D(); Vector3D regionNormal = new Vector3D(); for (int i = 0; i < 10000; i++) { PlanarRegion planarRegion = PlanarRegion.generatePlanarRegionFromRandomPolygonsWithRandomTransform(random, 1, 10.0, 5); planarRegion.getTransformToWorld(transformToWorld); Point2DReadOnly centroid = planarRegion.getLastConvexPolygon().getCentroid(); regionTranslation.set(centroid.getX(), centroid.getY(), 0.0); transformToWorld.transform(regionTranslation); regionTranslation.setZ(planarRegion.getPlaneZGivenXY(regionTranslation.getX(), regionTranslation.getY())); planarRegion.getNormal(regionNormal); regionNormal.normalize(); regionNormal.scale(1e-6); pointAbove.add(regionTranslation, regionNormal); pointBelow.sub(regionTranslation, regionNormal); assertTrue(planarRegion.isPointOnOrSlightlyAbove(pointAbove, 1e-5)); assertFalse(planarRegion.isPointOnOrSlightlyAbove(pointBelow, 1e-5)); } }