t3dPointTools.hpp

/* $Id$ */

#ifndef GEOM_T3D_POINT_TOOLS_HPP
#define GEOM_T3D_POINT_TOOLS_HPP

#include "geom/t3dEuler.hpp"
#include "geom/t3dQuaternion.hpp"

namespace jafar {
  namespace geom {

    namespace t3d {

      template<class VecPtB, class VecPtA>
      void pointFromFrame(T3D const& aToB, VecPtB const& ptB, VecPtA& ptA) {
  JFR_PRECOND(ptB.size() == ptA.size(),
        "t3d::from: size of ptA and ptB does not match: " << ptB.size() << "!=" << ptA.size());

  switch (ptB.size()) {
  case 3:
    ptA.assign( ublas::prod(aToB.getR(), ptB) + aToB.getT() );
    return;
  case 4:
    ptA.assign( ublas::prod(aToB.getM(), ptB) );
    return;
  default:
    JFR_PRECOND(ptB.size() == 3 || ptB.size() == 4,
          "t3d::from: invalid size for ptB: " << ptB.size());
  }
      }

      template<class VecPtB, class MatJT3D, class MatJPtB>
      void pointFromFrameJac(T3D const& aToB, VecPtB const& ptB, MatJT3D& JT3D, MatJPtB& JPtB) 
      {
  JFR_TRACE_BEGIN;
  switch (ptB.size()) {
  case 3:
    switch (aToB.type()) {
    case T3D::EULER:
      t3d::pointFromFrameJacEuler(aToB.getX(), ptB, JT3D, JPtB);
      return;
          case T3D::QUATERNION:
      t3d::pointFromFrameJacQuaternion(aToB.getX(), ptB, JT3D, JPtB);
      return;
    default:
      JFR_RUN_TIME("t3d::pointFromFrameJac: jacobian computation not yet implemented (" 
       << aToB.type() << " " << ptB.size() << ")" );
    } 
  case 4:
    JFR_RUN_TIME("t3d::pointFromFrameJac: jacobian computation not yet implemented (" 
           << aToB.type() << " " << ptB.size() << ")" );
  default:
    JFR_PRECOND(ptB.size() == 3 || ptB.size() == 4,
          "t3d::from: invalid size for ptB: " << ptB.size());
  }
  JFR_TRACE_END("t3d::pointFromFrameJac");
      }

      template<class VecPtB, class MatJT3D, class MatJPtB>
        void pointFromFrameJac(T3D const& aToB, VecPtB const& ptB, VecPtB& ptA, MatJT3D& JT3D, MatJPtB& JPtB) 
      {
        JFR_PRECOND(ptB.size() == ptA.size(),
                    "ptA and ptB must be of same size");
  JFR_TRACE_BEGIN;
  switch (ptB.size()) {
  case 3:
    switch (aToB.type()) {
    case T3D::EULER:
            t3d::pointFromFrame(aToB, ptB, ptA);
      t3d::pointFromFrameJacEuler(aToB.getX(), ptB, JT3D, JPtB);
      return;
          case T3D::QUATERNION:
            t3d::pointFromFrame(aToB, ptB, ptA);
      t3d::pointFromFrameJacQuaternion(aToB.getX(), ptB, JT3D, JPtB);
      return;
    default:
      JFR_RUN_TIME("t3d::pointFromFrameJac: jacobian computation not yet implemented (" 
       << aToB.type() << " " << ptB.size() << ")" );
    } 
  case 4:
    JFR_RUN_TIME("t3d::pointFromFrameJac: jacobian computation not yet implemented (" 
           << aToB.type() << " " << ptB.size() << ")" );
  default:
    JFR_PRECOND(ptB.size() == 3 || ptB.size() == 4,
          "t3d::pointFromFrameJac: invalid size for ptB: " << ptB.size());
  }
  JFR_TRACE_END("t3d::pointFromFrameJac");
      }

