Merge pull request #22 from adambrewster/dcm2q

Add a method to compute quaternion from direction cosine matrix

Author: SimonDanisch

src/Quaternion.jl

@@ -208,6 +208,32 @@ function qrotation(rotvec::Vector{T}) where {T <: Real}
     Quaternion(one(T), zero(T), zero(T), zero(T), true)
 end
 
+function qrotation{T<:Real}(dcm::Matrix{T})
+    # See https://arxiv.org/pdf/math/0701759.pdf
+    a2 = 1 + dcm[1,1] + dcm[2,2] + dcm[3,3]
+    b2 = 1 + dcm[1,1] - dcm[2,2] - dcm[3,3]
+    c2 = 1 - dcm[1,1] + dcm[2,2] - dcm[3,3]
+    d2 = 1 - dcm[1,1] - dcm[2,2] + dcm[3,3]
+
+    if a2 >= max(b2, c2, d2)
+        a = sqrt(a2)/2
+        return Quaternion(a, (dcm[3,2]-dcm[2,3])/4a, (dcm[1,3]-dcm[3,1])/4a, (dcm[2,1]-dcm[1,2])/4a)
+    elseif b2 >= max(c2, d2)
+        b = sqrt(b2)/2
+        return Quaternion((dcm[3,2]-dcm[2,3])/4b, b, (dcm[2,1]+dcm[1,2])/4b, (dcm[1,3]+dcm[3,1])/4b)
+    elseif c2 >= d2
+        c = sqrt(c2)/2
+        return Quaternion((dcm[1,3]-dcm[3,1])/4c, (dcm[2,1]+dcm[1,2])/4c, c, (dcm[3,2]+dcm[2,3])/4c)
+    else
+        d = sqrt(d2)/2
+        return Quaternion((dcm[2,1]-dcm[1,2])/4d, (dcm[1,3]+dcm[3,1])/4d, (dcm[3,2]+dcm[2,3])/4d, d)
+    end
+end
+
+function qrotation{T<:Real}(dcm::Matrix{T}, qa::Quaternion)
+    q = qrotation(dcm)
+    abs(q-qa) < abs(q+qa) ? q : -q
+end
 
 rotationmatrix(q::Quaternion) = rotationmatrix_normalized(normalize(q))
 

test/test_Quaternion.jl

@@ -108,6 +108,12 @@ for _ in 1:100
         @test normalize(qn) === qn
     end
 
+    let # test rotation <=> rotationmatrix
+        q1 = nquatrand()
+        q2 = qrotation(rotationmatrix(q1), q1)
+        @test q1 ≈ q2
+    end
+
     let # test slerp and linpol if q1 = 1
         q1 = quat(1, 0, 0, 0.)
         # there are numerical stability issues with slerp atm