} // not public
/**
* @brief 独自の球面線形補間（回転角度を保つ）。y軸90度、z軸90度の回転をSLERPで計算する場合、t が変化した時、軸は最小距離を移動するが、角度は、90度ではなく小さな値となる。この計算を 90度の回転を保って、中心を通る計算についてテスト実装
* @param &q1 クォータニオン(EQuat)をセットする
* @param t 補間位置を表す数値をセットする
* @return 補間結果を返す
*/
LDQuat mySlerp(LDQuat q1, ld_float t)
{
LDQuat q0 = this;
// 範囲外の時は端点を返す
if (t <= 0.0f)
{
return(q0);
}
if (t >= 1.0f)
{
return(q1);
}
LDVector3 n0 = q0.getAxis();
LDVector3 n1 = q1.getAxis();
ld_float a0 = q0.getAngle();
ld_float a1 = q1.getAngle();
//
ld_float adiff = LDMathUtil.getAngleDiff(a1, a0);
ld_float at = a0 + adiff * t;
LDVector3 nt = LDVector3.blend(n0, n1, 1 - t, t);
nt.normalize();
LDQuat ret = new LDQuat();
ret.setToRotateAxis(nt, at);
return(ret);
}
/**
* @brief コピーコンストラクタ
* @param quat EQuatをセットする
*/
public LDQuat(LDQuat quat)
{
x = quat.x;
y = quat.y;
z = quat.z;
w = quat.w;
}
/**
* @brief コピーコンストラクタ
* @param quat EQuatをセットする
*/
public LDQuat(LDQuat quat)
{
x = quat.x;
y = quat.y;
z = quat.z;
w = quat.w;
}
public void fromQuaterionTest()
{
LDQuat quat = new LDQuat(0.0f, 0.0f, 0.0f, 0.0f);
LDMatrix44 expected = new LDMatrix44();
LDMatrix44 actual = new LDMatrix44();
//ld_float delta = 0.00001f;
// Input : actual = 単位行列, quat = 0.0 (ゼロ)
actual.fromQuaterion(quat);
TestUtil.COMPARE(expected.m11, actual.m11);
TestUtil.COMPARE(expected.m12, actual.m12);
TestUtil.COMPARE(expected.m13, actual.m13);
TestUtil.COMPARE(expected.m21, actual.m21);
TestUtil.COMPARE(expected.m22, actual.m22);
TestUtil.COMPARE(expected.m23, actual.m23);
TestUtil.COMPARE(expected.m31, actual.m31);
TestUtil.COMPARE(expected.m32, actual.m32);
TestUtil.COMPARE(expected.m33, actual.m33);
TestUtil.COMPARE(expected.m41, actual.m41);
TestUtil.COMPARE(expected.m42, actual.m42);
TestUtil.COMPARE(expected.m43, actual.m43);
// Input : actual = 単位行列, quat x = 1.0, y = 2.0, z = 3.0, w = 0.5 (任意の値)
quat.x = 1.0f;
quat.y = 2.0f;
quat.z = 3.0f;
quat.w = 0.5f;
ld_float xx = 2.0f * quat.x;
ld_float yy = 2.0f * quat.y;
ld_float zz = 2.0f * quat.z;
ld_float ww = -2.0f * quat.w;
expected.m11 = 1.0f - yy * quat.y - zz * quat.z;
expected.m12 = xx * quat.y + ww * quat.z;
expected.m13 = xx * quat.z - ww * quat.y;
expected.m21 = xx * quat.y - ww * quat.z;
expected.m22 = 1.0f - xx * quat.x - zz * quat.z;
expected.m23 = yy * quat.z + ww * quat.x;
expected.m31 = xx * quat.z + ww * quat.y;
expected.m32 = yy * quat.z - ww * quat.x;
expected.m33 = 1.0f - xx * quat.x - yy * quat.y;
actual.fromQuaterion(quat);
TestUtil.COMPARE(expected.m11, actual.m11);
TestUtil.COMPARE(expected.m12, actual.m12);
TestUtil.COMPARE(expected.m13, actual.m13);
TestUtil.COMPARE(expected.m21, actual.m21);
TestUtil.COMPARE(expected.m22, actual.m22);
TestUtil.COMPARE(expected.m23, actual.m23);
TestUtil.COMPARE(expected.m31, actual.m31);
TestUtil.COMPARE(expected.m32, actual.m32);
TestUtil.COMPARE(expected.m33, actual.m33);
TestUtil.COMPARE(expected.m41, actual.m41);
TestUtil.COMPARE(expected.m42, actual.m42);
TestUtil.COMPARE(expected.m43, actual.m43);
}
//------ 設定 -------
public void fromQuaterion(LDQuat q)
{
#if Zero
// http://miffysora.wikidot.com/quaternion:matrix
ld_float ww = 2.0f * q.w;
ld_float xx = 2.0f * q.x;
ld_float yy = 2.0f * q.y;
ld_float zz = 2.0f * q.z;
// TODO 最適化
m11 = 1.0f - yy * q.y + zz * q.z;
m12 = xx * q.y + ww * q.z;
m13 = xx * q.z - ww * q.y;
m21 = xx * q.y - ww * q.z;
m22 = 1.0f - zz * q.z + xx * q.x;
m23 = yy * q.z + ww * q.x;
m31 = xx * q.z + ww * q.y;
m32 = yy * q.z - ww * q.x;
m33 = 1.0f - xx * q.x + yy * q.y;
// 平行移動をなくす
tx = ty = tz = 0.0f;
#else
//ゲーム3D数学方式（左手系）
#if EUC_3D_COORD_RIGHT_HAND
ld_float ww = -2.0f * q.w;
#else
ld_float ww = 2.0f * q.w;
#endif
ld_float xx = 2.0f * q.x;
ld_float yy = 2.0f * q.y;
ld_float zz = 2.0f * q.z;
// TODO 最適化
m11 = 1.0f - yy * q.y - zz * q.z;
m12 = xx * q.y + ww * q.z;
m13 = xx * q.z - ww * q.y;
m21 = xx * q.y - ww * q.z;
m22 = 1.0f - xx * q.x - zz * q.z;
m23 = yy * q.z + ww * q.x;
m31 = xx * q.z + ww * q.y;
m32 = yy * q.z - ww * q.x;
m33 = 1.0f - xx * q.x - yy * q.y;
// 平行移動をなくす
tx = ty = tz = 0.0f;
#endif
// 他の要素は要確認
m41 = m42 = m43 = 0.0f;
m44 = 1.0f;
}
public void fromQuaterionTest()
{
LDQuat quat=new LDQuat(0.0f, 0.0f, 0.0f, 0.0f );
LDMatrix44 expected = new LDMatrix44();
LDMatrix44 actual = new LDMatrix44();
//ld_float delta = 0.00001f;
// Input : actual = 単位行列, quat = 0.0 (ゼロ)
actual.fromQuaterion(quat);
TestUtil.COMPARE(expected.m11, actual.m11);
TestUtil.COMPARE(expected.m12, actual.m12);
TestUtil.COMPARE(expected.m13, actual.m13);
TestUtil.COMPARE(expected.m21, actual.m21);
TestUtil.COMPARE(expected.m22, actual.m22);
TestUtil.COMPARE(expected.m23, actual.m23);
TestUtil.COMPARE(expected.m31, actual.m31);
TestUtil.COMPARE(expected.m32, actual.m32);
TestUtil.COMPARE(expected.m33, actual.m33);
TestUtil.COMPARE(expected.m41, actual.m41);
TestUtil.COMPARE(expected.m42, actual.m42);
TestUtil.COMPARE(expected.m43, actual.m43);
// Input : actual = 単位行列, quat x = 1.0, y = 2.0, z = 3.0, w = 0.5 (任意の値)
quat.x = 1.0f;
quat.y = 2.0f;
quat.z = 3.0f;
quat.w = 0.5f;
ld_float xx = 2.0f * quat.x;
ld_float yy = 2.0f * quat.y;
ld_float zz = 2.0f * quat.z;
ld_float ww = -2.0f * quat.w;
expected.m11 = 1.0f - yy * quat.y - zz * quat.z;
expected.m12 = xx * quat.y + ww * quat.z;
expected.m13 = xx * quat.z - ww * quat.y;
expected.m21 = xx * quat.y - ww * quat.z;
expected.m22 = 1.0f - xx * quat.x - zz * quat.z;
expected.m23 = yy * quat.z + ww * quat.x;
expected.m31 = xx * quat.z + ww * quat.y;
expected.m32 = yy * quat.z - ww * quat.x;
expected.m33 = 1.0f - xx * quat.x - yy * quat.y;
actual.fromQuaterion(quat);
TestUtil.COMPARE(expected.m11, actual.m11);
TestUtil.COMPARE(expected.m12, actual.m12);
TestUtil.COMPARE(expected.m13, actual.m13);
TestUtil.COMPARE(expected.m21, actual.m21);
TestUtil.COMPARE(expected.m22, actual.m22);
TestUtil.COMPARE(expected.m23, actual.m23);
TestUtil.COMPARE(expected.m31, actual.m31);
TestUtil.COMPARE(expected.m32, actual.m32);
TestUtil.COMPARE(expected.m33, actual.m33);
TestUtil.COMPARE(expected.m41, actual.m41);
TestUtil.COMPARE(expected.m42, actual.m42);
TestUtil.COMPARE(expected.m43, actual.m43);
}
//------ operator ------
/**
* @brief 外積
*/
public static LDQuat operator *(LDQuat a, LDQuat b)
{
LDQuat result = new LDQuat();
result.w = a.w * b.w - a.x * b.x - a.y * b.y - a.z * b.z;
result.x = a.w * b.x + a.x * b.w + a.y * b.z - a.z * b.y;
result.y = a.w * b.y - a.x * b.z + a.y * b.w + a.z * b.x;
result.z = a.w * b.z + a.x * b.y - a.y * b.x + a.z * b.w;
// result.w = w*a.w - x*a.x - y*a.y - z*a.z ;
// result.x = w*a.x + x*a.w + y*a.y - z*a.z ;
// result.y = w*a.y + x*a.w + y*a.z - z*a.x ;
// result.z = w*a.z + x*a.w + y*a.x - z*a.y ;
return(result);
}
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 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);
}
/**
* @brief 慣性空間->オブジェクト空間へのクォータニオンからオイラー角を設定
* @param &q クォータニオンをセットする
*/
public void fromInertialToObjectQuaternion(LDQuat q)
{
ld_float sp = -2.0f * (q.y * q.z + q.w * q.x);
if ((ld_float)Math.Abs(sp) > 1.0f - LDMathUtil.GINBAL_TOLERANCE)
{
//真上か真下を向いている
pitch = LDMathUtil.PI_OVER_2 * sp;
heading = (ld_float)Math.Atan2(-q.x * q.z - q.w * q.y, 0.5f - q.y * q.y - q.z * q.z);
bank = 0.0f;
}
else
{
pitch = (ld_float)Math.Asin(sp);
heading = (ld_float)Math.Atan2(q.x * q.z - q.w * q.y, 0.5f - q.x * q.x - q.y * q.y);
bank = (ld_float)Math.Atan2(q.x * q.y - q.w * q.z, 0.5f - q.x * q.x - q.z * q.z);
}
}
/**
* @brief 慣性空間->オブジェクト空間へのクォータニオンからオイラー角を設定
* @param &q クォータニオンをセットする
*/
public void fromInertialToObjectQuaternion(LDQuat q)
{
ld_float sp = -2.0f * (q.y * q.z + q.w * q.x);
if ((ld_float)Math.Abs(sp) > 1.0f - LDMathUtil.GINBAL_TOLERANCE)
{
//真上か真下を向いている
pitch = LDMathUtil.PI_OVER_2 * sp;
heading = (ld_float)Math.Atan2(-q.x * q.z - q.w * q.y, 0.5f - q.y * q.y - q.z * q.z);
bank = 0.0f;
}
else
{
pitch = (ld_float)Math.Asin(sp);
heading = (ld_float)Math.Atan2(q.x * q.z - q.w * q.y, 0.5f - q.x * q.x - q.y * q.y);
bank = (ld_float)Math.Atan2(q.x * q.y - q.w * q.z, 0.5f - q.x * q.x - q.z * q.z);
}
}
public void powTest()
{
LDQuat quat = new LDQuat();
LDQuat actual = new LDQuat();
LDQuat expected = new LDQuat();
//ld_float delta = 0.00001f;
ld_float exponent;
// Input : quat1{x = 0.0, y = 0.0, z = 0.0, w = 1.0}, exponent = 0.0 (単位クォータニオン)
quat.x = 0.0f;
quat.y = 0.0f;
quat.z = 0.0f;
quat.w = 1.0f;
exponent = 0.0f;
expected.x = 0.0f;
expected.y = 0.0f;
expected.z = 0.0f;
expected.w = 1.0f;
actual = quat.pow(exponent);
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 : quat1{x = 0.1, y = 0.2, z = 0.3, w = 0.4}, exponent = 2.0 (任意の値)
quat.x = 0.1f;
quat.y = 0.2f;
quat.z = 0.3f;
quat.w = 0.4f;
exponent = 2.0f;
ld_float alpha = (float)Math.Acos(quat.w);
ld_float newAlpha = alpha * exponent;
ld_float mult = (float)Math.Sin(newAlpha) / (float)Math.Sin(alpha);
expected.x = quat.x * mult;
expected.y = quat.y * mult;
expected.z = quat.z * mult;
expected.w = (float)Math.Cos(newAlpha);
actual = quat.pow(exponent);
TestUtil.COMPARE(expected.x, actual.x);
TestUtil.COMPARE(expected.y, actual.y);
TestUtil.COMPARE(expected.z, actual.z);
TestUtil.COMPARE(expected.w, actual.w);
}
/**
* @brief オブジェクト空間->慣性空間へのクォータニオンからオイラー角を設定
* @param &q クォータニオンをセットする
*/
public void fromObjectToInertialQuaternion(LDQuat q)
{
ld_float sp = -2.0f * (q.y * q.z - q.w * q.x);
// ジンバルロックチェック
if (Math.Abs(sp) > 1.0f - LDMathUtil.GINBAL_TOLERANCE)
{
// 真上か下を向いている
pitch = LDMathUtil.PI_OVER_2 * sp;
heading = (ld_float)Math.Atan2(-q.x * q.z + q.w * q.y, 0.5f - q.y * q.y - q.z * q.z);
bank = 0.0f;
}
else
{
// 角度を計算する
pitch = (ld_float)Math.Asin(sp);
heading = (ld_float)Math.Atan2(q.x * q.z + q.w * q.y, 0.5f - q.x * q.x - q.y * q.y);
bank = (ld_float)Math.Atan2(q.x * q.y + q.w * q.z, 0.5f - q.x * q.x - q.z * q.z);
}
}
public void getAxisTest()
{
LDQuat quat = new LDQuat(0.0f, 0.0f, 0.0f, 0.0f);
LDVector3 actual = new LDVector3();
LDVector3 expected = new LDVector3();
//ld_float delta = 0.00001f;
// Input : x = 0.0, y = 0.0, z = 0.0, w = 0.0 (ゼロクォータニオン)
expected.zero();
actual = quat.getAxis();
TestUtil.COMPARE(expected.x, actual.x);
TestUtil.COMPARE(expected.y, actual.y);
TestUtil.COMPARE(expected.z, actual.z);
// Input : x = 0.0, y = 0.0, z = 0.0, w = 1.0 (単位クォータニオン)
quat.x = 0.0f;
quat.y = 0.0f;
quat.z = 0.0f;
quat.w = 1.0f;
expected.x = 1.0f;
expected.y = 0.0f;
expected.z = 0.0f;
actual = quat.getAxis();
TestUtil.COMPARE(expected.x, actual.x);
TestUtil.COMPARE(expected.y, actual.y);
TestUtil.COMPARE(expected.z, actual.z);
// Input : x = 1.0, y = 2.0, z = 3.0, w = 0.5 (任意の値)
quat.x = 1.0f;
quat.y = 2.0f;
quat.z = 3.0f;
quat.w = 0.5f;
expected.x = quat.x * 1.0f / (float)Math.Sqrt(1.0f - quat.w * quat.w);
expected.y = quat.y * 1.0f / (float)Math.Sqrt(1.0f - quat.w * quat.w);
expected.z = quat.z * 1.0f / (float)Math.Sqrt(1.0f - quat.w * quat.w);
actual = quat.getAxis();
TestUtil.COMPARE(expected.x, actual.x);
TestUtil.COMPARE(expected.y, actual.y);
TestUtil.COMPARE(expected.z, actual.z);
}
public void getAxisTest()
{
LDQuat quat = new LDQuat(0.0f, 0.0f, 0.0f, 0.0f);
LDVector3 actual = new LDVector3();
LDVector3 expected = new LDVector3();
//ld_float delta = 0.00001f;
// Input : x = 0.0, y = 0.0, z = 0.0, w = 0.0 (ゼロクォータニオン)
expected.zero();
actual = quat.getAxis();
TestUtil.COMPARE(expected.x, actual.x);
TestUtil.COMPARE(expected.y, actual.y);
TestUtil.COMPARE(expected.z, actual.z);
// Input : x = 0.0, y = 0.0, z = 0.0, w = 1.0 (単位クォータニオン)
quat.x = 0.0f;
quat.y = 0.0f;
quat.z = 0.0f;
quat.w = 1.0f;
expected.x = 1.0f;
expected.y = 0.0f;
expected.z = 0.0f;
actual = quat.getAxis();
TestUtil.COMPARE(expected.x, actual.x);
TestUtil.COMPARE(expected.y, actual.y);
TestUtil.COMPARE(expected.z, actual.z);
// Input : x = 1.0, y = 2.0, z = 3.0, w = 0.5 (任意の値)
quat.x = 1.0f;
quat.y = 2.0f;
quat.z = 3.0f;
quat.w = 0.5f;
expected.x = quat.x * 1.0f / (float)Math.Sqrt(1.0f - quat.w * quat.w);
expected.y = quat.y * 1.0f / (float)Math.Sqrt(1.0f - quat.w * quat.w);
expected.z = quat.z * 1.0f / (float)Math.Sqrt(1.0f - quat.w * quat.w);
actual = quat.getAxis();
TestUtil.COMPARE(expected.x, actual.x);
TestUtil.COMPARE(expected.y, actual.y);
TestUtil.COMPARE(expected.z, actual.z);
}
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 normalizeTest()
{
LDQuat actual = new LDQuat();
LDQuat expected = new LDQuat();
//ld_float delta = 0.00001f;
ld_float oneOverMag;
// Input : vector{x = 0.0, y = 0.0, z = 0.0, w = 1.0} (単位クォータニオン)
actual.x = 0.0f;
actual.y = 0.0f;
actual.z = 0.0f;
actual.w = 1.0f;
expected.x = 0.0f;
expected.y = 0.0f;
expected.z = 0.0f;
expected.w = 1.0f;
actual.normalize();
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 : vector{x = 1.0, y = 2.0, z = 3.0, w = 4.0} (任意の値)
actual.x = 1.0f;
actual.y = 2.0f;
actual.z = 3.0f;
actual.w = 4.0f;
oneOverMag = 1.0f / (ld_float)(float)Math.Sqrt(actual.w * actual.w + actual.x * actual.x
+ actual.y * actual.y + actual.z * actual.z);
expected.x = 1.0f * oneOverMag;
expected.y = 2.0f * oneOverMag;
expected.z = 3.0f * oneOverMag;
expected.w = 4.0f * oneOverMag;
actual.normalize();
TestUtil.COMPARE(expected.x, actual.x);
TestUtil.COMPARE(expected.y, actual.y);
TestUtil.COMPARE(expected.z, actual.z);
TestUtil.COMPARE(expected.w, actual.w);
}
//------ EQuat.h ------
public void setToRotateAxisTest()
{
LDVector3 vector = new LDVector3();
LDQuat actual = new LDQuat();
LDQuat expected = new LDQuat();
//ld_float delta = 0.00001f;
ld_float theta;
// Input : vector{x = 1.0, y = 0.0, z = 0.0}, theta = 0.0 (単位ベクトル)
vector.x = 1.0f;
vector.y = 0.0f;
vector.z = 0.0f;
vector.normalize();
theta = 0.0f;
expected.x = 0.0f;
expected.y = 0.0f;
expected.z = 0.0f;
actual.setToRotateAxis(vector, theta);
TestUtil.COMPARE(expected.x, actual.x);
TestUtil.COMPARE(expected.y, actual.y);
TestUtil.COMPARE(expected.z, actual.z);
// Input : vector{x = 1.0, y = 2.0, z = 3.0}, theta = EUC_PI_OVER_2 (任意の値)
vector.x = 1.0f;
vector.y = 2.0f;
vector.z = 3.0f;
vector.normalize();
theta = LDMathUtil.PI_OVER_2;
expected.x = vector.x * (float)Math.Sin(theta * 0.5f);
expected.y = vector.y * (float)Math.Sin(theta * 0.5f);
expected.z = vector.z * (float)Math.Sin(theta * 0.5f);
expected.w = (float)Math.Cos(theta * 0.5f);
actual.setToRotateAxis(vector, theta);
TestUtil.COMPARE(expected.x, actual.x);
TestUtil.COMPARE(expected.y, actual.y);
TestUtil.COMPARE(expected.z, actual.z);
TestUtil.COMPARE(expected.w, actual.w);
}
/**
* @brief 累乗 pow
* @param exponent クォータニオンの乗数をセットする
* @return クォータニオンの累乗を返す
*/
LDQuat pow(ld_float exponent)
{
LDQuat q = this;
if (Math.Abs(q.w) > 0.9999f)
{
return(q); //0-divideを防ぐ
}
ld_float alpha = (ld_float)Math.Acos(q.w);
ld_float newAlpha = alpha * exponent;
LDQuat result = new LDQuat();
result.w = (ld_float)Math.Cos(newAlpha);
ld_float mult = (ld_float)Math.Sin(newAlpha) / (ld_float)Math.Sin(alpha);
result.x = q.x * mult;
result.y = q.y * mult;
result.z = q.z * mult;
return(result);
} // not public
} // not public
/**
* @brief 内積
*/
public ld_float dot(LDQuat b)
{
return(w * b.w + x * b.x + y * b.y + z * b.z);
}
public static LDQuat mySlerp(LDQuat q0, LDQuat q1, ld_float t)
{
return(q0.mySlerp(q1, t));
}
/**
* @brief 球面線形補間
* @param &q1 クォータニオン(EQuat)をセットする
* @param t 補間位置を表す数値をセットする
* @return 補間結果を返す
*/
LDQuat slerp(LDQuat q1, ld_float t)
{
LDQuat q0 = this;
// 範囲外の時は端点を返す
if (t <= 0.0f)
{
return(q0);
}
if (t >= 1.0f)
{
return(q1);
}
// 内積でクォータニオン間の角度のcosを計算する
ld_float cosOmega = q0.dot(q1);
ld_float q1w = q1.w;
ld_float q1x = q1.x;
ld_float q1y = q1.y;
ld_float q1z = q1.z;
if (cosOmega < 0.0f)
{
q1w = -q1w;
q1x = -q1x;
q1y = -q1y;
q1z = -q1z;
cosOmega = -cosOmega;
}
// 各クォータニオンは、単位クォータニオンである必要があり、内積は <= 1.0 になるはず
Debug.Assert(cosOmega < 1.1f);
ld_float k0, k1;
if (cosOmega > 0.9999f)
{
// 非常に近いので、単純に線形補間を用いる（0-divideを防ぐ）
k0 = 1.0f - t;
k1 = t;
}
else
{
ld_float sinOmega = (ld_float)Math.Sqrt(1.0f - cosOmega * cosOmega);
ld_float omega = (ld_float)Math.Atan2(sinOmega, cosOmega);
ld_float one_overSinOmega = 1.0f / sinOmega;
k0 = (ld_float)Math.Sin((1.0f - t) * omega) * one_overSinOmega;
k1 = (ld_float)Math.Sin(t * omega) * one_overSinOmega;
}
// 補間する
LDQuat result = new LDQuat();
result.x = k0 * q0.x + k1 * q1x;
result.y = k0 * q0.y + k1 * q1y;
result.z = k0 * q0.z + k1 * q1z;
result.w = k0 * q0.w + k1 * q1w;
return(result);
} // not public
/**
* @brief 累乗 pow
* @param exponent クォータニオンの乗数をセットする
* @return クォータニオンの累乗を返す
*/
public LDQuat pow(ld_float exponent)
{
LDQuat q = this;
if (Math.Abs(q.w) > 0.9999f)
{
return q; //0-divideを防ぐ
}
ld_float alpha = (ld_float)Math.Acos(q.w);
ld_float newAlpha = alpha * exponent;
LDQuat result = new LDQuat();
result.w = (ld_float)Math.Cos(newAlpha);
ld_float mult = (ld_float)Math.Sin(newAlpha) / (ld_float)Math.Sin(alpha);
result.x = q.x * mult;
result.y = q.y * mult;
result.z = q.z * mult;
return result;
}
public void mySlerpTest()
{
LDQuat quat1 = new LDQuat();
LDQuat quat2 = new LDQuat();
LDQuat actual = new LDQuat();
LDQuat expected = new LDQuat();
//ld_float delta = 0.00001f;
ld_float t;
// Input : quat1{x = 1.0, y = 2.0, z = 3.0, w = 0.1}, quat2{x = 4.0, y = 5.0, z = 6.0, w = 0.2}, t = 0.0
// 範囲外t <= 0.0fのケース
quat1.x = 1.0f;
quat1.y = 2.0f;
quat1.z = 3.0f;
quat1.w = 0.1f;
quat2.x = 4.0f;
quat2.y = 5.0f;
quat2.z = 6.0f;
quat2.w = 0.2f;
t = 0.0f;
expected.x = 4.0f;
expected.y = 5.0f;
expected.z = 6.0f;
expected.w = 0.2f;
actual = quat2.mySlerp(quat1, t);
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 : quat1{x = 1.0, y = 2.0, z = 3.0, w = 0.1}, quat2{x = 4.0, y = 5.0, z = 6.0, w = 0.2}, t = 1.0
// 範囲外t >= 1.0fのケース
quat1.x = 1.0f;
quat1.y = 2.0f;
quat1.z = 3.0f;
quat1.w = 0.1f;
quat2.x = 4.0f;
quat2.y = 5.0f;
quat2.z = 6.0f;
quat2.w = 0.2f;
t = 1.0f;
expected.x = 1.0f;
expected.y = 2.0f;
expected.z = 3.0f;
expected.w = 0.1f;
actual = quat2.mySlerp(quat1, t);
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 : quat1{x = 0.49, y = 0.5, z = 0.5, w = 0.5}, quat2{x = 0.5, y = 0.49, z = 0.5, w = 0.5}, t = 0.5
// cosOmega <= 0.9999fのケース
quat1.x = 0.49f;
quat1.y = 0.5f;
quat1.z = 0.5f;
quat1.w = 0.5f;
quat2.x = 0.5f;
quat2.y = 0.49f;
quat2.z = 0.5f;
quat2.w = 0.5f;
t = 0.5f;
LDVector3 n0 = quat2.getAxis();
LDVector3 n1 = quat1.getAxis();
ld_float a0 = quat2.getAngle();
ld_float a1 = quat1.getAngle();
ld_float adiff = LDMathUtil.getAngleDiff(a1, a0);
ld_float at = a0 + adiff * t;
LDVector3 nt = LDVector3.blend(n0, n1, 1 - t, t);
nt.normalize();
expected.setToRotateAxis(nt, at);
actual = quat2.mySlerp(quat1, t);
TestUtil.COMPARE(expected.x, actual.x);
TestUtil.COMPARE(expected.y, actual.y);
TestUtil.COMPARE(expected.z, actual.z);
TestUtil.COMPARE(expected.w, actual.w);
}
//------ EQuat.h ------
public void setToRotateAxisTest()
{
LDVector3 vector=new LDVector3();
LDQuat actual = new LDQuat();
LDQuat expected = new LDQuat();
//ld_float delta = 0.00001f;
ld_float theta;
// Input : vector{x = 1.0, y = 0.0, z = 0.0}, theta = 0.0 (単位ベクトル)
vector.x = 1.0f;
vector.y = 0.0f;
vector.z = 0.0f;
vector.normalize();
theta = 0.0f;
expected.x = 0.0f;
expected.y = 0.0f;
expected.z = 0.0f;
actual.setToRotateAxis(vector, theta);
TestUtil.COMPARE(expected.x, actual.x);
TestUtil.COMPARE(expected.y, actual.y);
TestUtil.COMPARE(expected.z, actual.z);
// Input : vector{x = 1.0, y = 2.0, z = 3.0}, theta = EUC_PI_OVER_2 (任意の値)
vector.x = 1.0f;
vector.y = 2.0f;
vector.z = 3.0f;
vector.normalize();
theta = LDMathUtil.PI_OVER_2;
expected.x = vector.x * (float)Math.Sin(theta * 0.5f);
expected.y = vector.y * (float)Math.Sin(theta * 0.5f);
expected.z = vector.z * (float)Math.Sin(theta * 0.5f);
expected.w = (float)Math.Cos(theta * 0.5f);
actual.setToRotateAxis(vector, theta);
TestUtil.COMPARE(expected.x, actual.x);
TestUtil.COMPARE(expected.y, actual.y);
TestUtil.COMPARE(expected.z, actual.z);
TestUtil.COMPARE(expected.w, actual.w);
}
/**
* @brief オブジェクト空間->慣性空間へのクォータニオンからオイラー角を設定
* @param &q クォータニオンをセットする
*/
public void fromObjectToInertialQuaternion(LDQuat q)
{
ld_float sp = -2.0f * (q.y * q.z - q.w * q.x);
// ジンバルロックチェック
if (Math.Abs(sp) > 1.0f - LDMathUtil.GINBAL_TOLERANCE)
{
// 真上か下を向いている
pitch = LDMathUtil.PI_OVER_2 * sp;
heading = (ld_float)Math.Atan2(-q.x * q.z + q.w * q.y, 0.5f - q.y * q.y - q.z * q.z);
bank = 0.0f;
}
else
{
// 角度を計算する
pitch = (ld_float)Math.Asin(sp);
heading = (ld_float)Math.Atan2(q.x * q.z + q.w * q.y, 0.5f - q.x * q.x - q.y * q.y);
bank = (ld_float)Math.Atan2(q.x * q.y + q.w * q.z, 0.5f - q.x * q.x - q.z * q.z);
}
}
/**
* @brief 内積
*/
public ld_float dot(LDQuat b)
{
return w * b.w + x * b.x + y * b.y + z * b.z;
}
//------ 設定 -------
public void fromQuaterion(LDQuat q)
{
#if Zero
// http://miffysora.wikidot.com/quaternion:matrix
ld_float ww = 2.0f * q.w ;
ld_float xx = 2.0f * q.x ;
ld_float yy = 2.0f * q.y ;
ld_float zz = 2.0f * q.z ;
// TODO 最適化
m11 = 1.0f - yy*q.y + zz*q.z ;
m12 = xx*q.y + ww*q.z ;
m13 = xx*q.z - ww*q.y ;
m21 = xx*q.y - ww*q.z ;
m22 = 1.0f - zz*q.z + xx*q.x ;
m23 = yy*q.z + ww*q.x ;
m31 = xx*q.z + ww*q.y ;
m32 = yy*q.z - ww*q.x ;
m33 = 1.0f - xx*q.x + yy*q.y ;
// 平行移動をなくす
tx = ty = tz = 0.0f ;
#else
//ゲーム3D数学方式（左手系）
#if EUC_3D_COORD_RIGHT_HAND
ld_float ww = -2.0f * q.w;
#else
ld_float ww = 2.0f * q.w;
#endif
ld_float xx = 2.0f * q.x;
ld_float yy = 2.0f * q.y;
ld_float zz = 2.0f * q.z;
// TODO 最適化
m11 = 1.0f - yy * q.y - zz * q.z;
m12 = xx * q.y + ww * q.z;
m13 = xx * q.z - ww * q.y;
m21 = xx * q.y - ww * q.z;
m22 = 1.0f - xx * q.x - zz * q.z;
m23 = yy * q.z + ww * q.x;
m31 = xx * q.z + ww * q.y;
m32 = yy * q.z - ww * q.x;
m33 = 1.0f - xx * q.x - yy * q.y;
// 平行移動をなくす
tx = ty = tz = 0.0f;
#endif
}
/// <summary>
/// TODO:要実装 LDFUZZY_COMPARE
/// </summary>
public void setToRotationArcTest()
{
LDVector3 vector1 = new LDVector3();
LDVector3 vector2 = new LDVector3();
LDVector3 vector3 = new LDVector3();
LDQuat actual = new LDQuat();
LDQuat expected = new LDQuat();
//ld_float delta = 0.00001f;
ld_float d;
// Input : vector1{x = 0.0, y = 0.0, z = 0.0}, vector2 {x = 0.0, y = 0.0, z = 0.0}, useNormalize = false
// actual {x = 0.0, y = 0.0, z = 0.0, w = 0.0}
// useNormalize = false, (ゼロベクトル)
vector1.zero();
vector2.zero();
actual.x = 0.0f;
actual.y = 0.0f;
actual.z = 0.0f;
actual.w = 0.0f;
expected.x = 0.0f;
expected.y = 0.0f;
expected.z = 0.0f;
expected.w = (float)Math.Sqrt(2.0f) / 2.0f;
actual.setToRotationArc(vector1, vector2, false);
TestUtil.COMPARE(expected.x, actual.x);
TestUtil.COMPARE(expected.y, actual.y);
TestUtil.COMPARE(expected.z, actual.z);
//LDFUZZY_COMPARE(expected.w, actual.w, TEST_TOLERANCE);
// QVERIFY(qFuzzyCompare(expected.w, actual.w));
// TestUtil.COMPARE(expected.w, actual.w);//極小誤差によりFalse
// Input : vector1{x = 0.0, y = 0.0, z = 0.0}, vector2 {x = 0.0, y = 0.0, z = 0.0}, useNormalize = true
// actual {x = 0.0, y = 0.0, z = 0.0, w = 0.0}
// useNormalize = true, (ゼロベクトル)
vector1.zero();
vector2.zero();
actual.x = 0.0f;
actual.y = 0.0f;
actual.z = 0.0f;
actual.w = 0.0f;
expected.x = 0.0f;
expected.y = 0.0f;
expected.z = 0.0f;
expected.w = (float)Math.Sqrt(2.0f) / 2.0f;
actual.setToRotationArc(vector1, vector2, true);
TestUtil.COMPARE(expected.x, actual.x);
TestUtil.COMPARE(expected.y, actual.y);
TestUtil.COMPARE(expected.z, actual.z);
//LDFUZZY_COMPARE(expected.w, actual.w, TEST_TOLERANCE);
// TestUtil.COMPARE(expected.w, actual.w);//極小誤差によりFalse
// Input : vector1{x = 1.0, y = 2.0, z = 3.0}, vector2 {x = 4.0, y = 5.0, z = 6.0}, useNormalize = false
// actual {x = 1.0, y = 2.0, z = 3.0, w = 0.5}
// useNormalize = false, (任意の値)
vector1.x = 1.0f;
vector1.y = 2.0f;
vector1.z = 3.0f;
vector2.x = 4.0f;
vector2.y = 5.0f;
vector2.z = 6.0f;
actual.x = 1.0f;
actual.y = 2.0f;
actual.z = 3.0f;
actual.w = 0.5f;
vector3 = LDVector3.crossProduct(vector1, vector2);
d = vector1.dotProduct(vector2);
expected.x = vector3.x / (float)Math.Sqrt((1 + d) * 2);
expected.y = vector3.y / (float)Math.Sqrt((1 + d) * 2);
expected.z = vector3.z / (float)Math.Sqrt((1 + d) * 2);
expected.w = (float)Math.Sqrt((1 + d) * 2) / 2.0f;
actual.setToRotationArc(vector1, vector2, false);
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 : vector1{x = 1.0, y = 2.0, z = 3.0}, vector2 {x = 4.0, y = 5.0, z = 6.0}, useNormalize = true
// actual {x = 1.0, y = 2.0, z = 3.0, w = 0.5}
// useNormalize = true, (任意の値)
vector1.x = 1.0f;
vector1.y = 2.0f;
vector1.z = 3.0f;
vector2.x = 4.0f;
vector2.y = 5.0f;
vector2.z = 6.0f;
actual.x = 1.0f;
actual.y = 2.0f;
actual.z = 3.0f;
actual.w = 0.5f;
vector1.normalize();
vector2.normalize();
vector3 = LDVector3.crossProduct(vector1, vector2);
d = vector1.dotProduct(vector2);
expected.x = vector3.x / (float)Math.Sqrt((1 + d) * 2);
expected.y = vector3.y / (float)Math.Sqrt((1 + d) * 2);
expected.z = vector3.z / (float)Math.Sqrt((1 + d) * 2);
expected.w = (float)Math.Sqrt((1 + d) * 2) / 2.0f;
actual.setToRotationArc(vector1, vector2, true);
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 mySlerpTest()
{
LDQuat quat1 = new LDQuat();
LDQuat quat2 = new LDQuat();
LDQuat actual = new LDQuat();
LDQuat expected = new LDQuat();
//ld_float delta = 0.00001f;
ld_float t;
// Input : quat1{x = 1.0, y = 2.0, z = 3.0, w = 0.1}, quat2{x = 4.0, y = 5.0, z = 6.0, w = 0.2}, t = 0.0
// 範囲外t <= 0.0fのケース
quat1.x = 1.0f;
quat1.y = 2.0f;
quat1.z = 3.0f;
quat1.w = 0.1f;
quat2.x = 4.0f;
quat2.y = 5.0f;
quat2.z = 6.0f;
quat2.w = 0.2f;
t = 0.0f;
expected.x = 4.0f;
expected.y = 5.0f;
expected.z = 6.0f;
expected.w = 0.2f;
actual = quat2.mySlerp(quat1, t);
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 : quat1{x = 1.0, y = 2.0, z = 3.0, w = 0.1}, quat2{x = 4.0, y = 5.0, z = 6.0, w = 0.2}, t = 1.0
// 範囲外t >= 1.0fのケース
quat1.x = 1.0f;
quat1.y = 2.0f;
quat1.z = 3.0f;
quat1.w = 0.1f;
quat2.x = 4.0f;
quat2.y = 5.0f;
quat2.z = 6.0f;
quat2.w = 0.2f;
t = 1.0f;
expected.x = 1.0f;
expected.y = 2.0f;
expected.z = 3.0f;
expected.w = 0.1f;
actual = quat2.mySlerp(quat1, t);
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 : quat1{x = 0.49, y = 0.5, z = 0.5, w = 0.5}, quat2{x = 0.5, y = 0.49, z = 0.5, w = 0.5}, t = 0.5
// cosOmega <= 0.9999fのケース
quat1.x = 0.49f;
quat1.y = 0.5f;
quat1.z = 0.5f;
quat1.w = 0.5f;
quat2.x = 0.5f;
quat2.y = 0.49f;
quat2.z = 0.5f;
quat2.w = 0.5f;
t = 0.5f;
LDVector3 n0 = quat2.getAxis();
LDVector3 n1 = quat1.getAxis();
ld_float a0 = quat2.getAngle();
ld_float a1 = quat1.getAngle();
ld_float adiff = LDMathUtil.getAngleDiff(a1, a0);
ld_float at = a0 + adiff * t;
LDVector3 nt = LDVector3.blend(n0, n1, 1 - t, t);
nt.normalize();
expected.setToRotateAxis(nt, at);
actual = quat2.mySlerp(quat1, t);
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 fromQuaterion(LDQuat q)
{
}
/**
* @brief 球面線形補間
* @param &q1 クォータニオン(EQuat)をセットする
* @param t 補間位置を表す数値をセットする
* @return 補間結果を返す
*/
public LDQuat slerp(LDQuat q1, ld_float t)
{
LDQuat q0 = this;
// 範囲外の時は端点を返す
if (t <= 0.0f)
{
return q0;
}
if (t >= 1.0f)
{
return q1;
}
// 内積でクォータニオン間の角度のcosを計算する
ld_float cosOmega = q0.dot(q1);
ld_float q1w = q1.w;
ld_float q1x = q1.x;
ld_float q1y = q1.y;
ld_float q1z = q1.z;
if (cosOmega < 0.0f)
{
q1w = -q1w;
q1x = -q1x;
q1y = -q1y;
q1z = -q1z;
cosOmega = -cosOmega;
}
// 各クォータニオンは、単位クォータニオンである必要があり、内積は <= 1.0 になるはず
Debug.Assert(cosOmega < 1.1f);
ld_float k0, k1;
if (cosOmega > 0.9999f)
{
// 非常に近いので、単純に線形補間を用いる（0-divideを防ぐ）
k0 = 1.0f - t;
k1 = t;
}
else
{
ld_float sinOmega = (ld_float)Math.Sqrt(1.0f - cosOmega * cosOmega);
ld_float omega = (ld_float)Math.Atan2(sinOmega, cosOmega);
ld_float one_overSinOmega = 1.0f / sinOmega;
k0 = (ld_float)Math.Sin((1.0f - t) * omega) * one_overSinOmega;
k1 = (ld_float)Math.Sin(t * omega) * one_overSinOmega;
}
// 補間する
LDQuat result = new LDQuat();
result.x = k0 * q0.x + k1 * q1x;
result.y = k0 * q0.y + k1 * q1y;
result.z = k0 * q0.z + k1 * q1z;
result.w = k0 * q0.w + k1 * q1w;
return result;
}
/**
* @brief 独自の球面線形補間（回転角度を保つ）。y軸90度、z軸90度の回転をSLERPで計算する場合、t が変化した時、軸は最小距離を移動するが、角度は、90度ではなく小さな値となる。この計算を 90度の回転を保って、中心を通る計算についてテスト実装
* @param &q1 クォータニオン(EQuat)をセットする
* @param t 補間位置を表す数値をセットする
* @return 補間結果を返す
*/
public LDQuat mySlerp(LDQuat q1, ld_float t)
{
LDQuat q0 = this;
// 範囲外の時は端点を返す
if (t <= 0.0f)
{
return q0;
}
if (t >= 1.0f)
{
return q1;
}
LDVector3 n0 = q0.getAxis();
LDVector3 n1 = q1.getAxis();
ld_float a0 = q0.getAngle();
ld_float a1 = q1.getAngle();
//
ld_float adiff = LDMathUtil.getAngleDiff(a1, a0);
ld_float at = a0 + adiff * t;
LDVector3 nt = LDVector3.blend(n0, n1, 1 - t, t);
nt.normalize();
LDQuat ret = new LDQuat();
ret.setToRotateAxis(nt, at);
return ret;
}
public void powTest()
{
LDQuat quat = new LDQuat();
LDQuat actual = new LDQuat();
LDQuat expected = new LDQuat();
//ld_float delta = 0.00001f;
ld_float exponent;
// Input : quat1{x = 0.0, y = 0.0, z = 0.0, w = 1.0}, exponent = 0.0 (単位クォータニオン)
quat.x = 0.0f;
quat.y = 0.0f;
quat.z = 0.0f;
quat.w = 1.0f;
exponent = 0.0f;
expected.x = 0.0f;
expected.y = 0.0f;
expected.z = 0.0f;
expected.w = 1.0f;
actual = quat.pow(exponent);
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 : quat1{x = 0.1, y = 0.2, z = 0.3, w = 0.4}, exponent = 2.0 (任意の値)
quat.x = 0.1f;
quat.y = 0.2f;
quat.z = 0.3f;
quat.w = 0.4f;
exponent = 2.0f;
ld_float alpha = (float)Math.Acos(quat.w);
ld_float newAlpha = alpha * exponent;
ld_float mult = (float)Math.Sin(newAlpha) / (float)Math.Sin(alpha);
expected.x = quat.x * mult;
expected.y = quat.y * mult;
expected.z = quat.z * mult;
expected.w = (float)Math.Cos(newAlpha);
actual = quat.pow(exponent);
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 normalizeTest()
{
LDQuat actual = new LDQuat();
LDQuat expected = new LDQuat();
//ld_float delta = 0.00001f;
ld_float oneOverMag;
// Input : vector{x = 0.0, y = 0.0, z = 0.0, w = 1.0} (単位クォータニオン)
actual.x = 0.0f;
actual.y = 0.0f;
actual.z = 0.0f;
actual.w = 1.0f;
expected.x = 0.0f;
expected.y = 0.0f;
expected.z = 0.0f;
expected.w = 1.0f;
actual.normalize();
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 : vector{x = 1.0, y = 2.0, z = 3.0, w = 4.0} (任意の値)
actual.x = 1.0f;
actual.y = 2.0f;
actual.z = 3.0f;
actual.w = 4.0f;
oneOverMag = 1.0f / (ld_float)(float)Math.Sqrt(actual.w * actual.w + actual.x * actual.x
+ actual.y * actual.y + actual.z * actual.z);
expected.x = 1.0f * oneOverMag;
expected.y = 2.0f * oneOverMag;
expected.z = 3.0f * oneOverMag;
expected.w = 4.0f * oneOverMag;
actual.normalize();
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 fromQuaterion(LDQuat q)
{
}
public void slerpTest()
{
LDQuat quat1 = new LDQuat();
LDQuat quat2 = new LDQuat();
LDQuat actual = new LDQuat();
LDQuat expected = new LDQuat();
//ld_float delta = 0.00001f;
ld_float t;
// Input : quat1{x = 1.0, y = 2.0, z = 3.0, w = 0.1}, quat2{x = 4.0, y = 5.0, z = 6.0, w = 0.2}, t = 0.0
// 範囲外t <= 0.0fのケース
quat1.x = 1.0f;
quat1.y = 2.0f;
quat1.z = 3.0f;
quat1.w = 0.1f;
quat2.x = 4.0f;
quat2.y = 5.0f;
quat2.z = 6.0f;
quat2.w = 0.2f;
t = 0.0f;
expected.x = 4.0f;
expected.y = 5.0f;
expected.z = 6.0f;
expected.w = 0.2f;
actual = quat2.slerp(quat1, t);
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 : quat1{x = 1.0, y = 2.0, z = 3.0, w = 0.1}, quat2{x = 4.0, y = 5.0, z = 6.0, w = 0.2}, t = 1.0
// 範囲外t >= 1.0fのケース
quat1.x = 1.0f;
quat1.y = 2.0f;
quat1.z = 3.0f;
quat1.w = 0.1f;
quat2.x = 4.0f;
quat2.y = 5.0f;
quat2.z = 6.0f;
quat2.w = 0.2f;
t = 1.0f;
expected.x = 1.0f;
expected.y = 2.0f;
expected.z = 3.0f;
expected.w = 0.1f;
actual = quat2.slerp(quat1, t);
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 : quat1{x = 0.0, y = 0.0, z = 0.0, w = 1.0}, quat2{x = 0.0, y = 0.0, z = 0.0, w = 1.0}, t = 0.5
// cosOmega > 0.9999fのケース
quat1.x = 0.0f;
quat1.y = 0.0f;
quat1.z = 0.0f;
quat1.w = 1.0f;
quat2.x = 0.0f;
quat2.y = 0.0f;
quat2.z = 0.0f;
quat2.w = 1.0f;
t = 0.5f;
expected.x = 0.0f;
expected.y = 0.0f;
expected.z = 0.0f;
expected.w = 1.0f;
actual = quat2.slerp(quat1, t);
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 : quat1{x = 0.49, y = 0.5, z = 0.5, w = 0.5}, quat2{x = 0.5, y = 0.49, z = 0.5, w = 0.5}, t = 0.5
// cosOmega <= 0.9999fのケース
quat1.x = 0.49f;
quat1.y = 0.5f;
quat1.z = 0.5f;
quat1.w = 0.5f;
quat2.x = 0.5f;
quat2.y = 0.49f;
quat2.z = 0.5f;
quat2.w = 0.5f;
t = 0.5f;
ld_float cosOmega = quat2.dot(quat1);
ld_float sinOmega = (float)Math.Sqrt(1.0f - cosOmega * cosOmega);
ld_float omega = (ld_float)Math.Atan2(sinOmega, cosOmega);
ld_float one_overSinOmega = 1.0f / sinOmega;
ld_float k0 = (float)Math.Sin((1.0f - t) * omega) * one_overSinOmega;
ld_float k1 = (float)Math.Sin(t * omega) * one_overSinOmega;
expected.x = k0 * quat2.x + k1 * quat1.x;
expected.y = k0 * quat2.y + k1 * quat1.y;
expected.z = k0 * quat2.z + k1 * quat1.z;
expected.w = k0 * quat2.w + k1 * quat1.w;
actual = quat2.slerp(quat1, t);
TestUtil.COMPARE(expected.x, actual.x);
TestUtil.COMPARE(expected.y, actual.y);
TestUtil.COMPARE(expected.z, actual.z);
TestUtil.COMPARE(expected.w, actual.w);
}
//------ operator ------
/**
* @brief 外積
*/
public static LDQuat operator *(LDQuat a, LDQuat b)
{
LDQuat result = new LDQuat();
result.w = a.w * b.w - a.x * b.x - a.y * b.y - a.z * b.z;
result.x = a.w * b.x + a.x * b.w + a.y * b.z - a.z * b.y;
result.y = a.w * b.y - a.x * b.z + a.y * b.w + a.z * b.x;
result.z = a.w * b.z + a.x * b.y - a.y * b.x + a.z * b.w;
// result.w = w*a.w - x*a.x - y*a.y - z*a.z ;
// result.x = w*a.x + x*a.w + y*a.y - z*a.z ;
// result.y = w*a.y + x*a.w + y*a.z - z*a.x ;
// result.z = w*a.z + x*a.w + y*a.x - z*a.y ;
return result;
}
/// <summary>
/// TODO:要実装 LDFUZZY_COMPARE
/// </summary>
public void setToRotationArcTest()
{
LDVector3 vector1 = new LDVector3();
LDVector3 vector2 = new LDVector3();
LDVector3 vector3 = new LDVector3();
LDQuat actual = new LDQuat();
LDQuat expected = new LDQuat();
//ld_float delta = 0.00001f;
ld_float d;
// Input : vector1{x = 0.0, y = 0.0, z = 0.0}, vector2 {x = 0.0, y = 0.0, z = 0.0}, useNormalize = false
// actual {x = 0.0, y = 0.0, z = 0.0, w = 0.0}
// useNormalize = false, (ゼロベクトル)
vector1.zero();
vector2.zero();
actual.x = 0.0f;
actual.y = 0.0f;
actual.z = 0.0f;
actual.w = 0.0f;
expected.x = 0.0f;
expected.y = 0.0f;
expected.z = 0.0f;
expected.w = (float)Math.Sqrt(2.0f) / 2.0f;
actual.setToRotationArc(vector1, vector2, false);
TestUtil.COMPARE(expected.x, actual.x);
TestUtil.COMPARE(expected.y, actual.y);
TestUtil.COMPARE(expected.z, actual.z);
//LDFUZZY_COMPARE(expected.w, actual.w, TEST_TOLERANCE);
// QVERIFY(qFuzzyCompare(expected.w, actual.w));
// TestUtil.COMPARE(expected.w, actual.w);//極小誤差によりFalse
// Input : vector1{x = 0.0, y = 0.0, z = 0.0}, vector2 {x = 0.0, y = 0.0, z = 0.0}, useNormalize = true
// actual {x = 0.0, y = 0.0, z = 0.0, w = 0.0}
// useNormalize = true, (ゼロベクトル)
vector1.zero();
vector2.zero();
actual.x = 0.0f;
actual.y = 0.0f;
actual.z = 0.0f;
actual.w = 0.0f;
expected.x = 0.0f;
expected.y = 0.0f;
expected.z = 0.0f;
expected.w = (float)Math.Sqrt(2.0f) / 2.0f;
actual.setToRotationArc(vector1, vector2, true);
TestUtil.COMPARE(expected.x, actual.x);
TestUtil.COMPARE(expected.y, actual.y);
TestUtil.COMPARE(expected.z, actual.z);
//LDFUZZY_COMPARE(expected.w, actual.w, TEST_TOLERANCE);
// TestUtil.COMPARE(expected.w, actual.w);//極小誤差によりFalse
// Input : vector1{x = 1.0, y = 2.0, z = 3.0}, vector2 {x = 4.0, y = 5.0, z = 6.0}, useNormalize = false
// actual {x = 1.0, y = 2.0, z = 3.0, w = 0.5}
// useNormalize = false, (任意の値)
vector1.x = 1.0f;
vector1.y = 2.0f;
vector1.z = 3.0f;
vector2.x = 4.0f;
vector2.y = 5.0f;
vector2.z = 6.0f;
actual.x = 1.0f;
actual.y = 2.0f;
actual.z = 3.0f;
actual.w = 0.5f;
vector3 = LDVector3.crossProduct(vector1, vector2);
d = vector1.dotProduct(vector2);
expected.x = vector3.x / (float)Math.Sqrt((1 + d) * 2);
expected.y = vector3.y / (float)Math.Sqrt((1 + d) * 2);
expected.z = vector3.z / (float)Math.Sqrt((1 + d) * 2);
expected.w = (float)Math.Sqrt((1 + d) * 2) / 2.0f;
actual.setToRotationArc(vector1, vector2, false);
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 : vector1{x = 1.0, y = 2.0, z = 3.0}, vector2 {x = 4.0, y = 5.0, z = 6.0}, useNormalize = true
// actual {x = 1.0, y = 2.0, z = 3.0, w = 0.5}
// useNormalize = true, (任意の値)
vector1.x = 1.0f;
vector1.y = 2.0f;
vector1.z = 3.0f;
vector2.x = 4.0f;
vector2.y = 5.0f;
vector2.z = 6.0f;
actual.x = 1.0f;
actual.y = 2.0f;
actual.z = 3.0f;
actual.w = 0.5f;
vector1.normalize();
vector2.normalize();
vector3 = LDVector3.crossProduct(vector1, vector2);
d = vector1.dotProduct(vector2);
expected.x = vector3.x / (float)Math.Sqrt((1 + d) * 2);
expected.y = vector3.y / (float)Math.Sqrt((1 + d) * 2);
expected.z = vector3.z / (float)Math.Sqrt((1 + d) * 2);
expected.w = (float)Math.Sqrt((1 + d) * 2) / 2.0f;
actual.setToRotationArc(vector1, vector2, true);
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 static LDQuat slerp(LDQuat q0, LDQuat q1, ld_float t)
{
return q0.slerp(q1, t);
}
public void slerpTest()
{
LDQuat quat1 = new LDQuat();
LDQuat quat2 = new LDQuat();
LDQuat actual = new LDQuat();
LDQuat expected = new LDQuat();
//ld_float delta = 0.00001f;
ld_float t;
// Input : quat1{x = 1.0, y = 2.0, z = 3.0, w = 0.1}, quat2{x = 4.0, y = 5.0, z = 6.0, w = 0.2}, t = 0.0
// 範囲外t <= 0.0fのケース
quat1.x = 1.0f;
quat1.y = 2.0f;
quat1.z = 3.0f;
quat1.w = 0.1f;
quat2.x = 4.0f;
quat2.y = 5.0f;
quat2.z = 6.0f;
quat2.w = 0.2f;
t = 0.0f;
expected.x = 4.0f;
expected.y = 5.0f;
expected.z = 6.0f;
expected.w = 0.2f;
actual = quat2.slerp(quat1, t);
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 : quat1{x = 1.0, y = 2.0, z = 3.0, w = 0.1}, quat2{x = 4.0, y = 5.0, z = 6.0, w = 0.2}, t = 1.0
// 範囲外t >= 1.0fのケース
quat1.x = 1.0f;
quat1.y = 2.0f;
quat1.z = 3.0f;
quat1.w = 0.1f;
quat2.x = 4.0f;
quat2.y = 5.0f;
quat2.z = 6.0f;
quat2.w = 0.2f;
t = 1.0f;
expected.x = 1.0f;
expected.y = 2.0f;
expected.z = 3.0f;
expected.w = 0.1f;
actual = quat2.slerp(quat1, t);
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 : quat1{x = 0.0, y = 0.0, z = 0.0, w = 1.0}, quat2{x = 0.0, y = 0.0, z = 0.0, w = 1.0}, t = 0.5
// cosOmega > 0.9999fのケース
quat1.x = 0.0f;
quat1.y = 0.0f;
quat1.z = 0.0f;
quat1.w = 1.0f;
quat2.x = 0.0f;
quat2.y = 0.0f;
quat2.z = 0.0f;
quat2.w = 1.0f;
t = 0.5f;
expected.x = 0.0f;
expected.y = 0.0f;
expected.z = 0.0f;
expected.w = 1.0f;
actual = quat2.slerp(quat1, t);
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 : quat1{x = 0.49, y = 0.5, z = 0.5, w = 0.5}, quat2{x = 0.5, y = 0.49, z = 0.5, w = 0.5}, t = 0.5
// cosOmega <= 0.9999fのケース
quat1.x = 0.49f;
quat1.y = 0.5f;
quat1.z = 0.5f;
quat1.w = 0.5f;
quat2.x = 0.5f;
quat2.y = 0.49f;
quat2.z = 0.5f;
quat2.w = 0.5f;
t = 0.5f;
ld_float cosOmega = quat2.dot(quat1);
ld_float sinOmega = (float)Math.Sqrt(1.0f - cosOmega * cosOmega);
ld_float omega = (ld_float)Math.Atan2(sinOmega, cosOmega);
ld_float one_overSinOmega = 1.0f / sinOmega;
ld_float k0 = (float)Math.Sin((1.0f - t) * omega) * one_overSinOmega;
ld_float k1 = (float)Math.Sin(t * omega) * one_overSinOmega;
expected.x = k0 * quat2.x + k1 * quat1.x;
expected.y = k0 * quat2.y + k1 * quat1.y;
expected.z = k0 * quat2.z + k1 * quat1.z;
expected.w = k0 * quat2.w + k1 * quat1.w;
actual = quat2.slerp(quat1, t);
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 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);
}