Statistics
| Branch: | Revision:

adafruit_bno055 / utility / vector.h @ a64dd7d6

History | View | Annotate | Download (4.808 KB)

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

6
    Bug fixes and cleanups by Gé Vissers (gvissers@gmail.com)
7

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

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

18
    You should have received a copy of the GNU General Public License
19
    along with this program.  If not, see <http://www.gnu.org/licenses/>.
20
*/
21

    
22
#ifndef IMUMATH_VECTOR_HPP
23
#define IMUMATH_VECTOR_HPP
24

    
25
#include <string.h>
26
#include <stdint.h>
27
#include <math.h>
28

    
29

    
30
namespace imu
31
{
32

    
33
template <uint8_t N> class Vector
34
{
35
public:
36
    Vector()
37
    {
38
        memset(p_vec, 0, sizeof(double)*N);
39
    }
40

    
41
    Vector(double a)
42
    {
43
        memset(p_vec, 0, sizeof(double)*N);
44
        p_vec[0] = a;
45
    }
46

    
47
    Vector(double a, double b)
48
    {
49
        memset(p_vec, 0, sizeof(double)*N);
50
        p_vec[0] = a;
51
        p_vec[1] = b;
52
    }
53

    
54
    Vector(double a, double b, double c)
55
    {
56
        memset(p_vec, 0, sizeof(double)*N);
57
        p_vec[0] = a;
58
        p_vec[1] = b;
59
        p_vec[2] = c;
60
    }
61

    
62
    Vector(double a, double b, double c, double d)
63
    {
64
        memset(p_vec, 0, sizeof(double)*N);
65
        p_vec[0] = a;
66
        p_vec[1] = b;
67
        p_vec[2] = c;
68
        p_vec[3] = d;
69
    }
70

    
71
    Vector(const Vector<N> &v)
72
    {
73
        for (int x = 0; x < N; x++)
74
            p_vec[x] = v.p_vec[x];
75
    }
76

    
77
    ~Vector()
78
    {
79
    }
80

    
81
    uint8_t n() { return N; }
82

    
83
    double magnitude() const
84
    {
85
        double res = 0;
86
        for (int i = 0; i < N; i++)
87
            res += p_vec[i] * p_vec[i];
88

    
89
        return sqrt(res);
90
    }
91

    
92
    void normalize()
93
    {
94
        double mag = magnitude();
95
        if (isnan(mag) || mag == 0.0)
96
            return;
97

    
98
        for (int i = 0; i < N; i++)
99
            p_vec[i] /= mag;
100
    }
101

    
102
    double dot(const Vector& v) const
103
    {
104
        double ret = 0;
105
        for (int i = 0; i < N; i++)
106
            ret += p_vec[i] * v.p_vec[i];
107

    
108
        return ret;
109
    }
110

    
111
    // The cross product is only valid for vectors with 3 dimensions,
112
    // with the exception of higher dimensional stuff that is beyond
113
    // the intended scope of this library.
114
    // Only a definition for N==3 is given below this class, using
115
    // cross() with another value for N will result in a link error.
116
    Vector cross(const Vector& v) const;
117

    
118
    Vector scale(double scalar) const
119
    {
120
        Vector ret;
121
        for(int i = 0; i < N; i++)
122
            ret.p_vec[i] = p_vec[i] * scalar;
123
        return ret;
124
    }
125

    
126
    Vector invert() const
127
    {
128
        Vector ret;
129
        for(int i = 0; i < N; i++)
130
            ret.p_vec[i] = -p_vec[i];
131
        return ret;
132
    }
133

    
134
    Vector& operator=(const Vector& v)
135
    {
136
        for (int x = 0; x < N; x++ )
137
            p_vec[x] = v.p_vec[x];
138
        return *this;
139
    }
140

    
141
    double& operator [](int n)
142
    {
143
        return p_vec[n];
144
    }
145

    
146
    double operator [](int n) const
147
    {
148
        return p_vec[n];
149
    }
150

    
151
    double& operator ()(int n)
152
    {
153
        return p_vec[n];
154
    }
155

    
156
    double operator ()(int n) const
157
    {
158
        return p_vec[n];
159
    }
160

    
161
    Vector operator+(const Vector& v) const
162
    {
163
        Vector ret;
164
        for(int i = 0; i < N; i++)
165
            ret.p_vec[i] = p_vec[i] + v.p_vec[i];
166
        return ret;
167
    }
168

    
169
    Vector operator-(const Vector& v) const
170
    {
171
        Vector ret;
172
        for(int i = 0; i < N; i++)
173
            ret.p_vec[i] = p_vec[i] - v.p_vec[i];
174
        return ret;
175
    }
176

    
177
    Vector operator * (double scalar) const
178
    {
179
        return scale(scalar);
180
    }
181

    
182
    Vector operator / (double scalar) const
183
    {
184
        Vector ret;
185
        for(int i = 0; i < N; i++)
186
            ret.p_vec[i] = p_vec[i] / scalar;
187
        return ret;
188
    }
189

    
190
    void toDegrees()
191
    {
192
        for(int i = 0; i < N; i++)
193
            p_vec[i] *= 57.2957795131; //180/pi
194
    }
195

    
196
    void toRadians()
197
    {
198
        for(int i = 0; i < N; i++)
199
            p_vec[i] *= 0.01745329251;  //pi/180
200
    }
201

    
202
    double& x() { return p_vec[0]; }
203
    double& y() { return p_vec[1]; }
204
    double& z() { return p_vec[2]; }
205
    double x() const { return p_vec[0]; }
206
    double y() const { return p_vec[1]; }
207
    double z() const { return p_vec[2]; }
208

    
209

    
210
private:
211
    double p_vec[N];
212
};
213

    
214

    
215
template <>
216
inline Vector<3> Vector<3>::cross(const Vector& v) const
217
{
218
    return Vector(
219
        p_vec[1] * v.p_vec[2] - p_vec[2] * v.p_vec[1],
220
        p_vec[2] * v.p_vec[0] - p_vec[0] * v.p_vec[2],
221
        p_vec[0] * v.p_vec[1] - p_vec[1] * v.p_vec[0]
222
    );
223
}
224

    
225
} // namespace
226

    
227
#endif