diff --git a/libraries/AP_Math/AP_Math.h b/libraries/AP_Math/AP_Math.h index 3bf86f3191..a5b2022638 100644 --- a/libraries/AP_Math/AP_Math.h +++ b/libraries/AP_Math/AP_Math.h @@ -48,19 +48,22 @@ template float safe_sqrt(const T v); // return determinant of square matrix -float detnxn(const float C[], const uint8_t n); +float detnxn(const float C[], const uint8_t n); // Output inverted nxn matrix when returns true, otherwise matrix is Singular -bool inversenxn(const float x[], float y[], const uint8_t n); +bool inversenxn(const float x[], float y[], const uint8_t n); // 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[]); // 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[]); // matrix multiplication of two NxN matrices -float* mat_mul(float *A, float *B, uint8_t n); +float *mat_mul(float *A, float *B, uint8_t n); + +// matrix algebra +bool inverse(float x[], float y[], uint16_t dim); /* * Constrain an angle to be within the range: -180 to 180 degrees. The second @@ -112,18 +115,16 @@ auto const constrain_float = &constrain_value; auto const constrain_int16 = &constrain_value; auto const constrain_int32 = &constrain_value; - -//matrix algebra -bool inverse(float x[], float y[], uint16_t dim); - // degrees -> radians -static inline float radians(float deg) { - return deg * DEG_TO_RAD; +static inline float radians(float deg) +{ + return deg * DEG_TO_RAD; } // radians -> degrees -static inline float degrees(float rad) { - return rad * RAD_TO_DEG; +static inline float degrees(float rad) +{ + return rad * RAD_TO_DEG; } template @@ -153,12 +154,14 @@ float norm(const T first, const Params... parameters) } template -static inline auto MIN(const A &one, const B &two) -> decltype(one < two ? one : two) { +static inline auto MIN(const A &one, const B &two) -> decltype(one < two ? one : two) +{ return one < two ? one : two; } template -static inline auto MAX(const A &one, const B &two) -> decltype(one > two ? one : two) { +static inline auto MAX(const A &one, const B &two) -> decltype(one > two ? one : two) +{ return one > two ? one : two; } @@ -198,4 +201,3 @@ inline uint32_t usec_to_hz(uint32_t usec) float linear_interpolate(float low_output, float high_output, float var_value, float var_low, float var_high); -