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; }
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); }
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; }
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; }
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)); }
/// <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; }
/// <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; }
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; }
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); }
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; } }
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); } } } }
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); } } }
/// <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 }; }
/// <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; }