mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-17 05:07:35 +08:00
mathlib delete Matrix, Quaternion, Vector
This commit is contained in:
Daniel Agar
@@ -1,544 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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 Matrix.hpp
|
||||
*
|
||||
* Matrix class
|
||||
*/
|
||||
|
||||
#ifndef MATRIX_HPP
|
||||
#define MATRIX_HPP
|
||||
|
||||
#include <stdio.h>
|
||||
#include <cmath>
|
||||
|
||||
#include "Vector.hpp" // Vector and eigen_matrix_instance
|
||||
|
||||
#include "matrix/math.hpp"
|
||||
#include <platforms/px4_defines.h>
|
||||
|
||||
namespace math
|
||||
{
|
||||
|
||||
template<unsigned int M, unsigned int N>
|
||||
class __EXPORT Matrix;
|
||||
|
||||
// MxN matrix with float elements
|
||||
template <unsigned int M, unsigned int N>
|
||||
class __EXPORT MatrixBase
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* matrix data[row][col]
|
||||
*/
|
||||
float data[M][N];
|
||||
|
||||
/**
|
||||
* struct for using arm_math functions
|
||||
*/
|
||||
eigen_matrix_instance arm_mat;
|
||||
|
||||
/**
|
||||
* trivial ctor
|
||||
* Initializes the elements to zero.
|
||||
*/
|
||||
MatrixBase() :
|
||||
data{},
|
||||
arm_mat{M, N, &data[0][0]}
|
||||
{
|
||||
}
|
||||
|
||||
~MatrixBase() = default;
|
||||
|
||||
/**
|
||||
* copyt ctor
|
||||
*/
|
||||
MatrixBase(const MatrixBase<M, N> &m) :
|
||||
arm_mat{M, N, &data[0][0]}
|
||||
{
|
||||
memcpy(data, m.data, sizeof(data));
|
||||
}
|
||||
|
||||
MatrixBase(const float *d) :
|
||||
arm_mat{M, N, &data[0][0]}
|
||||
{
|
||||
memcpy(data, d, sizeof(data));
|
||||
}
|
||||
|
||||
MatrixBase(const float d[M][N]) :
|
||||
arm_mat{M, N, &data[0][0]}
|
||||
{
|
||||
memcpy(data, d, sizeof(data));
|
||||
}
|
||||
|
||||
/**
|
||||
* set data
|
||||
*/
|
||||
void set(const float *d)
|
||||
{
|
||||
memcpy(data, d, sizeof(data));
|
||||
}
|
||||
|
||||
/**
|
||||
* set data
|
||||
*/
|
||||
void set(const float d[M][N])
|
||||
{
|
||||
memcpy(data, d, sizeof(data));
|
||||
}
|
||||
|
||||
/**
|
||||
* set row from vector
|
||||
*/
|
||||
void set_row(unsigned int row, const Vector<N> v)
|
||||
{
|
||||
for (unsigned i = 0; i < N; i++) {
|
||||
data[row][i] = v.data[i];
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* set column from vector
|
||||
*/
|
||||
void set_col(unsigned int col, const Vector<M> v)
|
||||
{
|
||||
for (unsigned i = 0; i < M; i++) {
|
||||
data[i][col] = v.data[i];
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* access by index
|
||||
*/
|
||||
float &operator()(const unsigned int row, const unsigned int col)
|
||||
{
|
||||
return data[row][col];
|
||||
}
|
||||
|
||||
/**
|
||||
* access by index
|
||||
*/
|
||||
float operator()(const unsigned int row, const unsigned int col) const
|
||||
{
|
||||
return data[row][col];
|
||||
}
|
||||
|
||||
/**
|
||||
* get rows number
|
||||
*/
|
||||
unsigned int get_rows() const
|
||||
{
|
||||
return M;
|
||||
}
|
||||
|
||||
/**
|
||||
* get columns number
|
||||
*/
|
||||
unsigned int get_cols() const
|
||||
{
|
||||
return N;
|
||||
}
|
||||
|
||||
/**
|
||||
* test for equality
|
||||
*/
|
||||
bool operator ==(const Matrix<M, N> &m) const
|
||||
{
|
||||
for (unsigned int i = 0; i < M; i++) {
|
||||
for (unsigned int j = 0; j < N; j++) {
|
||||
if (data[i][j] != m.data[i][j]) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
/**
|
||||
* test for inequality
|
||||
*/
|
||||
bool operator !=(const Matrix<M, N> &m) const
|
||||
{
|
||||
for (unsigned int i = 0; i < M; i++) {
|
||||
for (unsigned int j = 0; j < N; j++) {
|
||||
if (data[i][j] != m.data[i][j]) {
|
||||
return true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
/**
|
||||
* set to value
|
||||
*/
|
||||
const Matrix<M, N> &operator =(const Matrix<M, N> &m)
|
||||
{
|
||||
memcpy(data, m.data, sizeof(data));
|
||||
return *static_cast<Matrix<M, N>*>(this);
|
||||
}
|
||||
|
||||
/**
|
||||
* negation
|
||||
*/
|
||||
Matrix<M, N> operator -() const
|
||||
{
|
||||
Matrix<M, N> res;
|
||||
|
||||
for (unsigned int i = 0; i < M; i++) {
|
||||
for (unsigned int j = 0; j < N; j++) {
|
||||
res.data[i][j] = -data[i][j];
|
||||
}
|
||||
}
|
||||
|
||||
return res;
|
||||
}
|
||||
|
||||
/**
|
||||
* addition
|
||||
*/
|
||||
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.data[i][j] = data[i][j] + m.data[i][j];
|
||||
}
|
||||
}
|
||||
|
||||
return res;
|
||||
}
|
||||
|
||||
Matrix<M, N> &operator +=(const Matrix<M, N> &m)
|
||||
{
|
||||
for (unsigned int i = 0; i < M; i++) {
|
||||
for (unsigned int j = 0; j < N; j++) {
|
||||
data[i][j] += m.data[i][j];
|
||||
}
|
||||
}
|
||||
|
||||
return *static_cast<Matrix<M, N>*>(this);
|
||||
}
|
||||
|
||||
/**
|
||||
* subtraction
|
||||
*/
|
||||
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.data[i][j] = data[i][j] - m.data[i][j];
|
||||
}
|
||||
}
|
||||
|
||||
return res;
|
||||
}
|
||||
|
||||
Matrix<M, N> &operator -=(const Matrix<M, N> &m)
|
||||
{
|
||||
for (unsigned int i = 0; i < M; i++) {
|
||||
for (unsigned int j = 0; j < N; j++) {
|
||||
data[i][j] -= m.data[i][j];
|
||||
}
|
||||
}
|
||||
|
||||
return *static_cast<Matrix<M, N>*>(this);
|
||||
}
|
||||
|
||||
/**
|
||||
* uniform scaling
|
||||
*/
|
||||
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.data[i][j] = data[i][j] * num;
|
||||
}
|
||||
}
|
||||
|
||||
return res;
|
||||
}
|
||||
|
||||
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 *static_cast<Matrix<M, N>*>(this);
|
||||
}
|
||||
|
||||
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.data[i][j] = data[i][j] / num;
|
||||
}
|
||||
}
|
||||
|
||||
return res;
|
||||
}
|
||||
|
||||
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 *static_cast<Matrix<M, N>*>(this);
|
||||
}
|
||||
|
||||
/**
|
||||
* multiplication by another matrix
|
||||
*/
|
||||
template <unsigned int P>
|
||||
Matrix<M, P> operator *(const Matrix<N, P> &m) const
|
||||
{
|
||||
matrix::Matrix<float, M, N> Me(this->arm_mat.pData);
|
||||
matrix::Matrix<float, N, P> Him(m.arm_mat.pData);
|
||||
matrix::Matrix<float, M, P> Product = Me * Him;
|
||||
Matrix<M, P> res(Product.data());
|
||||
return res;
|
||||
}
|
||||
|
||||
/**
|
||||
* transpose the matrix
|
||||
*/
|
||||
Matrix<N, M> transposed() const
|
||||
{
|
||||
matrix::Matrix<float, M, N> Me(this->arm_mat.pData);
|
||||
Matrix<N, M> res(Me.transpose().data());
|
||||
return res;
|
||||
}
|
||||
|
||||
/**
|
||||
* invert the matrix
|
||||
*/
|
||||
Matrix<M, N> inversed() const
|
||||
{
|
||||
matrix::SquareMatrix<float, M> Me = matrix::Matrix<float, M, N>(this->arm_mat.pData);
|
||||
Matrix<M, N> res(Me.I().data());
|
||||
return res;
|
||||
}
|
||||
|
||||
/**
|
||||
* set zero matrix
|
||||
*/
|
||||
void zero()
|
||||
{
|
||||
memset(data, 0, sizeof(data));
|
||||
}
|
||||
|
||||
/**
|
||||
* set identity matrix
|
||||
*/
|
||||
void identity()
|
||||
{
|
||||
memset(data, 0, sizeof(data));
|
||||
unsigned int n = (M < N) ? M : N;
|
||||
|
||||
for (unsigned int i = 0; i < n; i++) {
|
||||
data[i][i] = 1;
|
||||
}
|
||||
}
|
||||
|
||||
void print()
|
||||
{
|
||||
for (unsigned int i = 0; i < M; i++) {
|
||||
printf("[ ");
|
||||
|
||||
for (unsigned int j = 0; j < N; j++) {
|
||||
printf("%.3f\t", (double)data[i][j]);
|
||||
}
|
||||
|
||||
printf(" ]\n");
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
template <unsigned int M, unsigned int N>
|
||||
class __EXPORT Matrix : public MatrixBase<M, N>
|
||||
{
|
||||
public:
|
||||
using MatrixBase<M, N>::operator *;
|
||||
|
||||
Matrix() : MatrixBase<M, N>() {}
|
||||
|
||||
Matrix(const Matrix<M, N> &m) : MatrixBase<M, N>(m) {}
|
||||
|
||||
Matrix(const float *d) : MatrixBase<M, N>(d) {}
|
||||
|
||||
Matrix(const float d[M][N]) : MatrixBase<M, N>(d) {}
|
||||
|
||||
/**
|
||||
* set to value
|
||||
*/
|
||||
const Matrix<M, N> &operator =(const Matrix<M, N> &m)
|
||||
{
|
||||
memcpy(this->data, m.data, sizeof(this->data));
|
||||
return *this;
|
||||
}
|
||||
|
||||
/**
|
||||
* multiplication by a vector
|
||||
*/
|
||||
Vector<M> operator *(const Vector<N> &v) const
|
||||
{
|
||||
matrix::Matrix<float, M, N> Me(this->arm_mat.pData);
|
||||
matrix::Matrix<float, N, 1> Vec(v.arm_col.pData);
|
||||
matrix::Matrix<float, M, 1> Product = Me * Vec;
|
||||
Vector<M> res(Product.data());
|
||||
return res;
|
||||
}
|
||||
};
|
||||
|
||||
template <>
|
||||
class __EXPORT Matrix<3, 3> : public MatrixBase<3, 3>
|
||||
{
|
||||
public:
|
||||
using MatrixBase<3, 3>::operator *;
|
||||
|
||||
Matrix() : MatrixBase<3, 3>() {}
|
||||
|
||||
Matrix(const Matrix<3, 3> &m) : MatrixBase<3, 3>(m) {}
|
||||
|
||||
Matrix(const float *d) : MatrixBase<3, 3>(d) {}
|
||||
|
||||
Matrix(const float d[3][3]) : MatrixBase<3, 3>(d) {}
|
||||
/**
|
||||
* set data
|
||||
*/
|
||||
void set(const float d[9])
|
||||
{
|
||||
memcpy(data, d, sizeof(data));
|
||||
}
|
||||
|
||||
#if defined(__PX4_ROS)
|
||||
/**
|
||||
* set data from boost::array
|
||||
*/
|
||||
void set(const boost::array<float, 9ul> d)
|
||||
{
|
||||
set(static_cast<const float *>(d.data()));
|
||||
}
|
||||
#endif
|
||||
|
||||
/**
|
||||
* set to value
|
||||
*/
|
||||
const Matrix<3, 3> &operator =(const Matrix<3, 3> &m)
|
||||
{
|
||||
memcpy(this->data, m.data, sizeof(this->data));
|
||||
return *this;
|
||||
}
|
||||
|
||||
/**
|
||||
* multiplication by a vector
|
||||
*/
|
||||
Vector<3> operator *(const Vector<3> &v) const
|
||||
{
|
||||
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 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;
|
||||
}
|
||||
|
||||
/**
|
||||
* get euler angles from rotation matrix
|
||||
*/
|
||||
Vector<3> to_euler() 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]);
|
||||
}
|
||||
|
||||
return euler;
|
||||
}
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
#endif // MATRIX_HPP
|
||||
@@ -1,310 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013-2015 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* 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 Quaternion.hpp
|
||||
*
|
||||
* Quaternion class
|
||||
*
|
||||
* @author Anton Babushkin <anton.babushkin@me.com>
|
||||
* @author Pavel Kirienko <pavel.kirienko@gmail.com>
|
||||
* @author Lorenz Meier <lorenz@px4.io>
|
||||
*/
|
||||
|
||||
#ifndef QUATERNION_HPP
|
||||
#define QUATERNION_HPP
|
||||
|
||||
#include <math.h>
|
||||
|
||||
#include "Vector.hpp"
|
||||
#include "Matrix.hpp"
|
||||
|
||||
namespace math
|
||||
{
|
||||
|
||||
class __EXPORT Quaternion : public Vector<4>
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* trivial ctor
|
||||
*/
|
||||
Quaternion() : Vector<4>() {}
|
||||
|
||||
/**
|
||||
* copy ctor
|
||||
*/
|
||||
Quaternion(const Quaternion &q) : Vector<4>(q) {}
|
||||
|
||||
/**
|
||||
* casting from vector
|
||||
*/
|
||||
Quaternion(const Vector<4> &v) : Vector<4>(v) {}
|
||||
|
||||
/**
|
||||
* setting ctor
|
||||
*/
|
||||
Quaternion(const float d[4]) : Vector<4>(d) {}
|
||||
|
||||
/**
|
||||
* setting ctor
|
||||
*/
|
||||
Quaternion(const float a0, const float b0, const float c0, const float d0): Vector<4>(a0, b0, c0, d0) {}
|
||||
|
||||
using Vector<4>::operator *;
|
||||
|
||||
/**
|
||||
* multiplication
|
||||
*/
|
||||
const Quaternion operator *(const Quaternion &q) const
|
||||
{
|
||||
return Quaternion(
|
||||
data[0] * q.data[0] - data[1] * q.data[1] - data[2] * q.data[2] - data[3] * q.data[3],
|
||||
data[0] * q.data[1] + data[1] * q.data[0] + data[2] * q.data[3] - data[3] * q.data[2],
|
||||
data[0] * q.data[2] - data[1] * q.data[3] + data[2] * q.data[0] + data[3] * q.data[1],
|
||||
data[0] * q.data[3] + data[1] * q.data[2] - data[2] * q.data[1] + data[3] * q.data[0]);
|
||||
}
|
||||
|
||||
/**
|
||||
* division
|
||||
*/
|
||||
Quaternion operator /(const Quaternion &q) const
|
||||
{
|
||||
float norm = q.length_squared();
|
||||
return Quaternion(
|
||||
(data[0] * q.data[0] + data[1] * q.data[1] + data[2] * q.data[2] + data[3] * q.data[3]) / norm,
|
||||
(- data[0] * q.data[1] + data[1] * q.data[0] - data[2] * q.data[3] + data[3] * q.data[2]) / norm,
|
||||
(- data[0] * q.data[2] + data[1] * q.data[3] + data[2] * q.data[0] - data[3] * q.data[1]) / norm,
|
||||
(- data[0] * q.data[3] - data[1] * q.data[2] + data[2] * q.data[1] + data[3] * q.data[0]) / norm
|
||||
);
|
||||
}
|
||||
|
||||
/**
|
||||
* derivative
|
||||
*/
|
||||
const 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;
|
||||
}
|
||||
|
||||
/**
|
||||
* conjugate
|
||||
*/
|
||||
Quaternion conjugated() const
|
||||
{
|
||||
return Quaternion(data[0], -data[1], -data[2], -data[3]);
|
||||
}
|
||||
|
||||
/**
|
||||
* inversed
|
||||
*/
|
||||
Quaternion inversed() const
|
||||
{
|
||||
float norm = length_squared();
|
||||
return Quaternion(data[0] / norm, -data[1] / norm, -data[2] / norm, -data[3] / norm);
|
||||
}
|
||||
|
||||
/**
|
||||
* conjugation
|
||||
*/
|
||||
Vector<3> conjugate(const Vector<3> &v) const
|
||||
{
|
||||
float q0q0 = data[0] * data[0];
|
||||
float q1q1 = data[1] * data[1];
|
||||
float q2q2 = data[2] * data[2];
|
||||
float q3q3 = data[3] * data[3];
|
||||
|
||||
return Vector<3>(
|
||||
v.data[0] * (q0q0 + q1q1 - q2q2 - q3q3) +
|
||||
v.data[1] * 2.0f * (data[1] * data[2] - data[0] * data[3]) +
|
||||
v.data[2] * 2.0f * (data[0] * data[2] + data[1] * data[3]),
|
||||
|
||||
v.data[0] * 2.0f * (data[1] * data[2] + data[0] * data[3]) +
|
||||
v.data[1] * (q0q0 - q1q1 + q2q2 - q3q3) +
|
||||
v.data[2] * 2.0f * (data[2] * data[3] - data[0] * data[1]),
|
||||
|
||||
v.data[0] * 2.0f * (data[1] * data[3] - data[0] * data[2]) +
|
||||
v.data[1] * 2.0f * (data[0] * data[1] + data[2] * data[3]) +
|
||||
v.data[2] * (q0q0 - q1q1 - q2q2 + q3q3)
|
||||
);
|
||||
}
|
||||
|
||||
/**
|
||||
* conjugation with inversed quaternion
|
||||
*/
|
||||
Vector<3> conjugate_inversed(const Vector<3> &v) const
|
||||
{
|
||||
float q0q0 = data[0] * data[0];
|
||||
float q1q1 = data[1] * data[1];
|
||||
float q2q2 = data[2] * data[2];
|
||||
float q3q3 = data[3] * data[3];
|
||||
|
||||
return Vector<3>(
|
||||
v.data[0] * (q0q0 + q1q1 - q2q2 - q3q3) +
|
||||
v.data[1] * 2.0f * (data[1] * data[2] + data[0] * data[3]) +
|
||||
v.data[2] * 2.0f * (data[1] * data[3] - data[0] * data[2]),
|
||||
|
||||
v.data[0] * 2.0f * (data[1] * data[2] - data[0] * data[3]) +
|
||||
v.data[1] * (q0q0 - q1q1 + q2q2 - q3q3) +
|
||||
v.data[2] * 2.0f * (data[2] * data[3] + data[0] * data[1]),
|
||||
|
||||
v.data[0] * 2.0f * (data[1] * data[3] + data[0] * data[2]) +
|
||||
v.data[1] * 2.0f * (data[2] * data[3] - data[0] * data[1]) +
|
||||
v.data[2] * (q0q0 - q1q1 - q2q2 + q3q3)
|
||||
);
|
||||
}
|
||||
|
||||
/**
|
||||
* imaginary part of quaternion
|
||||
*/
|
||||
Vector<3> imag()
|
||||
{
|
||||
return Vector<3>(&data[1]);
|
||||
}
|
||||
|
||||
/**
|
||||
* set quaternion to rotation defined by euler angles
|
||||
*/
|
||||
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);
|
||||
|
||||
/* operations executed in double to avoid loss of precision through
|
||||
* consecutive multiplications. Result stored as float.
|
||||
*/
|
||||
data[0] = static_cast<float>(cosPhi_2 * cosTheta_2 * cosPsi_2 + sinPhi_2 * sinTheta_2 * sinPsi_2);
|
||||
data[1] = static_cast<float>(sinPhi_2 * cosTheta_2 * cosPsi_2 - cosPhi_2 * sinTheta_2 * sinPsi_2);
|
||||
data[2] = static_cast<float>(cosPhi_2 * sinTheta_2 * cosPsi_2 + sinPhi_2 * cosTheta_2 * sinPsi_2);
|
||||
data[3] = static_cast<float>(cosPhi_2 * cosTheta_2 * sinPsi_2 - sinPhi_2 * sinTheta_2 * cosPsi_2);
|
||||
}
|
||||
|
||||
/**
|
||||
* simplified version of the above method to create quaternion representing rotation only by yaw
|
||||
*/
|
||||
void from_yaw(float yaw)
|
||||
{
|
||||
data[0] = cosf(yaw / 2.0f);
|
||||
data[1] = 0.0f;
|
||||
data[2] = 0.0f;
|
||||
data[3] = sinf(yaw / 2.0f);
|
||||
}
|
||||
|
||||
/**
|
||||
* create Euler angles vector from the quaternion
|
||||
*/
|
||||
Vector<3> to_euler() const
|
||||
{
|
||||
return Vector<3>(
|
||||
atan2f(2.0f * (data[0] * data[1] + data[2] * data[3]), 1.0f - 2.0f * (data[1] * data[1] + data[2] * data[2])),
|
||||
asinf(2.0f * (data[0] * data[2] - data[3] * data[1])),
|
||||
atan2f(2.0f * (data[0] * data[3] + data[1] * data[2]), 1.0f - 2.0f * (data[2] * data[2] + data[3] * data[3]))
|
||||
);
|
||||
}
|
||||
|
||||
/**
|
||||
* set quaternion to rotation by DCM
|
||||
* Reference: Shoemake, Quaternions, http://www.cs.ucr.edu/~vbz/resources/quatut.pdf
|
||||
*/
|
||||
void from_dcm(const Matrix<3, 3> &dcm)
|
||||
{
|
||||
float tr = dcm.data[0][0] + dcm.data[1][1] + dcm.data[2][2];
|
||||
|
||||
if (tr > 0.0f) {
|
||||
float s = sqrtf(tr + 1.0f);
|
||||
data[0] = s * 0.5f;
|
||||
s = 0.5f / s;
|
||||
data[1] = (dcm.data[2][1] - dcm.data[1][2]) * s;
|
||||
data[2] = (dcm.data[0][2] - dcm.data[2][0]) * s;
|
||||
data[3] = (dcm.data[1][0] - dcm.data[0][1]) * s;
|
||||
|
||||
} else {
|
||||
/* Find maximum diagonal element in dcm
|
||||
* store index in dcm_i */
|
||||
int dcm_i = 0;
|
||||
|
||||
for (int i = 1; i < 3; i++) {
|
||||
if (dcm.data[i][i] > dcm.data[dcm_i][dcm_i]) {
|
||||
dcm_i = i;
|
||||
}
|
||||
}
|
||||
|
||||
int dcm_j = (dcm_i + 1) % 3;
|
||||
int dcm_k = (dcm_i + 2) % 3;
|
||||
float s = sqrtf((dcm.data[dcm_i][dcm_i] - dcm.data[dcm_j][dcm_j] -
|
||||
dcm.data[dcm_k][dcm_k]) + 1.0f);
|
||||
data[dcm_i + 1] = s * 0.5f;
|
||||
s = 0.5f / s;
|
||||
data[dcm_j + 1] = (dcm.data[dcm_i][dcm_j] + dcm.data[dcm_j][dcm_i]) * s;
|
||||
data[dcm_k + 1] = (dcm.data[dcm_k][dcm_i] + dcm.data[dcm_i][dcm_k]) * s;
|
||||
data[0] = (dcm.data[dcm_k][dcm_j] - dcm.data[dcm_j][dcm_k]) * s;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* create rotation matrix for the quaternion
|
||||
*/
|
||||
Matrix<3, 3> to_dcm() const
|
||||
{
|
||||
Matrix<3, 3> R;
|
||||
float aSq = data[0] * data[0];
|
||||
float bSq = data[1] * data[1];
|
||||
float cSq = data[2] * data[2];
|
||||
float dSq = data[3] * data[3];
|
||||
R.data[0][0] = aSq + bSq - cSq - dSq;
|
||||
R.data[0][1] = 2.0f * (data[1] * data[2] - data[0] * data[3]);
|
||||
R.data[0][2] = 2.0f * (data[0] * data[2] + data[1] * data[3]);
|
||||
R.data[1][0] = 2.0f * (data[1] * data[2] + data[0] * data[3]);
|
||||
R.data[1][1] = aSq - bSq + cSq - dSq;
|
||||
R.data[1][2] = 2.0f * (data[2] * data[3] - data[0] * data[1]);
|
||||
R.data[2][0] = 2.0f * (data[1] * data[3] - data[0] * data[2]);
|
||||
R.data[2][1] = 2.0f * (data[0] * data[1] + data[2] * data[3]);
|
||||
R.data[2][2] = aSq - bSq - cSq + dSq;
|
||||
return R;
|
||||
}
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
#endif // QUATERNION_HPP
|
||||
@@ -1,608 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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 <string.h>
|
||||
|
||||
#include <platforms/ros/eigen_math.h>
|
||||
|
||||
#include <platforms/px4_defines.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
|
||||
*/
|
||||
eigen_matrix_instance arm_col;
|
||||
|
||||
|
||||
/**
|
||||
* trivial ctor
|
||||
* initializes elements to zero
|
||||
*/
|
||||
VectorBase() :
|
||||
data{},
|
||||
arm_col{N, 1, &data[0]}
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
~VectorBase() = default;
|
||||
|
||||
/**
|
||||
* 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 -() 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()
|
||||
{
|
||||
memset(data, 0, sizeof(data));
|
||||
}
|
||||
|
||||
void print()
|
||||
{
|
||||
printf("[ ");
|
||||
|
||||
for (unsigned int i = 0; i < N; i++) {
|
||||
printf("%.3f\t", (double)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];
|
||||
}
|
||||
#if defined(__PX4_ROS)
|
||||
/**
|
||||
* set data from boost::array
|
||||
*/
|
||||
void set(const boost::array<float, 2ul> d)
|
||||
{
|
||||
set(static_cast<const float *>(d.data()));
|
||||
}
|
||||
#endif
|
||||
/**
|
||||
* 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;
|
||||
}
|
||||
#if defined(__PX4_ROS)
|
||||
/**
|
||||
* set data from boost::array
|
||||
*/
|
||||
void set(const boost::array<float, 3ul> d)
|
||||
{
|
||||
set(static_cast<const float *>(d.data()));
|
||||
}
|
||||
#endif
|
||||
|
||||
/**
|
||||
* 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;
|
||||
}
|
||||
#if defined(__PX4_ROS)
|
||||
/**
|
||||
* set data from boost::array
|
||||
*/
|
||||
void set(const boost::array<float, 4ul> d)
|
||||
{
|
||||
set(static_cast<const float *>(d.data()));
|
||||
}
|
||||
#endif
|
||||
|
||||
/**
|
||||
* 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
|
||||
Binary file not shown.
@@ -1,63 +0,0 @@
|
||||
clc
|
||||
clear
|
||||
function out = float_truncate(in, digits)
|
||||
out = round(in*10^digits)
|
||||
out = out/10^digits
|
||||
endfunction
|
||||
|
||||
phi = 0.1
|
||||
theta = 0.2
|
||||
psi = 0.3
|
||||
|
||||
cosPhi = cos(phi)
|
||||
cosPhi_2 = cos(phi/2)
|
||||
sinPhi = sin(phi)
|
||||
sinPhi_2 = sin(phi/2)
|
||||
|
||||
cosTheta = cos(theta)
|
||||
cosTheta_2 = cos(theta/2)
|
||||
sinTheta = sin(theta)
|
||||
sinTheta_2 = sin(theta/2)
|
||||
|
||||
cosPsi = cos(psi)
|
||||
cosPsi_2 = cos(psi/2)
|
||||
sinPsi = sin(psi)
|
||||
sinPsi_2 = sin(psi/2)
|
||||
|
||||
C_nb = [cosTheta*cosPsi, -cosPhi*sinPsi + sinPhi*sinTheta*cosPsi, sinPhi*sinPsi + cosPhi*sinTheta*cosPsi;
|
||||
cosTheta*sinPsi, cosPhi*cosPsi + sinPhi*sinTheta*sinPsi, -sinPhi*cosPsi + cosPhi*sinTheta*sinPsi;
|
||||
-sinTheta, sinPhi*cosTheta, cosPhi*cosTheta]
|
||||
|
||||
disp(C_nb)
|
||||
//C_nb = float_truncate(C_nb,3)
|
||||
//disp(C_nb)
|
||||
|
||||
theta = asin(-C_nb(3,1))
|
||||
phi = atan(C_nb(3,2), C_nb(3,3))
|
||||
psi = atan(C_nb(2,1), C_nb(1,1))
|
||||
printf('phi %f\n', phi)
|
||||
printf('theta %f\n', theta)
|
||||
printf('psi %f\n', psi)
|
||||
|
||||
q = [cosPhi_2*cosTheta_2*cosPsi_2 + sinPhi_2*sinTheta_2*sinPsi_2;
|
||||
sinPhi_2*cosTheta_2*cosPsi_2 - cosPhi_2*sinTheta_2*sinPsi_2;
|
||||
cosPhi_2*sinTheta_2*cosPsi_2 + sinPhi_2*cosTheta_2*sinPsi_2;
|
||||
cosPhi_2*cosTheta_2*sinPsi_2 - sinPhi_2*sinTheta_2*cosPsi_2]
|
||||
|
||||
//q = float_truncate(q,3)
|
||||
|
||||
a = q(1)
|
||||
b = q(2)
|
||||
c = q(3)
|
||||
d = q(4)
|
||||
printf('q: %f %f %f %f\n', a, b, c, d)
|
||||
a2 = a*a
|
||||
b2 = b*b
|
||||
c2 = c*c
|
||||
d2 = d*d
|
||||
|
||||
C2_nb = [a2 + b2 - c2 - d2, 2*(b*c - a*d), 2*(b*d + a*c);
|
||||
2*(b*c + a*d), a2 - b2 + c2 - d2, 2*(c*d - a*b);
|
||||
2*(b*d - a*c), 2*(c*d + a*b), a2 - b2 - c2 + d2]
|
||||
|
||||
disp(C2_nb)
|
||||
Reference in New Issue
Block a user