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/quaternion.cpp

@@ -23,15 +23,15 @@
 // return the rotation matrix equivalent for this quaternion
 void Quaternion::rotation_matrix(Matrix3f &m) const
 {
-    float q3q3 = q3 * q3;
-    float q3q4 = q3 * q4;
-    float q2q2 = q2 * q2;
-    float q2q3 = q2 * q3;
-    float q2q4 = q2 * q4;
-    float q1q2 = q1 * q2;
-    float q1q3 = q1 * q3;
-    float q1q4 = q1 * q4;
-    float q4q4 = q4 * q4;
+    const float q3q3 = q3 * q3;
+    const float q3q4 = q3 * q4;
+    const float q2q2 = q2 * q2;
+    const float q2q3 = q2 * q3;
+    const float q2q4 = q2 * q4;
+    const float q1q2 = q1 * q2;
+    const float q1q3 = q1 * q3;
+    const float q1q4 = q1 * q4;
+    const float q4q4 = q4 * q4;
 
     m.a.x = 1.0f-2.0f*(q3q3 + q4q4);
     m.a.y = 2.0f*(q2q3 - q1q4);
@@ -47,17 +47,17 @@ void Quaternion::rotation_matrix(Matrix3f &m) const
 // return the rotation matrix equivalent for this quaternion after normalization
 void Quaternion::rotation_matrix_norm(Matrix3f &m) const
 {
-    float q1q1 = q1 * q1;
-    float q1q2 = q1 * q2;
-    float q1q3 = q1 * q3;
-    float q1q4 = q1 * q4;
-    float q2q2 = q2 * q2;
-    float q2q3 = q2 * q3;
-    float q2q4 = q2 * q4;
-    float q3q3 = q3 * q3;
-    float q3q4 = q3 * q4;
-    float q4q4 = q4 * q4;
-    float invs = 1.0f / (q1q1 + q2q2 + q3q3 + q4q4);
+    const float q1q1 = q1 * q1;
+    const float q1q2 = q1 * q2;
+    const float q1q3 = q1 * q3;
+    const float q1q4 = q1 * q4;
+    const float q2q2 = q2 * q2;
+    const float q2q3 = q2 * q3;
+    const float q2q4 = q2 * q4;
+    const float q3q3 = q3 * q3;
+    const float q3q4 = q3 * q4;
+    const float q4q4 = q4 * q4;
+    const float invs = 1.0f / (q1q1 + q2q2 + q3q3 + q4q4);
 
     m.a.x = ( q2q2 - q3q3 - q4q4 + q1q1)*invs;
     m.a.y = 2.0f*(q2q3 - q1q4)*invs;
@@ -89,28 +89,28 @@ void Quaternion::from_rotation_matrix(const Matrix3f &m)
     float &qy = q3;
     float &qz = q4;
 
-    float tr = m00 + m11 + m22;
+    const float tr = m00 + m11 + m22;
 
     if (tr > 0) {
-        float S = sqrtf(tr+1) * 2;
+        const float S = sqrtf(tr+1) * 2;
         qw = 0.25f * S;
         qx = (m21 - m12) / S;
         qy = (m02 - m20) / S;
         qz = (m10 - m01) / S;
     } else if ((m00 > m11) && (m00 > m22)) {
-        float S = sqrtf(1.0f + m00 - m11 - m22) * 2.0f;
+        const float S = sqrtf(1.0f + m00 - m11 - m22) * 2.0f;
         qw = (m21 - m12) / S;
         qx = 0.25f * S;
         qy = (m01 + m10) / S;
         qz = (m02 + m20) / S;
     } else if (m11 > m22) {
-        float S = sqrtf(1.0f + m11 - m00 - m22) * 2.0f;
+        const float S = sqrtf(1.0f + m11 - m00 - m22) * 2.0f;
         qw = (m02 - m20) / S;
         qx = (m01 + m10) / S;
         qy = 0.25f * S;
         qz = (m12 + m21) / S;
     } else {
-        float S = sqrtf(1.0f + m22 - m00 - m11) * 2.0f;
+        const float S = sqrtf(1.0f + m22 - m00 - m11) * 2.0f;
         qw = (m10 - m01) / S;
         qx = (m02 + m20) / S;
         qy = (m12 + m21) / S;
@@ -129,12 +129,12 @@ void Quaternion::earth_to_body(Vector3f &v) const
 // create a quaternion from Euler angles
 void Quaternion::from_euler(float roll, float pitch, float yaw)
 {
-    float cr2 = cosf(roll*0.5f);
-    float cp2 = cosf(pitch*0.5f);
-    float cy2 = cosf(yaw*0.5f);
-    float sr2 = sinf(roll*0.5f);
-    float sp2 = sinf(pitch*0.5f);
-    float sy2 = sinf(yaw*0.5f);
+    const float cr2 = cosf(roll*0.5f);
+    const float cp2 = cosf(pitch*0.5f);
+    const float cy2 = cosf(yaw*0.5f);
+    const float sr2 = sinf(roll*0.5f);
+    const float sp2 = sinf(pitch*0.5f);
+    const float sy2 = sinf(yaw*0.5f);
 
     q1 = cr2*cp2*cy2 + sr2*sp2*sy2;
     q2 = sr2*cp2*cy2 - cr2*sp2*sy2;
@@ -153,7 +153,7 @@ void Quaternion::from_vector312(float roll ,float pitch, float yaw)
 
 void Quaternion::from_axis_angle(Vector3f v)
 {
-    float theta = v.length();
+    const float theta = v.length();
     if (is_zero(theta)) {
         q1 = 1.0f;
         q2=q3=q4=0.0f;
@@ -171,7 +171,7 @@ void Quaternion::from_axis_angle(const Vector3f &axis, float theta)
         q2=q3=q4=0.0f;
         return;
     }
-    float st2 = sinf(theta/2.0f);
+    const float st2 = sinf(theta/2.0f);
 
     q1 = cosf(theta/2.0f);
     q2 = axis.x * st2;
@@ -188,7 +188,7 @@ void Quaternion::rotate(const Vector3f &v)
 
 void Quaternion::to_axis_angle(Vector3f &v)
 {
-    float l = sqrtf(sq(q2)+sq(q3)+sq(q4));
+    const float l = sqrtf(sq(q2)+sq(q3)+sq(q4));
     v = Vector3f(q2,q3,q4);
     if (!is_zero(l)) {
         v /= l;
@@ -198,7 +198,7 @@ void Quaternion::to_axis_angle(Vector3f &v)
 
 void Quaternion::from_axis_angle_fast(Vector3f v)
 {
-    float theta = v.length();
+    const float theta = v.length();
     if (is_zero(theta)) {
         q1 = 1.0f;
         q2=q3=q4=0.0f;
@@ -210,9 +210,9 @@ void Quaternion::from_axis_angle_fast(Vector3f v)
 
 void Quaternion::from_axis_angle_fast(const Vector3f &axis, float theta)
 {
-    float t2 = theta/2.0f;
-    float sqt2 = sq(t2);
-    float st2 = t2-sqt2*t2/6.0f;
+    const float t2 = theta/2.0f;
+    const float sqt2 = sq(t2);
+    const float st2 = t2-sqt2*t2/6.0f;
 
     q1 = 1.0f-(sqt2/2.0f)+sq(sqt2)/24.0f;
     q2 = axis.x * st2;
@@ -222,26 +222,26 @@ void Quaternion::from_axis_angle_fast(const Vector3f &axis, float theta)
 
 void Quaternion::rotate_fast(const Vector3f &v)
 {
-    float theta = v.length();
+    const float theta = v.length();
     if (is_zero(theta)) {
         return;
     }
-    float t2 = theta/2.0f;
-    float sqt2 = sq(t2);
+    const float t2 = theta/2.0f;
+    const float sqt2 = sq(t2);
     float st2 = t2-sqt2*t2/6.0f;
     st2 /= theta;
 
     //"rotation quaternion"
-    float w2 = 1.0f-(sqt2/2.0f)+sq(sqt2)/24.0f;
-    float x2 = v.x * st2;
-    float y2 = v.y * st2;
-    float z2 = v.z * st2;
+    const float w2 = 1.0f-(sqt2/2.0f)+sq(sqt2)/24.0f;
+    const float x2 = v.x * st2;
+    const float y2 = v.y * st2;
+    const float z2 = v.z * st2;
 
     //copy our quaternion
-    float w1 = q1;
-    float x1 = q2;
-    float y1 = q3;
-    float z1 = q4;
+    const float w1 = q1;
+    const float x1 = q2;
+    const float y1 = q3;
+    const float z1 = q4;
 
     //do the multiply into our quaternion
     q1 = w1*w2 - x1*x2 - y1*y2 - z1*z2;
@@ -296,9 +296,9 @@ Quaternion Quaternion::inverse(void) const
 
 void Quaternion::normalize(void)
 {
-    float quatMag = length();
+    const float quatMag = length();
     if (!is_zero(quatMag)) {
-        float quatMagInv = 1.0f/quatMag;
+        const float quatMagInv = 1.0f/quatMag;
         q1 *= quatMagInv;
         q2 *= quatMagInv;
         q3 *= quatMagInv;
@@ -314,10 +314,10 @@ Quaternion Quaternion::operator*(const Quaternion &v) const
     const float &y1 = q3;
     const float &z1 = q4;
 
-    float w2 = v.q1;
-    float x2 = v.q2;
-    float y2 = v.q3;
-    float z2 = v.q4;
+    const float w2 = v.q1;
+    const float x2 = v.q2;
+    const float y2 = v.q3;
+    const float z2 = v.q4;
 
     ret.q1 = w1*w2 - x1*x2 - y1*y2 - z1*z2;
     ret.q2 = w1*x2 + x1*w2 + y1*z2 - z1*y2;
@@ -329,15 +329,15 @@ Quaternion Quaternion::operator*(const Quaternion &v) const
 
 Quaternion &Quaternion::operator*=(const Quaternion &v)
 {
-    float w1 = q1;
-    float x1 = q2;
-    float y1 = q3;
-    float z1 = q4;
+    const float w1 = q1;
+    const float x1 = q2;
+    const float y1 = q3;
+    const float z1 = q4;
 
-    float w2 = v.q1;
-    float x2 = v.q2;
-    float y2 = v.q3;
-    float z2 = v.q4;
+    const float w2 = v.q1;
+    const float x2 = v.q2;
+    const float y2 = v.q3;
+    const float z2 = v.q4;
 
     q1 = w1*w2 - x1*x2 - y1*y2 - z1*z2;
     q2 = w1*x2 + x1*w2 + y1*z2 - z1*y2;
@@ -355,10 +355,10 @@ Quaternion Quaternion::operator/(const Quaternion &v) const
     const float &quat2 = q3;
     const float &quat3 = q4;
 
-    float rquat0 = v.q1;
-    float rquat1 = v.q2;
-    float rquat2 = v.q3;
-    float rquat3 = v.q4;
+    const float rquat0 = v.q1;
+    const float rquat1 = v.q2;
+    const float rquat2 = v.q3;
+    const float rquat3 = v.q4;
 
     ret.q1 = (rquat0*quat0 + rquat1*quat1 + rquat2*quat2 + rquat3*quat3);
     ret.q2 = (rquat0*quat1 - rquat1*quat0 - rquat2*quat3 + rquat3*quat2);

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;