public static double roundToGivenPrecisionForAngle(double angleValue, double precisionFactor) { double centeredAngleValue = trimAngleMinusPiToPi(angleValue + 0.5 * precisionFactor); long longValue = (long) (centeredAngleValue / precisionFactor); double roundedValue = ((double) longValue) * precisionFactor; return trimAngleMinusPiToPi(roundedValue); }
public static double roundToGivenPrecisionForAngle(double angleValue, double precisionFactor) { double centeredAngleValue = AngleTools.trimAngleMinusPiToPi(angleValue + 0.5 * precisionFactor); long longValue = (long) (centeredAngleValue / precisionFactor); double roundedValue = ((double) longValue) * precisionFactor; return AngleTools.trimAngleMinusPiToPi(roundedValue); }
public double indexToAngle(int index) { double doubleIndex = (double) index; double doubleSectors = (double) sectors; double sectorsFraction = doubleIndex / doubleSectors; return AngleTools.trimAngleMinusPiToPi(sectorsFraction * Math.PI * 2.0); }
public static double toAngle(double y, double z) { double angle = Math.atan2(y, -z); angle = AngleTools.trimAngleMinusPiToPi(angle); return angle; }
public int angleToIndex(double angle) { double trimmedAngle = AngleTools.trimAngleMinusPiToPi(angle); double doubleIndex = (trimmedAngle) * sectors / (2 * Math.PI); return (int) (Math.floor(doubleIndex) + sectors) % sectors; }
public void setYaw(double yaw) { if (Double.isNaN(yaw)) { throw new RuntimeException("Orientation.setYaw() yaw = " + yaw); } this.yaw = AngleTools.trimAngleMinusPiToPi(yaw); }
public static double interpolateAngle(double angleA, double angleB, double alpha) { // A + alpha * (B-A)/2 double average = angleA + alpha * AngleTools.computeAngleDifferenceMinusPiToPi(angleB, angleA); return trimAngleMinusPiToPi(average); }
public static double computeAngleAverage(double angleA, double angleB) { // average = A + (B-A)/2 = (A+B)/2 double average = angleA + 0.5 * AngleTools.computeAngleDifferenceMinusPiToPi(angleB, angleA); return trimAngleMinusPiToPi(average); }
public void setFinalSteeringAngle(double finalSteeringAngle) { this.finalSteeringAngle.set(trimAngleMinusPiToPi(finalSteeringAngle)); }
public FootstepNode(double x, double y, double yaw, RobotSide robotSide) { xIndex = (int) Math.round(x / gridSizeXY); yIndex = (int) Math.round(y / gridSizeXY); yawIndex = (int) Math.round(AngleTools.trimAngleMinusPiToPi(yaw) / gridSizeYaw); this.robotSide = robotSide; hashCode = computeHashCode(this); planarRegionsHashCode = computePlanarRegionsHashCode(this); }
public boolean isEpsilonParallel(FrameVector frameVector, double epsilonAngle) { checkReferenceFrameMatch(frameVector); double angleMinusZeroToPi = Math.abs(AngleTools.trimAngleMinusPiToPi(this.angle(frameVector))); double errorFromParallel = Math.min(angleMinusZeroToPi, Math.PI - angleMinusZeroToPi); return errorFromParallel < epsilonAngle; }
@Override public void applyTransform(RigidBodyTransform transform) { checkIsTransformationInPlane(transform); Vector3d xVector = new Vector3d(1.0, 0.0, 0.0); transform.transform(xVector); double deltaYaw = Math.atan2(xVector.getY(), xVector.getX()); if (Double.isNaN(deltaYaw) || Double.isInfinite(deltaYaw)) deltaYaw = 0.0; this.yaw = AngleTools.trimAngleMinusPiToPi(this.yaw + deltaYaw); }
public void extrapolate(FrameOrientation2d orientationOne, FrameOrientation2d orientationTwo, double alpha) { orientationOne.checkReferenceFrameMatch(orientationTwo); double deltaYaw = AngleTools.computeAngleDifferenceMinusPiToPi(orientationTwo.yaw, orientationOne.yaw); this.yaw = AngleTools.trimAngleMinusPiToPi(orientationOne.yaw + alpha * deltaYaw); this.referenceFrame = orientationOne.getReferenceFrame(); }
public static void trimAxisAngleMinusPiToPi(AxisAngle4d axisAngle4d, AxisAngle4d axisAngleTrimmedToPack) { axisAngleTrimmedToPack.set(axisAngle4d); axisAngleTrimmedToPack.setAngle(AngleTools.trimAngleMinusPiToPi(axisAngleTrimmedToPack.getAngle())); }
private static double calculateHeading(Point2DReadOnly startPose, Point2DReadOnly endPoint) { double deltaX = endPoint.getX() - startPose.getX(); double deltaY = endPoint.getY() - startPose.getY(); double heading; double pathHeading = Math.atan2(deltaY, deltaX); heading = AngleTools.trimAngleMinusPiToPi(pathHeading); return heading; }
public static void trimAxisAngleMinusPiToPi(AxisAngleReadOnly axisAngle4d, AxisAngleBasics axisAngleTrimmedToPack) { axisAngleTrimmedToPack.set(axisAngle4d); axisAngleTrimmedToPack.setAngle(AngleTools.trimAngleMinusPiToPi(axisAngleTrimmedToPack.getAngle())); }
private boolean isOrientationEqual(QuaternionReadOnly initialQuat, QuaternionReadOnly finalQuat, double angleEpsilon) { Quaternion quatDifference = new Quaternion(initialQuat); quatDifference.multiplyConjugateOther(finalQuat); AxisAngle angleDifference = new AxisAngle(); angleDifference.set(quatDifference); AngleTools.trimAngleMinusPiToPi(angleDifference.getAngle()); return Math.abs(angleDifference.getAngle()) < angleEpsilon; }
@Override public FramePose2d getPoseAtDistance(double distanceAlongPath) { double interpolationValue = distanceAlongPath / distance; interpolationValue = Math.max(interpolationValue, 0.0); interpolationValue = Math.min(interpolationValue, 1.0); double x = (1-interpolationValue) * startPose.getX() + (interpolationValue) * endPose.getX(); double y = (1-interpolationValue) * startPose.getY() + (interpolationValue) * endPose.getY(); double yaw = AngleTools.trimAngleMinusPiToPi(startPose.getYaw() + (interpolationValue) * angleChange); return new FramePose2d(startPose.getReferenceFrame(), new Point2d(x,y), yaw); }
@Override public FramePose2D getPoseAtDistance(double distanceAlongPath) { double interpolationValue = distanceAlongPath / distance; interpolationValue = Math.max(interpolationValue, 0.0); interpolationValue = Math.min(interpolationValue, 1.0); double x = (1-interpolationValue) * startPose.getX() + (interpolationValue) * endPose.getX(); double y = (1-interpolationValue) * startPose.getY() + (interpolationValue) * endPose.getY(); double yaw = AngleTools.trimAngleMinusPiToPi(startPose.getYaw() + (interpolationValue) * angleChange); return new FramePose2D(startPose.getReferenceFrame(), new Point2D(x,y), yaw); }
public void startComputation() { imuSimulatedDriftYawVelocity.add(imuSimulatedDriftYawAcceleration.getDoubleValue() * estimatorDT); imuSimulatedDriftYawAngle.add(imuSimulatedDriftYawVelocity.getDoubleValue() * estimatorDT); imuSimulatedDriftYawAngle.set(AngleTools.trimAngleMinusPiToPi(imuSimulatedDriftYawAngle.getDoubleValue())); convertOrientationAndSetOnOutputPort(); convertAngularVelocityAndSetOnOutputPort(); }