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