rotation.hh

// Copyright (C) 2007, 2008, 2009, 2010 EPITA Research and Development
// Laboratory (LRDE)
//
// This file is part of Olena.
//
// Olena is free software: you can redistribute it and/or modify it under
// the terms of the GNU General Public License as published by the Free
// Software Foundation, version 2 of the License.
//
// Olena is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with Olena.  If not, see <http://www.gnu.org/licenses/>.
//
// As a special exception, you may use this file as part of a free
// software project without restriction.  Specifically, if other files
// instantiate templates or use macros or inline functions from this
// file, or you compile this file and link it with other files to produce
// an executable, this file does not by itself cause the resulting
// executable to be covered by the GNU General Public License.  This
// exception does not however invalidate any other reasons why the
// executable file might be covered by the GNU General Public License.

#ifndef MLN_FUN_X2X_ROTATION_HH
# define MLN_FUN_X2X_ROTATION_HH


# include <cstdlib>
# include <cmath>

# include <mln/core/concept/function.hh>
# include <mln/fun/internal/x2x_linear_impl.hh>
# include <mln/algebra/vec.hh>
# include <mln/algebra/mat.hh>
# include <mln/algebra/quat.hh>
# include <mln/make/h_mat.hh>

# include <mln/norm/l2.hh>


namespace mln
{

  namespace fun
  {

    namespace x2x
    {

      namespace internal
      {
        // (Axis, angle)-based rotation: general case (not implemented).
        template < unsigned n, typename C >
        algebra::h_mat<n, C>
        get_rot_h_mat(const C alpha_, const algebra::vec<n,C>& axis_)
        {
          std::cerr
            << __FILE__ << ":" << __LINE__ << ": error:"
            << " generic mln::fun::x2x::internal::get_rot_h_mat<n, C>"
            << " not implemented."
            << std::endl;
          std::abort();
        }

        // (Axis, angle)-based rotation: 2D case.
        template <typename C >
        algebra::h_mat<2, C>
        get_rot_h_mat(const C alpha, const algebra::vec<2,C>&)
        {
          const C cos_a = cos(alpha);
          const C sin_a = sin(alpha);

          algebra::h_mat<2, C> m;

          m(0,0) = cos_a; m(0,1) = -sin_a; m(0,2) = 0;
          m(1,0) = sin_a; m(1,1) = cos_a;  m(1,2) = 0;
          m(2,0) = 0;     m(2,1) = 0;      m(2,2) = 1;

          return m;
        }

        // (Axis, angle)-based rotation: 3D case.
        template <typename C >
        algebra::h_mat<3, C>
        get_rot_h_mat(const C alpha, const algebra::vec<3,C>& axis)
        {
          // Ensure axis is valid.
          typedef algebra::vec<3,C> vec_t;
          // FIXME: This check is not precise enought when the vector
          // holds floating point values.
          mln_precondition(axis != vec_t(literal::zero));

          algebra::vec<3,C> normed_axis = axis;
          normed_axis.normalize();

          const C cos_a = cos(alpha);
          const C sin_a = sin(alpha);
          const C u = normed_axis[0];
          const C v = normed_axis[1];
          const C w = normed_axis[2];
          const C u2 = u * u;
          const C v2 = v * v;
          const C w2 = w * w;

          algebra::h_mat<3, C> m;

          m(0,0) = u2 + (1 - u2) * cos_a;
          m(0,1) = u * v * (1 - cos_a) - w * sin_a;
          m(0,2) = u * w * (1 - cos_a) + v * sin_a;
          m(0,3) = 0;

          m(1,0) = u * v * (1 - cos_a) + w * sin_a;
          m(1,1) = v2 + (1 - v2) * cos_a;
          m(1,2) = v * w * (1 - cos_a) - u * sin_a;
          m(1,3) = 0;

          m(2,0) = u * w * (1 - cos_a) - v * sin_a;
          m(2,1) = v * w * (1 - cos_a) + u * sin_a;
          m(2,2) = w2 + (1 - w2) * cos_a;
          m(2,3) = 0;

          m(3,0) = 0;
          m(3,1) = 0;
          m(3,2) = 0;
          m(3,3) = 1;

          return m;
        }

      } // end of namespace internal


