fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-03 19:28:52 +08:00
Code Issues Packages Projects Releases Wiki Activity

Fix AngleAxis constructors (#117)

Browse Source
This commit is contained in:
kritz
2019-12-28 17:18:15 -05:00
committed by Daniel Agar
parent e81483a808
commit d18be0d0fa
1 changed files with 6 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
matrix/AxisAngle.hpp
+6 -11
View File
@@ -69,16 +69,11 @@ public:
AxisAngle(const Quaternion<Type> &q) AxisAngle(const Quaternion<Type> &q)
{ {
AxisAngle &v = *this; AxisAngle &v = *this;
Type ang = Type(2) * acos(q(0)); Type mag = Type(sqrt(q(1) * q(1) + q(2) * q(2) + q(3) * q(3)));
Type mag = sin(ang/2.0f); if (fabs(mag) >= Type(1e-10)) {
if (fabs(mag) > 0) { v = q.imag() * Type(Type(2) * atan2(mag, q(0)) / mag);
v(0) = ang*q(1)/mag;
v(1) = ang*q(2)/mag;
v(2) = ang*q(3)/mag;
} else { } else {
v(0) = 0; v = q.imag() * Type(Type(2) * Type(sign(q(0))));
v(1) = 0;
v(2) = 0;
} }
} }
@@ -93,7 +88,7 @@ public:
AxisAngle(const Dcm<Type> &dcm) AxisAngle(const Dcm<Type> &dcm)
{ {
AxisAngle &v = *this; AxisAngle &v = *this;
v = Quaternion<Type>(dcm); v = AxisAngle<Type>(Quaternion<Type>(dcm));
} }
/** /**
@@ -108,7 +103,7 @@ public:
AxisAngle(const Euler<Type> &euler) AxisAngle(const Euler<Type> &euler)
{ {
AxisAngle &v = *this; AxisAngle &v = *this;
v = Quaternion<Type>(euler); v = AxisAngle<Type>(Quaternion<Type>(euler));
} }
/** /**