Program Listing for File world_scoped_maps.cpp

↰ Return to documentation for file (src/tuw_geometry/world_scoped_maps.cpp)

#include <tgmath.h>

#include <cfloat>
#include <iomanip>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <tuw_geometry/utils.hpp>
#include <tuw_geometry/world_scoped_maps.hpp>

using namespace tuw;

WorldScopedMaps::WorldScopedMaps()
: width_pixel_(-1), height_pixel_(-1), min_x_(0), max_x_(0), min_y_(0), max_y_(0)
{
}

bool WorldScopedMaps::initialized() {return (width_pixel_ != -1) && (height_pixel_ != -1);}

void WorldScopedMaps::init()
{
  dx_ = max_x_ - min_x_;
  dy_ = max_y_ - min_y_;
  sx_ = width_pixel_ / dx_;
  sy_ = height_pixel_ / dy_;
  ox_ = width_pixel_ / 2.0;
  oy_ = height_pixel_ / 2.0;
  double ca = cos(rotation_), sa = sin(rotation_);
  mx_ = min_x_ + dx_ / 2.;
  my_ = min_y_ + dy_ / 2.;
  cv::Matx<double, 3, 3> Tw(1, 0, -mx_, 0, 1, -my_, 0, 0, 1);  // translation
  cv::Matx<double, 3, 3> Sc(sx_, 0, 0, 0, sy_, 0, 0, 0, 1);    // scaling
  cv::Matx<double, 3, 3> Sp(-1, 0, 0, 0, 1, 0, 0, 0, 1);       // mirroring
  cv::Matx<double, 3, 3> R(ca, -sa, 0, sa, ca, 0, 0, 0, 1);    // rotation
  cv::Matx<double, 3, 3> Tm(1, 0, ox_, 0, 1, oy_, 0, 0, 1);    // translation
  Mw2m_ = Tm * R * Sp * Sc * Tw;

  Mm2w_ = Mw2m_.inv();
}
void WorldScopedMaps::init(
  int width_pixel, int height_pixel, double min_x, double max_x, double min_y, double max_y,
  double rotation)
{
  width_pixel_ = width_pixel, height_pixel_ = height_pixel;
  min_y_ = std::min(min_y, max_y);
  max_y_ = std::max(min_y, max_y);
  min_x_ = std::min(min_x, max_x);
  max_x_ = std::max(min_x, max_x);
  rotation_ = rotation;

  init();
}

const cv::Matx33d & WorldScopedMaps::Mw2m() const {return Mw2m_;}
const cv::Matx33d & WorldScopedMaps::Mm2w() const {return Mm2w_;}
Point2D WorldScopedMaps::w2m(const Point2D & src) const {return Mw2m_ * src;}
Point2D WorldScopedMaps::w2m(double x, double y) const {return w2m(Point2D(x, y));}
Point2D & WorldScopedMaps::w2m(const Point2D & src, Point2D & des) const
{
  des = Mw2m_ * src;
  return des;
}
Point2D WorldScopedMaps::m2w(const Point2D & src) const {return Mm2w_ * src;}
Point2D WorldScopedMaps::m2w(double x, double y) const {return m2w(Point2D(x, y));}
Point2D & WorldScopedMaps::m2w(const Point2D & src, Point2D & des) const
{
  des = Mm2w_ * src;
  return des;
}
double WorldScopedMaps::max_x() const {return max_x_;}
double WorldScopedMaps::min_x() const {return min_x_;}
double WorldScopedMaps::scale_x() const {return sx_;}
double WorldScopedMaps::max_y() const {return max_y_;}
double WorldScopedMaps::min_y() const {return min_y_;}
double WorldScopedMaps::scale_y() const {return sy_;}
int WorldScopedMaps::width() const {return width_pixel_;}
int WorldScopedMaps::height() const {return height_pixel_;}

double WorldScopedMaps::scale_w2m(double v) const {return v * sx_;}
std::string WorldScopedMaps::infoHeader() const
{
  char buffer[0x1FF];
  Point2D p0 = m2w(0, 0);
  Point2D p1 = m2w(width_pixel_ / 2, height_pixel_ / 2);
  sprintf(
    buffer,
    "%4i,%4i [px];  %6.2f, %6.2f [m] => %6.2f, %6.2f [px/m]; 0, 0 [px] = %6.2f, %6.2f [m] @ %3.2f "
    "[rad]; %4i, %4i [px] = %6.2f, %6.2f [m] @ %3.2f [rad]",
    width_pixel_, height_pixel_, dx_, dy_, sx_, sy_, p0.x(), p0.y(), rotation_, width_pixel_ / 2,
    height_pixel_ / 2, p1.x(), p1.y(), rotation_);
  return std::string(buffer);
}