mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-16 02:33:33 +08:00
mathlib fixes
This commit is contained in:
Anton Babushkin
+134
-38
@@ -42,16 +42,19 @@
|
||||
#ifndef MATRIX_HPP
|
||||
#define MATRIX_HPP
|
||||
|
||||
#include <stdio.h>
|
||||
#include "../CMSIS/Include/arm_math.h"
|
||||
|
||||
namespace math
|
||||
{
|
||||
|
||||
template <unsigned int N, unsigned int M>
|
||||
template <unsigned int M, unsigned int N>
|
||||
class Matrix;
|
||||
|
||||
class Quaternion;
|
||||
|
||||
// MxN matrix with float elements
|
||||
template <unsigned int N, unsigned int M>
|
||||
template <unsigned int M, unsigned int N>
|
||||
class MatrixBase {
|
||||
public:
|
||||
/**
|
||||
@@ -69,10 +72,14 @@ public:
|
||||
* note that this ctor will not initialize elements
|
||||
*/
|
||||
MatrixBase() {
|
||||
//arm_mat_init_f32(&arm_mat, M, N, data);
|
||||
arm_mat = {M, N, &data[0][0]};
|
||||
}
|
||||
|
||||
MatrixBase(const float *d) {
|
||||
arm_mat = {M, N, &data[0][0]};
|
||||
memcpy(data, d, sizeof(data));
|
||||
}
|
||||
|
||||
/**
|
||||
* access by index
|
||||
*/
|
||||
@@ -98,10 +105,10 @@ public:
|
||||
/**
|
||||
* test for equality
|
||||
*/
|
||||
bool operator ==(const MatrixBase<M, N> &m) {
|
||||
bool operator ==(const Matrix<M, N> &m) {
|
||||
for (unsigned int i = 0; i < M; i++)
|
||||
for (unsigned int j = 0; j < N; j++)
|
||||
if (data[i][j] != m(i, j))
|
||||
if (data[i][j] != m.data[i][j])
|
||||
return false;
|
||||
return true;
|
||||
}
|
||||
@@ -109,10 +116,10 @@ public:
|
||||
/**
|
||||
* test for inequality
|
||||
*/
|
||||
bool operator !=(const MatrixBase<M, N> &m) {
|
||||
bool operator !=(const Matrix<M, N> &m) {
|
||||
for (unsigned int i = 0; i < M; i++)
|
||||
for (unsigned int j = 0; j < N; j++)
|
||||
if (data[i][j] != m(i, j))
|
||||
if (data[i][j] != m.data[i][j])
|
||||
return true;
|
||||
return false;
|
||||
}
|
||||
@@ -120,85 +127,97 @@ public:
|
||||
/**
|
||||
* set to value
|
||||
*/
|
||||
const MatrixBase<M, N> &operator =(const MatrixBase<M, N> &m) {
|
||||
const Matrix<M, N> &operator =(const Matrix<M, N> &m) {
|
||||
memcpy(data, m.data, sizeof(data));
|
||||
return *this;
|
||||
return *reinterpret_cast<Matrix<M, N>*>(this);
|
||||
}
|
||||
|
||||
/**
|
||||
* negation
|
||||
*/
|
||||
MatrixBase<M, N> operator -(void) const {
|
||||
MatrixBase<M, N> res;
|
||||
Matrix<M, N> operator -(void) const {
|
||||
Matrix<M, N> res;
|
||||
for (unsigned int i = 0; i < N; i++)
|
||||
for (unsigned int j = 0; j < M; j++)
|
||||
res[i][j] = -data[i][j];
|
||||
res.data[i][j] = -data[i][j];
|
||||
return res;
|
||||
}
|
||||
|
||||
/**
|
||||
* addition
|
||||
*/
|
||||
MatrixBase<M, N> operator +(const MatrixBase<M, N> &m) const {
|
||||
MatrixBase<M, N> res;
|
||||
Matrix<M, N> operator +(const Matrix<M, N> &m) const {
|
||||
Matrix<M, N> res;
|
||||
for (unsigned int i = 0; i < N; i++)
|
||||
for (unsigned int j = 0; j < M; j++)
|
||||
res[i][j] = data[i][j] + m(i, j);
|
||||
res.data[i][j] = data[i][j] + m.data[i][j];
|
||||
return res;
|
||||
}
|
||||
|
||||
MatrixBase<M, N> &operator +=(const MatrixBase<M, N> &m) {
|
||||
return *this = *this + m;
|
||||
Matrix<M, N> &operator +=(const Matrix<M, N> &m) {
|
||||
for (unsigned int i = 0; i < N; i++)
|
||||
for (unsigned int j = 0; j < M; j++)
|
||||
data[i][j] += m.data[i][j];
|
||||
return *reinterpret_cast<Matrix<M, N>*>(this);
|
||||
}
|
||||
|
||||
/**
|
||||
* subtraction
|
||||
*/
|
||||
MatrixBase<M, N> operator -(const MatrixBase<M, N> &m) const {
|
||||
MatrixBase<M, N> res;
|
||||
Matrix<M, N> operator -(const Matrix<M, N> &m) const {
|
||||
Matrix<M, N> res;
|
||||
for (unsigned int i = 0; i < M; i++)
|
||||
for (unsigned int j = 0; j < N; j++)
|
||||
res[i][j] = data[i][j] - m(i, j);
|
||||
res.data[i][j] = data[i][j] - m.data[i][j];
|
||||
return res;
|
||||
}
|
||||
|
||||
MatrixBase<M, N> &operator -=(const MatrixBase<M, N> &m) {
|
||||
return *this = *this - m;
|
||||
Matrix<M, N> &operator -=(const Matrix<M, N> &m) {
|
||||
for (unsigned int i = 0; i < N; i++)
|
||||
for (unsigned int j = 0; j < M; j++)
|
||||
data[i][j] -= m.data[i][j];
|
||||
return *reinterpret_cast<Matrix<M, N>*>(this);
|
||||
}
|
||||
|
||||
/**
|
||||
* uniform scaling
|
||||
*/
|
||||
MatrixBase<M, N> operator *(const float num) const {
|
||||
MatrixBase<M, N> res;
|
||||
Matrix<M, N> operator *(const float num) const {
|
||||
Matrix<M, N> res;
|
||||
for (unsigned int i = 0; i < M; i++)
|
||||
for (unsigned int j = 0; j < N; j++)
|
||||
res[i][j] = data[i][j] * num;
|
||||
res.data[i][j] = data[i][j] * num;
|
||||
return res;
|
||||
}
|
||||
|
||||
MatrixBase<M, N> &operator *=(const float num) {
|
||||
return *this = *this * num;
|
||||
Matrix<M, N> &operator *=(const float num) {
|
||||
for (unsigned int i = 0; i < M; i++)
|
||||
for (unsigned int j = 0; j < N; j++)
|
||||
data[i][j] *= num;
|
||||
return *reinterpret_cast<Matrix<M, N>*>(this);
|
||||
}
|
||||
|
||||
MatrixBase<M, N> operator /(const float num) const {
|
||||
MatrixBase<M, N> res;
|
||||
Matrix<M, N> operator /(const float num) const {
|
||||
Matrix<M, N> res;
|
||||
for (unsigned int i = 0; i < M; i++)
|
||||
for (unsigned int j = 0; j < N; j++)
|
||||
res[i][j] = data[i][j] / num;
|
||||
return res;
|
||||
}
|
||||
|
||||
MatrixBase<M, N> &operator /=(const float num) {
|
||||
return *this = *this / num;
|
||||
Matrix<M, N> &operator /=(const float num) {
|
||||
for (unsigned int i = 0; i < M; i++)
|
||||
for (unsigned int j = 0; j < N; j++)
|
||||
data[i][j] /= num;
|
||||
return *this;
|
||||
}
|
||||
|
||||
/**
|
||||
* multiplication by another matrix
|
||||
*/
|
||||
template <unsigned int P>
|
||||
Matrix<N, P> operator *(const Matrix<M, P> &m) const {
|
||||
Matrix<N, P> res;
|
||||
Matrix<M, P> operator *(const Matrix<N, P> &m) const {
|
||||
Matrix<M, P> res;
|
||||
arm_mat_mult_f32(&arm_mat, &m.arm_mat, &res.arm_mat);
|
||||
return res;
|
||||
}
|
||||
@@ -230,7 +249,7 @@ public:
|
||||
data[i][i] = 1;
|
||||
}
|
||||
|
||||
void dump(void) {
|
||||
void print(void) {
|
||||
for (unsigned int i = 0; i < M; i++) {
|
||||
for (unsigned int j = 0; j < N; j++)
|
||||
printf("%.3f\t", data[i][j]);
|
||||
@@ -244,6 +263,12 @@ class Matrix : public MatrixBase<M, N> {
|
||||
public:
|
||||
using MatrixBase<M, N>::operator *;
|
||||
|
||||
Matrix() : MatrixBase<M, N>() {
|
||||
}
|
||||
|
||||
Matrix(const float *d) : MatrixBase<M, N>(d) {
|
||||
}
|
||||
|
||||
/**
|
||||
* set to value
|
||||
*/
|
||||
@@ -255,18 +280,28 @@ public:
|
||||
/**
|
||||
* multiplication by a vector
|
||||
*/
|
||||
Vector<N> operator *(const Vector<N> &v) const {
|
||||
Vector<M> operator *(const Vector<N> &v) const {
|
||||
Vector<M> res;
|
||||
arm_mat_mult_f32(&this->arm_mat, &v.arm_col, &res.arm_col);
|
||||
return res;
|
||||
}
|
||||
};
|
||||
}
|
||||
|
||||
#include "Quaternion.hpp"
|
||||
|
||||
namespace math {
|
||||
template <>
|
||||
class Matrix<3, 3> : public MatrixBase<3, 3> {
|
||||
public:
|
||||
using MatrixBase<3, 3>::operator *;
|
||||
|
||||
Matrix() : MatrixBase<3, 3>() {
|
||||
}
|
||||
|
||||
Matrix(const float *d) : MatrixBase<3, 3>(d) {
|
||||
}
|
||||
|
||||
/**
|
||||
* set to value
|
||||
*/
|
||||
@@ -279,9 +314,70 @@ public:
|
||||
* multiplication by a vector
|
||||
*/
|
||||
Vector<3> operator *(const Vector<3> &v) const {
|
||||
return Vector<3>(data[0][0] * v.data[0] + data[0][1] * v.data[1] + data[0][2] * v.data[2],
|
||||
data[1][0] * v.data[0] + data[1][1] * v.data[1] + data[1][2] * v.data[2],
|
||||
data[2][0] * v.data[0] + data[2][1] * v.data[1] + data[2][2] * v.data[2]);
|
||||
Vector<3> res(data[0][0] * v.data[0] + data[0][1] * v.data[1] + data[0][2] * v.data[2],
|
||||
data[1][0] * v.data[0] + data[1][1] * v.data[1] + data[1][2] * v.data[2],
|
||||
data[2][0] * v.data[0] + data[2][1] * v.data[1] + data[2][2] * v.data[2]);
|
||||
return res;
|
||||
}
|
||||
|
||||
/**
|
||||
* create a rotation matrix from given quaternion
|
||||
*/
|
||||
void from_quaternion(const Quaternion &q) {
|
||||
float aSq = q.data[0] * q.data[0];
|
||||
float bSq = q.data[1] * q.data[1];
|
||||
float cSq = q.data[2] * q.data[2];
|
||||
float dSq = q.data[3] * q.data[3];
|
||||
data[0][0] = aSq + bSq - cSq - dSq;
|
||||
data[0][1] = 2.0f * (q.data[1] * q.data[2] - q.data[0] * q.data[3]);
|
||||
data[0][2] = 2.0f * (q.data[0] * q.data[2] + q.data[1] * q.data[3]);
|
||||
data[1][0] = 2.0f * (q.data[1] * q.data[2] + q.data[0] * q.data[3]);
|
||||
data[1][1] = aSq - bSq + cSq - dSq;
|
||||
data[1][2] = 2.0f * (q.data[2] * q.data[3] - q.data[0] * q.data[1]);
|
||||
data[2][0] = 2.0f * (q.data[1] * q.data[3] - q.data[0] * q.data[2]);
|
||||
data[2][1] = 2.0f * (q.data[0] * q.data[1] + q.data[2] * q.data[3]);
|
||||
data[2][2] = aSq - bSq - cSq + dSq;
|
||||
}
|
||||
|
||||
/**
|
||||
* create a rotation matrix from given euler angles
|
||||
* based on http://gentlenav.googlecode.com/files/EulerAngles.pdf
|
||||
*/
|
||||
void from_euler(float roll, float pitch, float yaw) {
|
||||
float cp = cosf(pitch);
|
||||
float sp = sinf(pitch);
|
||||
float sr = sinf(roll);
|
||||
float cr = cosf(roll);
|
||||
float sy = sinf(yaw);
|
||||
float cy = cosf(yaw);
|
||||
|
||||
data[0][0] = cp * cy;
|
||||
data[0][1] = (sr * sp * cy) - (cr * sy);
|
||||
data[0][2] = (cr * sp * cy) + (sr * sy);
|
||||
data[1][0] = cp * sy;
|
||||
data[1][1] = (sr * sp * sy) + (cr * cy);
|
||||
data[1][2] = (cr * sp * sy) - (sr * cy);
|
||||
data[2][0] = -sp;
|
||||
data[2][1] = sr * cp;
|
||||
data[2][2] = cr * cp;
|
||||
}
|
||||
|
||||
Vector<3> to_euler(void) const {
|
||||
Vector<3> euler;
|
||||
euler.data[1] = asinf(-data[2][0]);
|
||||
|
||||
if (fabsf(euler.data[1] - M_PI_2_F) < 1.0e-3f) {
|
||||
euler.data[0] = 0.0f;
|
||||
euler.data[2] = atan2f(data[1][2] - data[0][1], data[0][2] + data[1][1]) + euler.data[0];
|
||||
|
||||
} else if (fabsf(euler.data[1] + M_PI_2_F) < 1.0e-3f) {
|
||||
euler.data[0] = 0.0f;
|
||||
euler.data[2] = atan2f(data[1][2] - data[0][1], data[0][2] + data[1][1]) - euler.data[0];
|
||||
|
||||
} else {
|
||||
euler.data[0] = atan2f(data[2][1], data[2][2]);
|
||||
euler.data[2] = atan2f(data[1][0], data[0][0]);
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
@@ -1,46 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
|
||||
* Author: Will Perone <will.perone@gmail.com>
|
||||
* Anton Babushkin <anton.babushkin@me.com>
|
||||
*
|
||||
* 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 Matrix3.cpp
|
||||
*
|
||||
* 3x3 Matrix
|
||||
*/
|
||||
|
||||
#include "Matrix3.hpp"
|
||||
|
||||
namespace math
|
||||
{
|
||||
}
|
||||
@@ -1,356 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
|
||||
* Author: Will Perone <will.perone@gmail.com>
|
||||
* Anton Babushkin <anton.babushkin@me.com>
|
||||
*
|
||||
* 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 Matrix3.hpp
|
||||
*
|
||||
* 3x3 Matrix
|
||||
*/
|
||||
|
||||
#ifndef MATRIX3_HPP
|
||||
#define MATRIX3_HPP
|
||||
|
||||
#include "Vector3.hpp"
|
||||
#include "../CMSIS/Include/arm_math.h"
|
||||
|
||||
namespace math
|
||||
{
|
||||
|
||||
// 3x3 matrix with elements of type T
|
||||
template <typename T>
|
||||
class Matrix3 {
|
||||
public:
|
||||
/**
|
||||
* matrix data[row][col]
|
||||
*/
|
||||
T data[3][3];
|
||||
|
||||
/**
|
||||
* struct for using arm_math functions
|
||||
*/
|
||||
arm_matrix_instance_f32 arm_mat;
|
||||
|
||||
/**
|
||||
* trivial ctor
|
||||
* note that this ctor will not initialize elements
|
||||
*/
|
||||
Matrix3<T>() {
|
||||
arm_mat = {3, 3, &data[0][0]};
|
||||
}
|
||||
|
||||
/**
|
||||
* setting ctor
|
||||
*/
|
||||
Matrix3<T>(const T d[3][3]) {
|
||||
arm_mat = {3, 3, &data[0][0]};
|
||||
memcpy(data, d, sizeof(data));
|
||||
}
|
||||
|
||||
/**
|
||||
* setting ctor
|
||||
*/
|
||||
Matrix3<T>(const T ax, const T ay, const T az, const T bx, const T by, const T bz, const T cx, const T cy, const T cz) {
|
||||
arm_mat = {3, 3, &data[0][0]};
|
||||
data[0][0] = ax;
|
||||
data[0][1] = ay;
|
||||
data[0][2] = az;
|
||||
data[1][0] = bx;
|
||||
data[1][1] = by;
|
||||
data[1][2] = bz;
|
||||
data[2][0] = cx;
|
||||
data[2][1] = cy;
|
||||
data[2][2] = cz;
|
||||
}
|
||||
|
||||
/**
|
||||
* casting from a vector3f to a matrix is the tilde operator
|
||||
*/
|
||||
Matrix3<T>(const Vector3<T> &v) {
|
||||
arm_mat = {3, 3, &data[0][0]};
|
||||
data[0][0] = 0;
|
||||
data[0][1] = -v.z;
|
||||
data[0][2] = v.y;
|
||||
data[1][0] = v.z;
|
||||
data[1][1] = 0;
|
||||
data[1][2] = -v.x;
|
||||
data[2][0] = -v.y;
|
||||
data[2][1] = v.x;
|
||||
data[2][2] = 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* access by index
|
||||
*/
|
||||
inline T &operator ()(unsigned int row, unsigned int col) {
|
||||
return data[row][col];
|
||||
}
|
||||
|
||||
/**
|
||||
* access to elements by index
|
||||
*/
|
||||
inline const T &operator ()(unsigned int row, unsigned int col) const {
|
||||
return data[row][col];
|
||||
}
|
||||
|
||||
/**
|
||||
* set to value
|
||||
*/
|
||||
const Matrix3<T> &operator =(const Matrix3<T> &m) {
|
||||
memcpy(data, m.data, sizeof(data));
|
||||
return *this;
|
||||
}
|
||||
|
||||
/**
|
||||
* test for equality
|
||||
*/
|
||||
bool operator ==(const Matrix3<T> &m) {
|
||||
for (int i = 0; i < 3; i++)
|
||||
for (int j = 0; j < 3; j++)
|
||||
if (data[i][j] != m(i, j))
|
||||
return false;
|
||||
return true;
|
||||
}
|
||||
|
||||
/**
|
||||
* test for inequality
|
||||
*/
|
||||
bool operator !=(const Matrix3<T> &m) {
|
||||
for (int i = 0; i < 3; i++)
|
||||
for (int j = 0; j < 3; j++)
|
||||
if (data[i][j] != m(i, j))
|
||||
return true;
|
||||
return false;
|
||||
}
|
||||
|
||||
/**
|
||||
* negation
|
||||
*/
|
||||
Matrix3<T> operator -(void) const {
|
||||
Matrix3<T> res;
|
||||
for (int i = 0; i < 3; i++)
|
||||
for (int j = 0; j < 3; j++)
|
||||
res[i][j] = -data[i][j];
|
||||
return res;
|
||||
}
|
||||
|
||||
/**
|
||||
* addition
|
||||
*/
|
||||
Matrix3<T> operator +(const Matrix3<T> &m) const {
|
||||
Matrix3<T> res;
|
||||
for (int i = 0; i < 3; i++)
|
||||
for (int j = 0; j < 3; j++)
|
||||
res[i][j] = data[i][j] + m(i, j);
|
||||
return res;
|
||||
}
|
||||
|
||||
Matrix3<T> &operator +=(const Matrix3<T> &m) {
|
||||
return *this = *this + m;
|
||||
}
|
||||
|
||||
/**
|
||||
* subtraction
|
||||
*/
|
||||
Matrix3<T> operator -(const Matrix3<T> &m) const {
|
||||
Matrix3<T> res;
|
||||
for (int i = 0; i < 3; i++)
|
||||
for (int j = 0; j < 3; j++)
|
||||
res[i][j] = data[i][j] - m(i, j);
|
||||
return res;
|
||||
}
|
||||
|
||||
Matrix3<T> &operator -=(const Matrix3<T> &m) {
|
||||
return *this = *this - m;
|
||||
}
|
||||
|
||||
/**
|
||||
* uniform scaling
|
||||
*/
|
||||
Matrix3<T> operator *(const T num) const {
|
||||
Matrix3<T> res;
|
||||
for (int i = 0; i < 3; i++)
|
||||
for (int j = 0; j < 3; j++)
|
||||
res[i][j] = data[i][j] * num;
|
||||
return res;
|
||||
}
|
||||
|
||||
Matrix3<T> &operator *=(const T num) {
|
||||
return *this = *this * num;
|
||||
}
|
||||
|
||||
Matrix3<T> operator /(const T num) const {
|
||||
Matrix3<T> res;
|
||||
for (int i = 0; i < 3; i++)
|
||||
for (int j = 0; j < 3; j++)
|
||||
res[i][j] = data[i][j] / num;
|
||||
return res;
|
||||
}
|
||||
|
||||
Matrix3<T> &operator /=(const T num) {
|
||||
return *this = *this / num;
|
||||
}
|
||||
|
||||
/**
|
||||
* multiplication by a vector
|
||||
*/
|
||||
Vector3<T> operator *(const Vector3<T> &v) const {
|
||||
return Vector3<T>(
|
||||
data[0][0] * v.x + data[0][1] * v.y + data[0][2] * v.z,
|
||||
data[1][0] * v.x + data[1][1] * v.y + data[1][2] * v.z,
|
||||
data[2][0] * v.x + data[2][1] * v.y + data[2][2] * v.z);
|
||||
}
|
||||
|
||||
/**
|
||||
* multiplication of transpose by a vector
|
||||
*/
|
||||
Vector3<T> mul_transpose(const Vector3<T> &v) const {
|
||||
return Vector3<T>(
|
||||
data[0][0] * v.x + data[1][0] * v.y + data[2][0] * v.z,
|
||||
data[0][1] * v.x + data[1][1] * v.y + data[2][1] * v.z,
|
||||
data[0][2] * v.x + data[1][2] * v.y + data[2][2] * v.z);
|
||||
}
|
||||
|
||||
/**
|
||||
* multiplication by another matrix
|
||||
*/
|
||||
Matrix3<T> operator *(const Matrix3<T> &m) const {
|
||||
#if defined(CONFIG_ARCH_CORTEXM4) && defined(CONFIG_ARCH_FPU)
|
||||
Matrix3<T> res;
|
||||
arm_mat_mult_f32(&arm_mat, &m.arm_mat, &res.arm_mat);
|
||||
return res;
|
||||
#else
|
||||
return Matrix3<T>(data[0][0] * m(0, 0) + data[0][1] * m(1, 0) + data[0][2] * m(2, 0),
|
||||
data[0][0] * m(0, 1) + data[0][1] * m(1, 1) + data[0][2] * m(2, 1),
|
||||
data[0][0] * m(0, 2) + data[0][1] * m(1, 2) + data[0][2] * m(2, 2),
|
||||
data[1][0] * m(0, 0) + data[1][1] * m(1, 0) + data[1][2] * m(2, 0),
|
||||
data[1][0] * m(0, 1) + data[1][1] * m(1, 1) + data[1][2] * m(2, 1),
|
||||
data[1][0] * m(0, 2) + data[1][1] * m(1, 2) + data[1][2] * m(2, 2),
|
||||
data[2][0] * m(0, 0) + data[2][1] * m(1, 0) + data[2][2] * m(2, 0),
|
||||
data[2][0] * m(0, 1) + data[2][1] * m(1, 1) + data[2][2] * m(2, 1),
|
||||
data[2][0] * m(0, 2) + data[2][1] * m(1, 2) + data[2][2] * m(2, 2));
|
||||
#endif
|
||||
}
|
||||
|
||||
Matrix3<T> &operator *=(const Matrix3<T> &m) {
|
||||
return *this = *this * m;
|
||||
}
|
||||
|
||||
/**
|
||||
* transpose the matrix
|
||||
*/
|
||||
Matrix3<T> transposed(void) const {
|
||||
#if defined(CONFIG_ARCH_CORTEXM4) && defined(CONFIG_ARCH_FPU) && T == float
|
||||
Matrix3<T> res;
|
||||
arm_mat_trans_f32(&arm_mat, &res.arm_mat);
|
||||
return res;
|
||||
#else
|
||||
return Matrix3<T>(data[0][0], data[1][0], data[2][0],
|
||||
data[0][1], data[1][1], data[2][1],
|
||||
data[0][2], data[1][2], data[2][2]);
|
||||
#endif
|
||||
}
|
||||
|
||||
/**
|
||||
* inverse the matrix
|
||||
*/
|
||||
Matrix3<T> inversed(void) const {
|
||||
Matrix3<T> res;
|
||||
arm_mat_inverse_f32(&arm_mat, &res.arm_mat);
|
||||
return res;
|
||||
}
|
||||
|
||||
/**
|
||||
* zero the matrix
|
||||
*/
|
||||
void zero(void) {
|
||||
memset(data, 0, sizeof(data));
|
||||
}
|
||||
|
||||
/**
|
||||
* setup the identity matrix
|
||||
*/
|
||||
void identity(void) {
|
||||
memset(data, 0, sizeof(data));
|
||||
data[0][0] = 1;
|
||||
data[1][1] = 1;
|
||||
data[2][2] = 1;
|
||||
}
|
||||
|
||||
/**
|
||||
* check if any elements are NAN
|
||||
*/
|
||||
bool is_nan(void) {
|
||||
for (int i = 0; i < 3; i++)
|
||||
for (int j = 0; j < 3; j++)
|
||||
if (isnan(data[i][j]))
|
||||
return true;
|
||||
return false;
|
||||
}
|
||||
|
||||
/**
|
||||
* create a rotation matrix from given euler angles
|
||||
* based on http://gentlenav.googlecode.com/files/EulerAngles.pdf
|
||||
*/
|
||||
void from_euler(T roll, T pitch, T yaw) {
|
||||
T cp = (T)cosf(pitch);
|
||||
T sp = (T)sinf(pitch);
|
||||
T sr = (T)sinf(roll);
|
||||
T cr = (T)cosf(roll);
|
||||
T sy = (T)sinf(yaw);
|
||||
T cy = (T)cosf(yaw);
|
||||
|
||||
data[0][0] = cp * cy;
|
||||
data[0][1] = (sr * sp * cy) - (cr * sy);
|
||||
data[0][2] = (cr * sp * cy) + (sr * sy);
|
||||
data[1][0] = cp * sy;
|
||||
data[1][1] = (sr * sp * sy) + (cr * cy);
|
||||
data[1][2] = (cr * sp * sy) - (sr * cy);
|
||||
data[2][0] = -sp;
|
||||
data[2][1] = sr * cp;
|
||||
data[2][2] = cr * cp;
|
||||
}
|
||||
|
||||
// create eulers from a rotation matrix
|
||||
//void to_euler(float *roll, float *pitch, float *yaw);
|
||||
|
||||
// apply an additional rotation from a body frame gyro vector
|
||||
// to a rotation matrix.
|
||||
//void rotate(const Vector3<T> &g);
|
||||
};
|
||||
|
||||
typedef Matrix3<float> Matrix3f;
|
||||
}
|
||||
|
||||
#endif // MATRIX3_HPP
|
||||
@@ -44,15 +44,17 @@
|
||||
|
||||
#include <math.h>
|
||||
#include "../CMSIS/Include/arm_math.h"
|
||||
#include "Vector.hpp"
|
||||
#include "Matrix.hpp"
|
||||
|
||||
namespace math
|
||||
{
|
||||
|
||||
class Quaternion {
|
||||
public:
|
||||
float real;
|
||||
Vector3<float> imag;
|
||||
template <unsigned int N, unsigned int M>
|
||||
class Matrix;
|
||||
|
||||
class Quaternion : public Vector<4> {
|
||||
public:
|
||||
/**
|
||||
* trivial ctor
|
||||
*/
|
||||
@@ -62,7 +64,74 @@ public:
|
||||
/**
|
||||
* setting ctor
|
||||
*/
|
||||
Quaternion(const float a, const float b, const float c, const float d): real(a), imag(b, c, d) {
|
||||
Quaternion(const float a0, const float b0, const float c0, const float d0): Vector(a0, b0, c0, d0) {
|
||||
}
|
||||
|
||||
Quaternion(const Vector<4> &v) : Vector(v) {
|
||||
}
|
||||
|
||||
Quaternion(const float *v) : Vector(v) {
|
||||
}
|
||||
|
||||
/**
|
||||
* access to elements by index
|
||||
*/
|
||||
/*
|
||||
inline float &operator ()(unsigned int i) {
|
||||
return *(&a + i);
|
||||
}
|
||||
*/
|
||||
|
||||
/**
|
||||
* access to elements by index
|
||||
*/
|
||||
/*
|
||||
inline const float &operator ()(unsigned int i) const {
|
||||
return *(&a + i);
|
||||
}
|
||||
*/
|
||||
|
||||
/**
|
||||
* addition
|
||||
*/
|
||||
/*
|
||||
const Quaternion operator +(const Quaternion &q) const {
|
||||
return Quaternion(a + q.a, b + q.b, c + q.c, d + q.d);
|
||||
}
|
||||
*/
|
||||
|
||||
/**
|
||||
* subtraction
|
||||
*/
|
||||
/*
|
||||
const Quaternion operator -(const Quaternion &q) const {
|
||||
return Quaternion(a - q.a, b - q.b, c - q.c, d - q.d);
|
||||
}
|
||||
*/
|
||||
|
||||
Quaternion derivative(const Vector<3> &w) {
|
||||
float dataQ[] = {
|
||||
data[0], -data[1], -data[2], -data[3],
|
||||
data[1], data[0], -data[3], data[2],
|
||||
data[2], data[3], data[0], -data[1],
|
||||
data[3], -data[2], data[1], data[0]
|
||||
};
|
||||
Matrix<4,4> Q(dataQ);
|
||||
Vector<4> v(0.0f, w.data[0], w.data[1], w.data[2]);
|
||||
return Q * v * 0.5f;
|
||||
}
|
||||
|
||||
void from_euler(float roll, float pitch, float yaw) {
|
||||
double cosPhi_2 = cos(double(roll) / 2.0);
|
||||
double sinPhi_2 = sin(double(roll) / 2.0);
|
||||
double cosTheta_2 = cos(double(pitch) / 2.0);
|
||||
double sinTheta_2 = sin(double(pitch) / 2.0);
|
||||
double cosPsi_2 = cos(double(yaw) / 2.0);
|
||||
double sinPsi_2 = sin(double(yaw) / 2.0);
|
||||
data[0] = cosPhi_2 * cosTheta_2 * cosPsi_2 + sinPhi_2 * sinTheta_2 * sinPsi_2;
|
||||
data[1] = sinPhi_2 * cosTheta_2 * cosPsi_2 - cosPhi_2 * sinTheta_2 * sinPsi_2;
|
||||
data[2] = cosPhi_2 * sinTheta_2 * cosPsi_2 + sinPhi_2 * cosTheta_2 * sinPsi_2;
|
||||
data[3] = cosPhi_2 * cosTheta_2 * sinPsi_2 - sinPhi_2 * sinTheta_2 * cosPsi_2;
|
||||
}
|
||||
};
|
||||
}
|
||||
|
||||
+189
-41
@@ -42,38 +42,50 @@
|
||||
#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 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]};
|
||||
}
|
||||
|
||||
/**
|
||||
* copy ctor
|
||||
*/
|
||||
VectorBase(const VectorBase<N> &v) {
|
||||
arm_col = {N, 1, &data[0]};
|
||||
memcpy(data, v.data, sizeof(data));
|
||||
}
|
||||
|
||||
/**
|
||||
* setting ctor
|
||||
*/
|
||||
// VectorBase<N>(const float *d) {
|
||||
// memcpy(data, d, sizeof(data));
|
||||
//arm_col = {N, 1, &data[0]};
|
||||
//}
|
||||
|
||||
/**
|
||||
* 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,35 +254,113 @@ 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];
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -276,6 +374,56 @@ public:
|
||||
}
|
||||
};
|
||||
|
||||
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;
|
||||
}
|
||||
*/
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
#endif // VECTOR_HPP
|
||||
|
||||
@@ -1,269 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
|
||||
* Author: Will Perone <will.perone@gmail.com>
|
||||
* Anton Babushkin <anton.babushkin@me.com>
|
||||
*
|
||||
* 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 Vector2.hpp
|
||||
*
|
||||
* 2D Vector
|
||||
*/
|
||||
|
||||
#ifndef VECTOR2_HPP
|
||||
#define VECTOR2_HPP
|
||||
|
||||
#include <math.h>
|
||||
|
||||
namespace math
|
||||
{
|
||||
|
||||
template <typename T>
|
||||
class Vector2 {
|
||||
public:
|
||||
T x, y;
|
||||
|
||||
/**
|
||||
* trivial ctor
|
||||
*/
|
||||
Vector2<T>() {
|
||||
}
|
||||
|
||||
/**
|
||||
* setting ctor
|
||||
*/
|
||||
Vector2<T>(const T x0, const T y0): x(x0), y(y0) {
|
||||
}
|
||||
|
||||
/**
|
||||
* setter
|
||||
*/
|
||||
void set(const T x0, const T y0) {
|
||||
x = x0;
|
||||
y = y0;
|
||||
}
|
||||
|
||||
/**
|
||||
* access to elements by index
|
||||
*/
|
||||
T operator ()(unsigned int i) {
|
||||
return *(&x + i);
|
||||
}
|
||||
|
||||
/**
|
||||
* access to elements by index
|
||||
*/
|
||||
const T operator ()(unsigned int i) const {
|
||||
return *(&x + i);
|
||||
}
|
||||
|
||||
/**
|
||||
* test for equality
|
||||
*/
|
||||
bool operator ==(const Vector2<T> &v) {
|
||||
return (x == v.x && y == v.y);
|
||||
}
|
||||
|
||||
/**
|
||||
* test for inequality
|
||||
*/
|
||||
bool operator !=(const Vector2<T> &v) {
|
||||
return (x != v.x || y != v.y);
|
||||
}
|
||||
|
||||
/**
|
||||
* set to value
|
||||
*/
|
||||
const Vector2<T> &operator =(const Vector2<T> &v) {
|
||||
x = v.x;
|
||||
y = v.y;
|
||||
return *this;
|
||||
}
|
||||
|
||||
/**
|
||||
* negation
|
||||
*/
|
||||
const Vector2<T> operator -(void) const {
|
||||
return Vector2<T>(-x, -y);
|
||||
}
|
||||
|
||||
/**
|
||||
* addition
|
||||
*/
|
||||
const Vector2<T> operator +(const Vector2<T> &v) const {
|
||||
return Vector2<T>(x + v.x, y + v.y);
|
||||
}
|
||||
|
||||
/**
|
||||
* subtraction
|
||||
*/
|
||||
const Vector2<T> operator -(const Vector2<T> &v) const {
|
||||
return Vector2<T>(x - v.x, y - v.y);
|
||||
}
|
||||
|
||||
/**
|
||||
* uniform scaling
|
||||
*/
|
||||
const Vector2<T> operator *(const T num) const {
|
||||
Vector2<T> temp(*this);
|
||||
return temp *= num;
|
||||
}
|
||||
|
||||
/**
|
||||
* uniform scaling
|
||||
*/
|
||||
const Vector2<T> operator /(const T num) const {
|
||||
Vector2<T> temp(*this);
|
||||
return temp /= num;
|
||||
}
|
||||
|
||||
/**
|
||||
* addition
|
||||
*/
|
||||
const Vector2<T> &operator +=(const Vector2<T> &v) {
|
||||
x += v.x;
|
||||
y += v.y;
|
||||
return *this;
|
||||
}
|
||||
|
||||
/**
|
||||
* subtraction
|
||||
*/
|
||||
const Vector2<T> &operator -=(const Vector2<T> &v) {
|
||||
x -= v.x;
|
||||
y -= v.y;
|
||||
return *this;
|
||||
}
|
||||
|
||||
/**
|
||||
* uniform scaling
|
||||
*/
|
||||
const Vector2<T> &operator *=(const T num) {
|
||||
x *= num;
|
||||
y *= num;
|
||||
return *this;
|
||||
}
|
||||
|
||||
/**
|
||||
* uniform scaling
|
||||
*/
|
||||
const Vector2<T> &operator /=(const T num) {
|
||||
x /= num;
|
||||
y /= num;
|
||||
return *this;
|
||||
}
|
||||
|
||||
/**
|
||||
* dot product
|
||||
*/
|
||||
T operator *(const Vector2<T> &v) const {
|
||||
return x * v.x + y * v.y;
|
||||
}
|
||||
|
||||
/**
|
||||
* cross product
|
||||
*/
|
||||
const float operator %(const Vector2<T> &v) const {
|
||||
return x * v.y - y * v.x;
|
||||
}
|
||||
|
||||
/**
|
||||
* gets the length of this vector squared
|
||||
*/
|
||||
float length_squared() const {
|
||||
return (*this * *this);
|
||||
}
|
||||
|
||||
/**
|
||||
* gets the length of this vector
|
||||
*/
|
||||
float length() const {
|
||||
return (T)sqrt(*this * *this);
|
||||
}
|
||||
|
||||
/**
|
||||
* normalizes this vector
|
||||
*/
|
||||
void normalize() {
|
||||
*this /= length();
|
||||
}
|
||||
|
||||
/**
|
||||
* returns the normalized version of this vector
|
||||
*/
|
||||
Vector2<T> normalized() const {
|
||||
return *this / length();
|
||||
}
|
||||
|
||||
/**
|
||||
* reflects this vector about n
|
||||
*/
|
||||
void reflect(const Vector2<T> &n)
|
||||
{
|
||||
Vector2<T> orig(*this);
|
||||
project(n);
|
||||
*this = *this * 2 - orig;
|
||||
}
|
||||
|
||||
/**
|
||||
* projects this vector onto v
|
||||
*/
|
||||
void project(const Vector2<T> &v) {
|
||||
*this = v * (*this * v) / (v * v);
|
||||
}
|
||||
|
||||
/**
|
||||
* returns this vector projected onto v
|
||||
*/
|
||||
Vector2<T> projected(const Vector2<T> &v) {
|
||||
return v * (*this * v) / (v * v);
|
||||
}
|
||||
|
||||
/**
|
||||
* computes the angle between 2 arbitrary vectors
|
||||
*/
|
||||
static inline float angle(const Vector2<T> &v1, const Vector2<T> &v2) {
|
||||
return acosf((v1 * v2) / (v1.length() * v2.length()));
|
||||
}
|
||||
|
||||
/**
|
||||
* computes the angle between 2 arbitrary normalized vectors
|
||||
*/
|
||||
static inline float angle_normalized(const Vector2<T> &v1, const Vector2<T> &v2) {
|
||||
return acosf(v1 * v2);
|
||||
}
|
||||
};
|
||||
|
||||
typedef Vector2<float> Vector2f;
|
||||
}
|
||||
|
||||
#endif // VECTOR2_HPP
|
||||
@@ -1,287 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
|
||||
* Author: Will Perone <will.perone@gmail.com>
|
||||
* Anton Babushkin <anton.babushkin@me.com>
|
||||
*
|
||||
* 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 Vector3.hpp
|
||||
*
|
||||
* 3D Vector
|
||||
*/
|
||||
|
||||
#ifndef VECTOR3_HPP
|
||||
#define VECTOR3_HPP
|
||||
|
||||
#include <math.h>
|
||||
#include "../CMSIS/Include/arm_math.h"
|
||||
|
||||
namespace math
|
||||
{
|
||||
|
||||
template <typename T>
|
||||
class Vector3 {
|
||||
public:
|
||||
T x, y, z;
|
||||
arm_matrix_instance_f32 arm_col;
|
||||
|
||||
/**
|
||||
* trivial ctor
|
||||
*/
|
||||
Vector3<T>() {
|
||||
arm_col = {3, 1, &x};
|
||||
}
|
||||
|
||||
/**
|
||||
* setting ctor
|
||||
*/
|
||||
Vector3<T>(const T x0, const T y0, const T z0): x(x0), y(y0), z(z0) {
|
||||
arm_col = {3, 1, &x};
|
||||
}
|
||||
|
||||
/**
|
||||
* setting ctor
|
||||
*/
|
||||
Vector3<T>(const T data[3]): x(data[0]), y(data[1]), z(data[2]) {
|
||||
arm_col = {3, 1, &x};
|
||||
}
|
||||
|
||||
/**
|
||||
* setter
|
||||
*/
|
||||
void set(const T x0, const T y0, const T z0) {
|
||||
x = x0;
|
||||
y = y0;
|
||||
z = z0;
|
||||
}
|
||||
|
||||
/**
|
||||
* access to elements by index
|
||||
*/
|
||||
T operator ()(unsigned int i) {
|
||||
return *(&x + i);
|
||||
}
|
||||
|
||||
/**
|
||||
* access to elements by index
|
||||
*/
|
||||
const T operator ()(unsigned int i) const {
|
||||
return *(&x + i);
|
||||
}
|
||||
|
||||
/**
|
||||
* test for equality
|
||||
*/
|
||||
bool operator ==(const Vector3<T> &v) {
|
||||
return (x == v.x && y == v.y && z == v.z);
|
||||
}
|
||||
|
||||
/**
|
||||
* test for inequality
|
||||
*/
|
||||
bool operator !=(const Vector3<T> &v) {
|
||||
return (x != v.x || y != v.y || z != v.z);
|
||||
}
|
||||
|
||||
/**
|
||||
* set to value
|
||||
*/
|
||||
const Vector3<T> &operator =(const Vector3<T> &v) {
|
||||
x = v.x;
|
||||
y = v.y;
|
||||
z = v.z;
|
||||
return *this;
|
||||
}
|
||||
|
||||
/**
|
||||
* negation
|
||||
*/
|
||||
const Vector3<T> operator -(void) const {
|
||||
return Vector3<T>(-x, -y, -z);
|
||||
}
|
||||
|
||||
/**
|
||||
* addition
|
||||
*/
|
||||
const Vector3<T> operator +(const Vector3<T> &v) const {
|
||||
return Vector3<T>(x + v.x, y + v.y, z + v.z);
|
||||
}
|
||||
|
||||
/**
|
||||
* subtraction
|
||||
*/
|
||||
const Vector3<T> operator -(const Vector3<T> &v) const {
|
||||
return Vector3<T>(x - v.x, y - v.y, z - v.z);
|
||||
}
|
||||
|
||||
/**
|
||||
* uniform scaling
|
||||
*/
|
||||
const Vector3<T> operator *(const T num) const {
|
||||
Vector3<T> temp(*this);
|
||||
return temp *= num;
|
||||
}
|
||||
|
||||
/**
|
||||
* uniform scaling
|
||||
*/
|
||||
const Vector3<T> operator /(const T num) const {
|
||||
Vector3<T> temp(*this);
|
||||
return temp /= num;
|
||||
}
|
||||
|
||||
/**
|
||||
* addition
|
||||
*/
|
||||
const Vector3<T> &operator +=(const Vector3<T> &v) {
|
||||
x += v.x;
|
||||
y += v.y;
|
||||
z += v.z;
|
||||
return *this;
|
||||
}
|
||||
|
||||
/**
|
||||
* subtraction
|
||||
*/
|
||||
const Vector3<T> &operator -=(const Vector3<T> &v) {
|
||||
x -= v.x;
|
||||
y -= v.y;
|
||||
z -= v.z;
|
||||
return *this;
|
||||
}
|
||||
|
||||
/**
|
||||
* uniform scaling
|
||||
*/
|
||||
const Vector3<T> &operator *=(const T num) {
|
||||
x *= num;
|
||||
y *= num;
|
||||
z *= num;
|
||||
return *this;
|
||||
}
|
||||
|
||||
/**
|
||||
* uniform scaling
|
||||
*/
|
||||
const Vector3<T> &operator /=(const T num) {
|
||||
x /= num;
|
||||
y /= num;
|
||||
z /= num;
|
||||
return *this;
|
||||
}
|
||||
|
||||
/**
|
||||
* dot product
|
||||
*/
|
||||
T operator *(const Vector3<T> &v) const {
|
||||
return x * v.x + y * v.y + z * v.z;
|
||||
}
|
||||
|
||||
/**
|
||||
* cross product
|
||||
*/
|
||||
const Vector3<T> operator %(const Vector3<T> &v) const {
|
||||
Vector3<T> temp(y * v.z - z * v.y, z * v.x - x * v.z, x * v.y - y * v.x);
|
||||
return temp;
|
||||
}
|
||||
|
||||
/**
|
||||
* gets the length of this vector squared
|
||||
*/
|
||||
float length_squared() const {
|
||||
return (*this * *this);
|
||||
}
|
||||
|
||||
/**
|
||||
* gets the length of this vector
|
||||
*/
|
||||
float length() const {
|
||||
return (T)sqrt(*this * *this);
|
||||
}
|
||||
|
||||
/**
|
||||
* normalizes this vector
|
||||
*/
|
||||
void normalize() {
|
||||
*this /= length();
|
||||
}
|
||||
|
||||
/**
|
||||
* returns the normalized version of this vector
|
||||
*/
|
||||
Vector3<T> normalized() const {
|
||||
return *this / length();
|
||||
}
|
||||
|
||||
/**
|
||||
* reflects this vector about n
|
||||
*/
|
||||
void reflect(const Vector3<T> &n)
|
||||
{
|
||||
Vector3<T> orig(*this);
|
||||
project(n);
|
||||
*this = *this * 2 - orig;
|
||||
}
|
||||
|
||||
/**
|
||||
* projects this vector onto v
|
||||
*/
|
||||
void project(const Vector3<T> &v) {
|
||||
*this = v * (*this * v) / (v * v);
|
||||
}
|
||||
|
||||
/**
|
||||
* returns this vector projected onto v
|
||||
*/
|
||||
Vector3<T> projected(const Vector3<T> &v) {
|
||||
return v * (*this * v) / (v * v);
|
||||
}
|
||||
|
||||
/**
|
||||
* computes the angle between 2 arbitrary vectors
|
||||
*/
|
||||
static inline float angle(const Vector3<T> &v1, const Vector3<T> &v2) {
|
||||
return acosf((v1 * v2) / (v1.length() * v2.length()));
|
||||
}
|
||||
|
||||
/**
|
||||
* computes the angle between 2 arbitrary normalized vectors
|
||||
*/
|
||||
static inline float angle_normalized(const Vector3<T> &v1, const Vector3<T> &v2) {
|
||||
return acosf(v1 * v2);
|
||||
}
|
||||
};
|
||||
|
||||
typedef Vector3<float> Vector3f;
|
||||
}
|
||||
|
||||
#endif // VECTOR3_HPP
|
||||
@@ -42,10 +42,7 @@
|
||||
#pragma once
|
||||
|
||||
#include "math/Vector.hpp"
|
||||
#include "math/Vector2.hpp"
|
||||
#include "math/Vector3.hpp"
|
||||
#include "math/Matrix.hpp"
|
||||
#include "math/Matrix3.hpp"
|
||||
#include "math/Quaternion.hpp"
|
||||
#include "math/Limits.hpp"
|
||||
|
||||
|
||||
@@ -35,7 +35,6 @@
|
||||
# Math library
|
||||
#
|
||||
SRCS = math/test/test.cpp \
|
||||
math/Matrix3.cpp \
|
||||
math/Limits.cpp
|
||||
|
||||
#
|
||||
|
||||
Reference in New Issue
Block a user