Line data Source code
1 : /*
2 : This program is free software: you can redistribute it and/or modify
3 : it under the terms of the GNU General Public License as published by
4 : the Free Software Foundation, either version 3 of the License, or
5 : (at your option) any later version.
6 :
7 : This program is distributed in the hope that it will be useful,
8 : but WITHOUT ANY WARRANTY; without even the implied warranty of
9 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10 : GNU General Public License for more details.
11 :
12 : You should have received a copy of the GNU General Public License
13 : along with this program. If not, see <http://www.gnu.org/licenses/>.
14 : */
15 : #pragma once
16 :
17 : #include <cmath>
18 : #include <string.h>
19 : #include "matrixN.h"
20 :
21 : #ifndef MATH_CHECK_INDEXES
22 : # define MATH_CHECK_INDEXES 0
23 : #endif
24 :
25 : #if MATH_CHECK_INDEXES
26 : #include <assert.h>
27 : #endif
28 :
29 : template <typename T, uint8_t N>
30 : class MatrixN;
31 :
32 :
33 : template <typename T, uint8_t N>
34 : class VectorN
35 : {
36 : public:
37 : // trivial ctor
38 943370 : inline VectorN() {
39 1595283 : for (auto i = 0; i < N; i++) {
40 1489113 : _v[i] = T{};
41 : }
42 106170 : }
43 :
44 : // vector ctor
45 1 : inline VectorN(const T *v) {
46 1 : memcpy(_v, v, sizeof(T)*N);
47 1 : }
48 :
49 4123602938 : inline T & operator[](uint8_t i) {
50 : #if MATH_CHECK_INDEXES
51 : assert(i >= 0 && i < N);
52 : #endif
53 4123602938 : return _v[i];
54 : }
55 :
56 708136 : inline const T & operator[](uint8_t i) const {
57 : #if MATH_CHECK_INDEXES
58 : assert(i >= 0 && i < N);
59 : #endif
60 708136 : return _v[i];
61 : }
62 :
63 : // test for equality
64 : bool operator ==(const VectorN<T,N> &v) const {
65 : for (uint8_t i=0; i<N; i++) {
66 : if (_v[i] != v[i]) return false;
67 : }
68 : return true;
69 : }
70 :
71 : // zero the vector
72 : inline void zero()
73 : {
74 : memset(_v, 0, sizeof(T)*N);
75 : }
76 :
77 : // negation
78 : VectorN<T,N> operator -(void) const {
79 : VectorN<T,N> v2;
80 : for (uint8_t i=0; i<N; i++) {
81 : v2[i] = - _v[i];
82 : }
83 : return v2;
84 : }
85 :
86 : // addition
87 : VectorN<T,N> operator +(const VectorN<T,N> &v) const {
88 : VectorN<T,N> v2;
89 : for (uint8_t i=0; i<N; i++) {
90 : v2[i] = _v[i] + v[i];
91 : }
92 : return v2;
93 : }
94 :
95 : // subtraction
96 : VectorN<T,N> operator -(const VectorN<T,N> &v) const {
97 : VectorN<T,N> v2;
98 : for (uint8_t i=0; i<N; i++) {
99 : v2[i] = _v[i] - v[i];
100 : }
101 : return v2;
102 : }
103 :
104 : // uniform scaling
105 27236 : VectorN<T,N> operator *(const T num) const {
106 27236 : VectorN<T,N> v2;
107 136180 : for (uint8_t i=0; i<N; i++) {
108 108944 : v2[i] = _v[i] * num;
109 : }
110 27236 : return v2;
111 : }
112 :
113 : // uniform scaling
114 13618 : VectorN<T,N> operator /(const T num) const {
115 13618 : VectorN<T,N> v2;
116 68090 : for (uint8_t i=0; i<N; i++) {
117 54472 : v2[i] = _v[i] / num;
118 : }
119 13618 : return v2;
120 : }
121 :
122 : // addition
123 13618 : VectorN<T,N> &operator +=(const VectorN<T,N> &v) {
124 68090 : for (uint8_t i=0; i<N; i++) {
125 54472 : _v[i] += v[i];
126 : }
127 13618 : return *this;
128 : }
129 :
130 : // subtraction
131 : VectorN<T,N> &operator -=(const VectorN<T,N> &v) {
132 : for (uint8_t i=0; i<N; i++) {
133 : _v[i] -= v[i];
134 : }
135 : return *this;
136 : }
137 :
138 : // uniform scaling
139 : VectorN<T,N> &operator *=(const T num) {
140 : for (uint8_t i=0; i<N; i++) {
141 : _v[i] *= num;
142 : }
143 : return *this;
144 : }
145 :
146 : // uniform scaling
147 : VectorN<T,N> &operator /=(const T num) {
148 : for (uint8_t i=0; i<N; i++) {
149 : _v[i] /= num;
150 : }
151 : return *this;
152 : }
153 :
154 : // dot product
155 13618 : T operator *(const VectorN<T,N> &v) const {
156 13618 : float ret = 0;
157 68090 : for (uint8_t i=0; i<N; i++) {
158 54472 : ret += _v[i] * v._v[i];
159 : }
160 13618 : return ret;
161 : }
162 :
163 : // multiplication of a matrix by a vector, in-place
164 : // C = A * B
165 13618 : void mult(const MatrixN<T,N> &A, const VectorN<T,N> &B) {
166 68090 : for (uint8_t i = 0; i < N; i++) {
167 54472 : _v[i] = 0;
168 272360 : for (uint8_t k = 0; k < N; k++) {
169 217888 : _v[i] += A.v[i][k] * B[k];
170 : }
171 : }
172 13618 : }
173 :
174 : protected:
175 : T _v[N];
176 : };
|
