fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-02 00:41:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Added vector2.

Browse Source
This commit is contained in:
jgoppert
2015-11-07 14:27:12 -05:00
parent ace2751715
commit 9b995e19f3
12 changed files with 160 additions and 45 deletions
Download Patch File Download Diff File Expand all files Collapse all files
matrix/Quaternion.hpp
+8 -10
View File
@@ -25,6 +25,9 @@ class Quaternion : public Vector<Type, 4>
public:
virtual ~Quaternion() {};
typedef Matrix<Type, 4, 1> Matrix41;
typedef Matrix<Type, 3, 1> Matrix31;
Quaternion() :
Vector<Type, 4>()
{
@@ -35,12 +38,7 @@ public:
q(3) = 0;
}
Quaternion(const Vector<Type, 4> & other) :
Vector<Type, 4>(other)
{
}
Quaternion(const Matrix<Type, 4, 1> & other) :
Quaternion(const Matrix41 & other) :
Vector<Type, 4>(other)
{
}
@@ -100,7 +98,7 @@ public:
return r;
}
Matrix<Type, 4, 1> derivative(const Vector<Type, 3> & w) const {
Matrix41 derivative(const Matrix31 & w) const {
const Quaternion &q = *this;
Type dataQ[] = {
q(0), -q(1), -q(2), -q(3),
@@ -111,9 +109,9 @@ public:
Matrix<Type, 4, 4> Q(dataQ);
Vector<Type, 4> v;
v(0) = 0;
v(1) = w(0);
v(2) = w(1);
v(3) = w(2);
v(1) = w(0,0);
v(2) = w(1,0);
v(3) = w(2,0);
return Q * v * Type(0.5);
}
};