Ejemplo n.º 1
0
 public static Quaternion CreateFromRotationMatrix(Matrix matrix)
 {
     decimal diag = matrix.M11 + matrix.M22 + matrix.M33;
     Quaternion result = new Quaternion();
     if (diag > 0)
     {
         decimal temp1 = (decimal)Math.Sqrt((double)diag + 1);
         result.W = temp1 * 0.5m;
         temp1 = 0.5m / temp1;
         result.X = (matrix.M23 - matrix.M32) * temp1;
         result.Y = (matrix.M31 - matrix.M13) * temp1;
         result.Z = (matrix.M12 - matrix.M21) * temp1;
     }
     else
     {
         if (matrix.M11 >= matrix.M22 && matrix.M11 >= matrix.M33)
         {
             decimal temp2 = (decimal)Math.Sqrt((double)(1m + matrix.M11 - matrix.M22 - matrix.M33));
             decimal temp3 = 0.5m / temp2;
             result.X = 0.5m * temp2;
             result.Y = (matrix.M12 + matrix.M21) * temp3;
             result.Z = (matrix.M13 + matrix.M31) * temp3;
             result.W = (matrix.M23 - matrix.M32) * temp3;
         }
     }
     return result;
 }
Ejemplo n.º 2
0
 public static void Multiply(ref Quaternion quaternion1, ref Quaternion quaternion2, out Quaternion result)
 {
     result.X = quaternion1.X * quaternion2.W + quaternion2.X * quaternion1.W + quaternion1.Y * quaternion2.Z - quaternion1.Z * quaternion2.Y;
     result.Y = quaternion1.Y * quaternion2.W + quaternion2.Y * quaternion1.W + quaternion1.Z * quaternion2.X - quaternion1.X * quaternion2.Z;
     result.Z = quaternion1.Z * quaternion2.W + quaternion2.Z * quaternion1.W + quaternion1.X * quaternion2.Y - quaternion1.Y * quaternion2.X;
     result.W = quaternion1.W * quaternion2.W - (quaternion1.X * quaternion2.X + quaternion1.Y * quaternion2.Y + quaternion1.Z * quaternion2.Z);
 }
Ejemplo n.º 3
0
 public static void Slerp(ref Quaternion quaternion1, ref Quaternion quaternion2, decimal amount, out Quaternion result)
 {
     decimal rate2;
     decimal rate1;
     decimal dot = quaternion1.X * quaternion2.X + quaternion1.Y * quaternion2.Y + quaternion1.Z * quaternion2.Z + quaternion1.W * quaternion2.W;
     bool flag = false;
     if (dot < 0)
     {
         flag = true;
         dot = -dot;
     }
     if (dot > 0.999999m)
     {
         rate1 = 1 - amount;
         rate2 = (flag ? -amount : amount);
     }
     else
     {
         decimal ph = (decimal)Math.Acos((double)dot);
         rate1 = (decimal)Math.Sin((double)((1 - amount) * ph))  * (decimal)(1.0f/(float)Math.Sin((double)ph));
         rate2 = (decimal)Math.Sin((double)(amount * ph)) * (decimal)(1.0f/(float)Math.Sin((double)ph));
         if (flag)
         {
             rate2 = -rate2;
         }
     }
     result.X = rate1 * quaternion1.X + rate2 * quaternion2.X;
     result.Y = rate1 * quaternion1.Y + rate2 * quaternion2.Y;
     result.Z = rate1 * quaternion1.Z + rate2 * quaternion2.Z;
     result.W = rate1 * quaternion1.W + rate2 * quaternion2.W;
 }
