diff --git a/examples/2_Intermediate/stage_two_optimization_planar_coils.py b/examples/2_Intermediate/stage_two_optimization_planar_coils.py
index c84525868..d32ca4da8 100755
--- a/examples/2_Intermediate/stage_two_optimization_planar_coils.py
+++ b/examples/2_Intermediate/stage_two_optimization_planar_coils.py
@@ -114,7 +114,7 @@
 # Define the individual terms objective function:
 Jf = SquaredFlux(s, bs)
 Jls = [CurveLength(c) for c in base_curves]
-Jccdist = CurveCurveDistance(curves, CC_THRESHOLD, num_basecurves=ncoils)  # Error
+Jccdist = CurveCurveDistance(curves, CC_THRESHOLD, num_basecurves=ncoils)
 Jcsdist = CurveSurfaceDistance(curves, s, CS_THRESHOLD)
 Jcs = [LpCurveCurvature(c, 2, CURVATURE_THRESHOLD) for c in base_curves]
 Jmscs = [MeanSquaredCurvature(c) for c in base_curves]
diff --git a/src/simsopt/geo/curveplanarfourier.py b/src/simsopt/geo/curveplanarfourier.py
index 476ff9600..264e85a86 100644
--- a/src/simsopt/geo/curveplanarfourier.py
+++ b/src/simsopt/geo/curveplanarfourier.py
@@ -23,13 +23,14 @@ class CurvePlanarFourier(sopp.CurvePlanarFourier, Curve):
 
     where :math:`\theta` is the counterclockwise rotation about a unit axis
     :math:`(\hat{x},\hat{y},\hat{z})`.
-
-    The quaternion is normalized for calculations to prevent scaling. The dofs
-    themselves are not normalized. This results in a redundancy in the
-    optimization, where several different sets of dofs may correspond to the
-    same normalized quaternion. Normalizing the dofs directly would create a
-    dependence between the quaternion dofs, which may cause issues during
-    optimization.
+    
+    A quaternion is used for rotation rather than other methods for rotation to
+    prevent gimbal locking during optimization. The quaternion is normalized for 
+    calculations to prevent scaling. The dofs themselves are not normalized. This 
+    results in a redundancy in the optimization, where several different sets of 
+    dofs may correspond to the same normalized quaternion. Normalizing the dofs 
+    directly would create a dependence between the quaternion dofs, which may cause 
+    issues during optimization.
 
     The dofs are stored in the order
 
diff --git a/src/simsoptpp/curveplanarfourier.cpp b/src/simsoptpp/curveplanarfourier.cpp
index ceda2e9aa..a3db9e2bf 100644
--- a/src/simsoptpp/curveplanarfourier.cpp
+++ b/src/simsoptpp/curveplanarfourier.cpp
@@ -1,22 +1,25 @@
 #include "curveplanarfourier.h"
 
-
 template<class Array>
