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 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 fromWorldToObjectMatrixTest()
{
LDMatrix44 matrix = new LDMatrix44();
LDEulerAngles actual = new LDEulerAngles();
LDEulerAngles expected = new LDEulerAngles();
//ld_float delta = 0.00001f;
// Input : (単位行列)
actual.identity();
expected.identity();
actual.fromWorldToObjectMatrix(matrix);
TestUtil.COMPARE(expected.heading, actual.heading);
TestUtil.COMPARE(expected.pitch, actual.pitch);
TestUtil.COMPARE(expected.bank, actual.bank);
// Input : matrix.m23 = 1.0 (ジンバルロックのケース)
matrix.m23 = 1.0f;
expected.heading = (float)Math.Atan2(-matrix.m31, matrix.m11);
expected.pitch = LDMathUtil.PI_OVER_2 * -matrix.m23;
expected.bank = 0.0f;
actual.fromWorldToObjectMatrix(matrix);
TestUtil.COMPARE(expected.heading, actual.heading);
TestUtil.COMPARE(expected.pitch, actual.pitch);
TestUtil.COMPARE(expected.bank, actual.bank);
// Input : matrix.m23 = 1.0 (任意の値. 角度計算のケース)
matrix.m11 = 0.1f;
matrix.m12 = 0.2f;
matrix.m13 = 0.3f;
matrix.m14 = 0.4f;
matrix.m21 = 0.1f;
matrix.m22 = 0.2f;
matrix.m23 = 0.3f;
matrix.m24 = 0.4f;
matrix.m31 = 0.1f;
matrix.m32 = 0.2f;
matrix.m33 = 0.3f;
matrix.m34 = 0.4f;
matrix.m41 = 0.1f;
matrix.m42 = 0.2f;
matrix.m43 = 0.3f;
matrix.m44 = 0.4f;
expected.heading = (float)Math.Atan2(matrix.m13, matrix.m33);
expected.pitch = (float)Math.Asin(-matrix.m23);
expected.bank = (float)Math.Atan2(matrix.m21, matrix.m22);
actual.fromWorldToObjectMatrix(matrix);
TestUtil.COMPARE(expected.heading, actual.heading);
TestUtil.COMPARE(expected.pitch, actual.pitch);
TestUtil.COMPARE(expected.bank, actual.bank);
}
public void fromInertialToObjectQuaternionTest()
{
LDQuat quat=new LDQuat(0.0f, 0.0f, 0.0f, 0.0f );
LDEulerAngles actual=new LDEulerAngles();
LDEulerAngles expected=new LDEulerAngles();
//ld_float delta = 0.00001f;
ld_float sp;
// Input : x = 0.0, y = 0.0, z = 0.0, w = 0.0 (ゼロクォータニオン)
actual.identity();
expected.identity();
actual.fromInertialToObjectQuaternion(quat);
TestUtil.COMPARE(expected.heading, actual.heading);
TestUtil.COMPARE(expected.pitch, actual.pitch);
TestUtil.COMPARE(expected.bank, actual.bank);
// Input : x = 1.0, y = 0.0, z = 0.0, w = 1.0 (ジンバルロックのケース)
quat.x = 1.0f;
quat.y = 0.0f;
quat.z = 0.0f;
quat.w = 1.0f;
sp = -2.0f * (quat.y * quat.z - quat.w * quat.x);
expected.heading = (float)Math.Atan2(-quat.x * quat.z + quat.w * quat.y,
0.5f - quat.y * quat.y - quat.z * quat.z);
expected.pitch = LDMathUtil.PI_OVER_2 * sp;
expected.bank = 0.0f;
actual.fromObjectToInertialQuaternion(quat);
TestUtil.COMPARE(expected.heading, actual.heading);
TestUtil.COMPARE(expected.pitch, actual.pitch);
TestUtil.COMPARE(expected.bank, actual.bank);
// Input : x = 1.0, y = 0.25, z = 0.25, w = 0.5 (任意の値. 角度計算のケース)
quat.x = 1.0f;
quat.y = 0.25f;
quat.z = 0.25f;
quat.w = 0.5f;
sp = -2.0f * (quat.y * quat.z - quat.w * quat.x);
expected.heading = (float)Math.Atan2(quat.x * quat.z + quat.w * quat.y,
0.5f - quat.x * quat.x - quat.y * quat.y);
expected.pitch = (float)Math.Asin(sp);
expected.bank = (float)Math.Atan2(quat.x * quat.y + quat.w * quat.z,
0.5f - quat.x * quat.x - quat.z * quat.z);
actual.fromObjectToInertialQuaternion(quat);
TestUtil.COMPARE(expected.heading, actual.heading);
TestUtil.COMPARE(expected.pitch, actual.pitch);
TestUtil.COMPARE(expected.bank, actual.bank);
}
public void fromObjectToInertialQuaternionTest()
{
LDQuat quat = new LDQuat(0.0f, 0.0f, 0.0f, 0.0f);
LDEulerAngles actual = new LDEulerAngles();
LDEulerAngles expected = new LDEulerAngles();
//ld_float delta = 0.00001f;
ld_float sp;
// Input : x = 0.0, y = 0.0, z = 0.0, w = 0.0 (ゼロクォータニオン)
actual.identity();
expected.identity();
actual.fromObjectToInertialQuaternion(quat);
TestUtil.COMPARE(expected.heading, actual.heading);
TestUtil.COMPARE(expected.pitch, actual.pitch);
TestUtil.COMPARE(expected.bank, actual.bank);
// Input : x = 1.0, y = 0.0, z = 0.0, w = 1.0 (ジンバルロックのケース)
quat.x = 1.0f;
quat.y = 0.0f;
quat.z = 0.0f;
quat.w = 1.0f;
sp = -2.0f * (quat.y * quat.z - quat.w * quat.x);
expected.heading = (float)Math.Atan2(-quat.x * quat.z + quat.w * quat.y,
0.5f - quat.y * quat.y - quat.z * quat.z);
expected.pitch = LDMathUtil.PI_OVER_2 * sp;
expected.bank = 0.0f;
actual.fromObjectToInertialQuaternion(quat);
TestUtil.COMPARE(expected.heading, actual.heading);
TestUtil.COMPARE(expected.pitch, actual.pitch);
TestUtil.COMPARE(expected.bank, actual.bank);
// Input : x = 1.0, y = 0.25, z = 0.25, w = 0.5 (任意の値. 角度計算のケース)
quat.x = 1.0f;
quat.y = 0.25f;
quat.z = 0.25f;
quat.w = 0.5f;
sp = -2.0f * (quat.y * quat.z - quat.w * quat.x);
expected.heading = (float)Math.Atan2(quat.x * quat.z + quat.w * quat.y,
0.5f - quat.x * quat.x - quat.y * quat.y);
expected.pitch = (float)Math.Asin(sp);
expected.bank = (float)Math.Atan2(quat.x * quat.y + quat.w * quat.z,
0.5f - quat.x * quat.x - quat.z * quat.z);
actual.fromObjectToInertialQuaternion(quat);
TestUtil.COMPARE(expected.heading, actual.heading);
TestUtil.COMPARE(expected.pitch, actual.pitch);
TestUtil.COMPARE(expected.bank, actual.bank);
}
public void fromWorldToObjectMatrixTest()
{
LDMatrix44 matrix=new LDMatrix44();
LDEulerAngles actual=new LDEulerAngles();
LDEulerAngles expected=new LDEulerAngles();
//ld_float delta = 0.00001f;
// Input : (単位行列)
actual.identity();
expected.identity();
actual.fromWorldToObjectMatrix(matrix);
TestUtil.COMPARE(expected.heading, actual.heading);
TestUtil.COMPARE(expected.pitch, actual.pitch);
TestUtil.COMPARE(expected.bank, actual.bank);
// Input : matrix.m23 = 1.0 (ジンバルロックのケース)
matrix.m23 = 1.0f;
expected.heading = (float)Math.Atan2(-matrix.m31, matrix.m11);
expected.pitch = LDMathUtil.PI_OVER_2 * -matrix.m23;
expected.bank = 0.0f;
actual.fromWorldToObjectMatrix(matrix);
TestUtil.COMPARE(expected.heading, actual.heading);
TestUtil.COMPARE(expected.pitch, actual.pitch);
TestUtil.COMPARE(expected.bank, actual.bank);
// Input : matrix.m23 = 1.0 (任意の値. 角度計算のケース)
matrix.m11 = 0.1f;
matrix.m12 = 0.2f;
matrix.m13 = 0.3f;
matrix.m14 = 0.4f;
matrix.m21 = 0.1f;
matrix.m22 = 0.2f;
matrix.m23 = 0.3f;
matrix.m24 = 0.4f;
matrix.m31 = 0.1f;
matrix.m32 = 0.2f;
matrix.m33 = 0.3f;
matrix.m34 = 0.4f;
matrix.m41 = 0.1f;
matrix.m42 = 0.2f;
matrix.m43 = 0.3f;
matrix.m44 = 0.4f;
expected.heading = (float)Math.Atan2(matrix.m13, matrix.m33);
expected.pitch = (float)Math.Asin(-matrix.m23);
expected.bank = (float)Math.Atan2(matrix.m21, matrix.m22);
actual.fromWorldToObjectMatrix(matrix);
TestUtil.COMPARE(expected.heading, actual.heading);
TestUtil.COMPARE(expected.pitch, actual.pitch);
TestUtil.COMPARE(expected.bank, actual.bank);
}