      template<class VecPtB, class VecPtA>
      void pointToFrame(T3D const& aToB, VecPtA const& ptA, VecPtB& ptB) 
      {
  JFR_PRECOND(ptA.size() == ptB.size(),
        "t3d::to: size of ptA and ptB does not match: " << ptA.size() << "!=" << ptB.size());

  using namespace ublas;
  using namespace jblas;

  mat33 bToA_R;
  vec3  bToA_t;

  bToA_R.assign(trans( aToB.getR() ));
  bToA_t.assign(-1*prod(bToA_R, aToB.getT()));

  switch (ptA.size()) {
  case 3:
    {
      ptB.assign( ublas::prod(bToA_R, ptA) + bToA_t );
      return;
    }
  case 4:
    {
      mat44 bToA_M;
      // R
      project(bToA_M, range(0,3), range(0,3)).assign(bToA_R);
      // t
      bToA_M(0,3) = bToA_t(0);
      bToA_M(1,3) = bToA_t(1);
      bToA_M(2,3) = bToA_t(2);
      // transformation matrix
      bToA_M(3,0) = 0;
      bToA_M(3,1) = 0;
      bToA_M(3,2) = 0;
      bToA_M(3,3) = 1;
      
      ptB.assign( ublas::prod(bToA_M, ptA) );
      return;
    }
  default:
    JFR_PRECOND(ptA.size() == 3 || ptA.size() == 4,
          "t3d::from: invalid size for ptA: " << ptA.size());
  }
      }

      template<class VecPtB, class MatJT3D, class MatJPtA>
      void pointToFrameJac(T3D const& aToB, VecPtB const& ptA, MatJT3D& JT3D, MatJPtA& JPtA)
      {
  JFR_TRACE_BEGIN;
  switch (ptA.size()) {
  case 3:
    switch (aToB.type()) {
    case T3D::EULER:
      t3d::pointToFrameJacEuler(aToB.getX(), ptA, JT3D, JPtA);
      return;
          case T3D::QUATERNION:
      t3d::pointToFrameJacQuaternion(aToB.getX(), ptA, JT3D, JPtA);
      return;
    default:
      JFR_RUN_TIME("t3d::pointFromFrameJac: jacobian computation not yet implemented (" 
       << aToB.type() << " " << ptA.size() << ")" );
    }  
  case 4:
    JFR_RUN_TIME("t3d::pointFromFrameJac: jacobian computation not yet implemented (" 
       << aToB.type() << " " << ptA.size() << ")" );
  default:
    JFR_PRECOND(ptA.size() == 3 || ptA.size() == 4,
          "t3d::from: invalid size for ptB: " << ptA.size());
  }
  JFR_TRACE_END("t3d::pointFromFrameJac");
      }

      template<class VecPtB, class MatJT3D, class MatJPtA>
        void pointToFrameJac(T3D const& aToB, VecPtB const& ptA, VecPtB& ptB, MatJT3D& JT3D, MatJPtA& JPtA)
      {
        JFR_PRECOND(ptB.size() == ptA.size(),
                    "ptA and ptB must be of same size");
  JFR_TRACE_BEGIN;
  switch (ptA.size()) {
  case 3:
    switch (aToB.type()) {
    case T3D::EULER:
            pointToFrame(aToB, ptA, ptB);
      t3d::pointToFrameJacEuler(aToB.getX(), ptA, JT3D, JPtA);
      return;
          case T3D::QUATERNION:
            pointToFrame(aToB, ptA, ptB);
      t3d::pointToFrameJacQuaternion(aToB.getX(), ptA, JT3D, JPtA);
      return;
    default:
      JFR_RUN_TIME("t3d::pointFromFrameJac: jacobian computation not yet implemented (" 
       << aToB.type() << " " << ptA.size() << ")" );
    }  
  case 4:
    JFR_RUN_TIME("t3d::pointFromFrameJac: jacobian computation not yet implemented (" 
       << aToB.type() << " " << ptA.size() << ")" );
  default:
    JFR_PRECOND(ptA.size() == 3 || ptA.size() == 4,
          "t3d::from: invalid size for ptB: " << ptA.size());
  }
  JFR_TRACE_END("t3d::pointFromFrameJac");
      }

    } // namespace t3d

  } // namespace geom
} // namespace jafar

#endif // GEOM_T3D_POINT_TOOLS_HPP