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