Browse Source

AP_Math: quaternion: add from_euler(Vector3&)

gps-1.3.1
Peter Barker 4 years ago committed by Andrew Tridgell
parent
commit
118d3f085f
  1. 5
      libraries/AP_Math/quaternion.cpp
  2. 1
      libraries/AP_Math/quaternion.h

5
libraries/AP_Math/quaternion.cpp

@ -417,6 +417,11 @@ void QuaternionT<T>::from_euler(T roll, T pitch, T yaw) @@ -417,6 +417,11 @@ void QuaternionT<T>::from_euler(T roll, T pitch, T yaw)
q3 = cr2*sp2*cy2 + sr2*cp2*sy2;
q4 = cr2*cp2*sy2 - sr2*sp2*cy2;
}
template <typename T>
void QuaternionT<T>::from_euler(const Vector3<T> &v)
{
from_euler(v[0], v[1], v[2]);
}
// create a quaternion from Euler angles applied in yaw, roll, pitch order
// instead of the normal yaw, pitch, roll order

1
libraries/AP_Math/quaternion.h

@ -81,6 +81,7 @@ public: @@ -81,6 +81,7 @@ public:
// create a quaternion from Euler angles
void from_euler(T roll, T pitch, T yaw);
void from_euler(const Vector3<T> &v);
// create a quaternion from Euler angles applied in yaw, roll, pitch order
// instead of the normal yaw, pitch, roll order

Loading…
Cancel
Save