fix piracy and type instability (#53)

* fix piracy of normalize(::Vector) and type instability of qrotation(::Vector)

* Apply suggestions from code review

Co-authored-by: Seth Axen <[email protected]>

* zero axis in qrotation() forces zero rotation

* Update src/Quaternion.jl

Co-authored-by: Seth Axen <[email protected]>

Co-authored-by: Seth Axen <[email protected]>
Co-authored-by: mikmoore <[email protected]>

Author: mikmoore

src/Quaternion.jl

@@ -207,9 +207,14 @@ function qrotation(axis::Vector{T}, theta) where {T <: Real}
     if length(axis) != 3
         error("Must be a 3-vector")
     end
-    u = normalize(axis)
-    s = sin(theta / 2)
-    Quaternion(cos(theta / 2), s * u[1], s * u[2], s * u[3], true)
+    normaxis = norm(axis)
+    if iszero(normaxis)
+        normaxis = oneunit(normaxis)
+        theta = zero(theta)
+    end
+    s,c = sincos(theta / 2)
+    scaleby = s / normaxis
+    Quaternion(c, scaleby * axis[1], scaleby * axis[2], scaleby * axis[3], true)
 end
 
 # Variant of the above where norm(rotvec) encodes theta
@@ -218,11 +223,9 @@ function qrotation(rotvec::Vector{T}) where {T <: Real}
         error("Must be a 3-vector")
     end
     theta = norm(rotvec)
-    if theta > 0
-        s = sin(theta / 2) / theta  # divide by theta to make rotvec a unit vector
-        return Quaternion(cos(theta / 2), s * rotvec[1], s * rotvec[2], s * rotvec[3], true)
-    end
-    Quaternion(one(T), zero(T), zero(T), zero(T), true)
+    s,c = sincos(theta / 2)
+    scaleby = s / (iszero(theta) ? one(theta) : theta)
+    Quaternion(c, scaleby * rotvec[1], scaleby * rotvec[2], scaleby * rotvec[3], true)
 end
 
 function qrotation(dcm::Matrix{T}) where {T<:Real}
@@ -263,14 +266,6 @@ function rotationmatrix_normalized(q::Quaternion)
         xz - sy      yz + sx  1 - (xx + yy)]
 end
 
-function normalize(v::Vector{T}) where {T}
-    nv = norm(v)
-    if nv > 0
-        return v / nv
-    end
-    zeros(promote_type(T, typeof(nv)), length(v))
-end
-
 
 function slerp(qa::Quaternion{T}, qb::Quaternion{T}, t::T) where {T}
     # http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/slerp/

test/test_Quaternion.jl

@@ -26,6 +26,9 @@ for _ in 1:10, T in (Float32, Float64, Int32, Int64)
 end
 
 let # test rotations
+    @test qrotation([0, 0, 0], 1.0) == Quaternion(1.0) # a zero axis should act like zero rotation
+    @test qrotation([1, 0, 0], 0.0) == Quaternion(1.0)
+    @test qrotation([0, 0, 0]) == Quaternion(1.0)
     qx = qrotation([1, 0, 0], pi / 4)
     @test qx * qx ≈ qrotation([1, 0, 0], pi / 2)
     @test qx^2 ≈ qrotation([1, 0, 0], pi / 2)