mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-23 23:23:34 +08:00
refactoring rotation classes and adding initial description
This commit is contained in:
+62
-1
@@ -20,6 +20,11 @@ class Quaternion;
|
||||
template<typename Type>
|
||||
class Euler;
|
||||
|
||||
/**
|
||||
* Direction cosine matrix class
|
||||
*
|
||||
* More elaborate class description
|
||||
*/
|
||||
template<typename Type>
|
||||
class Dcm : public Matrix<Type, 3, 3>
|
||||
{
|
||||
@@ -28,20 +33,69 @@ public:
|
||||
|
||||
typedef Matrix<Type, 3, 1> Vector3;
|
||||
|
||||
/**
|
||||
* Standard constructor
|
||||
*
|
||||
* Initializes to identity
|
||||
*/
|
||||
Dcm() : Matrix<Type, 3, 3>()
|
||||
{
|
||||
(*this) = eye<Type, 3>();
|
||||
}
|
||||
|
||||
/**
|
||||
* Constructor from array
|
||||
*
|
||||
* Copies the given scalar value to all matrix fields.
|
||||
*
|
||||
* @param other array
|
||||
*/
|
||||
Dcm(const Type *data_) : Matrix<Type, 3, 3>(data_)
|
||||
{
|
||||
}
|
||||
|
||||
/**
|
||||
* Copy constructor
|
||||
*
|
||||
* More elaborate function description
|
||||
*
|
||||
* @param other Matrix33 to set dcm to
|
||||
*/
|
||||
Dcm(const Matrix<Type, 3, 3> &other) : Matrix<Type, 3, 3>(other)
|
||||
{
|
||||
}
|
||||
|
||||
/**
|
||||
* Constructor from quaternion
|
||||
*
|
||||
* More elaborate function description
|
||||
*
|
||||
* @param q quaternion to set dcm to
|
||||
*/
|
||||
Dcm(const Quaternion<Type> & q) {
|
||||
set_from_quaternion(q);
|
||||
}
|
||||
|
||||
/**
|
||||
* Constructor from euler angles
|
||||
*
|
||||
* More elaborate function description
|
||||
*
|
||||
* @param euler euler angles to set dcm to
|
||||
*/
|
||||
Dcm(const Euler<Type> & euler) {
|
||||
set_from_euler(euler);
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* Set from quaternion
|
||||
*
|
||||
* More elaborate function description
|
||||
*
|
||||
* @param q quaternion to set dcm to
|
||||
*/
|
||||
void set_from_quaternion(const Quaternion<Type> & q) {
|
||||
Dcm &dcm = *this;
|
||||
Type a = q(0);
|
||||
Type b = q(1);
|
||||
@@ -62,7 +116,14 @@ public:
|
||||
dcm(2, 2) = aSq - bSq - cSq + dSq;
|
||||
}
|
||||
|
||||
Dcm(const Euler<Type> & euler) {
|
||||
/**
|
||||
* Set from euler angles
|
||||
*
|
||||
* More elaborate function description
|
||||
*
|
||||
* @param euler euler angles to set dcm to
|
||||
*/
|
||||
void set_from_euler(const Euler<Type> & euler) {
|
||||
Dcm &dcm = *this;
|
||||
Type cosPhi = Type(cos(euler.phi()));
|
||||
Type sinPhi = Type(sin(euler.phi()));
|
||||
|
||||
Reference in New Issue
Block a user