|
|
|
@ -48,19 +48,22 @@ template <class T>
@@ -48,19 +48,22 @@ template <class T>
|
|
|
|
|
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<float>;
@@ -112,18 +115,16 @@ auto const constrain_float = &constrain_value<float>;
|
|
|
|
|
auto const constrain_int16 = &constrain_value<int16_t>; |
|
|
|
|
auto const constrain_int32 = &constrain_value<int32_t>; |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
//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<class T> |
|
|
|
@ -153,12 +154,14 @@ float norm(const T first, const Params... parameters)
@@ -153,12 +154,14 @@ float norm(const T first, const Params... parameters)
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
template<typename A, typename B> |
|
|
|
|
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<typename A, typename B> |
|
|
|
|
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)
@@ -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); |
|
|
|
|
|
|
|
|
|