2 C++ Linear Algegra Quaternion Copyright © 2017 Infinite Delta Corp
4 This program is free software: you can redistribute it and/or modify
5 it under the terms of the GNU Lesser General Public License as published by
6 the Free Software Foundation, either version 2 of the License, or
7 (at your option) any later version.
9 This program is distributed in the hope that it will be useful,
10 but WITHOUT ANY WARRANTY; without even the implied warranty of
11 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 GNU General Public License for more details.
14 You should have received a copy of the GNU Lesser General Public License
15 along with this program. If not, see <http://www.gnu.org/licenses/>.
19 #ifndef ECEF_QUATERNION_H
20 #define ECEF_QUATERNION_H
22 #include "ecef-vector.h"
29 double a; // cos(Angle / 2)
32 quat() {a=0.0; v=zAxis;}
33 quat(const double a, const vec v) {this->a=a; this->v=v;}
34 quat(const vec v1, const vec v2) {a=v1.dot(v2); v=zAxis; if (abs(a)<(1-toleranceLimit)) {a=sqrt((1+a)/2); v=v1.cross(v2);}}
36 quat operator*(const quat q) const {return quat(a*q.a - v.dot(q.v), q.v*a + v*q.a + v.cross(q.v));}
37 quat operator*(const double s) const {return quat(a*s, v*s);}
38 quat operator/(const double s) const {return quat(a/s, v/s);}
40 quat conj() const {return quat(a, -v);}
41 // double magSqr() const {return a*a + v.x*v.x + v.y*v.y + v.z*v.z;}
42 double mag() const {double ms=a*a + v.x*v.x + v.y*v.y + v.z*v.z; return tolerance(ms) ? 0.0 : sqrt(ms);}
43 quat versor() const {double m=mag(); return tolerance(m) ? quat(0, zAxis) : *this/m;} // Unit Quaternion
44 quat normVec() const {double s=a*a; s= a<=1 ? sqrt(1-s) : 0; return quat(a, v.norm(s));} // Normalize Quaternion by only scalling the vector
46 vec operator*(const vec v2) const {return (*this * quat(0,v2) * conj()).v;} // Rotate vector by quaternion (32nS on 2.1 GHz Intel E6420)