Esempio n. 1
0
        } // 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);
        }
Esempio n. 2
0
 /**
 * @brief コピーコンストラクタ
 * @param quat EQuatをセットする
 */
 public LDQuat(LDQuat quat)
 {
     x = quat.x;
     y = quat.y;
     z = quat.z;
     w = quat.w;
 }
Esempio n. 3
0
 /**
  * @brief コピーコンストラクタ
  * @param quat EQuatをセットする
  */
 public LDQuat(LDQuat quat)
 {
     x = quat.x;
     y = quat.y;
     z = quat.z;
     w = quat.w;
 }
Esempio n. 4
0
        public void fromQuaterionTest()
        {
            LDQuat     quat     = new LDQuat(0.0f, 0.0f, 0.0f, 0.0f);
            LDMatrix44 expected = new LDMatrix44();
            LDMatrix44 actual   = new LDMatrix44();

            //ld_float delta = 0.00001f;

            // Input : actual = 単位行列, quat = 0.0 (ゼロ)
            actual.fromQuaterion(quat);
            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 = 単位行列, quat x = 1.0, y = 2.0, z = 3.0, w = 0.5 (任意の値)
            quat.x = 1.0f;
            quat.y = 2.0f;
            quat.z = 3.0f;
            quat.w = 0.5f;
            ld_float xx = 2.0f * quat.x;
            ld_float yy = 2.0f * quat.y;
            ld_float zz = 2.0f * quat.z;
            ld_float ww = -2.0f * quat.w;

            expected.m11 = 1.0f - yy * quat.y - zz * quat.z;
            expected.m12 = xx * quat.y + ww * quat.z;
            expected.m13 = xx * quat.z - ww * quat.y;
            expected.m21 = xx * quat.y - ww * quat.z;
            expected.m22 = 1.0f - xx * quat.x - zz * quat.z;
            expected.m23 = yy * quat.z + ww * quat.x;
            expected.m31 = xx * quat.z + ww * quat.y;
            expected.m32 = yy * quat.z - ww * quat.x;
            expected.m33 = 1.0f - xx * quat.x - yy * quat.y;
            actual.fromQuaterion(quat);
            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);
        }
Esempio n. 5
0
        //------ 設定 -------
        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
            // 他の要素は要確認
            m41 = m42 = m43 = 0.0f;
            m44 = 1.0f;
        }
Esempio n. 6
0
        public void fromQuaterionTest()
        {
            LDQuat quat=new LDQuat(0.0f, 0.0f, 0.0f, 0.0f );
            LDMatrix44 expected = new LDMatrix44();
            LDMatrix44 actual = new LDMatrix44();
            //ld_float delta = 0.00001f;

            // Input : actual = 単位行列, quat = 0.0 (ゼロ)
            actual.fromQuaterion(quat);
            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 = 単位行列, quat x = 1.0, y = 2.0, z = 3.0, w = 0.5 (任意の値)
            quat.x = 1.0f;
            quat.y = 2.0f;
            quat.z = 3.0f;
            quat.w = 0.5f;
            ld_float xx = 2.0f * quat.x;
            ld_float yy = 2.0f * quat.y;
            ld_float zz = 2.0f * quat.z;
            ld_float ww = -2.0f * quat.w;
            expected.m11 = 1.0f - yy * quat.y - zz * quat.z;
            expected.m12 = xx * quat.y + ww * quat.z;
            expected.m13 = xx * quat.z - ww * quat.y;
            expected.m21 = xx * quat.y - ww * quat.z;
            expected.m22 = 1.0f - xx * quat.x - zz * quat.z;
            expected.m23 = yy * quat.z + ww * quat.x;
            expected.m31 = xx * quat.z + ww * quat.y;
            expected.m32 = yy * quat.z - ww * quat.x;
            expected.m33 = 1.0f - xx * quat.x - yy * quat.y;
            actual.fromQuaterion(quat);
            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);
        }
Esempio n. 7
0
        //------ operator ------

        /**
         * @brief 外積
         */
        public static LDQuat operator *(LDQuat a, LDQuat b)
        {
            LDQuat result = new LDQuat();

            result.w = a.w * b.w - a.x * b.x - a.y * b.y - a.z * b.z;
            result.x = a.w * b.x + a.x * b.w + a.y * b.z - a.z * b.y;
            result.y = a.w * b.y - a.x * b.z + a.y * b.w + a.z * b.x;
            result.z = a.w * b.z + a.x * b.y - a.y * b.x + a.z * b.w;

            //	result.w = w*a.w - x*a.x - y*a.y - z*a.z ;
            //	result.x = w*a.x + x*a.w + y*a.y - z*a.z ;
            //	result.y = w*a.y + x*a.w + y*a.z - z*a.x ;
            //	result.z = w*a.z + x*a.w + y*a.x - z*a.y ;

            return(result);
        }
