public Quaternion4d(Vector3d axis, double angle)
{
var axisN = axis.Normalized();
var a = angle * 0.5;
var sina = Math.Sin(a);
var cosa = Math.Cos(a);
x = axisN.x * sina;
y = axisN.y * sina;
z = axisN.z * sina;
w = cosa;
}
public Quaternion4d(Vector3d to, Vector3d from)
{
var f = from.Normalized();
var t = to.Normalized();
var dotProdPlus1 = 1.0 + f.Dot(t);
if (dotProdPlus1 < 1e-7)
{
w = 0;
if (Math.Abs(f.x) < 0.6)
{
var norm = Math.Sqrt(1 - f.x * f.x);
x = 0;
y = f.z / norm;
z = -f.y / norm;
}
else if (Math.Abs(f.y) < 0.6)
{
var norm = Math.Sqrt(1 - f.y * f.y);
x = -f.z / norm;
y = 0;
z = f.x / norm;
}
else
{
var norm = Math.Sqrt(1 - f.z * f.z);
x = f.y / norm;
y = -f.x / norm;
z = 0;
}
}
else
{
var s = Math.Sqrt(0.5 * dotProdPlus1);
var tmp = (f.Cross(t)) / (2.0 * s);
x = tmp.x;
y = tmp.y;
z = tmp.z;
w = s;
}
}
