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