Esempio n. 8
0
        public void fromObjectToInertialQuaternionTest()
        {
            LDQuat        quat     = new LDQuat(0.0f, 0.0f, 0.0f, 0.0f);
            LDEulerAngles actual   = new LDEulerAngles();
            LDEulerAngles expected = new LDEulerAngles();
            //ld_float delta = 0.00001f;
            ld_float sp;

            // Input : x = 0.0, y = 0.0, z = 0.0, w = 0.0 (ゼロクォータニオン)
            actual.identity();
            expected.identity();
            actual.fromObjectToInertialQuaternion(quat);
            TestUtil.COMPARE(expected.heading, actual.heading);
            TestUtil.COMPARE(expected.pitch, actual.pitch);
            TestUtil.COMPARE(expected.bank, actual.bank);

            // Input : x = 1.0, y = 0.0, z = 0.0, w = 1.0 (ジンバルロックのケース)
            quat.x           = 1.0f;
            quat.y           = 0.0f;
            quat.z           = 0.0f;
            quat.w           = 1.0f;
            sp               = -2.0f * (quat.y * quat.z - quat.w * quat.x);
            expected.heading = (float)Math.Atan2(-quat.x * quat.z + quat.w * quat.y,
                                                 0.5f - quat.y * quat.y - quat.z * quat.z);
            expected.pitch = LDMathUtil.PI_OVER_2 * sp;
            expected.bank  = 0.0f;
            actual.fromObjectToInertialQuaternion(quat);
            TestUtil.COMPARE(expected.heading, actual.heading);
            TestUtil.COMPARE(expected.pitch, actual.pitch);
            TestUtil.COMPARE(expected.bank, actual.bank);

            // Input : x = 1.0, y = 0.25, z = 0.25, w = 0.5 (任意の値. 角度計算のケース)
            quat.x           = 1.0f;
            quat.y           = 0.25f;
            quat.z           = 0.25f;
            quat.w           = 0.5f;
            sp               = -2.0f * (quat.y * quat.z - quat.w * quat.x);
            expected.heading = (float)Math.Atan2(quat.x * quat.z + quat.w * quat.y,
                                                 0.5f - quat.x * quat.x - quat.y * quat.y);
            expected.pitch = (float)Math.Asin(sp);
            expected.bank  = (float)Math.Atan2(quat.x * quat.y + quat.w * quat.z,
                                               0.5f - quat.x * quat.x - quat.z * quat.z);
            actual.fromObjectToInertialQuaternion(quat);
            TestUtil.COMPARE(expected.heading, actual.heading);
            TestUtil.COMPARE(expected.pitch, actual.pitch);
            TestUtil.COMPARE(expected.bank, actual.bank);
        }
Esempio n. 9
0
        public void fromInertialToObjectQuaternionTest()
        {
            LDQuat quat=new LDQuat(0.0f, 0.0f, 0.0f, 0.0f );
            LDEulerAngles actual=new LDEulerAngles();
            LDEulerAngles expected=new LDEulerAngles();
            //ld_float delta = 0.00001f;
            ld_float sp;

            // Input : x = 0.0, y = 0.0, z = 0.0, w = 0.0 (ゼロクォータニオン)
            actual.identity();
            expected.identity();
            actual.fromInertialToObjectQuaternion(quat);
            TestUtil.COMPARE(expected.heading, actual.heading);
            TestUtil.COMPARE(expected.pitch, actual.pitch);
            TestUtil.COMPARE(expected.bank, actual.bank);

            // Input : x = 1.0, y = 0.0, z = 0.0, w = 1.0 (ジンバルロックのケース)
            quat.x = 1.0f;
            quat.y = 0.0f;
            quat.z = 0.0f;
            quat.w = 1.0f;
            sp = -2.0f * (quat.y * quat.z - quat.w * quat.x);
            expected.heading = (float)Math.Atan2(-quat.x * quat.z + quat.w * quat.y,
                                      0.5f - quat.y * quat.y - quat.z * quat.z);
            expected.pitch = LDMathUtil.PI_OVER_2 * sp;
            expected.bank = 0.0f;
            actual.fromObjectToInertialQuaternion(quat);
            TestUtil.COMPARE(expected.heading, actual.heading);
            TestUtil.COMPARE(expected.pitch, actual.pitch);
            TestUtil.COMPARE(expected.bank, actual.bank);

            // Input : x = 1.0, y = 0.25, z = 0.25, w = 0.5 (任意の値. 角度計算のケース)
            quat.x = 1.0f;
            quat.y = 0.25f;
            quat.z = 0.25f;
            quat.w = 0.5f;
            sp = -2.0f * (quat.y * quat.z - quat.w * quat.x);
            expected.heading = (float)Math.Atan2(quat.x * quat.z + quat.w * quat.y,
                                      0.5f - quat.x * quat.x - quat.y * quat.y);
            expected.pitch = (float)Math.Asin(sp);
            expected.bank = (float)Math.Atan2(quat.x * quat.y + quat.w * quat.z,
                                      0.5f - quat.x * quat.x - quat.z * quat.z);
            actual.fromObjectToInertialQuaternion(quat);
            TestUtil.COMPARE(expected.heading, actual.heading);
            TestUtil.COMPARE(expected.pitch, actual.pitch);
            TestUtil.COMPARE(expected.bank, actual.bank);
        }
