public YoFrameLineSegment2d(DoubleYoVariable firstEndpointX, DoubleYoVariable firstEndpointY, DoubleYoVariable secondEndpointX, DoubleYoVariable secondEndpointY, ReferenceFrame frame) { this.firstEndpointX = firstEndpointX; this.firstEndpointY = firstEndpointY; this.secondEndpointX = secondEndpointX; this.secondEndpointY = secondEndpointY; this.referenceFrame = frame; frameLineSegment = new FrameLineSegment2d(referenceFrame); }
@Override public FrameLineSegment2d changeFrameAndProjectToXYPlaneCopy(ReferenceFrame desiredFrame) { FrameLineSegment2d copy = new FrameLineSegment2d(this); copy.changeFrameAndProjectToXYPlane(desiredFrame); return copy; }
@Override public FrameLineSegment2d applyTransformAndProjectToXYPlaneCopy(RigidBodyTransform transform) { FrameLineSegment2d copy = new FrameLineSegment2d(this); copy.applyTransformAndProjectToXYPlane(transform); return copy; }
@Override public FrameLineSegment2d applyTransformCopy(RigidBodyTransform transform) { FrameLineSegment2d copy = new FrameLineSegment2d(this); copy.applyTransform(transform); return copy; }
public FrameLineSegment2d getFrameLineSegment2d() { putYoValuesIntoFrameLineSegment(); return new FrameLineSegment2d(frameLineSegment); }
public FrameLineSegment2d shiftToLeftCopy(double distanceToShift) { return new FrameLineSegment2d(referenceFrame, lineSegment.shiftToLeftCopy(distanceToShift)); }
public FrameLineSegment2d shiftToRightCopy(double distanceToShift) { return new FrameLineSegment2d(referenceFrame, lineSegment.shiftToRightCopy(distanceToShift)); }
public static FrameLineSegment2d generateRandomFrameLineSegment2d(Random random, ReferenceFrame zUpFrame, double xMin, double xMax, double yMin, double yMax) { FramePoint2d randomPoint1 = FramePoint2d.generateRandomFramePoint2d(random, zUpFrame, xMin, xMax, yMin, yMax); FramePoint2d randomPoint2 = FramePoint2d.generateRandomFramePoint2d(random, zUpFrame, xMin, xMax, yMin, yMax); return new FrameLineSegment2d(randomPoint1, randomPoint2); }
public FrameLineSegment2d getClosestEdgeCopy(FramePoint2d point) { checkReferenceFrameMatch(point); return new FrameLineSegment2d(referenceFrame, convexPolygon.getClosestEdgeCopy(point.getPoint())); }
public YoFrameLineSegment2d(String namePrefix, String nameSuffix, String description, ReferenceFrame frame, YoVariableRegistry registry) { firstEndpointX = new DoubleYoVariable(namePrefix + "FirstEndpointX" + nameSuffix, description, registry); firstEndpointY = new DoubleYoVariable(namePrefix + "FirstEndpointY" + nameSuffix, description, registry); secondEndpointX = new DoubleYoVariable(namePrefix + "SecondEndpointX" + nameSuffix, description, registry); secondEndpointY = new DoubleYoVariable(namePrefix + "SecondEndpointY" + nameSuffix, description, registry); this.referenceFrame = frame; frameLineSegment = new FrameLineSegment2d(referenceFrame); }
/** * Returns the intersecting edges between this polygon and the given FrameLine2d. * The FrameLine2d is treated as a ray. This method returns null if: * - The start point of the ray starts inside the Polygon, * - The ray points away from the polygon. * - The intersection is a single vertex, * - There is no intersection. * * @param frameLine2d FrameLine2d Ray to check intersection with this Polygon. * @return FrameLineSegment2d[] The two edges that the Ray intersects on this Polygon. */ public FrameLineSegment2d[] getIntersectingEdges(FrameLine2d frameLine2d) { frameLine2d.checkReferenceFrameMatch(referenceFrame); LineSegment2d[] lineSegments = ConvexPolygon2dCalculator.getIntersectingEdgesCopy(frameLine2d.getLine2dCopy(), convexPolygon); if (lineSegments == null) return null; FrameLineSegment2d[] ret = new FrameLineSegment2d[lineSegments.length]; for (int i = 0; i < lineSegments.length; i++) { ret[i] = new FrameLineSegment2d(referenceFrame, lineSegments[i]); } return ret; }
private void updatedFinalHeadingLine(double desiredHeadingFinal) { FramePoint2d endpoint1 = processedSensors.getCenterOfMassGroundProjectionInFrame(ReferenceFrame.getWorldFrame()).toFramePoint2d(); FramePoint2d endpoint2 = new FramePoint2d(endpoint1); double length = 1.0; endpoint2.setX(endpoint2.getX() + length * Math.cos(desiredHeadingFinal)); endpoint2.setY(endpoint2.getY() + length * Math.sin(desiredHeadingFinal)); FrameLineSegment2d frameLineSegment2d = new FrameLineSegment2d(endpoint1, endpoint2); finalHeadingLine.setFrameLineSegment2d(frameLineSegment2d); } }
private void updatedDesiredHeadingLine(double desiredHeading) { FramePoint2d endpoint1 = processedSensors.getCenterOfMassGroundProjectionInFrame(ReferenceFrame.getWorldFrame()).toFramePoint2d(); FramePoint2d endpoint2 = new FramePoint2d(endpoint1); double length = 1.0; endpoint2.setX(endpoint2.getX() + length * Math.cos(desiredHeading)); endpoint2.setY(endpoint2.getY() + length * Math.sin(desiredHeading)); FrameLineSegment2d frameLineSegment2d = new FrameLineSegment2d(endpoint1, endpoint2); desiredHeadingLine.setFrameLineSegment2d(frameLineSegment2d); }
/** * generates garbage * @param polygon1 * @param polygon2 * @return */ //this should throw away points that are inside of the other polygon public static FrameConvexPolygon2dAndConnectingEdges combineDisjointPolygons(FrameConvexPolygon2d polygon1, FrameConvexPolygon2d polygon2) { ReferenceFrame referenceFrame = polygon1.getReferenceFrame(); polygon2.checkReferenceFrameMatch(referenceFrame); ConvexPolygon2dAndConnectingEdges polygonAndEdges = combineDisjointPolygons(polygon1.convexPolygon, polygon2.convexPolygon); if (polygonAndEdges == null) return null; // Return null if not disjoint FrameConvexPolygon2d frameConvexPolygon2d = new FrameConvexPolygon2d(referenceFrame, polygonAndEdges.getConvexPolygon2d()); FrameLineSegment2d connectingEdge1 = new FrameLineSegment2d(referenceFrame, polygonAndEdges.getConnectingEdge1()); FrameLineSegment2d connectingEdge2 = new FrameLineSegment2d(referenceFrame, polygonAndEdges.getConnectingEdge2()); FrameConvexPolygon2dAndConnectingEdges ret = new FrameConvexPolygon2dAndConnectingEdges(polygon1, polygon2, frameConvexPolygon2d, connectingEdge1, connectingEdge2); return ret; }
footPolygons.put(foot, footPolygon); FrameLineSegment2d tempLineSegment = new FrameLineSegment2d(new FramePoint2d(soleFrame), new FramePoint2d(soleFrame, 1.0, 1.0)); // TODO need to give distinct points that's not convenient footCenterCoPLineSegments.put(foot, tempLineSegment);
FrameLineSegment2d guideLineSegment = new FrameLineSegment2d(intersections); FrameVector2d frameVector2d = new FrameVector2d(); guideLineSegment.getFrameVector(frameVector2d);