3D rotation matrix to quaternion

Question

There are multiple ways to represent a 3D rotation. The most intuitive way is the rotation matrix – $$A=\begin{bmatrix}A_{11}&A_{12}&A_{13}\\A_{21}&A_{22}&A_{23}\\A_{31}&A_{32}&A_{33}\end{bmatrix}$$ rotates a point \$p=(x,y,z)^T\$ by left-multiplication: \$p'=Ap\$. This is however inefficient since a 3D rotation has only three degrees of freedom, so I personally prefer the quaternion representation where \$p\$ is interpreted as the quaternion \$0+xi+yj+zk\$ and then conjugated by the rotating unit quaternion \$q\$ to get \$p'=qpq^*\$. Dropping the scalar (first) part leaves the rotated 3D point.

\$A\$ can be converted to a quaternion \$q=s+ai+bj+ck\$ with the same effect as follows: $$s=\frac{\sqrt{1+A_{11}+A_{22}+A_{33}}}2$$ $$a=\frac{A_{32}-A_{23}}{4s}\qquad b=\frac{A_{13}-A_{31}}{4s}\qquad c=\frac{A_{21}-A_{12}}{4s}$$ Note that \$-q\$ has the same effect as \$q\$ by distribution of scalars in \$qpq^*\$, so the quaternion representation is not unique.

Task

Given a 3D rotation matrix – an orthogonal (to floating-point error) 3×3 real matrix with determinant 1 – output a quaternion with the same effect as that matrix to within \$10^{-6}\$ entrywise relative error.

You may use any reasonable and consistent formats for input and output, and any correct formula for the calculation (including the one in the previous section). In particular the code has to handle \$s=0\$ cases, for which alternate formulas are available from this paper: $$a=\frac{\sqrt{1+A_{11}-A_{22}-A_{33}}}2\\ b=\frac{\sqrt{1-A_{11}+A_{22}-A_{33}}}2\\ c=\frac{\sqrt{1-A_{11}-A_{22}+A_{33}}}2$$ $$b=\frac{A_{32}+A_{23}}{4c}\qquad c=\frac{A_{13}+A_{31}}{4a}\qquad a=\frac{A_{21}+A_{12}}{4b}$$

This is code-golf; fewest bytes wins.

Test cases

These are in the format \$A\to q\$. The random cases were generated through this program – you can make your own cases there too!

[[1, 0, 0], [0, 1, 0], [0, 0, 1]] -> [1.0, 0.0, 0.0, 0.0]
[[0, 0, 1], [1, 0, 0], [0, 1, 0]] -> [0.5, 0.5, 0.5, 0.5]
[[0, -1, 0], [1, 0, 0], [0, 0, 1]] -> [0.7071067811865476, 0.0, 0.0, 0.7071067811865476]
[[-1, 0, 0], [0, -1, 0], [0, 0, 1]] -> [0.0, 0.0, 0.0, 1.0]
[[0, 1, 0], [1, 0, 0], [0, 0, -1]] -> [0.0, 0.7071067811865476, 0.7071067811865476, 0.0]
[[0.1508272311227814, -0.2824279103927633, -0.9473571775117411], [-0.7156381696218376, -0.692324273056756, 0.09246140338941596], [-0.6819920501971715, 0.6640192590336603, -0.3065375459878405]] -> [0.19491370659742277, 0.7330908965072773, -0.3403623223156108, -0.5556436573799196]
[[0.13019043201085, -0.4676759473774085, -0.874259492174647], [0.4976035235357375, 0.7934932016832866, -0.35037019314894013], [0.8575786755614011, -0.3894197569808834, 0.33602242201273236]] -> [0.7516159351202696, -0.01298853643440075, -0.5760382686201726, 0.32106805677246825]

Answer 1

PARI/GP, 83 bytes

m->qfjacobi(matconcat([2*trace(m),s=[(n=m-m~)[3,2],n[1,3],n[2,1]];s~,m+m~]))[2][,4]

Attempt This Online!

Using this formula on Wikipedia:

