/home/hauberg/Dokumenter/Capture/humim-tracker-0.1/src/OpenTissue/OpenTissue/core/math/math_coordsys.h

Go to the documentation of this file.

#ifndef OPENTISSUE_CORE_MATH_MATH_COORDSYS_H
#define OPENTISSUE_CORE_MATH_MATH_COORDSYS_H
//
// OpenTissue Template Library
// - A generic toolbox for physics-based modeling and simulation.
// Copyright (C) 2008 Department of Computer Science, University of Copenhagen.
//
// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php
//
#include <OpenTissue/configuration.h>

#include <OpenTissue/core/math/math_vector3.h>
#include <OpenTissue/core/math/math_matrix3x3.h>
#include <OpenTissue/core/math/math_quaternion.h>
#include <OpenTissue/core/math/math_value_traits.h>

#include <iosfwd>

namespace OpenTissue
{

  namespace math
  {

    template<
      typename value_type_
      //, typename value_traits_ = ValueTraits<value_type_> 
    >
    class CoordSys
    {
    protected:

      typedef typename OpenTissue::math::ValueTraits<value_type_>  value_traits_ ;  // TODO value_traits_ should be parameterized as a class template parameter.

    public:

      typedef value_traits_           value_traits;  
      typedef value_type_             value_type;    

      typedef Vector3<value_type>     vector3_type;
      typedef Quaternion<value_type>  quaternion_type;
      typedef Matrix3x3<value_type>   matrix3x3_type;
      typedef Quaternion<value_type>  rotation_type; 

    protected:

      vector3_type     m_T;      
      quaternion_type  m_Q;      

    public:

      vector3_type       & T()       { return m_T; }
      vector3_type const & T() const { return m_T; }
      quaternion_type       & Q()       { return m_Q; }
      quaternion_type const & Q() const { return m_Q; }

    public:

      CoordSys()
        : m_T(value_traits::zero(),value_traits::zero(),value_traits::zero())
        , m_Q(value_traits::one(),value_traits::zero(),value_traits::zero(),value_traits::zero())
      {}

      explicit CoordSys(vector3_type const & T_val, quaternion_type const & Q_val) 
      {  
        m_T = T_val;
        m_Q = unit(Q_val);
      }

      explicit CoordSys(vector3_type const & T_val, matrix3x3_type const & R_val) 
      {
        m_T = T_val;
        m_Q = R_val;
      }

      CoordSys & operator=(CoordSys const & C)
      {
        m_T = C.m_T;
        m_Q = C.m_Q;
        return *this;
      }

    public:

      // TODO: Comparing floats with == or != is not safe NOTE T might not be a float type it could be anything? This suggest that we need some kinf of metaprogramming technique to deal with ths problem?
      bool operator==(CoordSys const & C) const {  return m_T == C.m_T && m_Q==C.m_Q; }

    public:

      void identity()
      {
        m_T.clear();
        m_Q.identity();
      }

      void xform_point(vector3_type & p) const {    p = m_Q.rotate(p) + m_T;  }

      void xform_vector(vector3_type & v) const  {     v = m_Q.rotate(v);   }

      void xform_matrix(matrix3x3_type & O) const
      {
        O = matrix3x3_type(m_Q) * O;
      }

      CoordSys operator*(CoordSys const & X1) const
      {
        return CoordSys(    m_Q.rotate(X1.T()) + m_T , unit( prod( m_Q , X1.Q()) )     );
      }

    public:

      bool is_equal(CoordSys const & C, value_type const & accuracy) const
      {
        return m_T.is_equal(C.m_T,accuracy) && m_Q.is_equal(C.m_Q,accuracy);
      }

    };  // class CoordSys



    template<typename T>
    inline CoordSys<T> prod(CoordSys<T> const & L, CoordSys<T> const & R)
    {
      return CoordSys<T>(    L.Q().rotate( R.T() )  +  L.T() , unit( prod( L.Q() , R.Q()) ) );
    }

    template<typename T>
    inline CoordSys<T> inverse(CoordSys<T> const & X)
    {
      //---
      //---   p' = R p + T
      //---
      //---  =>
      //---
      //---   p = R^{-1} p' + R^{-1}(-T)
      //---
      return CoordSys<T>(  conj(X.Q()).rotate(-X.T()) , conj(X.Q()) );
    }

    template<typename T>
    inline CoordSys<T> model_update(CoordSys<T> const & A, CoordSys<T> const & B)
    {
      return model_update(A.T(),A.Q(),B.T(),B.Q());
    }

    template<typename T>
    inline CoordSys<T> model_update(Vector3<T> const & TA, Quaternion<T> const & QA, Vector3<T> const & TB, Quaternion<T> const & QB)
    {
      //---
      //---  p' = RA p + TA         (*1)  from A->WCS
      //---
      //---  p = RB^T (p' - TB)     (*2)  from WCS-B
      //---
      //--- Insert (*1) into (*2)  A -> B
      //---
      //---   p = RB^T ( RA p + TA - TB)
      //---     =  RB^T  RA p + RB^T (TA - TB)
      //--- So
      //---   R = RB^T  RA
      //---   T = RB^T (TA - TB)
      //---
      Quaternion<T> q;
      if(QA==QB)
      {
        q.identity();
      }
      else
      {
        q = unit( prod( conj(QB), QA) );
      }
      return CoordSys<T>( conj(QB).rotate(TA - TB),  q);
    }

    template<typename T>
    inline std::ostream & operator<< (std::ostream & o, CoordSys<T> const & C)
    {
      o << "[" << C.T() << "," << C.Q() << "]";
      return o;
    }

    template<typename T>
    inline std::istream & operator>>(std::istream & i, CoordSys<T> & C)
    {
      char dummy;
      i >> dummy;
      i >> C.T();
      i >> dummy;
      i >> C.Q();
      i >> dummy;
      return i;
    }

  }  // namespace math

} // namespace OpenTissue

//OPENTISSUE_CORE_MATH_MATH_COORDSYS_H
#endif