public void getPoint2d(Point2d origin) { line.getPoint(origin); }
public void getPointAndNormalizedVector(Point2d pointToPack, Vector2d normalizedVectorToPack) { getPoint(pointToPack); getNormalizedVector(normalizedVectorToPack); }
public void getFramePoint2d(FramePoint2d framePoint2d) { framePoint2d.setToZero(referenceFrame); line.getPoint(framePoint2d.tuple); }
public Line2d(Line2d line2d) { point.set(line2d.getPoint()); normalizedVector.set(line2d.getNormalizedVector()); }
public void setPoint(FramePoint2d framePoint2d) { checkReferenceFrameMatch(framePoint2d); line.getPoint().set(framePoint2d.getPoint()); }
/** * Checks if a line intersects the edge with the given index. */ public static boolean doesLineIntersectEdge(Line2d line, int edgeIndex, ConvexPolygon2d polygon) { if (!polygon.hasAtLeastTwoVertices()) return false; Point2d edgePointOne = polygon.getVertex(edgeIndex); Point2d edgePointTwo = polygon.getNextVertex(edgeIndex); double edgeVectorX = edgePointTwo.x - edgePointOne.x; double edgeVectorY = edgePointTwo.y - edgePointOne.y; double lambdaOne = getIntersectionLambda(edgePointOne.x, edgePointOne.y, edgeVectorX, edgeVectorY, line.getPoint().x, line.getPoint().y, line.getNormalizedVector().x, line.getNormalizedVector().y); if (Double.isNaN(lambdaOne) || lambdaOne < 0.0 || lambdaOne > 1.0) return false; return true; }
/** * Computes the points of intersection between the ray and the polygon and packs them into pointToPack1 and * pointToPack2. If there is only one intersection it will be stored in pointToPack1. Returns the number of * intersections found. The ray is given as a Line2d, where the start point of the ray is the point used to * specify the line and the direction of the ray is given by the direction of the line. */ public static int intersectionWithRay(Line2d ray, Point2d pointToPack1, Point2d pointToPack2, ConvexPolygon2d polygon) { int intersections = intersectionWithLine(ray, pointToPack1, pointToPack2, polygon); Point2d rayStart = ray.getPoint(); Vector2d rayDirection = ray.getNormalizedVector(); if (intersections == 2) { // check line intersection 2: if (!isPointInFrontOfRay(rayStart, rayDirection, pointToPack2)) intersections--; } if (intersections >= 1) { // check line intersection 1: if (!isPointInFrontOfRay(rayStart, rayDirection, pointToPack1)) { pointToPack1.set(pointToPack2); intersections--; } } return intersections; }
public void setFrameLine2d(FrameLine2d frameLine2d) { if (frameLine2d == null) { pointX.set(Double.NaN); pointY.set(Double.NaN); vectorX.set(Double.NaN); vectorY.set(Double.NaN); return; } frameLine2d.checkReferenceFrameMatch(referenceFrame); Line2d line = frameLine2d.getLine2d(); pointX.set(line.getPoint().getX()); pointY.set(line.getPoint().getY()); vectorX.set(line.getNormalizedVector().getX()); vectorY.set(line.getNormalizedVector().getY()); }
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()); }
/** * Packs the point on the polygon that is closest to the given ray. If the ray is parallel to the * closest edge this will return the point on that edge closest to the ray origin. If the ray * intersects the polygon the result of this method will be wrong. If unsure check first using the * intersectionWithRay method. */ public static boolean getClosestPointToRay(Line2d ray, Point2d pointToPack, ConvexPolygon2d polygon) { if (!polygon.hasAtLeastOneVertex()) return false; Point2d rayStart = ray.getPoint(); Vector2d rayDirection = ray.getNormalizedVector(); int closestVertexIndex = getClosestVertexIndex(ray, polygon); Point2d closestVertexToLine = polygon.getVertex(closestVertexIndex); // validate the closest vertex is in front of the ray: boolean vertexValid = isPointInFrontOfRay(rayStart, rayDirection, closestVertexToLine); // check edges adjacent to the closest vertex to determine if they are parallel to the ray: boolean edge1Parallel = isEdgeParallel(closestVertexIndex, rayDirection, polygon); boolean edge2Parallel = isEdgeParallel(polygon.getNextVertexIndex(closestVertexIndex), rayDirection, polygon); boolean rayParallelToEdge = edge1Parallel || edge2Parallel; if (vertexValid && !rayParallelToEdge) pointToPack.set(closestVertexToLine); else { pointToPack.set(rayStart); orthogonalProjection(pointToPack, polygon); } return true; }
public void drawLine(PlotterReferenceFrame frame, Line2d line) { PlotterPoint2d start = pointBin[0]; PlotterVector2d direction = vectorBin[0]; start.setIncludingFrame(frame, line.getPoint()); direction.setIncludingFrame(frame, line.getNormalizedVector()); start.changeFrame(screenFrame); direction.changeFrame(screenFrame); direction.normalize(); PlotterPoint2d farPointPositive = pointBin[1]; PlotterPoint2d farPointNegative = pointBin[2]; PlotterVector2d far = vectorBin[1]; far.setIncludingFrame(direction); far.scale(1e4); farPointPositive.setIncludingFrame(start); farPointNegative.setIncludingFrame(start); farPointPositive.add(far); farPointNegative.sub(far); drawLineSegment(pixelate(farPointNegative.getX()), pixelate(farPointNegative.getY()), pixelate(farPointPositive.getX()), pixelate(farPointPositive.getY())); }
public void drawLine(PlotterReferenceFrame frame, Line2d line) { PlotterPoint2d start = pointBin[0]; PlotterVector2d direction = vectorBin[0]; start.setIncludingFrame(frame, line.getPoint()); direction.setIncludingFrame(frame, line.getNormalizedVector()); start.changeFrame(screenFrame); direction.changeFrame(screenFrame); direction.normalize(); PlotterPoint2d farPointPositive = pointBin[1]; PlotterPoint2d farPointNegative = pointBin[2]; PlotterVector2d far = vectorBin[1]; far.setIncludingFrame(direction); far.scale(1e4); farPointPositive.setIncludingFrame(start); farPointNegative.setIncludingFrame(start); farPointPositive.add(far); farPointNegative.sub(far); drawLineSegment(pixelate(farPointNegative.getX()), pixelate(farPointNegative.getY()), pixelate(farPointPositive.getX()), pixelate(farPointPositive.getY())); }
double edgeVectorX = edgeEnd.x - edgeStart.x; double edgeVectorY = edgeEnd.y - edgeStart.y; Point2d lineStart = line.getPoint(); Vector2d lineDirection = line.getNormalizedVector(); double lambda = getIntersectionLambda(edgeStart.x, edgeStart.y, edgeVectorX, edgeVectorY, lineStart.x, lineStart.y, lineDirection.x,