Esempio n. 10
0
        /**
         * @brief 慣性空間->オブジェクト空間へのクォータニオンからオイラー角を設定
         * @param &q クォータニオンをセットする
         */
        public void fromInertialToObjectQuaternion(LDQuat q)
        {
            ld_float sp = -2.0f * (q.y * q.z + q.w * q.x);

            if ((ld_float)Math.Abs(sp) > 1.0f - LDMathUtil.GINBAL_TOLERANCE)
            {
                //真上か真下を向いている
                pitch   = LDMathUtil.PI_OVER_2 * sp;
                heading = (ld_float)Math.Atan2(-q.x * q.z - q.w * q.y, 0.5f - q.y * q.y - q.z * q.z);
                bank    = 0.0f;
            }
            else
            {
                pitch   = (ld_float)Math.Asin(sp);
                heading = (ld_float)Math.Atan2(q.x * q.z - q.w * q.y, 0.5f - q.x * q.x - q.y * q.y);
                bank    = (ld_float)Math.Atan2(q.x * q.y - q.w * q.z, 0.5f - q.x * q.x - q.z * q.z);
            }
        }
Esempio n. 11
0
        /**
        * @brief 慣性空間->オブジェクト空間へのクォータニオンからオイラー角を設定
        * @param &q クォータニオンをセットする
        */
        public void fromInertialToObjectQuaternion(LDQuat q)
        {
            ld_float sp = -2.0f * (q.y * q.z + q.w * q.x);

            if ((ld_float)Math.Abs(sp) > 1.0f - LDMathUtil.GINBAL_TOLERANCE)
            {
                //真上か真下を向いている
                pitch = LDMathUtil.PI_OVER_2 * sp;
                heading = (ld_float)Math.Atan2(-q.x * q.z - q.w * q.y, 0.5f - q.y * q.y - q.z * q.z);
                bank = 0.0f;
            }
            else
            {
                pitch = (ld_float)Math.Asin(sp);
                heading = (ld_float)Math.Atan2(q.x * q.z - q.w * q.y, 0.5f - q.x * q.x - q.y * q.y);
                bank = (ld_float)Math.Atan2(q.x * q.y - q.w * q.z, 0.5f - q.x * q.x - q.z * q.z);
            }
        }
Esempio n. 12
0
        public void powTest()
        {
            LDQuat quat     = new LDQuat();
            LDQuat actual   = new LDQuat();
            LDQuat expected = new LDQuat();
            //ld_float delta = 0.00001f;
            ld_float exponent;

            // Input : quat1{x = 0.0, y = 0.0, z = 0.0, w = 1.0}, exponent = 0.0 (単位クォータニオン)
            quat.x     = 0.0f;
            quat.y     = 0.0f;
            quat.z     = 0.0f;
            quat.w     = 1.0f;
            exponent   = 0.0f;
            expected.x = 0.0f;
            expected.y = 0.0f;
            expected.z = 0.0f;
            expected.w = 1.0f;
            actual     = quat.pow(exponent);
            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.1, y = 0.2, z = 0.3, w = 0.4}, exponent = 2.0 (任意の値)
            quat.x   = 0.1f;
            quat.y   = 0.2f;
            quat.z   = 0.3f;
            quat.w   = 0.4f;
            exponent = 2.0f;
            ld_float alpha    = (float)Math.Acos(quat.w);
            ld_float newAlpha = alpha * exponent;
            ld_float mult     = (float)Math.Sin(newAlpha) / (float)Math.Sin(alpha);

            expected.x = quat.x * mult;
            expected.y = quat.y * mult;
            expected.z = quat.z * mult;
            expected.w = (float)Math.Cos(newAlpha);
            actual     = quat.pow(exponent);
            TestUtil.COMPARE(expected.x, actual.x);
            TestUtil.COMPARE(expected.y, actual.y);
            TestUtil.COMPARE(expected.z, actual.z);
            TestUtil.COMPARE(expected.w, actual.w);
        }
