How to use

PlanarRegion

in

us.ihmc.robotics.geometry

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));
 }
}