From 5ce33b78b261f04ed68fc418e544fc82d2a37cbb Mon Sep 17 00:00:00 2001
From: pvazteixeira <pvt@mit.edu>
Date: Fri, 7 Sep 2018 00:22:35 -0400
Subject: [PATCH] zero allocation in normalize.

---
 src/TransformUtils.jl      | 32 +++++++++++++++++---------------
 test/testInPlaceConvert.jl | 18 ++++++++++++++++--
 2 files changed, 33 insertions(+), 17 deletions(-)

diff --git a/src/TransformUtils.jl b/src/TransformUtils.jl
index 3ff074b..ea1e37b 100644
--- a/src/TransformUtils.jl
+++ b/src/TransformUtils.jl
@@ -169,9 +169,10 @@ function normalize!(q::Quaternion, tol=1e-6)
       s += q.v[i]^2
   end
   @fastmath mag1 = 1.0/sqrt(s)
-  @fastmath @inbounds for i in 1:3
-    q.v[i] .*= mag1
-  end
+  # @fastmath @inbounds for i in 1:3
+  #   q.v[i] *= mag1
+  # end
+  @fastmath @inbounds q.v .*= mag1
   @fastmath q.s *= mag1
   nothing
 end
@@ -345,17 +346,18 @@ function convert!(R::SO3, q::Quaternion)
       q.s = -q.s
       q.v[1:3] = -q.v[1:3]
     end
-    nrm = sqrt(q.s^2+sum(q.v[1:3].^2))
-    if (nrm < 0.999)
-      println("q2C -- not a unit quaternion nrm = $(nrm)")
-      R = eye(3)
-    else
-      nrm = 1.0/nrm
-      q.s *= nrm
-      q.v[1:3] .*= nrm
-      # x = x*nrm
-      # y = y*nrm
-      # z = z*nrm
+    # nrm = sqrt(q.s^2+sum(q.v[1:3].^2))
+    # if (nrm < 0.999)
+    #   println("q2C -- not a unit quaternion nrm = $(nrm)")
+    #   R = eye(3)
+    # else
+      normalize!(q)
+      # nrm = 1.0/nrm
+      # q.s *= nrm
+      # q.v .*= nrm
+      # # x = x*nrm
+      # # y = y*nrm
+      # # z = z*nrm
       w2, x2, y2, z2 = q.s*q.s, q.v[1]*q.v[1], q.v[2]*q.v[2], q.v[3]*q.v[3]
       # y2 = y*y
       # z2 = z*z
@@ -377,7 +379,7 @@ function convert!(R::SO3, q::Quaternion)
       R.R[3,1] = xz-wy
       R.R[3,2] = yz+wx
       R.R[3,3] = w2-x2-y2+z2
-    end
+    # end
   end
   # return SO3(R)
   nothing
diff --git a/test/testInPlaceConvert.jl b/test/testInPlaceConvert.jl
index f60378f..8c2f1b4 100644
--- a/test/testInPlaceConvert.jl
+++ b/test/testInPlaceConvert.jl
@@ -71,7 +71,6 @@ function normalize!(q::Quaternion, tol=1e-6)
 end
 
 
-q = convert(Quaternion, so3(randn(3)))
 
 @code_warntype normalize!(q)
 
@@ -94,8 +93,23 @@ q = convert(Quaternion, so3(randn(3)))
 
 
 q = convert(Quaternion, so3(randn(3)))
+E = convert(Euler, convert(SO3, so3(randn(3))))
+
 
 
-function convert!(q::Quaternion, E::Euler)
+@btime convert!(q, E)
 
 #
+Profile.clear()
+@profiler convert!(q, E)
+
+
+Profile.clear()
+@profiler for i in 1:10^6 convert!(q, E); end
+
+
+
+
+@code_warntype convert!(q, E)
+
+# julia --track-allocation=user testInPlaceConvert.jl