@Override public double distance(FrameConvexPolygon2d convexPolygon) { checkReferenceFrameMatch(convexPolygon); return this.line.distance(convexPolygon.convexPolygon); }
@Override public void set(FrameLine2d frameLine2d) { checkReferenceFrameMatch(frameLine2d); this.line.set(frameLine2d.line); }
@Override public double distance(FrameLine2d secondLine) { checkReferenceFrameMatch(secondLine); return this.line.distance(secondLine.line); }
public void set(FramePoint2d[] endpoints) { checkReferenceFrameMatch(endpoints[0]); checkReferenceFrameMatch(endpoints[1]); this.line.set(endpoints[0].getPoint(), endpoints[1].getPoint()); }
public void set(FramePoint2d endpoint0, FramePoint2d endpoint1) { checkReferenceFrameMatch(endpoint0); checkReferenceFrameMatch(endpoint1); this.line.set(endpoint0.getPoint(), endpoint1.getPoint()); }
public void set(FramePoint2d startPoint, FrameVector2d vector) { checkReferenceFrameMatch(startPoint); checkReferenceFrameMatch(vector); this.line.set(startPoint.getPoint(), vector.getVector()); }
public void getIntersectionWithLine(FrameLine2d line, FramePoint2d intersectionToPack) { checkReferenceFrameMatch(line); checkReferenceFrameMatch(intersectionToPack); this.line.intersectionWith(line.getLine2d(), intersectionToPack.getPoint()); }
@Override public double distance(FramePoint2d point) { checkReferenceFrameMatch(point); return this.line.distance(point.getPoint()); }
public boolean areLinesPerpendicular(FrameLine2d line) { checkReferenceFrameMatch(line); return this.line.areLinesPerpendicular(line.getLine2d()); }
public boolean isPointBehindLine(FramePoint2d point) { checkReferenceFrameMatch(point); return this.line.isPointBehindLine(point.getPointCopy()); }
public void setParallelLineThroughPoint(FramePoint2d point) { checkReferenceFrameMatch(point); this.line.setParallelLineThroughPoint(point.getPointCopy()); }
public void setVector(FrameVector2d frameVector2d) { checkReferenceFrameMatch(frameVector2d); line.getNormalizedVector().set(frameVector2d.getVector()); }
public void setPoint(FramePoint2d framePoint2d) { checkReferenceFrameMatch(framePoint2d); line.getPoint().set(framePoint2d.getPoint()); }
@Override public FramePoint2d intersectionWith(FrameLineSegment2d lineSegment) { checkReferenceFrameMatch(lineSegment); Point2d intersection = this.line.intersectionWith(lineSegment.lineSegment); return new FramePoint2d(lineSegment.getReferenceFrame(), intersection); }
public static int cutPolygonWithLine(FrameLine2d cuttingLine, FrameConvexPolygon2d polygonToCut, RobotSide sideOfLineToCut) { cuttingLine.checkReferenceFrameMatch(polygonToCut); return cutPolygonWithLine(cuttingLine.getLine2d(), polygonToCut.getConvexPolygon2d(), sideOfLineToCut); }
@Override public FramePoint2d intersectionWith(FrameLine2d secondLine) { checkReferenceFrameMatch(secondLine); Point2d intersection = this.line.intersectionWith(secondLine.line); return new FramePoint2d(secondLine.getReferenceFrame(), intersection); }
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 = line.orthogonalProjectionCopy(point.getPoint()); return new FramePoint2d(point.getReferenceFrame(), projected); }
public void setByProjectionOntoXYPlane(FramePoint startPoint, FramePoint endPoint) { checkReferenceFrameMatch(startPoint); checkReferenceFrameMatch(endPoint); line.getPoint().set(startPoint.getX(), startPoint.getY()); line.getNormalizedVector().set(endPoint.getX() - startPoint.getX(), endPoint.getY() - startPoint.getY()); }
@Override public void orthogonalProjection(FramePoint2d point) { checkReferenceFrameMatch(point); Point2d projected = line.orthogonalProjectionCopy(point.getPoint()); point.set(projected.getX(), projected.getY()); }