.. _program_listing_file_conversions_attitude_conversions.cc:

Program Listing for File attitude_conversions.cc
================================================

|exhale_lsh| :ref:`Return to documentation for file <file_conversions_attitude_conversions.cc>` (``conversions/attitude_conversions.cc``)

.. |exhale_lsh| unicode:: U+021B0 .. UPWARDS ARROW WITH TIP LEFTWARDS

.. code-block:: cpp

   
   
   #include "lupnt/conversions/attitude_conversions.h"
   
   namespace lupnt {
   
     Vec3 RotToAngularVelocity(const Mat3& rot_b2w, const Mat3& rot_dot_b2w) {
       Mat3 Omega = rot_dot_b2w * rot_b2w.transpose();
       Omega = 0.5 * (Omega - Omega.transpose());
       return Vec3(Omega(2, 1), Omega(0, 2), Omega(1, 0));
     }
   
     State ScalarFirstToLast(const State& quat) {
       return NormalizeQuat(State(Vec4(quat(3), quat(0), quat(1), quat(2))));
     }
     State ScalarLastToFirst(const State& quat) {
       return NormalizeQuat(State(Vec4(quat(1), quat(2), quat(3), quat(0))));
     }
   
     State NormalizeQuat(const State& quat) {
       State q = quat.normalized();
       if (q(0) < 0) q = -q;
       return q;  // [qw, qx, qy, qz] = [scalar, vector]
     }
   
     Mat3 QuatToRot(const State& quat) {
       Eigen::Quaternion<Real> q(quat(0), quat(1), quat(2), quat(3));  // [w, x, y, z]
       return q.toRotationMatrix();
     }
   
     State RotToQuat(const Mat3& rot) {
       Eigen::Quaternion<Real> q(rot);
       return NormalizeQuat(State(Vec4(q.w(), q.x(), q.y(), q.z())));
     }
   
     Mat3 RollPitchYawToRot(const Vec3& rpy) {
       Eigen::AngleAxis<Real> roll(rpy(0), Vec3::UnitX());
       Eigen::AngleAxis<Real> pitch(rpy(1), Vec3::UnitY());
       Eigen::AngleAxis<Real> yaw(rpy(2), Vec3::UnitZ());
       return (yaw * pitch * roll).toRotationMatrix();
     }
   
     Vec3 RotToRollPitchYaw(const Mat3& rot) {
       auto ypr = rot.eulerAngles(2, 1, 0);
       return Vec3(ypr[2], ypr[1], ypr[0]);
     }
   
     // Vector definitions
     VEC_IMP_VECTOR(ScalarFirstToLast, 4);
     VEC_IMP_VECTOR(ScalarLastToFirst, 4);
     VEC_IMP_VECTOR(NormalizeQuat, 4);
   
   }  // namespace lupnt