public FrameConvexPolygonWithLineIntersector2d() { intersectionPointOne = new FramePoint2d(); intersectionPointTwo = new FramePoint2d(); intersectionResult = IntersectionResult.NO_INTERSECTION; }
/** * Changes frame of this FramePoint2d to the given ReferenceFrame, projects into xy plane, and returns a copy. * * @param desiredFrame ReferenceFrame to change the FramePoint2d into. * @return Copied FramePoint2d in the new reference frame. */ public FramePoint2d changeFrameAndProjectToXYPlaneCopy(ReferenceFrame desiredFrame) { FramePoint2d ret = new FramePoint2d(this); ret.changeFrameAndProjectToXYPlane(desiredFrame); return ret; }
public FramePoint2d getFrameVertexCopy(int vertexIndex) { FramePoint2d frameVertexCopy = new FramePoint2d(); getFrameVertex(vertexIndex, frameVertexCopy); return frameVertexCopy; }
public FramePose2d(ReferenceFrame referenceFrame) { this.position = new FramePoint2d(referenceFrame); this.orientation = new FrameOrientation2d(referenceFrame); this.referenceFrame = referenceFrame; }
public FramePose2d(ReferenceFrame referenceFrame, Point2d position, double orientation) { this.position = new FramePoint2d(referenceFrame, position); this.orientation = new FrameOrientation2d(referenceFrame, orientation); this.referenceFrame = referenceFrame; }
public SphereWithConvexPolygonIntersector() { closestPointOnPolygon = new FramePoint(); closestPointOnPolygon2d = new FramePoint2d(); }
public FramePoint2d getCentroidCopy() { FramePoint2d centroidToReturn = new FramePoint2d(); getCentroid(centroidToReturn); return centroidToReturn; }
@Override public FramePoint2d[] intersectionWith(FrameConvexPolygon2d convexPolygon) { checkReferenceFrameMatch(convexPolygon); Point2d[] intersection = this.lineSegment.intersectionWith(convexPolygon.convexPolygon); FramePoint2d[] ret = new FramePoint2d[intersection.length]; for (int i = 0; i < intersection.length; i++) { ret[i] = new FramePoint2d(convexPolygon.referenceFrame, intersection[i]); } return ret; }
public FramePoint2d getClosestVertexCopy(FrameLine2d line) { line.checkReferenceFrameMatch(referenceFrame); Point2d closestVertexCopy = ConvexPolygon2dCalculator.getClosestVertexCopy(line.line, convexPolygon); if (closestVertexCopy == null) throw new RuntimeException("Closest vertex could not be found! Has at least one vertex: " + convexPolygon.hasAtLeastOneVertex()); return new FramePoint2d(referenceFrame, closestVertexCopy); }
public StraightLineOverheadPath(FramePose2d startPose, FramePoint2d endPosition) { startPose.checkReferenceFrameMatch(endPosition); this.startPoint = new FramePoint2d(); startPose.getPosition(startPoint); this.endPoint = new FramePoint2d(endPosition); this.orientation = new FrameOrientation2d(); startPose.getOrientation(orientation); this.distance = endPoint.distance(startPoint); }
public FramePoint2d[] getLineOfSightVerticesCopy(FramePoint2d observer) { checkReferenceFrameMatch(observer); Point2d[] vertices = ConvexPolygon2dCalculator.getLineOfSightVerticesCopy(observer.getPoint(), convexPolygon); FramePoint2d point1 = new FramePoint2d(getReferenceFrame(), vertices[0]); FramePoint2d point2 = new FramePoint2d(getReferenceFrame(), vertices[1]); return new FramePoint2d[] {point1, point2}; }
public FramePoint2d getClosestVertexCopy(FramePoint2d point) { point.checkReferenceFrameMatch(referenceFrame); return new FramePoint2d(referenceFrame, ConvexPolygon2dCalculator.getClosestVertexCopy(point.getPoint(), convexPolygon)); }
@Override public String toString() { FramePoint2d vertex = new FramePoint2d(); String ret = ""; for (int i = 0; i < getNumberOfVertices(); i++) { this.getFrameVertex(i, vertex); ret = ret + vertex.toString() + "\n"; } return ret; }
public void updatePointAndPolygon(FrameConvexPolygon2d polygon, FramePoint2d pointInAnyFrame) { FramePoint2d temp = new FramePoint2d(pointInAnyFrame); temp.changeFrame(polygon.getReferenceFrame()); updatePointAndPolygon(polygon, temp.getPoint()); }
private void addTurnInPlaceToFacePoint(ArrayList<FramePose2d> footstepList, FramePose2d robotPose, FramePoint2d goalPoint) { double turningAngle = AngleTools.calculateHeading(robotPose, goalPoint, -robotPose.getYaw(), 0.0); FramePoint2d pointToTurnAbout = new FramePoint2d(); robotPose.getPosition(pointToTurnAbout); addTurnInPlace(footstepList, turningAngle, pointToTurnAbout); }
private void computeDistanceAndAngleToDestination(ReferenceFrame supportAnkleZUpFrame, RobotSide swingLegSide, FramePoint2d desiredDestination) { FramePoint2d destinationInAnkleFrame = new FramePoint2d(desiredDestination); destinationInAnkleFrame.changeFrame(supportAnkleZUpFrame); FramePoint2d squaredUpMidpointInAnkleFrame = new FramePoint2d(supportAnkleZUpFrame, 0.0, swingLegSide.negateIfRightSide(desiredStepWidth.getDoubleValue() / 2.0)); FrameVector2d midpointToDestination = new FrameVector2d(destinationInAnkleFrame); midpointToDestination.sub(squaredUpMidpointInAnkleFrame); distanceToDestination.set(midpointToDestination.length()); angleToDestination.set(Math.atan2(midpointToDestination.getY(), midpointToDestination.getX())); }
public static void projectOntoPolygonAndCheckDistance(FramePoint2d point, FrameConvexPolygon2d polygon, double epsilon) { ReferenceFrame originalReferenceFrame = point.getReferenceFrame(); point.changeFrame(polygon.getReferenceFrame()); FramePoint2d originalPoint = new FramePoint2d(point); polygon.orthogonalProjection(point); double distance = originalPoint.distance(point); if (distance > epsilon) throw new RuntimeException("point outside polygon by " + distance); point.changeFrame(originalReferenceFrame); }
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); }
public FramePose2d(FramePoint2d position, FrameOrientation2d orientation) { if (position.getReferenceFrame() != orientation.getReferenceFrame()) { throw new ReferenceFrameMismatchException("FramePose: The position frame (" + position.getReferenceFrame() + ") does not match the orientation frame (" + orientation.getReferenceFrame() + ")"); } this.position = new FramePoint2d(position); this.orientation = new FrameOrientation2d(orientation); this.referenceFrame = position.getReferenceFrame(); }
public FrameVector2d computeDesiredMomentumXY(FramePoint2d desiredCoMXY, MutableDouble errorMagnitude) { FrameVector2d ret = new FrameVector2d(); FramePoint2d errorCoMXY = new FramePoint2d(desiredCoMXY); errorCoMXY.changeFrame(referenceFrames.getCenterOfMassFrame()); errorMagnitude.setValue(MathTools.square(errorCoMXY.getX()) + MathTools.square(errorCoMXY.getY())); errorMagnitude.setValue(Math.sqrt(errorMagnitude.doubleValue())); errorCoMXY.scale(1.0 / updateDT); ret.setIncludingFrame(errorCoMXY); ret.scale(toolbox.getTotalRobotMass()); return ret; }