If the matrix contains significant error, such as accumulated numerical error, we may construct a symmetric \$4\times 4\$ matrix, $$\frac13 \begin{bmatrix} Q_{xx}-Q_{yy}-Q_{zz} & Q_{yx}+Q_{xy} & Q_{zx}+Q_{xz} & Q_{zy}-Q_{yz} \\ Q_{yx}+Q_{xy} & Q_{yy}-Q_{xx}-Q_{zz} & Q_{zy}+Q_{yz} & Q_{xz}-Q_{zx} \\ Q_{zx}+Q_{xz} & Q_{zy}+Q_{yz} & Q_{zz}-Q_{xx}-Q_{yy} & Q_{yx}-Q_{xy} \\ Q_{zy}-Q_{yz} & Q_{xz}-Q_{zx} & Q_{yx}-Q_{xy} & Q_{xx}+Q_{yy}+Q_{zz} \end{bmatrix} ,$$ and find the eigenvector, \$(x,y,z,w)\$, of its largest magnitude eigenvalue.

Answer 2

JavaScript (ES7), 189 bytes

Edit: Thanks to Neil for pointing out that the Alternative Method described here was not reliable. Now using a port of the (hopefully safe) Java code on the same page.

Expects a flat array of 9 values.

a=>[0,s=1,2,3].map(i=>(j=m>>7+2*i&3)<3?(a[j^2||7]+a[7/j^6]*~-(m>>i+2&2))/s:s/4,m=a[0]+a[4]+a[8]>0?9607:a[4]>a[8]?a[0]>a[4]?3937:23122:28852,[0,1,2].map(i=>s+=a[i*4]*(m>>i&1||-1)),s=s**.5*2)

Try it online!

Answer 3

Charcoal, 77 66 bytes

≔Ｅθ§ικη⟦Ｉ⊘₂⊕Ση⟧ＩＥθ×⊘₂⊕ΣＥη⎇⁼κμλ±λ∨±›⁰∨⁻§§θ⊖κ⊕ꧧθ⊕κ⊖κ∧κ∨§ι⁰∧⊖κ§ι¹¦¹

Try it online! Link is to verbose version of code. Explanation:

≔Ｅθ§ικη

Get the main diagonal of A.

⟦Ｉ⊘₂⊕Ση⟧

Calculate and output s.

ＩＥθ×⊘₂⊕ΣＥη⎇⁼κμλ±λ

Calculate and output a, b and c, but...

∨±›⁰...¹

change the sign if one of the following is negative:

∨⁻§§θ⊖κ⊕ꧧθ⊕κ⊖κ∧κ

sa, sb or sc respectively, or if s is zero then when calculating b or c then...

∨§ι⁰∧⊖κ

... ab+sc or ac-sb respectively, or if both s and a are zero when calculating c then...

§ι¹

... bc+sa.

Answer 4

JavaScript, 283 bytes

m=>((a='21',b='10',c='20',T=([a,b,c])=>S=1+C(0)*~-a+C(1)*~-b+C(2)*~-c,K=([a,b,s])=>m[a][b]+m[b][a]*~-s,C=a=>m[a][a])(2)<0?C(0)>C(1)?[K(a+0),T(c+0),K(b+1),K(0+a)]:[K(0+c),K(b+1),T(0+c),K(a+1)]:C(0)<-C(1)?[K(b+0),K(0+a),K(a+1),T('002')]:[T('222'),K(a),K(0+c),K(b+0)]).map(x=>x/2/S**.5)

Converted Code from here to JavaScript code (based on this article):

if (m22 < 0) {
   if (m00 >m11) {
       t = 1 + m00 -m11 -m22;
       q = Quaternion( t, m01+m10, m20+m02, m12-m21 );
   }
   else {
       t = 1 -m00 + m11 -m22;
       q = Quaternion( m01+m10, t, m12+m21, m20-m02 );
   }
}
else {
   if (m00 < -m11) {
       t = 1 -m00 -m11 + m22;
       q = Quaternion( m20+m02, m12+m21, t, m01-m10 );
   }
   else {
       t = 1 + m00 + m11 + m22;
       q = Quaternion( m12-m21, m20-m02, m01-m10, t );
   }
}
q *= 0.5 / Sqrt(t);