      template <unsigned n, typename C>
      struct rotation
        : fun::internal::x2x_linear_impl_< algebra::vec<n,C>, C, rotation<n,C> >,
          public Function_v2v< rotation<n,C> >
      {
        typedef C data_t;

        typedef rotation<n,C> invert;
        invert inv() const;

        rotation();
        rotation(C alpha, const algebra::vec<n,C>& axis);
        rotation(const algebra::quat& q);
        rotation(const algebra::h_mat<n,C>& m);

        algebra::vec<n,C> operator()(const algebra::vec<n,C>& v) const;

        void set_alpha(C alpha);
        void set_axis(const algebra::vec<n,C>& axis);

      protected:
        void update();
        bool check_rotation(const algebra::quat& q);

        /* FIXME: Is it useful to keep these values, since they are
           primarily used to build the matrix `m_'?  */
        C alpha_;
        algebra::vec<n,C> axis_;
      };


# ifndef MLN_INCLUDE_ONLY

      template <unsigned n, typename C>
      inline
      rotation<n,C>::rotation()
      {
      }

      template <unsigned n, typename C>
      inline
      rotation<n,C>::rotation(C alpha, const algebra::vec<n,C>& axis)
        : alpha_(alpha),
          axis_(axis)
      {
        this->m_ = algebra::h_mat<n,C>::Id;
        update();
      }

      template <unsigned n, typename C>
      inline
      rotation<n,C>::rotation(const algebra::quat& q)
      {
        // FIXME: Should also work for 2D.
        mlc_bool(n == 3)::check();
        mln_precondition(q.is_unit());

        C
          w = q.to_vec()[0],
          x = q.to_vec()[1],  x2 = 2*x*x,  xw = 2*x*w,
          y = q.to_vec()[2],  y2 = 2*y*y,  xy = 2*x*y,  yw = 2*y*w,
          z = q.to_vec()[3],  z2 = 2*z*z,  xz = 2*x*z,  yz = 2*y*z,  zw = 2*z*w;

        C t[9] = {1.f - y2 - z2,  xy - zw,  xz + yw,
                      xy + zw,  1.f - x2 - z2,  yz - xw,
                      xz - yw,  yz + xw,  1.f - x2 - y2};

        this->m_ = mln::make::h_mat(t);
        mln_assertion(check_rotation(q));

        alpha_ = acos(w) * 2;
        axis_[0] = x;
        axis_[1] = y;
        axis_[2] = z;
        axis_.normalize();
      }


      template <unsigned n, typename C>
      inline
      rotation<n,C>::rotation(const algebra::h_mat<n,C>& m)
      {
        this->m_ = m;
      }


      template <unsigned n, typename C>
      inline
      algebra::vec<n,C>
      rotation<n,C>::operator()(const algebra::vec<n,C>& v) const
      {
        algebra::mat<n+1,1,C> hmg;
        algebra::mat<n+1,1,C> tmp;
        algebra::vec<n,C> res;
        for (unsigned i = 0; i < n; ++i)
          hmg(i,0) = v[i];
        hmg(n,0) = 1;
        tmp = this->m_ * hmg;
        mln_assertion(tmp(n,0) == 1);
        for (unsigned i = 0; i < n; ++i)
          res[i] = tmp(i,0);
        return res;
      }

      template <unsigned n, typename C>
      inline
      rotation<n,C>
      rotation<n,C>::inv() const
      {
        typename rotation::invert res(-alpha_, axis_);
        return res;
      }

      template <unsigned n, typename C>
      inline
      void
      rotation<n,C>::set_alpha(C alpha)
      {
        alpha_ = alpha;
        update();
      }

      template <unsigned n, typename C>
      inline
      void
      rotation<n,C>::set_axis(const algebra::vec<n,C>& axis)
      {
        axis_ = axis;
        update();
      }

      // Homogenous matrix for a rotation of a point (x,y,z)
      // about the vector (u,v,w) by the angle alpha.
      template <unsigned n, typename C>
      inline
      void
      rotation<n,C>::update()
      {
        this->m_ = internal::get_rot_h_mat(alpha_, axis_);
      }

      template <unsigned n, typename C>
      inline
      bool
      rotation<n,C>::check_rotation(const algebra::quat& q)
      {
        srand(time(0));
        assert(q.is_unit());

        algebra::vec<n,C>
          tmp = make::vec(rand(), rand(), rand()),
              p = tmp / norm::l2(tmp),
              p_rot_1 = q.rotate(p),
              p_rot_2 = (*this)(p);
        return norm::l2(p_rot_1 - p_rot_2) < mln_epsilon(C);
      }

# endif // ! MLN_INCLUDE_ONLY


    } // end of namespace mln::fun::x2x

  } // end of namespace mln::fun

} // end of namespace mln


#endif // ! MLN_FUN_X2X_ROTATION_HH