include/ecef-vector.h

   1 /*
   2   C++ Linear Algegra Vector Copyright © 2013-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_VECTOR_H
  20 #define ECEF_VECTOR_H
  21
  22 #include <math.h>
  23
  24 namespace ecef
  25 {
  26 const double toleranceLimit = 0.0000001;
  27 inline double sqr(const double a) {return a*a;}
  28 inline bool tolerance(const double d) {return d <= toleranceLimit;}
  29 inline double abs(const double a) {return (a>=0) ? a : -a;}
  30 inline double sign(const double a) {return (a>=0) ? 1 : -1;}
  31
  32 struct vec
  33   {
  34   double x, y, z;
  35
  36   vec() {x=y=z=0.0;};
  37   vec(const double x, const double y, const double z) {this->x=x; this->y=y; this->z=z;};
  38
  39   double distSqr(const vec v=vec()) const {return sqr(x-v.x)+sqr(y-v.y)+sqr(z-v.z);}
  40   double dist(const vec v=vec()) const {return sqrt(distSqr(v));}
  41   double distZ() const {return vec(x,y,0).dist();}
  42
  43   bool operator==(const vec v) const {return tolerance(distSqr(v));}
  44   bool operator!=(const vec v) const {return !tolerance(distSqr(v));}
  45
  46   vec operator+(const vec v) const {return vec(x+v.x, y+v.y, z+v.z);}
  47   vec operator-(const vec v) const {return vec(x-v.x, y-v.y, z-v.z);}
  48   vec operator*(const vec v) const {return vec(x*v.x, y*v.y, z*v.z);}
  49   vec operator*(const double s) const {return vec(x*s, y*s, z*s);}
  50   vec operator/(const double s) const {return vec(x/s, y/s, z/s);}
  51   vec operator-() const {return vec(-x, -y, -z);}
  52
  53   double dot(const vec v) const {return x*v.x + y*v.y + z*v.z;}
  54   vec cross(const vec v) const {return vec(y*v.z - z*v.y, z*v.x - x*v.z, x*v.y - y*v.x);}
  55   vec doubleCross(const vec v) const {return cross(v).cross(-v);} // http://en.wikipedia.org/wiki/Dot_product#Triple_product_expansion
  56   double cos(const vec v) const {return dot(v)/(dist()*v.dist());}
  57   double mag() const {return sqrt(x*x + y*y + z*z);}
  58   vec norm(const double s=1) const {double m = mag(); return tolerance(m) ? vec(0,0,1) * s : *this * s / m;}
  59   };
  60
  61 const vec zAxis(0,0,1);
  62
  63 struct line
  64   {
  65   vec p[2];     // Two points define a line
  66
  67   line(const vec end) {p[0]=vec(); p[1]=end;}
  68   line(const vec start, const vec end) {p[0]=start; p[1]=end;}
  69   };
  70 }
  71 #endif