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