ico-app-samplenavi/src/e3d.c

   1 /*
   2  * Copyright (c) 2013, TOYOTA MOTOR CORPORATION.
   3  *
   4  * This program is licensed under the terms and conditions of the
   5  * Apache License, version 2.0.  The full text of the Apache License is at
   6  * http://www.apache.org/licenses/LICENSE-2.0
   7  *
   8  */
   9 /**
  10  * @brief   library which displays 3D field of view
  11  *
  12  * @date    Apr-25-2013
  13  */
  14
  15 #include <stdlib.h>
  16 #include <string.h>
  17 #include "samplenavi.h"
  18 #include "e3d.h"
  19 #include "ico_apf_log.h"
  20
  21 /*============================================================================*/
  22 /* Define fixed parameters                                                    */
  23 /*============================================================================*/
  24 /* Get route from websocket */
  25 #define ROUTE_START_TAG "<route>"
  26 #define ROUTE_END_TAG   "</route>"
  27 #define BR_TAG          "<BR>"
  28
  29 /*============================================================================*/
  30 /* Define data types                                                          */
  31 /*============================================================================*/
  32 /* Landmark Points and Cubes */
  33 typedef struct _PointLength
  34 {
  35     int index;
  36     double length;
  37     int drawable;
  38 } PointLength;
  39
  40 /*============================================================================*/
  41 /* Function prototype for static(internal) functions                          */
  42 /*============================================================================*/
  43 static void load_route_from_websocket(char *p_in, size_t len);
  44 static void calc_route_coord_from_lonlat();
  45 static void load_landmarks_from_csv();
  46 static void calc_landmarks_coord_from_lonlat();
  47 static const char *landmark_img_path(int i);
  48 static void _camera_pos(Evas_Coord x, Evas_Coord y, Evas_Coord z,
  49                         double angle);
  50 static Plane *_plane_new(Evas *e3d, Evas_Coord w, Evas_Coord d,
  51                          Evas_Coord h);
  52 static void _plane_draw(Plane *plane);
  53 static void _plane_free(Plane *plane);
  54 static Cube *_cube_new(Evas *e3d, Evas_Coord w, Evas_Coord h, Evas_Coord d,
  55                        const char *img_path);
  56 static void _cube_draw(Cube *c, Evas_Coord x, Evas_Coord y, Evas_Coord z,
  57                        double dx, double dy, double dz);
  58 static void _cube_free(Cube *c);
  59 static void goal_object_new(Evas *e3d);
  60 static void goal_object_draw();
  61 static void goal_object_free();
  62 static void check_route_drawable_by_z_limit_fix();
  63 static void calc_route_coord_by_camera();
  64 static void calc_route_distance_to_camera();
  65 static void check_route_drawable_by_distance_to_camera();
  66 static void calc_fixed_box_points_all();
  67 static void calc_fixed_box_points(int routenum);
  68 static Point calc_fix_point(int routenum, int point_index);
  69 static void draw_curve(int routenum);
  70 static void calc_bezier_points(Point p0, Point p1, Point p2, Point *points);
  71 static void calc_base_box_points_all();
  72 static void calc_base_box_points(int routenum);
  73 static void calc_landmarks_coord_by_camera();
  74 static void calc_route_distance();
  75 static double calc_length(Point p0, Point p1);
  76 static double calc_square_length(Point p0, Point p1);
  77 static void rotate_xz(Point src, Point *dist, double angle);
  78 static void calc_intersection(Point p0, Point p1, Point p2, Point p3,
  79                               Point *point);
  80 static int length_comp(const void *pl0, const void *pl1);
  81
  82 /*============================================================================*/
  83 /* Tables and Valiables                                                       */
  84 /*============================================================================*/
  85 Camera camera;
  86 CameraGeocode camera_geocode;
  87 MapPos map_pos;
  88 char set_route;
  89 char enable_navi;
  90 #ifdef _SHOW_FPS_
  91 int polygon_count;
  92 #endif
  93
  94 /* Route Points and Planes */
  95 int route_data_count;
  96 CsvRoute csv_route[MAX_ROUTE_POINTS];
  97 Point route_raw[MAX_DRAW_ROUTE_POINTS];
  98 Point route[MAX_DRAW_ROUTE_POINTS];
  99 double route_square_length_camera[MAX_DRAW_ROUTE_POINTS];
 100 #ifdef _USE_Z_LIMIT_FIX_
 101 Point route_pre_fix[MAX_DRAW_ROUTE_POINTS];
 102 #endif
 103 Plane *base_plane[MAX_DRAW_ROUTE_POINTS - 1];
 104 Plane *fixed_plane[MAX_DRAW_ROUTE_POINTS - 1];
 105 Plane *division_plane[MAX_DRAW_ROUTE_POINTS - 2][ROUTE_DIVISION_NUMBER];
 106 int route_draw_num;
 107 double route_distance[MAX_DRAW_ROUTE_POINTS - 1];
 108 Evas_Object *goal_object;
 109 double goal_square_length;
 110
 111 /* Landmark Points and Cubes */
 112 int landmark_data_count;
 113 CsvLandmark csv_landmark[MAX_LANDMARK_POINTS];
 114 Point landmark_raw[MAX_LANDMARK_POINTS];
 115 Point landmark[MAX_LANDMARK_POINTS];
 116 Cube *landmark_cube[MAX_LANDMARK_POINTS];
 117
 118 static const char *goal_img_path = IMAGES_DIR "/na_goal.png";
 119 static const char *landmark_img_path_0 = IMAGES_DIR "/sg_m_01.png";
 120 static const char *landmark_img_path_1 = IMAGES_DIR "/sg_m_02.png";
 121 static const char *landmark_img_path_2 = IMAGES_DIR "/sg_m_03.png";
 122 static const char *landmark_img_path_3 = IMAGES_DIR "/sg_m_04.png";
 123 static const char *landmark_img_path_4 = IMAGES_DIR "/sg_m_05.png";
 124 static const char *landmark_img_path_5 = IMAGES_DIR "/sg_m_06.png";
 125 static const char *landmark_img_path_6 = IMAGES_DIR "/sg_m_07.png";
 126 static const char *landmark_img_path_7 = IMAGES_DIR "/sg_m_08.png";
 127 static const char *landmark_img_path_8 = IMAGES_DIR "/sg_l_01.png";
 128 static const char *landmark_img_path_9 = IMAGES_DIR "/sg_l_02.png";
 129 static const char *landmark_img_path_10 = IMAGES_DIR "/sg_l_03.png";
 130 static const char *landmark_img_path_11 = IMAGES_DIR "/sg_l_04.png";
 131 static const char *landmark_img_path_12 = IMAGES_DIR "/sg_l_05.png";
 132 static const char *landmark_img_path_13 = IMAGES_DIR "/sg_l_06.png";
 133 static const char *landmark_img_path_14 = IMAGES_DIR "/sg_l_07.png";
 134 static const char *landmark_img_path_15 = IMAGES_DIR "/sg_l_08.png";
 135 static const char *csv_landmark_path = RESOURCE_DIR "/landmark.csv";
 136
 137 /*============================================================================*/
 138 /* functions                                                                  */
 139 /*============================================================================*/
 140 /*--------------------------------------------------------------------------*/
 141 /*
 142  * @brief   init_e3d
 143  *          Initialization of 3D field-of-view information.
 144  *
 145  * @param[in]   e3d               3D view Evas object
 146  * @param[in]   p_in              receive message
 147  * @param[in]   len               message size[BYTE]
 148  * @return      none
 149  */
 150 /*--------------------------------------------------------------------------*/
 151 void init_e3d(Evas *e3d, void *p_in, size_t len)
 152 {
 153     int i, j;
 154
 155     goal_square_length = 1000000000000000.0;
 156
 157     /***********************
 158      * Route Initialize *
 159      ***********************/
 160
 161     /* load route data */
 162     load_route_from_websocket((char *) p_in, len);
 163
 164     calc_route_coord_from_lonlat();
 165     route_draw_num = route_data_count;
 166
 167     for (i = 0; i < route_draw_num - 1; i++) {
 168         base_plane[i] = _plane_new(e3d, 100, 400, 170);
 169         fixed_plane[i] = _plane_new(e3d, 100, 400, 170);
 170     }
 171     for (i = 0; i < route_draw_num - 2; i++) {
 172         for (j = 0; j < ROUTE_DIVISION_NUMBER; j++) {
 173             division_plane[i][j] = _plane_new(e3d, 10, 10, 10);
 174         }
 175     }
 176
 177     goal_object_new(e3d);
 178
 179 #ifndef _USE_Z_LIMIT_FIX_
 180     calc_route_distance();
 181 #endif
 182
 183     calc_route_coord_by_camera();
 184     calc_base_box_points_all();
 185     calc_fixed_box_points_all();
 186
 187 #ifdef _E3D_H_OUTPUT_PRINT_
 188     uim_debug("Base Plane point0 = %f, %f, %f \n", base_plane[0]->pt[0].x,
 189               base_plane[0]->pt[0].y, base_plane[0]->pt[0].z);
 190     uim_debug("Base Plane point1 = %f, %f, %f \n", base_plane[0]->pt[1].x,
 191               base_plane[0]->pt[1].y, base_plane[0]->pt[1].z);
 192     uim_debug("Base Plane point2 = %f, %f, %f \n", base_plane[0]->pt[2].x,
 193               base_plane[0]->pt[2].y, base_plane[0]->pt[2].z);
 194     uim_debug("Base Plane point3 = %f, %f, %f \n", base_plane[0]->pt[3].x,
 195               base_plane[0]->pt[3].y, base_plane[0]->pt[3].z);
 196 #endif
 197
 198     /***********************
 199      * Landmark Initialize *
 200      ***********************/
 201
 202     /* load landmark data */
 203     load_landmarks_from_csv();
 204     calc_landmarks_coord_from_lonlat();
 205
 206     for (i = 0; i < landmark_data_count; i++) {
 207         landmark_cube[i] = _cube_new(e3d, LANDMARK_WIDTH,
 208                                      (LANDMARK_WIDTH / 3) * 4, LANDMARK_WIDTH,
 209                                      landmark_img_path(csv_landmark[i].id));
 210     }
 211
 212     /***********************************************************************/
 213
 214     // for test
 215     uim_debug("Route CSV x = %f, z = %f\n",
 216               csv_route[1].lon, csv_route[1].lat);
 217     uim_debug("Route x = %f, z = %f\n", route_raw[1].x, route_raw[1].z);
 218     uim_debug("Landmark CSV x = %f, z = %f\n",
 219               csv_landmark[0].lon, csv_landmark[0].lat);
 220     uim_debug("Landmark x = %f, z = %f\n",
 221               landmark_raw[0].x, landmark_raw[0].z);
 222
 223     /***********************
 224      * Camera Initialize *
 225      ***********************/
 226     _camera_pos(0, 0, 0, 0);
 227     camera_geocode.lon = csv_route[0].lon;
 228     camera_geocode.lat = csv_route[0].lat;
 229     camera_geocode.dir = 0;
 230     calc_camera_coord();
 231
 232     /***********************
 233      * Map present location Initialize *
 234      ***********************/
 235     map_pos.lon = csv_route[0].lon;
 236     map_pos.lat = csv_route[0].lat;
 237
 238     /* draw e3d */
 239     draw_route(e3d);
 240     draw_landmark(e3d);
 241 }
 242
 243 /*--------------------------------------------------------------------------*/
 244 /*
 245  * @brief   e3d_cleanup
 246  *          3D field-of-view object cleaning.
 247  *
 248  * @param     none
 249  * @return    none
 250  */
 251 /*--------------------------------------------------------------------------*/
 252 void e3d_cleanup()
 253 {
 254     int i, j;
 255
 256     for (i = 0; i < route_draw_num - 1; i++) {
 257         _plane_free(base_plane[i]);
 258         _plane_free(fixed_plane[i]);
 259     }
 260
 261     for (i = 0; i < route_draw_num - 2; i++) {
 262         for (j = 0; j < ROUTE_DIVISION_NUMBER; j++) {
 263             _plane_free(division_plane[i][j]);
 264         }
 265     }
 266
 267     for (i = 0; i < landmark_data_count; i++) {
 268         _cube_free(landmark_cube[i]);
 269     }
 270
 271     goal_object_free();
 272 }
 273
 274 /*--------------------------------------------------------------------------*/
 275 /*
 276  * @brief   draw_route
 277  *          3D field-of-view route drawing.
 278  *
 279  * @param[in]   e3d               3D view Evas object
 280  * @return      none
 281  */
 282 /*--------------------------------------------------------------------------*/
 283 void draw_route(Evas *e3d)
 284 {
 285     int i;
 286     int j;
 287
 288 #ifdef _SHOW_FPS_
 289     polygon_count = 0;
 290 #endif
 291
 292     calc_route_coord_by_camera();
 293     calc_base_box_points_all();
 294     calc_fixed_box_points_all();
 295
 296     /***********************************************************************/
 297
 298     if (enable_navi == FALSE || set_route == FALSE) {
 299         evas_object_hide(goal_object);
 300         return;
 301     }
 302
 303     for (i = 0; i < route_draw_num - 1; i++) {
 304         if (fixed_plane[i]->drawable == TRUE) {
 305             _plane_draw(fixed_plane[i]);
 306         }
 307         else {
 308             evas_object_hide(fixed_plane[i]->o);
 309         }
 310     }
 311
 312     /* curve draw */
 313     for (i = 0; i < route_draw_num - 2; i++) {
 314         if (fixed_plane[i]->drawable == TRUE) {
 315             draw_curve(i);
 316         }
 317         else {
 318             for (j = 0; j < ROUTE_DIVISION_NUMBER; j++) {
 319                 evas_object_hide(division_plane[i][j]->o);
 320             }
 321         }
 322 #ifdef _E3D_H_OUTPUT_PRINT_
 323         uim_debug("curve draw. \n");
 324 #endif
 325     }
 326
 327 #ifdef _E3D_H_OUTPUT_PRINT_
 328     uim_debug("route draw comp.\n");
 329 #endif
 330
 331     goal_object_draw(fixed_plane[route_draw_num - 2]);
 332 }
 333
 334 /*--------------------------------------------------------------------------*/
 335 /*
 336  * @brief   draw_landmark
 337  *          3D field-of-view landmark drawing.
 338  *
 339  * @param[in]   e3d               3D view Evas object
 340  * @return      none
 341  */
 342 /*--------------------------------------------------------------------------*/
 343 void draw_landmark(Evas *e3d)
 344 {
 345     static unsigned long frame_count = 0;
 346     PointLength draw_order[landmark_data_count];
 347     int i, j, k;
 348     Point camera_coord;
 349     int landmark_angle;
 350     double culling_length;
 351
 352     if (enable_navi == FALSE) {
 353         for (i = 0; i < landmark_data_count; i++) {
 354             for (j = 0; j < 6; j++) {
 355                 evas_object_hide(landmark_cube[i]->side[j].o);
 356             }
 357         }
 358         return;
 359     }
 360     calc_landmarks_coord_by_camera();
 361
 362     camera_coord.x = camera.x;
 363     camera_coord.z = camera.z;
 364
 365     /* calc landmark rotation */
 366     landmark_angle =
 367         (int) ((conf_data[LANDMARK_ROTATION] / (1.0 / TIME_INTERVAL_AR)) *
 368                frame_count) % 360;
 369
 370     culling_length = CULLING_LENGTH_LANDMARKS * CULLING_LENGTH_LANDMARKS;
 371
 372     /* In order to draw from distant landmark. */
 373     for (i = 0; i < landmark_data_count; i++) {
 374         draw_order[i].index = i;
 375         draw_order[i].length = calc_square_length(landmark[i], camera_coord);
 376
 377         /* for culling */
 378         if (culling_length < draw_order[i].length) {
 379             draw_order[i].drawable = FALSE;
 380         }
 381         else {
 382             draw_order[i].drawable = TRUE;
 383         }
 384     }
 385
 386     qsort(draw_order, landmark_data_count, sizeof(PointLength), length_comp);
 387
 388     for (i = 0; i < landmark_data_count; i++) {
 389         j = draw_order[i].index;
 390         if (draw_order[i].drawable == TRUE) {
 391             _cube_draw(landmark_cube[j], landmark[j].x, landmark[j].y,
 392                        landmark[j].z, 0, landmark_angle, 0);
 393         }
 394         else {
 395             for (k = 0; k < 6; k++) {
 396                 evas_object_hide(landmark_cube[j]->side[k].o);
 397             }
 398         }
 399     }
 400
 401     frame_count++;
 402 }
 403
 404 /*--------------------------------------------------------------------------*/
 405 /*
 406  * @brief   calc_camera_coord
 407  *          Calculation of the coordinates of a camera.
 408  *
 409  * @param       none
 410  * @return      none
 411  */
 412 /*--------------------------------------------------------------------------*/
 413 void calc_camera_coord()
 414 {
 415     double x, z;
 416
 417     x = (camera_geocode.lon -
 418          csv_route[0].lon) * LON_CONVERT * COORD_CONVERT_COEFFICIENT;
 419     z = (camera_geocode.lat -
 420          csv_route[0].lat) * LAT_CONVERT * COORD_CONVERT_COEFFICIENT;
 421
 422     _camera_pos(x, camera.y, z, (360.0 - (double) camera_geocode.dir));
 423
 424 }
 425
 426 /*--------------------------------------------------------------------------*/
 427 /*
 428  * @brief   load_route_from_websocket
 429  *          route information acquisition.
 430  *
 431  * @param[in]   p_in              receive message
 432  * @param[in]   len               message size[BYTE]
 433  * @return      none
 434  */
 435 /*--------------------------------------------------------------------------*/
 436 static void load_route_from_websocket(char *p_in, size_t len)
 437 {
 438     /* Local variable */
 439     size_t index = 0;
 440     char str[256];
 441     route_data_count = 0;
 442
 443     /* Get string */
 444     while (index < len) {
 445         /* Get start tag */
 446         if ((strlen(&p_in[index]) >= strlen(ROUTE_START_TAG)) &&
 447             (strncmp(&p_in[index], ROUTE_START_TAG, strlen(ROUTE_START_TAG))
 448              == 0)) {
 449             index += strlen(ROUTE_START_TAG);
 450         }
 451         /* Get end tag */
 452         else if ((strlen(&p_in[index]) >= strlen(ROUTE_END_TAG)) &&
 453                  (strncmp
 454                   (&p_in[index], ROUTE_START_TAG,
 455                    strlen(ROUTE_END_TAG)) == 0)) {
 456             break;
 457         }
 458         /* Get br tag */
 459         else if ((strlen(&p_in[index]) >= strlen(BR_TAG)) &&
 460                  (strncmp(&p_in[index], BR_TAG, strlen(BR_TAG)) == 0)) {
 461             index += strlen(BR_TAG);
 462         }
 463         /* Get parameter */
 464         else {
 465             /* Get lng */
 466             memset(str, 0, sizeof(str));
 467             while ((p_in[index] != ',') && (index < len)) {
 468                 str[strlen(str)] = p_in[index];
 469                 index++;
 470             }
 471             csv_route[route_data_count].lon = atof(str);
 472             index++;
 473
 474             /* Get lat */
 475             memset(str, 0, sizeof(str));
 476             while ((p_in[index] != '<') && (index < len)) {
 477                 str[strlen(str)] = p_in[index];
 478                 index++;
 479             }
 480             csv_route[route_data_count].lat = atof(str);
 481             route_data_count++;
 482         }
 483     }
 484     if (route_data_count > 0) {
 485         route_data_count--;
 486     }
 487 }
 488
 489 /*--------------------------------------------------------------------------*/
 490 /*
 491  * @brief   calc_route_coord_from_lonlat
 492  *          The coordinates of a route are calculated from
 493  *          latitude and longitude.
 494  *
 495  * @param     none
 496  * @return    none
 497  */
 498 /*--------------------------------------------------------------------------*/
 499 static void calc_route_coord_from_lonlat()
 500 {
 501     int i;
 502
 503     for (i = 0; i < route_data_count; i++) {
 504         double x =
 505             (csv_route[i].lon -
 506              csv_route[0].lon) * LON_CONVERT * COORD_CONVERT_COEFFICIENT;
 507         double z =
 508             (csv_route[i].lat -
 509              csv_route[0].lat) * LAT_CONVERT * COORD_CONVERT_COEFFICIENT;
 510         POINT(route_raw[i], x, ROUTE_PLANE_HEIGHT, z, 0, 0);
 511     }
 512 }
 513
 514 /*--------------------------------------------------------------------------*/
 515 /*
 516  * @brief   load_landmarks_from_csv
 517  *          The information on a landmark is loaded from a CSV file.
 518  *
 519  * @param     none
 520  * @return    none
 521  */
 522 /*--------------------------------------------------------------------------*/
 523 static void load_landmarks_from_csv()
 524 {
 525     FILE *fp;
 526     int i;
 527
 528     if ((fp = fopen(csv_landmark_path, "r")) == NULL) {
 529         fprintf(stderr, "%s\n", "Error : can't open file.(landmark.csv)\n");
 530         landmark_data_count = 0;
 531         return;
 532     }
 533
 534     i = 0;
 535     while (fscanf(fp, "%lf,%lf,%hhd",
 536                   &csv_landmark[i].lon, &csv_landmark[i].lat,
 537                   &csv_landmark[i].id) != EOF) {
 538         i++;
 539     }
 540     landmark_data_count = i;
 541
 542     fclose(fp);
 543
 544     // for test
 545     for (i = 0; i < landmark_data_count; i++) {
 546         uim_debug("landmark data %d : %.4f, %.4f, %d\n",
 547                   i, csv_landmark[i].lon, csv_landmark[i].lat,
 548                   csv_landmark[i].id);
 549     }
 550
 551     uim_debug("landmark data read. count = %d\n", landmark_data_count);
 552
 553     return;
 554 }
 555
 556 /*--------------------------------------------------------------------------*/
 557 /*
 558  * @brief   calc_landmarks_coord_from_lonlat
 559  *          The coordinates of a landmark are calculated from
 560  *          latitude and longitude.
 561  *
 562  * @param     none
 563  * @return    none
 564  */
 565 /*--------------------------------------------------------------------------*/
 566 static void calc_landmarks_coord_from_lonlat()
 567 {
 568     int i;
 569
 570     for (i = 0; i < landmark_data_count; i++) {
 571         double x =
 572             (csv_landmark[i].lon -
 573              csv_route[0].lon) * LON_CONVERT * COORD_CONVERT_COEFFICIENT;
 574         double z =
 575             (csv_landmark[i].lat -
 576              csv_route[0].lat) * LAT_CONVERT * COORD_CONVERT_COEFFICIENT;
 577         POINT(landmark_raw[i], x, conf_data[LANDMARK_POSITION], z, 0, 0);
 578     }
 579 }
 580
 581 /*--------------------------------------------------------------------------*/
 582 /*
 583  * @brief   landmark_img_path
 584  *          Image file path acquisition of a landmark.
 585  *
 586  * @param[in]   landmark image file path identifier
 587  * @return      file path address
 588  * @retval      > 0      success
 589  * @retval      NULL     error
 590  */
 591 /*--------------------------------------------------------------------------*/
 592 static const char *landmark_img_path(int i)
 593 {
 594     const char *img_path;
 595     switch (i) {
 596     case 0:
 597         img_path = landmark_img_path_0;
 598         break;
 599     case 1:
 600         img_path = landmark_img_path_1;
 601         break;
 602     case 2:
 603         img_path = landmark_img_path_2;
 604         break;
 605     case 3:
 606         img_path = landmark_img_path_3;
 607         break;
 608     case 4:
 609         img_path = landmark_img_path_4;
 610         break;
 611     case 5:
 612         img_path = landmark_img_path_5;
 613         break;
 614     case 6:
 615         img_path = landmark_img_path_6;
 616         break;
 617     case 7:
 618         img_path = landmark_img_path_7;
 619         break;
 620     case 8:
 621         img_path = landmark_img_path_8;
 622         break;
 623     case 9:
 624         img_path = landmark_img_path_9;
 625         break;
 626     case 10:
 627         img_path = landmark_img_path_10;
 628         break;
 629     case 11:
 630         img_path = landmark_img_path_11;
 631         break;
 632     case 12:
 633         img_path = landmark_img_path_12;
 634         break;
 635     case 13:
 636         img_path = landmark_img_path_13;
 637         break;
 638     case 14:
 639         img_path = landmark_img_path_14;
 640         break;
 641     case 15:
 642         img_path = landmark_img_path_15;
 643         break;
 644     }
 645
 646     return img_path;
 647 }
 648
 649 /*--------------------------------------------------------------------------*/
 650 /*
 651  * @brief   _camera_pos
 652  *          The setup of camera position.
 653  *
 654  * @param[in]   x               x coordinates
 655  * @param[in]   y               y coordinates
 656  * @param[in]   z               z coordinates
 657  * @param[in]   angle           angle
 658  * @return      none
 659  */
 660 /*--------------------------------------------------------------------------*/
 661 static void
 662 _camera_pos(Evas_Coord x, Evas_Coord y, Evas_Coord z, double angle)
 663 {
 664     // uim_debug("camera_pos x:%d y:%d z:%d angle:%d", x, y, z, angle);
 665     camera.x = x;
 666     camera.y = y;
 667     camera.z = z;
 668     camera.angle = angle;
 669 }
 670
 671 /******************************************************************************
 672  * Functions for Drawing on Evas
 673  ******************************************************************************/
 674 /*--------------------------------------------------------------------------*/
 675 /*
 676  * @brief   _plane_new
 677  *          plane generation.
 678  *
 679  * @param[in]       e3d               3D view Evas object
 680  * @param[in]       w                 Width
 681  * @param[in]       d                 Depth
 682  * @param[in]       h                 Height
 683  * @return          plane address
 684  * @retval          > 0      success
 685  * @retval          NULL     error
 686  */
 687 /*--------------------------------------------------------------------------*/
 688 static Plane *_plane_new(Evas *e3d, Evas_Coord w, Evas_Coord d, Evas_Coord h)
 689 {
 690     Plane *plane;
 691     Evas_Object *o;
 692
 693     plane = calloc(1, sizeof(Plane));
 694
 695     /* origin point */
 696     w -= (w / 2);
 697
 698     o = evas_object_rectangle_add(e3d);
 699     plane->o = o;
 700
 701     evas_object_resize(o, 256, 256);
 702     evas_object_hide(o);
 703
 704     SIDE_POINT(0, -w, -h, d, 0, 0);
 705     SIDE_POINT(1, w, -h, d, 256, 0);
 706     SIDE_POINT(2, w, -h, -d, 256, 256);
 707     SIDE_POINT(3, -w, -h, -d, 0, 256);
 708
 709     evas_object_layer_set(o, LAYER_ROUTE);
 710
 711     plane->drawable = TRUE;
 712
 713     return plane;
 714 }
 715
 716 /*--------------------------------------------------------------------------*/
 717 /*
 718  * @brief   _plane_draw
 719  *          plane drawing.
 720  *
 721  * @param[in]       plane             plane address
 722  * @return          none
 723  */
 724 /*--------------------------------------------------------------------------*/
 725 static void _plane_draw(Plane *plane)
 726 {
 727     static Evas_Map *m2 = NULL;
 728     int i;
 729
 730     if (!m2)
 731         m2 = evas_map_new(4);
 732     evas_map_smooth_set(m2, 0);
 733
 734 #ifdef _USE_Z_LIMIT_FIX_
 735     if (plane->drawable == FALSE) {
 736         evas_object_hide(plane->o);
 737         return;
 738     }
 739 #endif
 740
 741     evas_object_color_set(plane->o, 51, 178, 0, 179);
 742
 743     /* set points */
 744     evas_map_point_coord_set(m2, 0,
 745                              (Evas_Coord) (plane->pt[0].x + ((W_WIDTH) / 4)),
 746                              (Evas_Coord) (plane->pt[0].y +
 747                                            ((W_NAVI_HEIGHT) / 4)),
 748                              (Evas_Coord) (plane->pt[0].z + FRONT_SIDE_Z));
 749     evas_map_point_coord_set(m2, 1,
 750                              (Evas_Coord) (plane->pt[1].x + ((W_WIDTH) / 4)),
 751                              (Evas_Coord) (plane->pt[1].y +
 752                                            ((W_NAVI_HEIGHT) / 4)),
 753                              (Evas_Coord) (plane->pt[1].z + FRONT_SIDE_Z));
 754     evas_map_point_coord_set(m2, 2,
 755                              (Evas_Coord) (plane->pt[2].x + ((W_WIDTH) / 4)),
 756                              (Evas_Coord) (plane->pt[2].y +
 757                                            ((W_NAVI_HEIGHT) / 4)),
 758                              (Evas_Coord) (plane->pt[2].z + FRONT_SIDE_Z));
 759     evas_map_point_coord_set(m2, 3,
 760                              (Evas_Coord) (plane->pt[3].x + ((W_WIDTH) / 4)),
 761                              (Evas_Coord) (plane->pt[3].y +
 762                                            ((W_NAVI_HEIGHT) / 4)),
 763                              (Evas_Coord) (plane->pt[3].z + FRONT_SIDE_Z));
 764
 765     for (i = 0; i < 4; i++) {
 766         evas_map_point_image_uv_set(m2, i, plane->pt[i].u, plane->pt[i].v);
 767         evas_map_point_color_set(m2, i, 255, 255, 255, 255);
 768     }
 769     evas_map_util_3d_perspective(m2, (W_WIDTH / 4), (W_NAVI_HEIGHT / 4) - 100,
 770                                  Z0_POSITION, FOCUS_LENGTH / 2);
 771     evas_object_map_enable_set(plane->o, 1);
 772     evas_object_map_set(plane->o, m2);
 773
 774     evas_object_show(plane->o);
 775
 776 #ifdef _SHOW_FPS_
 777     polygon_count++;
 778 #endif
 779
 780 }
 781
 782 /*--------------------------------------------------------------------------*/
 783 /*
 784  * @brief   _plane_free
 785  *          plane object release.
 786  *
 787  * @param[in]       plane             plane address
 788  * @return          none
 789  */
 790 /*--------------------------------------------------------------------------*/
 791 static void _plane_free(Plane *plane)
 792 {
 793     evas_object_del(plane->o);
 794     free(plane);
 795 }
 796
 797 /*--------------------------------------------------------------------------*/
 798 /*
 799  * @brief   _cube_new
 800  *          cube generation.
 801  *
 802  * @param[in]       e3d               3D view Evas object
 803  * @param[in]       w                 Width
 804  * @param[in]       h                 Height
 805  * @param[in]       d                 Depth
 806  * @param[in]       img_path          image file path address
 807  * @return          cube address
 808  * @retval          > 0      success
 809  * @retval          NULL     error
 810  */
 811 /*--------------------------------------------------------------------------*/
 812 static Cube *_cube_new(Evas *e3d, Evas_Coord w, Evas_Coord h, Evas_Coord d,
 813                        const char *img_path)
 814 {
 815     Cube *c;
 816     int i;
 817
 818     w -= (w / 2);
 819     h -= (h / 2);
 820     d -= (d / 2);
 821     c = calloc(1, sizeof(Cube));
 822     for (i = 0; i < 6; i++) {
 823         Evas_Object *o;
 824         switch (i) {
 825         case 0:
 826         case 1:
 827         case 2:
 828         case 3:
 829             o = evas_object_image_add(e3d);
 830             c->side[i].o = o;
 831             evas_object_image_file_set(o, img_path, NULL);
 832             evas_object_image_fill_set(o, 0, 0, 256, 256);
 833             evas_object_resize(o, 256, 256);
 834             evas_object_image_smooth_scale_set(o, 0);
 835             evas_object_show(o);
 836             evas_object_layer_set(o, LAYER_LANDMARK);
 837             break;
 838         case 4:
 839         case 5:
 840             o = evas_object_rectangle_add(e3d);
 841             c->side[i].o = o;
 842             evas_object_color_set(o, 0, 34, 119, 255);
 843             evas_object_resize(o, 256, 256);
 844             evas_object_show(o);
 845             evas_object_layer_set(o, LAYER_LANDMARK);
 846             break;
 847         }
 848     }
 849
 850     /* First Plane (Front) */
 851     CUBE_POINT(0, 0, -w, -h, -d, 0, 0);
 852     CUBE_POINT(0, 1, w, -h, -d, 600, 0);
 853     CUBE_POINT(0, 2, w, h, -d, 600, 800);
 854     CUBE_POINT(0, 3, -w, h, -d, 0, 800);
 855
 856     /* Second Plane (Right side) */
 857     CUBE_POINT(1, 0, w, -h, -d, 0, 0);
 858     CUBE_POINT(1, 1, w, -h, d, 600, 0);
 859     CUBE_POINT(1, 2, w, h, d, 600, 800);
 860     CUBE_POINT(1, 3, w, h, -d, 0, 800);
 861
 862     /* Third Plane (Back) */
 863     CUBE_POINT(2, 0, w, -h, d, 0, 0);
 864     CUBE_POINT(2, 1, -w, -h, d, 600, 0);
 865     CUBE_POINT(2, 2, -w, h, d, 600, 800);
 866     CUBE_POINT(2, 3, w, h, d, 0, 800);
 867
 868     /* Fourth Plane (Left side) */
 869     CUBE_POINT(3, 0, -w, -h, d, 0, 0);
 870     CUBE_POINT(3, 1, -w, -h, -d, 600, 0);
 871     CUBE_POINT(3, 2, -w, h, -d, 600, 800);
 872     CUBE_POINT(3, 3, -w, h, d, 0, 800);
 873
 874     /* Fifth Plane (Top side) */
 875     CUBE_POINT(4, 0, -w, -h, d, 0, 0);
 876     CUBE_POINT(4, 1, w, -h, d, 256, 0);
 877     CUBE_POINT(4, 2, w, -h, -d, 256, 256);
 878     CUBE_POINT(4, 3, -w, -h, -d, 0, 256);
 879
 880     /* Sixth Plane (Under side) */
 881     CUBE_POINT(5, 0, -w, h, -d, 0, 0);
 882     CUBE_POINT(5, 1, w, h, -d, 256, 0);
 883     CUBE_POINT(5, 2, w, h, d, 256, 256);
 884     CUBE_POINT(5, 3, -w, h, d, 0, 256);
 885
 886     return c;
 887 }
 888
 889 /*--------------------------------------------------------------------------*/
 890 /*
 891  * @brief   _cube_draw
 892  *          cube drawing.
 893  *
 894  * @param[in]    c     cube address
 895  * @param[in]    x     x coordinates
 896  * param[in]    y     y coordinates
 897  * @param[in]    z     z coordinates
 898  * @param[in]    dx    amount of degrees from 0.0 to 360.0 to rotate around X axis.
 899  * @param[in]    dy    amount of degrees from 0.0 to 360.0 to rotate around Y axis.
 900  * @param[in]    dz    amount of degrees from 0.0 to 360.0 to rotate around Z axis.
 901  * @return       none
 902  */
 903 /*--------------------------------------------------------------------------*/
 904 static void
 905 _cube_draw(Cube *c, Evas_Coord x, Evas_Coord y, Evas_Coord z,
 906            double dx, double dy, double dz)
 907 {
 908     static Evas_Map *m = NULL;
 909     int i, j, order[6], sorted, drawable, tmp_z;
 910     Evas_Coord mz[6];
 911
 912     drawable = TRUE;
 913
 914     /* only 4 point */
 915     if (!m)
 916         m = evas_map_new(4);
 917     /* smooth rendering disable */
 918     evas_map_smooth_set(m, 0);
 919
 920     for (i = 0; i < 6; i++) {
 921         Evas_Coord tz[4];
 922
 923         for (j = 0; j < 4; j++) {
 924
 925             evas_map_point_coord_set(m, j,
 926                                      c->side[i].pt[j].x + ((W_WIDTH) / 4) +
 927                                      x - camera.x,
 928                                      c->side[i].pt[j].y +
 929                                      (W_NAVI_HEIGHT / 2) + y - camera.y,
 930                                      c->side[i].pt[j].z + z - camera.z +
 931                                      FRONT_SIDE_Z);
 932             if (!(i == 4 || i == 5)) {
 933                 evas_map_point_image_uv_set(m, j,
 934                                             c->side[i].pt[j].u,
 935                                             c->side[i].pt[j].v);
 936             }
 937             evas_map_point_color_set(m, j, 255, 255, 255, 255);
 938         }
 939         evas_map_util_3d_rotate(m, dx, dy, dz,
 940                                 ((W_WIDTH) / 4) + x - camera.x,
 941                                 (W_NAVI_HEIGHT / 2) + y - camera.y,
 942                                 z - camera.z + FRONT_SIDE_Z);
 943
 944         /* for camera angle */
 945         evas_map_util_3d_rotate(m, 0, -camera.angle, 0,
 946                                 ((W_WIDTH) / 4), (W_NAVI_HEIGHT / 2),
 947                                 FRONT_SIDE_Z);
 948
 949         evas_map_util_3d_perspective(m, ((W_WIDTH) / 4), (W_NAVI_HEIGHT / 2),
 950                                      Z0_POSITION, FOCUS_LENGTH / 2);
 951
 952 #ifdef _USE_Z_LIMIT_FIX_
 953         for (j = 0; j < 4; j++) {
 954             evas_map_point_coord_get(m, j, NULL, NULL, &tmp_z);
 955             if (tmp_z < (Z_LIMIT + FRONT_SIDE_Z)) {
 956                 drawable = FALSE;
 957             }
 958         }
 959 #endif
 960         if (evas_map_util_clockwise_get(m)) {
 961             evas_object_map_enable_set(c->side[i].o, 1);
 962             evas_object_map_set(c->side[i].o, m);
 963 #ifdef _USE_Z_LIMIT_FIX_
 964             if (drawable == TRUE) {
 965 #endif
 966                 evas_object_show(c->side[i].o);
 967 #ifdef _SHOW_FPS_
 968                 polygon_count++;
 969 #endif
 970 #ifdef _USE_Z_LIMIT_FIX_
 971             }
 972             else {
 973                 evas_object_hide(c->side[i].o);
 974             }
 975 #endif
 976         }
 977         else {
 978             evas_object_hide(c->side[i].o);
 979         }
 980
 981         order[i] = i;
 982
 983         for (j = 0; j < 4; j++) {
 984             evas_map_point_coord_get(m, j, NULL, NULL, &(tz[j]));
 985         }
 986
 987         mz[i] = (tz[0] + tz[1] + tz[2] + tz[3]) / 4;
 988
 989         if (mz[i] < -512) {
 990             evas_object_hide(c->side[i].o);
 991         }
 992     }
 993
 994     /* sort by z */
 995     sorted = 0;
 996     do {
 997         sorted = 1;
 998         for (i = 0; i < 5; i++) {
 999             if (mz[order[i]] > mz[order[i + 1]]) {
1000                 j = order[i];
1001                 order[i] = order[i + 1];
1002                 order[i + 1] = j;
1003                 sorted = 0;
1004             }
1005         }
1006     }
1007     while (!sorted);
1008
1009     evas_object_raise(c->side[order[0]].o);
1010
1011     for (i = 1; i < 6; i++) {
1012         evas_object_stack_below(c->side[order[i]].o, c->side[order[i - 1]].o);
1013     }
1014 }
1015
1016 /*--------------------------------------------------------------------------*/
1017 /*
1018  * @brief   _cube_free
1019  *          cube object release.
1020  *
1021  * @param[in]       c             cube address
1022  * @return          none
1023  */
1024 /*--------------------------------------------------------------------------*/
1025 static void _cube_free(Cube *c)
1026 {
1027     int i;
1028
1029     for (i = 0; i < 6; i++)
1030         evas_object_del(c->side[i].o);
1031     free(c);
1032 }
1033
1034 /*--------------------------------------------------------------------------*/
1035 /*
1036  * @brief   goal_object_new
1037  *          goal generation.
1038  *
1039  * @param[in]       e3d               3D view Evas object
1040  * @return          none
1041  */
1042 /*--------------------------------------------------------------------------*/
1043 static void goal_object_new(Evas *e3d)
1044 {
1045     goal_object = evas_object_image_add(e3d);
1046     evas_object_image_file_set(goal_object, goal_img_path, NULL);
1047     evas_object_image_fill_set(goal_object, 0, 0, 600, 600);
1048
1049     evas_object_resize(goal_object, 256, 256);
1050     evas_object_image_smooth_scale_set(goal_object, 0);
1051     evas_object_hide(goal_object);
1052 }
1053
1054 /*--------------------------------------------------------------------------*/
1055 /*
1056  * @brief   goal_object_draw
1057  *          goal drawing.
1058  *
1059  * @param[in]       plane             plane address
1060  * @return          none
1061  */
1062 /*--------------------------------------------------------------------------*/
1063 static void goal_object_draw(Plane *plane)
1064 {
1065     static Evas_Map *m3 = NULL;
1066
1067     if (plane->drawable == FALSE) {
1068         evas_object_hide(goal_object);
1069         return;
1070     }
1071
1072     if (!m3)
1073         m3 = evas_map_new(4);
1074     evas_map_smooth_set(m3, 0);
1075
1076     evas_map_point_coord_set(m3, 0,
1077                              (Evas_Coord) (plane->pt[0].x + ((W_WIDTH) / 4)),
1078                              (Evas_Coord) (plane->pt[0].y +
1079                                            ((W_NAVI_HEIGHT) / 2)),
1080                              (Evas_Coord) (plane->pt[0].z + FRONT_SIDE_Z));
1081     evas_map_point_coord_set(m3, 1,
1082                              (Evas_Coord) (plane->pt[0].x + ((W_WIDTH) / 4)),
1083                              (Evas_Coord) ((-1) * plane->pt[0].y +
1084                                            ((W_NAVI_HEIGHT) / 2)),
1085                              (Evas_Coord) (plane->pt[0].z + FRONT_SIDE_Z));
1086     evas_map_point_coord_set(m3, 2,
1087                              (Evas_Coord) (plane->pt[3].x + ((W_WIDTH) / 4)),
1088                              (Evas_Coord) ((-1) * plane->pt[3].y +
1089                                            ((W_NAVI_HEIGHT) / 2)),
1090                              (Evas_Coord) (plane->pt[3].z + FRONT_SIDE_Z));
1091     evas_map_point_coord_set(m3, 3,
1092                              (Evas_Coord) (plane->pt[3].x + ((W_WIDTH) / 4)),
1093                              (Evas_Coord) (plane->pt[3].y +
1094                                            ((W_NAVI_HEIGHT) / 2)),
1095                              (Evas_Coord) (plane->pt[3].z + FRONT_SIDE_Z));
1096
1097     evas_map_point_image_uv_set(m3, 0, 600, 0);
1098     evas_map_point_image_uv_set(m3, 1, 600, 600);
1099     evas_map_point_image_uv_set(m3, 2, 0, 600);
1100     evas_map_point_image_uv_set(m3, 3, 0, 0);
1101
1102     evas_map_util_3d_perspective(m3, (W_WIDTH / 4), (W_NAVI_HEIGHT / 2) - 100,
1103                                  Z0_POSITION, FOCUS_LENGTH / 2);
1104
1105     evas_object_map_enable_set(goal_object, 1);
1106     evas_object_map_set(goal_object, m3);
1107     evas_object_show(goal_object);
1108
1109     evas_object_raise(goal_object);
1110 }
1111
1112 /*--------------------------------------------------------------------------*/
1113 /*
1114  * @brief   goal_object_free
1115  *          goal object release.
1116  *
1117  * @param           none
1118  * @return          none
1119  */
1120 /*--------------------------------------------------------------------------*/
1121 static void goal_object_free()
1122 {
1123     evas_object_del(goal_object);
1124 }
1125
1126 /******************************************************************************
1127  * Functions for Calculate Route Points
1128  ******************************************************************************/
1129
1130 #ifdef _USE_Z_LIMIT_FIX_
1131 /*--------------------------------------------------------------------------*/
1132 /*
1133  * @brief   check_route_drawable_by_z_limit_fix
1134  *          route check drawable by z limit fix.
1135  *
1136  * @param           none
1137  * @return          none
1138  */
1139 /*--------------------------------------------------------------------------*/
1140 static void check_route_drawable_by_z_limit_fix()
1141 {
1142     int i;
1143     Point limit_p0, limit_p1;
1144
1145     POINT(limit_p0, -100, ROUTE_PLANE_HEIGHT, Z_LIMIT, 0, 0);
1146     POINT(limit_p1, 100, ROUTE_PLANE_HEIGHT, Z_LIMIT, 0, 0);
1147
1148     for (i = 0; i < route_draw_num - 1; i++) {
1149         route[i].status = NONE;
1150         route[i].enable_fix = TRUE;
1151
1152         if (route_pre_fix[i].z < Z_LIMIT) {
1153             route[i].status = STARTING_POINT;
1154             route[i].enable_fix = FALSE;
1155             if (route_pre_fix[i + 1].z >= Z_LIMIT) {
1156                 calc_intersection(limit_p0, limit_p1, route_pre_fix[i],
1157                                   route_pre_fix[i + 1], &route[i]);
1158             }
1159             else {
1160                 if (!(i > 0 && (route[i - 1].status == ENDING_POINT))) {
1161                     route[i].status = BOTH_POINT;
1162                     route[i].x = route_pre_fix[i].x;
1163                     route[i].z = route_pre_fix[i].z;
1164                 }
1165             }
1166         }
1167         else {
1168             /* don't fix */
1169             route[i].x = route_pre_fix[i].x;
1170             route[i].z = route_pre_fix[i].z;
1171
1172             if (route_pre_fix[i].z < 0) {
1173                 if (i > 0 && (fixed_plane[i - 1]->drawable == FALSE)) {
1174                     route[i].enable_fix = FALSE;
1175                 }
1176             }
1177         }
1178
1179         if (route_pre_fix[i + 1].z < Z_LIMIT) {
1180             if ((route[i].status == STARTING_POINT)
1181                 || (route[i].status == BOTH_POINT)) {
1182                 route[i].status = BOTH_POINT;
1183
1184                 if ((i + 1) == route_draw_num - 1) {
1185                     route[i + 1].x = route_pre_fix[i + 1].x;
1186                     route[i + 1].z = route_pre_fix[i + 1].z;
1187                 }
1188                 else {
1189                     if (route_pre_fix[i + 2].z >= Z_LIMIT) {
1190                         calc_intersection(limit_p0, limit_p1,
1191                                           route_pre_fix[i + 1],
1192                                           route_pre_fix[i + 2],
1193                                           &route[i + 1]);
1194
1195                     }
1196                     else {
1197                         route[i + 1].x = route_pre_fix[i + 1].x;
1198                         route[i + 1].z = route_pre_fix[i + 1].z;
1199                     }
1200                 }
1201
1202                 if ((i < route_draw_num - 1)
1203                     && (route_pre_fix[i + 2].z > Z_LIMIT)) {
1204                     calc_intersection(limit_p0, limit_p1,
1205                                       route_pre_fix[i + 1],
1206                                       route_pre_fix[i + 2], &route[i + 1]);
1207                 }
1208                 else {
1209                     route[i + 1].x = route_pre_fix[i + 1].x;
1210                     route[i + 1].z = route_pre_fix[i + 1].z;
1211                 }
1212             }
1213             else {
1214                 if (route_pre_fix[i].z >= Z_LIMIT) {
1215                     route[i].status = ENDING_POINT;
1216                     calc_intersection(limit_p0, limit_p1, route_pre_fix[i],
1217                                       route_pre_fix[i + 1], &route[i + 1]);
1218                 }
1219                 else {
1220                     route[i].status = BOTH_POINT;
1221                 }
1222             }
1223         }
1224         else {
1225             route[i + 1].x = route_pre_fix[i + 1].x;
1226             route[i + 1].z = route_pre_fix[i + 1].z;
1227         }
1228
1229         if (route[i].status == BOTH_POINT) {
1230             fixed_plane[i]->drawable = FALSE;
1231         }
1232         else {
1233             fixed_plane[i]->drawable = TRUE;
1234         }
1235
1236     }
1237 }
1238 #endif
1239
1240 /*--------------------------------------------------------------------------*/
1241 /*
1242  * @brief   calc_route_coord_by_camera
1243  *          The coordinates of a route are calculated with a camera.
1244  *
1245  * @param           none
1246  * @return          none
1247  */
1248 /*--------------------------------------------------------------------------*/
1249 static void calc_route_coord_by_camera()
1250 {
1251     int i;
1252     Point orizin;
1253
1254     for (i = 0; i < route_draw_num; i++) {
1255 #ifdef _USE_Z_LIMIT_FIX_
1256         route_pre_fix[i].x = route_raw[i].x - camera.x;
1257         route_pre_fix[i].z = route_raw[i].z - camera.z;
1258         rotate_xz(route_pre_fix[i], &route_pre_fix[i], camera.angle);
1259 #else
1260         route[i].x = route_raw[i].x - camera.x;
1261         route[i].z = route_raw[i].z - camera.z;
1262         rotate_xz(route[i], &route[i], camera.angle);
1263 #endif
1264     }
1265
1266     orizin.x = 0;
1267     orizin.z = 0;
1268 #ifdef _USE_Z_LIMIT_FIX_
1269     goal_square_length =
1270         calc_square_length(orizin, route_pre_fix[route_draw_num - 1]);
1271 #else
1272     goal_square_length =
1273         calc_square_length(orizin, route[route_draw_num - 1]);
1274 #endif
1275
1276     for (i = 0; i < route_draw_num - 1; i++) {
1277         fixed_plane[i]->drawable = TRUE;
1278     }
1279
1280 #ifdef _USE_Z_LIMIT_FIX_
1281     check_route_drawable_by_z_limit_fix();
1282 #endif
1283
1284     calc_route_distance();
1285     calc_route_distance_to_camera();
1286     check_route_drawable_by_distance_to_camera();
1287 }
1288
1289 /*--------------------------------------------------------------------------*/
1290 /*
1291  * @brief   calc_route_distance_to_camera
1292  *          The route distance to a camera is calculated.
1293  *
1294  * @param           none
1295  * @return          none
1296  */
1297 /*--------------------------------------------------------------------------*/
1298 static void calc_route_distance_to_camera()
1299 {
1300     int i;
1301     Point origin;
1302
1303     origin.x = 0;
1304     origin.z = 0;
1305
1306     for (i = 0; i < route_draw_num; i++) {
1307         route_square_length_camera[i] = calc_square_length(route[i], origin);
1308     }
1309 }
1310
1311 /*--------------------------------------------------------------------------*/
1312 /*
1313  * @brief   check_route_drawable_by_distance_to_camera
1314  *          route check drawable by distance to camera.
1315  *
1316  * @param           none
1317  * @return          none
1318  */
1319 /*--------------------------------------------------------------------------*/
1320 static void check_route_drawable_by_distance_to_camera()
1321 {
1322     int i, culling_route_count;
1323     double culling_length;
1324
1325     culling_route_count = 0;
1326     culling_length = CULLING_LENGTH_ROUTE * CULLING_LENGTH_ROUTE;
1327
1328     for (i = 0; i < route_draw_num - 1; i++) {
1329         if ((route_square_length_camera[i] > culling_length) &&
1330             (route_square_length_camera[i + 1] > culling_length)) {
1331             fixed_plane[i]->drawable = FALSE;
1332             culling_route_count++;
1333         }
1334     }
1335 }
1336
1337 /*--------------------------------------------------------------------------*/
1338 /*
1339  * @brief   calc_fixed_box_points_all
1340  *          All fixed box points are calculated.
1341  *
1342  * @param           none
1343  * @return          none
1344  */
1345 /*--------------------------------------------------------------------------*/
1346 static void calc_fixed_box_points_all()
1347 {
1348     int i;
1349     for (i = 0; i < route_draw_num - 1; i++) {
1350         calc_fixed_box_points(i);
1351     }
1352 }
1353
1354 /*--------------------------------------------------------------------------*/
1355 /*
1356  * @brief   calc_fixed_box_points
1357  *          fixed box points are calculated.
1358  *
1359  * @param[in]       routenum     route number
1360  * @return          none
1361  */
1362 /*--------------------------------------------------------------------------*/
1363 static void calc_fixed_box_points(int routenum)
1364 {
1365 #ifdef _USE_Z_LIMIT_FIX_
1366     if (routenum == 0 || route[routenum].enable_fix == FALSE) {
1367 #else
1368     if (routenum == 0) {
1369 #endif
1370         fixed_plane[routenum]->pt[1] = base_plane[routenum]->pt[1];
1371         fixed_plane[routenum]->pt[2] = base_plane[routenum]->pt[2];
1372     }
1373     else {
1374         fixed_plane[routenum]->pt[1] = calc_fix_point(routenum, 1);
1375         fixed_plane[routenum]->pt[2] = calc_fix_point(routenum, 2);
1376     }
1377
1378 #ifdef _USE_Z_LIMIT_FIX_
1379     if (routenum == route_draw_num - 2
1380         || (route[routenum].status == ENDING_POINT
1381             || route[routenum].status == BOTH_POINT)) {
1382 #else
1383     if (routenum == route_draw_num - 2) {
1384 #endif
1385         fixed_plane[routenum]->pt[0] = base_plane[routenum]->pt[0];
1386         fixed_plane[routenum]->pt[3] = base_plane[routenum]->pt[3];
1387     }
1388     else {
1389         fixed_plane[routenum]->pt[0] = calc_fix_point(routenum, 0);
1390         fixed_plane[routenum]->pt[3] = calc_fix_point(routenum, 3);
1391     }
1392 }
1393
1394 /*--------------------------------------------------------------------------*/
1395 /*
1396  * @brief   calc_fix_point
1397  *          fixed point is calculated.
1398  *
1399  * @param[in]       routenum     route number
1400  * @param[in]       point_index  point index
1401  * @return          point
1402 */
1403 /*--------------------------------------------------------------------------*/
1404 static Point calc_fix_point(int routenum, int point_index)
1405 {
1406     double m;
1407     Point result, p0, p1;
1408
1409     if (route_distance[routenum] > (BOX_FIXED_LENGTH * 2)) {
1410         m = (route_distance[routenum] -
1411              BOX_FIXED_LENGTH) / route_distance[routenum];
1412     }
1413     else {
1414         m = 0.5;
1415     }
1416
1417     POINT(result, 0, ROUTE_PLANE_HEIGHT, 0, 0, 0);
1418     POINT(p0, 0, ROUTE_PLANE_HEIGHT, 0, 0, 0);
1419     POINT(p1, base_plane[routenum]->pt[point_index].x, ROUTE_PLANE_HEIGHT,
1420           base_plane[routenum]->pt[point_index].z, 0, 0);
1421
1422     if (point_index == 0) {
1423         p0.x = base_plane[routenum]->pt[1].x;
1424         p0.z = base_plane[routenum]->pt[1].z;
1425     }
1426     else if (point_index == 1) {
1427         p0.x = base_plane[routenum]->pt[0].x;
1428         p0.z = base_plane[routenum]->pt[0].z;
1429     }
1430     else if (point_index == 2) {
1431         p0.x = base_plane[routenum]->pt[3].x;
1432         p0.z = base_plane[routenum]->pt[3].z;
1433     }
1434     else {
1435         p0.x = base_plane[routenum]->pt[2].x;
1436         p0.z = base_plane[routenum]->pt[2].z;
1437     }
1438
1439     result.x = (m * (p1.x - p0.x)) + p0.x;
1440     result.z = (m * (p1.z - p0.z)) + p0.z;
1441
1442     return result;
1443 }
1444
1445 /*--------------------------------------------------------------------------*/
1446 /*
1447  * @brief   draw_curve
1448  *          curve is drawn.
1449  *
1450  * @param[in]       routenum     route number
1451  * @return          none
1452 */
1453 /*--------------------------------------------------------------------------*/
1454 static void draw_curve(int routenum)
1455 {
1456     int i;
1457
1458     Point con_point1;
1459     Point con_point2;
1460     Point curve_point1[ROUTE_DIVISION_NUMBER + 1];
1461     Point curve_point2[ROUTE_DIVISION_NUMBER + 1];
1462
1463     calc_intersection(base_plane[routenum]->pt[0],
1464                       base_plane[routenum]->pt[1],
1465                       base_plane[routenum + 1]->pt[0],
1466                       base_plane[routenum + 1]->pt[1], &con_point1);
1467     calc_intersection(base_plane[routenum]->pt[3],
1468                       base_plane[routenum]->pt[2],
1469                       base_plane[routenum + 1]->pt[3],
1470                       base_plane[routenum + 1]->pt[2], &con_point2);
1471
1472 #ifdef _E3D_H_OUTPUT_PRINT_
1473     uim_debug("con_point1 x = %f, z = %f \n", con_point1.x, con_point1.z);
1474     uim_debug("con_point2 x = %f, z = %f \n", con_point2.x, con_point2.z);
1475 #endif
1476
1477     calc_bezier_points(fixed_plane[routenum]->pt[0], con_point1,
1478                        fixed_plane[routenum + 1]->pt[1], curve_point1);
1479     calc_bezier_points(fixed_plane[routenum]->pt[3], con_point2,
1480                        fixed_plane[routenum + 1]->pt[2], curve_point2);
1481
1482     for (i = 0; i < (ROUTE_DIVISION_NUMBER) + 1; i++) {
1483         if (curve_point1[i].z < Z_LIMIT) {
1484             curve_point1[i].z = Z_LIMIT;
1485         }
1486
1487         if (curve_point2[i].z < Z_LIMIT) {
1488             curve_point2[i].z = Z_LIMIT;
1489         }
1490     }
1491
1492     for (i = 0; i < ROUTE_DIVISION_NUMBER; i++) {
1493         PLANE_POINT(division_plane[routenum][i], 0, curve_point1[i + 1].x,
1494                     ROUTE_PLANE_HEIGHT, curve_point1[i + 1].z, 0, 0);
1495         PLANE_POINT(division_plane[routenum][i], 1, curve_point1[i].x,
1496                     ROUTE_PLANE_HEIGHT, curve_point1[i].z, 256, 0);
1497         PLANE_POINT(division_plane[routenum][i], 2, curve_point2[i].x,
1498                     ROUTE_PLANE_HEIGHT, curve_point2[i].z, 256, 256);
1499         PLANE_POINT(division_plane[routenum][i], 3, curve_point2[i + 1].x,
1500                     ROUTE_PLANE_HEIGHT, curve_point2[i + 1].z, 0, 256);
1501
1502         _plane_draw(division_plane[routenum][i]);
1503
1504 #ifdef _E3D_H_OUTPUT_PRINT_
1505         uim_debug
1506             ("Division Plane %d : x0 = %f, z0 = %f, x1 = %f, z1 = %f, x2 = %f, z2 = %f, x3 = %f, z3 = %f\n",
1507              routenum, division_plane[routenum][i]->pt[0].x,
1508              division_plane[routenum][i]->pt[0].z,
1509              division_plane[routenum][i]->pt[1].x,
1510              division_plane[routenum][i]->pt[1].z,
1511              division_plane[routenum][i]->pt[2].x,
1512              division_plane[routenum][i]->pt[2].z,
1513              division_plane[routenum][i]->pt[3].x,
1514              division_plane[routenum][i]->pt[3].z);
1515         uim_debug("Draw division plane.\n");
1516 #endif
1517
1518     }
1519 }
1520
1521 /*--------------------------------------------------------------------------*/
1522 /*
1523  * @brief   calc_bezier_points
1524  *          bezier point is calculated.
1525  *
1526  * @param[in]        p0        point 0
1527  * @param[in]        p1        point 1
1528  * @param[in]        p2        point 2
1529  * @param[out]       points    point address
1530  * @return           none
1531 */
1532 /*--------------------------------------------------------------------------*/
1533 static void calc_bezier_points(Point p0, Point p1, Point p2, Point *points)
1534 {
1535     int i;
1536     double a, b, x, z;
1537     Point p;
1538
1539     for (i = 0; i < ROUTE_DIVISION_NUMBER + 1; i++) {
1540         b = (double) i / ROUTE_DIVISION_NUMBER;
1541         a = 1 - b;
1542
1543         x = a * a * p0.x + 2 * a * b * p1.x + b * b * p2.x;
1544         z = a * a * p0.z + 2 * a * b * p1.z + b * b * p2.z;
1545
1546         POINT(p, x, ROUTE_PLANE_HEIGHT, z, 0, 0);
1547         points[i] = p;
1548
1549 #ifdef _E3D_H_OUTPUT_PRINT_
1550         uim_debug("bezier_points %d : x = %f, y = %f, z = %f\n", i,
1551                   points[i].x, points[i].y, points[i].z);
1552 #endif
1553     }
1554 }
1555
1556 /*--------------------------------------------------------------------------*/
1557 /*
1558  * @brief   calc_base_box_points_all
1559  *          All base box points are calculated.
1560  *
1561  * @param        none
1562  * @return       none
1563 */
1564 /*--------------------------------------------------------------------------*/
1565 static void calc_base_box_points_all()
1566 {
1567     int i;
1568     for (i = 0; i < route_draw_num - 1; i++) {
1569         calc_base_box_points(i);
1570     }
1571 }
1572
1573 /*--------------------------------------------------------------------------*/
1574 /*
1575  * @brief   calc_base_box_points
1576  *          base box points are calculated.
1577  *
1578  * @param[in]       routenum     route number
1579  * @return          none
1580  */
1581 /*--------------------------------------------------------------------------*/
1582 static void calc_base_box_points(int routenum)
1583 {
1584     /* Determine the angle. */
1585     double th = atan2((route[routenum + 1].z - route[routenum].z),
1586                       (route[routenum + 1].x - route[routenum].x));
1587     /* Determine the distance. */
1588     double dis = route_distance[routenum];
1589
1590 #ifdef _E3D_H_OUTPUT_PRINT_
1591     uim_debug("th = %f\n", (th * 180 / M_PI));
1592 #endif
1593
1594     PLANE_POINT(base_plane[routenum], 0,
1595                 (cos(th) * dis - sin(th) * (-ROUTE_PLANE_WIDTH)) +
1596                 route[routenum].x, ROUTE_PLANE_HEIGHT,
1597                 (sin(th) * dis + cos(th) * (-ROUTE_PLANE_WIDTH)) +
1598                 route[routenum].z, 0, 0);
1599
1600     PLANE_POINT(base_plane[routenum], 1,
1601                 (cos(th) * 0 - sin(th) * (-ROUTE_PLANE_WIDTH)) +
1602                 route[routenum].x, ROUTE_PLANE_HEIGHT,
1603                 (sin(th) * 0 + cos(th) * (-ROUTE_PLANE_WIDTH)) +
1604                 route[routenum].z, 256, 0);
1605
1606     PLANE_POINT(base_plane[routenum], 2,
1607                 (cos(th) * 0 - sin(th) * (ROUTE_PLANE_WIDTH)) +
1608                 route[routenum].x, ROUTE_PLANE_HEIGHT,
1609                 (sin(th) * 0 + cos(th) * (ROUTE_PLANE_WIDTH)) +
1610                 route[routenum].z, 256, 256);
1611
1612     PLANE_POINT(base_plane[routenum], 3,
1613                 (cos(th) * dis - sin(th) * (ROUTE_PLANE_WIDTH)) +
1614                 route[routenum].x, ROUTE_PLANE_HEIGHT,
1615                 (sin(th) * dis + cos(th) * (ROUTE_PLANE_WIDTH)) +
1616                 route[routenum].z, 0, 256);
1617
1618 #ifdef _E3D_H_OUTPUT_PRINT_
1619     uim_debug
1620         ("Plane %d : x0 = %f, z0 = %f, x1 = %f, z1 = %f, x2 = %f, z2 = %f, x3 = %f, z3 = %f\n",
1621          routenum, base_plane[routenum]->pt[0].x,
1622          base_plane[routenum]->pt[0].z, base_plane[routenum]->pt[1].x,
1623          base_plane[routenum]->pt[1].z, base_plane[routenum]->pt[2].x,
1624          base_plane[routenum]->pt[2].z, base_plane[routenum]->pt[3].x,
1625          base_plane[routenum]->pt[3].z);
1626 #endif
1627 }
1628
1629 /******************************************************************************
1630  * Functions for Calculate Landmark Points
1631  ******************************************************************************/
1632 /*--------------------------------------------------------------------------*/
1633 /*
1634  * @brief   calc_landmarks_coord_by_camera
1635  *          The coordinates of landmarks are calculated with a camera.
1636  *
1637  * @param           none
1638  * @return          none
1639  */
1640 /*--------------------------------------------------------------------------*/
1641 static void calc_landmarks_coord_by_camera()
1642 {
1643     int i;
1644
1645     for (i = 0; i < landmark_data_count; i++) {
1646         landmark[i].x = landmark_raw[i].x;
1647         landmark[i].y = landmark_raw[i].y;
1648         landmark[i].z = landmark_raw[i].z;
1649     }
1650 }
1651
1652 /******************************************************************************
1653  * Functions for Calculate (Common)
1654  ******************************************************************************/
1655 /*--------------------------------------------------------------------------*/
1656 /*
1657  * @brief   calc_route_distance
1658  *          The route distance is calculated.
1659  *
1660  * @param           none
1661  * @return          none
1662  */
1663 /*--------------------------------------------------------------------------*/
1664 static void calc_route_distance()
1665 {
1666     int i;
1667     for (i = 0; i < route_draw_num - 1; i++) {
1668         route_distance[i] = calc_length(route[i], route[i + 1]);
1669     }
1670 }
1671
1672 /** Calculate the distance between two points.(only x-z plane) */
1673 /*--------------------------------------------------------------------------*/
1674 /*
1675  * @brief   calc_length
1676  *          The distance between two points is calculated.
1677  *
1678  * @param[in]        p0        point 0
1679  * @param[in]        p1        point 1
1680  * @return           distance between two points
1681 */
1682 /*--------------------------------------------------------------------------*/
1683 static double calc_length(Point p0, Point p1)
1684 {
1685     return sqrt((p1.x - p0.x) * (p1.x - p0.x) +
1686                 (p1.z - p0.z) * (p1.z - p0.z));
1687 }
1688
1689 /** Calculate the square of distance between two points.(only x-z plane) */
1690 /*--------------------------------------------------------------------------*/
1691 /*
1692  * @brief   calc_square_length
1693  *          The square of the distance between two points is calculated.
1694  *
1695  * @param[in]        p0        point 0
1696  * @param[in]        p1        point 1
1697  * @return           square of distance between two points
1698 */
1699 /*--------------------------------------------------------------------------*/
1700 static double calc_square_length(Point p0, Point p1)
1701 {
1702     return ((p1.x - p0.x) * (p1.x - p0.x) + (p1.z - p0.z) * (p1.z - p0.z));
1703 }
1704
1705 /* rotate point (center is orizin, xz-plane) */
1706 /*--------------------------------------------------------------------------*/
1707 /*
1708  * @brief   rotate_xz
1709  *          rotate point is calculated.
1710  *
1711  * @param[in]        str       point
1712  * @param[out]       dist      point address
1713  * @param[in]        angle     angle
1714  * @return           none
1715 */
1716 /*--------------------------------------------------------------------------*/
1717 static void rotate_xz(Point src, Point *dist, double angle)
1718 {
1719     double rx, rz, sine, cosine;
1720     double rad;
1721
1722     rad = (-angle) * M_PI / 180.0;
1723
1724     sine = sin(rad);
1725     cosine = cos(rad);
1726
1727     rx = cosine * src.x - sine * src.z;
1728     rz = sine * src.x + cosine * src.z;
1729
1730     dist->x = rx;
1731     dist->y = src.y;
1732     dist->z = rz;
1733     dist->u = src.u;
1734     dist->v = src.v;
1735 }
1736
1737 /*--------------------------------------------------------------------------*/
1738 /*
1739  * @brief   calc_intersection
1740  *          intersection is calculated.
1741  *
1742  * @param[in]        p0        point 0
1743  * @param[in]        p1        point 1
1744  * @param[in]        p2        point 2
1745  * @param[in]        p3        point 3
1746  * @param[out]       point     point address
1747  * @return           none
1748 */
1749 /*--------------------------------------------------------------------------*/
1750 static void
1751 calc_intersection(Point p0, Point p1, Point p2, Point p3, Point *point)
1752 {
1753     double S1, S2, result_x, result_z;
1754     S1 = ((p2.x - p3.x) * (p1.z - p3.z) - (p2.z - p3.z) * (p1.x - p3.x)) / 2;
1755     S2 = ((p2.x - p3.x) * (p3.z - p0.z) - (p2.z - p3.z) * (p3.x - p0.x)) / 2;
1756
1757     result_x = (p1.x + (p0.x - p1.x) * S1 / (S1 + S2));
1758     result_z = (p1.z + (p0.z - p1.z) * S1 / (S1 + S2));
1759
1760     point->x = result_x;
1761     point->y = ROUTE_PLANE_HEIGHT;
1762     point->z = result_z;
1763     point->u = 0;
1764     point->v = 0;
1765 }
1766
1767 /*--------------------------------------------------------------------------*/
1768 /*
1769  * @brief   length_comp
1770  *          Comparison of length of point.
1771  *
1772  * @param[in]    pl0        point length 0 address
1773  * @param[in]    pl1        point length 1 address
1774  * @return       result
1775  * @retval       =0         length coincidence
1776  * @retval       =1         point length 0 < point length 1
1777  * @retval       =-1        point length 0 > point length 1
1778 */
1779 /*--------------------------------------------------------------------------*/
1780 static int length_comp(const void *pl0, const void *pl1)
1781 {
1782     PointLength point_length0 = *(PointLength *) pl0;
1783     PointLength point_length1 = *(PointLength *) pl1;
1784
1785     if (point_length0.length == point_length1.length) {
1786         return 0;
1787     }
1788     else if (point_length0.length < point_length1.length) {
1789         return 1;
1790     }
1791     return -1;
1792 }