public void ShouldHandleInvalidQuaternionCorrectly() { Quat q = new Quat { x = 0, y = 0, z = 0, w = 10 }; AxisAngle aa = new AxisAngle(); aa.FromQuaternion(q); Assert.That(aa.Axis.x, Is.EqualTo(0).Within(EPS)); Assert.That(aa.Axis.y, Is.EqualTo(0).Within(EPS)); Assert.That(aa.Axis.z, Is.EqualTo(1).Within(EPS)); Assert.That(aa.Angle, Is.EqualTo(0).Within(EPS)); }
public void ShouldConvertFromQuaternionCorrectly() { Quat q = new Quat { x = 0.2430199533700943, y = 0.4860399067401886, z = 0.7290598750114441, w = -0.416146844625473 }; AxisAngle aa = new AxisAngle(); aa.FromQuaternion(q); Assert.That(aa.Axis.x, Is.EqualTo(0.26726123690605164).Within(EPS)); Assert.That(aa.Axis.y, Is.EqualTo(0.5345224738121033).Within(EPS)); Assert.That(aa.Axis.z, Is.EqualTo(0.8017837405204773).Within(EPS)); Assert.That(aa.Angle, Is.EqualTo(4.000000017768291).Within(EPS)); }
public Quat ToQuaternion() { var quat = new Quat(); var l = Math.Sqrt(Axis.x * Axis.x + Axis.y * Axis.y + Axis.z * Axis.z); if (l > 0.00001) { double s = Math.Sin(Angle / 2) / l; quat.x = Axis.x * s; quat.y = Axis.y * s; quat.z = Axis.z * s; quat.w = Math.Cos(Angle / 2); } else { quat.x = quat.y = quat.z = 0; quat.w = 1; } return quat; }
public void FromQuaternion(Quat quat) { double s = Math.Sqrt(1 - quat.w * quat.w); if (s < 0.001 || double.IsNaN(s)) { Axis.x = 0; Axis.y = 0; Axis.z = 1; Angle = 0; } else { s = 1 / s; Axis.x = quat.x * s; Axis.y = quat.y * s; Axis.z = quat.z * s; Angle = 2 * Math.Acos(quat.w); } }