diff --git a/quaternion.cc b/quaternion.cc index be772bd..134a154 100644 --- a/quaternion.cc +++ b/quaternion.cc @@ -1,6 +1,6 @@ /* LA: linear algebra C++ interface library - Copyright (C) 2008-2020 Jiri Pittner or + Copyright (C) 2020 Jiri Pittner or This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by @@ -186,6 +186,7 @@ return derivative; } + //optionally skip this for microcontrollers if not needed //note that C++ standard headers automatically use float versions of the goniometric functions for T=float template diff --git a/quaternion.h b/quaternion.h index dd2205a..a9db23f 100644 --- a/quaternion.h +++ b/quaternion.h @@ -1,6 +1,6 @@ /* LA: linear algebra C++ interface library - Copyright (C) 2008-2020 Jiri Pittner or + Copyright (C) 2020 Jiri Pittner or This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by diff --git a/vecmat3.cc b/vecmat3.cc index fc51749..cdb1e19 100644 --- a/vecmat3.cc +++ b/vecmat3.cc @@ -1,6 +1,6 @@ /* LA: linear algebra C++ interface library - Copyright (C) 2008-2020 Jiri Pittner or + Copyright (C) 2020 Jiri Pittner or This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by @@ -20,6 +20,33 @@ using namespace LA_Vecmat3; + +//http://en.wikipedia.org/wiki/Fast_inverse_square_root +float LA_Vecmat3::fast_sqrtinv(float number ) +{ + long i; + float x2, y; + const float threehalfs = 1.5F; + + x2 = number * 0.5F; + y = number; + i = * ( long * ) &y; // evil floating point bit level hacking + i = 0x5f3759df - ( i >> 1 ); // what the fuck? + y = * ( float * ) &i; + y = y * ( threehalfs - ( x2 * y * y ) ); // 1st iteration + y = y * ( threehalfs - ( x2 * y * y ) ); // 2nd iteration, this can be removed + + return y; +} + + +template<> +Vec3 & Vec3::fast_normalize(void) {*this *= fast_sqrtinv(normsqr()); return *this;}; + +template +Vec3& Vec3::fast_normalize(void) {normalize(); return *this;}; + + template const Vec3 Vec3::operator*(const Mat3 &rhs) const { diff --git a/vecmat3.h b/vecmat3.h index 3c85de2..5891989 100644 --- a/vecmat3.h +++ b/vecmat3.h @@ -1,6 +1,6 @@ /* LA: linear algebra C++ interface library - Copyright (C) 2008-2020 Jiri Pittner or + Copyright (C) 2020 Jiri Pittner or This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by @@ -33,6 +33,8 @@ namespace LA_Vecmat3 { +float fast_sqrtinv(float); + //forward declaration template class Mat3; @@ -60,6 +62,7 @@ public: inline T& operator[](const int i) {return q[i];}; //operations of Vec3s with scalars + void clear() {memset(q,0,3*sizeof(T));} Vec3& operator*=(const T rhs) {q[0]*=rhs; q[1]*=rhs; q[2]*=rhs; return *this;}; Vec3& operator/=(const T rhs) {return *this *= ((T)1/rhs);}; const Vec3 operator*(const T rhs) const {return Vec3(*this) *= rhs;}; @@ -76,6 +79,7 @@ public: T normsqr(void) const {return dot(*this);}; T norm(void) const {return sqrt(normsqr());}; Vec3& normalize(void) {*this /= norm(); return *this;}; + Vec3& fast_normalize(void); const Vec3 operator*(const Mat3 &rhs) const; //C-style IO void fprintf(FILE *f, const char *format) const {::fprintf(f,format,q[0],q[1],q[2]);}; @@ -113,6 +117,7 @@ public: //operations of Mat3s with scalars + void clear() {memset(&q[0][0],0,9*sizeof(T));} Mat3& operator+=(const T rhs) {q[0][0]+=rhs; q[1][1]+=rhs; q[2][2]+=rhs; return *this;}; Mat3& operator-=(const T rhs) {q[0][0]-=rhs; q[1][1]-=rhs; q[2][2]-=rhs; return *this;}; const Mat3 operator+(const T rhs) const {return Mat3(*this) += rhs;};