54 short int laser_index;
72const double PI = 3.14159265358979;
75double sqr(
double x) {
return x*x; }
78double abs(
double x) {
return (x<0.) ? -x : x; }
81double round(
double x) {
82 return (x<0.) ? -
static_cast<int>(0.5-x) : static_cast<int>(0.5+x);
95double pi_to_pi(
double angle) {
108double dist_sqr(
const Point& p,
const Point& q) {
109 return (sqr(p.x-q.x) + sqr(p.y-q.y));
112double dist(
const Point& p,
const Point& q);
113Pose compute_relative_pose(
const std::vector<Point>& a,
const std::vector<Point>& b);
119bool intersection_line_line (Point& p,
const Line& l,
const Line& m);
120double distance_line_point (
const Line& lne,
const Point& p);
121void 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);
143void Transform2D::transform_to_relative(
Point &p) {
152void 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;
159void Transform2D::transform_to_relative(
Pose &p) {
160 transform_to_relative(p.p);
161 p.phi= pi_to_pi(p.phi-base.phi);
165void 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:63
Definition geometry2D.h:51
Definition geometry2D.h:58