How to use

getPoint

method

in

us.ihmc.robotics.geometry.FramePoint2d

public void orthogonalProjection(FramePoint2d point, FramePoint2d projectedPointToPack)
{
 checkReferenceFrameMatch(point);
 projectedPointToPack.setToZero(referenceFrame);
 lineSegment.orthogonalProjection(point.getPoint(), projectedPointToPack.getPoint());
}

public void set(FramePoint2d endpoint0, FramePoint2d endpoint1)
{
 checkReferenceFrameMatch(endpoint0);
 checkReferenceFrameMatch(endpoint1);
 this.line.set(endpoint0.getPoint(), endpoint1.getPoint());
}

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

public boolean isBetweenEndpoints(FramePoint2d point2d, double epsilon)
{
 checkReferenceFrameMatch(point2d);
 return lineSegment.isBetweenEndpoints(point2d.getPoint(), epsilon);
}

/**
* Add a vertex to this polygon.
* Note that this method recycles memory.
* @param newVertex {@code FramePoint2d} the new vertex (it is not modified).
* @throws ReferenceFrameMismatchException
*/
public void addVertex(FramePoint2d vertex)
{
 vertex.checkReferenceFrameMatch(referenceFrame);
 addVertex(vertex.getPoint());
}

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

/**
* There is actually no ray class at the moment, so we use a FrameLine2d.
*
* TODO: Make ray classes and use them. @dcalvert
*/
public void intersectWithRay(FrameConvexPolygon2d frameConvexPolygon2d, FrameLine2d frameRay2d)
{
 checkAndSetFrames(frameConvexPolygon2d, frameRay2d);
 int intersectionTypeInt = ConvexPolygon2dCalculator.intersectionWithRay(frameRay2d.getLine2d(), intersectionPointOne.getPoint(),
                                      intersectionPointTwo.getPoint(), frameConvexPolygon2d.getConvexPolygon2d());
 packIntersectionType(intersectionTypeInt);
}

/**
* Add a vertex to this polygon after doing {@code newVertex.changeFrameAndProjectToXYPlane(this.referenceFrame)}.
* Note that this method recycles memory.
* @param newVertex {@code FramePoint2d} the new vertex (it is not modified).
*/
public void addVertexChangeFrameAndProjectToXYPlane(FramePoint2d newVertex)
{
 tempPoint2d.setIncludingFrame(newVertex);
 tempPoint2d.changeFrameAndProjectToXYPlane(referenceFrame);
 convexPolygon.addVertex(tempPoint2d.getPoint());
}

public void getClosestPointOnLineSegment(FramePoint2d framePoint2dToPack, FramePoint2d point)
{
 checkReferenceFrameMatch(point);
 
 lineSegment.getClosestPointOnLineSegment(tempPoint2d, point.getPoint());
 framePoint2dToPack.setIncludingFrame(referenceFrame, tempPoint2d);
}

public void setPoint(FramePoint2d framePoint2d)
{
 checkReferenceFrameMatch(framePoint2d);
 
 line.getPoint().set(framePoint2d.getPoint());
}

public void getClosestVertex(FramePoint2d closestVertexToPack, FramePoint2d point)
{
 point.checkReferenceFrameMatch(referenceFrame);
 closestVertexToPack.setIncludingFrame(referenceFrame, ConvexPolygon2dCalculator.getClosestVertexCopy(point.getPoint(), convexPolygon));
}

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

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

public boolean getClosestPointWithRay(FramePoint2d closestVertexToPack, FrameLine2d ray)
{
 ray.checkReferenceFrameMatch(referenceFrame);
 closestVertexToPack.setToZero(referenceFrame);
 boolean success = convexPolygon.getClosestPointWithRay(closestVertexToPack.getPoint(), ray.getLine2d());
 return success;
}

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

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

public void getIntersectionWithLine(FrameLine2d line, FramePoint2d intersectionToPack)
{
 checkReferenceFrameMatch(line);
 checkReferenceFrameMatch(intersectionToPack);
 
 this.line.intersectionWith(line.getLine2d(), intersectionToPack.getPoint());
}

@Override
public void orthogonalProjection(FramePoint2d point)
{
 checkReferenceFrameMatch(point);
 Point2d projected = line.orthogonalProjectionCopy(point.getPoint());
 point.set(projected.getX(), projected.getY());
}