DrawLanes.cc
Go to the documentation of this file.
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   // Stage crops white from around and image this fixes this by drawing a line
00029   // at the top and bottom of the image
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     x0+=(imageWidth/2.0);
00052     x1+=(imageWidth/2.0);
00053     y0+=(imageHeight/2.0);
00054     y1+=(imageHeight/2.0);
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); // Image width
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);//draw left
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


art_map
Author(s): David Li, Patrick Beeson, Bartley Gillen, Tarun Nimmagadda, Mickey Ristroph, Michael Quinlan, Jack O'Quin
autogenerated on Fri Jan 3 2014 11:08:34