How to use

checkReferenceFrameMatch

method

in

us.ihmc.robotics.geometry.FrameConvexPolygon2d

@Override
public double distance(FrameLine2d line)
{
 checkReferenceFrameMatch(line);
 return convexPolygon.distance(line.line);
}

@Override
public double distance(FrameLineSegment2d lineSegment)
{
 checkReferenceFrameMatch(lineSegment);
 return convexPolygon.distance(lineSegment.lineSegment);
}

@Override
public double distance(FrameConvexPolygon2d secondConvexPolygon)
{
 checkReferenceFrameMatch(secondConvexPolygon);
 return convexPolygon.distance(secondConvexPolygon.convexPolygon);
}

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

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

@Override
public void orthogonalProjection(FramePoint2d point)
{
 checkReferenceFrameMatch(point);
 convexPolygon.orthogonalProjection(point.getPoint());
}

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

/**
* Scale this convex polygon about pointToScaleAbout.
* @param pointToScaleAbout
* @param scaleFactor
*/
public void scale(FramePoint2d pointToScaleAbout, double scaleFactor)
{
 checkReferenceFrameMatch(pointToScaleAbout);
 scale(pointToScaleAbout.getPoint(), scaleFactor);
}

public void getClosestEdge(FrameLineSegment2d closestEdgeToPack, FramePoint2d point)
{
 checkReferenceFrameMatch(point);
 closestEdgeToPack.referenceFrame = referenceFrame;
 convexPolygon.getClosestEdge(closestEdgeToPack.lineSegment, point.getPoint());
}

  public static boolean combineDisjointPolygons(FrameConvexPolygon2d polygon1, FrameConvexPolygon2d polygon2, FrameConvexPolygon2d combinedPolygonToPack,
     FrameLineSegment2d connectingEdge1ToPack, FrameLineSegment2d connectingEdge2ToPack)
  {
   combinedPolygonToPack.clear(polygon1.getReferenceFrame());
   combinedPolygonToPack.checkReferenceFrameMatch(connectingEdge1ToPack);
   combinedPolygonToPack.checkReferenceFrameMatch(connectingEdge2ToPack);

   boolean success = combineDisjointPolygons(polygon1.convexPolygon, polygon2.convexPolygon, combinedPolygonToPack.convexPolygon,
      connectingEdge1ToPack.lineSegment, connectingEdge2ToPack.lineSegment);

   if (!success)
     return false;

//      combinedPolygonToPack.updateFramePoints();
   combinedPolygonToPack.update();

   return true;
  }

@Override
public FramePoint2d[] intersectionWith(FrameLineSegment2d lineSegment)
{
 //TODO: Memory inefficient. Don't create new objects...
 checkReferenceFrameMatch(lineSegment);
 Point2d[] intersection = this.convexPolygon.intersectionWith(lineSegment.lineSegment);
 if (intersection == null)
   return null;
 FramePoint2d[] ret = new FramePoint2d[intersection.length];
 for (int i = 0; i < intersection.length; i++)
 {
   ret[i] = new FramePoint2d(lineSegment.referenceFrame, intersection[i]);
 }
 return ret;
}

/**
* @deprecated Creates garbage. Use an instance of FrameConvexPolygonWithLineIntersector.
*/
@Override
public FramePoint2d[] intersectionWith(FrameLine2d line)
{
 checkReferenceFrameMatch(line);
 Point2d[] intersection = this.convexPolygon.intersectionWith(line.line);
 if (intersection == null)
   return null;
 FramePoint2d[] ret = new FramePoint2d[intersection.length];
 for (int i = 0; i < intersection.length; i++)
 {
   ret[i] = new FramePoint2d(line.referenceFrame, intersection[i]);
 }
 return ret;
}

public void setFrameConvexPolygon2d(FrameConvexPolygon2d polygon)
{
 if (polygon == null)
 {
   hide();
   setToNaN();
   return;
 }
 try
 {
   polygon.checkReferenceFrameMatch(referenceFrame);
   convexPolygon2dForWriting.setAndUpdate(polygon);
   getYoValuesFromFrameConvexPolygon2d();
 }
 catch (Exception e)
 {
   System.err.println("In YoFrameConvexPolygon2d.java: " + e.getClass().getSimpleName() + " while calling setConvexPolygon2d(FrameConvexPolygon2d).");
 }
}

@Override
public FrameConvexPolygon2d intersectionWith(FrameConvexPolygon2d secondConvexPolygon)
{
 checkReferenceFrameMatch(secondConvexPolygon);
 ConvexPolygon2d intersection = this.convexPolygon.intersectionWith(secondConvexPolygon.convexPolygon);
 if (intersection == null)
   return null;
 return new FrameConvexPolygon2d(secondConvexPolygon.getReferenceFrame(), intersection);
}

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

public boolean intersectionWith(FrameConvexPolygon2d secondConvexPolygon, FrameConvexPolygon2d intersectionToPack)
{
 checkReferenceFrameMatch(secondConvexPolygon);
 intersectionToPack.clear(secondConvexPolygon.getReferenceFrame());
 boolean success = convexPolygon.intersectionWith(secondConvexPolygon.convexPolygon, intersectionToPack.convexPolygon);
 return success;
}

public FramePoint2d[] intersectionWithRayCopy(FrameLine2d ray)
{
 checkReferenceFrameMatch(ray);
 Point2d[] intersections = convexPolygon.intersectionWithRayCopy(ray.getLine2dCopy());
 if (intersections == null)
   return null;
 FramePoint2d[] ret = new FramePoint2d[intersections.length];
 for (int i = 0; i < intersections.length; i++)
 {
   ret[i] = new FramePoint2d(referenceFrame, intersections[i]);
 }
 return ret;
}

@Override
public FramePoint2d orthogonalProjectionCopy(FramePoint2d point)
{
 checkReferenceFrameMatch(point);
 Point2d projected = convexPolygon.orthogonalProjectionCopy(point.getPoint());
 if (projected == null)
 {
   return null;
 }
 return new FramePoint2d(point.getReferenceFrame(), projected);
}

public FramePoint2d[] getLineOfSightVerticesCopy(FramePoint2d observer)
{
 checkReferenceFrameMatch(observer);
 Point2d[] vertices = ConvexPolygon2dCalculator.getLineOfSightVerticesCopy(observer.getPoint(), convexPolygon);
 FramePoint2d point1 = new FramePoint2d(getReferenceFrame(), vertices[0]);
 FramePoint2d point2 = new FramePoint2d(getReferenceFrame(), vertices[1]);
 return new FramePoint2d[] {point1, point2};
}

public void intersectionWith(FrameLine2d otherLine, Pair<FramePoint2d, FramePoint2d> intersection)
{
 checkReferenceFrameMatch(otherLine);
 int numberOfIntersections = ConvexPolygon2dCalculator.intersectionWithLine(otherLine.getLine2d(), intersection.getFirst().getPoint(),
                                       intersection.getSecond().getPoint(), convexPolygon);
 if (numberOfIntersections < 2)
 {
   intersection.getSecond().setToNaN();
   if (numberOfIntersections < 1)
    intersection.getFirst().setToNaN();
 }
}