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.hpp477
1 files changed, 460 insertions, 17 deletions
diff --git a/src/lib/mathlib/math/Vector.hpp b/src/lib/mathlib/math/Vector.hpp
index 73de793d5..c7323c215 100644
--- a/src/lib/mathlib/math/Vector.hpp
+++ b/src/lib/mathlib/math/Vector.hpp
@@ -1,6 +1,9 @@
/****************************************************************************
*
- * Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ * Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ * Author: Anton Babushkin <anton.babushkin@me.com>
+ * Pavel Kirienko <pavel.kirienko@gmail.com>
+ * Lorenz Meier <lm@inf.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -32,26 +35,466 @@
****************************************************************************/
/**
- * @file Vector.h
+ * @file Vector.hpp
*
- * math vector
+ * Vector class
*/
-#pragma once
+#ifndef VECTOR_HPP
+#define VECTOR_HPP
-#include <nuttx/config.h>
-
-#if defined(CONFIG_ARCH_CORTEXM4) && defined(CONFIG_ARCH_FPU)
-#include "arm/Vector.hpp"
-#else
-#include "generic/Vector.hpp"
-#endif
+#include <stdio.h>
+#include <math.h>
+#include "../CMSIS/Include/arm_math.h"
namespace math
{
-class Vector;
-int __EXPORT vectorTest();
-int __EXPORT vectorAddTest();
-int __EXPORT vectorSubTest();
-bool vectorEqual(const Vector &a, const Vector &b, float eps = 1.0e-5f);
-} // math
+
+template <unsigned int N>
+class __EXPORT Vector;
+
+template <unsigned int N>
+class __EXPORT VectorBase
+{
+public:
+ /**
+ * vector data
+ */
+ float data[N];
+
+ /**
+ * struct for using arm_math functions, represents column vector
+ */
+ arm_matrix_instance_f32 arm_col;
+
+ /**
+ * trivial ctor
+ * note that this ctor will not initialize elements
+ */
+ VectorBase() {
+ arm_col = {N, 1, &data[0]};
+ }
+
+ /**
+ * copy ctor
+ */
+ VectorBase(const VectorBase<N> &v) {
+ arm_col = {N, 1, &data[0]};
+ memcpy(data, v.data, sizeof(data));
+ }
+
+ /**
+ * setting ctor
+ */
+ VectorBase(const float d[N]) {
+ arm_col = {N, 1, &data[0]};
+ memcpy(data, d, sizeof(data));
+ }
+
+ /**
+ * set data
+ */
+ void set(const float d[N]) {
+ memcpy(data, d, sizeof(data));
+ }
+
+ /**
+ * access to elements by index
+ */
+ float &operator()(const unsigned int i) {
+ return data[i];
+ }
+
+ /**
+ * access to elements by index
+ */
+ float operator()(const unsigned int i) const {
+ return data[i];
+ }
+
+ /**
+ * get vector size
+ */
+ unsigned int get_size() const {
+ return N;
+ }
+
+ /**
+ * test for equality
+ */
+ bool operator ==(const Vector<N> &v) const {
+ for (unsigned int i = 0; i < N; i++)
+ if (data[i] != v.data[i])
+ return false;
+
+ return true;
+ }
+
+ /**
+ * test for inequality
+ */
+ bool operator !=(const Vector<N> &v) const {
+ for (unsigned int i = 0; i < N; i++)
+ if (data[i] != v.data[i])
+ return true;
+
+ return false;
+ }
+
+ /**
+ * set to value
+ */
+ const Vector<N> &operator =(const Vector<N> &v) {
+ memcpy(data, v.data, sizeof(data));
+ return *static_cast<const Vector<N>*>(this);
+ }
+
+ /**
+ * negation
+ */
+ const Vector<N> operator -(void) const {
+ Vector<N> res;
+
+ for (unsigned int i = 0; i < N; i++)
+ res.data[i] = -data[i];
+
+ return res;
+ }
+
+ /**
+ * addition
+ */
+ const Vector<N> operator +(const Vector<N> &v) const {
+ Vector<N> res;
+
+ for (unsigned int i = 0; i < N; i++)
+ res.data[i] = data[i] + v.data[i];
+
+ return res;
+ }
+
+ /**
+ * subtraction
+ */
+ const Vector<N> operator -(const Vector<N> &v) const {
+ Vector<N> res;
+
+ for (unsigned int i = 0; i < N; i++)
+ res.data[i] = data[i] - v.data[i];
+
+ return res;
+ }
+
+ /**
+ * uniform scaling
+ */
+ const Vector<N> operator *(const float num) const {
+ Vector<N> res;
+
+ for (unsigned int i = 0; i < N; i++)
+ res.data[i] = data[i] * num;
+
+ return res;
+ }
+
+ /**
+ * uniform scaling
+ */
+ const Vector<N> operator /(const float num) const {
+ Vector<N> res;
+
+ for (unsigned int i = 0; i < N; i++)
+ res.data[i] = data[i] / num;
+
+ return res;
+ }
+
+ /**
+ * addition
+ */
+ const Vector<N> &operator +=(const Vector<N> &v) {
+ for (unsigned int i = 0; i < N; i++)
+ data[i] += v.data[i];
+
+ return *static_cast<const Vector<N>*>(this);
+ }
+
+ /**
+ * subtraction
+ */
+ const Vector<N> &operator -=(const Vector<N> &v) {
+ for (unsigned int i = 0; i < N; i++)
+ data[i] -= v.data[i];
+
+ return *static_cast<const Vector<N>*>(this);
+ }
+
+ /**
+ * uniform scaling
+ */
+ const Vector<N> &operator *=(const float num) {
+ for (unsigned int i = 0; i < N; i++)
+ data[i] *= num;
+
+ return *static_cast<const Vector<N>*>(this);
+ }
+
+ /**
+ * uniform scaling
+ */
+ const Vector<N> &operator /=(const float num) {
+ for (unsigned int i = 0; i < N; i++)
+ data[i] /= num;
+
+ return *static_cast<const Vector<N>*>(this);
+ }
+
+ /**
+ * dot product
+ */
+ float operator *(const Vector<N> &v) const {
+ float res = 0.0f;
+
+ for (unsigned int i = 0; i < N; i++)
+ res += data[i] * v.data[i];
+
+ return res;
+ }
+
+ /**
+ * element by element multiplication
+ */
+ const Vector<N> emult(const Vector<N> &v) const {
+ Vector<N> res;
+
+ for (unsigned int i = 0; i < N; i++)
+ res.data[i] = data[i] * v.data[i];
+
+ return res;
+ }
+
+ /**
+ * element by element division
+ */
+ const Vector<N> edivide(const Vector<N> &v) const {
+ Vector<N> res;
+
+ for (unsigned int i = 0; i < N; i++)
+ res.data[i] = data[i] / v.data[i];
+
+ return res;
+ }
+
+ /**
+ * gets the length of this vector squared
+ */
+ float length_squared() const {
+ float res = 0.0f;
+
+ for (unsigned int i = 0; i < N; i++)
+ res += data[i] * data[i];
+
+ return res;
+ }
+
+ /**
+ * gets the length of this vector
+ */
+ float length() const {
+ float res = 0.0f;
+
+ for (unsigned int i = 0; i < N; i++)
+ res += data[i] * data[i];
+
+ return sqrtf(res);
+ }
+
+ /**
+ * normalizes this vector
+ */
+ void normalize() {
+ *this /= length();
+ }
+
+ /**
+ * returns the normalized version of this vector
+ */
+ Vector<N> normalized() const {
+ return *this / length();
+ }
+
+ /**
+ * set zero vector
+ */
+ void zero(void) {
+ memset(data, 0, sizeof(data));
+ }
+
+ void print(void) {
+ printf("[ ");
+
+ for (unsigned int i = 0; i < N; i++)
+ printf("%.3f\t", data[i]);
+
+ printf("]\n");
+ }
+};
+
+template <unsigned int N>
+class __EXPORT Vector : public VectorBase<N>
+{
+public:
+ Vector() : VectorBase<N>() {}
+
+ Vector(const Vector<N> &v) : VectorBase<N>(v) {}
+
+ Vector(const float d[N]) : VectorBase<N>(d) {}
+
+ /**
+ * set to value
+ */
+ const Vector<N> &operator =(const Vector<N> &v) {
+ memcpy(this->data, v.data, sizeof(this->data));
+ return *this;
+ }
+};
+
+template <>
+class __EXPORT Vector<2> : public VectorBase<2>
+{
+public:
+ Vector() : VectorBase<2>() {}
+
+ // simple copy is 1.6 times faster than memcpy
+ Vector(const Vector<2> &v) : VectorBase<2>() {
+ data[0] = v.data[0];
+ data[1] = v.data[1];
+ }
+
+ Vector(const float d[2]) : VectorBase<2>() {
+ data[0] = d[0];
+ data[1] = d[1];
+ }
+
+ Vector(const float x, const float y) : VectorBase<2>() {
+ data[0] = x;
+ data[1] = y;
+ }
+
+ /**
+ * set data
+ */
+ void set(const float d[2]) {
+ data[0] = d[0];
+ data[1] = d[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 operator %(const Vector<2> &v) const {
+ return data[0] * v.data[1] - data[1] * v.data[0];
+ }
+};
+
+template <>
+class __EXPORT Vector<3> : public VectorBase<3>
+{
+public:
+ Vector() : VectorBase<3>() {}
+
+ // simple copy is 1.6 times faster than memcpy
+ Vector(const Vector<3> &v) : VectorBase<3>() {
+ for (unsigned int i = 0; i < 3; i++)
+ data[i] = v.data[i];
+ }
+
+ Vector(const float d[3]) : VectorBase<3>() {
+ for (unsigned int i = 0; i < 3; i++)
+ data[i] = d[i];
+ }
+
+ Vector(const float x, const float y, const float z) : VectorBase<3>() {
+ data[0] = x;
+ data[1] = y;
+ data[2] = z;
+ }
+
+ /**
+ * set data
+ */
+ void set(const float d[3]) {
+ for (unsigned int i = 0; i < 3; i++)
+ data[i] = d[i];
+ }
+
+ /**
+ * set to value
+ */
+ const Vector<3> &operator =(const Vector<3> &v) {
+ for (unsigned int i = 0; i < 3; i++)
+ data[i] = v.data[i];
+
+ return *this;
+ }
+
+ Vector<3> operator %(const Vector<3> &v) const {
+ return Vector<3>(
+ data[1] * v.data[2] - data[2] * v.data[1],
+ data[2] * v.data[0] - data[0] * v.data[2],
+ data[0] * v.data[1] - data[1] * v.data[0]
+ );
+ }
+};
+
+template <>
+class __EXPORT Vector<4> : public VectorBase<4>
+{
+public:
+ Vector() : VectorBase() {}
+
+ Vector(const Vector<4> &v) : VectorBase<4>() {
+ for (unsigned int i = 0; i < 4; i++)
+ data[i] = v.data[i];
+ }
+
+ Vector(const float d[4]) : VectorBase<4>() {
+ for (unsigned int i = 0; i < 4; i++)
+ data[i] = d[i];
+ }
+
+ Vector(const float x0, const float x1, const float x2, const float x3) : VectorBase() {
+ data[0] = x0;
+ data[1] = x1;
+ data[2] = x2;
+ data[3] = x3;
+ }
+
+ /**
+ * set data
+ */
+ void set(const float d[4]) {
+ for (unsigned int i = 0; i < 4; i++)
+ data[i] = d[i];
+ }
+
+ /**
+ * set to value
+ */
+ const Vector<4> &operator =(const Vector<4> &v) {
+ for (unsigned int i = 0; i < 4; i++)
+ data[i] = v.data[i];
+
+ return *this;
+ }
+};
+
+}
+
+#endif // VECTOR_HPP
generated by cgit v1.2.3 (git 2.39.1) at 2024-06-19 04:58:12 +0000