-void CurvePlanarFourier<Array>::gamma_impl(Array& data, Array& quadpoints) {
-    int numquadpoints = quadpoints.size();
-    data *= 0;
-
-    /* Converts q dofs to unit quaternion */
-    Array q_norm = xt::zeros<double>({4});
+double CurvePlanarFourier<Array>::inv_magnitude() {
     double s = q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3];
     if(s != 0) {
-        for (int i = 0; i < 4; ++i)
-            q_norm[i] = q[i] / std::sqrt(s); 
+	    double sqrt_inv_s = 1 / std::sqrt(s);
+        return sqrt_inv_s;
     }
     else {
-        q_norm[0] = 1;
+        return 1;
     }
 
+}
+
+template<class Array>
+void CurvePlanarFourier<Array>::gamma_impl(Array& data, Array& quadpoints) {
+    int numquadpoints = quadpoints.size();
+    data *= 0;
+
+    Array q_norm = q * inv_magnitude();
+
 
     for (int k = 0; k < numquadpoints; ++k) {
         double phi = 2 * M_PI * quadpoints[k];
@@ -24,9 +27,6 @@ void CurvePlanarFourier<Array>::gamma_impl(Array& data, Array& quadpoints) {
             data(k, 0) += rc[i] * cos(i*phi) * cos(phi);
             data(k, 1) += rc[i] * cos(i*phi) * sin(phi);
         }
-    }
-    for (int k = 0; k < numquadpoints; ++k) {
-        double phi = 2 * M_PI * quadpoints[k];
         for (int i = 1; i < order+1; ++i) {
             data(k, 0) += rs[i-1] * sin(i*phi) * cos(phi);
             data(k, 1) += rs[i-1] * sin(i*phi) * sin(phi);
@@ -40,6 +40,7 @@ void CurvePlanarFourier<Array>::gamma_impl(Array& data, Array& quadpoints) {
         j = data(m, 1);
         k = data(m, 2);
 
+        /* Performs quaternion based rotation, see https://www.cis.upenn.edu/~cis5150/ws-book-Ib.pdf page 575, 576 for details regarding this rotation*/
         data(m, 0) = (i - 2 * (q_norm[2] * q_norm[2] + q_norm[3] * q_norm[3]) * i + 2 * (q_norm[1] * q_norm[2] - q_norm[3] * q_norm[0]) * j + 2 * (q_norm[1] * q_norm[3] + q_norm[2] * q_norm[0]) * k) + center[0];
         data(m, 1) = (2 * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * i + j - 2 * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * j + 2 * (q_norm[2] * q_norm[3] - q_norm[1] * q_norm[0]) * k) + center[1];
         data(m, 2) = (2 * (q_norm[1] * q_norm[3] - q_norm[2] * q_norm[0]) * i + 2 * (q_norm[2] * q_norm[3] + q_norm[1] * q_norm[0]) * j + k - 2 * (q_norm[1] * q_norm[1] + q_norm[2] * q_norm[2]) * k) + center[2];
@@ -50,16 +51,8 @@ template<class Array>
 void CurvePlanarFourier<Array>::gammadash_impl(Array& data) {
     data *= 0;
 
-    /* Converts q dofs to unit quaternion */
-    Array q_norm = xt::zeros<double>({4});
-    double s = q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3];
-    if(s != 0) {
-        for (int i = 0; i < 4; ++i)
-            q_norm[i] = q[i] / std::sqrt(s); 
-    }
-    else {
-        q_norm[0] = 1;
-    }
+    double inv_sqrt_s = inv_magnitude();
+    Array q_norm = q * inv_sqrt_s;
 
     for (int k = 0; k < numquadpoints; ++k) {
         double phi = 2 * M_PI * quadpoints[k];
@@ -67,26 +60,26 @@ void CurvePlanarFourier<Array>::gammadash_impl(Array& data) {
             data(k, 0) += rc[i] * ( -(i) * sin(i*phi) * cos(phi) - cos(i*phi) * sin(phi));
             data(k, 1) += rc[i] * ( -(i) * sin(i*phi) * sin(phi) + cos(i*phi) * cos(phi));
         }
-    }
-    for (int k = 0; k < numquadpoints; ++k) {
-        double phi = 2 * M_PI * quadpoints[k];
         for (int i = 1; i < order+1; ++i) {
             data(k, 0) += rs[i-1] * ( (i) * cos(i*phi) * cos(phi) - sin(i*phi) * sin(phi));
             data(k, 1) += rs[i-1] * ( (i) * cos(i*phi) * sin(phi) + sin(i*phi) * cos(phi));
         }
     }
+        
     data *= (2*M_PI);
     for (int m = 0; m < numquadpoints; ++m) {
-        Array i = xt::zeros<double>({1});
-        i[0] = data(m, 0);
-        Array j = xt::zeros<double>({1});
-        j[0] = data(m, 1);
-        Array k = xt::zeros<double>({1});
-        k[0] = data(m, 2);
-
-        data(m, 0) = (i[0] - 2 * (q_norm[2] * q_norm[2] + q_norm[3] * q_norm[3]) * i[0] + 2 * (q_norm[1] * q_norm[2] - q_norm[3] * q_norm[0]) * j[0] + 2 * (q_norm[1] * q_norm[3] + q_norm[2] * q_norm[0]) * k[0]);
-        data(m, 1) = (2 * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * i[0] + j[0] - 2 * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * j[0] + 2 * (q_norm[2] * q_norm[3] - q_norm[1] * q_norm[0]) * k[0]);
-        data(m, 2) = (2 * (q_norm[1] * q_norm[3] - q_norm[2] * q_norm[0]) * i[0] + 2 * (q_norm[2] * q_norm[3] + q_norm[1] * q_norm[0]) * j[0] + k[0] - 2 * (q_norm[1] * q_norm[1] + q_norm[2] * q_norm[2]) * k[0]);
+        double i;
+        double j;
+        double k;
+        i = data(m, 0);
+        j = data(m, 1);
+        k = data(m, 2);
+
+
+        /* Performs quaternion based rotation, see https://www.cis.upenn.edu/~cis5150/ws-book-Ib.pdf page 575, 576 for details regarding this rotation*/
+        data(m, 0) = (i - 2 * (q_norm[2] * q_norm[2] + q_norm[3] * q_norm[3]) * i + 2 * (q_norm[1] * q_norm[2] - q_norm[3] * q_norm[0]) * j + 2 * (q_norm[1] * q_norm[3] + q_norm[2] * q_norm[0]) * k);
+        data(m, 1) = (2 * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * i + j - 2 * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * j + 2 * (q_norm[2] * q_norm[3] - q_norm[1] * q_norm[0]) * k);
+        data(m, 2) = (2 * (q_norm[1] * q_norm[3] - q_norm[2] * q_norm[0]) * i + 2 * (q_norm[2] * q_norm[3] + q_norm[1] * q_norm[0]) * j + k - 2 * (q_norm[1] * q_norm[1] + q_norm[2] * q_norm[2]) * k);
     }
     
 }
@@ -95,45 +88,33 @@ template<class Array>
 void CurvePlanarFourier<Array>::gammadashdash_impl(Array& data) {
     data *= 0;
 
-    /* Converts q dofs to unit quaternion */
-    Array q_norm = xt::zeros<double>({4});
-    double s = q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3];
-    if(s != 0) {
-        for (int i = 0; i < 4; ++i)
-            q_norm[i] = q[i] / std::sqrt(s); 
-    }
-    else {
-        q_norm[0] = 1;
-    }
+    Array q_norm = q * inv_magnitude();
 
     for (int k = 0; k < numquadpoints; ++k) {
         double phi = 2 * M_PI * quadpoints[k];
         for (int i = 0; i < order+1; ++i) {
-            data(k, 0) += rc[i] * (+2*(i)*sin(i*phi)*sin(phi)-(pow(i, 2)+1)*cos(i*phi)*cos(phi));
-            data(k, 1) += rc[i] * (-2*(i)*sin(i*phi)*cos(phi)-(pow(i, 2)+1)*cos(i*phi)*sin(phi));
+            data(k, 0) += rc[i] * (+2*(i)*sin(i*phi)*sin(phi)-(i*i+1)*cos(i*phi)*cos(phi));
+            data(k, 1) += rc[i] * (-2*(i)*sin(i*phi)*cos(phi)-(i*i+1)*cos(i*phi)*sin(phi));
         }
-        data(k, 2) = 0;
-    }
-    for (int k = 0; k < numquadpoints; ++k) {
-        double phi = 2 * M_PI * quadpoints[k];
         for (int i = 1; i < order+1; ++i) {
-            data(k, 0) += rs[i-1] * (-(pow(i,2)+1)*sin(i*phi)*cos(phi) - 2*(i)*cos(i*phi)*sin(phi));
-            data(k, 1) += rs[i-1] * (-(pow(i,2)+1)*sin(i*phi)*sin(phi) + 2*(i)*cos(i*phi)*cos(phi));
+            data(k, 0) += rs[i-1] * (-(i*i+1)*sin(i*phi)*cos(phi) - 2*(i)*cos(i*phi)*sin(phi));
+            data(k, 1) += rs[i-1] * (-(i*i+1)*sin(i*phi)*sin(phi) + 2*(i)*cos(i*phi)*cos(phi));
         }
-        data(k, 2) = 0;
     }
     data *= 2*M_PI*2*M_PI;
     for (int m = 0; m < numquadpoints; ++m) {
-        Array i = xt::zeros<double>({1});
-        i[0] = data(m, 0);
-        Array j = xt::zeros<double>({1});
-        j[0] = data(m, 1);
-        Array k = xt::zeros<double>({1});
-        k[0] = data(m, 2);
+        double i;
+        double j;
+        double k;
+        i = data(m, 0);
+        j = data(m, 1);
+        k = data(m, 2);
+
 
-        data(m, 0) = (i[0] - 2 * (q_norm[2] * q_norm[2] + q_norm[3] * q_norm[3]) * i[0] + 2 * (q_norm[1] * q_norm[2] - q_norm[3] * q_norm[0]) * j[0] + 2 * (q_norm[1] * q_norm[3] + q_norm[2] * q_norm[0]) * k[0]);
-        data(m, 1) = (2 * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * i[0] + j[0] - 2 * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * j[0] + 2 * (q_norm[2] * q_norm[3] - q_norm[1] * q_norm[0]) * k[0]);
-        data(m, 2) = (2 * (q_norm[1] * q_norm[3] - q_norm[2] * q_norm[0]) * i[0] + 2 * (q_norm[2] * q_norm[3] + q_norm[1] * q_norm[0]) * j[0] + k[0] - 2 * (q_norm[1] * q_norm[1] + q_norm[2] * q_norm[2]) * k[0]);
+        /* Performs quaternion based rotation*/
+        data(m, 0) = (i - 2 * (q_norm[2] * q_norm[2] + q_norm[3] * q_norm[3]) * i + 2 * (q_norm[1] * q_norm[2] - q_norm[3] * q_norm[0]) * j + 2 * (q_norm[1] * q_norm[3] + q_norm[2] * q_norm[0]) * k);
+        data(m, 1) = (2 * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * i + j - 2 * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * j + 2 * (q_norm[2] * q_norm[3] - q_norm[1] * q_norm[0]) * k);
+        data(m, 2) = (2 * (q_norm[1] * q_norm[3] - q_norm[2] * q_norm[0]) * i + 2 * (q_norm[2] * q_norm[3] + q_norm[1] * q_norm[0]) * j + k - 2 * (q_norm[1] * q_norm[1] + q_norm[2] * q_norm[2]) * k);
 
     }
 }
@@ -142,55 +123,45 @@ template<class Array>
 void CurvePlanarFourier<Array>::gammadashdashdash_impl(Array& data) {
     data *= 0;
 
-    /* Converts q dofs to unit quaternion */
-    Array q_norm = xt::zeros<double>({4});
-    double s = q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3];
-    if(s != 0) {
-        for (int i = 0; i < 4; ++i)
-            q_norm[i] = q[i] / std::sqrt(s); 
-    }
-    else {
-        q_norm[0] = 1;
-    }
+    Array q_norm = q * inv_magnitude();
 
     for (int k = 0; k < numquadpoints; ++k) {
         double phi = 2 * M_PI * quadpoints[k];
         for (int i = 0; i < order+1; ++i) {
             data(k, 0) += rc[i]*(
-                    +(3*pow(i, 2) + 1)*cos(i*phi)*sin(phi)
-                    +(pow(i, 2) + 3)*(i)*sin(i*phi)*cos(phi)
+                    +(3*i*i + 1)*cos(i*phi)*sin(phi)
+                    +(i*i + 3)*(i)*sin(i*phi)*cos(phi)
                     );
             data(k, 1) += rc[i]*(
-                    +(pow(i, 2) + 3)*(i)*sin(i*phi)*sin(phi)
-                    -(3*pow(i, 2) + 1)*cos(i*phi)*cos(phi)
+                    +(i*i + 3)*(i)*sin(i*phi)*sin(phi)
+                    -(3*i*i + 1)*cos(i*phi)*cos(phi)
                     );
         }
-    }
-    for (int k = 0; k < numquadpoints; ++k) {
-        double phi = 2 * M_PI * quadpoints[k];
         for (int i = 1; i < order+1; ++i) {
             data(k, 0) += rs[i-1]*(
-                    -(pow(i,2)+3) * (i) * cos(i*phi)*cos(phi)
-                    +(3*pow(i,2)+1) * sin(i*phi)*sin(phi)
+                    -(i*i+3) * (i) * cos(i*phi)*cos(phi)
+                    +(3*i*i+1) * sin(i*phi)*sin(phi)
                     );
             data(k, 1) += rs[i-1]*(
-                    -(pow(i,2)+3)*(i)*cos(i*phi)*sin(phi) 
-                    -(3*pow(i,2)+1)*sin(i*phi)*cos(phi) 
+                    -(i*i+3)*(i)*cos(i*phi)*sin(phi) 
+                    -(3*i*i+1)*sin(i*phi)*cos(phi) 
                     );
         }
     }
     data *= 2*M_PI*2*M_PI*2*M_PI;
     for (int m = 0; m < numquadpoints; ++m) {
-        Array i = xt::zeros<double>({1});
-        i[0] = data(m, 0);
-        Array j = xt::zeros<double>({1});
-        j[0] = data(m, 1);
-        Array k = xt::zeros<double>({1});
-        k[0] = data(m, 2);
+        double i;
+        double j;
+        double k;
+        i = data(m, 0);
+        j = data(m, 1);
+        k = data(m, 2);
+
 
-        data(m, 0) = (i[0] - 2 * (q_norm[2] * q_norm[2] + q_norm[3] * q_norm[3]) * i[0] + 2 * (q_norm[1] * q_norm[2] - q_norm[3] * q_norm[0]) * j[0] + 2 * (q_norm[1] * q_norm[3] + q_norm[2] * q_norm[0]) * k[0]);
-        data(m, 1) = (2 * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * i[0] + j[0] - 2 * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * j[0] + 2 * (q_norm[2] * q_norm[3] - q_norm[1] * q_norm[0]) * k[0]);
-        data(m, 2) = (2 * (q_norm[1] * q_norm[3] - q_norm[2] * q_norm[0]) * i[0] + 2 * (q_norm[2] * q_norm[3] + q_norm[1] * q_norm[0]) * j[0] + k[0] - 2 * (q_norm[1] * q_norm[1] + q_norm[2] * q_norm[2]) * k[0]);
+        /* Performs quaternion based rotation*/
+        data(m, 0) = (i - 2 * (q_norm[2] * q_norm[2] + q_norm[3] * q_norm[3]) * i + 2 * (q_norm[1] * q_norm[2] - q_norm[3] * q_norm[0]) * j + 2 * (q_norm[1] * q_norm[3] + q_norm[2] * q_norm[0]) * k);
+        data(m, 1) = (2 * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * i + j - 2 * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * j + 2 * (q_norm[2] * q_norm[3] - q_norm[1] * q_norm[0]) * k);
+        data(m, 2) = (2 * (q_norm[1] * q_norm[3] - q_norm[2] * q_norm[0]) * i + 2 * (q_norm[2] * q_norm[3] + q_norm[1] * q_norm[0]) * j + k - 2 * (q_norm[1] * q_norm[1] + q_norm[2] * q_norm[2]) * k);
 
     }
 }
@@ -199,335 +170,327 @@ template<class Array>
 void CurvePlanarFourier<Array>::dgamma_by_dcoeff_impl(Array& data) {
     data *= 0;
     
-    /* Converts q dofs to unit quaternion */
-    Array q_norm = xt::zeros<double>({4});
-    double s = q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3];
-    if(s != 0) {
-        for (int i = 0; i < 4; ++i)
-            q_norm[i] = q[i] / std::sqrt(s); 
-    }
-    else {
-        q_norm[0] = 1;
-    }
+    Array q_norm = q * inv_magnitude();
 
     for (int m = 0; m < numquadpoints; ++m) {
         double phi = 2 * M_PI * quadpoints[m];
         int counter = 0;
-        Array i = xt::zeros<double>({1});
-        Array j = xt::zeros<double>({1});
-        Array k = xt::zeros<double>({1});
+        double i;
+        double j;
+        double k;
 
         for (int n = 0; n < order+1; ++n) {
-            i[0] = cos(n*phi) * cos(phi);
-            j[0] = cos(n*phi) * sin(phi);
-            k[0] = 0;
+            i = cos(n*phi) * cos(phi);
+            j = cos(n*phi) * sin(phi);
+            k = 0;
 
-            data(m, 0, counter) = (i[0] - 2 * (q_norm[2] * q_norm[2] + q_norm[3] * q_norm[3]) * i[0] + 2 * (q_norm[1] * q_norm[2] - q_norm[3] * q_norm[0]) * j[0] + 2 * (q_norm[1] * q_norm[3] + q_norm[2] * q_norm[0]) * k[0]);
-            data(m, 1, counter) = (2 * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * i[0] + j[0] - 2 * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * j[0] + 2 * (q_norm[2] * q_norm[3] - q_norm[1] * q_norm[0]) * k[0]);
-            data(m, 2, counter) = (2 * (q_norm[1] * q_norm[3] - q_norm[2] * q_norm[0]) * i[0] + 2 * (q_norm[2] * q_norm[3] + q_norm[1] * q_norm[0]) * j[0] + k[0] - 2 * (q_norm[1] * q_norm[1] + q_norm[2] * q_norm[2]) * k[0]);
+            data(m, 0, counter) = (i - 2 * (q_norm[2] * q_norm[2] + q_norm[3] * q_norm[3]) * i + 2 * (q_norm[1] * q_norm[2] - q_norm[3] * q_norm[0]) * j + 2 * (q_norm[1] * q_norm[3] + q_norm[2] * q_norm[0]) * k);
+            data(m, 1, counter) = (2 * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * i + j - 2 * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * j + 2 * (q_norm[2] * q_norm[3] - q_norm[1] * q_norm[0]) * k);
+            data(m, 2, counter) = (2 * (q_norm[1] * q_norm[3] - q_norm[2] * q_norm[0]) * i + 2 * (q_norm[2] * q_norm[3] + q_norm[1] * q_norm[0]) * j + k - 2 * (q_norm[1] * q_norm[1] + q_norm[2] * q_norm[2]) * k);
 
             counter++;
         }
         
         for (int n = 1; n < order+1; ++n) {
-            i[0] = sin(n*phi) * cos(phi);
-            j[0] = sin(n*phi) * sin(phi);
-            k[0] = 0;
+            i = sin(n*phi) * cos(phi);
+            j = sin(n*phi) * sin(phi);
+            k = 0;
 
-            data(m, 0, counter) = (i[0] - 2 * (q_norm[2] * q_norm[2] + q_norm[3] * q_norm[3]) * i[0] + 2 * (q_norm[1] * q_norm[2] - q_norm[3] * q_norm[0]) * j[0] + 2 * (q_norm[1] * q_norm[3] + q_norm[2] * q_norm[0]) * k[0]);
-            data(m, 1, counter) = (2 * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * i[0] + j[0] - 2 * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * j[0] + 2 * (q_norm[2] * q_norm[3] - q_norm[1] * q_norm[0]) * k[0]);
-            data(m, 2, counter) = (2 * (q_norm[1] * q_norm[3] - q_norm[2] * q_norm[0]) * i[0] + 2 * (q_norm[2] * q_norm[3] + q_norm[1] * q_norm[0]) * j[0] + k[0] - 2 * (q_norm[1] * q_norm[1] + q_norm[2] * q_norm[2]) * k[0]);
+            data(m, 0, counter) = (i - 2 * (q_norm[2] * q_norm[2] + q_norm[3] * q_norm[3]) * i + 2 * (q_norm[1] * q_norm[2] - q_norm[3] * q_norm[0]) * j + 2 * (q_norm[1] * q_norm[3] + q_norm[2] * q_norm[0]) * k);
+            data(m, 1, counter) = (2 * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * i + j - 2 * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * j + 2 * (q_norm[2] * q_norm[3] - q_norm[1] * q_norm[0]) * k);
+            data(m, 2, counter) = (2 * (q_norm[1] * q_norm[3] - q_norm[2] * q_norm[0]) * i + 2 * (q_norm[2] * q_norm[3] + q_norm[1] * q_norm[0]) * j + k - 2 * (q_norm[1] * q_norm[1] + q_norm[2] * q_norm[2]) * k);
 
             counter++;
         }
 
-        i[0] = 0;
-        j[0] = 0;
-        k[0] = 0;
+        i = 0;
+        j = 0;
+        k = 0;
 
         for (int n = 0; n < order+1; ++n) {
-            i[0] += rc[n] * cos(n*phi) * cos(phi);
-            j[0] += rc[n] * cos(n*phi) * sin(phi);
+            i += rc[n] * cos(n*phi) * cos(phi);
+            j += rc[n] * cos(n*phi) * sin(phi);
         }
         for (int n = 1; n < order+1; ++n) {
-            i[0] += rs[n-1] * sin(n*phi) * cos(phi);
-            j[0] += rs[n-1] * sin(n*phi) * sin(phi);
+            i += rs[n-1] * sin(n*phi) * cos(phi);
+            j += rs[n-1] * sin(n*phi) * sin(phi);
         }
+        double inv_sqrt_s = inv_magnitude();
         
-        data(m, 0, counter) = (4 * i[0] * (q_norm[2] * q_norm[2] + q_norm[3] * q_norm[3]) * q_norm[0]
-                            - 4 * j[0] * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[0]
-                            - 2 * j[0] * q_norm[3]) 
-                            * (1 / std::sqrt(s) - q[0] * q[0] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (4 * i[0] * (q_norm[2] * q_norm[2] + q_norm[3] * q_norm[3]) * q_norm[1]
-                            - 4 * j[0] * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[1]
-                            + 2 * j[0] * q_norm[2]) 
-                            * (- q[1] * q[0] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (- 4 * i[0] * (q_norm[0] * q_norm[0] + q_norm[1] * q_norm[1]) * q_norm[2]
-                            - 4 * j[0] * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[2]
-                            + 2 * j[0] * q_norm[1]) 
-                            * (- q[2] * q[0] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (- 4 * i[0] * (q_norm[1] * q_norm[1] + q_norm[0] * q_norm[0]) * q_norm[3]
-                            - 4 * j[0] * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[3]
-                            - 2 * j[0] * q_norm[0]) 
-                            * (- q[3] * q[0] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)));
-
-
-        data(m, 1, counter) = (- 4 * i[0] * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[0]
-                            + 2 * i[0] * q_norm[3]
-                            + 4 * j[0] * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[0])
-                            * (1 / std::sqrt(s) - q[0] * q[0] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (- 4 * i[0] * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[1]
-                            + 2 * i[0] * q_norm[2]
-                            + 4 * j[0] * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[1]
-                            - 4 * j[0] * q_norm[1])
-                            * (- q[1] * q[0] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (- 4 * i[0] * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[2]
-                            + 2 * i[0] * q_norm[1]
-                            + 4 * j[0] * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[2])
-                            * (- q[2] * q[0] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (- 4 * i[0] * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[3]
-                            + 2 * i[0] * q_norm[0]
-                            + 4 * j[0] * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[3]
-                            - 4 * j[0] * q_norm[3])
-                            * (- q[3] * q[0] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)));
-
-
-        data(m, 2, counter) = (- 4 * i[0] * (q_norm[1] * q_norm[3] - q_norm[2] * q_norm[0]) * q_norm[0]
-                            - 2 * i[0] * q_norm[2]
-                            - 4 * j[0] * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[0]
-                            + 2 * j[0] * q_norm[1])
-                            * (1 / std::sqrt(s) - q[0] * q[0] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (- 4 * i[0] * (q_norm[1] * q_norm[3] - q_norm[2] * q_norm[0]) * q_norm[1]
-                            + 2 * i[0] * q_norm[3]
-                            - 4 * j[0] * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[1]
-                            + 2 * j[0] * q_norm[0])
-                            * (- q[1] * q[0] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (- 2 * i[0] * q_norm[0]
-                            - 4 * i[0] * (q_norm[1] * q_norm[3] - q_norm[0] * q_norm[2]) * q_norm[2]
-                            - 4 * j[0] * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[2]
-                            + 2 * j[0] * q_norm[3])
-                            * (- q[2] * q[0] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (2 * i[0] * q_norm[1]
-                            + 4 * i[0] * (q_norm[2] * q_norm[0] - q_norm[1] * q_norm[3]) * q_norm[3]
-                            - 4 * j[0] * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[3]
-                            + 2 * j[0] * q_norm[2])
-                            * (- q[3] * q[0] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)));
+        data(m, 0, counter) = (4 * i * (q_norm[2] * q_norm[2] + q_norm[3] * q_norm[3]) * q_norm[0]
+                            - 4 * j * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[0]
+                            - 2 * j * q_norm[3]) 
+                            * (inv_sqrt_s - q[0] * q[0] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (4 * i * (q_norm[2] * q_norm[2] + q_norm[3] * q_norm[3]) * q_norm[1]
+                            - 4 * j * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[1]
+                            + 2 * j * q_norm[2]) 
+                            * (- q[1] * q[0] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (- 4 * i * (q_norm[0] * q_norm[0] + q_norm[1] * q_norm[1]) * q_norm[2]
+                            - 4 * j * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[2]
+                            + 2 * j * q_norm[1]) 
+                            * (- q[2] * q[0] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (- 4 * i * (q_norm[1] * q_norm[1] + q_norm[0] * q_norm[0]) * q_norm[3]
+                            - 4 * j * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[3]
+                            - 2 * j * q_norm[0]) 
+                            * (- q[3] * q[0] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s);
+
+
+        data(m, 1, counter) = (- 4 * i * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[0]
+                            + 2 * i * q_norm[3]
+                            + 4 * j * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[0])
+                            * (inv_sqrt_s - q[0] * q[0] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (- 4 * i * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[1]
+                            + 2 * i * q_norm[2]
+                            + 4 * j * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[1]
+                            - 4 * j * q_norm[1])
+                            * (- q[1] * q[0] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (- 4 * i * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[2]
+                            + 2 * i * q_norm[1]
+                            + 4 * j * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[2])
+                            * (- q[2] * q[0] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (- 4 * i * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[3]
+                            + 2 * i * q_norm[0]
+                            + 4 * j * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[3]
+                            - 4 * j * q_norm[3])
+                            * (- q[3] * q[0] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s);
+
+
+        data(m, 2, counter) = (- 4 * i * (q_norm[1] * q_norm[3] - q_norm[2] * q_norm[0]) * q_norm[0]
+                            - 2 * i * q_norm[2]
+                            - 4 * j * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[0]
+                            + 2 * j * q_norm[1])
+                            * (inv_sqrt_s - q[0] * q[0] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (- 4 * i * (q_norm[1] * q_norm[3] - q_norm[2] * q_norm[0]) * q_norm[1]
+                            + 2 * i * q_norm[3]
+                            - 4 * j * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[1]
+                            + 2 * j * q_norm[0])
+                            * (- q[1] * q[0] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (- 2 * i * q_norm[0]
+                            - 4 * i * (q_norm[1] * q_norm[3] - q_norm[0] * q_norm[2]) * q_norm[2]
+                            - 4 * j * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[2]
+                            + 2 * j * q_norm[3])
+                            * (- q[2] * q[0] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (2 * i * q_norm[1]
+                            + 4 * i * (q_norm[2] * q_norm[0] - q_norm[1] * q_norm[3]) * q_norm[3]
+                            - 4 * j * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[3]
+                            + 2 * j * q_norm[2])
+                            * (- q[3] * q[0] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s);
         counter++;
 
-        data(m, 0, counter) = (4 * i[0] * (q_norm[2] * q_norm[2] + q_norm[3] * q_norm[3]) * q_norm[0] 
-                            - 4 * j[0] * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[0]
-                            - 2 * j[0] * q_norm[3]) 
-                            * (- q[0] * q[1] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (4 * i[0] * (q_norm[2] * q_norm[2] + q_norm[3] * q_norm[3]) * q_norm[1]
-                            - 4 * j[0] * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[1]
-                            + 2 * j[0] * q_norm[2]) 
-                            * (1 / std::sqrt(s) - q[1] * q[1] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (- 4 * i[0] * (q_norm[0] * q_norm[0] + q_norm[1] * q_norm[1]) * q_norm[2]
-                            - 4 * j[0] * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[2]
-                            + 2 * j[0] * q_norm[1]) 
-                            * (- q[2] * q[1] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (- 4 * i[0] * (q_norm[1] * q_norm[1] + q_norm[0] * q_norm[0]) * q_norm[3]
-                            - 4 * j[0] * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[3]
-                            - 2 * j[0] * q_norm[0])
-                            * (- q[3] * q[1] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)));
-
-
-        data(m, 1, counter) = (- 4 * i[0] * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[0]
-                            + 2 * i[0] * q_norm[3]
-                            + 4 * j[0] * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[0])
-                            * (- q[0] * q[1] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (- 4 * i[0] * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[1]
-                            + 2 * i[0] * q_norm[2]
-                            + 4 * j[0] * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[1]
-                            - 4 * j[0] * q_norm[1])
-                            * (1 / std::sqrt(s) - q[1] * q[1] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (- 4 * i[0] * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[2]
-                            + 2 * i[0] * q_norm[1]
-                            + 4 * j[0] * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[2])
-                            * (- q[2] * q[1] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (- 4 * i[0] * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[3]
-                            + 2 * i[0] * q_norm[0]
-                            + 4 * j[0] * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[3]
-                            - 4 * j[0] * q_norm[3])
-                            * (- q[3] * q[1] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)));
-
-
-        data(m, 2, counter) = (- 4 * i[0] * (q_norm[1] * q_norm[3] - q_norm[2] * q_norm[0]) * q_norm[0]
-                            - 2 * i[0] * q_norm[2]
-                            - 4 * j[0] * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[0]
-                            + 2 * j[0] * q_norm[1])
-                            * (- q[0] * q[1] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (- 4 * i[0] * (q_norm[1] * q_norm[3] - q_norm[2] * q_norm[0]) * q_norm[1]
-                            + 2 * i[0] * q_norm[3]
-                            - 4 * j[0] * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[1]
-                            + 2 * j[0] * q_norm[0])
-                            * (1 / std::sqrt(s) - q[1] * q[1] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (- 2 * i[0] * q_norm[0]
-                            - 4 * i[0] * (q_norm[1] * q_norm[3] - q_norm[0] * q_norm[2]) * q_norm[2]
-                            - 4 * j[0] * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[2]
-                            + 2 * j[0] * q_norm[3])
-                            * (- q[2] * q[1] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (2 * i[0] * q_norm[1]
-                            + 4 * i[0] * (q_norm[2] * q_norm[0] - q_norm[1] * q_norm[3]) * q_norm[3]
-                            - 4 * j[0] * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[3]
-                            + 2 * j[0] * q_norm[2])
-                            * (- q[3] * q[1] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)));
+        data(m, 0, counter) = (4 * i * (q_norm[2] * q_norm[2] + q_norm[3] * q_norm[3]) * q_norm[0] 
+                            - 4 * j * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[0]
+                            - 2 * j * q_norm[3]) 
+                            * (- q[0] * q[1] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (4 * i * (q_norm[2] * q_norm[2] + q_norm[3] * q_norm[3]) * q_norm[1]
+                            - 4 * j * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[1]
+                            + 2 * j * q_norm[2]) 
+                            * (inv_sqrt_s - q[1] * q[1] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (- 4 * i * (q_norm[0] * q_norm[0] + q_norm[1] * q_norm[1]) * q_norm[2]
+                            - 4 * j * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[2]
+                            + 2 * j * q_norm[1]) 
+                            * (- q[2] * q[1] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (- 4 * i * (q_norm[1] * q_norm[1] + q_norm[0] * q_norm[0]) * q_norm[3]
+                            - 4 * j * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[3]
+                            - 2 * j * q_norm[0])
+                            * (- q[3] * q[1] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s);
+
+
+        data(m, 1, counter) = (- 4 * i * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[0]
+                            + 2 * i * q_norm[3]
+                            + 4 * j * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[0])
+                            * (- q[0] * q[1] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (- 4 * i * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[1]
+                            + 2 * i * q_norm[2]
+                            + 4 * j * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[1]
+                            - 4 * j * q_norm[1])
+                            * (inv_sqrt_s - q[1] * q[1] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (- 4 * i * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[2]
+                            + 2 * i * q_norm[1]
+                            + 4 * j * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[2])
+                            * (- q[2] * q[1] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (- 4 * i * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[3]
+                            + 2 * i * q_norm[0]
+                            + 4 * j * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[3]
+                            - 4 * j * q_norm[3])
+                            * (- q[3] * q[1] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s);
+
+
+        data(m, 2, counter) = (- 4 * i * (q_norm[1] * q_norm[3] - q_norm[2] * q_norm[0]) * q_norm[0]
+                            - 2 * i * q_norm[2]
+                            - 4 * j * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[0]
+                            + 2 * j * q_norm[1])
+                            * (- q[0] * q[1] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (- 4 * i * (q_norm[1] * q_norm[3] - q_norm[2] * q_norm[0]) * q_norm[1]
+                            + 2 * i * q_norm[3]
+                            - 4 * j * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[1]
+                            + 2 * j * q_norm[0])
+                            * (inv_sqrt_s - q[1] * q[1] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (- 2 * i * q_norm[0]
+                            - 4 * i * (q_norm[1] * q_norm[3] - q_norm[0] * q_norm[2]) * q_norm[2]
+                            - 4 * j * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[2]
+                            + 2 * j * q_norm[3])
+                            * (- q[2] * q[1] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (2 * i * q_norm[1]
+                            + 4 * i * (q_norm[2] * q_norm[0] - q_norm[1] * q_norm[3]) * q_norm[3]
+                            - 4 * j * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[3]
+                            + 2 * j * q_norm[2])
+                            * (- q[3] * q[1] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s);
         counter++;
 
-        data(m, 0, counter) = (4 * i[0] * (q_norm[2] * q_norm[2] + q_norm[3] * q_norm[3]) * q_norm[0] 
-                            - 4 * j[0] * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[0]
-                            - 2 * j[0] * q_norm[3]) 
-                            * (- q[0] * q[2] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
+        data(m, 0, counter) = (4 * i * (q_norm[2] * q_norm[2] + q_norm[3] * q_norm[3]) * q_norm[0] 
+                            - 4 * j * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[0]
+                            - 2 * j * q_norm[3]) 
+                            * (- q[0] * q[2] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
                             + 
-                            (4 * i[0] * (q_norm[2] * q_norm[2] + q_norm[3] * q_norm[3]) * q_norm[1]
-                            - 4 * j[0] * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[1]
-                            + 2 * j[0] * q_norm[2]) 
-                            * (- q[1] * q[2] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (- 4 * i[0] * (q_norm[0] * q_norm[0] + q_norm[1] * q_norm[1]) * q_norm[2]
-                            - 4 * j[0] * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[2]
-                            + 2 * j[0] * q_norm[1]) 
-                            * (1 / std::sqrt(s) - q[2] * q[2] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (- 4 * i[0] * (q_norm[1] * q_norm[1] + q_norm[0] * q_norm[0]) * q_norm[3]
-                            - 4 * j[0] * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[3]
-                            - 2 * j[0] * q_norm[0]) 
-                            * (- q[3] * q[2] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)));
-
-
-        data(m, 1, counter) = (- 4 * i[0] * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[0]
-                            + 2 * i[0] * q_norm[3]
-                            + 4 * j[0] * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[0])
-                            * (- q[0] * q[2] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (- 4 * i[0] * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[1]
-                            + 2 * i[0] * q_norm[2]
-                            + 4 * j[0] * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[1]
-                            - 4 * j[0] * q_norm[1])
-                            * (- q[1] * q[2] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (- 4 * i[0] * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[2]
-                            + 2 * i[0] * q_norm[1]
-                            + 4 * j[0] * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[2])
-                            * (1 / std::sqrt(s) - q[2] * q[2] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (- 4 * i[0] * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[3]
-                            + 2 * i[0] * q_norm[0]
-                            + 4 * j[0] * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[3]
-                            - 4 * j[0] * q_norm[3])
-                            * (- q[3] * q[2] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)));
-
-
-        data(m, 2, counter) = (- 4 * i[0] * (q_norm[1] * q_norm[3] - q_norm[2] * q_norm[0]) * q_norm[0]
-                            - 2 * i[0] * q_norm[2]
-                            - 4 * j[0] * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[0]
-                            + 2 * j[0] * q_norm[1])
-                            * (- q[0] * q[2] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (- 4 * i[0] * (q_norm[1] * q_norm[3] - q_norm[2] * q_norm[0]) * q_norm[1]
-                            + 2 * i[0] * q_norm[3]
-                            - 4 * j[0] * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[1]
-                            + 2 * j[0] * q_norm[0])
-                            * (- q[1] * q[2] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (- 2 * i[0] * q_norm[0]
-                            - 4 * i[0] * (q_norm[1] * q_norm[3] - q_norm[0] * q_norm[2]) * q_norm[2]
-                            - 4 * j[0] * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[2]
-                            + 2 * j[0] * q_norm[3])
-                            * (1 / std::sqrt(s) - q[2] * q[2] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (2 * i[0] * q_norm[1]
-                            + 4 * i[0] * (q_norm[2] * q_norm[0] - q_norm[1] * q_norm[3]) * q_norm[3]
-                            - 4 * j[0] * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[3]
-                            + 2 * j[0] * q_norm[2])
-                            * (- q[3] * q[2] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)));
+                            (4 * i * (q_norm[2] * q_norm[2] + q_norm[3] * q_norm[3]) * q_norm[1]
+                            - 4 * j * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[1]
+                            + 2 * j * q_norm[2]) 
+                            * (- q[1] * q[2] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (- 4 * i * (q_norm[0] * q_norm[0] + q_norm[1] * q_norm[1]) * q_norm[2]
+                            - 4 * j * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[2]
+                            + 2 * j * q_norm[1]) 
+                            * (inv_sqrt_s - q[2] * q[2] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (- 4 * i * (q_norm[1] * q_norm[1] + q_norm[0] * q_norm[0]) * q_norm[3]
+                            - 4 * j * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[3]
+                            - 2 * j * q_norm[0]) 
+                            * (- q[3] * q[2] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s);
+
+
+        data(m, 1, counter) = (- 4 * i * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[0]
+                            + 2 * i * q_norm[3]
+                            + 4 * j * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[0])
+                            * (- q[0] * q[2] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (- 4 * i * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[1]
+                            + 2 * i * q_norm[2]
+                            + 4 * j * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[1]
+                            - 4 * j * q_norm[1])
+                            * (- q[1] * q[2] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (- 4 * i * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[2]
+                            + 2 * i * q_norm[1]
+                            + 4 * j * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[2])
+                            * (inv_sqrt_s - q[2] * q[2] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (- 4 * i * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[3]
+                            + 2 * i * q_norm[0]
+                            + 4 * j * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[3]
+                            - 4 * j * q_norm[3])
+                            * (- q[3] * q[2] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s);
+
+
+        data(m, 2, counter) = (- 4 * i * (q_norm[1] * q_norm[3] - q_norm[2] * q_norm[0]) * q_norm[0]
+                            - 2 * i * q_norm[2]
+                            - 4 * j * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[0]
+                            + 2 * j * q_norm[1])
+                            * (- q[0] * q[2] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (- 4 * i * (q_norm[1] * q_norm[3] - q_norm[2] * q_norm[0]) * q_norm[1]
+                            + 2 * i * q_norm[3]
+                            - 4 * j * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[1]
+                            + 2 * j * q_norm[0])
+                            * (- q[1] * q[2] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (- 2 * i * q_norm[0]
+                            - 4 * i * (q_norm[1] * q_norm[3] - q_norm[0] * q_norm[2]) * q_norm[2]
+                            - 4 * j * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[2]
+                            + 2 * j * q_norm[3])
+                            * (inv_sqrt_s - q[2] * q[2] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (2 * i * q_norm[1]
+                            + 4 * i * (q_norm[2] * q_norm[0] - q_norm[1] * q_norm[3]) * q_norm[3]
+                            - 4 * j * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[3]
+                            + 2 * j * q_norm[2])
+                            * (- q[3] * q[2] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s);
         counter++;
 
-        data(m, 0, counter) = (4 * i[0] * (q_norm[2] * q_norm[2] + q_norm[3] * q_norm[3]) * q_norm[0] 
-                            - 4 * j[0] * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[0]
-                            - 2 * j[0] * q_norm[3]) 
-                            * (- q[0] * q[3] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
+        data(m, 0, counter) = (4 * i * (q_norm[2] * q_norm[2] + q_norm[3] * q_norm[3]) * q_norm[0] 
+                            - 4 * j * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[0]
+                            - 2 * j * q_norm[3]) 
+                            * (- q[0] * q[3] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
                             +
-                            + (4 * i[0] * (q_norm[2] * q_norm[2] + q_norm[3] * q_norm[3]) * q_norm[1]
-                            - 4 * j[0] * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[1]
-                            + 2 * j[0] * q_norm[2]) 
-                            * (- q[1] * q[3] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
+                            + (4 * i * (q_norm[2] * q_norm[2] + q_norm[3] * q_norm[3]) * q_norm[1]
+                            - 4 * j * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[1]
+                            + 2 * j * q_norm[2]) 
+                            * (- q[1] * q[3] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
                             +
-                            (- 4 * i[0] * (q_norm[0] * q_norm[0] + q_norm[1] * q_norm[1]) * q_norm[2]
-                            - 4 * j[0] * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[2]
-                            + 2 * j[0] * q_norm[1]) 
-                            * (- q[2] * q[3] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
+                            (- 4 * i * (q_norm[0] * q_norm[0] + q_norm[1] * q_norm[1]) * q_norm[2]
+                            - 4 * j * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[2]
+                            + 2 * j * q_norm[1]) 
+                            * (- q[2] * q[3] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
                             +                            
-                            (- 4 * i[0] * (q_norm[1] * q_norm[1] + q_norm[0] * q_norm[0]) * q_norm[3]
-                            - 4 * j[0] * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[3]
-                            - 2 * j[0] * q_norm[0]) 
-                            * (1 / std::sqrt(s) - q[3] * q[3] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)));
-
-
-        data(m, 1, counter) = (- 4 * i[0] * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[0]
-                            + 2 * i[0] * q_norm[3]
-                            + 4 * j[0] * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[0])
-                            * (- q[0] * q[3] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (- 4 * i[0] * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[1]
-                            + 2 * i[0] * q_norm[2]
-                            + 4 * j[0] * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[1]
-                            - 4 * j[0] * q_norm[1])
-                            * (- q[1] * q[3] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (- 4 * i[0] * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[2]
-                            + 2 * i[0] * q_norm[1]
-                            + 4 * j[0] * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[2])
-                            * (- q[2] * q[3] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (- 4 * i[0] * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[3]
-                            + 2 * i[0] * q_norm[0]
-                            + 4 * j[0] * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[3]
-                            - 4 * j[0] * q_norm[3])
-                            * (1 / std::sqrt(s) - q[3] * q[3] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)));
-
-
-        data(m, 2, counter) = (- 4 * i[0] * (q_norm[1] * q_norm[3] - q_norm[2] * q_norm[0]) * q_norm[0]
-                            - 2 * i[0] * q_norm[2]
-                            - 4 * j[0] * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[0]
-                            + 2 * j[0] * q_norm[1])
-                            * (- q[0] * q[3] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (- 4 * i[0] * (q_norm[1] * q_norm[3] - q_norm[2] * q_norm[0]) * q_norm[1]
-                            + 2 * i[0] * q_norm[3]
-                            - 4 * j[0] * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[1]
-                            + 2 * j[0] * q_norm[0])
-                            * (- q[1] * q[3] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (- 2 * i[0] * q_norm[0]
-                            - 4 * i[0] * (q_norm[1] * q_norm[3] - q_norm[0] * q_norm[2]) * q_norm[2]
-                            - 4 * j[0] * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[2]
-                            + 2 * j[0] * q_norm[3])
-                            * (- q[2] * q[3] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (2 * i[0] * q_norm[1]
-                            + 4 * i[0] * (q_norm[2] * q_norm[0] - q_norm[1] * q_norm[3]) * q_norm[3]
-                            - 4 * j[0] * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[3]
-                            + 2 * j[0] * q_norm[2])
-                            * (1 / std::sqrt(s) - q[3] * q[3] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)));
+                            (- 4 * i * (q_norm[1] * q_norm[1] + q_norm[0] * q_norm[0]) * q_norm[3]
+                            - 4 * j * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[3]
+                            - 2 * j * q_norm[0]) 
+                            * (inv_sqrt_s - q[3] * q[3] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s);
+
+
+        data(m, 1, counter) = (- 4 * i * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[0]
+                            + 2 * i * q_norm[3]
+                            + 4 * j * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[0])
+                            * (- q[0] * q[3] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (- 4 * i * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[1]
+                            + 2 * i * q_norm[2]
+                            + 4 * j * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[1]
+                            - 4 * j * q_norm[1])
+                            * (- q[1] * q[3] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (- 4 * i * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[2]
+                            + 2 * i * q_norm[1]
+                            + 4 * j * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[2])
+                            * (- q[2] * q[3] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (- 4 * i * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[3]
+                            + 2 * i * q_norm[0]
+                            + 4 * j * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[3]
+                            - 4 * j * q_norm[3])
+                            * (inv_sqrt_s - q[3] * q[3] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s);
+
+
+        data(m, 2, counter) = (- 4 * i * (q_norm[1] * q_norm[3] - q_norm[2] * q_norm[0]) * q_norm[0]
+                            - 2 * i * q_norm[2]
+                            - 4 * j * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[0]
+                            + 2 * j * q_norm[1])
+                            * (- q[0] * q[3] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (- 4 * i * (q_norm[1] * q_norm[3] - q_norm[2] * q_norm[0]) * q_norm[1]
+                            + 2 * i * q_norm[3]
+                            - 4 * j * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[1]
+                            + 2 * j * q_norm[0])
+                            * (- q[1] * q[3] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (- 2 * i * q_norm[0]
+                            - 4 * i * (q_norm[1] * q_norm[3] - q_norm[0] * q_norm[2]) * q_norm[2]
+                            - 4 * j * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[2]
+                            + 2 * j * q_norm[3])
+                            * (- q[2] * q[3] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (2 * i * q_norm[1]
+                            + 4 * i * (q_norm[2] * q_norm[0] - q_norm[1] * q_norm[3]) * q_norm[3]
+                            - 4 * j * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[3]
+                            + 2 * j * q_norm[2])
+                            * (inv_sqrt_s - q[3] * q[3] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s);
         counter++;
 
 
@@ -546,341 +509,333 @@ template<class Array>
 void CurvePlanarFourier<Array>::dgammadash_by_dcoeff_impl(Array& data) {
     data *= 0;
 
-    /* Converts q dofs to unit quaternion */
-    Array q_norm = xt::zeros<double>({4});
-    double s = q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3];
-    if(s != 0) {
-        for (int i = 0; i < 4; ++i)
-            q_norm[i] = q[i] / std::sqrt(s); 
-    }
-    else {
-        q_norm[0] = 1;
-    }
+    Array q_norm = q * inv_magnitude();
 
     for (int m = 0; m < numquadpoints; ++m) {
         double phi = 2 * M_PI * quadpoints[m];
         int counter = 0;
-        Array i = xt::zeros<double>({1});
-        Array j = xt::zeros<double>({1});
-        Array k = xt::zeros<double>({1});
+        double i;
+        double j;
+        double k;
         for (int n = 0; n < order+1; ++n) {
-            i[0] = ( -(n) * sin(n*phi) * cos(phi) - cos(n*phi) * sin(phi));
-            j[0] = ( -(n) * sin(n*phi) * sin(phi) + cos(n*phi) * cos(phi));
-            k[0] = 0;
+            i = ( -(n) * sin(n*phi) * cos(phi) - cos(n*phi) * sin(phi));
+            j = ( -(n) * sin(n*phi) * sin(phi) + cos(n*phi) * cos(phi));
+            k = 0;
 
-            i[0] *= (2*M_PI);
-            j[0] *= (2*M_PI);
+            i *= (2*M_PI);
+            j *= (2*M_PI);
 
-            data(m, 0, counter) = (i[0] - 2 * (q_norm[2] * q_norm[2] + q_norm[3] * q_norm[3]) * i[0] + 2 * (q_norm[1] * q_norm[2] - q_norm[3] * q_norm[0]) * j[0] + 2 * (q_norm[1] * q_norm[3] + q_norm[2] * q_norm[0]) * k[0]);
-            data(m, 1, counter) = (2 * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * i[0] + j[0] - 2 * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * j[0] + 2 * (q_norm[2] * q_norm[3] - q_norm[1] * q_norm[0]) * k[0]);
-            data(m, 2, counter) = (2 * (q_norm[1] * q_norm[3] - q_norm[2] * q_norm[0]) * i[0] + 2 * (q_norm[2] * q_norm[3] + q_norm[1] * q_norm[0]) * j[0] + k[0] - 2 * (q_norm[1] * q_norm[1] + q_norm[2] * q_norm[2]) * k[0]);
+            data(m, 0, counter) = (i - 2 * (q_norm[2] * q_norm[2] + q_norm[3] * q_norm[3]) * i + 2 * (q_norm[1] * q_norm[2] - q_norm[3] * q_norm[0]) * j + 2 * (q_norm[1] * q_norm[3] + q_norm[2] * q_norm[0]) * k);
+            data(m, 1, counter) = (2 * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * i + j - 2 * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * j + 2 * (q_norm[2] * q_norm[3] - q_norm[1] * q_norm[0]) * k);
+            data(m, 2, counter) = (2 * (q_norm[1] * q_norm[3] - q_norm[2] * q_norm[0]) * i + 2 * (q_norm[2] * q_norm[3] + q_norm[1] * q_norm[0]) * j + k - 2 * (q_norm[1] * q_norm[1] + q_norm[2] * q_norm[2]) * k);
             
 
             counter++;
         }
         
         for (int n = 1; n < order+1; ++n) {
-            i[0] = ( (n) * cos(n*phi) * cos(phi) - sin(n*phi) * sin(phi));
-            j[0] = ( (n) * cos(n*phi) * sin(phi) + sin(n*phi) * cos(phi));
-            k[0] = 0;
+            i = ( (n) * cos(n*phi) * cos(phi) - sin(n*phi) * sin(phi));
+            j = ( (n) * cos(n*phi) * sin(phi) + sin(n*phi) * cos(phi));
+            k = 0;
 
-            i[0] *= (2*M_PI);
-            j[0] *= (2*M_PI);
+            i *= (2*M_PI);
+            j *= (2*M_PI);
 
-            data(m, 0, counter) = (i[0] - 2 * (q_norm[2] * q_norm[2] + q_norm[3] * q_norm[3]) * i[0] + 2 * (q_norm[1] * q_norm[2] - q_norm[3] * q_norm[0]) * j[0] + 2 * (q_norm[1] * q_norm[3] + q_norm[2] * q_norm[0]) * k[0]);
-            data(m, 1, counter) = (2 * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * i[0] + j[0] - 2 * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * j[0] + 2 * (q_norm[2] * q_norm[3] - q_norm[1] * q_norm[0]) * k[0]);
-            data(m, 2, counter) = (2 * (q_norm[1] * q_norm[3] - q_norm[2] * q_norm[0]) * i[0] + 2 * (q_norm[2] * q_norm[3] + q_norm[1] * q_norm[0]) * j[0] + k[0] - 2 * (q_norm[1] * q_norm[1] + q_norm[2] * q_norm[2]) * k[0]);
+            data(m, 0, counter) = (i - 2 * (q_norm[2] * q_norm[2] + q_norm[3] * q_norm[3]) * i + 2 * (q_norm[1] * q_norm[2] - q_norm[3] * q_norm[0]) * j + 2 * (q_norm[1] * q_norm[3] + q_norm[2] * q_norm[0]) * k);
+            data(m, 1, counter) = (2 * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * i + j - 2 * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * j + 2 * (q_norm[2] * q_norm[3] - q_norm[1] * q_norm[0]) * k);
+            data(m, 2, counter) = (2 * (q_norm[1] * q_norm[3] - q_norm[2] * q_norm[0]) * i + 2 * (q_norm[2] * q_norm[3] + q_norm[1] * q_norm[0]) * j + k - 2 * (q_norm[1] * q_norm[1] + q_norm[2] * q_norm[2]) * k);
 
             counter++;
             
         }
         
-        i[0] = 0;
-        j[0] = 0;
-        k[0] = 0;
+        i = 0;
+        j = 0;
+        k = 0;
         for (int n = 0; n < order+1; ++n) {
-            i[0] += rc[n] * ( -(n) * sin(n*phi) * cos(phi) - cos(n*phi) * sin(phi)) * 2 * M_PI;
-            j[0] += rc[n] * ( -(n) * sin(n*phi) * sin(phi) + cos(n*phi) * cos(phi)) * 2 * M_PI;
+            i += rc[n] * ( -(n) * sin(n*phi) * cos(phi) - cos(n*phi) * sin(phi)) * 2 * M_PI;
+            j += rc[n] * ( -(n) * sin(n*phi) * sin(phi) + cos(n*phi) * cos(phi)) * 2 * M_PI;
         }
         for (int n = 1; n < order+1; ++n) {
-                i[0] += rs[n-1] * ( (n) * cos(n*phi) * cos(phi) - sin(n*phi) * sin(phi)) * 2 * M_PI;
-                j[0] += rs[n-1] * ( (n) * cos(n*phi) * sin(phi) + sin(n*phi) * cos(phi)) * 2 * M_PI;
+                i += rs[n-1] * ( (n) * cos(n*phi) * cos(phi) - sin(n*phi) * sin(phi)) * 2 * M_PI;
+                j += rs[n-1] * ( (n) * cos(n*phi) * sin(phi) + sin(n*phi) * cos(phi)) * 2 * M_PI;
         }
-
-        data(m, 0, counter) = (4 * i[0] * (q_norm[2] * q_norm[2] + q_norm[3] * q_norm[3]) * q_norm[0]
-                            - 4 * j[0] * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[0]
-                            - 2 * j[0] * q_norm[3]) 
-                            * (1 / std::sqrt(s) - q[0] * q[0] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (4 * i[0] * (q_norm[2] * q_norm[2] + q_norm[3] * q_norm[3]) * q_norm[1]
-                            - 4 * j[0] * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[1]
-                            + 2 * j[0] * q_norm[2]) 
-                            * (- q[1] * q[0] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (- 4 * i[0] * (q_norm[0] * q_norm[0] + q_norm[1] * q_norm[1]) * q_norm[2]
-                            - 4 * j[0] * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[2]
-                            + 2 * j[0] * q_norm[1]) 
-                            * (- q[2] * q[0] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (- 4 * i[0] * (q_norm[1] * q_norm[1] + q_norm[0] * q_norm[0]) * q_norm[3]
-                            - 4 * j[0] * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[3]
-                            - 2 * j[0] * q_norm[0]) 
-                            * (- q[3] * q[0] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)));
-
-
-        data(m, 1, counter) = (- 4 * i[0] * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[0]
-                            + 2 * i[0] * q_norm[3]
-                            + 4 * j[0] * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[0])
-                            * (1 / std::sqrt(s) - q[0] * q[0] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (- 4 * i[0] * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[1]
-                            + 2 * i[0] * q_norm[2]
-                            + 4 * j[0] * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[1]
-                            - 4 * j[0] * q_norm[1])
-                            * (- q[1] * q[0] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (- 4 * i[0] * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[2]
-                            + 2 * i[0] * q_norm[1]
-                            + 4 * j[0] * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[2])
-                            * (- q[2] * q[0] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (- 4 * i[0] * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[3]
-                            + 2 * i[0] * q_norm[0]
-                            + 4 * j[0] * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[3]
-                            - 4 * j[0] * q_norm[3])
-                            * (- q[3] * q[0] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)));
-
-
-        data(m, 2, counter) = (- 4 * i[0] * (q_norm[1] * q_norm[3] - q_norm[2] * q_norm[0]) * q_norm[0]
-                            - 2 * i[0] * q_norm[2]
-                            - 4 * j[0] * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[0]
-                            + 2 * j[0] * q_norm[1])
-                            * (1 / std::sqrt(s) - q[0] * q[0] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (- 4 * i[0] * (q_norm[1] * q_norm[3] - q_norm[2] * q_norm[0]) * q_norm[1]
-                            + 2 * i[0] * q_norm[3]
-                            - 4 * j[0] * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[1]
-                            + 2 * j[0] * q_norm[0])
-                            * (- q[1] * q[0] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (- 2 * i[0] * q_norm[0]
-                            - 4 * i[0] * (q_norm[1] * q_norm[3] - q_norm[0] * q_norm[2]) * q_norm[2]
-                            - 4 * j[0] * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[2]
-                            + 2 * j[0] * q_norm[3])
-                            * (- q[2] * q[0] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (2 * i[0] * q_norm[1]
-                            + 4 * i[0] * (q_norm[2] * q_norm[0] - q_norm[1] * q_norm[3]) * q_norm[3]
-                            - 4 * j[0] * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[3]
-                            + 2 * j[0] * q_norm[2])
-                            * (- q[3] * q[0] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)));
+        double inv_sqrt_s = inv_magnitude();
+
+        data(m, 0, counter) = (4 * i * (q_norm[2] * q_norm[2] + q_norm[3] * q_norm[3]) * q_norm[0]
+                            - 4 * j * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[0]
+                            - 2 * j * q_norm[3]) 
+                            * (inv_sqrt_s - q[0] * q[0] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (4 * i * (q_norm[2] * q_norm[2] + q_norm[3] * q_norm[3]) * q_norm[1]
+                            - 4 * j * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[1]
+                            + 2 * j * q_norm[2]) 
+                            * (- q[1] * q[0] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (- 4 * i * (q_norm[0] * q_norm[0] + q_norm[1] * q_norm[1]) * q_norm[2]
+                            - 4 * j * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[2]
+                            + 2 * j * q_norm[1]) 
+                            * (- q[2] * q[0] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (- 4 * i * (q_norm[1] * q_norm[1] + q_norm[0] * q_norm[0]) * q_norm[3]
+                            - 4 * j * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[3]
+                            - 2 * j * q_norm[0]) 
+                            * (- q[3] * q[0] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s);
+
+
+        data(m, 1, counter) = (- 4 * i * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[0]
+                            + 2 * i * q_norm[3]
+                            + 4 * j * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[0])
+                            * (inv_sqrt_s - q[0] * q[0] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (- 4 * i * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[1]
+                            + 2 * i * q_norm[2]
+                            + 4 * j * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[1]
+                            - 4 * j * q_norm[1])
+                            * (- q[1] * q[0] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (- 4 * i * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[2]
+                            + 2 * i * q_norm[1]
+                            + 4 * j * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[2])
+                            * (- q[2] * q[0] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (- 4 * i * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[3]
+                            + 2 * i * q_norm[0]
+                            + 4 * j * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[3]
+                            - 4 * j * q_norm[3])
+                            * (- q[3] * q[0] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s);
+
+
+        data(m, 2, counter) = (- 4 * i * (q_norm[1] * q_norm[3] - q_norm[2] * q_norm[0]) * q_norm[0]
+                            - 2 * i * q_norm[2]
+                            - 4 * j * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[0]
+                            + 2 * j * q_norm[1])
+                            * (inv_sqrt_s - q[0] * q[0] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (- 4 * i * (q_norm[1] * q_norm[3] - q_norm[2] * q_norm[0]) * q_norm[1]
+                            + 2 * i * q_norm[3]
+                            - 4 * j * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[1]
+                            + 2 * j * q_norm[0])
+                            * (- q[1] * q[0] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (- 2 * i * q_norm[0]
+                            - 4 * i * (q_norm[1] * q_norm[3] - q_norm[0] * q_norm[2]) * q_norm[2]
+                            - 4 * j * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[2]
+                            + 2 * j * q_norm[3])
+                            * (- q[2] * q[0] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (2 * i * q_norm[1]
+                            + 4 * i * (q_norm[2] * q_norm[0] - q_norm[1] * q_norm[3]) * q_norm[3]
+                            - 4 * j * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[3]
+                            + 2 * j * q_norm[2])
+                            * (- q[3] * q[0] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s);
         counter++;
 
-        data(m, 0, counter) = (4 * i[0] * (q_norm[2] * q_norm[2] + q_norm[3] * q_norm[3]) * q_norm[0] 
-                            - 4 * j[0] * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[0]
-                            - 2 * j[0] * q_norm[3]) 
-                            * (- q[0] * q[1] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (4 * i[0] * (q_norm[2] * q_norm[2] + q_norm[3] * q_norm[3]) * q_norm[1]
-                            - 4 * j[0] * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[1]
-                            + 2 * j[0] * q_norm[2]) 
-                            * (1 / std::sqrt(s) - q[1] * q[1] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (- 4 * i[0] * (q_norm[0] * q_norm[0] + q_norm[1] * q_norm[1]) * q_norm[2]
-                            - 4 * j[0] * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[2]
-                            + 2 * j[0] * q_norm[1]) 
-                            * (- q[2] * q[1] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (- 4 * i[0] * (q_norm[1] * q_norm[1] + q_norm[0] * q_norm[0]) * q_norm[3]
-                            - 4 * j[0] * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[3]
-                            - 2 * j[0] * q_norm[0])
-                            * (- q[3] * q[1] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)));
-
-
-        data(m, 1, counter) = (- 4 * i[0] * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[0]
-                            + 2 * i[0] * q_norm[3]
-                            + 4 * j[0] * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[0])
-                            * (- q[0] * q[1] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (- 4 * i[0] * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[1]
-                            + 2 * i[0] * q_norm[2]
-                            + 4 * j[0] * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[1]
-                            - 4 * j[0] * q_norm[1])
-                            * (1 / std::sqrt(s) - q[1] * q[1] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (- 4 * i[0] * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[2]
-                            + 2 * i[0] * q_norm[1]
-                            + 4 * j[0] * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[2])
-                            * (- q[2] * q[1] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (- 4 * i[0] * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[3]
-                            + 2 * i[0] * q_norm[0]
-                            + 4 * j[0] * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[3]
-                            - 4 * j[0] * q_norm[3])
-                            * (- q[3] * q[1] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)));
-
-
-        data(m, 2, counter) = (- 4 * i[0] * (q_norm[1] * q_norm[3] - q_norm[2] * q_norm[0]) * q_norm[0]
-                            - 2 * i[0] * q_norm[2]
-                            - 4 * j[0] * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[0]
-                            + 2 * j[0] * q_norm[1])
-                            * (- q[0] * q[1] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (- 4 * i[0] * (q_norm[1] * q_norm[3] - q_norm[2] * q_norm[0]) * q_norm[1]
-                            + 2 * i[0] * q_norm[3]
-                            - 4 * j[0] * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[1]
-                            + 2 * j[0] * q_norm[0])
-                            * (1 / std::sqrt(s) - q[1] * q[1] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (- 2 * i[0] * q_norm[0]
-                            - 4 * i[0] * (q_norm[1] * q_norm[3] - q_norm[0] * q_norm[2]) * q_norm[2]
-                            - 4 * j[0] * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[2]
-                            + 2 * j[0] * q_norm[3])
-                            * (- q[2] * q[1] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (2 * i[0] * q_norm[1]
-                            + 4 * i[0] * (q_norm[2] * q_norm[0] - q_norm[1] * q_norm[3]) * q_norm[3]
-                            - 4 * j[0] * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[3]
-                            + 2 * j[0] * q_norm[2])
-                            * (- q[3] * q[1] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)));
+        data(m, 0, counter) = (4 * i * (q_norm[2] * q_norm[2] + q_norm[3] * q_norm[3]) * q_norm[0] 
+                            - 4 * j * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[0]
+                            - 2 * j * q_norm[3]) 
+                            * (- q[0] * q[1] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (4 * i * (q_norm[2] * q_norm[2] + q_norm[3] * q_norm[3]) * q_norm[1]
+                            - 4 * j * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[1]
+                            + 2 * j * q_norm[2]) 
+                            * (inv_sqrt_s - q[1] * q[1] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (- 4 * i * (q_norm[0] * q_norm[0] + q_norm[1] * q_norm[1]) * q_norm[2]
+                            - 4 * j * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[2]
+                            + 2 * j * q_norm[1]) 
+                            * (- q[2] * q[1] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (- 4 * i * (q_norm[1] * q_norm[1] + q_norm[0] * q_norm[0]) * q_norm[3]
+                            - 4 * j * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[3]
+                            - 2 * j * q_norm[0])
+                            * (- q[3] * q[1] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s);
+
+
+        data(m, 1, counter) = (- 4 * i * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[0]
+                            + 2 * i * q_norm[3]
+                            + 4 * j * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[0])
+                            * (- q[0] * q[1] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (- 4 * i * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[1]
+                            + 2 * i * q_norm[2]
+                            + 4 * j * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[1]
+                            - 4 * j * q_norm[1])
+                            * (inv_sqrt_s - q[1] * q[1] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (- 4 * i * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[2]
+                            + 2 * i * q_norm[1]
+                            + 4 * j * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[2])
+                            * (- q[2] * q[1] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (- 4 * i * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[3]
+                            + 2 * i * q_norm[0]
+                            + 4 * j * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[3]
+                            - 4 * j * q_norm[3])
+                            * (- q[3] * q[1] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s);
+
+
+        data(m, 2, counter) = (- 4 * i * (q_norm[1] * q_norm[3] - q_norm[2] * q_norm[0]) * q_norm[0]
+                            - 2 * i * q_norm[2]
+                            - 4 * j * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[0]
+                            + 2 * j * q_norm[1])
+                            * (- q[0] * q[1] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (- 4 * i * (q_norm[1] * q_norm[3] - q_norm[2] * q_norm[0]) * q_norm[1]
+                            + 2 * i * q_norm[3]
+                            - 4 * j * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[1]
+                            + 2 * j * q_norm[0])
+                            * (inv_sqrt_s - q[1] * q[1] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (- 2 * i * q_norm[0]
+                            - 4 * i * (q_norm[1] * q_norm[3] - q_norm[0] * q_norm[2]) * q_norm[2]
+                            - 4 * j * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[2]
+                            + 2 * j * q_norm[3])
+                            * (- q[2] * q[1] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (2 * i * q_norm[1]
+                            + 4 * i * (q_norm[2] * q_norm[0] - q_norm[1] * q_norm[3]) * q_norm[3]
+                            - 4 * j * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[3]
+                            + 2 * j * q_norm[2])
+                            * (- q[3] * q[1] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s);
         counter++;
 
-        data(m, 0, counter) = (4 * i[0] * (q_norm[2] * q_norm[2] + q_norm[3] * q_norm[3]) * q_norm[0] 
-                            - 4 * j[0] * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[0]
-                            - 2 * j[0] * q_norm[3]) 
-                            * (- q[0] * q[2] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
+        data(m, 0, counter) = (4 * i * (q_norm[2] * q_norm[2] + q_norm[3] * q_norm[3]) * q_norm[0] 
+                            - 4 * j * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[0]
+                            - 2 * j * q_norm[3]) 
+                            * (- q[0] * q[2] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
                             + 
-                            (4 * i[0] * (q_norm[2] * q_norm[2] + q_norm[3] * q_norm[3]) * q_norm[1]
-                            - 4 * j[0] * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[1]
-                            + 2 * j[0] * q_norm[2]) 
-                            * (- q[1] * q[2] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (- 4 * i[0] * (q_norm[0] * q_norm[0] + q_norm[1] * q_norm[1]) * q_norm[2]
-                            - 4 * j[0] * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[2]
-                            + 2 * j[0] * q_norm[1]) 
-                            * (1 / std::sqrt(s) - q[2] * q[2] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (- 4 * i[0] * (q_norm[1] * q_norm[1] + q_norm[0] * q_norm[0]) * q_norm[3]
-                            - 4 * j[0] * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[3]
-                            - 2 * j[0] * q_norm[0]) 
-                            * (- q[3] * q[2] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)));
-
-
-        data(m, 1, counter) = (- 4 * i[0] * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[0]
-                            + 2 * i[0] * q_norm[3]
-                            + 4 * j[0] * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[0])
-                            * (- q[0] * q[2] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (- 4 * i[0] * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[1]
-                            + 2 * i[0] * q_norm[2]
-                            + 4 * j[0] * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[1]
-                            - 4 * j[0] * q_norm[1])
-                            * (- q[1] * q[2] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (- 4 * i[0] * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[2]
-                            + 2 * i[0] * q_norm[1]
-                            + 4 * j[0] * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[2])
-                            * (1 / std::sqrt(s) - q[2] * q[2] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (- 4 * i[0] * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[3]
-                            + 2 * i[0] * q_norm[0]
-                            + 4 * j[0] * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[3]
-                            - 4 * j[0] * q_norm[3])
-                            * (- q[3] * q[2] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)));
-
-
-        data(m, 2, counter) = (- 4 * i[0] * (q_norm[1] * q_norm[3] - q_norm[2] * q_norm[0]) * q_norm[0]
-                            - 2 * i[0] * q_norm[2]
-                            - 4 * j[0] * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[0]
-                            + 2 * j[0] * q_norm[1])
-                            * (- q[0] * q[2] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (- 4 * i[0] * (q_norm[1] * q_norm[3] - q_norm[2] * q_norm[0]) * q_norm[1]
-                            + 2 * i[0] * q_norm[3]
-                            - 4 * j[0] * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[1]
-                            + 2 * j[0] * q_norm[0])
-                            * (- q[1] * q[2] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (- 2 * i[0] * q_norm[0]
-                            - 4 * i[0] * (q_norm[1] * q_norm[3] - q_norm[0] * q_norm[2]) * q_norm[2]
-                            - 4 * j[0] * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[2]
-                            + 2 * j[0] * q_norm[3])
-                            * (1 / std::sqrt(s) - q[2] * q[2] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (2 * i[0] * q_norm[1]
-                            + 4 * i[0] * (q_norm[2] * q_norm[0] - q_norm[1] * q_norm[3]) * q_norm[3]
-                            - 4 * j[0] * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[3]
-                            + 2 * j[0] * q_norm[2])
-                            * (- q[3] * q[2] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)));
+                            (4 * i * (q_norm[2] * q_norm[2] + q_norm[3] * q_norm[3]) * q_norm[1]
+                            - 4 * j * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[1]
+                            + 2 * j * q_norm[2]) 
+                            * (- q[1] * q[2] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (- 4 * i * (q_norm[0] * q_norm[0] + q_norm[1] * q_norm[1]) * q_norm[2]
+                            - 4 * j * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[2]
+                            + 2 * j * q_norm[1]) 
+                            * (inv_sqrt_s - q[2] * q[2] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (- 4 * i * (q_norm[1] * q_norm[1] + q_norm[0] * q_norm[0]) * q_norm[3]
+                            - 4 * j * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[3]
+                            - 2 * j * q_norm[0]) 
+                            * (- q[3] * q[2] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s);
+
+
+        data(m, 1, counter) = (- 4 * i * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[0]
+                            + 2 * i * q_norm[3]
+                            + 4 * j * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[0])
+                            * (- q[0] * q[2] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (- 4 * i * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[1]
+                            + 2 * i * q_norm[2]
+                            + 4 * j * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[1]
+                            - 4 * j * q_norm[1])
+                            * (- q[1] * q[2] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (- 4 * i * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[2]
+                            + 2 * i * q_norm[1]
+                            + 4 * j * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[2])
+                            * (inv_sqrt_s - q[2] * q[2] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (- 4 * i * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[3]
+                            + 2 * i * q_norm[0]
+                            + 4 * j * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[3]
+                            - 4 * j * q_norm[3])
+                            * (- q[3] * q[2] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s);
+
+
+        data(m, 2, counter) = (- 4 * i * (q_norm[1] * q_norm[3] - q_norm[2] * q_norm[0]) * q_norm[0]
+                            - 2 * i * q_norm[2]
+                            - 4 * j * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[0]
+                            + 2 * j * q_norm[1])
+                            * (- q[0] * q[2] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (- 4 * i * (q_norm[1] * q_norm[3] - q_norm[2] * q_norm[0]) * q_norm[1]
+                            + 2 * i * q_norm[3]
+                            - 4 * j * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[1]
+                            + 2 * j * q_norm[0])
+                            * (- q[1] * q[2] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (- 2 * i * q_norm[0]
+                            - 4 * i * (q_norm[1] * q_norm[3] - q_norm[0] * q_norm[2]) * q_norm[2]
+                            - 4 * j * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[2]
+                            + 2 * j * q_norm[3])
+                            * (inv_sqrt_s - q[2] * q[2] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (2 * i * q_norm[1]
+                            + 4 * i * (q_norm[2] * q_norm[0] - q_norm[1] * q_norm[3]) * q_norm[3]
+                            - 4 * j * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[3]
+                            + 2 * j * q_norm[2])
+                            * (- q[3] * q[2] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s);
         counter++;
 
-        data(m, 0, counter) = (4 * i[0] * (q_norm[2] * q_norm[2] + q_norm[3] * q_norm[3]) * q_norm[0] 
-                            - 4 * j[0] * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[0]
-                            - 2 * j[0] * q_norm[3]) 
-                            * (- q[0] * q[3] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
+        data(m, 0, counter) = (4 * i * (q_norm[2] * q_norm[2] + q_norm[3] * q_norm[3]) * q_norm[0] 
+                            - 4 * j * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[0]
+                            - 2 * j * q_norm[3]) 
+                            * (- q[0] * q[3] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
                             +
-                            + (4 * i[0] * (q_norm[2] * q_norm[2] + q_norm[3] * q_norm[3]) * q_norm[1]
-                            - 4 * j[0] * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[1]
-                            + 2 * j[0] * q_norm[2]) 
-                            * (- q[1] * q[3] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
+                            + (4 * i * (q_norm[2] * q_norm[2] + q_norm[3] * q_norm[3]) * q_norm[1]
+                            - 4 * j * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[1]
+                            + 2 * j * q_norm[2]) 
+                            * (- q[1] * q[3] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
                             +
-                            (- 4 * i[0] * (q_norm[0] * q_norm[0] + q_norm[1] * q_norm[1]) * q_norm[2]
-                            - 4 * j[0] * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[2]
-                            + 2 * j[0] * q_norm[1]) 
-                            * (- q[2] * q[3] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
+                            (- 4 * i * (q_norm[0] * q_norm[0] + q_norm[1] * q_norm[1]) * q_norm[2]
+                            - 4 * j * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[2]
+                            + 2 * j * q_norm[1]) 
+                            * (- q[2] * q[3] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
                             +                            
-                            (- 4 * i[0] * (q_norm[1] * q_norm[1] + q_norm[0] * q_norm[0]) * q_norm[3]
-                            - 4 * j[0] * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[3]
-                            - 2 * j[0] * q_norm[0]) 
-                            * (1 / std::sqrt(s) - q[3] * q[3] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)));
-
-
-        data(m, 1, counter) = (- 4 * i[0] * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[0]
-                            + 2 * i[0] * q_norm[3]
-                            + 4 * j[0] * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[0])
-                            * (- q[0] * q[3] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (- 4 * i[0] * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[1]
-                            + 2 * i[0] * q_norm[2]
-                            + 4 * j[0] * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[1]
-                            - 4 * j[0] * q_norm[1])
-                            * (- q[1] * q[3] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (- 4 * i[0] * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[2]
-                            + 2 * i[0] * q_norm[1]
-                            + 4 * j[0] * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[2])
-                            * (- q[2] * q[3] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (- 4 * i[0] * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[3]
-                            + 2 * i[0] * q_norm[0]
-                            + 4 * j[0] * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[3]
-                            - 4 * j[0] * q_norm[3])
-                            * (1 / std::sqrt(s) - q[3] * q[3] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)));
-
-
-        data(m, 2, counter) = (- 4 * i[0] * (q_norm[1] * q_norm[3] - q_norm[2] * q_norm[0]) * q_norm[0]
-                            - 2 * i[0] * q_norm[2]
-                            - 4 * j[0] * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[0]
-                            + 2 * j[0] * q_norm[1])
-                            * (- q[0] * q[3] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (- 4 * i[0] * (q_norm[1] * q_norm[3] - q_norm[2] * q_norm[0]) * q_norm[1]
-                            + 2 * i[0] * q_norm[3]
-                            - 4 * j[0] * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[1]
-                            + 2 * j[0] * q_norm[0])
-                            * (- q[1] * q[3] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (- 2 * i[0] * q_norm[0]
-                            - 4 * i[0] * (q_norm[1] * q_norm[3] - q_norm[0] * q_norm[2]) * q_norm[2]
-                            - 4 * j[0] * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[2]
-                            + 2 * j[0] * q_norm[3])
-                            * (- q[2] * q[3] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (2 * i[0] * q_norm[1]
-                            + 4 * i[0] * (q_norm[2] * q_norm[0] - q_norm[1] * q_norm[3]) * q_norm[3]
-                            - 4 * j[0] * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[3]
-                            + 2 * j[0] * q_norm[2])
-                            * (1 / std::sqrt(s) - q[3] * q[3] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)));
+                            (- 4 * i * (q_norm[1] * q_norm[1] + q_norm[0] * q_norm[0]) * q_norm[3]
+                            - 4 * j * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[3]
+                            - 2 * j * q_norm[0]) 
+                            * (inv_sqrt_s - q[3] * q[3] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s);
+
+
+        data(m, 1, counter) = (- 4 * i * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[0]
+                            + 2 * i * q_norm[3]
+                            + 4 * j * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[0])
+                            * (- q[0] * q[3] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (- 4 * i * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[1]
+                            + 2 * i * q_norm[2]
+                            + 4 * j * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[1]
+                            - 4 * j * q_norm[1])
+                            * (- q[1] * q[3] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (- 4 * i * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[2]
+                            + 2 * i * q_norm[1]
+                            + 4 * j * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[2])
+                            * (- q[2] * q[3] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (- 4 * i * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[3]
+                            + 2 * i * q_norm[0]
+                            + 4 * j * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[3]
+                            - 4 * j * q_norm[3])
+                            * (inv_sqrt_s - q[3] * q[3] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s);
+
+
+        data(m, 2, counter) = (- 4 * i * (q_norm[1] * q_norm[3] - q_norm[2] * q_norm[0]) * q_norm[0]
+                            - 2 * i * q_norm[2]
+                            - 4 * j * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[0]
+                            + 2 * j * q_norm[1])
+                            * (- q[0] * q[3] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (- 4 * i * (q_norm[1] * q_norm[3] - q_norm[2] * q_norm[0]) * q_norm[1]
+                            + 2 * i * q_norm[3]
+                            - 4 * j * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[1]
+                            + 2 * j * q_norm[0])
+                            * (- q[1] * q[3] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (- 2 * i * q_norm[0]
+                            - 4 * i * (q_norm[1] * q_norm[3] - q_norm[0] * q_norm[2]) * q_norm[2]
+                            - 4 * j * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[2]
+                            + 2 * j * q_norm[3])
+                            * (- q[2] * q[3] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (2 * i * q_norm[1]
+                            + 4 * i * (q_norm[2] * q_norm[0] - q_norm[1] * q_norm[3]) * q_norm[3]
+                            - 4 * j * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[3]
+                            + 2 * j * q_norm[2])
+                            * (inv_sqrt_s - q[3] * q[3] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s);
         counter++;
 
         for (int i = 0; i < 2; ++i) {
@@ -898,344 +853,336 @@ template<class Array>
 void CurvePlanarFourier<Array>::dgammadashdash_by_dcoeff_impl(Array& data) {
     data *= 0;
 
-    /* Converts q dofs to unit quaternion */
-    Array q_norm = xt::zeros<double>({4});
-    double s = q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3];
-    if(s != 0) {
-        for (int i = 0; i < 4; ++i)
-            q_norm[i] = q[i] / std::sqrt(s); 
-    }
-    else {
-        q_norm[0] = 1;
-    }
+    Array q_norm = q * inv_magnitude();
 
     for (int m = 0; m < numquadpoints; ++m) {
         double phi = 2 * M_PI * quadpoints[m];
         int counter = 0;
-        Array i = xt::zeros<double>({1});
-        Array j = xt::zeros<double>({1});
-        Array k = xt::zeros<double>({1});
+        double i;
+        double j;
+        double k;
         for (int n = 0; n < order+1; ++n) {
-            i[0] = (+2*(n)*sin(n*phi)*sin(phi)-(pow(n, 2)+1)*cos(n*phi)*cos(phi));
-            j[0] = (-2*(n)*sin(n*phi)*cos(phi)-(pow(n, 2)+1)*cos(n*phi)*sin(phi));
-            k[0] = 0;
+            i = (+2*(n)*sin(n*phi)*sin(phi)-(n*n+1)*cos(n*phi)*cos(phi));
+            j = (-2*(n)*sin(n*phi)*cos(phi)-(n*n+1)*cos(n*phi)*sin(phi));
+            k = 0;
 
-            i[0] *= 2*M_PI*2*M_PI;
-            j[0] *= 2*M_PI*2*M_PI;
-            k[0] *= 2*M_PI*2*M_PI;
+            i *= 2*M_PI*2*M_PI;
+            j *= 2*M_PI*2*M_PI;
+            k *= 2*M_PI*2*M_PI;
 
-            data(m, 0, counter) = (i[0] - 2 * (q_norm[2] * q_norm[2] + q_norm[3] * q_norm[3]) * i[0] + 2 * (q_norm[1] * q_norm[2] - q_norm[3] * q_norm[0]) * j[0] + 2 * (q_norm[1] * q_norm[3] + q_norm[2] * q_norm[0]) * k[0]);
-            data(m, 1, counter) = (2 * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * i[0] + j[0] - 2 * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * j[0] + 2 * (q_norm[2] * q_norm[3] - q_norm[1] * q_norm[0]) * k[0]);
-            data(m, 2, counter) = (2 * (q_norm[1] * q_norm[3] - q_norm[2] * q_norm[0]) * i[0] + 2 * (q_norm[2] * q_norm[3] + q_norm[1] * q_norm[0]) * j[0] + k[0] - 2 * (q_norm[1] * q_norm[1] + q_norm[2] * q_norm[2]) * k[0]);
+            data(m, 0, counter) = (i - 2 * (q_norm[2] * q_norm[2] + q_norm[3] * q_norm[3]) * i + 2 * (q_norm[1] * q_norm[2] - q_norm[3] * q_norm[0]) * j + 2 * (q_norm[1] * q_norm[3] + q_norm[2] * q_norm[0]) * k);
+            data(m, 1, counter) = (2 * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * i + j - 2 * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * j + 2 * (q_norm[2] * q_norm[3] - q_norm[1] * q_norm[0]) * k);
+            data(m, 2, counter) = (2 * (q_norm[1] * q_norm[3] - q_norm[2] * q_norm[0]) * i + 2 * (q_norm[2] * q_norm[3] + q_norm[1] * q_norm[0]) * j + k - 2 * (q_norm[1] * q_norm[1] + q_norm[2] * q_norm[2]) * k);
             
             counter++;
         }
         
         for (int n = 1; n < order+1; ++n) {
-            i[0] = (-(pow(n,2)+1)*sin(n*phi)*cos(phi) - 2*(n)*cos(n*phi)*sin(phi));
-            j[0] = (-(pow(n,2)+1)*sin(n*phi)*sin(phi) + 2*(n)*cos(n*phi)*cos(phi));
-            k[0] = 0;
+            i = (-(n*n+1)*sin(n*phi)*cos(phi) - 2*(n)*cos(n*phi)*sin(phi));
+            j = (-(n*n+1)*sin(n*phi)*sin(phi) + 2*(n)*cos(n*phi)*cos(phi));
+            k = 0;
 
-            i[0] *= 2*M_PI*2*M_PI;
-            j[0] *= 2*M_PI*2*M_PI;
-            k[0] *= 2*M_PI*2*M_PI;
+            i *= 2*M_PI*2*M_PI;
+            j *= 2*M_PI*2*M_PI;
+            k *= 2*M_PI*2*M_PI;
 
-            data(m, 0, counter) = (i[0] - 2 * (q_norm[2] * q_norm[2] + q_norm[3] * q_norm[3]) * i[0] + 2 * (q_norm[1] * q_norm[2] - q_norm[3] * q_norm[0]) * j[0] + 2 * (q_norm[1] * q_norm[3] + q_norm[2] * q_norm[0]) * k[0]);
-            data(m, 1, counter) = (2 * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * i[0] + j[0] - 2 * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * j[0] + 2 * (q_norm[2] * q_norm[3] - q_norm[1] * q_norm[0]) * k[0]);
-            data(m, 2, counter) = (2 * (q_norm[1] * q_norm[3] - q_norm[2] * q_norm[0]) * i[0] + 2 * (q_norm[2] * q_norm[3] + q_norm[1] * q_norm[0]) * j[0] + k[0] - 2 * (q_norm[1] * q_norm[1] + q_norm[2] * q_norm[2]) * k[0]);
+            data(m, 0, counter) = (i - 2 * (q_norm[2] * q_norm[2] + q_norm[3] * q_norm[3]) * i + 2 * (q_norm[1] * q_norm[2] - q_norm[3] * q_norm[0]) * j + 2 * (q_norm[1] * q_norm[3] + q_norm[2] * q_norm[0]) * k);
+            data(m, 1, counter) = (2 * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * i + j - 2 * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * j + 2 * (q_norm[2] * q_norm[3] - q_norm[1] * q_norm[0]) * k);
+            data(m, 2, counter) = (2 * (q_norm[1] * q_norm[3] - q_norm[2] * q_norm[0]) * i + 2 * (q_norm[2] * q_norm[3] + q_norm[1] * q_norm[0]) * j + k - 2 * (q_norm[1] * q_norm[1] + q_norm[2] * q_norm[2]) * k);
             
             counter++;
         }
         
-        i[0] = 0;
-        j[0] = 0;
-        k[0] = 0;
+        i = 0;
+        j = 0;
+        k = 0;
         for (int n = 0; n < order+1; ++n) {
-            i[0] += rc[n] * (+2*(n)*sin(n*phi)*sin(phi)-(pow(n, 2)+1)*cos(n*phi)*cos(phi));
-            j[0] += rc[n] * (-2*(n)*sin(n*phi)*cos(phi)-(pow(n, 2)+1)*cos(n*phi)*sin(phi));
+            i += rc[n] * (+2*(n)*sin(n*phi)*sin(phi)-(n*n+1)*cos(n*phi)*cos(phi));
+            j += rc[n] * (-2*(n)*sin(n*phi)*cos(phi)-(n*n+1)*cos(n*phi)*sin(phi));
         }
         for (int n = 1; n < order+1; ++n) {
-            i[0] += rs[n-1] * (-(pow(n,2)+1)*sin(n*phi)*cos(phi) - 2*(n)*cos(n*phi)*sin(phi));
-            j[0] += rs[n-1] * (-(pow(n,2)+1)*sin(n*phi)*sin(phi) + 2*(n)*cos(n*phi)*cos(phi));
+            i += rs[n-1] * (-(n*n+1)*sin(n*phi)*cos(phi) - 2*(n)*cos(n*phi)*sin(phi));
+            j += rs[n-1] * (-(n*n+1)*sin(n*phi)*sin(phi) + 2*(n)*cos(n*phi)*cos(phi));
         }
-        i[0] *= 2*M_PI*2*M_PI;
-        j[0] *= 2*M_PI*2*M_PI;
-        k[0] *= 2*M_PI*2*M_PI;
+        i *= 2*M_PI*2*M_PI;
+        j *= 2*M_PI*2*M_PI;
+        k *= 2*M_PI*2*M_PI;
+        double inv_sqrt_s = inv_magnitude();
         
-        data(m, 0, counter) = (4 * i[0] * (q_norm[2] * q_norm[2] + q_norm[3] * q_norm[3]) * q_norm[0]
-                            - 4 * j[0] * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[0]
-                            - 2 * j[0] * q_norm[3]) 
-                            * (1 / std::sqrt(s) - q[0] * q[0] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (4 * i[0] * (q_norm[2] * q_norm[2] + q_norm[3] * q_norm[3]) * q_norm[1]
-                            - 4 * j[0] * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[1]
-                            + 2 * j[0] * q_norm[2]) 
-                            * (- q[1] * q[0] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (- 4 * i[0] * (q_norm[0] * q_norm[0] + q_norm[1] * q_norm[1]) * q_norm[2]
-                            - 4 * j[0] * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[2]
-                            + 2 * j[0] * q_norm[1]) 
-                            * (- q[2] * q[0] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (- 4 * i[0] * (q_norm[1] * q_norm[1] + q_norm[0] * q_norm[0]) * q_norm[3]
-                            - 4 * j[0] * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[3]
-                            - 2 * j[0] * q_norm[0]) 
-                            * (- q[3] * q[0] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)));
-
-
-        data(m, 1, counter) = (- 4 * i[0] * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[0]
-                            + 2 * i[0] * q_norm[3]
-                            + 4 * j[0] * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[0])
-                            * (1 / std::sqrt(s) - q[0] * q[0] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (- 4 * i[0] * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[1]
-                            + 2 * i[0] * q_norm[2]
-                            + 4 * j[0] * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[1]
-                            - 4 * j[0] * q_norm[1])
-                            * (- q[1] * q[0] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (- 4 * i[0] * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[2]
-                            + 2 * i[0] * q_norm[1]
-                            + 4 * j[0] * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[2])
-                            * (- q[2] * q[0] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (- 4 * i[0] * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[3]
-                            + 2 * i[0] * q_norm[0]
-                            + 4 * j[0] * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[3]
-                            - 4 * j[0] * q_norm[3])
-                            * (- q[3] * q[0] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)));
-
-
-        data(m, 2, counter) = (- 4 * i[0] * (q_norm[1] * q_norm[3] - q_norm[2] * q_norm[0]) * q_norm[0]
-                            - 2 * i[0] * q_norm[2]
-                            - 4 * j[0] * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[0]
-                            + 2 * j[0] * q_norm[1])
-                            * (1 / std::sqrt(s) - q[0] * q[0] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (- 4 * i[0] * (q_norm[1] * q_norm[3] - q_norm[2] * q_norm[0]) * q_norm[1]
-                            + 2 * i[0] * q_norm[3]
-                            - 4 * j[0] * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[1]
-                            + 2 * j[0] * q_norm[0])
-                            * (- q[1] * q[0] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (- 2 * i[0] * q_norm[0]
-                            - 4 * i[0] * (q_norm[1] * q_norm[3] - q_norm[0] * q_norm[2]) * q_norm[2]
-                            - 4 * j[0] * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[2]
-                            + 2 * j[0] * q_norm[3])
-                            * (- q[2] * q[0] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (2 * i[0] * q_norm[1]
-                            + 4 * i[0] * (q_norm[2] * q_norm[0] - q_norm[1] * q_norm[3]) * q_norm[3]
-                            - 4 * j[0] * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[3]
-                            + 2 * j[0] * q_norm[2])
-                            * (- q[3] * q[0] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)));
+        data(m, 0, counter) = (4 * i * (q_norm[2] * q_norm[2] + q_norm[3] * q_norm[3]) * q_norm[0]
+                            - 4 * j * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[0]
+                            - 2 * j * q_norm[3]) 
+                            * (inv_sqrt_s - q[0] * q[0] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (4 * i * (q_norm[2] * q_norm[2] + q_norm[3] * q_norm[3]) * q_norm[1]
+                            - 4 * j * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[1]
+                            + 2 * j * q_norm[2]) 
+                            * (- q[1] * q[0] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (- 4 * i * (q_norm[0] * q_norm[0] + q_norm[1] * q_norm[1]) * q_norm[2]
+                            - 4 * j * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[2]
+                            + 2 * j * q_norm[1]) 
+                            * (- q[2] * q[0] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (- 4 * i * (q_norm[1] * q_norm[1] + q_norm[0] * q_norm[0]) * q_norm[3]
+                            - 4 * j * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[3]
+                            - 2 * j * q_norm[0]) 
+                            * (- q[3] * q[0] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s);
+
+
+        data(m, 1, counter) = (- 4 * i * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[0]
+                            + 2 * i * q_norm[3]
+                            + 4 * j * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[0])
+                            * (inv_sqrt_s - q[0] * q[0] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (- 4 * i * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[1]
+                            + 2 * i * q_norm[2]
+                            + 4 * j * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[1]
+                            - 4 * j * q_norm[1])
+                            * (- q[1] * q[0] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (- 4 * i * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[2]
+                            + 2 * i * q_norm[1]
+                            + 4 * j * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[2])
+                            * (- q[2] * q[0] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (- 4 * i * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[3]
+                            + 2 * i * q_norm[0]
+                            + 4 * j * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[3]
+                            - 4 * j * q_norm[3])
+                            * (- q[3] * q[0] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s);
+
+
+        data(m, 2, counter) = (- 4 * i * (q_norm[1] * q_norm[3] - q_norm[2] * q_norm[0]) * q_norm[0]
+                            - 2 * i * q_norm[2]
+                            - 4 * j * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[0]
+                            + 2 * j * q_norm[1])
+                            * (inv_sqrt_s - q[0] * q[0] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (- 4 * i * (q_norm[1] * q_norm[3] - q_norm[2] * q_norm[0]) * q_norm[1]
+                            + 2 * i * q_norm[3]
+                            - 4 * j * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[1]
+                            + 2 * j * q_norm[0])
+                            * (- q[1] * q[0] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (- 2 * i * q_norm[0]
+                            - 4 * i * (q_norm[1] * q_norm[3] - q_norm[0] * q_norm[2]) * q_norm[2]
+                            - 4 * j * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[2]
+                            + 2 * j * q_norm[3])
+                            * (- q[2] * q[0] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (2 * i * q_norm[1]
+                            + 4 * i * (q_norm[2] * q_norm[0] - q_norm[1] * q_norm[3]) * q_norm[3]
+                            - 4 * j * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[3]
+                            + 2 * j * q_norm[2])
+                            * (- q[3] * q[0] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s);
         counter++;
 
-        data(m, 0, counter) = (4 * i[0] * (q_norm[2] * q_norm[2] + q_norm[3] * q_norm[3]) * q_norm[0] 
-                            - 4 * j[0] * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[0]
-                            - 2 * j[0] * q_norm[3]) 
-                            * (- q[0] * q[1] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (4 * i[0] * (q_norm[2] * q_norm[2] + q_norm[3] * q_norm[3]) * q_norm[1]
-                            - 4 * j[0] * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[1]
-                            + 2 * j[0] * q_norm[2]) 
-                            * (1 / std::sqrt(s) - q[1] * q[1] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (- 4 * i[0] * (q_norm[0] * q_norm[0] + q_norm[1] * q_norm[1]) * q_norm[2]
-                            - 4 * j[0] * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[2]
-                            + 2 * j[0] * q_norm[1]) 
-                            * (- q[2] * q[1] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (- 4 * i[0] * (q_norm[1] * q_norm[1] + q_norm[0] * q_norm[0]) * q_norm[3]
-                            - 4 * j[0] * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[3]
-                            - 2 * j[0] * q_norm[0])
-                            * (- q[3] * q[1] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)));
-
-
-        data(m, 1, counter) = (- 4 * i[0] * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[0]
-                            + 2 * i[0] * q_norm[3]
-                            + 4 * j[0] * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[0])
-                            * (- q[0] * q[1] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (- 4 * i[0] * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[1]
-                            + 2 * i[0] * q_norm[2]
-                            + 4 * j[0] * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[1]
-                            - 4 * j[0] * q_norm[1])
-                            * (1 / std::sqrt(s) - q[1] * q[1] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (- 4 * i[0] * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[2]
-                            + 2 * i[0] * q_norm[1]
-                            + 4 * j[0] * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[2])
-                            * (- q[2] * q[1] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (- 4 * i[0] * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[3]
-                            + 2 * i[0] * q_norm[0]
-                            + 4 * j[0] * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[3]
-                            - 4 * j[0] * q_norm[3])
-                            * (- q[3] * q[1] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)));
-
-
-        data(m, 2, counter) = (- 4 * i[0] * (q_norm[1] * q_norm[3] - q_norm[2] * q_norm[0]) * q_norm[0]
-                            - 2 * i[0] * q_norm[2]
-                            - 4 * j[0] * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[0]
-                            + 2 * j[0] * q_norm[1])
-                            * (- q[0] * q[1] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (- 4 * i[0] * (q_norm[1] * q_norm[3] - q_norm[2] * q_norm[0]) * q_norm[1]
-                            + 2 * i[0] * q_norm[3]
-                            - 4 * j[0] * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[1]
-                            + 2 * j[0] * q_norm[0])
-                            * (1 / std::sqrt(s) - q[1] * q[1] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (- 2 * i[0] * q_norm[0]
-                            - 4 * i[0] * (q_norm[1] * q_norm[3] - q_norm[0] * q_norm[2]) * q_norm[2]
-                            - 4 * j[0] * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[2]
-                            + 2 * j[0] * q_norm[3])
-                            * (- q[2] * q[1] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (2 * i[0] * q_norm[1]
-                            + 4 * i[0] * (q_norm[2] * q_norm[0] - q_norm[1] * q_norm[3]) * q_norm[3]
-                            - 4 * j[0] * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[3]
-                            + 2 * j[0] * q_norm[2])
-                            * (- q[3] * q[1] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)));
+        data(m, 0, counter) = (4 * i * (q_norm[2] * q_norm[2] + q_norm[3] * q_norm[3]) * q_norm[0] 
+                            - 4 * j * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[0]
+                            - 2 * j * q_norm[3]) 
+                            * (- q[0] * q[1] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (4 * i * (q_norm[2] * q_norm[2] + q_norm[3] * q_norm[3]) * q_norm[1]
+                            - 4 * j * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[1]
+                            + 2 * j * q_norm[2]) 
+                            * (inv_sqrt_s - q[1] * q[1] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (- 4 * i * (q_norm[0] * q_norm[0] + q_norm[1] * q_norm[1]) * q_norm[2]
+                            - 4 * j * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[2]
+                            + 2 * j * q_norm[1]) 
+                            * (- q[2] * q[1] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (- 4 * i * (q_norm[1] * q_norm[1] + q_norm[0] * q_norm[0]) * q_norm[3]
+                            - 4 * j * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[3]
+                            - 2 * j * q_norm[0])
+                            * (- q[3] * q[1] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s);
+
+
+        data(m, 1, counter) = (- 4 * i * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[0]
+                            + 2 * i * q_norm[3]
+                            + 4 * j * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[0])
+                            * (- q[0] * q[1] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (- 4 * i * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[1]
+                            + 2 * i * q_norm[2]
+                            + 4 * j * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[1]
+                            - 4 * j * q_norm[1])
+                            * (inv_sqrt_s - q[1] * q[1] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (- 4 * i * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[2]
+                            + 2 * i * q_norm[1]
+                            + 4 * j * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[2])
+                            * (- q[2] * q[1] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (- 4 * i * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[3]
+                            + 2 * i * q_norm[0]
+                            + 4 * j * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[3]
+                            - 4 * j * q_norm[3])
+                            * (- q[3] * q[1] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s);
+
+
+        data(m, 2, counter) = (- 4 * i * (q_norm[1] * q_norm[3] - q_norm[2] * q_norm[0]) * q_norm[0]
+                            - 2 * i * q_norm[2]
+                            - 4 * j * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[0]
+                            + 2 * j * q_norm[1])
+                            * (- q[0] * q[1] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (- 4 * i * (q_norm[1] * q_norm[3] - q_norm[2] * q_norm[0]) * q_norm[1]
+                            + 2 * i * q_norm[3]
+                            - 4 * j * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[1]
+                            + 2 * j * q_norm[0])
+                            * (inv_sqrt_s - q[1] * q[1] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (- 2 * i * q_norm[0]
+                            - 4 * i * (q_norm[1] * q_norm[3] - q_norm[0] * q_norm[2]) * q_norm[2]
+                            - 4 * j * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[2]
+                            + 2 * j * q_norm[3])
+                            * (- q[2] * q[1] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (2 * i * q_norm[1]
+                            + 4 * i * (q_norm[2] * q_norm[0] - q_norm[1] * q_norm[3]) * q_norm[3]
+                            - 4 * j * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[3]
+                            + 2 * j * q_norm[2])
+                            * (- q[3] * q[1] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s);
         counter++;
 
-        data(m, 0, counter) = (4 * i[0] * (q_norm[2] * q_norm[2] + q_norm[3] * q_norm[3]) * q_norm[0] 
-                            - 4 * j[0] * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[0]
-                            - 2 * j[0] * q_norm[3]) 
-                            * (- q[0] * q[2] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
+        data(m, 0, counter) = (4 * i * (q_norm[2] * q_norm[2] + q_norm[3] * q_norm[3]) * q_norm[0] 
+                            - 4 * j * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[0]
+                            - 2 * j * q_norm[3]) 
+                            * (- q[0] * q[2] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
                             + 
-                            (4 * i[0] * (q_norm[2] * q_norm[2] + q_norm[3] * q_norm[3]) * q_norm[1]
-                            - 4 * j[0] * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[1]
-                            + 2 * j[0] * q_norm[2]) 
-                            * (- q[1] * q[2] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (- 4 * i[0] * (q_norm[0] * q_norm[0] + q_norm[1] * q_norm[1]) * q_norm[2]
-                            - 4 * j[0] * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[2]
-                            + 2 * j[0] * q_norm[1]) 
-                            * (1 / std::sqrt(s) - q[2] * q[2] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (- 4 * i[0] * (q_norm[1] * q_norm[1] + q_norm[0] * q_norm[0]) * q_norm[3]
-                            - 4 * j[0] * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[3]
-                            - 2 * j[0] * q_norm[0]) 
-                            * (- q[3] * q[2] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)));
-
-
-        data(m, 1, counter) = (- 4 * i[0] * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[0]
-                            + 2 * i[0] * q_norm[3]
-                            + 4 * j[0] * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[0])
-                            * (- q[0] * q[2] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (- 4 * i[0] * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[1]
-                            + 2 * i[0] * q_norm[2]
-                            + 4 * j[0] * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[1]
-                            - 4 * j[0] * q_norm[1])
-                            * (- q[1] * q[2] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (- 4 * i[0] * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[2]
-                            + 2 * i[0] * q_norm[1]
-                            + 4 * j[0] * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[2])
-                            * (1 / std::sqrt(s) - q[2] * q[2] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (- 4 * i[0] * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[3]
-                            + 2 * i[0] * q_norm[0]
-                            + 4 * j[0] * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[3]
-                            - 4 * j[0] * q_norm[3])
-                            * (- q[3] * q[2] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)));
-
-
-        data(m, 2, counter) = (- 4 * i[0] * (q_norm[1] * q_norm[3] - q_norm[2] * q_norm[0]) * q_norm[0]
-                            - 2 * i[0] * q_norm[2]
-                            - 4 * j[0] * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[0]
-                            + 2 * j[0] * q_norm[1])
-                            * (- q[0] * q[2] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (- 4 * i[0] * (q_norm[1] * q_norm[3] - q_norm[2] * q_norm[0]) * q_norm[1]
-                            + 2 * i[0] * q_norm[3]
-                            - 4 * j[0] * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[1]
-                            + 2 * j[0] * q_norm[0])
-                            * (- q[1] * q[2] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (- 2 * i[0] * q_norm[0]
-                            - 4 * i[0] * (q_norm[1] * q_norm[3] - q_norm[0] * q_norm[2]) * q_norm[2]
-                            - 4 * j[0] * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[2]
-                            + 2 * j[0] * q_norm[3])
-                            * (1 / std::sqrt(s) - q[2] * q[2] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (2 * i[0] * q_norm[1]
-                            + 4 * i[0] * (q_norm[2] * q_norm[0] - q_norm[1] * q_norm[3]) * q_norm[3]
-                            - 4 * j[0] * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[3]
-                            + 2 * j[0] * q_norm[2])
-                            * (- q[3] * q[2] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)));
+                            (4 * i * (q_norm[2] * q_norm[2] + q_norm[3] * q_norm[3]) * q_norm[1]
+                            - 4 * j * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[1]
+                            + 2 * j * q_norm[2]) 
+                            * (- q[1] * q[2] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (- 4 * i * (q_norm[0] * q_norm[0] + q_norm[1] * q_norm[1]) * q_norm[2]
+                            - 4 * j * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[2]
+                            + 2 * j * q_norm[1]) 
+                            * (inv_sqrt_s - q[2] * q[2] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (- 4 * i * (q_norm[1] * q_norm[1] + q_norm[0] * q_norm[0]) * q_norm[3]
+                            - 4 * j * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[3]
+                            - 2 * j * q_norm[0]) 
+                            * (- q[3] * q[2] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s);
+
+
+        data(m, 1, counter) = (- 4 * i * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[0]
+                            + 2 * i * q_norm[3]
+                            + 4 * j * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[0])
+                            * (- q[0] * q[2] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (- 4 * i * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[1]
+                            + 2 * i * q_norm[2]
+                            + 4 * j * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[1]
+                            - 4 * j * q_norm[1])
+                            * (- q[1] * q[2] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (- 4 * i * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[2]
+                            + 2 * i * q_norm[1]
+                            + 4 * j * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[2])
+                            * (inv_sqrt_s - q[2] * q[2] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (- 4 * i * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[3]
+                            + 2 * i * q_norm[0]
+                            + 4 * j * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[3]
+                            - 4 * j * q_norm[3])
+                            * (- q[3] * q[2] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s);
+
+
+        data(m, 2, counter) = (- 4 * i * (q_norm[1] * q_norm[3] - q_norm[2] * q_norm[0]) * q_norm[0]
+                            - 2 * i * q_norm[2]
+                            - 4 * j * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[0]
+                            + 2 * j * q_norm[1])
+                            * (- q[0] * q[2] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (- 4 * i * (q_norm[1] * q_norm[3] - q_norm[2] * q_norm[0]) * q_norm[1]
+                            + 2 * i * q_norm[3]
+                            - 4 * j * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[1]
+                            + 2 * j * q_norm[0])
+                            * (- q[1] * q[2] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (- 2 * i * q_norm[0]
+                            - 4 * i * (q_norm[1] * q_norm[3] - q_norm[0] * q_norm[2]) * q_norm[2]
+                            - 4 * j * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[2]
+                            + 2 * j * q_norm[3])
+                            * (inv_sqrt_s - q[2] * q[2] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (2 * i * q_norm[1]
+                            + 4 * i * (q_norm[2] * q_norm[0] - q_norm[1] * q_norm[3]) * q_norm[3]
+                            - 4 * j * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[3]
+                            + 2 * j * q_norm[2])
+                            * (- q[3] * q[2] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s);
         counter++;
 
-        data(m, 0, counter) = (4 * i[0] * (q_norm[2] * q_norm[2] + q_norm[3] * q_norm[3]) * q_norm[0] 
-                            - 4 * j[0] * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[0]
-                            - 2 * j[0] * q_norm[3]) 
-                            * (- q[0] * q[3] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
+        data(m, 0, counter) = (4 * i * (q_norm[2] * q_norm[2] + q_norm[3] * q_norm[3]) * q_norm[0] 
+                            - 4 * j * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[0]
+                            - 2 * j * q_norm[3]) 
+                            * (- q[0] * q[3] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
                             +
-                            + (4 * i[0] * (q_norm[2] * q_norm[2] + q_norm[3] * q_norm[3]) * q_norm[1]
-                            - 4 * j[0] * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[1]
-                            + 2 * j[0] * q_norm[2]) 
-                            * (- q[1] * q[3] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
+                            + (4 * i * (q_norm[2] * q_norm[2] + q_norm[3] * q_norm[3]) * q_norm[1]
+                            - 4 * j * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[1]
+                            + 2 * j * q_norm[2]) 
+                            * (- q[1] * q[3] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
                             +
-                            (- 4 * i[0] * (q_norm[0] * q_norm[0] + q_norm[1] * q_norm[1]) * q_norm[2]
-                            - 4 * j[0] * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[2]
-                            + 2 * j[0] * q_norm[1]) 
-                            * (- q[2] * q[3] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
+                            (- 4 * i * (q_norm[0] * q_norm[0] + q_norm[1] * q_norm[1]) * q_norm[2]
+                            - 4 * j * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[2]
+                            + 2 * j * q_norm[1]) 
+                            * (- q[2] * q[3] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
                             +                            
-                            (- 4 * i[0] * (q_norm[1] * q_norm[1] + q_norm[0] * q_norm[0]) * q_norm[3]
-                            - 4 * j[0] * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[3]
-                            - 2 * j[0] * q_norm[0]) 
-                            * (1 / std::sqrt(s) - q[3] * q[3] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)));
-
-
-        data(m, 1, counter) = (- 4 * i[0] * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[0]
-                            + 2 * i[0] * q_norm[3]
-                            + 4 * j[0] * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[0])
-                            * (- q[0] * q[3] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (- 4 * i[0] * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[1]
-                            + 2 * i[0] * q_norm[2]
-                            + 4 * j[0] * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[1]
-                            - 4 * j[0] * q_norm[1])
-                            * (- q[1] * q[3] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (- 4 * i[0] * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[2]
-                            + 2 * i[0] * q_norm[1]
-                            + 4 * j[0] * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[2])
-                            * (- q[2] * q[3] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (- 4 * i[0] * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[3]
-                            + 2 * i[0] * q_norm[0]
-                            + 4 * j[0] * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[3]
-                            - 4 * j[0] * q_norm[3])
-                            * (1 / std::sqrt(s) - q[3] * q[3] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)));
-
-
-        data(m, 2, counter) = (- 4 * i[0] * (q_norm[1] * q_norm[3] - q_norm[2] * q_norm[0]) * q_norm[0]
-                            - 2 * i[0] * q_norm[2]
-                            - 4 * j[0] * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[0]
-                            + 2 * j[0] * q_norm[1])
-                            * (- q[0] * q[3] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (- 4 * i[0] * (q_norm[1] * q_norm[3] - q_norm[2] * q_norm[0]) * q_norm[1]
-                            + 2 * i[0] * q_norm[3]
-                            - 4 * j[0] * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[1]
-                            + 2 * j[0] * q_norm[0])
-                            * (- q[1] * q[3] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (- 2 * i[0] * q_norm[0]
-                            - 4 * i[0] * (q_norm[1] * q_norm[3] - q_norm[0] * q_norm[2]) * q_norm[2]
-                            - 4 * j[0] * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[2]
-                            + 2 * j[0] * q_norm[3])
-                            * (- q[2] * q[3] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (2 * i[0] * q_norm[1]
-                            + 4 * i[0] * (q_norm[2] * q_norm[0] - q_norm[1] * q_norm[3]) * q_norm[3]
-                            - 4 * j[0] * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[3]
-                            + 2 * j[0] * q_norm[2])
-                            * (1 / std::sqrt(s) - q[3] * q[3] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)));
+                            (- 4 * i * (q_norm[1] * q_norm[1] + q_norm[0] * q_norm[0]) * q_norm[3]
+                            - 4 * j * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[3]
+                            - 2 * j * q_norm[0]) 
+                            * (inv_sqrt_s - q[3] * q[3] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s);
+
+
+        data(m, 1, counter) = (- 4 * i * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[0]
+                            + 2 * i * q_norm[3]
+                            + 4 * j * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[0])
+                            * (- q[0] * q[3] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (- 4 * i * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[1]
+                            + 2 * i * q_norm[2]
+                            + 4 * j * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[1]
+                            - 4 * j * q_norm[1])
+                            * (- q[1] * q[3] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (- 4 * i * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[2]
+                            + 2 * i * q_norm[1]
+                            + 4 * j * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[2])
+                            * (- q[2] * q[3] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (- 4 * i * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[3]
+                            + 2 * i * q_norm[0]
+                            + 4 * j * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[3]
+                            - 4 * j * q_norm[3])
+                            * (inv_sqrt_s - q[3] * q[3] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s);
+
+
+        data(m, 2, counter) = (- 4 * i * (q_norm[1] * q_norm[3] - q_norm[2] * q_norm[0]) * q_norm[0]
+                            - 2 * i * q_norm[2]
+                            - 4 * j * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[0]
+                            + 2 * j * q_norm[1])
+                            * (- q[0] * q[3] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (- 4 * i * (q_norm[1] * q_norm[3] - q_norm[2] * q_norm[0]) * q_norm[1]
+                            + 2 * i * q_norm[3]
+                            - 4 * j * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[1]
+                            + 2 * j * q_norm[0])
+                            * (- q[1] * q[3] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (- 2 * i * q_norm[0]
+                            - 4 * i * (q_norm[1] * q_norm[3] - q_norm[0] * q_norm[2]) * q_norm[2]
+                            - 4 * j * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[2]
+                            + 2 * j * q_norm[3])
+                            * (- q[2] * q[3] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (2 * i * q_norm[1]
+                            + 4 * i * (q_norm[2] * q_norm[0] - q_norm[1] * q_norm[3]) * q_norm[3]
+                            - 4 * j * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[3]
+                            + 2 * j * q_norm[2])
+                            * (inv_sqrt_s - q[3] * q[3] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s);
         counter++;
 
         for (int i = 0; i < 3; ++i) {
@@ -1252,368 +1199,360 @@ template<class Array>
 void CurvePlanarFourier<Array>::dgammadashdashdash_by_dcoeff_impl(Array& data) {
     data *= 0;
 
-    /* Converts q dofs to unit quaternion */
-    Array q_norm = xt::zeros<double>({4});
-    double s = q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3];
-    if(s != 0) {
-        for (int i = 0; i < 4; ++i)
-            q_norm[i] = q[i] / std::sqrt(s); 
-    }
-    else {
-        q_norm[0] = 1;
-    }
+    Array q_norm = q * inv_magnitude();
 
     for (int m = 0; m < numquadpoints; ++m) {
         double phi = 2 * M_PI * quadpoints[m];
         int counter = 0;
-        Array i = xt::zeros<double>({1});
-        Array j = xt::zeros<double>({1});
-        Array k = xt::zeros<double>({1});
+        double i;
+        double j;
+        double k;
         for (int n = 0; n < order+1; ++n) {
-            i[0] = (
-                    +(3*pow(n, 2) + 1)*cos(n*phi)*sin(phi)
-                    +(pow(n, 2) + 3)*(n)*sin(n*phi)*cos(phi)
+            i = (
+                    +(3*n*n + 1)*cos(n*phi)*sin(phi)
+                    +(n*n + 3)*(n)*sin(n*phi)*cos(phi)
                     );
-            j[0] = (
-                    +(pow(n, 2) + 3)*(n)*sin(n*phi)*sin(phi)
-                    -(3*pow(n, 2) + 1)*cos(n*phi)*cos(phi)
+            j = (
+                    +(n*n + 3)*(n)*sin(n*phi)*sin(phi)
+                    -(3*n*n + 1)*cos(n*phi)*cos(phi)
                     );
-            k[0] = 0;
+            k = 0;
 
-            i[0] *= 2*M_PI*2*M_PI*2*M_PI;
-            j[0] *= 2*M_PI*2*M_PI*2*M_PI;
-            k[0] *= 2*M_PI*2*M_PI*2*M_PI;
+            i *= 2*M_PI*2*M_PI*2*M_PI;
+            j *= 2*M_PI*2*M_PI*2*M_PI;
+            k *= 2*M_PI*2*M_PI*2*M_PI;
 
-            data(m, 0, counter) = (i[0] - 2 * (q_norm[2] * q_norm[2] + q_norm[3] * q_norm[3]) * i[0] + 2 * (q_norm[1] * q_norm[2] - q_norm[3] * q_norm[0]) * j[0] + 2 * (q_norm[1] * q_norm[3] + q_norm[2] * q_norm[0]) * k[0]);
-            data(m, 1, counter) = (2 * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * i[0] + j[0] - 2 * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * j[0] + 2 * (q_norm[2] * q_norm[3] - q_norm[1] * q_norm[0]) * k[0]);
-            data(m, 2, counter) = (2 * (q_norm[1] * q_norm[3] - q_norm[2] * q_norm[0]) * i[0] + 2 * (q_norm[2] * q_norm[3] + q_norm[1] * q_norm[0]) * j[0] + k[0] - 2 * (q_norm[1] * q_norm[1] + q_norm[2] * q_norm[2]) * k[0]);
+            data(m, 0, counter) = (i - 2 * (q_norm[2] * q_norm[2] + q_norm[3] * q_norm[3]) * i + 2 * (q_norm[1] * q_norm[2] - q_norm[3] * q_norm[0]) * j + 2 * (q_norm[1] * q_norm[3] + q_norm[2] * q_norm[0]) * k);
+            data(m, 1, counter) = (2 * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * i + j - 2 * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * j + 2 * (q_norm[2] * q_norm[3] - q_norm[1] * q_norm[0]) * k);
+            data(m, 2, counter) = (2 * (q_norm[1] * q_norm[3] - q_norm[2] * q_norm[0]) * i + 2 * (q_norm[2] * q_norm[3] + q_norm[1] * q_norm[0]) * j + k - 2 * (q_norm[1] * q_norm[1] + q_norm[2] * q_norm[2]) * k);
 
             counter++;
         }
         
         for (int n = 1; n < order+1; ++n) {
-            i[0] = (
-                    -(pow(n,2)+3) * (n) * cos(n*phi)*cos(phi)
-                    +(3*pow(n,2)+1) * sin(n*phi)*sin(phi)
+            i = (
+                    -(n*n+3) * (n) * cos(n*phi)*cos(phi)
+                    +(3*n*n+1) * sin(n*phi)*sin(phi)
                     );
-            j[0] = (
-                    -(pow(n,2)+3)*(n)*cos(n*phi)*sin(phi) 
-                    -(3*pow(n,2)+1)*sin(n*phi)*cos(phi) 
+            j = (
+                    -(n*n+3)*(n)*cos(n*phi)*sin(phi) 
+                    -(3*n*n+1)*sin(n*phi)*cos(phi) 
                     );
-            k[0] = 0;
+            k = 0;
 
-            i[0] *= 2*M_PI*2*M_PI*2*M_PI;
-            j[0] *= 2*M_PI*2*M_PI*2*M_PI;
-            k[0] *= 2*M_PI*2*M_PI*2*M_PI;
+            i *= 2*M_PI*2*M_PI*2*M_PI;
+            j *= 2*M_PI*2*M_PI*2*M_PI;
+            k *= 2*M_PI*2*M_PI*2*M_PI;
 
-            data(m, 0, counter) = (i[0] - 2 * (q_norm[2] * q_norm[2] + q_norm[3] * q_norm[3]) * i[0] + 2 * (q_norm[1] * q_norm[2] - q_norm[3] * q_norm[0]) * j[0] + 2 * (q_norm[1] * q_norm[3] + q_norm[2] * q_norm[0]) * k[0]);
-            data(m, 1, counter) = (2 * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * i[0] + j[0] - 2 * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * j[0] + 2 * (q_norm[2] * q_norm[3] - q_norm[1] * q_norm[0]) * k[0]);
-            data(m, 2, counter) = (2 * (q_norm[1] * q_norm[3] - q_norm[2] * q_norm[0]) * i[0] + 2 * (q_norm[2] * q_norm[3] + q_norm[1] * q_norm[0]) * j[0] + k[0] - 2 * (q_norm[1] * q_norm[1] + q_norm[2] * q_norm[2]) * k[0]);
+            data(m, 0, counter) = (i - 2 * (q_norm[2] * q_norm[2] + q_norm[3] * q_norm[3]) * i + 2 * (q_norm[1] * q_norm[2] - q_norm[3] * q_norm[0]) * j + 2 * (q_norm[1] * q_norm[3] + q_norm[2] * q_norm[0]) * k);
+            data(m, 1, counter) = (2 * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * i + j - 2 * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * j + 2 * (q_norm[2] * q_norm[3] - q_norm[1] * q_norm[0]) * k);
+            data(m, 2, counter) = (2 * (q_norm[1] * q_norm[3] - q_norm[2] * q_norm[0]) * i + 2 * (q_norm[2] * q_norm[3] + q_norm[1] * q_norm[0]) * j + k - 2 * (q_norm[1] * q_norm[1] + q_norm[2] * q_norm[2]) * k);
 
             counter++;
         }
         
-        i[0] = 0;
-        j[0] = 0;
-        k[0] = 0;
+        i = 0;
+        j = 0;
+        k = 0;
         for (int n = 0; n < order+1; ++n) {
-            i[0] += rc[n]*(
-                    +(3*pow(n, 2) + 1)*cos(n*phi)*sin(phi)
-                    +(pow(n, 2) + 3)*(n)*sin(n*phi)*cos(phi)
+            i += rc[n]*(
+                    +(3*n*n + 1)*cos(n*phi)*sin(phi)
+                    +(n*n + 3)*(n)*sin(n*phi)*cos(phi)
                     );
-            j[0] += rc[n]*(
-                    +(pow(n, 2) + 3)*(n)*sin(n*phi)*sin(phi)
-                    -(3*pow(n, 2) + 1)*cos(n*phi)*cos(phi)
+            j += rc[n]*(
+                    +(n*n + 3)*(n)*sin(n*phi)*sin(phi)
+                    -(3*n*n + 1)*cos(n*phi)*cos(phi)
                     );
         }
         for (int n = 1; n < order+1; ++n) {
-            i[0] += rs[n-1]*(
-                    -(pow(n,2)+3) * (n) * cos(n*phi)*cos(phi)
-                    +(3*pow(n,2)+1) * sin(n*phi)*sin(phi)
+            i += rs[n-1]*(
+                    -(n*n+3) * (n) * cos(n*phi)*cos(phi)
+                    +(3*n*n+1) * sin(n*phi)*sin(phi)
                     );
-            j[0] += rs[n-1]*(
-                    -(pow(n,2)+3)*(n)*cos(n*phi)*sin(phi) 
-                    -(3*pow(n,2)+1)*sin(n*phi)*cos(phi) 
+            j += rs[n-1]*(
+                    -(n*n+3)*(n)*cos(n*phi)*sin(phi) 
+                    -(3*n*n+1)*sin(n*phi)*cos(phi) 
                     );
         }
-        i[0] *= 2*M_PI*2*M_PI*2*M_PI;
-        j[0] *= 2*M_PI*2*M_PI*2*M_PI;
-        k[0] *= 2*M_PI*2*M_PI*2*M_PI;
+        i *= 2*M_PI*2*M_PI*2*M_PI;
+        j *= 2*M_PI*2*M_PI*2*M_PI;
+        k *= 2*M_PI*2*M_PI*2*M_PI;
+        double inv_sqrt_s = inv_magnitude();
         
-        data(m, 0, counter) = (4 * i[0] * (q_norm[2] * q_norm[2] + q_norm[3] * q_norm[3]) * q_norm[0]
-                            - 4 * j[0] * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[0]
-                            - 2 * j[0] * q_norm[3]) 
-                            * (1 / std::sqrt(s) - q[0] * q[0] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (4 * i[0] * (q_norm[2] * q_norm[2] + q_norm[3] * q_norm[3]) * q_norm[1]
-                            - 4 * j[0] * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[1]
-                            + 2 * j[0] * q_norm[2]) 
-                            * (- q[1] * q[0] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (- 4 * i[0] * (q_norm[0] * q_norm[0] + q_norm[1] * q_norm[1]) * q_norm[2]
-                            - 4 * j[0] * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[2]
-                            + 2 * j[0] * q_norm[1]) 
-                            * (- q[2] * q[0] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (- 4 * i[0] * (q_norm[1] * q_norm[1] + q_norm[0] * q_norm[0]) * q_norm[3]
-                            - 4 * j[0] * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[3]
-                            - 2 * j[0] * q_norm[0]) 
-                            * (- q[3] * q[0] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)));
-
-
-        data(m, 1, counter) = (- 4 * i[0] * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[0]
-                            + 2 * i[0] * q_norm[3]
-                            + 4 * j[0] * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[0])
-                            * (1 / std::sqrt(s) - q[0] * q[0] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (- 4 * i[0] * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[1]
-                            + 2 * i[0] * q_norm[2]
-                            + 4 * j[0] * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[1]
-                            - 4 * j[0] * q_norm[1])
-                            * (- q[1] * q[0] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (- 4 * i[0] * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[2]
-                            + 2 * i[0] * q_norm[1]
-                            + 4 * j[0] * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[2])
-                            * (- q[2] * q[0] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (- 4 * i[0] * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[3]
-                            + 2 * i[0] * q_norm[0]
-                            + 4 * j[0] * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[3]
-                            - 4 * j[0] * q_norm[3])
-                            * (- q[3] * q[0] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)));
-
-
-        data(m, 2, counter) = (- 4 * i[0] * (q_norm[1] * q_norm[3] - q_norm[2] * q_norm[0]) * q_norm[0]
-                            - 2 * i[0] * q_norm[2]
-                            - 4 * j[0] * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[0]
-                            + 2 * j[0] * q_norm[1])
-                            * (1 / std::sqrt(s) - q[0] * q[0] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (- 4 * i[0] * (q_norm[1] * q_norm[3] - q_norm[2] * q_norm[0]) * q_norm[1]
-                            + 2 * i[0] * q_norm[3]
-                            - 4 * j[0] * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[1]
-                            + 2 * j[0] * q_norm[0])
-                            * (- q[1] * q[0] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (- 2 * i[0] * q_norm[0]
-                            - 4 * i[0] * (q_norm[1] * q_norm[3] - q_norm[0] * q_norm[2]) * q_norm[2]
-                            - 4 * j[0] * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[2]
-                            + 2 * j[0] * q_norm[3])
-                            * (- q[2] * q[0] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (2 * i[0] * q_norm[1]
-                            + 4 * i[0] * (q_norm[2] * q_norm[0] - q_norm[1] * q_norm[3]) * q_norm[3]
-                            - 4 * j[0] * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[3]
-                            + 2 * j[0] * q_norm[2])
-                            * (- q[3] * q[0] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)));
+        data(m, 0, counter) = (4 * i * (q_norm[2] * q_norm[2] + q_norm[3] * q_norm[3]) * q_norm[0]
+                            - 4 * j * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[0]
+                            - 2 * j * q_norm[3]) 
+                            * (inv_sqrt_s - q[0] * q[0] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (4 * i * (q_norm[2] * q_norm[2] + q_norm[3] * q_norm[3]) * q_norm[1]
+                            - 4 * j * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[1]
+                            + 2 * j * q_norm[2]) 
+                            * (- q[1] * q[0] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (- 4 * i * (q_norm[0] * q_norm[0] + q_norm[1] * q_norm[1]) * q_norm[2]
+                            - 4 * j * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[2]
+                            + 2 * j * q_norm[1]) 
+                            * (- q[2] * q[0] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (- 4 * i * (q_norm[1] * q_norm[1] + q_norm[0] * q_norm[0]) * q_norm[3]
+                            - 4 * j * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[3]
+                            - 2 * j * q_norm[0]) 
+                            * (- q[3] * q[0] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s);
+
+
+        data(m, 1, counter) = (- 4 * i * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[0]
+                            + 2 * i * q_norm[3]
+                            + 4 * j * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[0])
+                            * (inv_sqrt_s - q[0] * q[0] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (- 4 * i * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[1]
+                            + 2 * i * q_norm[2]
+                            + 4 * j * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[1]
+                            - 4 * j * q_norm[1])
+                            * (- q[1] * q[0] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (- 4 * i * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[2]
+                            + 2 * i * q_norm[1]
+                            + 4 * j * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[2])
+                            * (- q[2] * q[0] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (- 4 * i * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[3]
+                            + 2 * i * q_norm[0]
+                            + 4 * j * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[3]
+                            - 4 * j * q_norm[3])
+                            * (- q[3] * q[0] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s);
+
+
+        data(m, 2, counter) = (- 4 * i * (q_norm[1] * q_norm[3] - q_norm[2] * q_norm[0]) * q_norm[0]
+                            - 2 * i * q_norm[2]
+                            - 4 * j * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[0]
+                            + 2 * j * q_norm[1])
+                            * (inv_sqrt_s - q[0] * q[0] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (- 4 * i * (q_norm[1] * q_norm[3] - q_norm[2] * q_norm[0]) * q_norm[1]
+                            + 2 * i * q_norm[3]
+                            - 4 * j * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[1]
+                            + 2 * j * q_norm[0])
+                            * (- q[1] * q[0] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (- 2 * i * q_norm[0]
+                            - 4 * i * (q_norm[1] * q_norm[3] - q_norm[0] * q_norm[2]) * q_norm[2]
+                            - 4 * j * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[2]
+                            + 2 * j * q_norm[3])
+                            * (- q[2] * q[0] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (2 * i * q_norm[1]
+                            + 4 * i * (q_norm[2] * q_norm[0] - q_norm[1] * q_norm[3]) * q_norm[3]
+                            - 4 * j * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[3]
+                            + 2 * j * q_norm[2])
+                            * (- q[3] * q[0] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s);
         counter++;
 
-        data(m, 0, counter) = (4 * i[0] * (q_norm[2] * q_norm[2] + q_norm[3] * q_norm[3]) * q_norm[0] 
-                            - 4 * j[0] * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[0]
-                            - 2 * j[0] * q_norm[3]) 
-                            * (- q[0] * q[1] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (4 * i[0] * (q_norm[2] * q_norm[2] + q_norm[3] * q_norm[3]) * q_norm[1]
-                            - 4 * j[0] * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[1]
-                            + 2 * j[0] * q_norm[2]) 
-                            * (1 / std::sqrt(s) - q[1] * q[1] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (- 4 * i[0] * (q_norm[0] * q_norm[0] + q_norm[1] * q_norm[1]) * q_norm[2]
-                            - 4 * j[0] * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[2]
-                            + 2 * j[0] * q_norm[1]) 
-                            * (- q[2] * q[1] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (- 4 * i[0] * (q_norm[1] * q_norm[1] + q_norm[0] * q_norm[0]) * q_norm[3]
-                            - 4 * j[0] * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[3]
-                            - 2 * j[0] * q_norm[0])
-                            * (- q[3] * q[1] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)));
-
-
-        data(m, 1, counter) = (- 4 * i[0] * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[0]
-                            + 2 * i[0] * q_norm[3]
-                            + 4 * j[0] * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[0])
-                            * (- q[0] * q[1] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (- 4 * i[0] * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[1]
-                            + 2 * i[0] * q_norm[2]
-                            + 4 * j[0] * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[1]
-                            - 4 * j[0] * q_norm[1])
-                            * (1 / std::sqrt(s) - q[1] * q[1] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (- 4 * i[0] * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[2]
-                            + 2 * i[0] * q_norm[1]
-                            + 4 * j[0] * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[2])
-                            * (- q[2] * q[1] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (- 4 * i[0] * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[3]
-                            + 2 * i[0] * q_norm[0]
-                            + 4 * j[0] * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[3]
-                            - 4 * j[0] * q_norm[3])
-                            * (- q[3] * q[1] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)));
-
-
-        data(m, 2, counter) = (- 4 * i[0] * (q_norm[1] * q_norm[3] - q_norm[2] * q_norm[0]) * q_norm[0]
-                            - 2 * i[0] * q_norm[2]
-                            - 4 * j[0] * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[0]
-                            + 2 * j[0] * q_norm[1])
-                            * (- q[0] * q[1] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (- 4 * i[0] * (q_norm[1] * q_norm[3] - q_norm[2] * q_norm[0]) * q_norm[1]
-                            + 2 * i[0] * q_norm[3]
-                            - 4 * j[0] * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[1]
-                            + 2 * j[0] * q_norm[0])
-                            * (1 / std::sqrt(s) - q[1] * q[1] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (- 2 * i[0] * q_norm[0]
-                            - 4 * i[0] * (q_norm[1] * q_norm[3] - q_norm[0] * q_norm[2]) * q_norm[2]
-                            - 4 * j[0] * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[2]
-                            + 2 * j[0] * q_norm[3])
-                            * (- q[2] * q[1] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (2 * i[0] * q_norm[1]
-                            + 4 * i[0] * (q_norm[2] * q_norm[0] - q_norm[1] * q_norm[3]) * q_norm[3]
-                            - 4 * j[0] * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[3]
-                            + 2 * j[0] * q_norm[2])
-                            * (- q[3] * q[1] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)));
+        data(m, 0, counter) = (4 * i * (q_norm[2] * q_norm[2] + q_norm[3] * q_norm[3]) * q_norm[0] 
+                            - 4 * j * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[0]
+                            - 2 * j * q_norm[3]) 
+                            * (- q[0] * q[1] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (4 * i * (q_norm[2] * q_norm[2] + q_norm[3] * q_norm[3]) * q_norm[1]
+                            - 4 * j * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[1]
+                            + 2 * j * q_norm[2]) 
+                            * (inv_sqrt_s - q[1] * q[1] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (- 4 * i * (q_norm[0] * q_norm[0] + q_norm[1] * q_norm[1]) * q_norm[2]
+                            - 4 * j * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[2]
+                            + 2 * j * q_norm[1]) 
+                            * (- q[2] * q[1] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (- 4 * i * (q_norm[1] * q_norm[1] + q_norm[0] * q_norm[0]) * q_norm[3]
+                            - 4 * j * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[3]
+                            - 2 * j * q_norm[0])
+                            * (- q[3] * q[1] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s);
+
+
+        data(m, 1, counter) = (- 4 * i * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[0]
+                            + 2 * i * q_norm[3]
+                            + 4 * j * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[0])
+                            * (- q[0] * q[1] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (- 4 * i * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[1]
+                            + 2 * i * q_norm[2]
+                            + 4 * j * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[1]
+                            - 4 * j * q_norm[1])
+                            * (inv_sqrt_s - q[1] * q[1] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (- 4 * i * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[2]
+                            + 2 * i * q_norm[1]
+                            + 4 * j * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[2])
+                            * (- q[2] * q[1] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (- 4 * i * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[3]
+                            + 2 * i * q_norm[0]
+                            + 4 * j * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[3]
+                            - 4 * j * q_norm[3])
+                            * (- q[3] * q[1] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s);
+
+
+        data(m, 2, counter) = (- 4 * i * (q_norm[1] * q_norm[3] - q_norm[2] * q_norm[0]) * q_norm[0]
+                            - 2 * i * q_norm[2]
+                            - 4 * j * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[0]
+                            + 2 * j * q_norm[1])
+                            * (- q[0] * q[1] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (- 4 * i * (q_norm[1] * q_norm[3] - q_norm[2] * q_norm[0]) * q_norm[1]
+                            + 2 * i * q_norm[3]
+                            - 4 * j * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[1]
+                            + 2 * j * q_norm[0])
+                            * (inv_sqrt_s - q[1] * q[1] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (- 2 * i * q_norm[0]
+                            - 4 * i * (q_norm[1] * q_norm[3] - q_norm[0] * q_norm[2]) * q_norm[2]
+                            - 4 * j * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[2]
+                            + 2 * j * q_norm[3])
+                            * (- q[2] * q[1] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (2 * i * q_norm[1]
+                            + 4 * i * (q_norm[2] * q_norm[0] - q_norm[1] * q_norm[3]) * q_norm[3]
+                            - 4 * j * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[3]
+                            + 2 * j * q_norm[2])
+                            * (- q[3] * q[1] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s);
         counter++;
 
-        data(m, 0, counter) = (4 * i[0] * (q_norm[2] * q_norm[2] + q_norm[3] * q_norm[3]) * q_norm[0] 
-                            - 4 * j[0] * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[0]
-                            - 2 * j[0] * q_norm[3]) 
-                            * (- q[0] * q[2] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
+        data(m, 0, counter) = (4 * i * (q_norm[2] * q_norm[2] + q_norm[3] * q_norm[3]) * q_norm[0] 
+                            - 4 * j * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[0]
+                            - 2 * j * q_norm[3]) 
+                            * (- q[0] * q[2] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
                             + 
-                            (4 * i[0] * (q_norm[2] * q_norm[2] + q_norm[3] * q_norm[3]) * q_norm[1]
-                            - 4 * j[0] * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[1]
-                            + 2 * j[0] * q_norm[2]) 
-                            * (- q[1] * q[2] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (- 4 * i[0] * (q_norm[0] * q_norm[0] + q_norm[1] * q_norm[1]) * q_norm[2]
-                            - 4 * j[0] * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[2]
-                            + 2 * j[0] * q_norm[1]) 
-                            * (1 / std::sqrt(s) - q[2] * q[2] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (- 4 * i[0] * (q_norm[1] * q_norm[1] + q_norm[0] * q_norm[0]) * q_norm[3]
-                            - 4 * j[0] * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[3]
-                            - 2 * j[0] * q_norm[0]) 
-                            * (- q[3] * q[2] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)));
-
-
-        data(m, 1, counter) = (- 4 * i[0] * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[0]
-                            + 2 * i[0] * q_norm[3]
-                            + 4 * j[0] * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[0])
-                            * (- q[0] * q[2] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (- 4 * i[0] * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[1]
-                            + 2 * i[0] * q_norm[2]
-                            + 4 * j[0] * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[1]
-                            - 4 * j[0] * q_norm[1])
-                            * (- q[1] * q[2] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (- 4 * i[0] * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[2]
-                            + 2 * i[0] * q_norm[1]
-                            + 4 * j[0] * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[2])
-                            * (1 / std::sqrt(s) - q[2] * q[2] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (- 4 * i[0] * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[3]
-                            + 2 * i[0] * q_norm[0]
-                            + 4 * j[0] * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[3]
-                            - 4 * j[0] * q_norm[3])
-                            * (- q[3] * q[2] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)));
-
-
-        data(m, 2, counter) = (- 4 * i[0] * (q_norm[1] * q_norm[3] - q_norm[2] * q_norm[0]) * q_norm[0]
-                            - 2 * i[0] * q_norm[2]
-                            - 4 * j[0] * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[0]
-                            + 2 * j[0] * q_norm[1])
-                            * (- q[0] * q[2] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (- 4 * i[0] * (q_norm[1] * q_norm[3] - q_norm[2] * q_norm[0]) * q_norm[1]
-                            + 2 * i[0] * q_norm[3]
-                            - 4 * j[0] * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[1]
-                            + 2 * j[0] * q_norm[0])
-                            * (- q[1] * q[2] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (- 2 * i[0] * q_norm[0]
-                            - 4 * i[0] * (q_norm[1] * q_norm[3] - q_norm[0] * q_norm[2]) * q_norm[2]
-                            - 4 * j[0] * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[2]
-                            + 2 * j[0] * q_norm[3])
-                            * (1 / std::sqrt(s) - q[2] * q[2] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (2 * i[0] * q_norm[1]
-                            + 4 * i[0] * (q_norm[2] * q_norm[0] - q_norm[1] * q_norm[3]) * q_norm[3]
-                            - 4 * j[0] * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[3]
-                            + 2 * j[0] * q_norm[2])
-                            * (- q[3] * q[2] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)));
+                            (4 * i * (q_norm[2] * q_norm[2] + q_norm[3] * q_norm[3]) * q_norm[1]
+                            - 4 * j * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[1]
+                            + 2 * j * q_norm[2]) 
+                            * (- q[1] * q[2] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (- 4 * i * (q_norm[0] * q_norm[0] + q_norm[1] * q_norm[1]) * q_norm[2]
+                            - 4 * j * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[2]
+                            + 2 * j * q_norm[1]) 
+                            * (inv_sqrt_s - q[2] * q[2] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (- 4 * i * (q_norm[1] * q_norm[1] + q_norm[0] * q_norm[0]) * q_norm[3]
+                            - 4 * j * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[3]
+                            - 2 * j * q_norm[0]) 
+                            * (- q[3] * q[2] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s);
+
+
+        data(m, 1, counter) = (- 4 * i * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[0]
+                            + 2 * i * q_norm[3]
+                            + 4 * j * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[0])
+                            * (- q[0] * q[2] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (- 4 * i * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[1]
+                            + 2 * i * q_norm[2]
+                            + 4 * j * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[1]
+                            - 4 * j * q_norm[1])
+                            * (- q[1] * q[2] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (- 4 * i * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[2]
+                            + 2 * i * q_norm[1]
+                            + 4 * j * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[2])
+                            * (inv_sqrt_s - q[2] * q[2] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (- 4 * i * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[3]
+                            + 2 * i * q_norm[0]
+                            + 4 * j * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[3]
+                            - 4 * j * q_norm[3])
+                            * (- q[3] * q[2] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s);
+
+
+        data(m, 2, counter) = (- 4 * i * (q_norm[1] * q_norm[3] - q_norm[2] * q_norm[0]) * q_norm[0]
+                            - 2 * i * q_norm[2]
+                            - 4 * j * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[0]
+                            + 2 * j * q_norm[1])
+                            * (- q[0] * q[2] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (- 4 * i * (q_norm[1] * q_norm[3] - q_norm[2] * q_norm[0]) * q_norm[1]
+                            + 2 * i * q_norm[3]
+                            - 4 * j * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[1]
+                            + 2 * j * q_norm[0])
+                            * (- q[1] * q[2] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (- 2 * i * q_norm[0]
+                            - 4 * i * (q_norm[1] * q_norm[3] - q_norm[0] * q_norm[2]) * q_norm[2]
+                            - 4 * j * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[2]
+                            + 2 * j * q_norm[3])
+                            * (inv_sqrt_s - q[2] * q[2] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (2 * i * q_norm[1]
+                            + 4 * i * (q_norm[2] * q_norm[0] - q_norm[1] * q_norm[3]) * q_norm[3]
+                            - 4 * j * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[3]
+                            + 2 * j * q_norm[2])
+                            * (- q[3] * q[2] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s);
         counter++;
 
-        data(m, 0, counter) = (4 * i[0] * (q_norm[2] * q_norm[2] + q_norm[3] * q_norm[3]) * q_norm[0] 
-                            - 4 * j[0] * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[0]
-                            - 2 * j[0] * q_norm[3]) 
-                            * (- q[0] * q[3] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
+        data(m, 0, counter) = (4 * i * (q_norm[2] * q_norm[2] + q_norm[3] * q_norm[3]) * q_norm[0] 
+                            - 4 * j * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[0]
+                            - 2 * j * q_norm[3]) 
+                            * (- q[0] * q[3] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
                             +
-                            + (4 * i[0] * (q_norm[2] * q_norm[2] + q_norm[3] * q_norm[3]) * q_norm[1]
-                            - 4 * j[0] * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[1]
-                            + 2 * j[0] * q_norm[2]) 
-                            * (- q[1] * q[3] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
+                            + (4 * i * (q_norm[2] * q_norm[2] + q_norm[3] * q_norm[3]) * q_norm[1]
+                            - 4 * j * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[1]
+                            + 2 * j * q_norm[2]) 
+                            * (- q[1] * q[3] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
                             +
-                            (- 4 * i[0] * (q_norm[0] * q_norm[0] + q_norm[1] * q_norm[1]) * q_norm[2]
-                            - 4 * j[0] * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[2]
-                            + 2 * j[0] * q_norm[1]) 
-                            * (- q[2] * q[3] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
+                            (- 4 * i * (q_norm[0] * q_norm[0] + q_norm[1] * q_norm[1]) * q_norm[2]
+                            - 4 * j * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[2]
+                            + 2 * j * q_norm[1]) 
+                            * (- q[2] * q[3] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
                             +                            
-                            (- 4 * i[0] * (q_norm[1] * q_norm[1] + q_norm[0] * q_norm[0]) * q_norm[3]
-                            - 4 * j[0] * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[3]
-                            - 2 * j[0] * q_norm[0]) 
-                            * (1 / std::sqrt(s) - q[3] * q[3] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)));
-
-
-        data(m, 1, counter) = (- 4 * i[0] * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[0]
-                            + 2 * i[0] * q_norm[3]
-                            + 4 * j[0] * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[0])
-                            * (- q[0] * q[3] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (- 4 * i[0] * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[1]
-                            + 2 * i[0] * q_norm[2]
-                            + 4 * j[0] * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[1]
-                            - 4 * j[0] * q_norm[1])
-                            * (- q[1] * q[3] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (- 4 * i[0] * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[2]
-                            + 2 * i[0] * q_norm[1]
-                            + 4 * j[0] * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[2])
-                            * (- q[2] * q[3] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (- 4 * i[0] * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[3]
-                            + 2 * i[0] * q_norm[0]
-                            + 4 * j[0] * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[3]
-                            - 4 * j[0] * q_norm[3])
-                            * (1 / std::sqrt(s) - q[3] * q[3] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)));
-
-
-        data(m, 2, counter) = (- 4 * i[0] * (q_norm[1] * q_norm[3] - q_norm[2] * q_norm[0]) * q_norm[0]
-                            - 2 * i[0] * q_norm[2]
-                            - 4 * j[0] * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[0]
-                            + 2 * j[0] * q_norm[1])
-                            * (- q[0] * q[3] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (- 4 * i[0] * (q_norm[1] * q_norm[3] - q_norm[2] * q_norm[0]) * q_norm[1]
-                            + 2 * i[0] * q_norm[3]
-                            - 4 * j[0] * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[1]
-                            + 2 * j[0] * q_norm[0])
-                            * (- q[1] * q[3] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (- 2 * i[0] * q_norm[0]
-                            - 4 * i[0] * (q_norm[1] * q_norm[3] - q_norm[0] * q_norm[2]) * q_norm[2]
-                            - 4 * j[0] * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[2]
-                            + 2 * j[0] * q_norm[3])
-                            * (- q[2] * q[3] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)))
-                            +
-                            (2 * i[0] * q_norm[1]
-                            + 4 * i[0] * (q_norm[2] * q_norm[0] - q_norm[1] * q_norm[3]) * q_norm[3]
-                            - 4 * j[0] * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[3]
-                            + 2 * j[0] * q_norm[2])
-                            * (1 / std::sqrt(s) - q[3] * q[3] / (std::sqrt(s) * std::sqrt(s) * std::sqrt(s)));
+                            (- 4 * i * (q_norm[1] * q_norm[1] + q_norm[0] * q_norm[0]) * q_norm[3]
+                            - 4 * j * (q_norm[1] * q_norm[2] - q_norm[0] * q_norm[3]) * q_norm[3]
+                            - 2 * j * q_norm[0]) 
+                            * (inv_sqrt_s - q[3] * q[3] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s);
+
+
+        data(m, 1, counter) = (- 4 * i * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[0]
+                            + 2 * i * q_norm[3]
+                            + 4 * j * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[0])
+                            * (- q[0] * q[3] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (- 4 * i * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[1]
+                            + 2 * i * q_norm[2]
+                            + 4 * j * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[1]
+                            - 4 * j * q_norm[1])
+                            * (- q[1] * q[3] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (- 4 * i * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[2]
+                            + 2 * i * q_norm[1]
+                            + 4 * j * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[2])
+                            * (- q[2] * q[3] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (- 4 * i * (q_norm[1] * q_norm[2] + q_norm[3] * q_norm[0]) * q_norm[3]
+                            + 2 * i * q_norm[0]
+                            + 4 * j * (q_norm[1] * q_norm[1] + q_norm[3] * q_norm[3]) * q_norm[3]
+                            - 4 * j * q_norm[3])
+                            * (inv_sqrt_s - q[3] * q[3] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s);
+
+
+        data(m, 2, counter) = (- 4 * i * (q_norm[1] * q_norm[3] - q_norm[2] * q_norm[0]) * q_norm[0]
+                            - 2 * i * q_norm[2]
+                            - 4 * j * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[0]
+                            + 2 * j * q_norm[1])
+                            * (- q[0] * q[3] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (- 4 * i * (q_norm[1] * q_norm[3] - q_norm[2] * q_norm[0]) * q_norm[1]
+                            + 2 * i * q_norm[3]
+                            - 4 * j * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[1]
+                            + 2 * j * q_norm[0])
+                            * (- q[1] * q[3] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (- 2 * i * q_norm[0]
+                            - 4 * i * (q_norm[1] * q_norm[3] - q_norm[0] * q_norm[2]) * q_norm[2]
+                            - 4 * j * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[2]
+                            + 2 * j * q_norm[3])
+                            * (- q[2] * q[3] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s)
+                            +
+                            (2 * i * q_norm[1]
+                            + 4 * i * (q_norm[2] * q_norm[0] - q_norm[1] * q_norm[3]) * q_norm[3]
+                            - 4 * j * (q_norm[1] * q_norm[0] + q_norm[2] * q_norm[3]) * q_norm[3]
+                            + 2 * j * q_norm[2])
+                            * (inv_sqrt_s - q[3] * q[3] * inv_sqrt_s * inv_sqrt_s * inv_sqrt_s);
         counter++;
 
         for (int i = 0; i < 3; ++i) {
diff --git a/src/simsoptpp/curveplanarfourier.h b/src/simsoptpp/curveplanarfourier.h
index 4b8b4baee..f365ba88e 100644
--- a/src/simsoptpp/curveplanarfourier.h
+++ b/src/simsoptpp/curveplanarfourier.h
@@ -101,6 +101,8 @@ class CurvePlanarFourier : public Curve<Array> {
         }
 
 
+        double inv_magnitude();
+
         void gamma_impl(Array& data, Array& quadpoints) override;
         void gammadash_impl(Array& data) override;
         void gammadashdash_impl(Array& data) override;
diff --git a/tests/geo/test_curve.py b/tests/geo/test_curve.py
index 59c480c48..275b6464b 100644
--- a/tests/geo/test_curve.py
+++ b/tests/geo/test_curve.py
@@ -509,6 +509,5 @@ def test_load_curves_from_makegrid_file(self):
 
             os.remove("coils.file_to_load")
 
-
 if __name__ == "__main__":
     unittest.main()
diff --git a/tests/geo/test_equally_spaced_planar_coils.py b/tests/geo/test_equally_spaced_planar_coils.py
new file mode 100644
index 000000000..4470430fd
--- /dev/null
+++ b/tests/geo/test_equally_spaced_planar_coils.py
@@ -0,0 +1,26 @@
+import unittest
+from simsopt.geo.curve import create_equally_spaced_curves, create_equally_spaced_planar_curves
+from simsopt.field import coils_via_symmetries, BiotSavart, Current
+
+class Testing(unittest.TestCase):
+    def test_equally_spaced_planar_coils(self):
+        ncoils = 4
+        nfp = 4
+        stellsym = False
+
+        curves = create_equally_spaced_curves(ncoils, nfp, False)
+        currents = [Current(1e5) for i in range(ncoils)]
+
+        curves_planar = create_equally_spaced_planar_curves(ncoils, nfp, False)
+        currents_planar = [Current(1e5) for i in range(ncoils)]
+
+        coils = coils_via_symmetries(curves, currents, nfp, stellsym)
+        coils_planar = coils_via_symmetries(curves_planar, currents_planar, nfp, stellsym)
+        bs = BiotSavart(coils)
+        bs_planar = BiotSavart(coils_planar)
+        err = bs.AbsB()[0][0] - bs_planar.AbsB()[0][0]
+        print(err)
+        assert(err < 3e-10)
+
+if __name__ == "__main__":
+    unittest.main()
\ No newline at end of file