Statistics
| Branch: | Revision:

adafruit_bno055 / utility / vector.h @ 67f3cff5

History | View | Annotate | Download (4.729 KB)

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

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