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);
}
/**
* @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
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);
}