Esempio n. 13
0
        /**
         * @brief オブジェクト空間->慣性空間へのクォータニオンからオイラー角を設定
         * @param &q クォータニオンをセットする
         */
        public void fromObjectToInertialQuaternion(LDQuat q)
        {
            ld_float sp = -2.0f * (q.y * q.z - q.w * q.x);

            // ジンバルロックチェック
            if (Math.Abs(sp) > 1.0f - LDMathUtil.GINBAL_TOLERANCE)
            {
                // 真上か下を向いている
                pitch   = LDMathUtil.PI_OVER_2 * sp;
                heading = (ld_float)Math.Atan2(-q.x * q.z + q.w * q.y, 0.5f - q.y * q.y - q.z * q.z);
                bank    = 0.0f;
            }
            else
            {
                // 角度を計算する
                pitch   = (ld_float)Math.Asin(sp);
                heading = (ld_float)Math.Atan2(q.x * q.z + q.w * q.y, 0.5f - q.x * q.x - q.y * q.y);
                bank    = (ld_float)Math.Atan2(q.x * q.y + q.w * q.z, 0.5f - q.x * q.x - q.z * q.z);
            }
        }
Esempio n. 14
0
        public void getAxisTest()
        {
            LDQuat    quat     = new LDQuat(0.0f, 0.0f, 0.0f, 0.0f);
            LDVector3 actual   = new LDVector3();
            LDVector3 expected = new LDVector3();

            //ld_float delta = 0.00001f;

            // Input : x = 0.0, y = 0.0, z = 0.0, w = 0.0 (ゼロクォータニオン)
            expected.zero();
            actual = quat.getAxis();
            TestUtil.COMPARE(expected.x, actual.x);
            TestUtil.COMPARE(expected.y, actual.y);
            TestUtil.COMPARE(expected.z, actual.z);

            // Input : x = 0.0, y = 0.0, z = 0.0, w = 1.0 (単位クォータニオン)
            quat.x     = 0.0f;
            quat.y     = 0.0f;
            quat.z     = 0.0f;
            quat.w     = 1.0f;
            expected.x = 1.0f;
            expected.y = 0.0f;
            expected.z = 0.0f;
            actual     = quat.getAxis();
            TestUtil.COMPARE(expected.x, actual.x);
            TestUtil.COMPARE(expected.y, actual.y);
            TestUtil.COMPARE(expected.z, actual.z);

            // Input : x = 1.0, y = 2.0, z = 3.0, w = 0.5 (任意の値)
            quat.x     = 1.0f;
            quat.y     = 2.0f;
            quat.z     = 3.0f;
            quat.w     = 0.5f;
            expected.x = quat.x * 1.0f / (float)Math.Sqrt(1.0f - quat.w * quat.w);
            expected.y = quat.y * 1.0f / (float)Math.Sqrt(1.0f - quat.w * quat.w);
            expected.z = quat.z * 1.0f / (float)Math.Sqrt(1.0f - quat.w * quat.w);
            actual     = quat.getAxis();
            TestUtil.COMPARE(expected.x, actual.x);
            TestUtil.COMPARE(expected.y, actual.y);
            TestUtil.COMPARE(expected.z, actual.z);
        }