Ejemplo n.º 4
0
 public static void Transform(ref Vector3 value, ref Quaternion rotation, out Vector3 result)
 {
     decimal xx = rotation.X * rotation.X * 2m;
     decimal xy = rotation.X * rotation.Y * 2m;
     decimal xz = rotation.X * rotation.Z * 2m;
     decimal xw = rotation.W * rotation.X * 2m;
     decimal yy = rotation.Y * rotation.Y * 2m;
     decimal yz = rotation.Y * rotation.Z * 2m;
     decimal yw = rotation.W * rotation.Y * 2m;
     decimal zz = rotation.Z * rotation.Z * 2m;
     decimal zw = rotation.W * rotation.Z * 2m;
     result.X = value.X * (1 - yy - zz) + value.Y * (xy - zw) + value.Z * (xz + yw);
     result.Y = value.X * (xy + zw) + value.Y * (1 - xx - zz) + value.Z * (yz - xw);
     result.Z = value.X * (xz - yw) + value.Y * (yz + xw) + value.Z * (1 - xx - yy);
     result.NaN = false;
 }
Ejemplo n.º 5
0
 public static Quaternion Round(Quaternion rotation, int decimals)
 {
     return new Quaternion(Math.Round(rotation.X, decimals), Math.Round(rotation.Y, decimals), Math.Round(rotation.Z, decimals), Math.Round(rotation.W, decimals));
 }
Ejemplo n.º 6
0
 /// <summary>
 /// クォータニオンをY,Z,X回転に分解する関数
 /// </summary>
 /// <param name="input">分解するクォータニオン</param>
 /// <param name="YRot">Y軸回転</param>
 /// <param name="ZRot">Z軸回転(-PI/2~PI/2)</param>
 /// <param name="XRot">X軸回転</param>
 /// <returns></returns>
 public static bool FactoringQuaternionYZX(Quaternion input, out float YRot, out float ZRot, out float XRot)
 {
     //クォータニオンの正規化
     Quaternion inputQ = new Quaternion(input.X, input.Y, input.Z, input.W);
     inputQ.Normalize();
     //マトリクスを生成する
     Matrix rot;
     Matrix.CreateFromQuaternion(ref inputQ, out rot);
     //Z軸回りの回転を取得
     if ((double)rot.M21 > 1 - 1.0e-4 || (double)rot.M21 < -1 + 1.0e-4)
     {//ジンバルロック判定
         YRot = 0;
         ZRot = (rot.M21 < 0 ? MathHelper.PiOver2 : -MathHelper.PiOver2);
         XRot = -(float)Math.Atan2(-(double)rot.M32, (double)rot.M33);
         return false;
     }
     ZRot = -(float)Math.Asin((double)rot.M21);
     //Y軸回りの回転を取得
     YRot = (float)Math.Asin((double)rot.M31 / Math.Cos(ZRot));
     if (float.IsNaN(YRot))
     {//ジンバルロック判定(漏れ対策)
         YRot = 0;
         ZRot = (rot.M21 < 0 ? MathHelper.PiOver2 : -MathHelper.PiOver2);
         XRot = -(float)Math.Atan2(-(double)rot.M32, (double)rot.M33);
         return false;
     }
     if (rot.M11 < 0)
         YRot = MathHelper.Pi - YRot;
     //X軸回りの回転を取得
     XRot = (float)Math.Atan2((double)rot.M23, (double)rot.M22);
     return true;
 }
Ejemplo n.º 7
0
 /// <summary>
 /// クォータニオンをYaw(Y回転), Pitch(X回転), Roll(Z回転)に分解する関数
 /// </summary>
 /// <param name="input">分解するクォータニオン</param>
 /// <param name="ZRot">Z軸回転</param>
 /// <param name="XRot">X軸回転(-PI/2~PI/2)</param>
 /// <param name="YRot">Y軸回転</param>
 /// <returns>ジンバルロックが発生した時はfalse。ジンバルロックはX軸回転で発生</returns>
 public static bool FactoringQuaternionZXY(Quaternion input, out float ZRot, out float XRot, out float YRot)
 {
     //クォータニオンの正規化
     Quaternion inputQ = new Quaternion(input.X, input.Y, input.Z, input.W);
     inputQ.Normalize();
     //マトリクスを生成する
     Matrix rot;
     Matrix.CreateFromQuaternion(ref inputQ, out rot);
     //ヨー(X軸周りの回転)を取得
     if ((double)rot.M32 > 1 - 1.0e-4 || (double)rot.M32 < -1 + 1.0e-4)
     {//ジンバルロック判定
         XRot = (rot.M32 < 0 ? MathHelper.PiOver2 : -MathHelper.PiOver2);
         ZRot = 0; YRot = (float)Math.Atan2(-(double)rot.M13, (double) rot.M11);
         return false;
     }
     XRot = -(float)Math.Asin((double)rot.M32);
     //ロールを取得
     ZRot = (float)Math.Asin((double)rot.M12 / Math.Cos(XRot));
     if (float.IsNaN(ZRot))
     {//漏れ対策
         XRot = (rot.M32 < 0 ? MathHelper.PiOver2 : -MathHelper.PiOver2);
         ZRot = 0; YRot = (float)Math.Atan2(-(double)rot.M13, (double)rot.M11);
         return false;
     }
     if (rot.M22 < 0)
         ZRot = MathHelper.Pi - ZRot;
     //ピッチを取得
     YRot = (float)Math.Atan2((double)rot.M31, (double)rot.M33);
     return true;
 }
