draw.cpp
Go to the documentation of this file.
00001 /* Copyright (C) 2015-2017 Michele Colledanchise - All Rights Reserved
00002 *
00003 *   Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"),
00004 *   to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense,
00005 *   and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
00006 *   The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
00007 *
00008 *   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
00009 *   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
00010 *   WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
00011 */
00012 
00013 
00014 
00015 #include<draw.h>
00016 #include<string>
00017 #include<vector>
00018 
00019 #include <X11/Xlib.h>
00020 
00021 const float DEG2RAD = 3.14159/180.0;
00022 
00023 
00024 BT::ControlNode* tree;
00025 bool init = false;
00026 
00027 
00028 void * font_array[3] = {GLUT_BITMAP_8_BY_13, GLUT_BITMAP_8_BY_13, GLUT_BITMAP_8_BY_13};
00029 void * font = font_array[0];
00030 float additional_spacing_array[10];
00031 bool is_number_pressed_array[10];
00032 unsigned char number_char[4] = {'0', '1', '2', '3'};
00033 
00034 
00035 float x = 0.0;
00036 float y = 0.4;
00037 float x_offset = 0.01;
00038 float y_offset = 0.15;
00039 float r_color = 1;
00040 float g_color = 1;
00041 float b_color = 1;
00042 GLfloat x_space = 0.06;
00043 
00044 int depth;
00045 
00046 double zoom = 1.0f;
00047 
00048 float fraction = 0.1f;
00049 float zoom_fraction = 0.1f;
00050 
00051 
00052 
00053 
00054 void drawEllipse(float xpos, float ypos, float xradius, float yradius)
00055 {
00056     glBegin(GL_LINE_LOOP);
00057 
00058     for (int i=0; i < 359; i++)
00059     {
00060         // convert degrees into radians
00061         float degInRad = i*DEG2RAD;
00062         glVertex2d(xpos+cos(degInRad)*xradius,  ypos + sin(degInRad)*yradius);
00063     }
00064     glEnd();
00065 }
00066 
00067 void drawString(void * font, const char *string, float x, float y, float z)
00068 {
00069     renderBitmapString(x, y, font, string);
00070 }
00071 
00072 int compute_node_lines(const char *string)
00073 {
00074     const char *c;
00075     int i = 0;
00076     int new_line_num = 1;
00077     glRasterPos2f(x, y);
00078     for (c=string; *c != '\0'; c++)
00079     {
00080         if ((*c == '\n') || ((*c == ' ' && i > 6) || i > 9))
00081         {
00082             new_line_num++;
00083             i = 0;
00084             continue;
00085         }
00086         i++;
00087     }
00088     return new_line_num;
00089 }
00090 
00091 int compute_max_width(const char *string)
00092 {
00093     const char *current_char;
00094     int current_line_width = 0;
00095     int max_width = 0;
00096 
00097     glRasterPos2f(x, y);
00098     for (current_char = string; *current_char != '\0'; current_char++)
00099     {
00100         if ((*current_char == '\n') || ((*current_char == ' ' && current_line_width > 6) || current_line_width > 9))
00101         {
00102             if (current_line_width > max_width)
00103             {
00104                 max_width = current_line_width;
00105             }
00106             current_line_width = 0;
00107             continue;
00108         }
00109         current_line_width++;
00110     }
00111 
00112     if (max_width == 0)  // if the lable fits in a single line
00113     {
00114         max_width = current_line_width;
00115     }
00116     return max_width;
00117 }
00118 
00119 void renderBitmapString(float x, float y, void *font, const char *string)
00120 {
00121     const char *c;
00122     int i = 0;
00123     int new_line_num = 0;
00124     glRasterPos2f(x, y);
00125     for (c=string; *c != '\0'; c++)
00126     {
00127         if ((*c == '\n') || ((*c == ' ' && i > 6) || i > 9))
00128         {
00129             new_line_num++;
00130             glRasterPos2f(x, y - 0.025*(new_line_num));
00131             i = 0;
00132             continue;
00133         }
00134         i++;
00135         glutBitmapCharacter(font, *c);
00136     }
00137 }
00138 
00139 
00140 
00141 void draw_node(float x, float y, int node_type, const char *leafName, int status)
00142 {
00143     float NODE_WIDTH = 0.04;
00144     float NODE_HEIGHT = 0.02;
00145     switch (node_type)
00146     {
00147     case BT::SELECTORSTAR:
00148         drawString(font, "?*", (x + NODE_WIDTH/2 -0.005), (y - NODE_HEIGHT/2), 0);
00149         break;
00150     case BT::SEQUENCESTAR:
00151         drawString(font, ">*", (x + NODE_WIDTH/2 -0.0051), (y - NODE_HEIGHT/2), 0);
00152         break;
00153     case BT::SELECTOR:
00154         drawString(font, "?", (x + NODE_WIDTH/2 -0.005), (y - NODE_HEIGHT/2), 0);
00155         break;
00156     case BT::SEQUENCE:
00157         drawString(font, ">", (x + NODE_WIDTH/2 -0.005), (y - NODE_HEIGHT/2), 0);
00158         break;
00159     case BT::PARALLEL:
00160         drawString(font, "=", (x + NODE_WIDTH/2 -0.005), (y - NODE_HEIGHT/2), 0);
00161         break;
00162     case BT::DECORATOR:
00163         drawString(font, "D", (x + NODE_WIDTH/2 -0.005), (y - NODE_HEIGHT/2), 0);
00164         break;
00165     case BT::ACTION:
00166     {
00167         NODE_HEIGHT = 0.02*(compute_node_lines(leafName));
00168         std::string st(leafName, 0, 15);
00169         NODE_WIDTH = 0.02*compute_max_width(leafName);
00170 //            for (unsigned int i = 0; i < st.size(); i++)
00171 //              NODE_WIDTH +=  0.01;
00172     }
00173     renderBitmapString((x + 0.015), (y - 0.01), font, leafName);
00174     // glColor3f(0.2, 1.0, 0.2);
00175     break;
00176     case BT::CONDITION:
00177     {
00178         NODE_HEIGHT = 0.02*compute_node_lines(leafName);
00179         std::string st(leafName, 0, 15);
00180         NODE_WIDTH = 0.02*compute_max_width(leafName);
00181     }
00182     renderBitmapString((x  + 2*0.015), (y - 0.01), font, leafName);
00183     break;
00184     default:
00185         break;
00186     }
00187 
00188     switch (status)
00189     {
00190     case BT::RUNNING:
00191         glColor3f(0.8, 0.8, 0.8);
00192         break;
00193     case BT::SUCCESS:
00194         glColor3f(0.0, 1.0, 0.0);
00195         break;
00196     case BT::FAILURE:
00197         glColor3f(1.0, 0.0, 0.0);
00198         break;
00199     case BT::IDLE:
00200         glColor3f(0.0, 0.0, 0.0);
00201         break;
00202     case BT::HALTED:
00203         glColor3f(0.0, 0.0, 0.0);
00204         break;
00205     default:
00206         break;
00207     }
00208 
00209     switch (node_type)
00210     {
00211     case BT::CONDITION:
00212         // drawEllipse(x,y,NODE_WIDTH,NODE_HEIGHT);
00213         // drawEllipse(x,y,0.1,0.021);
00214         glBegin(GL_LINE_LOOP);
00215         glVertex2f((GLfloat) (x + NODE_WIDTH), (GLfloat) (y - NODE_HEIGHT - 0.015));
00216         glVertex2f((GLfloat) (x + NODE_WIDTH), (GLfloat) (y + 0.02));
00217         glVertex2f((GLfloat) (x), (GLfloat) (y + 0.02));
00218         glVertex2f((GLfloat) (x), (GLfloat) (y - NODE_HEIGHT - 0.015));
00219         glColor3f(0.0, 0.0, 0.0);
00220         glEnd();
00221         break;
00222 
00223         break;
00224     case BT::ACTION:
00225 
00226         glBegin(GL_LINE_LOOP);
00227         glVertex2f((GLfloat) (x + NODE_WIDTH), (GLfloat) (y - NODE_HEIGHT - 0.015));
00228         glVertex2f((GLfloat) (x + NODE_WIDTH), (GLfloat) (y + 0.02));
00229         glVertex2f((GLfloat) (x), (GLfloat) (y + 0.02));
00230         glVertex2f((GLfloat) (x), (GLfloat) (y - NODE_HEIGHT - 0.015));
00231         glColor3f(0.0, 0.0, 0.0);
00232         glEnd();
00233         break;
00234 
00235     default:
00236         glBegin(GL_LINE_LOOP);
00237         glVertex2f((GLfloat) (x + NODE_WIDTH), (GLfloat) (y - NODE_HEIGHT));
00238         glVertex2f((GLfloat) (x + NODE_WIDTH), (GLfloat) (y + NODE_HEIGHT));
00239         glVertex2f((GLfloat) (x), (GLfloat) (y + NODE_HEIGHT));
00240         glVertex2f((GLfloat) (x), (GLfloat) (y - NODE_HEIGHT));
00241         glColor3f(0.0, 0.0, 0.0);
00242         glEnd();
00243         break;
00244     }
00245 }
00246 
00247 // draw the edge connecting one node to the other
00248 void draw_edge(GLfloat parent_x, GLfloat parent_y,
00249                GLfloat parent_size, GLfloat child_x, GLfloat child_y, GLfloat child_size)
00250 {
00251     glLineWidth(1.5);
00252     glColor3f(0.0, 0.0, 0.0);
00253     // GLfloat bottom_spacing = 0.1;  // commented-out as unused variable
00254     GLfloat above_spacing = 0.04;
00255 
00256     glBegin(GL_LINES);
00257     glVertex3f(parent_x, parent_y - parent_size, 0);
00258     glVertex3f(parent_x, child_y + child_size + above_spacing, 0);
00259     glEnd();
00260 
00261     glBegin(GL_LINES);
00262     glVertex3f(parent_x, child_y + child_size + above_spacing, 0);
00263     glVertex3f(child_x, child_y + child_size + above_spacing, 0);
00264     glEnd();
00265 
00266     glBegin(GL_LINES);
00267     glVertex3f(child_x, child_y + child_size + above_spacing, 0);
00268     glVertex3f(child_x, child_y+child_size, 0);
00269     glEnd();
00270 }
00271 
00272 // draw the edge connecting one node to the other
00273 void draw_straight_edge(GLfloat parent_x, GLfloat parent_y,
00274                         GLfloat parent_size, GLfloat child_x, GLfloat child_y, GLfloat child_size)
00275 {
00276     glLineWidth(1.5);
00277     glColor3f(0.0, 0.0, 0.0);
00278 
00279     glBegin(GL_LINES);
00280     glVertex3f(parent_x, parent_y-parent_size, 0.0);
00281     glVertex3f(child_x, child_y+child_size, 0);
00282     glEnd();
00283 }
00284 
00285 // Keyboard callback function ( called on keyboard event handling )
00286 void keyboard(unsigned char key, int x, int y)
00287 {
00288     for (int i = 1; i < 4; i++)
00289     {
00290         if (key == number_char[i])
00291         {
00292             is_number_pressed_array[i] = true;
00293         }
00294         else
00295         {
00296             is_number_pressed_array[i] = false;
00297         }
00298     }
00299 }
00300 
00301 void keyboard_release(unsigned char key, int x, int y)
00302 {
00303     for (int i = 1; i < 4; i++)
00304     {
00305         if (key == number_char[i])
00306         {
00307             is_number_pressed_array[i] = false;
00308         }
00309     }
00310 }
00311 
00312 
00313 void drawCircle(float radius)
00314 {
00315     glBegin(GL_LINE_LOOP);
00316 
00317     for (int i=0; i<= 360; i++)
00318     {
00319         float degInRad = i*3.14142/180;
00320         glVertex2f(cos(degInRad)*radius, sin(degInRad)*radius);
00321     }
00322     glEnd();
00323 }
00324 
00325 
00326 void updateTree(BT::TreeNode* tree, GLfloat x_pos, GLfloat y_pos, GLfloat y_offset, int depth )
00327 {
00328     BT::ControlNode* d = dynamic_cast<BT::ControlNode*> (tree);
00329     if (d == NULL)
00330     {
00331         // if it is a leaf node, draw it
00332         draw_node(x_pos , (GLfloat) y_pos, tree->DrawType(), tree->get_name().c_str(), tree->get_color_status());
00333     }
00334     else
00335     {
00336         // if it is a control flow node, draw it and its children
00337         draw_node((GLfloat) x_pos, (GLfloat) y_pos, tree->DrawType(),
00338                   tree->get_name().c_str(), tree->get_color_status());
00339         std::vector<BT::TreeNode*> children = d->GetChildren();
00340         int M = d->GetChildrenNumber();
00341         std::vector<GLfloat> children_x_end;
00342         std::vector<GLfloat> children_x_middle_relative;
00343 
00344         GLfloat max_x_end = 0;
00345         // GLfloat max_x_start = 0;  // commented out as unused variable
00346         GLfloat current_x_end = 0;
00347 
00348         for (int i = 0; i < M; i++)
00349         {
00350             if (children[i]->DrawType() != BT::ACTION && children[i]->DrawType() != BT::CONDITION)
00351             {
00352                 current_x_end = 0.04;
00353                 children_x_middle_relative.push_back(0.02);
00354             }
00355             else
00356             {
00357                 current_x_end = 0.02*compute_max_width(children[i]->get_name().c_str());
00358                 children_x_middle_relative.push_back(current_x_end/2);
00359             }
00360 
00361             if (i < M-1)
00362             {
00363                 max_x_end = max_x_end + current_x_end + x_space + additional_spacing_array[depth];
00364             }
00365             else
00366             {
00367                 max_x_end = max_x_end + current_x_end;
00368             }
00369             children_x_end.push_back(max_x_end);
00370         }
00371 
00372         // GLfloat x_min = 0.0;  // commented-out as unused variable
00373         // GLfloat x_max = 0.0;  // commented-out as unused variable
00374         GLfloat x_shift = x_pos - max_x_end/2;
00375         // GLfloat x_shift_new = 0;  // commented-out as unused variable
00376 
00377         for (int i = 0; i < M; i++)
00378         {
00379             if (i > 0)
00380             {
00381                 updateTree(children[i], x_shift + children_x_end.at(i - 1) , y_pos - y_offset  , y_offset, depth + 1);
00382                 draw_edge(x_pos + 0.015, y_pos, 0.02,
00383                           x_shift + children_x_end.at(i-1) + children_x_middle_relative.at(i),
00384                           y_pos - y_offset, 0.02);
00385             }
00386             else
00387             {
00388                 updateTree(children[i], x_shift , y_pos - y_offset  , y_offset, depth + 1);
00389                 draw_edge(x_pos + 0.015, y_pos, 0.02,
00390                           x_shift + children_x_middle_relative.at(i), y_pos - y_offset, 0.02);
00391             }
00392         }
00393     }
00394 }
00395 
00396 
00397 
00398 
00399 
00400 void display()
00401 {
00402     glClearColor(r_color, g_color, b_color, 0.1);    // clear the draw buffer .
00403     glClear(GL_COLOR_BUFFER_BIT);   // Erase everything
00404     updateTree(tree, x , y, y_offset, 1);
00405     glutSwapBuffers();
00406     glutPostRedisplay();
00407 }
00408 
00409 
00410 void processSpecialKeys(int key, int xx, int yy)
00411 {
00412     switch (key)
00413     {
00414     case GLUT_KEY_UP :
00415         y +=  fraction;
00416         break;
00417     case GLUT_KEY_DOWN :
00418         y -=  fraction;
00419         break;
00420     case GLUT_KEY_LEFT:
00421         x -=  fraction;
00422         break;
00423     case GLUT_KEY_RIGHT:
00424         x +=  fraction;
00425         break;
00426     case  GLUT_KEY_PAGE_UP:
00427         for (int i = 1; i < 10; i++)
00428         {
00429             if (is_number_pressed_array[i])
00430             {
00431                 additional_spacing_array[i] += fraction;
00432             }
00433         }
00434         break;
00435     case  GLUT_KEY_PAGE_DOWN:
00436         for (int i = 1; i < 10; i++)
00437         {
00438             if (is_number_pressed_array[i] && additional_spacing_array[i] >= 0 )
00439             {
00440                 additional_spacing_array[i] -= fraction;
00441             }
00442         }
00443         break;
00444     case  GLUT_KEY_F1:
00445         if (r_color < 1)  r_color +=  fraction;
00446         break;
00447     case  GLUT_KEY_F2:
00448         if (r_color > 0) r_color -=  fraction;
00449         break;
00450     case  GLUT_KEY_F3:
00451         if (g_color < 1) g_color +=  fraction;
00452         break;
00453     case  GLUT_KEY_F4:
00454         if (g_color > 0) g_color -=  fraction;
00455         break;
00456     case  GLUT_KEY_F5:
00457         if (b_color < 1) b_color +=  fraction;
00458         break;
00459     case  GLUT_KEY_F6:
00460         if (b_color > 0) b_color -=  fraction;
00461         break;
00462     case GLUT_KEY_HOME:
00463         if (zoom < 1.0f)
00464         {
00465             glScalef(1.0f + zoom_fraction, 1.0f + zoom_fraction, 1.0f);
00466             zoom += zoom_fraction;
00467         }
00468         else
00469         {
00470             glScalef(1.0f, 1.0f, 1.0f);
00471         }
00472         break;
00473     case GLUT_KEY_END:
00474         glScalef(1.0f  - zoom_fraction, 1.0f  - zoom_fraction, 1.0f);
00475         zoom -= zoom_fraction;
00476         break;
00477     }
00478 }
00479 
00480 void drawTree(BT::ControlNode* tree_)
00481 {
00482     /***************************BT VISUALIZATION****************************/
00483     int argc = 1;
00484     char *argv[1] = {const_cast<char*>("")};
00485 
00486     if (!init)
00487     {
00488         XInitThreads();
00489         glutInit(&argc, argv);
00490         init = true;
00491         glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGBA | GLUT_DEPTH | GLUT_MULTISAMPLE);  //  Antialiasing
00492         glEnable(GL_MULTISAMPLE);
00493     }
00494     tree = tree_;
00495     depth = tree->Depth();
00496 
00497     glutInitWindowSize(1024, 860);
00498 
00499     glutCreateWindow("Behavior Tree");  // Create a window
00500 
00501     glClearColor(0, 0.71, 0.00, 0.1);
00502     glutDisplayFunc(display);  // Register display callback
00503 
00504     glutKeyboardFunc(keyboard);  // Register keyboard presscallback
00505     glutKeyboardUpFunc(keyboard_release);  // Register keyboard release callback
00506 
00507     glutSpecialFunc(processSpecialKeys);  // Register keyboard arrow callback
00508 
00509     glutMainLoop();             // Enter main event loop
00510 
00511     /***************************ENDOF BT VISUALIZATION ****************************/
00512 }
00513 
00514 
00515 
00516 
00517 
00518 
00519 
00520 
00521 
00522 
00523 


behavior_tree_core
Author(s): Michele Colledanchise
autogenerated on Sun Sep 10 2017 02:31:49