public Quaternion ToQuaternion()
{
// https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
// [X=Yaw, Y=Pitch, Z=Roll] to Quaternion
float yaw = RyneMath.DegreesToRadians(X) * 0.5f;
float pitch = RyneMath.DegreesToRadians(Y) * 0.5f;
float roll = RyneMath.DegreesToRadians(Z) * 0.5f;
float cy = RyneMath.Cos(yaw);
float sy = RyneMath.Sin(yaw);
float cp = RyneMath.Cos(pitch);
float sp = RyneMath.Sin(pitch);
float cr = RyneMath.Cos(roll);
float sr = RyneMath.Sin(roll);
Quaternion result = new Quaternion
{
W = cy * cp * cr + sy * sp * sr,
X = cy * cp * sr - sy * sp * cr,
Y = sy * cp * sr + cy * sp * cr,
Z = sy * cp * cr - cy * sp * sr
};
return(result);
}
// TODO: untested
public Quaternion Interpolate(Quaternion to, float dt)
{
Float4 v0 = ToFloat4();
Float4 v1 = to.ToFloat4();
float dot = v0.Dot(v1);
if (dot < 0.0f)
{
v1 = new Float4(-v1.X, -v1.Y, -v1.Z, -v1.W);
dot = -dot;
}
if (dot > 0.9995f)
{
return(to);
//// If the inputs are too close for comfort, linearly interpolate and normalize the result
//Float4 result = v0 + dt * (v1 - v0);
//result.Normalize();
//return new Quaternion(result);
}
float theta = RyneMath.Acos(dot);
float deltaTheta = theta * dt;
float sinTheta = RyneMath.Sin(theta);
float sinDeltaTheta = RyneMath.Sin(deltaTheta);
float s0 = RyneMath.Cos(deltaTheta) - dot * sinDeltaTheta / sinTheta;
float s1 = sinDeltaTheta / sinTheta;
return(new Quaternion((s0 * v0) + (s1 * v1)));
}
public Rotator ToRotator()
{
// http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
Rotator result = new Rotator();
// Roll (rotation around x-axis)
float sinr = 2.0f * (W * X + Y * Z);
float cosr = 1.0f - 2.0f * (X * X + Y * Y);
result.Z = RyneMath.Atan2(sinr, cosr);
// Pitch (rotation around y-axis)
float sinp = 2.0f * (W * Y - Z * X);
if (RyneMath.Abs(sinp) >= 1)
{
result.Y = Constants.PI / 2.0f * RyneMath.Sign(sinp); // use 90 degrees if out of range
}
else
{
result.Y = RyneMath.Asin(sinp);
}
// Yaw (rotation around z-axis)
float siny = 2.0f * (W * Z + X * Y);
float cosy = 1.0f - 2.0f * (Y * Y + Z * Z);
result.X = RyneMath.Atan2(siny, cosy);
result.X = RyneMath.RadiansToDegrees(result.X);
result.Y = RyneMath.RadiansToDegrees(result.Y);
result.Z = RyneMath.RadiansToDegrees(result.Z);
return(result);
}
public Quaternion(Float3 axis, float angle, bool degrees = true)
{
//qx = ax * sin(angle/2)
//qy = ay * sin(angle/2)
//qz = az * sin(angle/2)
//qw = cos(angle/2)
if (degrees)
{
angle = RyneMath.DegreesToRadians(angle);
}
float halfAngle = angle * 0.5f;
Float4 data = new Float4(axis * RyneMath.Sin(halfAngle), RyneMath.Cos(halfAngle)).Normalize();
SetData(out this, data);
}
public float RotationAngle(bool degrees = true)
{
return(RyneMath.RadiansToDegrees(2.0f * RyneMath.Acos(W)));
}
