public FrameLineSegment2d(FramePoint2d firstEndpoint, FramePoint2d secondEndpoint) { this(firstEndpoint.getReferenceFrame(), new LineSegment2d(firstEndpoint.getPointCopy(), secondEndpoint.getPointCopy())); firstEndpoint.checkReferenceFrameMatch(secondEndpoint); }
public FrameLine2d(FramePoint2d firstPointOnLine, FramePoint2d secondPointOnLine) { this(firstPointOnLine.getReferenceFrame(), new Line2d(firstPointOnLine.getPointCopy(), secondPointOnLine.getPointCopy())); firstPointOnLine.checkReferenceFrameMatch(secondPointOnLine); }
public FrameLineSegment2d(FramePoint2d[] endpoints) { this(endpoints[0].getReferenceFrame(), new LineSegment2d(endpoints[0].getPointCopy(), endpoints[1].getPointCopy())); endpoints[0].checkReferenceFrameMatch(endpoints[1]); }
public boolean isPointOnRightSideOfLineSegment(FramePoint2d point) { checkReferenceFrameMatch(point); return this.lineSegment.isPointOnRightSideOfLineSegment(point.getPointCopy()); }
public boolean containsEpsilon(FramePoint2d framePoint, double epsilon) { framePoint.checkReferenceFrameMatch(referenceFrame); return line.containsEpsilon(framePoint.getPointCopy(), epsilon); }
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()); }
/** * isPointInFrontOfLine * returns whether the point is in front of the line or not. The front * direction is defined as the positive x-direction * * @param point FramePoint2d * @return boolean */ public boolean isPointInFrontOfLine(FramePoint2d point) { checkReferenceFrameMatch(point); return this.line.isPointInFrontOfLine(point.getPointCopy()); }
public static FrameLine2d perpendicularLineThroughPoint(FrameLine2d line, FramePoint2d point) { line.checkReferenceFrameMatch(point); ReferenceFrame referenceFrame = line.referenceFrame; Line2d perpLine2d = line.line.perpendicularLineThroughPoint(point.getPointCopy()); return new FrameLine2d(referenceFrame, perpLine2d); }
public FrameLine2d(FramePoint2d framePoint2d, FrameVector2d frameVector2d) { this(framePoint2d.getReferenceFrame(), new Line2d(framePoint2d.getPointCopy(), frameVector2d.getVectorCopy())); framePoint2d.checkReferenceFrameMatch(frameVector2d); }
public FootstepVisualizer(String name, String graphicListName, RobotSide robotSide, ContactablePlaneBody contactableFoot, AppearanceDefinition footstepColor, YoGraphicsListRegistry yoGraphicsListRegistry, YoVariableRegistry registry) { this.robotSide = robotSide; yoFootstepPose = new YoFramePose(name + "Pose", worldFrame, registry); yoFoothold = new YoFrameConvexPolygon2d(name + "Foothold", "", worldFrame, maxNumberOfContactPoints, registry); double coordinateSystemSize = 0.2; double footholdScale = 1.0; poseViz = new YoGraphicCoordinateSystem(name + "Pose", yoFootstepPose, coordinateSystemSize, footstepColor); footholdViz = new YoGraphicPolygon(name + "Foothold", yoFoothold, yoFootstepPose, footholdScale, footstepColor); yoGraphicsListRegistry.registerYoGraphic(graphicListName, poseViz); yoGraphicsListRegistry.registerYoGraphic(graphicListName, footholdViz); List<FramePoint2d> contactPoints2d = contactableFoot.getContactPoints2d(); for (int i = 0; i < contactPoints2d.size(); i++) defaultContactPointsInSoleFrame.add(contactPoints2d.get(i).getPointCopy()); }
predictedContactPoints.add(point.getPointCopy());
for (FramePoint2d contactFramePoint : contactFramePoints) contactPoint = contactFramePoint.getPointCopy();