Program Listing for File pose2d.cpp

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

#include "tuw_geometry/pose2d.hpp"
using namespace tuw;

Pose2D::Pose2D()
: position_(), orientation_(0) {}
Pose2D::Pose2D(const Point2D & p, double orientation_)
: position_(p), orientation_(orientation_), cossin_uptodate_(false)
{
}
Pose2D::Pose2D(const Pose2D & p)
: position_(p.position_), orientation_(p.orientation_), cossin_uptodate_(false)
{
}
Pose2D::Pose2D(double x, double y, double orientation_)
: position_(x, y), orientation_(orientation_), cossin_uptodate_(false)
{
}
Pose2D::Pose2D(const cv::Vec<double, 3> & s)
: position_(s(0), s(1)), orientation_(s(2)), cossin_uptodate_(false)
{
}

Pose2D & Pose2D::set(double x, double y, double phi)
{
  angle_normalize(phi, -M_PI, +M_PI);
  position_.set(x, y, 1);
  orientation_ = phi;
  cossin_uptodate_ = false;
  return *this;
}
Pose2D & Pose2D::set(const Point2D & position, const Point2D & point_ahead)
{
  position_.set(position.x(), position.y());
  double dx = point_ahead.x() - position.x();
  double dy = point_ahead.y() - position.y();
  orientation_ = atan2(dy, dx);
  cossin_uptodate_ = false;
  return *this;
}
Pose2D & Pose2D::set(const Pose2D & p)
{
  position_ = p.position_;
  orientation_ = p.orientation_;
  cossin_uptodate_ = false;
  return *this;
}
const Point2D & Pose2D::position() const {return position_;}
Point2D Pose2D::point_ahead(double d) const
{
  update_cached_cos_sin();
  return Point2D(x() + costheta_ * d, y() + sintheta_ * d);
}
const double & Pose2D::x() const {return position_[0];}
const double & Pose2D::y() const {return position_[1];}
const double & Pose2D::theta() const {return orientation_;}
Point2D & Pose2D::position() {return position_;}
double & Pose2D::x() {return position_[0];}
double & Pose2D::y() {return position_[1];}
double & Pose2D::theta()
{
  cossin_uptodate_ = false;
  return orientation_;
}

void Pose2D::set_x(double v) {this->x() = v;}
double Pose2D::get_x() const {return this->x();}
void Pose2D::set_y(double v) {this->y() = v;}
double Pose2D::get_y() const {return this->y();}
void Pose2D::set_theta(double v) {this->theta() = v;}
double Pose2D::get_theta() const {return this->theta();}

void Pose2D::normalizeOrientation() {angle_normalize(orientation_, -M_PI, +M_PI);}
Tf2D Pose2D::tf() const
{
  update_cached_cos_sin();
  return cv::Matx33d(costheta_, -sintheta_, x(), sintheta_, costheta_, y(), 0, 0, 1.);
}
cv::Vec<double, 3> Pose2D::state_vector() const {return cv::Vec<double, 3>(x(), y(), theta());}
Pose2D Pose2D::inv() const
{
  Pose2D p(-this->x(), -this->y(), -this->theta());
  return p;
}
Point2D & Pose2D::transform_into_base(const Point2D & src, Point2D & des) const
{
  des.set(
    src.x() * costheta_ - src.y() * sintheta_ + src.h() * x(),
    src.x() * sintheta_ + src.y() * costheta_ + src.h() * y(), src.h());
  return des;
}
Point2D Pose2D::transform_into_base(const Point2D & src) const
{
  Point2D des;
  return transform_into_base(src, des);
}

Pose2D & Pose2D::operator+=(const cv::Vec<double, 3> & s)
{
  this->x() += s.val[0], this->y() += s.val[1], this->theta() += s.val[2];
  angle_normalize(this->theta());
  cossin_uptodate_ = false;
  return *this;
}
Pose2D & Pose2D::operator-=(const cv::Vec<double, 3> & s)
{
  this->x() -= s.val[0], this->y() -= s.val[1], angle_difference(this->theta(), s.val[2]);
  cossin_uptodate_ = false;
  return *this;
}
Pose2D Pose2D::transform_into(const Pose2D & target) const
{
  return Pose2D(
    this->x() - target.x(), this->y() - target.y(),
    angle_normalize(this->theta() - target.theta()));
}

void Pose2D::recompute_cached_cos_sin() const
{
  costheta_ = cos(orientation_);
  sintheta_ = sin(orientation_);
  cossin_uptodate_ = true;
}
void Pose2D::update_cached_cos_sin() const
{
  if (cossin_uptodate_) {
    return;
  }
  recompute_cached_cos_sin();
}
double Pose2D::theta_cos() const
{
  update_cached_cos_sin();
  return costheta_;
}
double Pose2D::theta_sin() const
{
  update_cached_cos_sin();
  return sintheta_;
}
std::string Pose2D::str(const char * format) const
{
  char str[0xFF];
  sprintf(str, format, x(), y(), theta());
  return std::string(str);
}

bool Pose2D::equal(const Pose2D & o, double tolerance) const
{
  double d_position = cv::norm(o.position() - this->position());
  double d_angle = angle_difference(o.theta(), this->theta());
  return (d_position < tolerance) && (fabs(d_angle) < tolerance);
}