public LineSegment2d getLineSegment2dCopy() { return new LineSegment2d(lineSegment); }
public FrameLineSegment2d(ReferenceFrame referenceFrame, Point2d firstEndpoint, Point2d secondEndpoint) { this(referenceFrame, new LineSegment2d(firstEndpoint, secondEndpoint)); }
public static LineSegment2d[] getIntersectingEdgesCopy(Line2d line, ConvexPolygon2d polygon) { LineSegment2d edge1 = new LineSegment2d(); LineSegment2d edge2 = new LineSegment2d(); int edges = getIntersectingEdges(line, edge1, edge2, polygon); if (edges == 2) return new LineSegment2d[] {edge1, edge2}; if (edges == 1) return new LineSegment2d[] {edge1}; return null; }
public static LineSegment2d getClosestEdgeCopy(Point2d point, ConvexPolygon2d polygon) { LineSegment2d ret = new LineSegment2d(); if (getClosestEdge(point, polygon, ret)) return ret; return null; } }
@Override public LineSegment2d applyTransformCopy(RigidBodyTransform transform) { LineSegment2d copy = new LineSegment2d(this); copy.applyTransform(transform); return copy; }
@Override public LineSegment2d applyTransformAndProjectToXYPlaneCopy(RigidBodyTransform transform) { LineSegment2d copy = new LineSegment2d(this); copy.applyTransformAndProjectToXYPlane(transform); return copy; }
public FrameLineSegment2d(ReferenceFrame referenceFrame) { super(referenceFrame, new LineSegment2d()); this.lineSegment = this.getGeometryObject(); }
public FrameLineSegment2d(FrameLineSegment2d frameLineSegment2d) { this(frameLineSegment2d.getReferenceFrame(), new LineSegment2d(frameLineSegment2d.lineSegment)); }
/** * Efficiently combines two Disjoint Polygons. Returns null if not disjoint. * Note: Generates garbage! * * @param polygon1 ConvexPolygon2d * @param polygon2 ConvexPolygon2d * @return ConvexPolygon2dAndConnectingEdges */ public static ConvexPolygon2dAndConnectingEdges combineDisjointPolygons(ConvexPolygon2d polygon1, ConvexPolygon2d polygon2) { ConvexPolygon2d combinedPolygon = new ConvexPolygon2d(); LineSegment2d connectingEdge1 = new LineSegment2d(); LineSegment2d connectingEdge2 = new LineSegment2d(); boolean success = combineDisjointPolygons(polygon1, polygon2, combinedPolygon, connectingEdge1, connectingEdge2); if (!success) return null; return new ConvexPolygon2dAndConnectingEdges(combinedPolygon, connectingEdge1, connectingEdge2); }
private Point2d getPointInPolygonNearestPoint(ConvexPolygon2d polygon, Point2d point2d) { LineSegment2d closestEdge = new LineSegment2d(); polygon.getClosestEdge(closestEdge, point2d); return closestEdge.getClosestPointOnLineSegmentCopy(point2d); } }
public FrameLineSegment2d(FramePoint2d[] endpoints) { this(endpoints[0].getReferenceFrame(), new LineSegment2d(endpoints[0].getPointCopy(), endpoints[1].getPointCopy())); endpoints[0].checkReferenceFrameMatch(endpoints[1]); }
public FrameLineSegment2d(FramePoint2d firstEndpoint, FramePoint2d secondEndpoint) { this(firstEndpoint.getReferenceFrame(), new LineSegment2d(firstEndpoint.getPointCopy(), secondEndpoint.getPointCopy())); firstEndpoint.checkReferenceFrameMatch(secondEndpoint); }
/** * Compute the 2D equivalent of this line segment. * The 2D equivalent of each end point is computed as follows: * {@code endPoint2d = endPoint1d * direction2d + zero2d}. * @param zero2d position of the 2D equivalent of an endpoint equal to zero. * @param direction2d direction toward greater values of {@code endPoint1d}. * @return the 2D equivalent of this line segment. */ public LineSegment2d toLineSegment2d(Point2d zero2d, Vector2d direction2d) { LineSegment2d lineSegment2d = new LineSegment2d(); lineSegment2d.getFirstEndpoint().scaleAdd(endpoint1, direction2d, zero2d); lineSegment2d.getSecondEndpoint().scaleAdd(endpoint2, direction2d, zero2d); return lineSegment2d; }
/** * Final phase helper method called if one range has a size of exactly one * @return The two points on each respective polygon which are closest to one another */ private static Point2d[] finalPhasePointAndEdge(Point2d edgePoint1, Point2d edgePoint2, Point2d lonePoint) { Point2d proj = GeometryTools.getOrthogonalProjectionOnLine(lonePoint, edgePoint1, edgePoint2); LineSegment2d p = new LineSegment2d(edgePoint1, edgePoint2); if (p.isBetweenEndpoints(proj, 0)) { return new Point2d[] {lonePoint, proj}; } else { return new Point2d[] {lonePoint, (lonePoint.distance(edgePoint1) < lonePoint.distance(edgePoint2)) ? edgePoint1 : edgePoint2}; } }
private static boolean computeIntersectionOfPolygonsIfOnePolygonHasExactlyTwoVerticesAndTheOtherHasAtLeastTwoVertices( ConvexPolygon2d polygonWithExactlyTwoVertices, ConvexPolygon2d polygonWithAtLeastTwoVertices, ConvexPolygon2d intersectingPolygon) { LineSegment2d polygonWithTwoVerticesAsLineSegment = new LineSegment2d(polygonWithExactlyTwoVertices.getVertex(0), polygonWithExactlyTwoVertices.getVertex(1)); Point2d[] intersection = polygonWithAtLeastTwoVertices.intersectionWith(polygonWithTwoVerticesAsLineSegment); if (intersection == null) { intersectingPolygon.clearAndUpdate(); return false; } else { intersectingPolygon.setAndUpdate(intersection, intersection.length); return true; } }
private LineSegment2d shiftAndCopy(boolean shiftToLeft, double distanceToShift) { double vectorX = endpoints[1].getX() - endpoints[0].getX(); double vectorY = endpoints[1].getY() - endpoints[0].getY(); double vectorXPerpToRight = -vectorY; double vectorYPerpToRight = vectorX; if (!shiftToLeft) { vectorXPerpToRight = -vectorXPerpToRight; vectorYPerpToRight = -vectorYPerpToRight; } double vectorPerpToRightLength = Math.sqrt(vectorXPerpToRight * vectorXPerpToRight + vectorYPerpToRight * vectorYPerpToRight); vectorXPerpToRight = distanceToShift * vectorXPerpToRight / vectorPerpToRightLength; vectorYPerpToRight = distanceToShift * vectorYPerpToRight / vectorPerpToRightLength; Point2d newEndpoint0 = new Point2d(endpoints[0].getX() + vectorXPerpToRight, endpoints[0].getY() + vectorYPerpToRight); Point2d newEndpoint1 = new Point2d(endpoints[1].getX() + vectorXPerpToRight, endpoints[1].getY() + vectorYPerpToRight); LineSegment2d ret = new LineSegment2d(newEndpoint0, newEndpoint1); return ret; }
LineSegment2d edge1 = new LineSegment2d(edgePoint1A, edgePoint1B); LineSegment2d edge2 = new LineSegment2d(edgePoint2A, edgePoint2B); Point2d proj1AOnto2 = GeometryTools.getOrthogonalProjectionOnLine(edgePoint1A, edgePoint2A, edgePoint2B); Point2d proj1BOnto2 = GeometryTools.getOrthogonalProjectionOnLine(edgePoint1B, edgePoint2A, edgePoint2B);
LineSegment2d p = new LineSegment2d(polygon1.getVertex(v1Start), polygon1.getVertex(v1End)); if (p.isBetweenEndpoints(proj, 0))
LineSegment2d lineSegmentInWorldFrame = new LineSegment2d(); Point3d pointOneInWorldFrame = new Point3d(); Point3d pointTwoInWorldFrame = new Point3d();
/** * Projects the input LineSegment2d to the plane defined by this PlanarRegion by translating each vertex in world z. * Then puts each vertex in local frame. In doing so, the length of the rotated lineSegment will actually increase on tilted PlanarRegions. * @param lineSegmentInWorld LineSegment2d to project * @return new projected LineSegment2d */ private LineSegment2d projectLineSegmentVerticallyToRegion(LineSegment2d lineSegmentInWorld) { Point2d[] snappedEndpoints = new Point2d[2]; Point2d originalVertex = lineSegmentInWorld.getFirstEndpoint(); Point3d snappedVertex3d = new Point3d(); // Find the vertex 3d that is snapped to the plane following z-world. snappedVertex3d.setX(originalVertex.getX()); snappedVertex3d.setY(originalVertex.getY()); snappedVertex3d.setZ(getPlaneZGivenXY(originalVertex.getX(), originalVertex.getY())); // Transform to local coordinates fromWorldToLocalTransform.transform(snappedVertex3d); snappedEndpoints[0] = new Point2d(snappedVertex3d.getX(), snappedVertex3d.getY()); originalVertex = lineSegmentInWorld.getSecondEndpoint(); // Find the vertex 3d that is snapped to the plane following z-world. snappedVertex3d.setX(originalVertex.getX()); snappedVertex3d.setY(originalVertex.getY()); snappedVertex3d.setZ(getPlaneZGivenXY(originalVertex.getX(), originalVertex.getY())); // Transform to local coordinates fromWorldToLocalTransform.transform(snappedVertex3d); snappedEndpoints[1] = new Point2d(snappedVertex3d.getX(), snappedVertex3d.getY()); LineSegment2d projectedLineSegment = new LineSegment2d(snappedEndpoints); return projectedLineSegment; }