aboutsummaryrefslogtreecommitdiff
path: root/src/lib/mathlib/math/Vector.hpp
diff options
context:
space:
mode:
Diffstat (limited to 'src/lib/mathlib/math/Vector.hpp')
-rw-r--r--src/lib/mathlib/math/Vector.hpp224
1 files changed, 186 insertions, 38 deletions
diff --git a/src/lib/mathlib/math/Vector.hpp b/src/lib/mathlib/math/Vector.hpp
index cd00058b5..2df87c74b 100644
--- a/src/lib/mathlib/math/Vector.hpp
+++ b/src/lib/mathlib/math/Vector.hpp
@@ -42,6 +42,7 @@
#ifndef VECTOR_HPP
#define VECTOR_HPP
+#include <stdio.h>
#include <math.h>
#include "../CMSIS/Include/arm_math.h"
@@ -49,31 +50,42 @@ namespace math
{
template <unsigned int N>
+class Vector;
+
+template <unsigned int N>
class VectorBase {
public:
+ /**
+ * vector data
+ */
float data[N];
+
+ /**
+ * struct for using arm_math functions, represents column vector
+ */
arm_matrix_instance_f32 arm_col;
/**
* trivial ctor
*/
- VectorBase<N>() {
+ VectorBase() {
arm_col = {N, 1, &data[0]};
}
/**
- * setting ctor
+ * copy ctor
*/
-// VectorBase<N>(const float *d) {
- // memcpy(data, d, sizeof(data));
- //arm_col = {N, 1, &data[0]};
- //}
+ VectorBase(const VectorBase<N> &v) {
+ arm_col = {N, 1, &data[0]};
+ memcpy(data, v.data, sizeof(data));
+ }
/**
* setting ctor
*/
- VectorBase<N>(const float d[]) : data(d) {
+ VectorBase(const float *d) {
arm_col = {N, 1, &data[0]};
+ memcpy(data, d, sizeof(data));
}
/**
@@ -90,10 +102,18 @@ public:
return data[i];
}
+ unsigned int getRows() {
+ return N;
+ }
+
+ unsigned int getCols() {
+ return 1;
+ }
+
/**
* test for equality
*/
- bool operator ==(const VectorBase<N> &v) {
+ bool operator ==(const Vector<N> &v) {
for (unsigned int i = 0; i < N; i++)
if (data[i] != v(i))
return false;
@@ -103,7 +123,7 @@ public:
/**
* test for inequality
*/
- bool operator !=(const VectorBase<N> &v) {
+ bool operator !=(const Vector<N> &v) {
for (unsigned int i = 0; i < N; i++)
if (data[i] != v(i))
return true;
@@ -113,98 +133,98 @@ public:
/**
* set to value
*/
- const VectorBase<N> &operator =(const VectorBase<N> &v) {
+ const Vector<N> &operator =(const Vector<N> &v) {
memcpy(data, v.data, sizeof(data));
- return *this;
+ return *reinterpret_cast<const Vector<N>*>(this);
}
/**
* negation
*/
- const VectorBase<N> operator -(void) const {
- VectorBase<N> res;
+ const Vector<N> operator -(void) const {
+ Vector<N> res;
for (unsigned int i = 0; i < N; i++)
- res[i] = -data[i];
+ res.data[i] = -data[i];
return res;
}
/**
* addition
*/
- const VectorBase<N> operator +(const VectorBase<N> &v) const {
- VectorBase<N> res;
+ const Vector<N> operator +(const Vector<N> &v) const {
+ Vector<N> res;
for (unsigned int i = 0; i < N; i++)
- res[i] = data[i] + v(i);
+ res.data[i] = data[i] + v(i);
return res;
}
/**
* subtraction
*/
- const VectorBase<N> operator -(const VectorBase<N> &v) const {
- VectorBase<N> res;
+ const Vector<N> operator -(const Vector<N> &v) const {
+ Vector<N> res;
for (unsigned int i = 0; i < N; i++)
- res[i] = data[i] - v(i);
+ res.data[i] = data[i] - v(i);
return res;
}
/**
* uniform scaling
*/
- const VectorBase<N> operator *(const float num) const {
- VectorBase<N> temp(*this);
+ const Vector<N> operator *(const float num) const {
+ Vector<N> temp(*this);
return temp *= num;
}
/**
* uniform scaling
*/
- const VectorBase<N> operator /(const float num) const {
- VectorBase<N> temp(*this);
+ const Vector<N> operator /(const float num) const {
+ Vector<N> temp(*reinterpret_cast<const Vector<N>*>(this));
return temp /= num;
}
/**
* addition
*/
- const VectorBase<N> &operator +=(const VectorBase<N> &v) {
+ const Vector<N> &operator +=(const Vector<N> &v) {
for (unsigned int i = 0; i < N; i++)
data[i] += v(i);
- return *this;
+ return *reinterpret_cast<const Vector<N>*>(this);
}
/**
* subtraction
*/
- const VectorBase<N> &operator -=(const VectorBase<N> &v) {
+ const Vector<N> &operator -=(const Vector<N> &v) {
for (unsigned int i = 0; i < N; i++)
data[i] -= v(i);
- return *this;
+ return *reinterpret_cast<const Vector<N>*>(this);
}
/**
* uniform scaling
*/
- const VectorBase<N> &operator *=(const float num) {
+ const Vector<N> &operator *=(const float num) {
for (unsigned int i = 0; i < N; i++)
data[i] *= num;
- return *this;
+ return *reinterpret_cast<const Vector<N>*>(this);
}
/**
* uniform scaling
*/
- const VectorBase<N> &operator /=(const float num) {
+ const Vector<N> &operator /=(const float num) {
for (unsigned int i = 0; i < N; i++)
data[i] /= num;
- return *this;
+ return *reinterpret_cast<const Vector<N>*>(this);
}
/**
* dot product
*/
- float operator *(const VectorBase<N> &v) const {
- float res;
+ float operator *(const Vector<N> &v) const {
+ float res = 0.0f;
for (unsigned int i = 0; i < N; i++)
res += data[i] * v(i);
return res;
@@ -221,7 +241,7 @@ public:
* gets the length of this vector
*/
float length() const {
- return sqrtf(*this * *this);
+ return sqrtf(*this * *reinterpret_cast<const Vector<N>*>(this));
}
/**
@@ -234,46 +254,174 @@ public:
/**
* returns the normalized version of this vector
*/
- VectorBase<N> normalized() const {
+ Vector<N> normalized() const {
return *this / length();
}
+
+ void print(void) {
+ printf("[ ");
+ for (unsigned int i = 0; i < N; i++)
+ printf("%.3f\t", data[i]);
+ printf("]\n");
+ }
};
template <unsigned int N>
class Vector : public VectorBase<N> {
public:
+ using VectorBase<N>::operator *;
+
+ Vector() : VectorBase<N>() {
+ }
+
+ Vector(const float d[]) : VectorBase<N>(d) {
+ }
+
+ Vector(const Vector<N> &v) : VectorBase<N>(v) {
+ }
+
+ Vector(const VectorBase<N> &v) : VectorBase<N>(v) {
+ }
+
/**
* set to value
*/
const Vector<N> &operator =(const Vector<N> &v) {
+ this->arm_col = {N, 1, &this->data[0]};
memcpy(this->data, v.data, sizeof(this->data));
return *this;
}
};
template <>
+class Vector<2> : public VectorBase<2> {
+public:
+ Vector() : VectorBase<2>() {
+ }
+
+ Vector(const float x, const float y) : VectorBase() {
+ data[0] = x;
+ data[1] = y;
+ }
+
+ Vector(const Vector<2> &v) : VectorBase() {
+ data[0] = v.data[0];
+ data[1] = v.data[1];
+ }
+
+ Vector(const VectorBase<2> &v) : VectorBase() {
+ data[0] = v.data[0];
+ data[1] = v.data[1];
+ }
+
+ /**
+ * set to value
+ */
+ const Vector<2> &operator =(const Vector<2> &v) {
+ data[0] = v.data[0];
+ data[1] = v.data[1];
+ return *this;
+ }
+
+ float cross(const Vector<2> &b) const {
+ return data[0]*b.data[1] - data[1]*b.data[0];
+ }
+
+ float operator %(const Vector<2> &v) const {
+ return cross(v);
+ }
+
+};
+
+template <>
class Vector<3> : public VectorBase<3> {
public:
- Vector<3>() {
+ Vector() {
arm_col = {3, 1, &this->data[0]};
}
- Vector<3>(const float x, const float y, const float z) {
+ Vector(const float x, const float y, const float z) {
+ arm_col = {3, 1, &this->data[0]};
data[0] = x;
data[1] = y;
data[2] = z;
+ }
+
+ Vector(const Vector<3> &v) : VectorBase<3>() {
+ data[0] = v.data[0];
+ data[1] = v.data[1];
+ data[2] = v.data[2];
+ }
+
+ /**
+ * setting ctor
+ */
+ Vector(const float d[]) {
arm_col = {3, 1, &this->data[0]};
+ data[0] = d[0];
+ data[1] = d[1];
+ data[2] = d[2];
+ }
+
+ /**
+ * set to value
+ */
+ const Vector<3> &operator =(const Vector<3> &v) {
+ data[0] = v.data[0];
+ data[1] = v.data[1];
+ data[2] = v.data[2];
+ return *this;
+ }
+};
+
+template <>
+class Vector<4> : public VectorBase<4> {
+public:
+ Vector() : VectorBase() {}
+
+ Vector(const float x, const float y, const float z, const float t) : VectorBase() {
+ data[0] = x;
+ data[1] = y;
+ data[2] = z;
+ data[3] = t;
}
+ Vector(const Vector<4> &v) : VectorBase() {
+ data[0] = v.data[0];
+ data[1] = v.data[1];
+ data[2] = v.data[2];
+ data[3] = v.data[3];
+ }
+
+ Vector(const VectorBase<4> &v) : VectorBase() {
+ data[0] = v.data[0];
+ data[1] = v.data[1];
+ data[2] = v.data[2];
+ data[3] = v.data[3];
+ }
+
+ /**
+ * setting ctor
+ */
+ /*
+ Vector(const float d[]) {
+ arm_col = {3, 1, &this->data[0]};
+ data[0] = d[0];
+ data[1] = d[1];
+ data[2] = d[2];
+ }
+*/
/**
* set to value
*/
+ /*
const Vector<3> &operator =(const Vector<3> &v) {
data[0] = v.data[0];
data[1] = v.data[1];
data[2] = v.data[2];
return *this;
}
+ */
};
}
generated by cgit v1.2.3 (git 2.39.1) at 2024-08-30 16:25:48 +0000