39 #ifndef GEOMETRY_2D_H_
40 #define GEOMETRY_2D_H_
54 short int laser_index;
72 const double PI = 3.14159265358979;
75 double sqr(
double x) {
return x*x; }
78 double abs(
double x) {
return (x<0.) ? -x : x; }
81 double round(
double x) {
82 return (x<0.) ? -
static_cast<int>(0.5-x) : static_cast<int>(0.5+x);
95 double pi_to_pi(
double angle) {
108 double dist_sqr(
const Point& p,
const Point& q) {
109 return (sqr(p.x-q.x) + sqr(p.y-q.y));
112 double dist(
const Point& p,
const Point& q);
113 Pose compute_relative_pose(
const std::vector<Point>& a,
const std::vector<Point>& b);
119 bool intersection_line_line (Point& p,
const Line& l,
const Line& m);
120 double distance_line_point (
const Line& lne,
const Point& p);
121 void intersection_line_point(Point& p,
const Line& l,
const Point& q);
131 void transform_to_relative(
Point &p);
132 void transform_to_relative(
Pose &p);
133 void transform_to_global(
Point &p);
134 void transform_to_global(
Pose &p);
143 void Transform2D::transform_to_relative(
Point &p) {
152 void Transform2D::transform_to_global(Point &p) {
154 p.x = base.p.x + c*p.x - s*p.y;
155 p.y = base.p.y + s*t + c*p.y;
159 void Transform2D::transform_to_relative(Pose &p) {
160 transform_to_relative(p.p);
161 p.phi= pi_to_pi(p.phi-base.phi);
165 void Transform2D::transform_to_global(Pose &p) {
166 transform_to_global(p.p);
167 p.phi= pi_to_pi(p.phi+base.phi);
Definition: geometry2D.h:57
Definition: geometry2D.h:63
Definition: geometry2D.h:50