//------ 設定 -------
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
}
/**
* @brief 安全な乗算. a , b と dst が同じでも良い安全な乗算(ld_float[16]を別に用意するため負荷大)
* @param *a 行列aをセットする
* @param *b 行列bをセットする
* @param *dst 変換行列を受け取るポインタをセットする
*/
public static void mult_safe(ld_float[] a, ld_float[] b, ld_float[] dst)
{
ld_float[] tmp = new ld_float[12];
if (a == dst)
{
mult_fast(a, b, tmp);
for (int i = 11; i >= 0; --i)
{
dst[i] = tmp[i];
}
}
else
{
mult_fast(a, b, dst);
}
}
/**
* @brief 高速な乗算. a , b と dst が同じ場合は不正な計算になる
* @param *a 行列aをセットする
* @param *b 行列bをセットする
* @param *dst 乗算した行列を受け取るポインタをセットする
*/
public static void mult_fast(ld_float[] a, ld_float[] b, ld_float[] dst)
{
dst[0] = a[0] * b[0] + a[4] * b[1] + a[8] * b[2];
dst[4] = a[0] * b[4] + a[4] * b[5] + a[8] * b[6];
dst[8] = a[0] * b[8] + a[4] * b[9] + a[8] * b[10];
// dst[12] = a[ 0]*b[12] + a[ 4]*b[13] + a[ 8]*b[14] + a[12] ;
dst[1] = a[1] * b[0] + a[5] * b[1] + a[9] * b[2];
dst[5] = a[1] * b[4] + a[5] * b[5] + a[9] * b[6];
dst[9] = a[1] * b[8] + a[5] * b[9] + a[9] * b[10];
// dst[13] = a[ 1]*b[12] + a[ 5]*b[13] + a[ 9]*b[14] + a[13] ;
dst[2] = a[2] * b[0] + a[6] * b[1] + a[10] * b[2];
dst[6] = a[2] * b[4] + a[6] * b[5] + a[10] * b[6];
dst[10] = a[2] * b[8] + a[6] * b[9] + a[10] * b[10];
// dst[14] = a[ 2]*b[12] + a[ 6]*b[13] + a[10]*b[14] + a[14] ;
}
/**
* @brief Y軸周りの回転を行う
* @param theta 角度をセットする
*/
public void rotateY(ld_float thetaY)
{
ld_float c = (ld_float)Math.Cos(thetaY);
ld_float s = (ld_float)Math.Sin(thetaY);
ld_float tmp__ = m11;
m11 = tmp__ * c + m13 * -s;
m13 = tmp__ * s + m13 * c;
tmp__ = m21;
m21 = tmp__ * c + m23 * -s;
m23 = tmp__ * s + m23 * c;
tmp__ = m31;
m31 = tmp__ * c + m33 * -s;
m33 = tmp__ * s + m33 * c;
}
/**
* @brief Z軸周りの回転を行う
* @param theta 角度をセットする
*/
public void rotateZ(ld_float thetaZ)
{
ld_float c = (ld_float)Math.Cos(thetaZ);
ld_float s = (ld_float)Math.Sin(thetaZ);
ld_float tmp__ = m11;
m11 = tmp__ * c + m12 * s;
m12 = tmp__ * -s + m12 * c;
tmp__ = m21;
m21 = tmp__ * c + m22 * s;
m22 = tmp__ * -s + m22 * c;
tmp__ = m31;
m31 = tmp__ * c + m32 * s;
m32 = tmp__ * -s + m32 * c;
}
/**
* @brief スケール変換
* @param scaleX Xスケール変換係数をセットする
* @param scaleY Yスケール変換係数をセットする
* @param scaleZ Zスケール変換係数をセットする
*/
public void scale(ld_float scaleX, ld_float scaleY, ld_float scaleZ)
{
m11 *= scaleX;
m12 *= scaleY;
m13 *= scaleZ;
m21 *= scaleX;
m22 *= scaleY;
m23 *= scaleZ;
m31 *= scaleX;
m32 *= scaleY;
m33 *= scaleZ;
}
/**
* @brief X軸周りの回転を行う
* @param theta 角度をセットする
*/
public void rotateX(ld_float thetaX)
{
ld_float c = (ld_float)Math.Cos(thetaX);
ld_float s = (ld_float)Math.Sin(thetaX);
ld_float tmp = m12;
m12 = tmp * c + m13 * s;
m13 = tmp * -s + m13 * c;
tmp = m22;
m22 = tmp * c + m23 * s;
m23 = tmp * -s + m23 * c;
tmp = m32;
m32 = tmp * c + m33 * s;
m33 = tmp * -s + m33 * c;
}
/**
* @brief 行列を設定
* @param *_m 行列配列をセットする
*/
public void setMatrix(ld_float[] _m)
{
for (int i = 11; i >= 0; --i)
{
m[i] = _m[i];
}
}
//------ 行列の変換 -------
/**
* @brief 変換
* @param x 行列の要素xをセットする
* @param y 行列の要素yをセットする
* @param z 行列の要素zをセットする
*/
public void translate(ld_float x, ld_float y, ld_float z)
{
m14 = m11 * x + m12 * y + m13 * z + m14;
m24 = m21 * x + m22 * y + m23 * z + m24;
m34 = m31 * x + m32 * y + m33 * z + m34;
}
/**
* @brief 安全な乗算. a , b と dst が同じでも良い安全な乗算(ld_float[16]を別に用意するため負荷大)
* @param *a 行列aをセットする
* @param *b 行列bをセットする
* @param *dst 変換行列を受け取るポインタをセットする
*/
public static void mult_safe(ld_float[] a, ld_float[] b, ld_float[] dst, ld_bool omit4thRow = false)
{
ld_float[] tmp = new ld_float[16];
if (a == dst)
{
mult_fast(a, b, tmp, omit4thRow);
for (int i = 15; i >= 0; --i)
{
dst[i] = tmp[i];
}
}
else
{
mult_fast(a, b, dst, omit4thRow);
}
}
/**
* @brief 高速な乗算. a , b と dst が同じ場合は不正な計算になる
* @param *a 行列aをセットする
* @param *b 行列bをセットする
* @param *dst 乗算した行列を受け取るポインタをセットする
*/
public static void mult_fast(ld_float[] a, ld_float[] b, ld_float[] dst, ld_bool omit4thRow = false)
{
// 透視変換を除けば通常4行は 0,0,0,1 であるため計算を省略できる場合が多い
if (omit4thRow)
{
dst[0] = a[0] * b[0] + a[4] * b[1] + a[8] * b[2];
dst[4] = a[0] * b[4] + a[4] * b[5] + a[8] * b[6];
dst[8] = a[0] * b[8] + a[4] * b[9] + a[8] * b[10];
dst[12] = a[0] * b[12] + a[4] * b[13] + a[8] * b[14] + a[12];
dst[1] = a[1] * b[0] + a[5] * b[1] + a[9] * b[2];
dst[5] = a[1] * b[4] + a[5] * b[5] + a[9] * b[6];
dst[9] = a[1] * b[8] + a[5] * b[9] + a[9] * b[10];
dst[13] = a[1] * b[12] + a[5] * b[13] + a[9] * b[14] + a[13];
dst[2] = a[2] * b[0] + a[6] * b[1] + a[10] * b[2];
dst[6] = a[2] * b[4] + a[6] * b[5] + a[10] * b[6];
dst[10] = a[2] * b[8] + a[6] * b[9] + a[10] * b[10];
dst[14] = a[2] * b[12] + a[6] * b[13] + a[10] * b[14] + a[14];
dst[3] = dst[7] = dst[11] = 0;
dst[15] = 1;
}
else
{
dst[0] = a[0] * b[0] + a[4] * b[1] + a[8] * b[2] + a[12] * b[3];
dst[4] = a[0] * b[4] + a[4] * b[5] + a[8] * b[6] + a[12] * b[7];
dst[8] = a[0] * b[8] + a[4] * b[9] + a[8] * b[10] + a[12] * b[11];
dst[12] = a[0] * b[12] + a[4] * b[13] + a[8] * b[14] + a[12] * b[15];
dst[1] = a[1] * b[0] + a[5] * b[1] + a[9] * b[2] + a[13] * b[3];
dst[5] = a[1] * b[4] + a[5] * b[5] + a[9] * b[6] + a[13] * b[7];
dst[9] = a[1] * b[8] + a[5] * b[9] + a[9] * b[10] + a[13] * b[11];
dst[13] = a[1] * b[12] + a[5] * b[13] + a[9] * b[14] + a[13] * b[15];
dst[2] = a[2] * b[0] + a[6] * b[1] + a[10] * b[2] + a[14] * b[3];
dst[6] = a[2] * b[4] + a[6] * b[5] + a[10] * b[6] + a[14] * b[7];
dst[10] = a[2] * b[8] + a[6] * b[9] + a[10] * b[10] + a[14] * b[11];
dst[14] = a[2] * b[12] + a[6] * b[13] + a[10] * b[14] + a[14] * b[15];
dst[3] = a[3] * b[0] + a[7] * b[1] + a[11] * b[2] + a[15] * b[3];
dst[7] = a[3] * b[4] + a[7] * b[5] + a[11] * b[6] + a[15] * b[7];
dst[11] = a[3] * b[8] + a[7] * b[9] + a[11] * b[10] + a[15] * b[11];
dst[15] = a[3] * b[12] + a[7] * b[13] + a[11] * b[14] + a[15] * b[15];
}
}
/**
* @brief 直接値を設定する
* @param x 行列の要素xをセットする
* @param y 行列の要素yをセットする
* @param z 行列の要素zをセットする
*/
public void setToTranslate(ld_float x, ld_float y, ld_float z)
{
m14 = x;
m24 = y;
m34 = z;
}
/// <summary>
/// 行列を設定
/// </summary>
/// <param name="_m">行列配列をセットする</param>
public void setMatrix(ld_float[] _m)
{
this.m = _m;
}
/**
* @brief コンストラクタ
* @param h headingをセットする
* @param p pitchをセットする
* @param b bankをセットする
*/
public LDEulerAngles(ld_float h, float p, float b)
{
heading = h; pitch = p; bank = b;
}
public void setMatrixTest()
{
LDMatrix44 matrix = new LDMatrix44();
LDMatrix44 actual = new LDMatrix44();
LDMatrix44 expected = new LDMatrix44();
//ld_float delta = 0.00001f;
ld_float[] m = new ld_float[16];
// EMatrix44.setMatrix( const ld_float* _m ) のテスト
for (int i = 0; i < 16; ++i)
{
m[i] = 1.0f;
}
actual.setMatrix(m);
expected.m11 = 1.0f;
expected.m12 = 1.0f;
expected.m13 = 1.0f;
expected.m14 = 1.0f;
expected.m21 = 1.0f;
expected.m22 = 1.0f;
expected.m23 = 1.0f;
expected.m24 = 1.0f;
expected.m31 = 1.0f;
expected.m32 = 1.0f;
expected.m33 = 1.0f;
expected.m34 = 1.0f;
expected.m41 = 1.0f;
expected.m42 = 1.0f;
expected.m43 = 1.0f;
expected.m44 = 1.0f;
TestUtil.COMPARE(expected.m11, actual.m11);
TestUtil.COMPARE(expected.m12, actual.m12);
TestUtil.COMPARE(expected.m13, actual.m13);
TestUtil.COMPARE(expected.m14, actual.m14);
TestUtil.COMPARE(expected.m21, actual.m21);
TestUtil.COMPARE(expected.m22, actual.m22);
TestUtil.COMPARE(expected.m23, actual.m23);
TestUtil.COMPARE(expected.m24, actual.m24);
TestUtil.COMPARE(expected.m31, actual.m31);
TestUtil.COMPARE(expected.m32, actual.m32);
TestUtil.COMPARE(expected.m33, actual.m33);
TestUtil.COMPARE(expected.m34, actual.m34);
TestUtil.COMPARE(expected.m41, actual.m41);
TestUtil.COMPARE(expected.m42, actual.m42);
TestUtil.COMPARE(expected.m43, actual.m43);
TestUtil.COMPARE(expected.m44, actual.m44);
// EMatrix43.setMatrix( const EMatrix43 &m2 ) のテスト
for (int i = 0; i < 16; ++i)
{
m[i] = 2.0f;
}
matrix.setMatrix(m);
actual.setMatrix(matrix);
expected.m11 = 2.0f;
expected.m12 = 2.0f;
expected.m13 = 2.0f;
expected.m14 = 2.0f;
expected.m21 = 2.0f;
expected.m22 = 2.0f;
expected.m23 = 2.0f;
expected.m24 = 2.0f;
expected.m31 = 2.0f;
expected.m32 = 2.0f;
expected.m33 = 2.0f;
expected.m34 = 2.0f;
expected.m41 = 2.0f;
expected.m42 = 2.0f;
expected.m43 = 2.0f;
expected.m44 = 2.0f;
TestUtil.COMPARE(expected.m11, actual.m11);
TestUtil.COMPARE(expected.m12, actual.m12);
TestUtil.COMPARE(expected.m13, actual.m13);
TestUtil.COMPARE(expected.m14, actual.m14);
TestUtil.COMPARE(expected.m21, actual.m21);
TestUtil.COMPARE(expected.m22, actual.m22);
TestUtil.COMPARE(expected.m23, actual.m23);
TestUtil.COMPARE(expected.m24, actual.m24);
TestUtil.COMPARE(expected.m31, actual.m31);
TestUtil.COMPARE(expected.m32, actual.m32);
TestUtil.COMPARE(expected.m33, actual.m33);
TestUtil.COMPARE(expected.m34, actual.m34);
TestUtil.COMPARE(expected.m41, actual.m41);
TestUtil.COMPARE(expected.m42, actual.m42);
TestUtil.COMPARE(expected.m43, actual.m43);
TestUtil.COMPARE(expected.m44, actual.m44);
}
public void scaleTest()
{
LDMatrix44 expected = new LDMatrix44();
LDMatrix44 actual = new LDMatrix44();
//ld_float delta = 0.00001f;
// Input : actual = 単位行列, x = 0.0, y = 0.0, z = 0.0 (ゼロ)
expected.m11 = 0.0f;
expected.m12 = 0.0f;
expected.m13 = 0.0f;
expected.m21 = 0.0f;
expected.m22 = 0.0f;
expected.m23 = 0.0f;
expected.m31 = 0.0f;
expected.m32 = 0.0f;
expected.m33 = 0.0f;
expected.m41 = 0.0f;
expected.m42 = 0.0f;
expected.m43 = 0.0f;
actual.scale(0.0f, 0.0f, 0.0f);
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 = 任意の値, x = 1.0, y = 2.0, z = 3.0 (任意の値)
ld_float x = 1.0f;
ld_float y = 2.0f;
ld_float z = 3.0f;
actual.m11 = 1.0f;
actual.m12 = 2.0f;
actual.m13 = 3.0f;
actual.m21 = 1.0f;
actual.m22 = 2.0f;
actual.m23 = 3.0f;
actual.m31 = 1.0f;
actual.m32 = 2.0f;
actual.m33 = 3.0f;
actual.m41 = 1.0f;
actual.m42 = 2.0f;
actual.m43 = 3.0f;
expected.m11 = actual.m11 * x;
expected.m12 = actual.m12 * y;
expected.m13 = actual.m13 * z;
expected.m21 = actual.m21 * x;
expected.m22 = actual.m22 * y;
expected.m23 = actual.m23 * z;
expected.m31 = actual.m31 * x;
expected.m32 = actual.m32 * y;
expected.m33 = actual.m33 * z;
expected.m41 = actual.m41 * x;
expected.m42 = actual.m42 * y;
expected.m43 = actual.m43 * z;
actual.scale(x, y, z);
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);
}
