mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-14 10:07:39 +08:00
284 lines
9.1 KiB
C++
284 lines
9.1 KiB
C++
/****************************************************************************
|
|
*
|
|
* 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(void) {
|
|
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);
|
|
}
|
|
|
|
/**
|
|
* 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(void) 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
|