public static double trimAngleMinusPiToPi(double angle) { return AngleTools.shiftAngleToStartOfRange(angle, -PI); }
/** * This will shift an angle to be in the range [<i>startOfAngleRange</i>, * (<i>startOfAngleRange + 2*pi</i>) * * @param angleToShift the angle to shift * @param startOfAngleRange start of the range. * @return the shifted angle */ public static double shiftAngleToStartOfRange(double angleToShift, double startOfAngleRange) { return shiftAngleToStartOfRange(angleToShift, startOfAngleRange, TwoPI); }
/** * This will shift an angle to be in the range [<i>startOfAngleRange</i>, * (<i>startOfAngleRange + 2*pi</i>) * * @param angleToShift the angle to shift * @param startOfAngleRange start of the range. * @return the shifted angle */ public static double shiftAngleToStartOfRange(double angleToShift, double startOfAngleRange) { return shiftAngleToStartOfRange(angleToShift, startOfAngleRange, TwoPI); }
public static double trimAngleMinusPiToPi(double angle) { return AngleTools.shiftAngleToStartOfRange(angle, -PI); }
/** * computeAngleDifferenceMinusPiToPi: returns (angleA - angleB), where the return value is [-2.0*pi, 0.0) * * @param angleA double * @param angleB double * @return double */ public static double computeAngleDifferenceMinusTwoPiToZero(double angleA, double angleB) { double difference = angleA - angleB; difference = difference % TwoPI; difference = AngleTools.shiftAngleToStartOfRange(difference, -TwoPI); return difference; }
/** * computeAngleDifferenceMinusPiToPi: returns (angleA - angleB), where the return value is [-pi, pi) * * @param angleA double * @param angleB double * @return double */ public static double computeAngleDifferenceMinusPiToPi(double angleA, double angleB) { double difference = angleA - angleB; difference = difference % TwoPI; difference = AngleTools.shiftAngleToStartOfRange(difference, -PI); return difference; }
/** * computeAngleDifferenceMinusPiToPi: returns (angleA - angleB), where the return value is [-pi, pi) * * @param angleA double * @param angleB double * @return double */ public static double computeAngleDifferenceMinusPiToPi(double angleA, double angleB) { double difference = angleA - angleB; difference = difference % TwoPI; difference = AngleTools.shiftAngleToStartOfRange(difference, -PI); return difference; }
/** * computeAngleDifferenceMinusPiToPi: returns (angleA - angleB), where the return value is [-2.0*pi, 0.0) * * @param angleA double * @param angleB double * @return double */ public static double computeAngleDifferenceMinusTwoPiToZero(double angleA, double angleB) { double difference = angleA - angleB; difference = difference % TwoPI; difference = AngleTools.shiftAngleToStartOfRange(difference, -TwoPI); return difference; }
double angleToShift = 1.6 * range; double expectedReturn = 0.6 * range; double actualReturn = AngleTools.shiftAngleToStartOfRange(angleToShift, 0.0, endOfAngleRange); assertEquals(expectedReturn, actualReturn, 1e-12); actualReturn = AngleTools.shiftAngleToStartOfRange(angleToShift, 0.0, endOfAngleRange); assertEquals(expectedReturn, actualReturn, 1e-12); actualReturn = AngleTools.shiftAngleToStartOfRange(angleToShift, 0.0, endOfAngleRange); assertEquals(expectedReturn, actualReturn, 1e-12); expectedReturn = angleToShift - Math.floor((angleToShift) / endOfAngleRange) * endOfAngleRange; actualReturn = AngleTools.shiftAngleToStartOfRange(angleToShift, 0.0, endOfAngleRange); assertEquals(expectedReturn, actualReturn, 1e-12);
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testShiftAngleToStartOfRange() { double angleToShift = 0.5; double startOfAngleRange = Math.PI; double expectedReturn = 3.0 * Math.PI - (Math.PI - 0.5); double actualReturn = AngleTools.shiftAngleToStartOfRange(angleToShift, startOfAngleRange); assertEquals(expectedReturn, actualReturn, 1e-12); angleToShift = 12.0; startOfAngleRange = 0.5 * Math.PI; expectedReturn = 12.0 - 2 * Math.PI; actualReturn = AngleTools.shiftAngleToStartOfRange(angleToShift, startOfAngleRange); assertEquals(expectedReturn, actualReturn, 1e-12); angleToShift = 0.8; startOfAngleRange = -0.25 * Math.PI; expectedReturn = 0.8; actualReturn = AngleTools.shiftAngleToStartOfRange(angleToShift, startOfAngleRange); assertEquals(expectedReturn, actualReturn, 1e-12); }
yaw2 = AngleTools.shiftAngleToStartOfRange(yaw2, -Math.PI); yaw2 = AngleTools.shiftAngleToStartOfRange(yaw2, -Math.PI); assertEquals(yawPitchRoll[0], (alpha-1)*yaw1 + alpha * yaw2, 1e-6);