public Vector3d ToAxisAngle(out double angle)
{
Quaterniond quat = this;
if (Math.Abs(quat.w) > 1.0)
{
quat.Normalize();
}
Vector3d r;
double d = Math.Sqrt(1.0 - quat.w * quat.w);
if (d > 0)
{
r = new Vector3d(quat.x / d, quat.y / d, quat.z / d);
}
else
{
r = new Vector3d(1, 0, 0);
}
angle = 2 * Math.Acos(quat.w);
return(r);
}
public Quaterniond ExtractRotation()
{
Vector3d row0xyz = new Vector3d(Row0.x, Row0.y, Row0.z).Normalized;
Vector3d row1xyz = new Vector3d(Row1.x, Row1.y, Row1.z).Normalized;
Vector3d row2xyz = new Vector3d(Row2.x, Row2.y, Row2.z).Normalized;
Quaterniond q = new Quaterniond();
double trace = 0.25 * (row0xyz.x + row1xyz.y + row2xyz.z + 1.0);
if (trace > 0)
{
double sq = Math.Sqrt(trace);
q.w = sq;
sq = 1.0 / (4.0 * sq);
q.x = (row1xyz.z - row2xyz.y) * sq;
q.y = (row2xyz.x - row0xyz.z) * sq;
q.z = (row0xyz.y - row1xyz.x) * sq;
}
else if (row0xyz.x > row1xyz.y && row0xyz.x > row2xyz.z)
{
double sq = 2.0 * Math.Sqrt(1.0 + row0xyz.x - row1xyz.y - row2xyz.z);
q.w = 0.25 * sq;
sq = 1.0 / sq;
q.x = (row2xyz.y - row1xyz.z) * sq;
q.y = (row1xyz.x - row0xyz.y) * sq;
q.z = (row2xyz.x - row0xyz.z) * sq;
}
else if (row1xyz.y > row2xyz.z)
{
double sq = 2.0 * Math.Sqrt(1.0 + row1xyz.y - row0xyz.x - row2xyz.z);
q.w = 0.25 * sq;
sq = 1.0 / sq;
q.x = (row2xyz.x - row0xyz.z) * sq;
q.y = (row1xyz.x - row0xyz.y) * sq;
q.z = (row2xyz.y - row1xyz.z) * sq;
}
else
{
double sq = 2.0 * Math.Sqrt(1.0 + row2xyz.z - row0xyz.x - row1xyz.y);
q.w = 0.25 * sq;
sq = 1.0 / sq;
q.x = (row1xyz.x - row0xyz.y) * sq;
q.y = (row2xyz.x - row0xyz.z) * sq;
q.z = (row2xyz.y - row1xyz.z) * sq;
}
q.Normalize();
return(q);
}
