public void normalizeTest()
{
LDVector3 vector = new LDVector3();
ld_float expected_x;
ld_float expected_y;
ld_float expected_z;
//ld_float delta = 0.00001f;
// Input : x = 0.0, y = 0.0, z = 0.0 (ゼロベクトル)
vector.zero();
expected_x = 0.0f;
expected_y = 0.0f;
expected_z = 0.0f;
vector.normalize();
TestUtil.COMPARE(expected_x, vector.x);
TestUtil.COMPARE(expected_y, vector.y);
TestUtil.COMPARE(expected_z, vector.z);
// Input : x = 1.0, y = 2.0, z = 3.0 (任意の値)
vector.x = 1.0f;
vector.y = 2.0f;
vector.z = 3.0f;
expected_x = vector.x / (ld_float)Math.Sqrt(vector.x * vector.x + vector.y * vector.y + vector.z * vector.z);
expected_y = vector.y / (ld_float)Math.Sqrt(vector.x * vector.x + vector.y * vector.y + vector.z * vector.z);
expected_z = vector.z / (ld_float)Math.Sqrt(vector.x * vector.x + vector.y * vector.y + vector.z * vector.z);
vector.normalize();
TestUtil.COMPARE(expected_x, vector.x);
TestUtil.COMPARE(expected_y, vector.y);
TestUtil.COMPARE(expected_z, vector.z);
}
} // 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 最小弧クォータニオン 二点を結ぶ最小の弧を描く。(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
//------ 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);
}
/// <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);
}
/**
* @brief 最小弧クォータニオン 二点を結ぶ最小の弧を描く。(v1,v2が単位ベクトルであることがわかっている場合は useNormalizeを false にすると計算量が多少減る)
* @param &v1 EVector3をセットする
* @param &v2 EVector3をセットする
* @param useNormalize 正規化する場合はtrue, そうでない場合はfalseをセットする。(入力無き場合はtrue)
*/
public 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;
}
public void normalizeTest()
{
LDVector3 vector = new LDVector3();
ld_float expected_x;
ld_float expected_y;
ld_float expected_z;
//ld_float delta = 0.00001f;
// Input : x = 0.0, y = 0.0, z = 0.0 (ゼロベクトル)
vector.zero();
expected_x = 0.0f;
expected_y = 0.0f;
expected_z = 0.0f;
vector.normalize();
TestUtil.COMPARE(expected_x, vector.x);
TestUtil.COMPARE(expected_y, vector.y);
TestUtil.COMPARE(expected_z, vector.z);
// Input : x = 1.0, y = 2.0, z = 3.0 (任意の値)
vector.x = 1.0f;
vector.y = 2.0f;
vector.z = 3.0f;
expected_x = vector.x / (ld_float)Math.Sqrt(vector.x * vector.x + vector.y * vector.y + vector.z * vector.z);
expected_y = vector.y / (ld_float)Math.Sqrt(vector.x * vector.x + vector.y * vector.y + vector.z * vector.z);
expected_z = vector.z / (ld_float)Math.Sqrt(vector.x * vector.x + vector.y * vector.y + vector.z * vector.z);
vector.normalize();
TestUtil.COMPARE(expected_x, vector.x);
TestUtil.COMPARE(expected_y, vector.y);
TestUtil.COMPARE(expected_z, vector.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);
}
//------ 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);
}