mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-14 10:07:39 +08:00
446 lines
8.6 KiB
C++
446 lines
8.6 KiB
C++
/****************************************************************************
|
|
*
|
|
* 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
|
|
* are met:
|
|
*
|
|
* 1. Redistributions of source code must retain the above copyright
|
|
* notice, this list of conditions and the following disclaimer.
|
|
* 2. Redistributions in binary form must reproduce the above copyright
|
|
* notice, this list of conditions and the following disclaimer in
|
|
* the documentation and/or other materials provided with the
|
|
* distribution.
|
|
* 3. Neither the name PX4 nor the names of its contributors may be
|
|
* used to endorse or promote products derived from this software
|
|
* without specific prior written permission.
|
|
*
|
|
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
|
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
|
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
|
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
|
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
|
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
|
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
|
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
|
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
|
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
|
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
|
* POSSIBILITY OF SUCH DAMAGE.
|
|
*
|
|
****************************************************************************/
|
|
|
|
/**
|
|
* @file Vector.hpp
|
|
*
|
|
* Vector class
|
|
*/
|
|
|
|
#ifndef VECTOR_HPP
|
|
#define VECTOR_HPP
|
|
|
|
#include <stdio.h>
|
|
#include <math.h>
|
|
#include "../CMSIS/Include/arm_math.h"
|
|
|
|
namespace 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));
|
|
}
|
|
|
|
/**
|
|
* 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;
|
|
}
|
|
|
|
/**
|
|
* 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 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 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 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
|