98 lines
3.6 KiB
C#
98 lines
3.6 KiB
C#
using System;
|
|
|
|
namespace Passer.LinearAlgebra
|
|
{
|
|
public class Quat32
|
|
{
|
|
public float x;
|
|
public float y;
|
|
public float z;
|
|
public float w;
|
|
|
|
public Quat32()
|
|
{
|
|
this.x = 0;
|
|
this.y = 0;
|
|
this.z = 0;
|
|
this.w = 1;
|
|
}
|
|
|
|
public Quat32(float x, float y, float z, float w)
|
|
{
|
|
this.x = x;
|
|
this.y = y;
|
|
this.z = z;
|
|
this.w = w;
|
|
}
|
|
|
|
public static Quat32 FromSwingTwist(SwingTwist s)
|
|
{
|
|
Quat32 q32 = Quat32.Euler(-s.swing.vertical, s.swing.horizontal, s.twist);
|
|
return q32;
|
|
}
|
|
|
|
public static Quat32 Euler(float yaw, float pitch, float roll)
|
|
{
|
|
float rollOver2 = roll * Angle.Deg2Rad * 0.5f;
|
|
float sinRollOver2 = (float)Math.Sin((float)rollOver2);
|
|
float cosRollOver2 = (float)Math.Cos((float)rollOver2);
|
|
float pitchOver2 = pitch * 0.5f;
|
|
float sinPitchOver2 = (float)Math.Sin((float)pitchOver2);
|
|
float cosPitchOver2 = (float)Math.Cos((float)pitchOver2);
|
|
float yawOver2 = yaw * 0.5f;
|
|
float sinYawOver2 = (float)Math.Sin((float)yawOver2);
|
|
float cosYawOver2 = (float)Math.Cos((float)yawOver2);
|
|
Quat32 result = new Quat32()
|
|
{
|
|
w = cosYawOver2 * cosPitchOver2 * cosRollOver2 +
|
|
sinYawOver2 * sinPitchOver2 * sinRollOver2,
|
|
x = sinYawOver2 * cosPitchOver2 * cosRollOver2 +
|
|
cosYawOver2 * sinPitchOver2 * sinRollOver2,
|
|
y = cosYawOver2 * sinPitchOver2 * cosRollOver2 -
|
|
sinYawOver2 * cosPitchOver2 * sinRollOver2,
|
|
z = cosYawOver2 * cosPitchOver2 * sinRollOver2 -
|
|
sinYawOver2 * sinPitchOver2 * cosRollOver2
|
|
};
|
|
return result;
|
|
}
|
|
|
|
public void ToAngles(out float right, out float up, out float forward)
|
|
{
|
|
float test = this.x * this.y + this.z * this.w;
|
|
if (test > 0.499f)
|
|
{ // singularity at north pole
|
|
right = 0;
|
|
up = 2 * (float)Math.Atan2(this.x, this.w) * Angle.Rad2Deg;
|
|
forward = 90;
|
|
return;
|
|
//return Vector3(0, 2 * (float)atan2(this.x, this.w) * Angle.Rad2Deg, 90);
|
|
}
|
|
else if (test < -0.499f)
|
|
{ // singularity at south pole
|
|
right = 0;
|
|
up = -2 * (float)Math.Atan2(this.x, this.w) * Angle.Rad2Deg;
|
|
forward = -90;
|
|
return;
|
|
//return Vector3(0, -2 * (float)atan2(this.x, this.w) * Angle.Rad2Deg, -90);
|
|
}
|
|
else
|
|
{
|
|
float sqx = this.x * this.x;
|
|
float sqy = this.y * this.y;
|
|
float sqz = this.z * this.z;
|
|
|
|
right = (float)Math.Atan2(2 * this.x * this.w - 2 * this.y * this.z, 1 - 2 * sqx - 2 * sqz) * Angle.Rad2Deg;
|
|
up = (float)Math.Atan2(2 * this.y * this.w - 2 * this.x * this.z, 1 - 2 * sqy - 2 * sqz) * Angle.Rad2Deg;
|
|
forward = (float)Math.Asin(2 * test) * Angle.Rad2Deg;
|
|
return;
|
|
// return Vector3(
|
|
// atan2f(2 * this.x * this.w - 2 * this.y * this.z, 1 - 2 * sqx - 2 * sqz) *
|
|
// Rad2Deg,
|
|
// atan2f(2 * this.y * this.w - 2 * this.x * this.z, 1 - 2 * sqy - 2 * sqz) *
|
|
// Rad2Deg,
|
|
// asinf(2 * test) * Angle.Rad2Deg);
|
|
}
|
|
}
|
|
|
|
}
|
|
} |