public static TheQuaternion Slerp(TheQuaternion a, TheQuaternion b, float value)
{
TheQuaternion result;
float dot = DotProduct(a, b);
if (dot < 0)
{
dot = -dot;
result = -b;
}
else
{
result = b;
}
if (dot < 0.95f)
{
float angle = Math.Math.Acos(dot);
return((a * Math.Math.Sin(angle * (1 - value)) + result * Math.Math.Sin(angle * value)) / Math.Math.Sin(angle));
}
else
{
return(Lerp(a, result, value));
}
}
public static TheQuaternion FromAngleAxis(float angle, TheVector3 axis)
{
TheVector3 tmp = axis * Math.Math.Sin(angle / 2);
TheQuaternion ret = new TheQuaternion
{
x = tmp.x,
y = tmp.y,
z = tmp.z,
w = Math.Math.Cos(angle / 2)
};
return(ret);
}
public override bool Equals(object other)
{
bool ret;
if (other == null || !(other is TheQuaternion))
{
ret = false;
}
else
{
TheQuaternion quaternion = (TheQuaternion)other;
ret = (x == quaternion.x && y == quaternion.y && z == quaternion.z && w == quaternion.w);
}
return(ret);
}
public static TheQuaternion Normalize(TheQuaternion quaternion)
{
TheQuaternion ret;
float magnitude = Magnitude(quaternion);
if (magnitude > 1E-05f)
{
ret = quaternion / magnitude;
}
else
{
ret = Identity;
}
return(ret);
}
public static void ToAngleAxis(TheQuaternion a, out float angle, out TheVector3 axis)
{
angle = Math.Math.Acos(a.w);
float sin_theta_inv = 1.0f / Math.Math.Sin(angle);
axis = new TheVector3
{
x = a.x * sin_theta_inv,
y = a.y * sin_theta_inv,
z = a.z * sin_theta_inv
};
angle *= 2;
}
public static TheQuaternion RotateTowards(TheQuaternion from, TheQuaternion to, float step)
{
float angle = AngleBetween(from, to);
TheQuaternion result;
if (angle == 0f)
{
result = to;
}
else
{
float value = Math.Math.Min(1f, step / angle);
result = Slerp(from, to, value);
}
return(result);
}
public static TheQuaternion FromEulerAngles(TheVector3 angles)
{
float cos_z_2 = Math.Math.Cos(0.5f * angles.z);
float cos_y_2 = Math.Math.Cos(0.5f * angles.y);
float cos_x_2 = Math.Math.Cos(0.5f * angles.x);
float sin_z_2 = Math.Math.Sin(0.5f * angles.z);
float sin_y_2 = Math.Math.Sin(0.5f * angles.y);
float sin_x_2 = Math.Math.Sin(0.5f * angles.x);
TheQuaternion ret = new TheQuaternion
{
x = cos_z_2 * cos_y_2 * sin_x_2 - sin_z_2 * sin_y_2 * cos_x_2,
y = cos_z_2 * sin_y_2 * cos_x_2 + sin_z_2 * cos_y_2 * sin_x_2,
z = sin_z_2 * cos_y_2 * cos_x_2 - cos_z_2 * sin_y_2 * sin_x_2,
w = cos_z_2 * cos_y_2 * cos_x_2 + sin_z_2 * sin_y_2 * sin_x_2
};
return(ret);
}
public static TheQuaternion Lerp(TheQuaternion a, TheQuaternion b, float value)
{
return((a * (1 - value) + b * value).Normalized);
}
public static float AngleBetween(TheQuaternion a, TheQuaternion b)
{
float dot = DotProduct(a, b);
return(Math.Math.Acos(Math.Math.Min(Math.Math.Abs(dot), 1f)) * 2f * Math.Math.RadToDeg);
}
public static float DotProduct(TheQuaternion a, TheQuaternion b)
{
return(a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w);
}
public static float Magnitude(TheQuaternion quaternion)
{
return(Math.Math.Sqrt(quaternion.x * quaternion.x + quaternion.y * quaternion.y + quaternion.z * quaternion.z + quaternion.w * quaternion.w));
}
