Fix some template type conversions and style

PX4 · Sep 23, 2019 · 973999a · 973999a
1 parent c34e8dc
commit 973999a
Show file tree

Hide file tree

Showing 4 changed files with 17 additions and 17 deletions.
diff --git a/matrix/AxisAngle.hpp b/matrix/AxisAngle.hpp
@@ -69,7 +69,7 @@ class AxisAngle : public Vector<Type, 3>
     AxisAngle(const Quaternion<Type> &q)
     {
         AxisAngle &v = *this;
-        Type ang = Type(2.0f)*acos(q(0));
+        Type ang = Type(2) * acos(q(0));
         Type mag = sin(ang/2.0f);
         if (fabs(mag) > 0) {
             v(0) = ang*q(1)/mag;

diff --git a/matrix/Euler.hpp b/matrix/Euler.hpp
@@ -97,11 +97,11 @@ class Euler : public Vector<Type, 3>
         Type pi = Type(M_PI);
 
         if (Type(fabs(theta_val - pi / Type(2))) < Type(1.0e-3)) {
-            phi_val = Type(0.0);
+            phi_val = Type(0);
             psi_val = Type(atan2(dcm(1, 2), dcm(0, 2)));
 
         } else if (Type(fabs(theta_val + pi / Type(2))) < Type(1.0e-3)) {
-            phi_val = Type(0.0);
+            phi_val = Type(0);
             psi_val = Type(atan2(-dcm(1, 2), -dcm(0, 2)));
         }
 

diff --git a/matrix/Matrix.hpp b/matrix/Matrix.hpp
@@ -278,7 +278,7 @@ class Matrix
     void operator/=(Type scalar)
     {
         Matrix<Type, M, N> &self = *this;
-        self = self * (Type(1.0f) / scalar);
+        self = self * (Type(1) / scalar);
     }
 
     inline void operator+=(Type scalar)

diff --git a/matrix/Quaternion.hpp b/matrix/Quaternion.hpp
@@ -143,12 +143,12 @@ class Quaternion : public Vector<Type, 4>
     Quaternion(const Euler<Type> &euler)
     {
         Quaternion &q = *this;
-        Type cosPhi_2 = Type(cos(euler.phi() / Type(2.0)));
-        Type cosTheta_2 = Type(cos(euler.theta() / Type(2.0)));
-        Type cosPsi_2 = Type(cos(euler.psi() / Type(2.0)));
-        Type sinPhi_2 = Type(sin(euler.phi() / Type(2.0)));
-        Type sinTheta_2 = Type(sin(euler.theta() / Type(2.0)));
-        Type sinPsi_2 = Type(sin(euler.psi() / Type(2.0)));
+        Type cosPhi_2 = Type(cos(euler.phi() / Type(2)));
+        Type cosTheta_2 = Type(cos(euler.theta() / Type(2)));
+        Type cosPsi_2 = Type(cos(euler.psi() / Type(2)));
+        Type sinPhi_2 = Type(sin(euler.phi() / Type(2)));
+        Type sinTheta_2 = Type(sin(euler.theta() / Type(2)));
+        Type sinPsi_2 = Type(sin(euler.psi() / Type(2)));
         q(0) = cosPhi_2 * cosTheta_2 * cosPsi_2 +
                sinPhi_2 * sinTheta_2 * sinPsi_2;
         q(1) = sinPhi_2 * cosTheta_2 * cosPsi_2 -
@@ -170,10 +170,10 @@ class Quaternion : public Vector<Type, 4>
         Type angle = aa.norm();
         Vector<Type, 3> axis = aa.unit();
         if (angle < Type(1e-10)) {
-            q(0) = Type(1.0);
+            q(0) = Type(1);
             q(1) = q(2) = q(3) = 0;
         } else {
-            Type magnitude = sin(angle / 2.0f);
+            Type magnitude = sin(angle / Type(2));
             q(0) = cos(angle / 2.0f);
             q(1) = axis(0) * magnitude;
             q(2) = axis(1) * magnitude;
@@ -368,7 +368,7 @@ class Quaternion : public Vector<Type, 4>
     Quaternion canonical() const
     {
         const Quaternion &q = *this;
-        if(q(0)<Type(0)) {
+        if (q(0) < Type(0)) {
             return Quaternion(-q(0),-q(1),-q(2),-q(3));
         } else {
             return Quaternion(q(0),q(1),q(2),q(3));
@@ -434,7 +434,7 @@ class Quaternion : public Vector<Type, 4>
         Type theta = vec.norm();
 
         if (theta < Type(1e-10)) {
-            q(0) = Type(1.0);
+            q(0) = Type(1);
             q(1) = q(2) = q(3) = 0;
             return;
         }
@@ -456,8 +456,8 @@ class Quaternion : public Vector<Type, 4>
         Quaternion &q = *this;
 
         if (theta < Type(1e-10)) {
-            q(0) = Type(1.0);
-            q(1) = q(2) = q(3) = 0;
+            q(0) = Type(1);
+            q(1) = q(2) = q(3) = Type(0);
         }
 
         Type magnitude = sin(theta / 2.0f);
@@ -489,7 +489,7 @@ class Quaternion : public Vector<Type, 4>
 
         if (axis_magnitude >= Type(1e-10)) {
             vec = vec / axis_magnitude;
-            vec = vec * wrap_pi(Type(2.0) * atan2(axis_magnitude, q(0)));
+            vec = vec * wrap_pi(Type(2) * atan2(axis_magnitude, q(0)));
         }
 
         return vec;