Esempio n. 15
0
        public void getAxisTest()
        {
            LDQuat quat = new LDQuat(0.0f, 0.0f, 0.0f, 0.0f);
            LDVector3 actual = new LDVector3();
            LDVector3 expected = new LDVector3();
            //ld_float delta = 0.00001f;

            // Input : x = 0.0, y = 0.0, z = 0.0, w = 0.0 (ゼロクォータニオン)
            expected.zero();
            actual = quat.getAxis();
            TestUtil.COMPARE(expected.x, actual.x);
            TestUtil.COMPARE(expected.y, actual.y);
            TestUtil.COMPARE(expected.z, actual.z);

            // Input : x = 0.0, y = 0.0, z = 0.0, w = 1.0 (単位クォータニオン)
            quat.x = 0.0f;
            quat.y = 0.0f;
            quat.z = 0.0f;
            quat.w = 1.0f;
            expected.x = 1.0f;
            expected.y = 0.0f;
            expected.z = 0.0f;
            actual = quat.getAxis();
            TestUtil.COMPARE(expected.x, actual.x);
            TestUtil.COMPARE(expected.y, actual.y);
            TestUtil.COMPARE(expected.z, actual.z);

            // Input : x = 1.0, y = 2.0, z = 3.0, w = 0.5 (任意の値)
            quat.x = 1.0f;
            quat.y = 2.0f;
            quat.z = 3.0f;
            quat.w = 0.5f;
            expected.x = quat.x * 1.0f / (float)Math.Sqrt(1.0f - quat.w * quat.w);
            expected.y = quat.y * 1.0f / (float)Math.Sqrt(1.0f - quat.w * quat.w);
            expected.z = quat.z * 1.0f / (float)Math.Sqrt(1.0f - quat.w * quat.w);
            actual = quat.getAxis();
            TestUtil.COMPARE(expected.x, actual.x);
            TestUtil.COMPARE(expected.y, actual.y);
            TestUtil.COMPARE(expected.z, actual.z);
        }
Esempio n. 16
0
        public void setToRotateInertialToObjectTest()
        {
            LDEulerAngles angle1   = new LDEulerAngles();
            LDQuat        actual   = new LDQuat();
            LDQuat        expected = new LDQuat();

            //ld_float delta = 0.00001f;

            // Input : angle1{heading = 0.0, pitch = 0.0, bank = 0.0} (ゼロベクトル)
            angle1.heading = 0.0f;
            angle1.pitch   = 0.0f;
            angle1.bank    = 0.0f;
            expected.x     = 0.0f;
            expected.y     = 0.0f;
            expected.z     = 0.0f;
            expected.w     = 1.0f;
            actual.setToRotateObjectToInertial(angle1);
            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 : angle1{heading = PI/4, pitch = PI/2, bank = PI} (任意の値)
            angle1.heading = LDMathUtil.PI / 4.0f;
            angle1.pitch   = LDMathUtil.PI / 2.0f;
            angle1.bank    = LDMathUtil.PI;
            expected.x     = -(float)Math.Cos(angle1.heading * 0.5f) * (float)Math.Sin(angle1.pitch * 0.5f) * (float)Math.Cos(angle1.bank * 0.5f) -
                             (float)Math.Sin(angle1.heading * 0.5f) * (float)Math.Cos(angle1.pitch * 0.5f) * (float)Math.Sin(angle1.bank * 0.5f);
            expected.y = (float)Math.Cos(angle1.heading * 0.5f) * (float)Math.Sin(angle1.pitch * 0.5f) * (float)Math.Sin(angle1.bank * 0.5f) -
                         (float)Math.Sin(angle1.heading * 0.5f) * (float)Math.Cos(angle1.pitch * 0.5f) * (float)Math.Cos(angle1.bank * 0.5f);
            expected.z = (float)Math.Sin(angle1.heading * 0.5f) * (float)Math.Sin(angle1.pitch * 0.5f) * (float)Math.Cos(angle1.bank * 0.5f) -
                         (float)Math.Cos(angle1.heading * 0.5f) * (float)Math.Cos(angle1.pitch * 0.5f) * (float)Math.Sin(angle1.bank * 0.5f);
            expected.w = (float)Math.Cos(angle1.heading * 0.5f) * (float)Math.Cos(angle1.pitch * 0.5f) * (float)Math.Cos(angle1.bank * 0.5f) +
                         (float)Math.Sin(angle1.heading * 0.5f) * (float)Math.Sin(angle1.pitch * 0.5f) * (float)Math.Sin(angle1.bank * 0.5f);
            actual.setToRotateInertialToObject(angle1);
            TestUtil.COMPARE(expected.x, actual.x);
            TestUtil.COMPARE(expected.y, actual.y);
            TestUtil.COMPARE(expected.z, actual.z);
            TestUtil.COMPARE(expected.w, actual.w);
        }
