Statistics
| Branch: | Revision:

adafruit_bno055 / utility / vector.h @ f12bf4b5

History | View | Annotate | Download (4.79 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 4bc1c0c1 Kevin Townsend
        memset(p_vec, 0, sizeof(double)*N);
38 d964148c Tony DiCola
    }
39 4bc1c0c1 Kevin Townsend
40 d964148c Tony DiCola
    Vector(double a)
41
    {
42 4bc1c0c1 Kevin Townsend
        memset(p_vec, 0, sizeof(double)*N);
43 d964148c Tony DiCola
        p_vec[0] = a;
44
    }
45 4bc1c0c1 Kevin Townsend
46 d964148c Tony DiCola
    Vector(double a, double b)
47
    {
48 4bc1c0c1 Kevin Townsend
        memset(p_vec, 0, sizeof(double)*N);
49 d964148c Tony DiCola
        p_vec[0] = a;
50
        p_vec[1] = b;
51
    }
52 4bc1c0c1 Kevin Townsend
53 d964148c Tony DiCola
    Vector(double a, double b, double c)
54
    {
55 4bc1c0c1 Kevin Townsend
        memset(p_vec, 0, sizeof(double)*N);
56 d964148c Tony DiCola
        p_vec[0] = a;
57
        p_vec[1] = b;
58
        p_vec[2] = c;
59
    }
60 4bc1c0c1 Kevin Townsend
61
    Vector(double a, double b, double c, double d)
62
    {
63
        memset(p_vec, 0, sizeof(double)*N);
64
        p_vec[0] = a;
65 d964148c Tony DiCola
        p_vec[1] = b;
66
        p_vec[2] = c;
67
        p_vec[3] = d;
68 4bc1c0c1 Kevin Townsend
    }
69
70
    Vector(const Vector<N> &v)
71
    {
72 b2d499c7 Paul Du Bois (laptop)
        for (int x = 0; x < N; x++)
73 4bc1c0c1 Kevin Townsend
            p_vec[x] = v.p_vec[x];
74
    }
75
76
    ~Vector()
77
    {
78
    }
79
80
    uint8_t n() { return N; }
81
82
    double magnitude()
83
    {
84
        double res = 0;
85
        int i;
86
        for(i = 0; i < N; i++)
87
            res += (p_vec[i] * p_vec[i]);
88
89
        if(isnan(res))
90
            return 0;
91 abce6607 Paul Du Bois (laptop)
        if((fabs(res-1)) >= 0.000001) // Avoid a sqrt if possible.
92 4bc1c0c1 Kevin Townsend
            return sqrt(res);
93
        return 1;
94
    }
95
96
    void normalize()
97
    {
98
        double mag = magnitude();
99
        if(abs(mag) <= 0.0001)
100
            return;
101
102
        int i;
103
        for(i = 0; i < N; i++)
104
            p_vec[i] = p_vec[i]/mag;
105
    }
106
107
    double dot(Vector v)
108
    {
109
        double ret = 0;
110
        int i;
111
        for(i = 0; i < N; i++)
112
            ret += p_vec[i] * v.p_vec[i];
113
114
        return ret;
115
    }
116
117
    Vector cross(Vector v)
118
    {
119
        Vector ret;
120
121 0695bf91 Paul Du Bois (laptop)
        // The cross product is only valid for vectors with 3 dimensions,
122
        // with the exception of higher dimensional stuff that is beyond the intended scope of this library
123 4bc1c0c1 Kevin Townsend
        if(N != 3)
124
            return ret;
125
126
        ret.p_vec[0] = (p_vec[1] * v.p_vec[2]) - (p_vec[2] * v.p_vec[1]);
127
        ret.p_vec[1] = (p_vec[2] * v.p_vec[0]) - (p_vec[0] * v.p_vec[2]);
128
        ret.p_vec[2] = (p_vec[0] * v.p_vec[1]) - (p_vec[1] * v.p_vec[0]);
129
        return ret;
130
    }
131
132 0695bf91 Paul Du Bois (laptop)
    Vector scale(double scalar) const
133 4bc1c0c1 Kevin Townsend
    {
134
        Vector ret;
135
        for(int i = 0; i < N; i++)
136
            ret.p_vec[i] = p_vec[i] * scalar;
137
        return ret;
138
    }
139
140 0695bf91 Paul Du Bois (laptop)
    Vector invert() const
141 4bc1c0c1 Kevin Townsend
    {
142
        Vector ret;
143
        for(int i = 0; i < N; i++)
144
            ret.p_vec[i] = -p_vec[i];
145
        return ret;
146
    }
147
148
    Vector operator = (Vector v)
149
    {
150
        for (int x = 0; x < N; x++ )
151
            p_vec[x] = v.p_vec[x];
152 d964148c Tony DiCola
        return *this;
153 4bc1c0c1 Kevin Townsend
    }
154
155
    double& operator [](int n)
156
    {
157
        return p_vec[n];
158
    }
159
160 0695bf91 Paul Du Bois (laptop)
    double operator [](int n) const
161
    {
162
        return p_vec[n];
163
    }
164
165 4bc1c0c1 Kevin Townsend
    double& operator ()(int n)
166
    {
167
        return p_vec[n];
168
    }
169
170 0695bf91 Paul Du Bois (laptop)
    double operator ()(int n) const
171
    {
172
        return p_vec[n];
173
    }
174
175
    Vector operator + (Vector v) const
176 4bc1c0c1 Kevin Townsend
    {
177
        Vector ret;
178
        for(int i = 0; i < N; i++)
179
            ret.p_vec[i] = p_vec[i] + v.p_vec[i];
180
        return ret;
181
    }
182
183 0695bf91 Paul Du Bois (laptop)
    Vector operator - (Vector v) const
184 4bc1c0c1 Kevin Townsend
    {
185
        Vector ret;
186
        for(int i = 0; i < N; i++)
187
            ret.p_vec[i] = p_vec[i] - v.p_vec[i];
188
        return ret;
189
    }
190
191 0695bf91 Paul Du Bois (laptop)
    Vector operator * (double scalar) const
192 4bc1c0c1 Kevin Townsend
    {
193
        return scale(scalar);
194
    }
195
196 0695bf91 Paul Du Bois (laptop)
    Vector operator / (double scalar) const
197 4bc1c0c1 Kevin Townsend
    {
198
        Vector ret;
199
        for(int i = 0; i < N; i++)
200
            ret.p_vec[i] = p_vec[i] / scalar;
201
        return ret;
202
    }
203
204
    void toDegrees()
205
    {
206
        for(int i = 0; i < N; i++)
207
            p_vec[i] *= 57.2957795131; //180/pi
208
    }
209
210
    void toRadians()
211
    {
212
        for(int i = 0; i < N; i++)
213
            p_vec[i] *= 0.01745329251;  //pi/180
214
    }
215
216
    double& x() { return p_vec[0]; }
217
    double& y() { return p_vec[1]; }
218
    double& z() { return p_vec[2]; }
219 0695bf91 Paul Du Bois (laptop)
    double x() const { return p_vec[0]; }
220
    double y() const { return p_vec[1]; }
221
    double z() const { return p_vec[2]; }
222 4bc1c0c1 Kevin Townsend
223
224
private:
225 b2d499c7 Paul Du Bois (laptop)
    double  p_vec[N];
226 4bc1c0c1 Kevin Townsend
};
227
228
229
};
230
231
#endif