72 lines
2.7 KiB
C#
72 lines
2.7 KiB
C#
using System;
|
|
using Quaternion = UnityEngine.Quaternion;
|
|
namespace Passer.LinearAlgebra {
|
|
|
|
public class QuaternionOf<T> {
|
|
public T x;
|
|
public T y;
|
|
public T z;
|
|
public T w;
|
|
|
|
public QuaternionOf(T x, T y, T z, T w) {
|
|
this.x = x;
|
|
this.y = y;
|
|
this.z = z;
|
|
this.w = w;
|
|
}
|
|
|
|
public static Matrix2 ToRotationMatrix(Quaternion q) {
|
|
float w = q.x, x = q.y, y = q.z, z = q.w;
|
|
|
|
float[,] result = new float[,]
|
|
{
|
|
{ 1 - 2 * (y * y + z * z), 2 * (x * y - w * z), 2 * (x * z + w * y) },
|
|
{ 2 * (x * y + w * z), 1 - 2 * (x * x + z * z), 2 * (y * z - w * x) },
|
|
{ 2 * (x * z - w * y), 2 * (y * z + w * x), 1 - 2 * (x * x + y * y) }
|
|
};
|
|
return new Matrix2(result);
|
|
}
|
|
|
|
public static Quaternion FromRotationMatrix(Matrix2 m) {
|
|
float trace = m.data[0, 0] + m.data[1, 1] + m.data[2, 2];
|
|
float w, x, y, z;
|
|
|
|
if (trace > 0) {
|
|
float s = 0.5f / (float)Math.Sqrt(trace + 1.0f);
|
|
w = 0.25f / s;
|
|
x = (m.data[2, 1] - m.data[1, 2]) * s;
|
|
y = (m.data[0, 2] - m.data[2, 0]) * s;
|
|
z = (m.data[1, 0] - m.data[0, 1]) * s;
|
|
}
|
|
else {
|
|
if (m.data[0, 0] > m.data[1, 1] && m.data[0, 0] > m.data[2, 2]) {
|
|
float s = 2.0f * (float)Math.Sqrt(1.0f + m.data[0, 0] - m.data[1, 1] - m.data[2, 2]);
|
|
w = (m.data[2, 1] - m.data[1, 2]) / s;
|
|
x = 0.25f * s;
|
|
y = (m.data[0, 1] + m.data[1, 0]) / s;
|
|
z = (m.data[0, 2] + m.data[2, 0]) / s;
|
|
}
|
|
else if (m.data[1, 1] > m.data[2, 2]) {
|
|
float s = 2.0f * (float)Math.Sqrt(1.0f + m.data[1, 1] - m.data[0, 0] - m.data[2, 2]);
|
|
w = (m.data[0, 2] - m.data[2, 0]) / s;
|
|
x = (m.data[0, 1] + m.data[1, 0]) / s;
|
|
y = 0.25f * s;
|
|
z = (m.data[1, 2] + m.data[2, 1]) / s;
|
|
}
|
|
else {
|
|
float s = 2.0f * (float)Math.Sqrt(1.0f + m.data[2, 2] - m.data[0, 0] - m.data[1, 1]);
|
|
w = (m.data[1, 0] - m.data[0, 1]) / s;
|
|
x = (m.data[0, 2] + m.data[2, 0]) / s;
|
|
y = (m.data[1, 2] + m.data[2, 1]) / s;
|
|
z = 0.25f * s;
|
|
}
|
|
}
|
|
|
|
return new Quaternion(x, y, z, w);
|
|
}
|
|
}
|
|
|
|
// public class Quaternion : QuaternionOf<float> {
|
|
// public Quaternion(float x, float y, float z, float w) : base(x, y, z, w) { }
|
|
// }
|
|
} |