/**
* @brief 正準オイラー角の３つ組を決定する
*/
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);
}
