tools.hpp

/* $Id$ */

#ifndef _TOOLS_HPP_
#define _TOOLS_HPP_

#include <vector>
#include "jmath/jblas.hpp"
#include "jmath/ublasExtra.hpp"

#define BIG 100000

namespace jafar {
  namespace sams {
    namespace tools {
      // 
      // jblas::vec f2m (jblas::vec const & v, jblas::mat const & t) {
      //  jblas::vec res(4);
      //  res(0) = floor(t(0,0)*v(0)+t(0,1)*v(1)+t(0,2)+0.5);
      //  res(1) = floor(t(1,0)*v(0)+t(1,1)*v(1)+t(1,2)+0.5);
      //  res(2) = atan2(t(1,0),t(0,0))+v(2);
      //  while (res(2) < -M_PI) res(2) += 2.0F * M_PI;
      //  while (res(2) > M_PI) res(2) -= 2.0F * M_PI;
      //  res(3) = v(3) * sqrt(pow(t(0,0),2)+pow(t(1,0),2));
      //  return res;
      // };
      // 
      // jblas::vec m2f (jblas::vec const & v, jblas::mat const & t) {
      //  jblas::mat t_inv(3,3);
      //  jafar::jmath::ublasExtra::lu_inv(t,t_inv);
      //  jblas::vec res(4);
      //  res(0) = floor(t_inv(0,0)*v(0)+t_inv(0,1)*v(1)+t_inv(0,2)+0.5);
      //  res(1) = floor(t_inv(1,0)*v(0)+t_inv(1,1)*v(1)+t_inv(1,2)+0.5);
      //  res(2) = atan2(t_inv(1,0),t_inv(0,0))+v(2);
      //  while (res(2) < -M_PI) res(2) += 2.0F * M_PI;
      //  while (res(2) > M_PI) res(2) -= 2.0F * M_PI;
      //  res(3) = v(3) * sqrt(pow(t_inv(0,0),2)+pow(t_inv(1,0),2));
      //  return res;
      // };
      // 

      template <class ForwardIterator>
      void map_delete (ForwardIterator first, ForwardIterator last) {
        while (first != last) delete (first++)->second;
      };

      double lap(int dim_,
        jblas::mat& assigncost_,
        ublas::vector<int>& rowsol_,
        ublas::vector<int>& colsol_,
        ublas::vector<int>& u_,
        ublas::vector<int>& v_);

    } // namespace tools
  } // namespace sams
} // namespace jafar


#endif /* _TOOLS_HPP_ */