AP_Math: const correctness

libraries/AP_Math/matrix3.cpp

@@ -25,12 +25,12 @@
 template <typename T>
 void Matrix3<T>::from_euler(float roll, float pitch, float yaw)
 {
-    float cp = cosf(pitch);
-    float sp = sinf(pitch);
-    float sr = sinf(roll);
-    float cr = cosf(roll);
-    float sy = sinf(yaw);
-    float cy = cosf(yaw);
+    const float cp = cosf(pitch);
+    const float sp = sinf(pitch);
+    const float sr = sinf(roll);
+    const float cr = cosf(roll);
+    const float sy = sinf(yaw);
+    const float cy = cosf(yaw);
 
     a.x = cp * cy;
     a.y = (sr * sp * cy) - (cr * sy);
@@ -91,12 +91,12 @@ Vector3<T> Matrix3<T>::to_euler312() const
 template <typename T>
 void Matrix3<T>::from_euler312(float roll, float pitch, float yaw)
 {
-    float c3 = cosf(pitch);
-    float s3 = sinf(pitch);
-    float s2 = sinf(roll);
-    float c2 = cosf(roll);
-    float s1 = sinf(yaw);
-    float c1 = cosf(yaw);
+    const float c3 = cosf(pitch);
+    const float s3 = sinf(pitch);
+    const float s2 = sinf(roll);
+    const float c2 = cosf(roll);
+    const float s1 = sinf(yaw);
+    const float c1 = cosf(yaw);
 
     a.x = c1 * c3 - s1 * s2 * s3;
     b.y = c1 * c2;
@@ -134,10 +134,10 @@ void Matrix3<T>::rotate(const Vector3<T> &g)
 template <typename T>
 void Matrix3<T>::normalize(void)
 {
-    float error = a * b;
-    Vector3<T> t0 = a - (b * (0.5f * error));
-    Vector3<T> t1 = b - (a * (0.5f * error));
-    Vector3<T> t2 = t0 % t1;
+    const float error = a * b;
+    const Vector3<T> t0 = a - (b * (0.5f * error));
+    const Vector3<T> t1 = b - (a * (0.5f * error));
+    const Vector3<T> t2 = t0 % t1;
     a = t0 * (1.0f / t0.length());
     b = t1 * (1.0f / t1.length());
     c = t2 * (1.0f / t2.length());
@@ -204,7 +204,7 @@ T Matrix3<T>::det() const
 template <typename T>
 bool Matrix3<T>::inverse(Matrix3<T>& inv) const
 {
-    T d = det();
+    const T d = det();
 
     if (is_zero(d)) {
         return false;
@@ -247,14 +247,14 @@ void Matrix3<T>::zero(void)
 template <typename T>
 void Matrix3<T>::from_axis_angle(const Vector3<T> &v, float theta)
 {
-    float C = cosf(theta);
-    float S = sinf(theta);
-    float t = 1.0f - C;
-    Vector3f normv = v.normalized();
-    float x = normv.x;
-    float y = normv.y;
-    float z = normv.z;
-    
+    const float C = cosf(theta);
+    const float S = sinf(theta);
+    const float t = 1.0f - C;
+    const Vector3f normv = v.normalized();
+    const float x = normv.x;
+    const float y = normv.y;
+    const float z = normv.z;
+
     a.x = t*x*x + C;
     a.y = t*x*y - z*S;
     a.z = t*x*z + y*S;

libraries/AP_Math/spline5.cpp

@@ -50,7 +50,7 @@ void splinterp5(const float x[5], float out[4][4])
         } else if (p > -0.01f && p < 0.0f) {
             p = -0.01f;
         }
-        float p_inv = 1.0f / p;
+        const float p_inv = 1.0f / p;
         z[i] = -0.5f * p_inv;
         u[i] = x[i+1] + x[i-1] - 2.0f * x[i];
         u[i] = (3.0f * u[i] - 0.5f * u[i-1]) * p_inv;

libraries/AP_Math/vector2.cpp

@@ -126,11 +126,11 @@ bool Vector2<T>::operator !=(const Vector2<T> &v) const
 template <typename T>
 float Vector2<T>::angle(const Vector2<T> &v2) const
 {
-    float len = this->length() * v2.length();
+    const float len = this->length() * v2.length();
     if (len <= 0) {
         return 0.0f;
     }
-    float cosv = ((*this)*v2) / len;
+    const float cosv = ((*this)*v2) / len;
     if (cosv >= 1) {
         return 0.0f;
     }
@@ -158,8 +158,8 @@ bool Vector2<T>::segment_intersection(const Vector2<T>& seg1_start, const Vector
     } else {
         // t = (q - p) * s / (r * s)
         // u = (q - p) * r / (r * s)
-        float t = (ss2_ss1 % r2) / r1xr2;
-        float u = q_pxr / r1xr2;
+        const float t = (ss2_ss1 % r2) / r1xr2;
+        const float u = q_pxr / r1xr2;
         if ((u >= 0) && (u <= 1) && (t >= 0) && (t <= 1)) {
             // lines intersect
             // t can be any non-negative value because (p, p + r) is a ray

libraries/AP_Math/vector2.h

@@ -154,7 +154,7 @@ struct Vector2
     // reflects this vector about n
     void    reflect(const Vector2<T> &n)
     {
-        Vector2<T>        orig(*this);
+        const Vector2<T>        orig(*this);
         project(n);
         *this= *this*2 - orig;
     }
@@ -175,10 +175,10 @@ struct Vector2
     // perpendicular to v1 maximising distance from p1
     static Vector2<T> perpendicular(const Vector2<T> &pos_delta, const Vector2<T> &v1)
     {
-        Vector2<T> perpendicular1 = Vector2<T>(-v1[1], v1[0]);
-        Vector2<T> perpendicular2 = Vector2<T>(v1[1], -v1[0]);
-        T d1 = perpendicular1 * pos_delta;
-        T d2 = perpendicular2 * pos_delta;
+        const Vector2<T> perpendicular1 = Vector2<T>(-v1[1], v1[0]);
+        const Vector2<T> perpendicular2 = Vector2<T>(v1[1], -v1[0]);
+        const T d1 = perpendicular1 * pos_delta;
+        const T d2 = perpendicular2 * pos_delta;
         if (d1 > d2) {
             return perpendicular1;
         }

libraries/AP_Math/vector3.cpp

@@ -375,11 +375,11 @@ bool Vector3<T>::operator !=(const Vector3<T> &v) const
 template <typename T>
 float Vector3<T>::angle(const Vector3<T> &v2) const
 {
-    float len = this->length() * v2.length();
+    const float len = this->length() * v2.length();
     if (len <= 0) {
         return 0.0f;
     }
-    float cosv = ((*this)*v2) / len;
+    const float cosv = ((*this)*v2) / len;
     if (fabsf(cosv) >= 1) {
         return 0.0f;
     }
@@ -410,9 +410,9 @@ template <typename T>
 float Vector3<T>::distance_to_segment(const Vector3<T> &seg_start, const Vector3<T> &seg_end) const
 {
     // triangle side lengths
-    float a = (*this-seg_start).length();
-    float b = (seg_start-seg_end).length();
-    float c = (seg_end-*this).length();
+    const float a = (*this-seg_start).length();
+    const float b = (seg_start-seg_end).length();
+    const float c = (seg_end-*this).length();
 
     // protect against divide by zero later
     if (::is_zero(b)) {
@@ -420,14 +420,14 @@ float Vector3<T>::distance_to_segment(const Vector3<T> &seg_start, const Vector3
     }
 
     // semiperimeter of triangle
-    float s = (a+b+c) * 0.5f;
+    const float s = (a+b+c) * 0.5f;
 
     float area_squared = s*(s-a)*(s-b)*(s-c);
     // area must be constrained above 0 because a triangle could have 3 points could be on a line and float rounding could push this under 0
     if (area_squared < 0.0f) {
         area_squared = 0.0f;
     }
-    float area = safe_sqrt(area_squared);
+    const float area = safe_sqrt(area_squared);
     return 2.0f*area/b;
 }
 

libraries/AP_Math/vector3.h

@@ -218,9 +218,9 @@ public:
 
     // distance from the tip of this vector to another vector squared (so as to avoid the sqrt calculation)
     float distance_squared(const Vector3<T> &v) const {
-        float dist_x = x-v.x;
-        float dist_y = y-v.y;
-        float dist_z = z-v.z;
+        const float dist_x = x-v.x;
+        const float dist_y = y-v.y;
+        const float dist_z = z-v.z;
         return (dist_x*dist_x + dist_y*dist_y + dist_z*dist_z);
     }
 
@@ -233,11 +233,11 @@ public:
     // zero vector - that should be checked for.
     static Vector3<T> perpendicular(const Vector3<T> &p1, const Vector3<T> &v1)
     {
-        T d = p1 * v1;
+        const T d = p1 * v1;
         if (fabsf(d) < FLT_EPSILON) {
             return p1;
         }
-        Vector3<T> parallel = (v1 * d) / v1.length_squared();
+        const Vector3<T> parallel = (v1 * d) / v1.length_squared();
         Vector3<T> perpendicular = p1 - parallel;
 
         return perpendicular;