How to use us.ihmc.robotics.geometry

private void addRectangle(RigidBodyTransformGenerator transformGenerator, double lengthX, double widthY)
{
 ConvexPolygon2D rectangle = createRectanglePolygon(lengthX, widthY);
 PlanarRegion planarRegion = new PlanarRegion(transformGenerator.getRigidBodyTransformCopy(), rectangle);
 planarRegion.setRegionId(id++);
 planarRegionsList.addPlanarRegion(planarRegion);
}

public FrameLineSegment2d getClosestEdgeCopy(FramePoint2d point)
{
 checkReferenceFrameMatch(point);
 return new FrameLineSegment2d(referenceFrame, convexPolygon.getClosestEdgeCopy(point.getPoint()));
}

@Override
public FramePoint2d intersectionWith(FrameLineSegment2d lineSegment)
{
 checkReferenceFrameMatch(lineSegment);
 Point2d intersection = this.line.intersectionWith(lineSegment.lineSegment);
 return new FramePoint2d(lineSegment.getReferenceFrame(), intersection);
}

public void setIncludingFrame(FramePoint2d startPoint, FrameVector2d vector)
{
 startPoint.checkReferenceFrameMatch(vector);
 this.referenceFrame = startPoint.getReferenceFrame();
 this.line.set(startPoint.getPoint(), vector.getVector());
}

public void set(FramePoint firstEndpoint, FrameVector fromFirstToSecondEndpoint)
{
 checkReferenceFrameMatch(firstEndpoint);
 checkReferenceFrameMatch(fromFirstToSecondEndpoint);
 lineSegment3d.set(firstEndpoint.getPoint(), fromFirstToSecondEndpoint.getVector());
}

public FrameLine2d(FramePoint2d framePoint2d, FrameVector2d frameVector2d)
{
 this(framePoint2d.getReferenceFrame(), new Line2d(framePoint2d.getPointCopy(), frameVector2d.getVectorCopy()));
 framePoint2d.checkReferenceFrameMatch(frameVector2d);
}

@Override
public FramePoint2d intersectionWith(FrameLine2d line)
{
 checkReferenceFrameMatch(line);
 Point2d intersection = this.lineSegment.intersectionWith(line.line);
 if (intersection == null)
 {
   return null;
 }
 return new FramePoint2d(line.getReferenceFrame(), intersection);
}

public void set(FramePoint2d firstEndpoint, FrameVector2d vectorToSecondEndpoint)
{
 checkReferenceFrameMatch(firstEndpoint);
 checkReferenceFrameMatch(vectorToSecondEndpoint);
 this.lineSegment.set(firstEndpoint.getPoint(), vectorToSecondEndpoint.getVector());
}

public boolean isPointInFrontOfLine(FrameVector2d frontDirection, FramePoint2d framePoint)
{
 checkReferenceFrameMatch(frontDirection);
 checkReferenceFrameMatch(framePoint);
 
 return line.isPointInFrontOfLine(frontDirection.getVector(), framePoint.getPoint());
}

public double getDistance(FramePoint framePoint)
{
 checkReferenceFrameMatch(framePoint);
 return lineSegment3d.distance(framePoint.getPoint());
}

public void setVector(FrameVector vector)
{
 checkReferenceFrameMatch(vector);
 
 line.setVector(vector.getVector());
}

public void set(FramePoint2d[] endpoints)
{
 checkReferenceFrameMatch(endpoints[0]);
 checkReferenceFrameMatch(endpoints[1]);
 this.line.set(endpoints[0].getPoint(), endpoints[1].getPoint());
}

public double percentageAlongLineSegment(FramePoint2d point2d)
{
 checkReferenceFrameMatch(point2d);
 return lineSegment.percentageAlongLineSegment(point2d.getPoint());
}

@Override
public double distance(FramePoint2d point)
{
 checkReferenceFrameMatch(point);
 return this.line.distance(point.getPoint());
}

public boolean isPointBehindLine(FramePoint2d point)
{
 checkReferenceFrameMatch(point);
 return this.line.isPointBehindLine(point.getPointCopy());
}

public boolean isPointOnRightSideOfLineSegment(FramePoint2d point)
{
 checkReferenceFrameMatch(point);
 return this.lineSegment.isPointOnRightSideOfLineSegment(point.getPointCopy());
}

public int[] getLineOfSightVertexIndicesCopy(FramePoint2d observer)
{
 checkReferenceFrameMatch(observer);
 return ConvexPolygon2dCalculator.getLineOfSightVertexIndicesCopy(observer.getPoint(), convexPolygon);
}

private void addRectangle(RigidBodyTransformGenerator transformGenerator, double lengthX, double widthY)
{
 ConvexPolygon2d rectangle = createRectanglePolygon(lengthX, widthY);
 PlanarRegion planarRegion = new PlanarRegion(transformGenerator.getRigidBodyTransformCopy(), rectangle);
 planarRegion.setRegionId(id++);
 planarRegionsList.addPlanarRegion(planarRegion);
}

public void addPolygon(RigidBodyTransformGenerator transformGenerator, ConvexPolygon2D polygon)
{
 PlanarRegion planarRegion = new PlanarRegion(transformGenerator.getRigidBodyTransformCopy(), polygon);
 planarRegion.setRegionId(id++);
 planarRegionsList.addPlanarRegion(planarRegion);
}

public void addPolygons(ArrayList<ConvexPolygon2d> polygons)
{
 PlanarRegion planarRegion = new PlanarRegion(transformGenerator.getRigidBodyTransformCopy(), polygons);
 planarRegion.setRegionId(id++);
 planarRegionsList.addPlanarRegion(planarRegion);
}