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);
}
/**
* @brief 慣性空間->オブジェクト空間への回転
* @param &orientation オイラー角(EEulerAngles)をセットする
*/
void setToRotateInertialToObject(LDEulerAngles orientation)
{
ld_float sp, sb, sh;
ld_float cp, cb, ch;
LDMathUtil.sinCos(out sp, out cp, orientation.pitch * 0.5f);
LDMathUtil.sinCos(out sb, out cb, orientation.bank * 0.5f);
LDMathUtil.sinCos(out sh, out ch, orientation.heading * 0.5f);
w = ch * cp * cb + sh * sp * sb;
x = -ch * sp * cb - sh * cp * sb;
y = ch * sp * sb - sh * cp * cb;
z = sh * sp * cb - ch * cp * sb;
}
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 setToRotateInertialToObjectTest()
{
LDEulerAngles angle1 = new LDEulerAngles();
LDQuat actual = new LDQuat();
LDQuat expected = new LDQuat();
//ld_float delta = 0.00001f;
// Input : angle1{heading = 0.0, pitch = 0.0, bank = 0.0} (ゼロベクトル)
angle1.heading = 0.0f;
angle1.pitch = 0.0f;
angle1.bank = 0.0f;
expected.x = 0.0f;
expected.y = 0.0f;
expected.z = 0.0f;
expected.w = 1.0f;
actual.setToRotateObjectToInertial(angle1);
TestUtil.COMPARE(expected.x, actual.x);
TestUtil.COMPARE(expected.y, actual.y);
TestUtil.COMPARE(expected.z, actual.z);
TestUtil.COMPARE(expected.w, actual.w);
// Input : angle1{heading = PI/4, pitch = PI/2, bank = PI} (任意の値)
angle1.heading = LDMathUtil.PI / 4.0f;
angle1.pitch = LDMathUtil.PI / 2.0f;
angle1.bank = LDMathUtil.PI;
expected.x = -(float)Math.Cos(angle1.heading * 0.5f) * (float)Math.Sin(angle1.pitch * 0.5f) * (float)Math.Cos(angle1.bank * 0.5f) -
(float)Math.Sin(angle1.heading * 0.5f) * (float)Math.Cos(angle1.pitch * 0.5f) * (float)Math.Sin(angle1.bank * 0.5f);
expected.y = (float)Math.Cos(angle1.heading * 0.5f) * (float)Math.Sin(angle1.pitch * 0.5f) * (float)Math.Sin(angle1.bank * 0.5f) -
(float)Math.Sin(angle1.heading * 0.5f) * (float)Math.Cos(angle1.pitch * 0.5f) * (float)Math.Cos(angle1.bank * 0.5f);
expected.z = (float)Math.Sin(angle1.heading * 0.5f) * (float)Math.Sin(angle1.pitch * 0.5f) * (float)Math.Cos(angle1.bank * 0.5f) -
(float)Math.Cos(angle1.heading * 0.5f) * (float)Math.Cos(angle1.pitch * 0.5f) * (float)Math.Sin(angle1.bank * 0.5f);
expected.w = (float)Math.Cos(angle1.heading * 0.5f) * (float)Math.Cos(angle1.pitch * 0.5f) * (float)Math.Cos(angle1.bank * 0.5f) +
(float)Math.Sin(angle1.heading * 0.5f) * (float)Math.Sin(angle1.pitch * 0.5f) * (float)Math.Sin(angle1.bank * 0.5f);
actual.setToRotateInertialToObject(angle1);
TestUtil.COMPARE(expected.x, actual.x);
TestUtil.COMPARE(expected.y, actual.y);
TestUtil.COMPARE(expected.z, actual.z);
TestUtil.COMPARE(expected.w, actual.w);
}
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 setToRotateObjectToInertial( EEulerAngles & orientation ) ;
//public void setToRotateInertialToObject( EEulerAngles & orientation ) ;
/**
* @brief オブジェクト空間->慣性空間への回転
* @param &orientation オイラー角(EEulerAngles)をセットする
*/
public void setToRotateObjectToInertial(LDEulerAngles orientation)
{
ld_float sp, sb, sh;
ld_float cp, cb, ch;
LDMathUtil.sinCos(out sp, out cp, orientation.pitch * 0.5f);
LDMathUtil.sinCos(out sb, out cb, orientation.bank * 0.5f);
LDMathUtil.sinCos(out sh, out ch, orientation.heading * 0.5f);
w = ch * cp * cb + sh * sp * sb;
x = ch * sp * cb + sh * cp * sb;
y = -ch * sp * sb + sh * cp * cb;
z = -sh * sp * cb + ch * cp * sb;
}
public void setToRotateObjectToInertialTest()
{
LDEulerAngles angle1 = new LDEulerAngles();
LDQuat actual = new LDQuat();
LDQuat expected = new LDQuat();
//ld_float delta = 0.00001f;
// Input : angle1{heading = 0.0, pitch = 0.0, bank = 0.0} (ゼロベクトル)
angle1.heading = 0.0f;
angle1.pitch = 0.0f;
angle1.bank = 0.0f;
expected.x = 0.0f;
expected.y = 0.0f;
expected.z = 0.0f;
expected.w = 1.0f;
actual.setToRotateObjectToInertial(angle1);
TestUtil.COMPARE(expected.x, actual.x);
TestUtil.COMPARE(expected.y, actual.y);
TestUtil.COMPARE(expected.z, actual.z);
TestUtil.COMPARE(expected.w, actual.w);
// Input : angle1{heading = PI/4, pitch = PI/2, bank = PI} (任意の値)
angle1.heading = LDMathUtil.PI / 4.0f;
angle1.pitch = LDMathUtil.PI / 2.0f;
angle1.bank = LDMathUtil.PI;
expected.x = (float)Math.Cos(angle1.heading * 0.5f) * (float)Math.Sin(angle1.pitch * 0.5f) * (float)Math.Cos(angle1.bank * 0.5f) +
(float)Math.Sin(angle1.heading * 0.5f) * (float)Math.Cos(angle1.pitch * 0.5f) * (float)Math.Sin(angle1.bank * 0.5f);
expected.y = -(float)Math.Cos(angle1.heading * 0.5f) * (float)Math.Sin(angle1.pitch * 0.5f) * (float)Math.Sin(angle1.bank * 0.5f) +
(float)Math.Sin(angle1.heading * 0.5f) * (float)Math.Cos(angle1.pitch * 0.5f) * (float)Math.Cos(angle1.bank * 0.5f);
expected.z = -(float)Math.Sin(angle1.heading * 0.5f) * (float)Math.Sin(angle1.pitch * 0.5f) * (float)Math.Cos(angle1.bank * 0.5f) +
(float)Math.Cos(angle1.heading * 0.5f) * (float)Math.Cos(angle1.pitch * 0.5f) * (float)Math.Sin(angle1.bank * 0.5f);
expected.w = (float)Math.Cos(angle1.heading * 0.5f) * (float)Math.Cos(angle1.pitch * 0.5f) * (float)Math.Cos(angle1.bank * 0.5f) +
(float)Math.Sin(angle1.heading * 0.5f) * (float)Math.Sin(angle1.pitch * 0.5f) * (float)Math.Sin(angle1.bank * 0.5f);
actual.setToRotateObjectToInertial(angle1);
TestUtil.COMPARE(expected.x, actual.x);
TestUtil.COMPARE(expected.y, actual.y);
TestUtil.COMPARE(expected.z, actual.z);
TestUtil.COMPARE(expected.w, actual.w);
}
