public static Quaternion Normalize(Quaternion q) {
Quaternion result = new Quaternion();
float qLength = Length(q);
result.X /= qLength;
result.Y /= qLength;
result.Z /= qLength;
result.W /= qLength;
return result;
}
public static Quaternion AngleAxis(float angle, Vector3 axis) {
angle = angle * (float)(Math.PI / 180.0f);
Quaternion result = new Quaternion();
float sin = (float)Math.Sin(angle);
axis = Vector3.Normalize(axis);
result.W = (float)Math.Cos(angle);
result.X = axis.X * sin;
result.Y = axis.Y * sin;
result.Z = axis.Z * sin;
return result;
}
public static Matrix4 ToMatrix(Quaternion q) {
Matrix4 result = new Matrix4();
float xSq = q.X * q.X;
float ySq = q.Y * q.Y;
float zSq = q.Z * q.Z;
float wSq = q.W * q.W;
float twoX = 2.0f * q.X;
float twoY = 2.0f * q.Y;
float twoW = 2.0f * q.W;
float xy = twoX * q.Y;
float xz = twoX * q.Z;
float yz = twoY * q.Z;
float wx = twoW * q.X;
float wy = twoW * q.Y;
float wz = twoW * q.Z;
result[0, 0] = wSq + xSq - ySq - zSq;
result[0, 1] = xy - wz;
result[0, 2] = xz + wy;
result[0, 3] = 0.0f;
result[1, 0] = xy + wz;
result[1, 1] = wSq - xSq + ySq - zSq;
result[1, 2] = yz - wx;
result[1, 3] = 0.0f;
result[2, 0] = xz - wy;
result[2, 1] = yz + wx;
result[2, 2] = wSq - xSq - ySq + zSq;
result[2, 3] = 0.0f;
result[3, 0] = 0.0f;
result[3, 1] = 0.0f;
result[3, 2] = 0.0f;
result[3, 3] = 1.0f;
return result;
}
public static float LengthSquared(Quaternion q) {
return (q.X * q.X + q.Y * q.Y + q.Z * q.Z + q.W * q.W);
}
public static Vector3 ToEuler(Quaternion q) {
float x, y, z = 0.0f;
q = Normalize(q);
float test = (q.X * q.Y) + (q.Z * q.W);
if (test > 0.499) {//north singularity
x = 0.0f * 57.2958f;
y = 2.0f * (float)Math.Atan2(q.X,q.W) * 57.2958f;
z = (float)Math.PI / 2.0f * 57.2958f;
return new Vector3(x, y, z);
}
else if (test < -0.499) { //south singluarity
x = 0.0f * 57.2958f;
y = -2.0f * (float)Math.Atan2(q.X, q.W) * 57.2958f;
z = -(float)Math.PI / 2.0f * 57.2958f;
return new Vector3(x, y, z);
}
float xSq = q.X * q.X;
float ySq = q.Y * q.Y;
float zSq = q.Z * q.Z;
x = (float)Math.Atan2((2.0f * q.X * q.W - 2.0f * q.Y * q.Z), (1.0f - (2.0f * xSq) - (2.0f * zSq))) * 57.2958f;
y = (float)Math.Atan2((2.0f * q.Y * q.W - 2.0f * q.X * q.Z), (1.0f - (2.0f * ySq) - (2.0f * zSq))) * 57.2958f;
z = (float)Math.Asin(2.0f * test) * 57.2958f;
return new Vector3(x, y, z);
}
public static float Length(Quaternion q) {
return (float)Math.Sqrt(q.X * q.X + q.Y * q.Y + q.Z * q.Z + q.W * q.W);
}
public static Quaternion FromEuler(float x,float y, float z) {
float radConversion = (float)Math.PI / 180.0f;
x *= radConversion;
y *= radConversion;
z *= radConversion;
Quaternion result = new Quaternion();
float cosX = (float)Math.Cos(x/2);
float sinX = (float)Math.Sin(x/2);
float cosY = (float)Math.Cos(y/2);
float sinY = (float)Math.Sin(y/2);
float cosZ = (float)Math.Cos(z/2);
float sinZ = (float)Math.Sin(z/2);
float cosYZ = cosY * cosZ;
float sinYZ = sinY * sinZ;
result.W = (cosYZ * sinX) + (sinYZ * sinX);
result.X = (cosYZ * sinX) + (sinYZ * cosX);
result.Y = (sinY * cosZ * cosX) + (cosY * sinZ * sinX);
result.Z = (cosY * sinZ * cosX) - (sinY * cosZ * sinX);
return result;
}
public static Quaternion Multiply(Quaternion A, Quaternion B) {
Quaternion result = new Quaternion();
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 = A.W * B.W - A.X * B.X - A.Y * B.Y - A.Z * B.Z;
return result;
}
public float Dot(Quaternion a) {
return X * a.X + Y * a.Y + Z * a.Z + W * a.W;
}
public static float Dot(Quaternion a, Quaternion b) {
return a.X * b.X + a.Y * b.Y + a.Z * b.Z + a.W * b.W;
}
public static Quaternion Inverse(Quaternion q) {
Quaternion result = new Quaternion();
result.X = -q.X;
result.Y = -q.Y;
result.Z = -q.Z;
result.W = q.W;
return result;
}
