vector.h

00001 /*
00002     Inertial Measurement Unit Maths Library
00003     Copyright (C) 2013-2014  Samuel Cowen
00004     www.camelsoftware.com
00005 
00006     This program is free software: you can redistribute it and/or modify
00007     it under the terms of the GNU General Public License as published by
00008     the Free Software Foundation, either version 3 of the License, or
00009     (at your option) any later version.
00010 
00011     This program is distributed in the hope that it will be useful,
00012     but WITHOUT ANY WARRANTY; without even the implied warranty of
00013     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00014     GNU General Public License for more details.
00015 
00016     You should have received a copy of the GNU General Public License
00017     along with this program.  If not, see <http://www.gnu.org/licenses/>.
00018 */
00019 
00020 #ifndef IMUMATH_VECTOR_HPP
00021 #define IMUMATH_VECTOR_HPP
00022 
00023 #include <stdlib.h>
00024 #include <string.h>
00025 #include <stdint.h>
00026 #include <math.h>
00027 
00028 
00029 namespace imu
00030 {
00031 
00032 template <uint8_t N> class Vector
00033 {
00034 public:
00035     Vector()
00036     {
00037         memset(p_vec, 0, sizeof(double)*N);
00038     }
00039 
00040     Vector(double a)
00041     {
00042         memset(p_vec, 0, sizeof(double)*N);
00043         p_vec[0] = a;
00044     }
00045 
00046     Vector(double a, double b)
00047     {
00048         memset(p_vec, 0, sizeof(double)*N);
00049         p_vec[0] = a;
00050         p_vec[1] = b;
00051     }
00052 
00053     Vector(double a, double b, double c)
00054     {
00055         memset(p_vec, 0, sizeof(double)*N);
00056         p_vec[0] = a;
00057         p_vec[1] = b;
00058         p_vec[2] = c;
00059     }
00060 
00061     Vector(double a, double b, double c, double d)
00062     {
00063         memset(p_vec, 0, sizeof(double)*N);
00064         p_vec[0] = a;
00065         p_vec[1] = b;
00066         p_vec[2] = c;
00067         p_vec[3] = d;
00068     }
00069 
00070     Vector(const Vector<N> &v)
00071     {
00072         for (int x = 0; x < N; x++)
00073             p_vec[x] = v.p_vec[x];
00074     }
00075 
00076     ~Vector()
00077     {
00078     }
00079 
00080     uint8_t n() { return N; }
00081 
00082     double magnitude()
00083     {
00084         double res = 0;
00085         int i;
00086         for(i = 0; i < N; i++)
00087             res += (p_vec[i] * p_vec[i]);
00088 
00089         if(isnan(res))
00090             return 0;
00091         if((fabs(res-1)) >= 0.000001) // Avoid a sqrt if possible.
00092             return sqrt(res);
00093         return 1;
00094     }
00095 
00096     void normalize()
00097     {
00098         double mag = magnitude();
00099         if(abs(mag) <= 0.0001)
00100             return;
00101 
00102         int i;
00103         for(i = 0; i < N; i++)
00104             p_vec[i] = p_vec[i]/mag;
00105     }
00106 
00107     double dot(Vector v)
00108     {
00109         double ret = 0;
00110         int i;
00111         for(i = 0; i < N; i++)
00112             ret += p_vec[i] * v.p_vec[i];
00113 
00114         return ret;
00115     }
00116 
00117     Vector cross(Vector v)
00118     {
00119         Vector ret;
00120 
00121         // The cross product is only valid for vectors with 3 dimensions,
00122         // with the exception of higher dimensional stuff that is beyond the intended scope of this library
00123         if(N != 3)
00124             return ret;
00125 
00126         ret.p_vec[0] = (p_vec[1] * v.p_vec[2]) - (p_vec[2] * v.p_vec[1]);
00127         ret.p_vec[1] = (p_vec[2] * v.p_vec[0]) - (p_vec[0] * v.p_vec[2]);
00128         ret.p_vec[2] = (p_vec[0] * v.p_vec[1]) - (p_vec[1] * v.p_vec[0]);
00129         return ret;
00130     }
00131 
00132     Vector scale(double scalar) const
00133     {
00134         Vector ret;
00135         for(int i = 0; i < N; i++)
00136             ret.p_vec[i] = p_vec[i] * scalar;
00137         return ret;
00138     }
00139 
00140     Vector invert() const
00141     {
00142         Vector ret;
00143         for(int i = 0; i < N; i++)
00144             ret.p_vec[i] = -p_vec[i];
00145         return ret;
00146     }
00147 
00148     Vector operator = (Vector v)
00149     {
00150         for (int x = 0; x < N; x++ )
00151             p_vec[x] = v.p_vec[x];
00152         return *this;
00153     }
00154 
00155     double& operator [](int n)
00156     {
00157         return p_vec[n];
00158     }
00159 
00160     double operator [](int n) const
00161     {
00162         return p_vec[n];
00163     }
00164 
00165     double& operator ()(int n)
00166     {
00167         return p_vec[n];
00168     }
00169 
00170     double operator ()(int n) const
00171     {
00172         return p_vec[n];
00173     }
00174 
00175     Vector operator + (Vector v) const
00176     {
00177         Vector ret;
00178         for(int i = 0; i < N; i++)
00179             ret.p_vec[i] = p_vec[i] + v.p_vec[i];
00180         return ret;
00181     }
00182 
00183     Vector operator - (Vector v) const
00184     {
00185         Vector ret;
00186         for(int i = 0; i < N; i++)
00187             ret.p_vec[i] = p_vec[i] - v.p_vec[i];
00188         return ret;
00189     }
00190 
00191     Vector operator * (double scalar) const
00192     {
00193         return scale(scalar);
00194     }
00195 
00196     Vector operator / (double scalar) const
00197     {
00198         Vector ret;
00199         for(int i = 0; i < N; i++)
00200             ret.p_vec[i] = p_vec[i] / scalar;
00201         return ret;
00202     }
00203 
00204     void toDegrees()
00205     {
00206         for(int i = 0; i < N; i++)
00207             p_vec[i] *= 57.2957795131; //180/pi
00208     }
00209 
00210     void toRadians()
00211     {
00212         for(int i = 0; i < N; i++)
00213             p_vec[i] *= 0.01745329251;  //pi/180
00214     }
00215 
00216     double& x() { return p_vec[0]; }
00217     double& y() { return p_vec[1]; }
00218     double& z() { return p_vec[2]; }
00219     double x() const { return p_vec[0]; }
00220     double y() const { return p_vec[1]; }
00221     double z() const { return p_vec[2]; }
00222 
00223 
00224 private:
00225     double  p_vec[N];
00226 };
00227 
00228 
00229 };
00230 
00231 #endif