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"
32 quat() {a=0.0; v=zAxis;}
33 quat(const double a, const vec v) {this->a=a; this->v=v;}
35 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));}
36 quat operator*(const double s) const {return quat(a*s, v*s);}
37 quat operator/(const double s) const {return quat(a/s, v/s);}
39 quat conj() const {return quat(a, -v);}
40 // double magSqr() const {return a*a + v.x*v.x + v.y*v.y + v.z*v.z;}
41 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);}
42 quat versor() const {double m=mag(); return tolerance(m) ? quat(0, zAxis) : *this/m;} // Unit Quaternion
43 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
45 // vec operator*(const vec v2) const {return (*this * quat(0,v2) * conj()).v;} // Rotate vector by quaternion
46 vec operator*(const vec v2) const {return (quat(-v.dot(v2), v2*a + v.cross(v2)) * conj()).v;} // but skip the initial mult-by-zero