Ejemplo n.º 8
0
 public static void CreateFromQuaternion(ref Quaternion quaternion, out Matrix result)
 {
     decimal xx = quaternion.X * quaternion.X;
     decimal yy = quaternion.Y * quaternion.Y;
     decimal zz = quaternion.Z * quaternion.Z;
     decimal xy = quaternion.X * quaternion.Y;
     decimal zw = quaternion.Z * quaternion.W;
     decimal zx = quaternion.Z * quaternion.X;
     decimal yw = quaternion.Y * quaternion.W;
     decimal yz = quaternion.Y * quaternion.Z;
     decimal xw = quaternion.X * quaternion.W;
     result.M11 = 1 - 2 * (yy + zz);
     result.M12 = 2 * (xy + zw);
     result.M13 = 2 * (zx - yw);
     result.M14 = 0;
     result.M21 = 2 * (xy - zw);
     result.M22 = 1 - 2 * (zz + xx);
     result.M23 = 2 * (yz + xw);
     result.M24 = 0;
     result.M31 = 2 * (zx + yw);
     result.M32 = 2 * (yz - xw);
     result.M33 = 1 - 2 * (yy + xx);
     result.M34 = 0;
     result.M41 = 0;
     result.M42 = 0;
     result.M43 = 0;
     result.M44 = 1;
 }
Ejemplo n.º 9
0
 public static void Compose(Vector3 scale,Quaternion rotation, Vector3 translation, out Matrix result)
 {
     Matrix scaleMat, rotateMat, translationMat;
     Matrix.CreateScale(ref scale, out scaleMat);
     Matrix.CreateFromQuaternion(rotation, out rotateMat);
     Matrix.CreateTranslation(ref translation, out translationMat);
     Matrix temp;
     Multiply(ref scaleMat, ref rotateMat, out temp);
     Multiply(ref temp, ref translationMat, out result);
 }