Esempio n. 17
0
        public void normalizeTest()
        {
            LDQuat actual   = new LDQuat();
            LDQuat expected = new LDQuat();
            //ld_float delta = 0.00001f;
            ld_float oneOverMag;

            // Input : vector{x = 0.0, y = 0.0, z = 0.0, w = 1.0} (単位クォータニオン)
            actual.x   = 0.0f;
            actual.y   = 0.0f;
            actual.z   = 0.0f;
            actual.w   = 1.0f;
            expected.x = 0.0f;
            expected.y = 0.0f;
            expected.z = 0.0f;
            expected.w = 1.0f;
            actual.normalize();
            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 : vector{x = 1.0, y = 2.0, z = 3.0, w = 4.0} (任意の値)
            actual.x   = 1.0f;
            actual.y   = 2.0f;
            actual.z   = 3.0f;
            actual.w   = 4.0f;
            oneOverMag = 1.0f / (ld_float)(float)Math.Sqrt(actual.w * actual.w + actual.x * actual.x
                                                           + actual.y * actual.y + actual.z * actual.z);
            expected.x = 1.0f * oneOverMag;
            expected.y = 2.0f * oneOverMag;
            expected.z = 3.0f * oneOverMag;
            expected.w = 4.0f * oneOverMag;
            actual.normalize();
            TestUtil.COMPARE(expected.x, actual.x);
            TestUtil.COMPARE(expected.y, actual.y);
            TestUtil.COMPARE(expected.z, actual.z);
            TestUtil.COMPARE(expected.w, actual.w);
        }
Esempio n. 18
0
        //------ 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);
        }
Esempio n. 19
0
        /**
         * @brief 累乗 pow
         * @param exponent クォータニオンの乗数をセットする
         * @return クォータニオンの累乗を返す
         */
        LDQuat pow(ld_float exponent)
        {
            LDQuat q = this;

            if (Math.Abs(q.w) > 0.9999f)
            {
                return(q);    //0-divideを防ぐ
            }

            ld_float alpha    = (ld_float)Math.Acos(q.w);
            ld_float newAlpha = alpha * exponent;
            LDQuat   result   = new LDQuat();

            result.w = (ld_float)Math.Cos(newAlpha);

            ld_float mult = (ld_float)Math.Sin(newAlpha) / (ld_float)Math.Sin(alpha);

            result.x = q.x * mult;
            result.y = q.y * mult;
            result.z = q.z * mult;

            return(result);
        } // not public
Esempio n. 20
0
        } // not public

        /**
         * @brief 内積
         */
        public ld_float dot(LDQuat b)
        {
            return(w * b.w + x * b.x + y * b.y + z * b.z);
        }
Esempio n. 21
0
 public static LDQuat mySlerp(LDQuat q0, LDQuat q1, ld_float t)
 {
     return(q0.mySlerp(q1, t));
 }
Esempio n. 22
0
        /**
         * @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
Esempio n. 23
0
        /**
        * @brief 累乗 pow
        * @param exponent クォータニオンの乗数をセットする
        * @return クォータニオンの累乗を返す
        */
        public LDQuat pow(ld_float exponent)
        {
            LDQuat q = this;
            if (Math.Abs(q.w) > 0.9999f)
            {
                return q;    //0-divideを防ぐ
            }

            ld_float alpha = (ld_float)Math.Acos(q.w);
            ld_float newAlpha = alpha * exponent;
            LDQuat result = new LDQuat();
            result.w = (ld_float)Math.Cos(newAlpha);

            ld_float mult = (ld_float)Math.Sin(newAlpha) / (ld_float)Math.Sin(alpha);
            result.x = q.x * mult;
            result.y = q.y * mult;
            result.z = q.z * mult;

            return result;
        }
Esempio n. 24
0
        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);
        }
Esempio n. 25
0
        //------ 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);
        }
Esempio n. 26
0
        /**
        * @brief オブジェクト空間->慣性空間へのクォータニオンからオイラー角を設定
        * @param &q クォータニオンをセットする
        */
        public void fromObjectToInertialQuaternion(LDQuat q)
        {
            ld_float sp = -2.0f * (q.y * q.z - q.w * q.x);

            // ジンバルロックチェック
            if (Math.Abs(sp) > 1.0f - LDMathUtil.GINBAL_TOLERANCE)
            {
                // 真上か下を向いている
                pitch = LDMathUtil.PI_OVER_2 * sp;
                heading = (ld_float)Math.Atan2(-q.x * q.z + q.w * q.y, 0.5f - q.y * q.y - q.z * q.z);
                bank = 0.0f;
            }
            else
            {
                // 角度を計算する
                pitch = (ld_float)Math.Asin(sp);
                heading = (ld_float)Math.Atan2(q.x * q.z + q.w * q.y, 0.5f - q.x * q.x - q.y * q.y);
                bank = (ld_float)Math.Atan2(q.x * q.y + q.w * q.z, 0.5f - q.x * q.x - q.z * q.z);
            }
        }
