dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Fix AngleAxis constructors (#117)

This commit is contained in:
kritz 2019-12-28 23:18:15 +01:00 committed by Daniel Agar
parent e81483a808
commit d18be0d0fa
1 changed files with 6 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
matrix/AxisAngle.hpp
View File

@ -69,16 +69,11 @@ public:
AxisAngle(const Quaternion<Type> &q)
{
AxisAngle &v = *this;
Type ang = Type(2) * acos(q(0));
Type mag = sin(ang/2.0f);
if (fabs(mag) > 0) {
v(0) = ang*q(1)/mag;
v(1) = ang*q(2)/mag;
v(2) = ang*q(3)/mag;
Type mag = Type(sqrt(q(1) * q(1) + q(2) * q(2) + q(3) * q(3)));
if (fabs(mag) >= Type(1e-10)) {
v = q.imag() * Type(Type(2) * atan2(mag, q(0)) / mag);
} else {
v(0) = 0;
v(1) = 0;
v(2) = 0;
v = q.imag() * Type(Type(2) * Type(sign(q(0))));
}
}
@ -93,7 +88,7 @@ public:
AxisAngle(const Dcm<Type> &dcm)
{
AxisAngle &v = *this;
v = Quaternion<Type>(dcm);
v = AxisAngle<Type>(Quaternion<Type>(dcm));
}
/**
@ -108,7 +103,7 @@ public:
AxisAngle(const Euler<Type> &euler)
{
AxisAngle &v = *this;
v = Quaternion<Type>(euler);
v = AxisAngle<Type>(Quaternion<Type>(euler));
}
/**