/// <summary>
/// Sets this quaternion to the rotation of the given angle-axis</summary>
/// <param name="a">Angle-axis</param>
public void Set(AngleAxisF a)
{
double mag = Math.Sqrt(a.Axis.X * a.Axis.X + a.Axis.Y * a.Axis.Y + a.Axis.Z * a.Axis.Z);
if (mag < EPS)
{
X = Y = Z = W = 0;
}
else
{
double sin = Math.Sin(a.Angle / 2.0);
double scale = sin / mag;
W = (float)Math.Cos(a.Angle / 2.0);
X = (float)(a.Axis.X * scale);
Y = (float)(a.Axis.Y * scale);
Z = (float)(a.Axis.Z * scale);
}
}
/// <summary>
/// Sets the matrix to the rotation specified by the angle-axis</summary>
/// <param name="a">Angle-axis representation of rotation</param>
public void Set(AngleAxisF a)
{
double mag = Math.Sqrt(a.Axis.X * a.Axis.X + a.Axis.Y * a.Axis.Y + a.Axis.Z * a.Axis.Z);
if (mag < EPS)
{
Set(Identity);
}
else
{
double ooMag = 1.0 / mag;
double x = a.Axis.X * ooMag;
double y = a.Axis.Y * ooMag;
double z = a.Axis.Z * ooMag;
double sin = Math.Sin(a.Angle);
double cos = Math.Cos(a.Angle);
double t = 1.0 - cos;
double xz = a.Axis.X * a.Axis.Z;
double xy = a.Axis.X * a.Axis.Y;
double yz = a.Axis.Y * a.Axis.Z;
M11 = (float)(t * x * x + cos);
M12 = (float)(t * xy - sin * z);
M13 = (float)(t * xz + sin * y);
M21 = (float)(t * xy + sin * z);
M22 = (float)(t * y * y + cos);
M23 = (float)(t * yz - sin * x);
M31 = (float)(t * xz - sin * y);
M32 = (float)(t * yz + sin * x);
M33 = (float)(t * z * z + cos);
M14 = M24 = M34 = M41 = M42 = M43 = 0;
M44 = 1;
}
}
/// <summary>
/// Constructs a matrix with the same rotation as the given angle axis</summary>
/// <param name="angleAxis">AngleAxis representing rotation</param>
public Matrix4F(AngleAxisF angleAxis)
{
Set(angleAxis);
}
