include/ecef-quaternion.h

   1 /*
   2   C++ Linear Algegra Quaternion Copyright © 2017  Infinite Delta Corp
   3
   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.
   8
   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.
  13
  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/>.
  16
  17 */
  18
  19 #ifndef ECEF_QUATERNION_H
  20 #define ECEF_QUATERNION_H
  21
  22 #include "ecef-vector.h"
  23
  24 namespace ecef
  25 {
  26
  27 struct quat
  28   {
  29   double a;
  30   vec v;
  31
  32   quat() {a=0.0; v=zAxis;}
  33   quat(const double a, const vec v) {this->a=a; this->v=v;}
  34
  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);}
  38
  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
  44
  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
  47   };
  48
  49 }
  50 #endif