Esempio n. 27
0
 /**
 * @brief 内積
 */
 public ld_float dot(LDQuat b)
 {
     return w * b.w + x * b.x + y * b.y + z * b.z;
 }
Esempio n. 28
0
        //------ 設定 -------
        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
        }
Esempio n. 29
0
        /// <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);
        }
Esempio n. 30
0
        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);
        }
Esempio n. 31
0
 //------ 設定 -------
 public void fromQuaterion(LDQuat q)
 {
 }
Esempio n. 32
0
        /**
        * @brief 球面線形補間
        * @param &q1 クォータニオン(EQuat)をセットする
        * @param t 補間位置を表す数値をセットする
        * @return 補間結果を返す
        */
        public 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;
        }
Esempio n. 33
0
        /**
        * @brief 独自の球面線形補間(回転角度を保つ)。y軸90度、z軸90度の回転をSLERPで計算する場合、t が変化した時、軸は最小距離を移動するが、角度は、90度ではなく小さな値となる。この計算を 90度の回転を保って、中心を通る計算についてテスト実装
        * @param &q1 クォータニオン(EQuat)をセットする
        * @param t 補間位置を表す数値をセットする
        * @return 補間結果を返す
        */
        public 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;
        }
Esempio n. 34
0
        public void powTest()
        {
            LDQuat quat = new LDQuat();
            LDQuat actual = new LDQuat();
            LDQuat expected = new LDQuat();
            //ld_float delta = 0.00001f;
            ld_float exponent;

            // Input : quat1{x = 0.0, y = 0.0, z = 0.0, w = 1.0}, exponent = 0.0 (単位クォータニオン)
            quat.x = 0.0f;
            quat.y = 0.0f;
            quat.z = 0.0f;
            quat.w = 1.0f;
            exponent = 0.0f;
            expected.x = 0.0f;
            expected.y = 0.0f;
            expected.z = 0.0f;
            expected.w = 1.0f;
            actual = quat.pow(exponent);
            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.1, y = 0.2, z = 0.3, w = 0.4}, exponent = 2.0 (任意の値)
            quat.x = 0.1f;
            quat.y = 0.2f;
            quat.z = 0.3f;
            quat.w = 0.4f;
            exponent = 2.0f;
            ld_float alpha = (float)Math.Acos(quat.w);
            ld_float newAlpha = alpha * exponent;
            ld_float mult = (float)Math.Sin(newAlpha) / (float)Math.Sin(alpha);
            expected.x = quat.x * mult;
            expected.y = quat.y * mult;
            expected.z = quat.z * mult;
            expected.w = (float)Math.Cos(newAlpha);
            actual = quat.pow(exponent);
            TestUtil.COMPARE(expected.x, actual.x);
            TestUtil.COMPARE(expected.y, actual.y);
            TestUtil.COMPARE(expected.z, actual.z);
            TestUtil.COMPARE(expected.w, actual.w);
        }
Esempio n. 35
0
        public void normalizeTest()
        {
            LDQuat actual = new LDQuat();
            LDQuat expected = new LDQuat();
            //ld_float delta = 0.00001f;
            ld_float oneOverMag;

            // Input : vector{x = 0.0, y = 0.0, z = 0.0, w = 1.0} (単位クォータニオン)
            actual.x = 0.0f;
            actual.y = 0.0f;
            actual.z = 0.0f;
            actual.w = 1.0f;
            expected.x = 0.0f;
            expected.y = 0.0f;
            expected.z = 0.0f;
            expected.w = 1.0f;
            actual.normalize();
            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 : vector{x = 1.0, y = 2.0, z = 3.0, w = 4.0} (任意の値)
            actual.x = 1.0f;
            actual.y = 2.0f;
            actual.z = 3.0f;
            actual.w = 4.0f;
            oneOverMag = 1.0f / (ld_float)(float)Math.Sqrt(actual.w * actual.w + actual.x * actual.x
                                                  + actual.y * actual.y + actual.z * actual.z);
            expected.x = 1.0f * oneOverMag;
            expected.y = 2.0f * oneOverMag;
            expected.z = 3.0f * oneOverMag;
            expected.w = 4.0f * oneOverMag;
            actual.normalize();
            TestUtil.COMPARE(expected.x, actual.x);
            TestUtil.COMPARE(expected.y, actual.y);
            TestUtil.COMPARE(expected.z, actual.z);
            TestUtil.COMPARE(expected.w, actual.w);
        }