Try it:

f=m=>((a='21',b='10',c='20',T=([a,b,c])=>S=1+C(0)*~-a+C(1)*~-b+C(2)*~-c,K=([a,b,s])=>m[a][b]+m[b][a]*~-s,C=a=>m[a][a])(2)<0?C(0)>C(1)?[K(a+0),T(c+0),K(b+1),K(0+a)]:[K(0+c),K(b+1),T(0+c),K(a+1)]:C(0)<-C(1)?[K(b+0),K(0+a),K(a+1),T('002')]:[T('222'),K(a),K(0+c),K(b+0)]).map(x=>x/2/S**.5)

;[
  [
    [-7 / 9, 4 / 9, -4 / 9],
    [4 / 9, -1 / 9, -8 / 9],
    [-4 / 9, -8 / 9, -1 / 9],
  ],
  [
    [1, 0, 0],
    [0, 1, 0],
    [0, 0, 1],
  ],
  [
    [0, 0, 1],
    [1, 0, 0],
    [0, 1, 0],
  ],
  [
    [0, -1, 0],
    [1, 0, 0],
    [0, 0, 1],
  ],
  [
    [-1, 0, 0],
    [0, -1, 0],
    [0, 0, 1],
  ],
  [
    [0, 1, 0],
    [1, 0, 0],
    [0, 0, -1],
  ],
  [
    [0.1508272311227814, -0.2824279103927633, -0.9473571775117411],
    [-0.7156381696218376, -0.692324273056756, 0.09246140338941596],
    [-0.6819920501971715, 0.6640192590336603, -0.3065375459878405],
  ],
  [
    [0.13019043201085, -0.4676759473774085, -0.874259492174647],
    [0.4976035235357375, 0.7934932016832866, -0.35037019314894013],
    [0.8575786755614011, -0.3894197569808834, 0.33602242201273236],
  ],
].forEach(m => console.log(JSON.stringify(f(m))));

Thanks to Arnauld for the tip to reduce bytes count

Answer 5

MATLAB/Octave, 539 110 bytes

saved 429 bytes.

Thanks to all useful comments (@ceilingcat, @alephalpha, et al.).

---

the 110 bytes version proprosed by @ceilingcat is as follows:

function q=f(R)S=R-R';[V,L]=eig([2*trace(R),T=[S(3,2),S(1,3),S(2,1)];T',R+R']);[M,I]=max(diag(L));q=V(:,I);end

Try it online!

one original explicit code (maybe more fast?) is

function q = rotm_to_quat(R)
    tr = trace(R);
    if tr > 0
        S = sqrt(tr + 1.0) * 2;
        qw = 0.25 * S;
        qx = (R(3,2) - R(2,3)) / S;
        qy = (R(1,3) - R(3,1)) / S;
        qz = (R(2,1) - R(1,2)) / S;
    elseif (R(1,1) > R(2,2)) && (R(1,1) > R(3,3))
        S = sqrt(1.0 + R(1,1) - R(2,2) - R(3,3)) * 2;
        qw = (R(3,2) - R(2,3)) / S;
        qx = 0.25 * S;
        qy = (R(1,2) + R(2,1)) / S;
        qz = (R(1,3) + R(3,1)) / S;
    elseif R(2,2) > R(3,3)
        S = sqrt(1.0 + R(2,2) - R(1,1) - R(3,3)) * 2;
        qw = (R(1,3) - R(3,1)) / S;
        qx = (R(1,2) + R(2,1)) / S;
        qy = 0.25 * S;
        qz = (R(2,3) + R(3,2)) / S;
    else
        S = sqrt(1.0 + R(3,3) - R(1,1) - R(2,2)) * 2;
        qw = (R(2,1) - R(1,2)) / S;
        qx = (R(1,3) + R(3,1)) / S;
        qy = (R(2,3) + R(3,2)) / S;
        qz = 0.25 * S;
    end
    q = [qw, qx, qy, qz];
end