00001 #include <art_map/DrawLanes.h>
00002 #include <art_map/euclidean_distance.h>
00003
00004 #include <iostream>
00005
00006 DrawLanes::DrawLanes(int x, int y, float multi) {
00007 MULT=multi;
00008 imageWidth=int(ceil(x*MULT));
00009 imageHeight=int(ceil(y*MULT));
00010 image = new RGB[imageWidth*imageHeight];
00011 clear();
00012 }
00013
00014 DrawLanes::~DrawLanes() {
00015
00016 delete[] image;
00017 }
00018
00019 void DrawLanes::clear() {
00020 for (int h=0; h<imageHeight; h++) {
00021 for (int w=0; w<imageWidth; w++) {
00022 int index=h*imageWidth+w;
00023 image[index].r=255;
00024 image[index].g=255;
00025 image[index].b=255;
00026 }
00027 }
00028
00029
00030 for (int w=0; w<imageWidth; w++) {
00031 int index=w;
00032 image[index].r=0;
00033 image[index].g=0;
00034 image[index].b=0;
00035 index=((imageHeight-1)*imageWidth)+w;
00036 image[index].r=0;
00037 image[index].g=0;
00038 image[index].b=0;
00039 }
00040 }
00041
00042
00043 void DrawLanes::line(float x0, float y0, float x1, float y1, RGB colour)
00044 {
00045 x0*=MULT;
00046 y0*=MULT;
00047 x1*=MULT;
00048 y1*=MULT;
00049
00050
00051
00052
00053
00054
00055
00056
00057
00058 float full_dist=Euclidean::DistanceTo(x0,y0,x1,y1);
00059
00060 for (float i=0.0; i<=1.0; i+=1.0/(full_dist)) {
00061 float newx=i*x0+(1-i)*x1;
00062 float newy=i*y0+(1-i)*y1;
00063
00064 int xcell=(int)roundf(newx);
00065 int ycell=(int)roundf(newy);
00066
00067 int index=imageWidth*ycell+xcell;
00068
00069 image[index]=colour;
00070 }
00071 }
00072
00073 void DrawLanes::savePGM(const char *filename) {
00074 FILE *f = fopen(filename, "w+");
00075
00076 fprintf(f,"P3\n");
00077 fprintf(f,"#%s\n",filename);
00078 fprintf(f,"%i %i\n",imageWidth,imageHeight);
00079 fprintf(f,"%i\n",256);
00080 for (int h=0; h<imageHeight; h++) {
00081 for (int w=0; w<imageWidth; w++) {
00082 int index=h*imageWidth+w;
00083 fprintf(f,"%i %i %i ",image[index].r,image[index].g,image[index].b);
00084 }
00085 fprintf(f,"\n");
00086 }
00087 fclose(f);
00088 }
00089
00090 void DrawLanes::addTrace(float w1lat, float w1long, float w2lat, float w2long){
00091 RGB color;
00092 color.r=0;
00093 color.g=0;
00094 color.b=0;
00095 line(w1lat,w1long,w2lat,w2long,color);
00096 }
00097
00098 void DrawLanes::addWay(float w1lat, float w1long) {
00099 RGB color;
00100 color.r=0;
00101 color.g=0;
00102 color.b=0;
00103 line(w1lat,w1long,w1lat,w1long,color);
00104 }
00105
00106 void DrawLanes::addRobot(float w1lat, float w1long) {
00107 RGB color;
00108 color.r=0;
00109 color.g=0;
00110 color.b=255;
00111 line(w1lat,w1long,w1lat,w1long,color);
00112 }
00113
00114
00115
00116 void DrawLanes::addPoly(float x1, float x2, float x3, float x4, float y1,
00117 float y2, float y3, float y4, bool is_stop,
00118 bool is_exit){
00119 RGB color;
00120 color.r=0;
00121 color.g=0;
00122 color.b=0;
00123 if (!is_exit)
00124 {
00125 DrawLanes::line(x1, y1, x2, y2, color);
00126 DrawLanes::line(x3, y3, x4, y4, color);
00127 }
00128 if (is_stop) {
00129 DrawLanes::line(x1, y1, x4, y4, color);
00130 DrawLanes::line(x2, y2, x3, y3, color);
00131 }
00132 }
00133
00134 #if 0 //TODO
00135 void DrawLanes::addZone(const ZonePerimeter &zone, float min_x, float max_y)
00136 {
00137 RGB color;
00138 color.r=0;
00139 color.g=1;
00140 color.b=0;
00141 unsigned size = zone.perimeter_points.size();
00142 for(unsigned i = 0; i < size; i++)
00143 {
00144 DrawLanes::line(zone.perimeter_points[i].map.x - min_x,
00145 max_y - zone.perimeter_points[i].map.y,
00146 zone.perimeter_points[(i+1)%size].map.x - min_x,
00147 max_y - zone.perimeter_points[(i+1)%size].map.y,
00148 color);
00149 }
00150 }
00151 #endif