/**
* @brief 最小弧クォータニオン 二点を結ぶ最小の弧を描く。(v1,v2が単位ベクトルであることがわかっている場合は useNormalizeを false にすると計算量が多少減る)
* @param &v1 EVector3をセットする
* @param &v2 EVector3をセットする
* @param useNormalize 正規化する場合はtrue, そうでない場合はfalseをセットする。(入力無き場合はtrue)
*/
void setToRotationArc(LDVector3 v1, LDVector3 v2, bool useNormalize = true)
{
LDVector3 vv1;
LDVector3 vv2;
LDVector3 tmp1;
LDVector3 tmp2;
if (useNormalize)
{
tmp1 = v1;
tmp1.normalize();
tmp2 = v2;
tmp2.normalize();
vv1 = tmp1;
vv2 = tmp2;
}
else
{
vv1 = v1;
vv2 = v2;
}
LDVector3 c = LDVector3.crossProduct(vv1, vv2);
ld_float d = vv1.dotProduct(vv2);
ld_float s = (ld_float)Math.Sqrt((1 + d) * 2);
this.x = c.x / s;
this.y = c.y / s;
this.z = c.z / s;
this.w = s / 2.0f;
} // not public
//====================================================
// 非メンバ関数のテスト
//====================================================
public void crossProductTest()
{
LDVector3 vector1 = new LDVector3();
LDVector3 vector2 = new LDVector3();
LDVector3 actual = new LDVector3();
LDVector3 expected = new LDVector3();
//ld_float delta = 0.00001f;
// Input : vector1{x = 0.0, y = 0.0, z = 0.0}, vector2{x = 0.0, y = 0.0, z = 0.0} (ゼロベクトル)
vector1.zero();
vector2.zero();
expected.zero();
actual = LDVector3.crossProduct(vector1, vector2);
TestUtil.COMPARE(expected.x, actual.x);
TestUtil.COMPARE(expected.y, actual.y);
TestUtil.COMPARE(expected.z, actual.z);
// Input : vector1{x = 1.0, y = 2.0, z = 3.0}, vector2{x = 1.0, y = 2.0, z = 3.0} (任意の値)
vector1.x = 1.0f;
vector1.y = 2.0f;
vector1.z = 3.0f;
vector2.x = 1.0f;
vector2.y = 2.0f;
vector2.z = 3.0f;
expected.x = vector1.y * vector2.z - vector1.z * vector2.y;
expected.y = vector1.z * vector2.x - vector1.x * vector2.z;
expected.z = vector1.x * vector2.y - vector1.y * vector2.x;
actual = LDVector3.crossProduct(vector1, vector2);
TestUtil.COMPARE(expected.x, actual.x);
TestUtil.COMPARE(expected.y, actual.y);
TestUtil.COMPARE(expected.z, actual.z);
}
/// <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);
}
