00001 #ifndef CAJUN_SEGMENT_H
00002 #define CAJUN_SEGMENT_H
00003
00004 #include <cmath>
00005 #include <iostream>
00006
00007 namespace cajun
00008 {
00009
00010 class segment_t
00011 {
00012
00013 public:
00014 enum side_t { ON_PATH, LEFT_OF_PATH, RIGHT_OF_PATH };
00015 segment_t () {;};
00016 segment_t (lane_point_t start_, lane_point_t end_)
00017 {
00018 m_start = start_;
00019 m_end = end_;
00020 m_segment_heading = atan2 (m_end.y - m_start.y, m_end.x - m_start.x);
00021 }
00022 ~segment_t () {;};
00023
00024 segment_t reverse ()
00025 {
00026 return segment_t (m_end, m_start);
00027 }
00028
00029 side_t point_side (lane_point_t pt_)
00030 {
00031 double xd = cajun::xtrack_distance ( m_start.x, m_start.y, m_end.x, m_end.y, pt_.x, pt_.y);
00032
00033 if (xd > 0)
00034 return LEFT_OF_PATH;
00035 else if (xd < 0)
00036 return RIGHT_OF_PATH;
00037 else
00038 return ON_PATH;
00039
00040 }
00041 lane_point_t point_along_segment (float dis_)
00042 {
00043 lane_point_t new_pt;
00044 new_pt.x = m_start.x + (dis_ * cos (m_segment_heading));
00045 new_pt.y = m_start.y + (dis_ * sin (m_segment_heading));
00046
00047 return new_pt;
00048 }
00049 segment_t segment_on_the_right (float perp_dis_)
00050 {
00051 lane_point_t new_start_pt;
00052 new_start_pt.x = m_start.x + (perp_dis_ * sin (m_segment_heading));
00053 new_start_pt.y = m_start.y - (perp_dis_ * cos (m_segment_heading));
00054
00055 lane_point_t new_end_pt;
00056 new_end_pt.x = m_end.x + (perp_dis_ * cos (m_segment_heading));
00057 new_end_pt.y = m_end.y - (perp_dis_ * sin (m_segment_heading));
00058
00059 return segment_t (new_start_pt, new_end_pt);
00060
00061
00062 }
00063 segment_t segment_on_the_left (float perp_dis_)
00064 {
00065 lane_point_t new_start_pt;
00066 new_start_pt.x = m_start.x - (perp_dis_ * sin (m_segment_heading));
00067 new_start_pt.y = m_start.y + (perp_dis_ * cos (m_segment_heading));
00068
00069 lane_point_t new_end_pt;
00070 new_end_pt.x = m_end.x - (perp_dis_ * cos (m_segment_heading));
00071 new_end_pt.y = m_end.y + (perp_dis_ * sin (m_segment_heading));
00072
00073 return segment_t (new_start_pt, new_end_pt);
00074 }
00075 lane_point_t point_to_the_right (float perp_dis_, float horiz_dis_)
00076 {
00077 return (segment_on_the_right (perp_dis_)).point_along_segment (horiz_dis_);
00078 }
00079
00080 lane_point_t point_to_the_left (float perp_dis_, float horiz_dis_)
00081 {
00082 return (segment_on_the_left (perp_dis_)).point_along_segment (horiz_dis_);
00083 }
00084 double segment_angle (const segment_t &another_seg_)
00085 {
00086 return m_segment_heading - another_seg_.m_segment_heading;
00087 }
00088 lane_point_t start_point () { return m_start; }
00089 lane_point_t end_point () { return m_end; }
00090 double slope () { return (m_start.y - m_end.y) / (m_start.x - m_end.x);};
00091
00092 private:
00093 lane_point_t m_start;
00094 lane_point_t m_end;
00095 double m_segment_heading;
00096 };
00097
00098 };
00099
00100 #endif
