Statistics
| Branch: | Revision:

adafruit_bno055 / utility / vector.h @ 9474f653

History | View | Annotate | Download (4.74 KB)

1 4bc1c0c1 Kevin Townsend
/*
2
    Inertial Measurement Unit Maths Library
3
    Copyright (C) 2013-2014  Samuel Cowen
4 d964148c Tony DiCola
    www.camelsoftware.com
5 4bc1c0c1 Kevin Townsend

6
    This program is free software: you can redistribute it and/or modify
7
    it under the terms of the GNU General Public License as published by
8
    the Free Software Foundation, either version 3 of the License, or
9
    (at your option) any later version.
10

11
    This program is distributed in the hope that it will be useful,
12
    but WITHOUT ANY WARRANTY; without even the implied warranty of
13
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
14
    GNU General Public License for more details.
15

16
    You should have received a copy of the GNU General Public License
17
    along with this program.  If not, see <http://www.gnu.org/licenses/>.
18
*/
19
20
#ifndef IMUMATH_VECTOR_HPP
21
#define IMUMATH_VECTOR_HPP
22
23
#include <stdlib.h>
24
#include <string.h>
25
#include <stdint.h>
26
#include <math.h>
27
28
29
namespace imu
30
{
31
32
template <uint8_t N> class Vector
33
{
34
public:
35 d964148c Tony DiCola
    Vector()
36
    {
37
        p_vec = &p_vec_data[0];
38 4bc1c0c1 Kevin Townsend
        memset(p_vec, 0, sizeof(double)*N);
39 d964148c Tony DiCola
    }
40 4bc1c0c1 Kevin Townsend
41 d964148c Tony DiCola
    Vector(double a)
42
    {
43
        p_vec = &p_vec_data[0];
44 4bc1c0c1 Kevin Townsend
        memset(p_vec, 0, sizeof(double)*N);
45 d964148c Tony DiCola
        p_vec[0] = a;
46
    }
47 4bc1c0c1 Kevin Townsend
48 d964148c Tony DiCola
    Vector(double a, double b)
49
    {
50
        p_vec = &p_vec_data[0];
51 4bc1c0c1 Kevin Townsend
        memset(p_vec, 0, sizeof(double)*N);
52 d964148c Tony DiCola
        p_vec[0] = a;
53
        p_vec[1] = b;
54
    }
55 4bc1c0c1 Kevin Townsend
56 d964148c Tony DiCola
    Vector(double a, double b, double c)
57
    {
58
        p_vec = &p_vec_data[0];
59 4bc1c0c1 Kevin Townsend
        memset(p_vec, 0, sizeof(double)*N);
60 d964148c Tony DiCola
        p_vec[0] = a;
61
        p_vec[1] = b;
62
        p_vec[2] = c;
63
    }
64 4bc1c0c1 Kevin Townsend
65
    Vector(double a, double b, double c, double d)
66
    {
67 d964148c Tony DiCola
        p_vec = &p_vec_data[0];
68 4bc1c0c1 Kevin Townsend
        memset(p_vec, 0, sizeof(double)*N);
69
        p_vec[0] = a;
70 d964148c Tony DiCola
        p_vec[1] = b;
71
        p_vec[2] = c;
72
        p_vec[3] = d;
73 4bc1c0c1 Kevin Townsend
    }
74
75
    Vector(const Vector<N> &v)
76
    {
77 d964148c Tony DiCola
        p_vec = &p_vec_data[0];
78 4bc1c0c1 Kevin Townsend
        memset(p_vec, 0, sizeof(double)*N);
79
        for (int x = 0; x < N; x++ )
80
            p_vec[x] = v.p_vec[x];
81
    }
82
83
    ~Vector()
84
    {
85
    }
86
87
    uint8_t n() { return N; }
88
89
    double magnitude()
90
    {
91
        double res = 0;
92
        int i;
93
        for(i = 0; i < N; i++)
94
            res += (p_vec[i] * p_vec[i]);
95
96
        if(isnan(res))
97
            return 0;
98
        if((fabs(res)-1) >= 0.000001) //avoid a sqrt if possible
99
            return sqrt(res);
100
        return 1;
101
    }
102
103
    void normalize()
104
    {
105
        double mag = magnitude();
106
        if(abs(mag) <= 0.0001)
107
            return;
108
109
        int i;
110
        for(i = 0; i < N; i++)
111
            p_vec[i] = p_vec[i]/mag;
112
    }
113
114
    double dot(Vector v)
115
    {
116
        double ret = 0;
117
        int i;
118
        for(i = 0; i < N; i++)
119
            ret += p_vec[i] * v.p_vec[i];
120
121
        return ret;
122
    }
123
124
    Vector cross(Vector v)
125
    {
126
        Vector ret;
127
128
         //the cross product is only valid for vectors with 3 dimensions,
129
         //with the exception of higher dimensional stuff that is beyond the intended scope of this library
130
        if(N != 3)
131
            return ret;
132
133
        ret.p_vec[0] = (p_vec[1] * v.p_vec[2]) - (p_vec[2] * v.p_vec[1]);
134
        ret.p_vec[1] = (p_vec[2] * v.p_vec[0]) - (p_vec[0] * v.p_vec[2]);
135
        ret.p_vec[2] = (p_vec[0] * v.p_vec[1]) - (p_vec[1] * v.p_vec[0]);
136
        return ret;
137
    }
138
139
    Vector scale(double scalar)
140
    {
141
        Vector ret;
142
        for(int i = 0; i < N; i++)
143
            ret.p_vec[i] = p_vec[i] * scalar;
144
        return ret;
145
    }
146
147
    Vector invert()
148
    {
149
        Vector ret;
150
        for(int i = 0; i < N; i++)
151
            ret.p_vec[i] = -p_vec[i];
152
        return ret;
153
    }
154
155
    Vector operator = (Vector v)
156
    {
157
        for (int x = 0; x < N; x++ )
158
            p_vec[x] = v.p_vec[x];
159 d964148c Tony DiCola
        return *this;
160 4bc1c0c1 Kevin Townsend
    }
161
162
    double& operator [](int n)
163
    {
164
        return p_vec[n];
165
    }
166
167
    double& operator ()(int n)
168
    {
169
        return p_vec[n];
170
    }
171
172
    Vector operator + (Vector v)
173
    {
174
        Vector ret;
175
        for(int i = 0; i < N; i++)
176
            ret.p_vec[i] = p_vec[i] + v.p_vec[i];
177
        return ret;
178
    }
179
180
    Vector operator - (Vector v)
181
    {
182
        Vector ret;
183
        for(int i = 0; i < N; i++)
184
            ret.p_vec[i] = p_vec[i] - v.p_vec[i];
185
        return ret;
186
    }
187
188
    Vector operator * (double scalar)
189
    {
190
        return scale(scalar);
191
    }
192
193
    Vector operator / (double scalar)
194
    {
195
        Vector ret;
196
        for(int i = 0; i < N; i++)
197
            ret.p_vec[i] = p_vec[i] / scalar;
198
        return ret;
199
    }
200
201
    void toDegrees()
202
    {
203
        for(int i = 0; i < N; i++)
204
            p_vec[i] *= 57.2957795131; //180/pi
205
    }
206
207
    void toRadians()
208
    {
209
        for(int i = 0; i < N; i++)
210
            p_vec[i] *= 0.01745329251;  //pi/180
211
    }
212
213
    double& x() { return p_vec[0]; }
214
    double& y() { return p_vec[1]; }
215
    double& z() { return p_vec[2]; }
216
217
218
private:
219
    double* p_vec;
220 d964148c Tony DiCola
    double  p_vec_data[N];
221 4bc1c0c1 Kevin Townsend
};
222
223
224
};
225
226
#endif