Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

Draw.cc

Go to the documentation of this file.
00001 #include "Draw.h"
00002 
00003 Draw& Draw::setColor(Colors c, double alpha) {
00004   switch(c) {
00005     case BLACK:
00006       setColorRGB(0,0,0,alpha); break;
00007     case DK_GRAY:
00008       setColorRGB(0.25,0.25,0.25,alpha); break;
00009     case GRAY:
00010       setColorRGB(0.5,0.5,0.5,alpha); break;
00011     case LT_GRAY:
00012       setColorRGB(0.75,0.75,0.75,alpha); break;
00013     case WHITE:
00014       setColorRGB(1,1,1,alpha); break;
00015     case RED:
00016       setColorRGB(1,0,0,alpha); break;
00017     case YELLOW:
00018       setColorRGB(1,1,0,alpha); break;
00019     case GREEN:
00020       setColorRGB(0,1,0,alpha); break;
00021     case CYAN:
00022       setColorRGB(0,1,1,alpha); break;
00023     case BLUE:
00024       setColorRGB(0,0,1,alpha); break;
00025     case MAGENTA:
00026       setColorRGB(1,0,1,alpha); break;
00027   }
00028   return *this;
00029 }
00030 
00031 #ifndef HAVE_CAIRO
00032 
00033 struct _cairo {
00034   _cairo() : lineWidth(1) {}
00035   double lineWidth;
00036 };
00037 
00038 Draw& Draw::setFile(const std::string&, double width, double height) {
00039   teardown();
00040   cr = new cairo_t;
00041   surfaceWidth=width;
00042   surfaceHeight=height;
00043   return *this;
00044 }
00045 Draw& Draw::setScale(double x, bool scaleStroke) {
00046   if(!scaleStroke && cr!=NULL)
00047     cr->lineWidth *= scale/x;
00048   scale=x;
00049   return *this;
00050 }
00051 Draw& Draw::setRegion(const BoundingBox2D& region, bool scaleStroke/*=false*/) {
00052   double s0 = surfaceWidth / region.getDimension(0);
00053   double s1 = surfaceHeight / region.getDimension(1);
00054   double s = s0 < s1 ? s0 : s1;
00055 #ifndef PLATFORM_APERIOS // missing isfinite
00056   if(std::isfinite(s) && s>0)
00057 #endif
00058     setScale(s,scaleStroke);
00059   centerView(region.getCenter());
00060   return *this;
00061 }
00062 Draw& Draw::drawGrid(double xRes, double yRes, double xOff/*=0*/, double yOff/*=0*/) { return *this; }
00063 Draw& Draw::drawAxes() { return *this; }
00064 Draw& Draw::setStrokeWidth(double x) { if(cr!=NULL) cr->lineWidth = x/scale; return *this; }
00065 Draw& Draw::setScaledStrokeWidth(double x) { if(cr!=NULL) cr->lineWidth = x; return *this; }
00066 double Draw::getScaledStrokeWidth() const { return (cr!=NULL)?cr->lineWidth:1; }
00067 void Draw::teardown() { delete cr; cr=NULL; }
00068  
00069 Draw& Draw::flushPage() { return *this; }
00070 Draw& Draw::centerView(double, double) { return *this; }
00071 Draw& Draw::setClip() { return *this; }
00072 Draw& Draw::clearClip() { return *this; }
00073 Draw& Draw::setColorRGB(double, double, double, double) { return *this; }
00074 Draw& Draw::stroke() { return *this; }
00075 Draw& Draw::strokeLine(double, double, double, double) { return *this; }
00076 Draw& Draw::fill() { return *this; }
00077 Draw& Draw::point(double, double, double, PointStyles) { return *this; }
00078 Draw& Draw::arc(double, double, double, double, double) { return *this; }
00079 Draw& Draw::ellipse(const fmat::Column<2>&, double, double, double) { return *this; }
00080 Draw& Draw::rect(const fmat::Column<2>&, const fmat::Column<2>&) { return *this; }
00081 Draw& Draw::arrow(const fmat::Column<2>& p1, const fmat::Column<2>& p2, double headWidth, double headLength) { return * this; }
00082 Draw& Draw::draw(const PlannerObstacle2D&) { return *this; }
00083 Draw& Draw::draw(const RectangularObstacle&) { return *this; }
00084 Draw& Draw::draw(const CircularObstacle&) { return *this; }
00085 Draw& Draw::draw(const EllipticalObstacle&) { return *this; }
00086 Draw& Draw::draw(const ConvexPolyObstacle&) { return *this; }
00087 Draw& Draw::draw(const HierarchicalObstacle&) { return *this; }
00088 Draw& Draw::moveTo(double, double) { return *this; }
00089 Draw& Draw::lineTo(double, double) { return *this; }
00090 Draw& Draw::arcTo(double, double, double, double, double) { return *this; }
00091 Draw& Draw::closePath() { return *this; }
00092 Draw& Draw::clearPath() { return *this; }
00093 void Draw::resetTransform() {}
00094 void Draw::checkSurfaceStatus(const std::string&) {}
00095 void Draw::checkStatus(const std::string&) {}
00096 
00097 #else
00098 
00099 #include <cairo-pdf.h>
00100 #include <stdexcept>
00101 #include "Planners/PlannerObstacles.h"
00102 
00103 void Draw::teardown() {
00104   if(filename.size()>=4 && filename.substr(filename.size()-4)==".png") {
00105     cairo_surface_write_to_png(surface, filename.c_str());
00106   }
00107   cairo_destroy(cr);
00108   cr=NULL;
00109   cairo_surface_destroy(surface);
00110   surface=NULL;
00111 }
00112 
00113 Draw& Draw::setFile(const std::string& file, double width, double height) {
00114   teardown();
00115   filename = file;
00116   if(file.size()>=4 && file.substr(file.size()-4)==".pdf") {
00117     surface = cairo_pdf_surface_create(file.c_str(), width, height);
00118   } else {
00119     surface = cairo_image_surface_create(CAIRO_FORMAT_ARGB32, width, height);
00120   }
00121   checkSurfaceStatus("cairo_pdf_surface_create");
00122   cr = cairo_create(surface);
00123   checkStatus("cairo_create");
00124   surfaceWidth=width;
00125   surfaceHeight=height;
00126   resetTransform();
00127   return *this;
00128 }
00129 
00130 Draw& Draw::flushPage() {
00131   cairo_surface_show_page(surface);
00132   checkStatus("cairo_surface_show_page");
00133   resetTransform();
00134   return *this;
00135 }
00136 
00137 Draw& Draw::centerView(double x, double y) {
00138   origin[0] = x - surfaceWidth/scale/2;
00139   origin[1] = y - surfaceHeight/scale/2;
00140   resetTransform();
00141   return *this;
00142 }
00143 
00144 Draw& Draw::setClip() {
00145   cairo_clip_preserve(cr);
00146   return *this;
00147 }
00148 
00149 Draw& Draw::clearClip() {
00150   cairo_reset_clip(cr);
00151   return *this;
00152 }
00153 
00154 Draw& Draw::setScale(double x, bool scaleStroke/*=false*/) {
00155   if(!scaleStroke)
00156     cairo_set_line_width(cr, cairo_get_line_width(cr)*scale/x);
00157   scale=x;
00158   resetTransform();
00159   return *this;
00160 }
00161 
00162 Draw& Draw::setRegion(const BoundingBox2D& region, bool scaleStroke/*=false*/) {
00163   double s0 = surfaceWidth / region.getDimension(0);
00164   double s1 = surfaceHeight / region.getDimension(1);
00165   double s = s0 < s1 ? s0 : s1;
00166 #ifndef PLATFORM_APERIOS // missing isfinite
00167   if(std::isfinite(s) && s>0)
00168 #endif
00169     setScale(s,scaleStroke);
00170   centerView(region.getCenter());
00171   return *this;
00172 }
00173 
00174 Draw& Draw::drawGrid(double xRes, double yRes, double xOff/*=0*/, double yOff/*=0*/) {
00175   if(!addPath)
00176     cairo_new_path(cr);
00177   double right = origin[0] + surfaceWidth/scale;
00178   double top = origin[1] + surfaceHeight/scale;
00179   if(xRes>0) {
00180     xOff = std::fmod(xOff,xRes);
00181     double begin = std::ceil((origin[0]-xOff) / xRes) * xRes + xOff;
00182     for(double x=begin; x<=right; x+=xRes) {
00183       cairo_move_to(cr, x, origin[1]);
00184       cairo_line_to(cr, x, top);
00185     }
00186   }
00187   if(yRes>0) {
00188     yOff = std::fmod(yOff,yRes);
00189     double begin = std::ceil((origin[1]-yOff) / yRes) * yRes + yOff;
00190     for(double y=begin; y<=top; y+=yRes) {
00191       cairo_move_to(cr, origin[0], y);
00192       cairo_line_to(cr, right, y);
00193     }
00194   }
00195   return *this;
00196 }
00197 
00198 Draw& Draw::drawAxes() {
00199   if(!addPath)
00200     cairo_new_path(cr);
00201   double right = origin[0] + surfaceWidth/scale;
00202   double top = origin[1] + surfaceHeight/scale;
00203   cairo_move_to(cr, 0, origin[1]);
00204   cairo_line_to(cr, 0, top);
00205   cairo_move_to(cr, origin[0], 0);
00206   cairo_line_to(cr, right, 0);
00207   return *this;
00208 }
00209 
00210 Draw& Draw::setStrokeWidth(double x) {
00211   cairo_set_line_width(cr, x/scale);
00212   return *this;
00213 }
00214 
00215 Draw& Draw::setScaledStrokeWidth(double x) {
00216   cairo_set_line_width(cr, x);
00217   return *this;
00218 }
00219 
00220 double Draw::getScaledStrokeWidth() const {
00221   return cairo_get_line_width(cr);
00222 }
00223 
00224 Draw& Draw::setColorRGB(double red, double green, double blue, double alpha/*=1*/) {
00225   cairo_set_source_rgba(cr, red, green, blue, alpha);
00226   return *this;
00227 }
00228 
00229 Draw& Draw::stroke() {
00230   cairo_stroke_preserve(cr);
00231   return *this;
00232 }
00233 
00234 Draw& Draw::strokeLine(double x1, double y1, double x2, double y2) {
00235   if(!addPath)
00236     cairo_new_path(cr);
00237   cairo_move_to(cr, x1, y1);
00238   cairo_line_to(cr, x2, y2);
00239   cairo_stroke_preserve(cr);
00240   return *this;
00241 }
00242 
00243 
00244 Draw& Draw::fill() {
00245   cairo_fill_preserve(cr);
00246   return *this;
00247 }
00248 
00249 Draw& Draw::point(double x, double y, double size, PointStyles style/*=CIRCLE*/) {
00250   double r=size/2/scale;
00251   switch(style) {
00252     case CIRCLE: {
00253       if(!addPath)
00254         cairo_new_path(cr);
00255       else
00256         cairo_new_sub_path(cr);
00257       cairo_arc(cr, x, y, r, 0, 2*M_PI);
00258     } break;
00259     case SQUARE: {
00260       if(!addPath)
00261         cairo_new_path(cr);
00262       cairo_move_to(cr, x-r, y-r);
00263       cairo_line_to(cr, x+r, y-r);
00264       cairo_line_to(cr, x+r, y+r);
00265       cairo_line_to(cr, x-r, y+r);
00266       cairo_close_path(cr);
00267     } break;
00268     case DIAMOND: {
00269       if(!addPath)
00270         cairo_new_path(cr);
00271       cairo_move_to(cr, x, y-r);
00272       cairo_line_to(cr, x+r, y);
00273       cairo_line_to(cr, x, y+r);
00274       cairo_line_to(cr, x-r, y);
00275       cairo_close_path(cr);
00276     } break;
00277   }
00278   return *this;
00279 }
00280 
00281 Draw& Draw::arc(double x, double y, double r, double begin, double end) {
00282   if(!addPath)
00283     cairo_new_path(cr);
00284   else
00285     cairo_new_sub_path(cr);
00286   return arcTo(x,y,r,begin,end);
00287 }
00288 
00289 Draw& Draw::ellipse(const fmat::Column<2>& c, double width, double height, double orientation/*=0*/) {
00290   cairo_matrix_t om;
00291   cairo_get_matrix(cr, &om);
00292   cairo_matrix_t m;
00293   cairo_matrix_init(&m, scale, 0, 0, -scale, (-origin[0]+c[0])*scale, (origin[1]-c[1])*scale+surfaceHeight);
00294   cairo_set_matrix(cr, &m);
00295   cairo_rotate(cr, orientation);
00296   cairo_scale(cr, width/2, height/2);
00297   circle(0,0,1);
00298   cairo_set_matrix(cr, &om);
00299   return *this;
00300 }
00301 
00302 Draw& Draw::rect(const fmat::Column<2>& p1, const fmat::Column<2>& p2) {
00303   const fmat::Column<2> d = p2-p1;
00304   if(!addPath)
00305     cairo_new_path(cr);
00306   cairo_rectangle(cr, p1[0],p1[1], d[0],d[1]);
00307   return *this;
00308 }
00309 
00310 Draw& Draw::arrow(const fmat::Column<2>& p1, const fmat::Column<2>& p2, double headWidth, double headLength) {
00311   fmat::Column<2> d = p2-p1;
00312   d/=d.norm();
00313   fmat::Matrix<2,2> r;
00314   r(0,0) = r(1,1) = d[0];
00315   r(0,1) = -(r(1,0) = d[1]);
00316   fmat::Column<2> a = fmat::pack(-headLength,headWidth) * getScaledStrokeWidth();
00317   fmat::Column<2> p3 = r * a;
00318   a[1]=-a[1];
00319   fmat::Column<2> p4 = r * a;
00320   if(!addPath)
00321     cairo_new_path(cr);
00322   cairo_move_to(cr, p1[0], p1[1]);
00323   cairo_line_to(cr, p2[0], p2[1]);
00324   cairo_move_to(cr, p2[0]+p3[0], p2[1]+p3[1]);
00325   cairo_line_to(cr, p2[0], p2[1]);
00326   cairo_line_to(cr, p2[0]+p4[0], p2[1]+p4[1]);
00327   return *this;
00328 }
00329 
00330 
00331 Draw& Draw::draw(const PlannerObstacle2D& o) {
00332   switch(o.getObstacleGeometry()) {
00333     case PlannerObstacle2D::RECTANGULAR_OBS: return draw(static_cast<const RectangularObstacle&>(o));
00334     case PlannerObstacle2D::CIRCULAR_OBS: return draw(static_cast<const CircularObstacle&>(o));
00335     case PlannerObstacle2D::ELLIPTICAL_OBS: return draw(static_cast<const EllipticalObstacle&>(o));
00336     case PlannerObstacle2D::CONVEX_POLY_OBS: return draw(static_cast<const ConvexPolyObstacle&>(o));
00337     case PlannerObstacle2D::HIERARCHICAL_OBS: return draw(static_cast<const HierarchicalObstacle&>(o));
00338   }
00339   return *this;
00340 }
00341 
00342 Draw& Draw::draw(const RectangularObstacle& o) {
00343   if(!addPath)
00344     cairo_new_path(cr);
00345   cairo_move_to(cr, o.getCorner(0)[0], o.getCorner(0)[1]);
00346   for(size_t i=1; i<RectangularObstacle::NUM_CORNERS; ++i) {
00347     cairo_line_to(cr, o.getCorner(i)[0], o.getCorner(i)[1]);
00348   }
00349   cairo_close_path(cr);
00350   return *this;
00351 }
00352 Draw& Draw::draw(const CircularObstacle& o) {
00353   return circle(o.getCenter(), o.getRadius());
00354 }
00355 Draw& Draw::draw(const EllipticalObstacle& o) {
00356   return ellipse(o.center, o.semimajor*2, o.semiminor*2, o.getAngle());
00357 }
00358 Draw& Draw::draw(const ConvexPolyObstacle& o) {
00359   if(!addPath)
00360     cairo_new_path(cr);
00361   cairo_move_to(cr, o.getPoints()[0][0], o.getPoints()[0][1]);
00362   for(std::vector<fmat::Column<2> >::const_iterator it=o.getPoints().begin()+1; it!=o.getPoints().end(); ++it) {
00363     cairo_line_to(cr, (*it)[0], (*it)[1]);
00364   }
00365   cairo_close_path(cr);
00366   return *this;
00367   return *this;
00368 }
00369 Draw& Draw::draw(const HierarchicalObstacle& o) {
00370   bool oldHold = getHoldMode();
00371   if(!oldHold)
00372     holdPath();
00373   const std::vector<PlannerObstacle2D*>& sub = o.getObstacles();
00374   for(size_t i=0; i<sub.size(); ++i) {
00375     sub[i]->rotate(fmat::ZERO2, o.getOrientation());
00376     fmat::Column<2> oldCenter = sub[i]->getCenter();
00377     sub[i]->updatePosition(oldCenter+o.getCenter());
00378     draw(*sub[i]);
00379     sub[i]->updatePosition(oldCenter);
00380     sub[i]->rotate(fmat::ZERO2, o.getOrientation().transpose());
00381   }
00382   if(!oldHold)
00383     releasePath();
00384   return *this;
00385 }
00386 
00387 Draw& Draw::moveTo(double x, double y) {
00388   if(!addPath)
00389     cairo_new_path(cr);
00390   cairo_move_to(cr, x, y);
00391   return *this;
00392 }
00393 
00394 Draw& Draw::lineTo(double x, double y) {
00395   cairo_line_to(cr, x, y);
00396   return *this;
00397 }
00398 
00399 Draw& Draw::arcTo(double x, double y, double r, double begin, double end) {
00400   if(begin<end)
00401     cairo_arc(cr, x, y, r, begin, end);
00402   else
00403     cairo_arc_negative(cr, x, y, r, begin, end);
00404   return *this;
00405 }
00406 
00407 Draw& Draw::closePath() {
00408   cairo_close_path(cr);
00409   return *this;
00410 }
00411 
00412 Draw& Draw::clearPath() {
00413   cairo_new_path(cr);
00414   return *this;
00415 }
00416 
00417 void Draw::resetTransform() {
00418   cairo_matrix_t m;
00419   cairo_matrix_init(&m, scale, 0, 0, -scale, -origin[0]*scale, origin[1]*scale+surfaceHeight);
00420   cairo_set_matrix(cr, &m);
00421   checkStatus("cairo_create");
00422 }
00423 
00424 void Draw::checkSurfaceStatus(const std::string& msg) {
00425   if(cairo_surface_status(surface)!=CAIRO_STATUS_SUCCESS) {
00426     throw std::runtime_error(msg+": " +cairo_status_to_string(cairo_surface_status(surface)));
00427   }
00428 }
00429 
00430 void Draw::checkStatus(const std::string& msg) {
00431   checkSurfaceStatus(msg);
00432   if(cairo_status(cr)!=CAIRO_STATUS_SUCCESS) {
00433     throw std::runtime_error(msg+": " +cairo_status_to_string(cairo_surface_status(surface)));
00434   }
00435 }
00436 
00437 #endif

Tekkotsu v5.1CVS
Generated Mon May 9 04:58:37 2016 by Doxygen 1.6.3