00001
00002
00003 #ifndef CAJUN_UTIL_H
00004 #define CAJUN_UTIL_H
00005
00006 #include <cstdio>
00007 #include <string>
00008 #include <vector>
00009
00010
00011 #define CLOSE_TO_ZERO 0.0001
00012
00013 namespace cajun
00014 {
00015 double gettstamp (void);
00016
00017
00018 double parse_human_time (char const *str);
00019
00020 std::string human_time_str (double tstamp);
00021
00022 double clipangle (double angle);
00023
00024 double angular_deviation (double ref_angle, double angle);
00025
00026 bool get_file_tstamp (char const *fname, unsigned &tstamp_);
00027
00028 bool within_rect_region (double x1, double y1, double x2, double y2,
00029 double width, double x, double y);
00030 double atrack_distance (double x1, double y1, double x2, double y2,
00031 double x, double y);
00032 double xtrack_distance (double x1, double y1, double x2, double y2,
00033 double x, double y);
00034
00035 bool inter_lane_seg (double x11_, double y11_, double x12_,
00036 double y12_,
00037 double x21_, double y21_, double x22_,
00038 double y22_);
00039
00040
00041
00042
00043
00044 bool are_same_points (double x1_, double y1_, double x2_, double y2_);
00045
00046 bool are_too_close_points (double x1_, double y1_, double x2_,
00047 double y2_, double dis_);
00048
00049
00050 bool are_equal (double val1_, double val2_);
00051
00054 bool are_close (double val1_, double val2_, double max_diff_);
00055
00056
00057 double dist_to_track (double x_, double y_,
00058 double beg_x_, double beg_y_,
00059 double end_x_, double end_y_);
00060
00061
00062
00063 void multiply_4x4_4x4 (double a[4][4], double b[4][4], double results[4][4]);
00064
00065 void multiply_4x4_4x1 (double a[4][4], double b[4], double results[4]);
00066
00067 void compute_transformation_matrix (double roll, double heading, double pitch,
00068 double tx, double ty, double tz,
00069 double results[4][4]);
00070
00071 void compute_global_velocity (double vel_x, double vel_y, double sog, double heading, double *velocity, double *velocity_angle);
00072
00073
00074 double interpolate (double t,
00075 double t0, double v0,
00076 double t1, double v1);
00077
00078 double interpolate_angle (double t,
00079 double t0, double v0,
00080 double t1, double v1);
00087 void tokenize (const std::string &str,
00088 std::vector<std::string> &tokens,
00089 const std::string &delims);
00090 };
00091
00092
00093 #endif