Statistics
| Branch: | Revision:

adafruit_bno055 / utility / vector.h @ 5dd991c7

History | View | Annotate | Download (4.808 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 b5582106 Gé Vissers
    Bug fixes and cleanups by Gé Vissers (gvissers@gmail.com)
7

8 4bc1c0c1 Kevin Townsend
    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 d964148c Tony DiCola
    Vector()
37
    {
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 4bc1c0c1 Kevin Townsend
        memset(p_vec, 0, sizeof(double)*N);
44 d964148c Tony DiCola
        p_vec[0] = a;
45
    }
46 4bc1c0c1 Kevin Townsend
47 d964148c Tony DiCola
    Vector(double a, double b)
48
    {
49 4bc1c0c1 Kevin Townsend
        memset(p_vec, 0, sizeof(double)*N);
50 d964148c Tony DiCola
        p_vec[0] = a;
51
        p_vec[1] = b;
52
    }
53 4bc1c0c1 Kevin Townsend
54 d964148c Tony DiCola
    Vector(double a, double b, double c)
55
    {
56 4bc1c0c1 Kevin Townsend
        memset(p_vec, 0, sizeof(double)*N);
57 d964148c Tony DiCola
        p_vec[0] = a;
58
        p_vec[1] = b;
59
        p_vec[2] = c;
60
    }
61 4bc1c0c1 Kevin Townsend
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 d964148c Tony DiCola
        p_vec[1] = b;
67
        p_vec[2] = c;
68
        p_vec[3] = d;
69 4bc1c0c1 Kevin Townsend
    }
70
71
    Vector(const Vector<N> &v)
72
    {
73 b2d499c7 Paul Du Bois (laptop)
        for (int x = 0; x < N; x++)
74 4bc1c0c1 Kevin Townsend
            p_vec[x] = v.p_vec[x];
75
    }
76
77
    ~Vector()
78
    {
79
    }
80
81
    uint8_t n() { return N; }
82
83 d7b28532 Gé Vissers
    double magnitude() const
84 4bc1c0c1 Kevin Townsend
    {
85
        double res = 0;
86 651c5f56 Gé Vissers
        for (int i = 0; i < N; i++)
87
            res += p_vec[i] * p_vec[i];
88 4bc1c0c1 Kevin Townsend
89 3e12eaa8 Gé Vissers
        return sqrt(res);
90 4bc1c0c1 Kevin Townsend
    }
91
92
    void normalize()
93
    {
94
        double mag = magnitude();
95 3cae40b9 Gé Vissers
        if (isnan(mag) || mag == 0.0)
96 4bc1c0c1 Kevin Townsend
            return;
97
98 3cae40b9 Gé Vissers
        for (int i = 0; i < N; i++)
99
            p_vec[i] /= mag;
100 4bc1c0c1 Kevin Townsend
    }
101
102 d7b28532 Gé Vissers
    double dot(const Vector& v) const
103 4bc1c0c1 Kevin Townsend
    {
104
        double ret = 0;
105 651c5f56 Gé Vissers
        for (int i = 0; i < N; i++)
106 4bc1c0c1 Kevin Townsend
            ret += p_vec[i] * v.p_vec[i];
107
108
        return ret;
109
    }
110
111 fd9de024 Gé Vissers
    // 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 4bc1c0c1 Kevin Townsend
118 0695bf91 Paul Du Bois (laptop)
    Vector scale(double scalar) const
119 4bc1c0c1 Kevin Townsend
    {
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 0695bf91 Paul Du Bois (laptop)
    Vector invert() const
127 4bc1c0c1 Kevin Townsend
    {
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 d7b28532 Gé Vissers
    Vector& operator=(const Vector& v)
135 4bc1c0c1 Kevin Townsend
    {
136
        for (int x = 0; x < N; x++ )
137
            p_vec[x] = v.p_vec[x];
138 d964148c Tony DiCola
        return *this;
139 4bc1c0c1 Kevin Townsend
    }
140
141
    double& operator [](int n)
142
    {
143
        return p_vec[n];
144
    }
145
146 0695bf91 Paul Du Bois (laptop)
    double operator [](int n) const
147
    {
148
        return p_vec[n];
149
    }
150
151 4bc1c0c1 Kevin Townsend
    double& operator ()(int n)
152
    {
153
        return p_vec[n];
154
    }
155
156 0695bf91 Paul Du Bois (laptop)
    double operator ()(int n) const
157
    {
158
        return p_vec[n];
159
    }
160
161 d7b28532 Gé Vissers
    Vector operator+(const Vector& v) const
162 4bc1c0c1 Kevin Townsend
    {
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 d7b28532 Gé Vissers
    Vector operator-(const Vector& v) const
170 4bc1c0c1 Kevin Townsend
    {
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 0695bf91 Paul Du Bois (laptop)
    Vector operator * (double scalar) const
178 4bc1c0c1 Kevin Townsend
    {
179
        return scale(scalar);
180
    }
181
182 0695bf91 Paul Du Bois (laptop)
    Vector operator / (double scalar) const
183 4bc1c0c1 Kevin Townsend
    {
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 0695bf91 Paul Du Bois (laptop)
    double x() const { return p_vec[0]; }
206
    double y() const { return p_vec[1]; }
207
    double z() const { return p_vec[2]; }
208 4bc1c0c1 Kevin Townsend
209
210
private:
211 651c5f56 Gé Vissers
    double p_vec[N];
212 4bc1c0c1 Kevin Townsend
};
213
214
215 fd9de024 Gé Vissers
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 4bc1c0c1 Kevin Townsend
227
#endif