public static Quatf Rotate(Quatf q, float angleInDegrees, Vec3f axis)
{
var tmp = axis;
// Axis of rotation must be normalized.
float len = tmp.Length;
if ((float)Math.Abs(len - 1.0f) > 0.001f)
{
float oneOverLen = 1.0f / len;
tmp.x *= oneOverLen;
tmp.y *= oneOverLen;
tmp.z *= oneOverLen;
}
//float AngleRad = MathFunctions.Radians(angleInDegrees);
float AngleRad = MathF.ToRadians(angleInDegrees);
float fSin = (float)Math.Sin(AngleRad * 0.5f);
return(q * new Quatf()
{
w = (float)Math.Cos(AngleRad * 0.5f),
x = tmp.x * fSin,
y = tmp.y * fSin,
z = tmp.z * fSin
});
}
public static Quatf AngleAxis(float angle, Vec3f axis)
{
Quatf result = UnitW;
//float a = MathFunctions.Radians(angle);
float a = MathF.ToRadians(angle);
float s = (float)Math.Sin(a * 0.5f);
result.w = (float)Math.Cos(a * 0.5f);
result.x = axis.x * s;
result.y = axis.y * s;
result.z = axis.z * s;
return(result);
}
