/** * @brief 正準オイラー角の3つ組を決定する */ public void canonize() { pitch = LDMathUtil.wrapPi(pitch); if (pitch < -LDMathUtil.PI_OVER_2) { pitch = -LDMathUtil.PI - pitch; heading += LDMathUtil.PI; bank += LDMathUtil.PI; } else if (pitch > LDMathUtil.PI_OVER_2) { pitch = LDMathUtil.PI - pitch; heading += LDMathUtil.PI; bank += LDMathUtil.PI; } // ジンバルロックのケースをチェックする if (Math.Abs(pitch) > LDMathUtil.PI_OVER_2 - LDMathUtil.GINBAL_TOLERANCE) { // ジンバルロック内にいる。回転を差し替える heading += bank; bank = 0.0f; } else { bank = LDMathUtil.wrapPi(bank); } heading = LDMathUtil.wrapPi(heading); }
public void canonizeTest() { LDEulerAngles actual = new LDEulerAngles(); LDEulerAngles expected = new LDEulerAngles(); //ld_float delta = 0.00001f; // Input : heading = 0.0, pitch = 0.0, bank = 0.0 (ゼロ) actual.identity(); expected.identity(); actual.canonize(); TestUtil.COMPARE(expected.heading, actual.heading); TestUtil.COMPARE(expected.pitch, actual.pitch); TestUtil.COMPARE(expected.bank, actual.bank); // Input : heading = EMathUtil.PI_OVER_2, pitch = -EMathUtil.PI_OVER_2 - 0.1, // bank = EMathUtil.PI_OVER_2 (pitch < - EMathUtil.PI_OVER_2のケース) actual.heading = LDMathUtil.PI_OVER_2; actual.pitch = -LDMathUtil.PI_OVER_2 - 0.1f; actual.bank = LDMathUtil.PI_OVER_2; expected.heading = LDMathUtil.wrapPi(actual.heading + LDMathUtil.PI); expected.pitch = -LDMathUtil.PI - actual.pitch; expected.bank = LDMathUtil.wrapPi(actual.bank + LDMathUtil.PI); actual.canonize(); TestUtil.COMPARE(expected.heading, actual.heading); TestUtil.COMPARE(expected.pitch, actual.pitch); TestUtil.COMPARE(expected.bank, actual.bank); // Input : heading = EMathUtil.PI_OVER_2, pitch = EMathUtil.PI_OVER_2 + 0.1, // bank = EMathUtil.PI_OVER_2 (pitch > EMathUtil.PI_OVER_2のケース) actual.heading = LDMathUtil.PI_OVER_2; actual.pitch = LDMathUtil.PI_OVER_2 + 0.1f; actual.bank = LDMathUtil.PI_OVER_2; expected.heading = LDMathUtil.wrapPi(actual.heading + LDMathUtil.PI); expected.pitch = LDMathUtil.PI - actual.pitch; expected.bank = LDMathUtil.wrapPi(actual.bank + LDMathUtil.PI); actual.canonize(); TestUtil.COMPARE(expected.heading, actual.heading); TestUtil.COMPARE(expected.pitch, actual.pitch); TestUtil.COMPARE(expected.bank, actual.bank); // Input : heading = EMathUtil.PI_OVER_2, pitch = EMathUtil.PI_OVER_2, // bank = EMathUtil.PI_OVER_2 (全てPI_OVER_2, ジンバルロックのケース) actual.heading = LDMathUtil.PI_OVER_2; actual.pitch = LDMathUtil.PI_OVER_2; actual.bank = LDMathUtil.PI_OVER_2; expected.heading = actual.heading + actual.bank; expected.pitch = LDMathUtil.PI_OVER_2; expected.bank = 0.0f; actual.canonize(); TestUtil.COMPARE(expected.heading, actual.heading); TestUtil.COMPARE(expected.pitch, actual.pitch); TestUtil.COMPARE(expected.bank, actual.bank); }
public void wrapPiTest() { ld_float actual; ld_float expected; ld_float theta; //ld_float delta = 0.00001f; // Input : 0.0 theta = 0.0f; expected = 0.0f; actual = LDMathUtil.wrapPi(theta); TestUtil.COMPARE(expected, actual); // Input : 2PI theta = LDMathUtil.PI2; expected = 0.0f; actual = LDMathUtil.wrapPi(theta); TestUtil.COMPARE(expected, actual); // Input : PI, PI±0.01におけるラップテスト theta = LDMathUtil.PI - 0.01f; expected = LDMathUtil.PI - 0.01f; actual = LDMathUtil.wrapPi(theta); TestUtil.COMPARE(expected, actual); theta = LDMathUtil.PI; expected = LDMathUtil.PI; actual = LDMathUtil.wrapPi(theta); TestUtil.COMPARE(expected, actual); theta = LDMathUtil.PI + 0.01f; expected = 0.01f - LDMathUtil.PI; actual = LDMathUtil.wrapPi(theta); TestUtil.COMPARE(expected, actual); // Input : -PI, -PI±0.01におけるラップテスト theta = -LDMathUtil.PI - 0.01f; expected = LDMathUtil.PI - 0.01f; actual = LDMathUtil.wrapPi(theta); TestUtil.COMPARE(expected, actual); theta = -LDMathUtil.PI; expected = -LDMathUtil.PI; actual = LDMathUtil.wrapPi(theta); TestUtil.COMPARE(expected, actual); theta = -LDMathUtil.PI + 0.01f; expected = -LDMathUtil.PI + 0.01f; actual = LDMathUtil.wrapPi(theta); TestUtil.COMPARE(expected, actual); }