AngleAxis.h

Go to the documentation of this file.

/*

             *
            ***
           *****
   *********************       Mercenaries Engineering SARL
     *****************              Copyright (C) 2025
       *************
         *********        http://www.mercenaries-engineering.com
        ***********
       ****     ****
      **           **

*/
#pragma once

#include <ImathQuat.h>

namespace maquina
{

template<class T>
class AngleAxis
{
public:
  AngleAxis() : angle_axis(0.0) {}
  AngleAxis(T x, T y, T z) : angle_axis(x, y, z) {}
  AngleAxis(const Imath::Quat<T>& q)
  {
    T theta = 2.0 * atan2(q.v.length(), q.r);
    const Imath::Vec3<T> w = theta != 0.0 ? q.v / sin(theta/T(2)) : Imath::Vec3<T>{0};
    angle_axis = theta*w;
  }

  Imath::Quat<T> to_quat() const
  {
    const T theta = angle_axis.length();
    const Imath::Vec3<T> w = theta == 0.0 ? Imath::Vec3<T>{0.0} : angle_axis/theta;
    return Imath::Quat<T>(cos(theta), sin(theta)*w);
  }

  Imath::Vec3<T> angle_axis;
};

using AngleAxisf = AngleAxis<float>;
using AngleAxisd = AngleAxis<double>;

}