public FrameLine2d negateDirectionCopy() { FrameLine2d ret = new FrameLine2d(this.referenceFrame, line.negateDirectionCopy()); return ret; }
@Override public FrameLine2d changeFrameAndProjectToXYPlaneCopy(ReferenceFrame desiredFrame) { FrameLine2d copy = new FrameLine2d(this); copy.changeFrameAndProjectToXYPlane(desiredFrame); return copy; }
@Override public double distance(FrameConvexPolygon2d convexPolygon) { checkReferenceFrameMatch(convexPolygon); return this.line.distance(convexPolygon.convexPolygon); }
@Override public FrameLine2d applyTransformAndProjectToXYPlaneCopy(RigidBodyTransform transform) { FrameLine2d copy = new FrameLine2d(this); copy.applyTransformAndProjectToXYPlane(transform); return copy; }
@Override public FrameLine2d applyTransformCopy(RigidBodyTransform transform) { FrameLine2d copy = new FrameLine2d(this); copy.applyTransform(transform); return copy; }
public int computeNumberOfCellsOccupiedOnSideOfLine(FrameLine2d frameLine, RobotSide sideToLookAt, double minDistanceFromLine) frameLine.checkReferenceFrameMatch(soleFrame); frameLine.getNormalizedFrameVector(shiftingVector); frameLine.getFramePoint2d(shiftedLinePoint); frameLine.getNormalizedFrameVector(shiftedLineVector); shiftedLine.setIncludingFrame(shiftedLinePoint, shiftedLineVector); if (shiftedLine.isPointOnSideOfLine(cellCenter, sideToLookAt)) numberOfCellsActivatedOnSideToLookAt += occupancyGrid.get(xIndex, yIndex);
lineOfRotationInWorldFrame.set(centerOfRotation2d, lineOfRotation2d); lineOfRotationInSoleFrame.setIncludingFrame(lineOfRotationInWorldFrame); lineOfRotationInSoleFrame.changeFrameAndProjectToXYPlane(soleFrame); lineOfRotationInSoleFrame.setPoint(centerOfPressure);
public boolean areLinesPerpendicular(FrameLine2d line) { checkReferenceFrameMatch(line); return this.line.areLinesPerpendicular(line.getLine2d()); }
public boolean isRotating(FramePoint2d cop, FramePoint2d desiredCop, FrameLine2d lineOfRotation) { cop.checkReferenceFrameMatch(soleFrame); desiredCop.checkReferenceFrameMatch(soleFrame); lineOfRotation.checkReferenceFrameMatch(soleFrame); if (!lineOfRotation.isPointOnLine(cop)) return false; copError2d.setToZero(soleFrame); copError2d.sub(desiredCop, cop); yoCopError.set(copError2d); perpendicularCopError.set(lineOfRotation.distance(desiredCop)); boolean errorAboveThreshold = perpendicularCopError.getDoubleValue() >= perpendicluarCopErrorThreshold.getDoubleValue(); perpendicularCopErrorAboveThreshold.set(errorAboveThreshold); double acos = perpendicularCopError.getDoubleValue() / copError2d.length(); angleBetweenCopErrorAndLine.set(Math.acos(Math.abs(acos))); boolean angleInBounds = angleBetweenCopErrorAndLine.getDoubleValue() <= angleThreshold.getDoubleValue(); angleOkay.set(angleInBounds); boolean correctSide = lineOfRotation.isPointOnRightSideOfLine(desiredCop); desiredCopOnCorrectSide.set(correctSide); return errorAboveThreshold && angleInBounds && correctSide; } }
public FrameLine2d interiorBisector(FrameLine2d secondLine) { this.checkReferenceFrameMatch(secondLine); ReferenceFrame referenceFrame = this.referenceFrame; Line2d bisectorLine2d = this.line.interiorBisector(secondLine.line); if (bisectorLine2d == null) { return null; } else { return new FrameLine2d(referenceFrame, bisectorLine2d); } }
lineOfRotationInSoleFrame.set(centerOfRotation, angularVelocity2d); lineOfRotationInWorldFrame.setIncludingFrame(lineOfRotationInSoleFrame); lineOfRotationInWorldFrame.changeFrameAndProjectToXYPlane(worldFrame);
protected void putYoValuesIntoFrameLine() { if (frameLine == null) { frameLine = new FrameLine2d(referenceFrame, new Point2d(pointX.getDoubleValue(), pointY.getDoubleValue()), new Vector2d(vectorX.getDoubleValue(), vectorY.getDoubleValue())); } else { frameLine.set(referenceFrame, pointX.getDoubleValue(), pointY.getDoubleValue(), vectorX.getDoubleValue(), vectorY.getDoubleValue()); } } }
private double estimateDeltaTimeBetweenDesiredICPAndActualICP(double time, FramePoint2d actualCapturePointPosition) { computeDesiredCapturePoint(computeAndReturnTimeInCurrentState(time)); computeDesiredCentroidalMomentumPivot(); desiredCapturePointPosition.getFrameTuple2dIncludingFrame(desiredICP2d); singleSupportFinalICP.getFrameTuple2dIncludingFrame(finalICP2d); if (desiredICP2d.distance(finalICP2d) < 1.0e-10) return Double.NaN; desiredICPToFinalICPLineSegment.set(desiredICP2d, finalICP2d); actualICP2d.setIncludingFrame(actualCapturePointPosition); double percentAlongLineSegmentICP = desiredICPToFinalICPLineSegment.percentageAlongLineSegment(actualICP2d); if (percentAlongLineSegmentICP < 0.0) { desiredICPToFinalICPLine.set(desiredICP2d, finalICP2d); desiredICPToFinalICPLine.orthogonalProjection(actualICP2d); } else { desiredICPToFinalICPLineSegment.orthogonalProjection(actualICP2d); } double actualDistanceDueToDisturbance = desiredCentroidalMomentumPivotPosition.getXYPlaneDistance(actualICP2d); double expectedDistanceAccordingToPlan = desiredCentroidalMomentumPivotPosition.getXYPlaneDistance(desiredCapturePointPosition); computeTimeInCurrentState(time); double distanceRatio = actualDistanceDueToDisturbance / expectedDistanceAccordingToPlan; if (distanceRatio < 1.0e-3) return 0.0; else return Math.log(distanceRatio) / omega0.getDoubleValue(); }
public double distance(FrameLineSegment2d secondLineSegment) { putYoValuesIntoFrameLine(); return frameLine.distance(secondLineSegment); }
@Override public FramePoint2d intersectionWith(FrameLine2d secondLine) { checkReferenceFrameMatch(secondLine); Point2d intersection = this.line.intersectionWith(secondLine.line); return new FramePoint2d(secondLine.getReferenceFrame(), intersection); }
/** * 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 drawLine(FrameLine2d frameLine2d, Graphics graphics) { Point2d point = new Point2d(); frameLine2d.getPoint2d(point); Vector2d vector = new Vector2d(); frameLine2d.getNormalizedFrameVector(vector); double largeNumber = scale * 100; // 1.0e3; double X2 = point.getX() + largeNumber * vector.getX(); double Y2 = point.getY() + largeNumber * vector.getY(); drawLine(point.getX(), point.getY(), X2, Y2, graphics); }
public void orthogonalProjection(FramePoint2d point) { putYoValuesIntoFrameLine(); frameLine.orthogonalProjection(point); }
public boolean isPointOnLeftSideOfLine(FramePoint2d point) { return isPointOnSideOfLine(point, RobotSide.LEFT); }