diff --git a/.github/releaseBody.md b/.github/releaseBody.md index d305bec..c4e0ad0 100644 --- a/.github/releaseBody.md +++ b/.github/releaseBody.md @@ -1,7 +1,6 @@ -## Added new operations to basicMath functions +## Minor improvement to quaternion.c -**New features:** -- Added bitwise operations macros to `basicMath` -- Added `fastSin` and `fastCos` to `basicMath` +**Improvements:** +- Changed `quaternion.c` to give the user the possiblity to choose whether to use fast or standard math functions through the addition of `USE_FAST_MATH` to compile definitions See [Changelog](Changelog.md) \ No newline at end of file diff --git a/Changelog.md b/Changelog.md index 8587b2c..347bdc9 100644 --- a/Changelog.md +++ b/Changelog.md @@ -1,5 +1,10 @@ # Changelog +## v1.10.1 + +**Improvements:** +- Changed `quaternion.c` to give the user the possiblity to choose whether to use fast or standard math functions through the addition of `USE_FAST_MATH` to compile definitions + ## v1.10.0 **New features:** diff --git a/Readme.md b/Readme.md index fda4496..339dd82 100644 --- a/Readme.md +++ b/Readme.md @@ -32,6 +32,7 @@ - `ADVUTILS_MALLOC` - `ADVUTILS_CALLOC` - `ADVUTILS_FREE` +- User can select to use fast math functions from `basicMath` by adding `USE_FAST_MATH` to compile definitions diff --git a/src/quaternion.c b/src/quaternion.c index 51da19a..00d6b35 100644 --- a/src/quaternion.c +++ b/src/quaternion.c @@ -39,16 +39,20 @@ /* Macros --------------------------------------------------------------------*/ -#define PI_2 1.57 +#define PI_2 constPI * 0.5f -/* Private variables ---------------------------------------------------------*/ +#ifdef USE_FAST_MATH +#define INVSQRT(x) fastInvSqrt(x); +#else +#define INVSQRT(x) 1.0f / sqrtf(x); +#endif /* USE_FAST_MATH */ /* Functions -----------------------------------------------------------------*/ void quaternionNorm(quaternion_t* q) { float inv_norm; - inv_norm = fastInvSqrt(q->q0 * q->q0 + q->q1 * q->q1 + q->q2 * q->q2 + q->q3 * q->q3); + inv_norm = INVSQRT(q->q0 * q->q0 + q->q1 * q->q1 + q->q2 * q->q2 + q->q3 * q->q3); if (isnan(inv_norm) || isinf(inv_norm)) { inv_norm = 1.f; } @@ -134,7 +138,7 @@ void quaternionToEuler(quaternion_t* qr, axis3f_t* ea) { ea->x = atan2f(dq0q1 + dq2q3, q0q0 + q3q3 - q1q1 - q2q2); ea->y = asinf(dq0q2 - dq1q3); - /* This part is needed to manage angle >90 deg */ + /* This part is needed to manage angle >90 deg */ #ifdef AVOID_GIMBAL_LOCK if (ea->x > PI_2 || ea->x < -PI_2) { ea->x = ea_pre.x;