public double distance(FramePoint2d point) { putYoValuesIntoFrameLine(); return frameLine.distance(point); }
public double distance(FrameLine2d line) { putYoValuesIntoFrameLine(); return frameLine.distance(line); }
public double distance(FrameLineSegment2d secondLineSegment) { putYoValuesIntoFrameLine(); return frameLine.distance(secondLineSegment); }
public double distance(FrameConvexPolygon2d convexPolygon) { putYoValuesIntoFrameLine(); return frameLine.distance(convexPolygon); }
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; } }