/// <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); }