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

Dcm: more efficient conversion from quaternion, extend reuse of multiplications

Browse Source
This commit is contained in:
Matthias Grob
2017-09-26 11:20:56 +02:00
parent baf54ad29f
commit 976461eb0f
1 changed files with 23 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files
matrix/Dcm.hpp
+23 -17
View File
@@ -80,23 +80,29 @@ public:
Dcm(const Quaternion<Type> &q)
{
Dcm &dcm = *this;
Type a = q(0);
Type b = q(1);
Type c = q(2);
Type d = q(3);
Type aSq = a * a;
Type bSq = b * b;
Type cSq = c * c;
Type dSq = d * d;
dcm(0, 0) = aSq + bSq - cSq - dSq;
dcm(0, 1) = 2 * (b * c - a * d);
dcm(0, 2) = 2 * (a * c + b * d);
dcm(1, 0) = 2 * (b * c + a * d);
dcm(1, 1) = aSq - bSq + cSq - dSq;
dcm(1, 2) = 2 * (c * d - a * b);
dcm(2, 0) = 2 * (b * d - a * c);
dcm(2, 1) = 2 * (a * b + c * d);
dcm(2, 2) = aSq - bSq - cSq + dSq;
const Type a = q(0);
const Type b = q(1);
const Type c = q(2);
const Type d = q(3);
const Type aa = a * a;
const Type ab = a * b;
const Type ac = a * c;
const Type ad = a * d;
const Type bb = b * b;
const Type bc = b * c;
const Type bd = b * d;
const Type cc = c * c;
const Type cd = c * d;
const Type dd = d * d;
dcm(0, 0) = aa + bb - cc - dd;
dcm(0, 1) = 2 * (bc - ad);
dcm(0, 2) = 2 * (ac + bd);
dcm(1, 0) = 2 * (bc + ad);
dcm(1, 1) = aa - bb + cc - dd;
dcm(1, 2) = 2 * (cd - ab);
dcm(2, 0) = 2 * (bd - ac);
dcm(2, 1) = 2 * (ab + cd);
dcm(2, 2) = aa - bb - cc + dd;
}
/**