Esempio n. 36
0
        //------ 設定 -------
        public void fromQuaterion(LDQuat q)
        {

        }
Esempio n. 37
0
        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);
        }
Esempio n. 38
0
        //------ operator ------
        /**
        * @brief 外積
        */
        public static LDQuat operator *(LDQuat a, LDQuat b)
        {
            LDQuat result = new LDQuat();

            result.w = a.w * b.w - a.x * b.x - a.y * b.y - a.z * b.z;
            result.x = a.w * b.x + a.x * b.w + a.y * b.z - a.z * b.y;
            result.y = a.w * b.y - a.x * b.z + a.y * b.w + a.z * b.x;
            result.z = a.w * b.z + a.x * b.y - a.y * b.x + a.z * b.w;

            //	result.w = w*a.w - x*a.x - y*a.y - z*a.z ;
            //	result.x = w*a.x + x*a.w + y*a.y - z*a.z ;
            //	result.y = w*a.y + x*a.w + y*a.z - z*a.x ;
            //	result.z = w*a.z + x*a.w + y*a.x - z*a.y ;

            return result;
        }
Esempio n. 39
0
        /// <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);
        }
Esempio n. 40
0
 public static LDQuat slerp(LDQuat q0, LDQuat q1, ld_float t)
 {
     return q0.slerp(q1, t);
 }
Esempio n. 41
0
        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);
        }
Esempio n. 42
0
        public void setToRotateObjectToInertialTest()
        {
            LDEulerAngles angle1 = new LDEulerAngles();
            LDQuat actual = new LDQuat();
            LDQuat expected = new LDQuat();
            //ld_float delta = 0.00001f;

            // Input : angle1{heading = 0.0, pitch = 0.0, bank = 0.0} (ゼロベクトル)
            angle1.heading = 0.0f;
            angle1.pitch = 0.0f;
            angle1.bank = 0.0f;
            expected.x = 0.0f;
            expected.y = 0.0f;
            expected.z = 0.0f;
            expected.w = 1.0f;
            actual.setToRotateObjectToInertial(angle1);
            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 : angle1{heading = PI/4, pitch = PI/2, bank = PI} (任意の値)
            angle1.heading = LDMathUtil.PI / 4.0f;
            angle1.pitch = LDMathUtil.PI / 2.0f;
            angle1.bank = LDMathUtil.PI;
            expected.x = (float)Math.Cos(angle1.heading * 0.5f) * (float)Math.Sin(angle1.pitch * 0.5f) * (float)Math.Cos(angle1.bank * 0.5f) +
                          (float)Math.Sin(angle1.heading * 0.5f) * (float)Math.Cos(angle1.pitch * 0.5f) * (float)Math.Sin(angle1.bank * 0.5f);
            expected.y = -(float)Math.Cos(angle1.heading * 0.5f) * (float)Math.Sin(angle1.pitch * 0.5f) * (float)Math.Sin(angle1.bank * 0.5f) +
                         (float)Math.Sin(angle1.heading * 0.5f) * (float)Math.Cos(angle1.pitch * 0.5f) * (float)Math.Cos(angle1.bank * 0.5f);
            expected.z = -(float)Math.Sin(angle1.heading * 0.5f) * (float)Math.Sin(angle1.pitch * 0.5f) * (float)Math.Cos(angle1.bank * 0.5f) +
                         (float)Math.Cos(angle1.heading * 0.5f) * (float)Math.Cos(angle1.pitch * 0.5f) * (float)Math.Sin(angle1.bank * 0.5f);
            expected.w = (float)Math.Cos(angle1.heading * 0.5f) * (float)Math.Cos(angle1.pitch * 0.5f) * (float)Math.Cos(angle1.bank * 0.5f) +
                          (float)Math.Sin(angle1.heading * 0.5f) * (float)Math.Sin(angle1.pitch * 0.5f) * (float)Math.Sin(angle1.bank * 0.5f);
            actual.setToRotateObjectToInertial(angle1);
            TestUtil.COMPARE(expected.x, actual.x);
            TestUtil.COMPARE(expected.y, actual.y);
            TestUtil.COMPARE(expected.z, actual.z);
            TestUtil.COMPARE(expected.w, actual.w);
        }