Ejemplo n.º 10
0
 public void Decompose(out Vector3 scale, out Quaternion rotation, out Vector3 translation)
 {
     //移動行列の切り出し
     translation = new Vector3(M41, M42, M43);
     //スケールの切り出しと回転行列の作成
     scale = new Vector3();
     Matrix rotMatrix = new Matrix();
     Vector3 temp;
     temp = new Vector3(M11, M12, M13);
     scale.X = temp.Length();
     if (scale.X > 0)
     {
         rotMatrix.M11 = M11 / scale.X;
         rotMatrix.M12 = M12 / scale.X;
         rotMatrix.M13 = M13 / scale.X;
     }
     temp = new Vector3(M21, M22, M23);
     scale.Y = temp.Length();
     if (scale.X > 0)
     {
         rotMatrix.M21 = M21 / scale.Y;
         rotMatrix.M22 = M22 / scale.Y;
         rotMatrix.M23 = M23 / scale.Y;
     }
     temp = new Vector3(M31, M32, M33);
     scale.Z = temp.Length();
     if (scale.X > 0)
     {
         rotMatrix.M31 = M31 / scale.Z;
         rotMatrix.M32 = M32 / scale.Z;
         rotMatrix.M33 = M33 / scale.Z;
     }
     if (scale.Length() == 0)
     {
         throw new ArgumentException("scale成分が不明");
     }
     //回転行列をクォータニオンに変換する
     rotation = new Quaternion();
     decimal w = (decimal)Math.Sqrt((double)Math.Max(rotMatrix.M11 + rotMatrix.M22 + rotMatrix.M33 + 1, 0)) / 2;
     decimal tempX = (decimal)Math.Sqrt((double)Math.Max(rotMatrix.M11 - rotMatrix.M22 - rotMatrix.M33 + 1, 0)) / 2;
     decimal tempY = (decimal)Math.Sqrt((double)Math.Max(-rotMatrix.M11 + rotMatrix.M22 - rotMatrix.M33 + 1, 0)) / 2;
     decimal tempZ = (decimal)Math.Sqrt((double)Math.Max(-rotMatrix.M11 - rotMatrix.M22 + rotMatrix.M33 + 1, 0)) / 2;
     int MaxIndex = MathHelper.GetMaxArgIndex(tempX, tempY, tempZ, w);
     switch (MaxIndex)
     {
         case 0://x
             rotation.X = tempX;
             rotation.Y = (rotMatrix.M12 + rotMatrix.M21) / (4 * Math.Abs(tempX));
             rotation.Z = (rotMatrix.M31 + rotMatrix.M13) / (4 * Math.Abs(tempX));
             rotation.W = (rotMatrix.M23 - rotMatrix.M32) / (4 * Math.Abs(tempX));
             break;
         case 1:
             rotation.X = (rotMatrix.M12 + rotMatrix.M21) / (4 * Math.Abs(tempY));
             rotation.Y = tempY;
             rotation.Z = (rotMatrix.M23 + rotMatrix.M32) / (4 * Math.Abs(tempY));
             rotation.W = (rotMatrix.M31 - rotMatrix.M13) / (4 * Math.Abs(tempY));
             break;
         case 2:
             rotation.X = (rotMatrix.M31 + rotMatrix.M13) / (4 * Math.Abs(tempZ));
             rotation.Y = (rotMatrix.M23 + rotMatrix.M32) / (4 * Math.Abs(tempZ));
             rotation.Z = tempZ;
             rotation.W = (rotMatrix.M12 - rotMatrix.M21) / (4 * Math.Abs(tempZ));
             break;
         default://w
             rotation.X = (rotMatrix.M23 - rotMatrix.M32) / (4 * Math.Abs(w));
             rotation.Y = (rotMatrix.M31 - rotMatrix.M13) / (4 * Math.Abs(w));
             rotation.Z = (rotMatrix.M12 - rotMatrix.M21) / (4 * Math.Abs(w));
             rotation.W = w;
             break;
     }
 }
Ejemplo n.º 11
0
        public void SlerpTest()
        {
            Quaternion[] QuaternionPatterns = { /*Quaternion.Identity,*/ Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), 0.72m), Quaternion.CreateFromAxisAngle(new Vector3(0.5m, 0.5m, 0), 0.72m), Quaternion.CreateFromAxisAngle(new Vector3(0, 0.5m, 0.5m), 0.72m) };
            decimal[] ratePatterns = { 0m, 0.2m, 0.25m, 0.6m, 0.8m, 1m };

            foreach (Quaternion q1 in QuaternionPatterns)
            {
                foreach (Quaternion q2 in QuaternionPatterns)
                {
                    foreach (decimal rate in ratePatterns)
                    {
                        Quaternion quaternion1 = q1, quaternion2 = q2;
                        Quaternion result;
                        quaternion1.Normalize();
                        quaternion2.Normalize();
                        Quaternion.Slerp(ref quaternion1, ref quaternion2, rate, out result);
                        Microsoft.Xna.Framework.Quaternion xnaq1 = new Microsoft.Xna.Framework.Quaternion((float)q1.X, (float)q1.Y, (float)q1.Z, (float)q1.W);
                        Microsoft.Xna.Framework.Quaternion xnaq2 = new Microsoft.Xna.Framework.Quaternion((float)q2.X, (float)q2.Y, (float)q2.Z, (float)q2.W);
                        Microsoft.Xna.Framework.Quaternion xnaactual;
                        Microsoft.Xna.Framework.Quaternion.Slerp(ref xnaq1, ref xnaq2, (float)rate, out xnaactual);
                        Quaternion actual = new Quaternion { X = (decimal)xnaactual.X, Y = (decimal)xnaactual.Y, Z = (decimal)xnaactual.Z, W = (decimal)xnaactual.W };
                        actual.Normalize();
                        result.Normalize();
                        Assert.AreEqual(Math.Abs(actual.X- result.X)<0.01m, true);
                        Assert.AreEqual(Math.Abs(actual.Y - result.Y) < 0.01m, true);
                        Assert.AreEqual(Math.Abs(actual.Z- result.Z)<0.01m, true);
                        Assert.AreEqual(Math.Abs(actual.W- result.W) < 0.01m, true);
                    }
                }
            }
        }
