diff --git a/libraries/AP_Math/AP_Math.h b/libraries/AP_Math/AP_Math.h index 64881f994e..3f841ceba9 100644 --- a/libraries/AP_Math/AP_Math.h +++ b/libraries/AP_Math/AP_Math.h @@ -82,16 +82,16 @@ template float safe_sqrt(const T v); // invOut is an inverted 4x4 matrix when returns true, otherwise matrix is Singular -bool inverse3x3(float m[], float invOut[]); +bool inverse3x3(float m[], float invOut[]) WARN_IF_UNUSED; // invOut is an inverted 3x3 matrix when returns true, otherwise matrix is Singular -bool inverse4x4(float m[],float invOut[]); +bool inverse4x4(float m[],float invOut[]) WARN_IF_UNUSED; // matrix multiplication of two NxN matrices float *mat_mul(float *A, float *B, uint8_t n); // matrix algebra -bool inverse(float x[], float y[], uint16_t dim); +bool inverse(float x[], float y[], uint16_t dim) WARN_IF_UNUSED; /* * Constrain an angle to be within the range: -180 to 180 degrees. The second @@ -270,7 +270,7 @@ float rand_float(void); Vector3f rand_vec3f(void); // confirm a value is a valid octal value -bool is_valid_octal(uint16_t octal); +bool is_valid_octal(uint16_t octal) WARN_IF_UNUSED; // return true if two rotations are equal -bool rotation_equal(enum Rotation r1, enum Rotation r2); +bool rotation_equal(enum Rotation r1, enum Rotation r2) WARN_IF_UNUSED; diff --git a/libraries/AP_Math/location.h b/libraries/AP_Math/location.h index 5dd40964fd..0d8c7e04f9 100644 --- a/libraries/AP_Math/location.h +++ b/libraries/AP_Math/location.h @@ -26,9 +26,9 @@ void wgsllh2ecef(const Vector3d &llh, Vector3d &ecef); void wgsecef2llh(const Vector3d &ecef, Vector3d &llh); // return true when lat and lng are within range -bool check_lat(float lat); -bool check_lng(float lng); -bool check_lat(int32_t lat); -bool check_lng(int32_t lng); -bool check_latlng(float lat, float lng); -bool check_latlng(int32_t lat, int32_t lng); +bool check_lat(float lat) WARN_IF_UNUSED; +bool check_lng(float lng) WARN_IF_UNUSED; +bool check_lat(int32_t lat) WARN_IF_UNUSED; +bool check_lng(int32_t lng) WARN_IF_UNUSED; +bool check_latlng(float lat, float lng) WARN_IF_UNUSED; +bool check_latlng(int32_t lat, int32_t lng) WARN_IF_UNUSED; diff --git a/libraries/AP_Math/matrix3.h b/libraries/AP_Math/matrix3.h index 3e74b807c5..80c19c3045 100644 --- a/libraries/AP_Math/matrix3.h +++ b/libraries/AP_Math/matrix3.h @@ -203,7 +203,7 @@ public: * @return If this matrix is invertible, then true is returned. Otherwise, * \p inv is unmodified and false is returned. */ - bool inverse(Matrix3& inv) const; + bool inverse(Matrix3& inv) const WARN_IF_UNUSED; /** * Invert this matrix if it is invertible. @@ -211,7 +211,7 @@ public: * @return Return true if this matrix could be successfully inverted and * false otherwise. */ - bool invert(); + bool invert() WARN_IF_UNUSED; // zero the matrix void zero(void); @@ -225,7 +225,7 @@ public: } // check if any elements are NAN - bool is_nan(void) + bool is_nan(void) WARN_IF_UNUSED { return a.is_nan() || b.is_nan() || c.is_nan(); } diff --git a/libraries/AP_Math/polygon.h b/libraries/AP_Math/polygon.h index e9399b0ea1..e85071d357 100644 --- a/libraries/AP_Math/polygon.h +++ b/libraries/AP_Math/polygon.h @@ -20,16 +20,16 @@ #include "vector2.h" template -bool Polygon_outside(const Vector2 &P, const Vector2 *V, unsigned n); +bool Polygon_outside(const Vector2 &P, const Vector2 *V, unsigned n) WARN_IF_UNUSED; template -bool Polygon_complete(const Vector2 *V, unsigned n); +bool Polygon_complete(const Vector2 *V, unsigned n) WARN_IF_UNUSED; /* determine if the polygon of N verticies defined by points V is intersected by a line from point p1 to point p2 intersection argument returns the intersection closest to p1 */ -bool Polygon_intersects(const Vector2f *V, unsigned N, const Vector2f &p1, const Vector2f &p2, Vector2f &intersection); +bool Polygon_intersects(const Vector2f *V, unsigned N, const Vector2f &p1, const Vector2f &p2, Vector2f &intersection) WARN_IF_UNUSED; /* diff --git a/libraries/AP_Math/quaternion.h b/libraries/AP_Math/quaternion.h index 2393b67ea3..81aff5c935 100644 --- a/libraries/AP_Math/quaternion.h +++ b/libraries/AP_Math/quaternion.h @@ -57,7 +57,7 @@ public: } // check if any elements are NAN - bool is_nan(void) const + bool is_nan(void) const WARN_IF_UNUSED { return isnan(q1) || isnan(q2) || isnan(q3) || isnan(q4); } diff --git a/libraries/AP_Math/vector2.h b/libraries/AP_Math/vector2.h index 01cf270490..9ad34162ad 100644 --- a/libraries/AP_Math/vector2.h +++ b/libraries/AP_Math/vector2.h @@ -30,6 +30,7 @@ #pragma once #include +#include template struct Vector2 @@ -100,13 +101,15 @@ struct Vector2 float angle(void) const; // check if any elements are NAN - bool is_nan(void) const; + bool is_nan(void) const WARN_IF_UNUSED; // check if any elements are infinity - bool is_inf(void) const; + bool is_inf(void) const WARN_IF_UNUSED; // check if all elements are zero - bool is_zero(void) const { return (fabsf(x) < FLT_EPSILON) && (fabsf(y) < FLT_EPSILON); } + bool is_zero(void) const WARN_IF_UNUSED { + return (fabsf(x) < FLT_EPSILON) && (fabsf(y) < FLT_EPSILON); + } // allow a vector2 to be used as an array, 0 indexed T & operator[](uint8_t i) { @@ -207,15 +210,15 @@ struct Vector2 // find the intersection between two line segments // returns true if they intersect, false if they do not // the point of intersection is returned in the intersection argument - static bool segment_intersection(const Vector2& seg1_start, const Vector2& seg1_end, const Vector2& seg2_start, const Vector2& seg2_end, Vector2& intersection); + static bool segment_intersection(const Vector2& seg1_start, const Vector2& seg1_end, const Vector2& seg2_start, const Vector2& seg2_end, Vector2& intersection) WARN_IF_UNUSED; // find the intersection between a line segment and a circle // returns true if they intersect and intersection argument is updated with intersection closest to seg_start - static bool circle_segment_intersection(const Vector2& seg_start, const Vector2& seg_end, const Vector2& circle_center, float radius, Vector2& intersection); + static bool circle_segment_intersection(const Vector2& seg_start, const Vector2& seg_end, const Vector2& circle_center, float radius, Vector2& intersection) WARN_IF_UNUSED; static bool point_on_segment(const Vector2& point, const Vector2& seg_start, - const Vector2& seg_end) { + const Vector2& seg_end) WARN_IF_UNUSED { const float expected_run = seg_end.x-seg_start.x; const float intersection_run = point.x-seg_start.x; // check slopes are identical: diff --git a/libraries/AP_Math/vector3.h b/libraries/AP_Math/vector3.h index 9ebe3c1b1e..dd4ca7908c 100644 --- a/libraries/AP_Math/vector3.h +++ b/libraries/AP_Math/vector3.h @@ -156,13 +156,17 @@ public: float angle(const Vector3 &v2) const; // check if any elements are NAN - bool is_nan(void) const; + bool is_nan(void) const WARN_IF_UNUSED; // check if any elements are infinity - bool is_inf(void) const; + bool is_inf(void) const WARN_IF_UNUSED; // check if all elements are zero - bool is_zero(void) const { return (fabsf(x) < FLT_EPSILON) && (fabsf(y) < FLT_EPSILON) && (fabsf(z) < FLT_EPSILON); } + bool is_zero(void) const WARN_IF_UNUSED { + return (fabsf(x) < FLT_EPSILON) && + (fabsf(y) < FLT_EPSILON) && + (fabsf(z) < FLT_EPSILON); + } // rotate by a standard rotation