Ejemplo n.º 12
0
        public void MultiplyTest()
        {
            Quaternion[] QuaternionPatterns = { Quaternion.Identity, Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), 0.72m), Quaternion.CreateFromAxisAngle(new Vector3(0.5m, 0.5m, 0), 0.72m), Quaternion.CreateFromAxisAngle(new Vector3(0, 0.5m, 0.5m), 0.72m) };
            foreach (Quaternion q1 in QuaternionPatterns)
            {
                foreach (Quaternion q2 in QuaternionPatterns)
                {
                    Quaternion quaternion1 = q1, quaternion2 = q2;
                    Quaternion result;
                    quaternion1.Normalize();
                    quaternion2.Normalize();
                    Quaternion.Multiply(ref quaternion1, ref quaternion2, out result);
                    Microsoft.Xna.Framework.Quaternion xnaq1 = new Microsoft.Xna.Framework.Quaternion((float)q1.X, (float)q1.Y, (float)q1.Z, (float)q1.W);
                    Microsoft.Xna.Framework.Quaternion xnaq2 = new Microsoft.Xna.Framework.Quaternion((float)q2.X, (float)q2.Y, (float)q2.Z, (float)q2.W);
                    Microsoft.Xna.Framework.Quaternion xnaactual;
                    Microsoft.Xna.Framework.Quaternion.Multiply(ref xnaq1, ref xnaq2, out xnaactual);
                    Quaternion actual = new Quaternion { X = (decimal)xnaactual.X, Y = (decimal)xnaactual.Y, Z = (decimal)xnaactual.Z, W = (decimal)xnaactual.W };
                    actual.Normalize();
                    result.Normalize();
                    Assert.IsTrue(Math.Abs(actual.X - result.X) < 0.01m);
                    Assert.IsTrue(Math.Abs(actual.Y - result.Y) < 0.01m);
                    Assert.IsTrue(Math.Abs(actual.Z - result.Z) < 0.01m);
                    Assert.IsTrue(Math.Abs(actual.W - result.W) < 0.01m);
                }
            }

        }
Ejemplo n.º 13
0
 /// <summary>
 /// SQTTransformを生成
 /// </summary>
 /// <param name="scales">スケールベクトル</param>
 /// <param name="rotation">回転クォータニオン</param>
 /// <param name="translation">移動ベクトル</param>
 /// <param name="result">SQTTransform</param>
 public static void Create(ref Vector3 scales, ref Quaternion rotation, ref Vector3 translation, out SQTTransform result)
 {
     result = new SQTTransform() { Scales = scales, Rotation = rotation, Translation = translation };
 }
Ejemplo n.º 14
0
 /// <summary>
 /// SQTTransformを生成
 /// </summary>
 /// <param name="scales">スケールベクトル</param>
 /// <param name="rotation">回転クォータニオン</param>
 /// <param name="translation">移動ベクトル</param>
 public SQTTransform(Vector3 scales, Quaternion rotation, Vector3 translation)
 {
     Scales = scales;
     Rotation = rotation;
     Translation = translation;
 }