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     int index;
  35     double length;
  36     int drawable;
  37 } PointLength;
  38
  39 /*============================================================================*/
  40 /* Function prototype for static(internal) functions                          */
  41 /*============================================================================*/
  42 static void load_route_from_websocket(char *p_in, size_t len);
  43
  44 /* Local Functions for Route. */
  45 static void calc_route_coord_from_lonlat();
  46
  47 /* Local Functions for Landmark. */
  48 static void load_landmarks_from_csv();
  49 static void calc_landmarks_coord_from_lonlat();
  50
  51 void init_e3d(Evas *e3d, void *p_in, size_t len);
  52
  53 /* Landmark Points and Cubes */
  54 static const char *landmark_img_path(int i);
  55
  56 void e3d_cleanup();
  57 void calc_route_distance_to_camera();
  58 void check_route_drawable_by_distance_to_camera();
  59 double calc_square_length(Point p0, Point p1);
  60 int length_comp(const void *pl0, const void *pl1);
  61
  62 /* Local Functions for Drawing on Evas */
  63 Plane *_plane_new(Evas *e3d, Evas_Coord w, Evas_Coord d, Evas_Coord h);
  64 void _plane_draw(Plane *plane);
  65 void _plane_free(Plane *plane);
  66 Cube *_cube_new(Evas *e3d, Evas_Coord w, Evas_Coord h, Evas_Coord d, const char *img_path);
  67 void _cube_draw(Cube *c, Evas_Coord x, Evas_Coord y, Evas_Coord z, double dx, double dy, double dz);
  68 void _cube_free(Cube *c);
  69 void goal_object_new(Evas *e3d);
  70 void goal_object_draw();
  71 void goal_object_free();
  72
  73 /* Local Functions for Route. */
  74 void calc_route_coord_by_camera();
  75 void check_route_darawable_by_z_limit_fix();
  76 void draw_route(Evas *e3d);
  77 double calc_length(Point p0, Point p1);
  78 void calc_route_distance();
  79 void rotate_xz(Point src, Point *dist, double angle);
  80 void calc_base_box_points(int routenum);
  81 void calc_base_box_points_all();
  82 void calc_intersection(Point p0, Point p1, Point p2, Point p3, Point *point);
  83 void calc_bezier_points(Point p0, Point p1, Point p2, Point *points);
  84 void draw_curve(int routenum);
  85 Point calc_fix_point(int routenum, int point_index);
  86 void calc_fixed_box_points(int routenum);
  87 void calc_fixed_box_points_all();
  88
  89 /* Local Functions for Landmark. */
  90 void calc_landmarks_coord_by_camera();
  91 void draw_landmark(Evas *e3d);
  92
  93 /*============================================================================*/
  94 /* Tables and Valiables                                                       */
  95 /*============================================================================*/
  96 Camera camera;
  97 CameraGeocode camera_geocode;
  98 MapPos map_pos;
  99 char set_route;
 100 char enable_navi;
 101 #ifdef _SHOW_FPS_
 102 int polygon_count;
 103 #endif
 104
 105 /* Route Points and Planes*/
 106 int route_data_count;
 107 CsvRoute csv_route[MAX_ROUTE_POINTS];
 108 Point route_raw[MAX_DRAW_ROUTE_POINTS];
 109 Point route[MAX_DRAW_ROUTE_POINTS];
 110 double route_square_length_camera[MAX_DRAW_ROUTE_POINTS];
 111 #ifdef _USE_Z_LIMIT_FIX_
 112 Point route_pre_fix[MAX_DRAW_ROUTE_POINTS];
 113 #endif
 114 Plane *base_plane[MAX_DRAW_ROUTE_POINTS - 1];
 115 Plane *fixed_plane[MAX_DRAW_ROUTE_POINTS - 1];
 116 Plane *division_plane[MAX_DRAW_ROUTE_POINTS - 2][ROUTE_DIVISION_NUMBER];
 117 int route_draw_num;
 118 double route_distance[MAX_DRAW_ROUTE_POINTS - 1];
 119 Evas_Object *goal_object;
 120
 121 /* Landmark Points and Cubes */
 122 int landmark_data_count;
 123 CsvLandmark csv_landmark[MAX_LANDMARK_POINTS];
 124 Point landmark_raw[MAX_LANDMARK_POINTS];
 125 Point landmark[MAX_LANDMARK_POINTS];
 126 Cube *landmark_cube[MAX_LANDMARK_POINTS];
 127
 128 /*============================================================================*/
 129 /* functions                                                                  */
 130 /*============================================================================*/
 131 /*--------------------------------------------------------------------------*/
 132 /*
 133  * @brief   load_route_from_websocket
 134  *          route information acquisition.
 135  *
 136  * @param[in]   p_in              receive message
 137  * @param[in]   len               message size[BYTE]
 138  * @return      none
 139  */
 140 /*--------------------------------------------------------------------------*/
 141 static void
 142 load_route_from_websocket(char *p_in, size_t len)
 143 {
 144     /* Local variable */
 145     size_t  index = 0;
 146     char    str[256];
 147     route_data_count = 0;
 148
 149     /* Get string */
 150     while( index < len ) {
 151         /* Get start tag */
 152         if( ( strlen( &p_in[index] ) >= strlen( ROUTE_START_TAG ) ) &&
 153             ( strncmp( &p_in[index], ROUTE_START_TAG, strlen( ROUTE_START_TAG ) ) == 0 ) )
 154         {
 155             index += strlen( ROUTE_START_TAG );
 156         }
 157         /* Get end tag */
 158         else if( ( strlen( &p_in[index] ) >= strlen( ROUTE_END_TAG ) ) &&
 159                  ( strncmp( &p_in[index], ROUTE_START_TAG, strlen( ROUTE_END_TAG ) ) == 0 ) )
 160         {
 161             break;
 162         }
 163         /* Get br tag */
 164         else if( ( strlen( &p_in[index] ) >= strlen( BR_TAG ) ) &&
 165                  ( strncmp( &p_in[index], BR_TAG, strlen( BR_TAG ) ) == 0 ) )
 166         {
 167             index += strlen( BR_TAG );
 168         }
 169         /* Get parameter */
 170         else {
 171             /* Get lng */
 172             memset( str, 0, sizeof( str ) );
 173             while( ( p_in[index] != ',' ) && ( index < len ) ) {
 174                 str[strlen( str )] = p_in[index];
 175                 index++;
 176             }
 177             csv_route[route_data_count].lon = atof( str );
 178             index++;
 179
 180             /* Get lat */
 181             memset( str, 0, sizeof( str ) );
 182             while( ( p_in[index] != '<' ) && ( index < len ) ) {
 183                 str[strlen( str )] = p_in[index];
 184                 index++;
 185             }
 186             csv_route[route_data_count].lat = atof( str );
 187             route_data_count++;
 188         }
 189     }
 190     if(route_data_count > 0){
 191         route_data_count--;
 192     }
 193 }
 194
 195 /*--------------------------------------------------------------------------*/
 196 /*
 197  * @brief   calc_route_coord_from_lonlat
 198  *          The coordinates of a route are calculated from
 199  *          latitude and longitude.
 200  *
 201  * @param     none
 202  * @return    none
 203  */
 204 /*--------------------------------------------------------------------------*/
 205 static void
 206 calc_route_coord_from_lonlat()
 207 {
 208     int i;
 209
 210     for (i = 0; i < route_data_count; i++) {
 211         double x = (csv_route[i].lon - csv_route[0].lon) * LON_CONVERT * COORD_CONVERT_COEFFICIENT;
 212         double z = (csv_route[i].lat - csv_route[0].lat) * LAT_CONVERT * COORD_CONVERT_COEFFICIENT;
 213         POINT(route_raw[i], x, ROUTE_PLANE_HEIGHT, z, 0, 0);
 214     }
 215 }
 216
 217 /*--------------------------------------------------------------------------*/
 218 /*
 219  * @brief   load_landmarks_from_csv
 220  *          The information on a landmark is loaded from a CSV file.
 221  *
 222  * @param     none
 223  * @return    none
 224  */
 225 /*--------------------------------------------------------------------------*/
 226 static void
 227 load_landmarks_from_csv()
 228 {
 229     FILE *fp;
 230     int i;
 231
 232     if ((fp = fopen(csv_landmark_path, "r")) == NULL) {
 233         fprintf(stderr, "%s\n", "Error : can't open file.(landmark.csv)\n");
 234         landmark_data_count = 0;
 235         return;
 236     }
 237
 238     i = 0;
 239     while(fscanf(fp, "%lf,%lf,%hhd",
 240                 &csv_landmark[i].lon, &csv_landmark[i].lat, &csv_landmark[i].id) != EOF) {
 241         i++;
 242     }
 243     landmark_data_count = i;
 244
 245     fclose(fp);
 246
 247     // for test
 248     for (i = 0; i < landmark_data_count; i++) {
 249         uim_debug("landmark data %d : %.4f, %.4f, %d\n",
 250                 i, csv_landmark[i].lon, csv_landmark[i].lat, csv_landmark[i].id);
 251     }
 252
 253     uim_debug("landmark data read. count = %d\n", landmark_data_count);
 254
 255     return;
 256 }
 257
 258 /*--------------------------------------------------------------------------*/
 259 /*
 260  * @brief   calc_landmarks_coord_from_lonlat
 261  *          The coordinates of a landmark are calculated from
 262  *          latitude and longitude.
 263  *
 264  * @param     none
 265  * @return    none
 266  */
 267 /*--------------------------------------------------------------------------*/
 268 static void calc_landmarks_coord_from_lonlat()
 269 {
 270     int i;
 271
 272     for (i = 0; i < landmark_data_count; i++) {
 273         double x = (csv_landmark[i].lon - csv_route[0].lon) * LON_CONVERT * COORD_CONVERT_COEFFICIENT;
 274         double z = (csv_landmark[i].lat - csv_route[0].lat) * LAT_CONVERT * COORD_CONVERT_COEFFICIENT;
 275         POINT(landmark_raw[i], x, conf_data[LANDMARK_POSITION], z, 0, 0);
 276     }
 277 }
 278
 279 /*--------------------------------------------------------------------------*/
 280 /*
 281  * @brief   load_route_from_websocket
 282  *          route information acquisition.
 283  *
 284  * @param[in]   e3d               3D view Evas object
 285  * @param[in]   p_in              receive message
 286  * @param[in]   len               message size[BYTE]
 287  * @return      none
 288  */
 289 /*--------------------------------------------------------------------------*/
 290 void
 291 init_e3d( Evas *e3d, void *p_in, size_t len )
 292 {
 293     int i, j;
 294
 295     goal_square_length = 1000000000000000.0;
 296
 297     /***********************
 298      * Route Initialize *
 299      ***********************/
 300
 301     /* load route data */
 302     load_route_from_websocket( (char *)p_in, len );
 303
 304     calc_route_coord_from_lonlat();
 305     route_draw_num = route_data_count;
 306
 307     for (i = 0; i < route_draw_num - 1; i++) {
 308         base_plane[i] = _plane_new(e3d, 100, 400, 170);
 309         fixed_plane[i] = _plane_new(e3d, 100, 400, 170);
 310     }
 311     for (i = 0; i < route_draw_num - 2; i++) {
 312         for (j = 0; j < ROUTE_DIVISION_NUMBER; j++) {
 313             division_plane[i][j] = _plane_new(e3d, 10, 10, 10);
 314         }
 315     }
 316
 317     goal_object_new(e3d);
 318
 319 #ifndef _USE_Z_LIMIT_FIX_
 320     calc_route_distance();
 321 #endif
 322
 323     calc_route_coord_by_camera();
 324     calc_base_box_points_all();
 325     calc_fixed_box_points_all();
 326
 327 #ifdef _E3D_H_OUTPUT_PRINT_
 328     uim_debug("Base Plane point0 = %f, %f, %f \n", base_plane[0]->pt[0].x,
 329       base_plane[0]->pt[0].y, base_plane[0]->pt[0].z);
 330     uim_debug("Base Plane point1 = %f, %f, %f \n", base_plane[0]->pt[1].x,
 331       base_plane[0]->pt[1].y, base_plane[0]->pt[1].z);
 332     uim_debug("Base Plane point2 = %f, %f, %f \n", base_plane[0]->pt[2].x,
 333       base_plane[0]->pt[2].y, base_plane[0]->pt[2].z);
 334     uim_debug("Base Plane point3 = %f, %f, %f \n", base_plane[0]->pt[3].x,
 335       base_plane[0]->pt[3].y, base_plane[0]->pt[3].z);
 336 #endif
 337
 338     /***********************
 339      * Landmark Initialize *
 340      ***********************/
 341
 342     /* load landmark data */
 343     load_landmarks_from_csv();
 344     calc_landmarks_coord_from_lonlat();
 345
 346     for (i = 0; i < landmark_data_count; i++) {
 347         landmark_cube[i] = _cube_new(e3d, LANDMARK_WIDTH, (LANDMARK_WIDTH / 3) * 4, LANDMARK_WIDTH,
 348                 landmark_img_path(csv_landmark[i].id));
 349     }
 350
 351 /*****************************************************************************/
 352
 353     // for test
 354     uim_debug("Route CSV x = %f, z = %f\n", csv_route[1].lon, csv_route[1].lat);
 355     uim_debug("Route x = %f, z = %f\n", route_raw[1].x, route_raw[1].z);
 356     uim_debug("Landmark CSV x = %f, z = %f\n", csv_landmark[0].lon, csv_landmark[0].lat);
 357     uim_debug("Landmark x = %f, z = %f\n", landmark_raw[0].x, landmark_raw[0].z);
 358
 359     /***********************
 360      * Camera Initialize *
 361      ***********************/
 362     _camera_pos(0, 0, 0, 0);
 363     camera_geocode.lon = csv_route[0].lon;
 364     camera_geocode.lat = csv_route[0].lat;
 365     camera_geocode.dir = 0;
 366     calc_camera_coord();
 367
 368     /***********************
 369      * Map present location Initialize *
 370      ***********************/
 371     map_pos.lon = csv_route[0].lon;
 372     map_pos.lat = csv_route[0].lat;
 373
 374     /* draw e3d */
 375     draw_route(e3d);
 376     draw_landmark(e3d);
 377 }
 378
 379 /*--------------------------------------------------------------------------*/
 380 /*
 381  * @brief   landmark_img_path
 382  *          Image file path acquisition of a landmark.
 383  *
 384  * @param[in]   i     landmark image file path identifier
 385  * @return      file path address
 386  * @retval       > 0      success
 387  * @retval       NULL     error
 388  */
 389 /*--------------------------------------------------------------------------*/
 390 static const char *
 391 landmark_img_path(int i)
 392 {
 393     const char *img_path;
 394     switch (i) {
 395         case 0:
 396             img_path = landmark_img_path_0;
 397             break;
 398         case 1:
 399             img_path = landmark_img_path_1;
 400             break;
 401         case 2:
 402             img_path = landmark_img_path_2;
 403             break;
 404         case 3:
 405             img_path = landmark_img_path_3;
 406             break;
 407         case 4:
 408             img_path = landmark_img_path_4;
 409             break;
 410         case 5:
 411             img_path = landmark_img_path_5;
 412             break;
 413         case 6:
 414             img_path = landmark_img_path_6;
 415             break;
 416         case 7:
 417             img_path = landmark_img_path_7;
 418             break;
 419         case 8:
 420             img_path = landmark_img_path_8;
 421             break;
 422         case 9:
 423             img_path = landmark_img_path_9;
 424             break;
 425         case 10:
 426             img_path = landmark_img_path_10;
 427             break;
 428         case 11:
 429             img_path = landmark_img_path_11;
 430             break;
 431         case 12:
 432             img_path = landmark_img_path_12;
 433             break;
 434         case 13:
 435             img_path = landmark_img_path_13;
 436             break;
 437         case 14:
 438             img_path = landmark_img_path_14;
 439             break;
 440         case 15:
 441             img_path = landmark_img_path_15;
 442             break;
 443     }
 444     return img_path;
 445 }
 446
 447 /*--------------------------------------------------------------------------*/
 448 /*
 449  * @brief   e3d_cleanup
 450  *          The information on a landmark is loaded from a CSV file.
 451  *
 452  * @param     none
 453  * @return    none
 454  */
 455 /*--------------------------------------------------------------------------*/
 456 void
 457 e3d_cleanup()
 458 {
 459     int i, j;
 460
 461     for (i = 0; i < route_draw_num - 1; i++) {
 462         _plane_free(base_plane[i]);
 463         _plane_free(fixed_plane[i]);
 464     }
 465
 466     for (i = 0; i < route_draw_num - 2; i++) {
 467         for (j = 0; j < ROUTE_DIVISION_NUMBER; j++) {
 468             _plane_free(division_plane[i][j]);
 469         }
 470     }
 471
 472     for (i = 0; i < landmark_data_count; i++) {
 473         _cube_free(landmark_cube[i]);
 474     }
 475
 476     goal_object_free();
 477 }
 478
 479 void draw_route(Evas *e3d) {
 480     int i;
 481     int j;
 482
 483 #ifdef _SHOW_FPS_
 484     polygon_count = 0;
 485 #endif
 486
 487     calc_route_coord_by_camera();
 488     calc_base_box_points_all();
 489     calc_fixed_box_points_all();
 490
 491 // ----------------------------------------------------------------------------
 492
 493     if (enable_navi == FALSE || set_route == FALSE) {
 494         evas_object_hide(goal_object);
 495         return;
 496     }
 497
 498     for (i = 0; i < route_draw_num - 1; i++) {
 499         if (fixed_plane[i]->drawable == TRUE) {
 500             _plane_draw(fixed_plane[i]);
 501         } else {
 502             evas_object_hide(fixed_plane[i]->o);
 503         }
 504     }
 505
 506     /* curve draw */
 507     for (i = 0; i < route_draw_num - 2; i++) {
 508         if (fixed_plane[i]->drawable == TRUE) {
 509             draw_curve(i);
 510         } else {
 511             for (j = 0; j < ROUTE_DIVISION_NUMBER; j++) {
 512                 evas_object_hide(division_plane[i][j]->o);
 513             }
 514         }
 515 #ifdef _E3D_H_OUTPUT_PRINT_
 516         uim_debug("curve draw. \n");
 517 #endif
 518     }
 519
 520 #ifdef _E3D_H_OUTPUT_PRINT_
 521     uim_debug("route draw comp.\n");
 522 #endif
 523
 524     goal_object_draw(fixed_plane[route_draw_num - 2]);
 525
 526 }
 527
 528 void draw_landmark(Evas *e3d) {
 529     static unsigned long frame_count = 0;
 530     PointLength draw_order[landmark_data_count];
 531     int i, j, k;
 532     Point camera_coord;
 533     int landmark_angle;
 534     double culling_length;
 535
 536     if (enable_navi == FALSE) {
 537         for (i = 0; i < landmark_data_count; i++) {
 538             for (j = 0; j < 6; j++) {
 539                 evas_object_hide(landmark_cube[i]->side[j].o);
 540             }
 541         }
 542         return;
 543     }
 544     calc_landmarks_coord_by_camera();
 545
 546     camera_coord.x = camera.x;
 547     camera_coord.z = camera.z;
 548
 549     /* calc landmark rotation */
 550     landmark_angle = (int)((conf_data[LANDMARK_ROTATION] / (1.0 / TIME_INTERVAL_AR)) * frame_count) % 360;
 551
 552     culling_length = CULLING_LENGTH_LANDMARKS * CULLING_LENGTH_LANDMARKS;
 553
 554     /* In order to draw from distant landmark. */
 555     for (i = 0; i < landmark_data_count; i++) {
 556         draw_order[i].index = i;
 557         draw_order[i].length = calc_square_length(landmark[i], camera_coord);
 558
 559         /* for culling */
 560         if (culling_length < draw_order[i].length) {
 561             draw_order[i].drawable = FALSE;
 562         } else {
 563             draw_order[i].drawable = TRUE;
 564         }
 565     }
 566
 567     qsort(draw_order, landmark_data_count, sizeof(PointLength), length_comp);
 568
 569     for (i = 0; i < landmark_data_count; i++) {
 570         j = draw_order[i].index;
 571         if (draw_order[i].drawable == TRUE) {
 572             _cube_draw(landmark_cube[j], landmark[j].x, landmark[j].y, landmark[j].z,
 573                     0, landmark_angle, 0);
 574         } else {
 575             for (k = 0; k < 6; k++) {
 576                 evas_object_hide(landmark_cube[j]->side[k].o);
 577             }
 578         }
 579     }
 580
 581     frame_count++;
 582 }
 583
 584 void
 585 _camera_pos(Evas_Coord x, Evas_Coord y, Evas_Coord z, double angle)
 586 {
 587     // uim_debug("camera_pos x:%d y:%d z:%d angle:%d", x, y, z, angle);
 588     camera.x = x;
 589     camera.y = y;
 590     camera.z = z;
 591     camera.angle = angle;
 592 }
 593
 594 void _camera_move(double speed, double angle) {
 595     double x, y, z;
 596     double rad;
 597
 598     rad = (-angle) * M_PI / 180.0;
 599
 600     x = camera.x + speed * sin(rad);
 601     y = camera.y;
 602     z = camera.z + speed * cos(rad);
 603
 604     _camera_pos(x, y, z, angle);
 605 }
 606
 607 void calc_camera_coord() {
 608     double x, z;
 609
 610     x = (camera_geocode.lon - csv_route[0].lon) * LON_CONVERT * COORD_CONVERT_COEFFICIENT;
 611     z = (camera_geocode.lat - csv_route[0].lat) * LAT_CONVERT * COORD_CONVERT_COEFFICIENT;
 612
 613     _camera_pos(x, camera.y, z, (360.0 - (double)camera_geocode.dir));
 614
 615 }
 616
 617 /******************************************************************************
 618  * Functions for Drawing on Evas
 619  ******************************************************************************/
 620 Plane* _plane_new(Evas *e3d, Evas_Coord w, Evas_Coord d, Evas_Coord h)
 621 {
 622     Plane *plane;
 623     Evas_Object *o;
 624
 625     plane = calloc(1, sizeof(Plane));
 626
 627     /* origin point */
 628     w -= (w / 2);
 629
 630     o = evas_object_rectangle_add(e3d);
 631     plane->o = o;
 632
 633     evas_object_resize(o, 256, 256);
 634     evas_object_hide(o);
 635
 636     SIDE_POINT(0, -w, -h,  d,   0,   0);
 637     SIDE_POINT(1,  w, -h,  d, 256,   0);
 638     SIDE_POINT(2,  w, -h, -d, 256, 256);
 639     SIDE_POINT(3, -w, -h, -d,   0, 256);
 640
 641     evas_object_layer_set(o, LAYER_ROUTE);
 642
 643     plane->drawable = TRUE;
 644
 645     return plane;
 646 }
 647
 648 void _plane_draw(Plane *plane) {
 649     static Evas_Map *m2 = NULL;
 650     int i;
 651
 652     Evas_Coord fix_z[4];
 653
 654     if (!m2) m2 = evas_map_new(4);
 655     evas_map_smooth_set(m2, 0);
 656
 657 #ifdef _USE_Z_LIMIT_FIX_
 658     if(plane->drawable == FALSE) {
 659         evas_object_hide(plane->o);
 660         return;
 661     }
 662 #endif
 663
 664     evas_object_color_set(plane->o, 51, 178, 0, 179);
 665
 666     /* set points */
 667     evas_map_point_coord_set(m2, 0, (Evas_Coord)(plane->pt[0].x + ((W_WIDTH) / 2)),
 668             (Evas_Coord)(plane->pt[0].y + ((W_NAVI_HEIGHT) / 2)), (Evas_Coord)(plane->pt[0].z + FRONT_SIDE_Z));
 669     evas_map_point_coord_set(m2, 1, (Evas_Coord)(plane->pt[1].x + ((W_WIDTH) / 2)),
 670             (Evas_Coord)(plane->pt[1].y + ((W_NAVI_HEIGHT) / 2)), (Evas_Coord)(plane->pt[1].z + FRONT_SIDE_Z));
 671     evas_map_point_coord_set(m2, 2, (Evas_Coord)(plane->pt[2].x + ((W_WIDTH) / 2)),
 672             (Evas_Coord)(plane->pt[2].y + ((W_NAVI_HEIGHT) / 2)), (Evas_Coord)(plane->pt[2].z + FRONT_SIDE_Z));
 673     evas_map_point_coord_set(m2, 3, (Evas_Coord)(plane->pt[3].x + ((W_WIDTH) / 2)),
 674             (Evas_Coord)(plane->pt[3].y + ((W_NAVI_HEIGHT) / 2)), (Evas_Coord)(plane->pt[3].z + FRONT_SIDE_Z));
 675
 676     for (i = 0; i < 4; i++) {
 677         evas_map_point_image_uv_set(m2, i, plane->pt[i].u, plane->pt[i].v);
 678         evas_map_point_color_set(m2, i, 255, 255, 255, 255);
 679     }
 680     evas_map_util_3d_perspective(m2, (W_WIDTH / 2), (W_NAVI_HEIGHT / 2) - 100,
 681             Z0_POSITION, FOCUS_LENGTH);
 682
 683     evas_object_map_enable_set(plane->o, 1);
 684     evas_object_map_set(plane->o, m2);
 685
 686     evas_object_show(plane->o);
 687
 688 #ifdef _SHOW_FPS_
 689     polygon_count++;
 690 #endif
 691
 692 }
 693
 694 void _plane_free(Plane *plane) {
 695     evas_object_del(plane->o);
 696     free(plane);
 697 }
 698
 699 Cube *_cube_new(Evas *e3d, Evas_Coord w, Evas_Coord h, Evas_Coord d, const char *img_path) {
 700     Cube *c;
 701     int i;
 702
 703     w -= (w / 2);
 704     h -= (h / 2);
 705     d -= (d / 2);
 706     c = calloc(1, sizeof(Cube));
 707     for (i = 0; i < 6; i++)
 708     {
 709         Evas_Object *o;
 710         switch (i) {
 711             case 0:
 712             case 1:
 713             case 2:
 714             case 3:
 715                 o = evas_object_image_add(e3d);
 716                 c->side[i].o = o;
 717                 evas_object_image_file_set(o, img_path, NULL);
 718                 evas_object_image_fill_set(o, 0, 0, 256, 256);
 719                 evas_object_resize(o, 256, 256);
 720                 evas_object_image_smooth_scale_set(o, 0);
 721                 evas_object_show(o);
 722                 evas_object_layer_set(o, LAYER_LANDMARK);
 723                 break;
 724             case 4:
 725             case 5:
 726                 o = evas_object_rectangle_add(e3d);
 727                 c->side[i].o = o;
 728                 evas_object_color_set(o, 0, 34, 119, 255);
 729                 evas_object_resize(o, 256, 256);
 730                 evas_object_show(o);
 731                 evas_object_layer_set(o, LAYER_LANDMARK);
 732                 break;
 733         }
 734     }
 735     // First Plane (Front)
 736     CUBE_POINT(0, 0, -w, -h, -d,   0,   0);
 737     CUBE_POINT(0, 1,  w, -h, -d, 600,   0);
 738     CUBE_POINT(0, 2,  w,  h, -d, 600, 800);
 739     CUBE_POINT(0, 3, -w,  h, -d,   0, 800);
 740
 741     // Second Plane (Right side)
 742     CUBE_POINT(1, 0,  w, -h, -d,   0,   0);
 743     CUBE_POINT(1, 1,  w, -h,  d, 600,   0);
 744     CUBE_POINT(1, 2,  w,  h,  d, 600, 800);
 745     CUBE_POINT(1, 3,  w,  h, -d,   0, 800);
 746
 747     // Third Plane (Back)
 748     CUBE_POINT(2, 0,  w, -h,  d,   0,   0);
 749     CUBE_POINT(2, 1, -w, -h,  d, 600,   0);
 750     CUBE_POINT(2, 2, -w,  h,  d, 600, 800);
 751     CUBE_POINT(2, 3,  w,  h,  d,   0, 800);
 752
 753     // Fourth Plane (Left side)
 754     CUBE_POINT(3, 0, -w, -h,  d,   0,   0);
 755     CUBE_POINT(3, 1, -w, -h, -d, 600,   0);
 756     CUBE_POINT(3, 2, -w,  h, -d, 600, 800);
 757     CUBE_POINT(3, 3, -w,  h,  d,   0, 800);
 758
 759     // Fifth Plane (Top side)
 760     CUBE_POINT(4, 0, -w, -h,  d,   0,   0);
 761     CUBE_POINT(4, 1,  w, -h,  d, 256,   0);
 762     CUBE_POINT(4, 2,  w, -h, -d, 256, 256);
 763     CUBE_POINT(4, 3, -w, -h, -d,   0, 256);
 764
 765     // Sixth Plane (Under side)
 766     CUBE_POINT(5, 0, -w,  h, -d,   0,   0);
 767     CUBE_POINT(5, 1,  w,  h, -d, 256,   0);
 768     CUBE_POINT(5, 2,  w,  h,  d, 256, 256);
 769     CUBE_POINT(5, 3, -w,  h,  d,   0, 256);
 770
 771     return c;
 772 }
 773
 774 void _cube_draw(Cube *c, Evas_Coord x, Evas_Coord y, Evas_Coord z,
 775         double dx, double dy, double dz)
 776 {
 777     static Evas_Map *m = NULL;
 778     int i, j, k, order[6], sorted, drawable, tmp_z;
 779     Evas_Coord mz[6];
 780
 781     drawable = TRUE;
 782
 783         // only 4 point
 784     if (!m) m = evas_map_new(4);
 785     // smooth rendering disable
 786     evas_map_smooth_set(m, 0);
 787
 788     for (i = 0; i < 6; i++)
 789     {
 790         Evas_Coord tz[4];
 791
 792         for (j = 0; j < 4; j++)
 793         {
 794
 795             evas_map_point_coord_set(m, j,
 796                     c->side[i].pt[j].x + ((W_WIDTH) / 2) + x - camera.x,
 797                     c->side[i].pt[j].y + ((W_NAVI_HEIGHT) / 2) + y - camera.y,
 798                     c->side[i].pt[j].z + z - camera.z + FRONT_SIDE_Z);
 799             if ( !(i == 4 || i == 5)) {
 800                 evas_map_point_image_uv_set(m, j,
 801                         c->side[i].pt[j].u,
 802                         c->side[i].pt[j].v);
 803             }
 804             evas_map_point_color_set(m, j, 255, 255, 255, 255);
 805              }
 806         evas_map_util_3d_rotate(m, dx, dy, dz,
 807             (W_WIDTH / 2) + x - camera.x, (W_NAVI_HEIGHT / 2) + y - camera.y, z - camera.z + FRONT_SIDE_Z);
 808
 809         // for camera angle
 810         evas_map_util_3d_rotate(m, 0, -camera.angle, 0,
 811                 (W_WIDTH / 2), (W_NAVI_HEIGHT / 2), FRONT_SIDE_Z);
 812
 813         evas_map_util_3d_perspective(m, (W_WIDTH / 2), (W_NAVI_HEIGHT / 2) , Z0_POSITION, FOCUS_LENGTH);
 814
 815 #ifdef _USE_Z_LIMIT_FIX_
 816         for (j = 0; j < 4; j++) {
 817             evas_map_point_coord_get(m, j, NULL, NULL, &tmp_z);
 818             if (tmp_z < (Z_LIMIT + FRONT_SIDE_Z)) {
 819                 drawable = FALSE;
 820             }
 821         }
 822 #endif
 823         if (evas_map_util_clockwise_get(m))
 824         {
 825             evas_object_map_enable_set(c->side[i].o, 1);
 826             evas_object_map_set(c->side[i].o, m);
 827 #ifdef _USE_Z_LIMIT_FIX_
 828             if (drawable == TRUE) {
 829 #endif
 830                 evas_object_show(c->side[i].o);
 831 #ifdef _SHOW_FPS_
 832                 polygon_count++;
 833 #endif
 834 #ifdef _USE_Z_LIMIT_FIX_
 835             } else {
 836                 evas_object_hide(c->side[i].o);
 837             }
 838 #endif
 839         }
 840         else {
 841             evas_object_hide(c->side[i].o);
 842         }
 843
 844         order[i] = i;
 845
 846         for (j = 0; j < 4; j++) {
 847             evas_map_point_coord_get(m, j, NULL, NULL, &(tz[j]));
 848         }
 849
 850         mz[i] = (tz[0] + tz[1] + tz[2] + tz[3]) / 4;
 851
 852         if (mz[i] < -512) {
 853             evas_object_hide(c->side[i].o);
 854         }
 855     }
 856
 857     // sort by z
 858     sorted = 0;
 859     do
 860     {
 861         sorted = 1;
 862         for (i = 0; i < 5; i++)
 863         {
 864             if (mz[order[i]] > mz[order[i + 1]])
 865             {
 866                 j = order[i];
 867                 order[i] = order[i + 1];
 868                 order[i + 1] = j;
 869                 sorted = 0;
 870             }
 871         }
 872     }
 873     while (!sorted);
 874
 875     evas_object_raise(c->side[order[0]].o);
 876
 877     for (i = 1; i < 6; i++) {
 878         evas_object_stack_below(c->side[order[i]].o, c->side[order[i - 1]].o);
 879     }
 880 }
 881
 882 void _cube_free(Cube *c)
 883 {
 884     int i;
 885
 886     for (i = 0; i < 6; i++) evas_object_del(c->side[i].o);
 887     free(c);
 888 }
 889
 890 void goal_object_new(Evas *e3d) {
 891     goal_object = evas_object_image_add(e3d);
 892     evas_object_image_file_set(goal_object, goal_img_path, NULL);
 893     evas_object_image_fill_set(goal_object, 0, 0, 600, 600);
 894
 895     evas_object_resize(goal_object, 256, 256);
 896     evas_object_image_smooth_scale_set(goal_object, 0);
 897     evas_object_hide(goal_object);
 898 }
 899
 900 void goal_object_draw(Plane *plane) {
 901     static Evas_Map *m3 = NULL;
 902     int i;
 903
 904     if (plane->drawable == FALSE) {
 905         evas_object_hide(goal_object);
 906         return;
 907     }
 908
 909     if (!m3) m3 = evas_map_new(4);
 910     evas_map_smooth_set(m3, 0);
 911
 912     evas_map_point_coord_set(m3, 0, (Evas_Coord)(plane->pt[0].x + ((W_WIDTH) / 2)),
 913             (Evas_Coord)(plane->pt[0].y + ((W_NAVI_HEIGHT) / 2)), (Evas_Coord)(plane->pt[0].z + FRONT_SIDE_Z));
 914     evas_map_point_coord_set(m3, 1, (Evas_Coord)(plane->pt[0].x + ((W_WIDTH) / 2)),
 915             (Evas_Coord)((-1) * plane->pt[0].y + (W_NAVI_HEIGHT) / 2), (Evas_Coord)(plane->pt[0].z + FRONT_SIDE_Z));
 916     evas_map_point_coord_set(m3, 2,(Evas_Coord)(plane->pt[3].x + ((W_WIDTH) / 2)),
 917             (Evas_Coord)((-1) * plane->pt[3].y + (W_NAVI_HEIGHT) / 2), (Evas_Coord)(plane->pt[3].z + FRONT_SIDE_Z));
 918     evas_map_point_coord_set(m3, 3, (Evas_Coord)(plane->pt[3].x + ((W_WIDTH) / 2)),
 919             (Evas_Coord)(plane->pt[3].y + ((W_NAVI_HEIGHT) / 2)), (Evas_Coord)(plane->pt[3].z + FRONT_SIDE_Z));
 920
 921     evas_map_point_image_uv_set(m3, 0, 600,   0);
 922     evas_map_point_image_uv_set(m3, 1, 600, 600);
 923     evas_map_point_image_uv_set(m3, 2,   0, 600);
 924     evas_map_point_image_uv_set(m3, 3,   0,   0);
 925
 926     evas_map_util_3d_perspective(m3, (W_WIDTH / 2), (W_NAVI_HEIGHT / 2) - 100,
 927             Z0_POSITION, FOCUS_LENGTH);
 928
 929     evas_object_map_enable_set(goal_object, 1);
 930     evas_object_map_set(goal_object, m3);
 931     evas_object_show(goal_object);
 932
 933     evas_object_raise(goal_object);
 934 }
 935
 936 void goal_object_free() {
 937     evas_object_del(goal_object);
 938 }
 939
 940
 941 /******************************************************************************
 942  * Functions for Calculate Route Points
 943  ******************************************************************************/
 944
 945 #ifdef _USE_Z_LIMIT_FIX_
 946 void check_route_darawable_by_z_limit_fix() {
 947     int i;
 948     Point limit_p0, limit_p1;
 949
 950     POINT(limit_p0, -100, ROUTE_PLANE_HEIGHT, Z_LIMIT, 0, 0);
 951     POINT(limit_p1, 100, ROUTE_PLANE_HEIGHT, Z_LIMIT, 0, 0);
 952
 953     for (i = 0; i < route_draw_num - 1; i++) {
 954         route[i].status = NONE;
 955         route[i].enable_fix = TRUE;
 956
 957         if (route_pre_fix[i].z < Z_LIMIT) {
 958             route[i].status = STARTING_POINT;
 959             route[i].enable_fix = FALSE;
 960             if (route_pre_fix[i+1].z >= Z_LIMIT) {
 961                 calc_intersection(limit_p0, limit_p1, route_pre_fix[i], route_pre_fix[i+1], &route[i]);
 962             } else {
 963                 if (!(i > 0 && (route[i-1].status == ENDING_POINT))) {
 964                     route[i].status = BOTH_POINT;
 965                     route[i].x = route_pre_fix[i].x;
 966                     route[i].z = route_pre_fix[i].z;
 967                 }
 968             }
 969         } else {
 970             // don't fix
 971             route[i].x = route_pre_fix[i].x;
 972             route[i].z = route_pre_fix[i].z;
 973
 974             if (route_pre_fix[i].z < 0) {
 975                 if (i > 0 && (fixed_plane[i - 1]->drawable == FALSE)) {
 976                     route[i].enable_fix = FALSE;
 977                 }
 978             }
 979         }
 980
 981         if (route_pre_fix[i+1].z < Z_LIMIT) {
 982             if ((route[i].status == STARTING_POINT) || (route[i].status == BOTH_POINT)) {
 983                 route[i].status = BOTH_POINT;
 984
 985                 if ((i+1) == route_draw_num - 1) {
 986                     route[i+1].x = route_pre_fix[i+1].x;
 987                     route[i+1].z = route_pre_fix[i+1].z;
 988                 } else {
 989                     if (route_pre_fix[i+2].z >= Z_LIMIT) {
 990                         calc_intersection(limit_p0, limit_p1, route_pre_fix[i+1], route_pre_fix[i+2], &route[i+1]);
 991
 992                     } else {
 993                         route[i+1].x = route_pre_fix[i+1].x;
 994                         route[i+1].z = route_pre_fix[i+1].z;
 995                     }
 996                 }
 997
 998                 if ((i < route_draw_num - 1) && (route_pre_fix[i+2].z > Z_LIMIT)) {
 999                     calc_intersection(limit_p0, limit_p1, route_pre_fix[i+1], route_pre_fix[i+2], &route[i+1]);
1000                 } else {
1001                     route[i+1].x = route_pre_fix[i+1].x;
1002                     route[i+1].z = route_pre_fix[i+1].z;
1003                 }
1004             } else {
1005                 if (route_pre_fix[i].z >= Z_LIMIT) {
1006                     route[i].status = ENDING_POINT;
1007                     calc_intersection(limit_p0, limit_p1, route_pre_fix[i], route_pre_fix[i+1], &route[i+1]);
1008                 } else {
1009                     route[i].status = BOTH_POINT;
1010                 }
1011             }
1012         } else {
1013             route[i+1].x = route_pre_fix[i+1].x;
1014             route[i+1].z = route_pre_fix[i+1].z;
1015         }
1016
1017         if (route[i].status == BOTH_POINT) {
1018             fixed_plane[i]->drawable = FALSE;
1019         } else {
1020             fixed_plane[i]->drawable = TRUE;
1021         }
1022
1023     }
1024
1025
1026 }
1027 #endif
1028
1029 void calc_route_coord_by_camera() {
1030     int i;
1031     Point orizin;
1032
1033     for (i = 0; i < route_draw_num; i++) {
1034 #ifdef _USE_Z_LIMIT_FIX_
1035         route_pre_fix[i].x = route_raw[i].x - camera.x;
1036         route_pre_fix[i].z = route_raw[i].z - camera.z;
1037         rotate_xz(route_pre_fix[i], &route_pre_fix[i], camera.angle);
1038
1039 #else
1040         route[i].x = route_raw[i].x - camera.x;
1041         route[i].z = route_raw[i].z - camera.z;
1042         rotate_xz(route[i], &route[i], camera.angle);
1043
1044 #endif
1045     }
1046
1047     orizin.x = 0;
1048     orizin.y = 0;
1049 #ifdef _USE_Z_LIMIT_FIX_
1050     goal_square_length = calc_square_length(orizin, route_pre_fix[route_draw_num - 1]);
1051 #else
1052     goal_square_length = calc_square_length(orizin, route[route_draw_num - 1]);
1053 #endif
1054
1055     for (i = 0; i < route_draw_num - 1; i++) {
1056         fixed_plane[i]->drawable = TRUE;
1057     }
1058
1059 #ifdef _USE_Z_LIMIT_FIX_
1060     check_route_darawable_by_z_limit_fix();
1061 #endif
1062
1063     calc_route_distance();
1064     calc_route_distance_to_camera();
1065     check_route_drawable_by_distance_to_camera();
1066 }
1067
1068 void calc_route_distance_to_camera() {
1069     int i;
1070     Point origin;
1071
1072     origin.x = 0;
1073     origin.z = 0;
1074
1075     for ( i = 0; i < route_draw_num; i++) {
1076         route_square_length_camera[i] = calc_square_length(route[i], origin);
1077     }
1078
1079 }
1080
1081 void check_route_drawable_by_distance_to_camera() {
1082     int i, culling_route_count;
1083     double culling_length;
1084
1085     culling_route_count = 0;
1086     culling_length = CULLING_LENGTH_ROUTE * CULLING_LENGTH_ROUTE;
1087
1088     for ( i = 0; i < route_draw_num - 1; i++) {
1089         if ((route_square_length_camera[i] > culling_length) &&
1090                 (route_square_length_camera[i+1] > culling_length)) {
1091             fixed_plane[i]->drawable = FALSE;
1092             culling_route_count++;
1093         }
1094     }
1095
1096 }
1097
1098 void calc_fixed_box_points_all() {
1099     int i;
1100     for (i = 0; i < route_draw_num - 1; i++) {
1101         calc_fixed_box_points(i);
1102     }
1103 }
1104
1105 void calc_fixed_box_points(int routenum) {
1106 #ifdef _USE_Z_LIMIT_FIX_
1107     if (routenum == 0 || route[routenum].enable_fix == FALSE) {
1108 #else
1109     if (routenum == 0) {
1110 #endif
1111         fixed_plane[routenum]->pt[1] = base_plane[routenum]->pt[1];
1112         fixed_plane[routenum]->pt[2] = base_plane[routenum]->pt[2];
1113     } else {
1114         fixed_plane[routenum]->pt[1] = calc_fix_point(routenum, 1);
1115         fixed_plane[routenum]->pt[2] = calc_fix_point(routenum, 2);
1116     }
1117
1118 #ifdef _USE_Z_LIMIT_FIX_
1119     if (routenum == route_draw_num - 2 || (route[routenum].status == ENDING_POINT || route[routenum].status == BOTH_POINT)) {
1120 #else
1121     if (routenum == route_draw_num - 2) {
1122 #endif
1123         fixed_plane[routenum]->pt[0] = base_plane[routenum]->pt[0];
1124         fixed_plane[routenum]->pt[3] = base_plane[routenum]->pt[3];
1125     } else {
1126         fixed_plane[routenum]->pt[0] = calc_fix_point(routenum, 0);
1127         fixed_plane[routenum]->pt[3] = calc_fix_point(routenum, 3);
1128     }
1129 }
1130
1131 Point calc_fix_point(int routenum, int point_index) {
1132     double m;
1133     Point result, p0, p1;
1134
1135     if (route_distance[routenum] > (BOX_FIXED_LENGTH * 2)) {
1136         m = (route_distance[routenum] - BOX_FIXED_LENGTH) / route_distance[routenum];
1137     } else {
1138         m = 0.5;
1139     }
1140
1141
1142     POINT(result, 0, ROUTE_PLANE_HEIGHT, 0, 0, 0);
1143     POINT(p0, 0, ROUTE_PLANE_HEIGHT, 0, 0, 0);
1144     POINT(p1, base_plane[routenum]->pt[point_index].x, ROUTE_PLANE_HEIGHT, base_plane[routenum]->pt[point_index].z, 0, 0);
1145
1146     if (point_index == 0) {
1147         p0.x = base_plane[routenum]->pt[1].x;
1148         p0.z = base_plane[routenum]->pt[1].z;
1149     } else if (point_index == 1) {
1150         p0.x = base_plane[routenum]->pt[0].x;
1151         p0.z = base_plane[routenum]->pt[0].z;
1152     } else if (point_index == 2) {
1153         p0.x = base_plane[routenum]->pt[3].x;
1154         p0.z = base_plane[routenum]->pt[3].z;
1155     } else {
1156         p0.x = base_plane[routenum]->pt[2].x;
1157         p0.z = base_plane[routenum]->pt[2].z;
1158     }
1159
1160     result.x = (m * (p1.x - p0.x)) + p0.x;
1161     result.z = (m * (p1.z - p0.z)) + p0.z;
1162
1163     return result;
1164 }
1165
1166 void draw_curve(int routenum) {
1167     int i;
1168
1169     Point con_point1;
1170     Point con_point2;
1171     Point curve_point1[ROUTE_DIVISION_NUMBER+1];
1172     Point curve_point2[ROUTE_DIVISION_NUMBER+1];
1173
1174     calc_intersection(base_plane[routenum]->pt[0], base_plane[routenum]->pt[1],
1175                     base_plane[routenum+1]->pt[0], base_plane[routenum+1]->pt[1], &con_point1);
1176     calc_intersection(base_plane[routenum]->pt[3], base_plane[routenum]->pt[2],
1177                     base_plane[routenum+1]->pt[3], base_plane[routenum+1]->pt[2], &con_point2);
1178
1179 #ifdef _E3D_H_OUTPUT_PRINT_
1180     uim_debug("con_point1 x = %f, z = %f \n", con_point1.x, con_point1.z);
1181     uim_debug("con_point2 x = %f, z = %f \n", con_point2.x, con_point2.z);
1182 #endif
1183
1184     calc_bezier_points(fixed_plane[routenum]->pt[0], con_point1, fixed_plane[routenum+1]->pt[1], curve_point1);
1185     calc_bezier_points(fixed_plane[routenum]->pt[3], con_point2, fixed_plane[routenum+1]->pt[2], curve_point2);
1186
1187     for (i = 0; i < (ROUTE_DIVISION_NUMBER) + 1 ;i++) {
1188         if (curve_point1[i].z < Z_LIMIT) {
1189             curve_point1[i].z = Z_LIMIT;
1190         }
1191         if (curve_point2[i].z < Z_LIMIT) {
1192             curve_point2[i].z = Z_LIMIT;
1193         }
1194     }
1195
1196     for (i = 0; i < ROUTE_DIVISION_NUMBER; i++) {
1197         PLANE_POINT(division_plane[routenum][i], 0, curve_point1[i+1].x, ROUTE_PLANE_HEIGHT, curve_point1[i+1].z, 0, 0);
1198         PLANE_POINT(division_plane[routenum][i], 1, curve_point1[i].x, ROUTE_PLANE_HEIGHT, curve_point1[i].z, 256, 0);
1199         PLANE_POINT(division_plane[routenum][i], 2, curve_point2[i].x, ROUTE_PLANE_HEIGHT, curve_point2[i].z, 256, 256);
1200         PLANE_POINT(division_plane[routenum][i], 3, curve_point2[i+1].x, ROUTE_PLANE_HEIGHT, curve_point2[i+1].z, 0, 256);
1201
1202         _plane_draw(division_plane[routenum][i]);
1203
1204 #ifdef _E3D_H_OUTPUT_PRINT_
1205         uim_debug("Division Plane %d : x0 = %f, z0 = %f, x1 = %f, z1 = %f, x2 = %f, z2 = %f, x3 = %f, z3 = %f\n",
1206             routenum, division_plane[routenum][i]->pt[0].x, division_plane[routenum][i]->pt[0].z,
1207                division_plane[routenum][i]->pt[1].x, division_plane[routenum][i]->pt[1].z,
1208                division_plane[routenum][i]->pt[2].x, division_plane[routenum][i]->pt[2].z,
1209                division_plane[routenum][i]->pt[3].x, division_plane[routenum][i]->pt[3].z);
1210         uim_debug("Draw division plane.\n");
1211 #endif
1212
1213     }
1214 }
1215
1216
1217 void calc_bezier_points(Point p0, Point p1, Point p2, Point *points) {
1218     int i;
1219     double a, b, x, z;
1220     Point p;
1221
1222     for(i = 0; i < ROUTE_DIVISION_NUMBER + 1; i++) {
1223         b = (double)i / ROUTE_DIVISION_NUMBER;
1224         a = 1 - b;
1225
1226         x = a * a * p0.x + 2 * a * b * p1.x + b * b * p2.x;
1227         z = a * a * p0.z + 2 * a * b * p1.z + b * b * p2.z;
1228
1229         POINT(p, x, ROUTE_PLANE_HEIGHT, z, 0, 0);
1230         points[i] = p;
1231
1232 #ifdef _E3D_H_OUTPUT_PRINT_
1233         uim_debug("bezier_points %d : x = %f, y = %f, z = %f\n", i, points[i].x, points[i].y, points[i].z);
1234 #endif
1235     }
1236 }
1237
1238 void calc_base_box_points_all() {
1239     int i;
1240     for (i = 0; i < route_draw_num - 1; i++) {
1241         calc_base_box_points(i);
1242     }
1243
1244 }
1245
1246 void calc_base_box_points(int routenum) {
1247     /* Determine the angle. */
1248     double th = atan2((route[routenum+1].z - route[routenum].z),
1249                     (route[routenum+1].x - route[routenum].x));
1250     /* Determine the distance. */
1251     double dis = route_distance[routenum];
1252
1253 #ifdef _E3D_H_OUTPUT_PRINT_
1254     uim_debug("th = %f\n", (th*180/M_PI));
1255 #endif
1256
1257     PLANE_POINT(base_plane[routenum], 0,
1258             (cos(th)*dis - sin(th)*(-ROUTE_PLANE_WIDTH)) + route[routenum].x,
1259             ROUTE_PLANE_HEIGHT,
1260             (sin(th)*dis + cos(th)*(-ROUTE_PLANE_WIDTH)) + route[routenum].z, 0, 0);
1261
1262     PLANE_POINT(base_plane[routenum], 1,
1263             (cos(th)*0 - sin(th)*(-ROUTE_PLANE_WIDTH)) + route[routenum].x,
1264             ROUTE_PLANE_HEIGHT,
1265             (sin(th)*0 + cos(th)*(-ROUTE_PLANE_WIDTH)) + route[routenum].z, 256, 0);
1266
1267     PLANE_POINT(base_plane[routenum], 2,
1268             (cos(th)*0 - sin(th)*(ROUTE_PLANE_WIDTH)) + route[routenum].x,
1269             ROUTE_PLANE_HEIGHT,
1270             (sin(th)*0 + cos(th)*(ROUTE_PLANE_WIDTH)) + route[routenum].z, 256, 256);
1271
1272     PLANE_POINT(base_plane[routenum], 3,
1273             (cos(th)*dis - sin(th)*(ROUTE_PLANE_WIDTH)) + route[routenum].x,
1274             ROUTE_PLANE_HEIGHT,
1275             (sin(th)*dis + cos(th)*(ROUTE_PLANE_WIDTH)) + route[routenum].z, 0, 256);
1276
1277 #ifdef _E3D_H_OUTPUT_PRINT_
1278     uim_debug("Plane %d : x0 = %f, z0 = %f, x1 = %f, z1 = %f, x2 = %f, z2 = %f, x3 = %f, z3 = %f\n",
1279             routenum, base_plane[routenum]->pt[0].x, base_plane[routenum]->pt[0].z,
1280                base_plane[routenum]->pt[1].x, base_plane[routenum]->pt[1].z,
1281                base_plane[routenum]->pt[2].x, base_plane[routenum]->pt[2].z,
1282                base_plane[routenum]->pt[3].x, base_plane[routenum]->pt[3].z);
1283 #endif
1284 }
1285
1286 /******************************************************************************
1287  * Functions for Calculate Landmark Points
1288  ******************************************************************************/
1289 void calc_landmarks_coord_by_camera() {
1290     int i;
1291
1292     for (i = 0; i < landmark_data_count; i++) {
1293         landmark[i].x = landmark_raw[i].x;
1294         landmark[i].y = landmark_raw[i].y;
1295         landmark[i].z = landmark_raw[i].z;
1296     }
1297
1298 }
1299
1300 /******************************************************************************
1301  * Functions for Calculate (Common)
1302  ******************************************************************************/
1303 void calc_route_distance() {
1304     int i;
1305     for (i = 0; i < route_draw_num - 1; i++) {
1306         route_distance[i] = calc_length(route[i], route[i+1]);
1307     }
1308 }
1309
1310 /** Calculate the distance between two points.(only x-z plane) */
1311 double calc_length(Point p0, Point p1) {
1312     return sqrt( (p1.x - p0.x) * (p1.x - p0.x) + (p1.z - p0.z) * (p1.z - p0.z) );
1313 }
1314
1315 /** Calculate the square of distance between two points.(only x-z plane) */
1316 double calc_square_length(Point p0, Point p1) {
1317     return ((p1.x - p0.x) * (p1.x - p0.x) + (p1.z - p0.z) * (p1.z - p0.z));
1318 }
1319
1320 /* rotate point (center is orizin, xz-plane) */
1321 void rotate_xz(Point src, Point *dist, double angle) {
1322     double rx, rz, sine, cosine;
1323     double rad;
1324
1325     rad = (-angle) * M_PI / 180.0;
1326
1327     sine = sin(rad);
1328     cosine = cos(rad);
1329
1330     rx = cosine * src.x - sine * src.z;
1331     rz = sine * src.x + cosine * src.z;
1332
1333     dist->x = rx;
1334     dist->y = src.y;
1335     dist->z = rz;
1336     dist->u = src.u;
1337     dist->v = src.v;
1338 }
1339
1340 void calc_intersection(Point p0, Point p1, Point p2, Point p3, Point *point) {
1341     double S1, S2, result_x, result_z;
1342     S1 = ((p2.x - p3.x) * (p1.z - p3.z) - (p2.z - p3.z) * (p1.x - p3.x))/2;
1343     S2 = ((p2.x - p3.x) * (p3.z - p0.z) - (p2.z - p3.z) * (p3.x - p0.x))/2;
1344
1345     result_x = (p1.x + (p0.x - p1.x) * S1 / (S1 + S2));
1346     result_z = (p1.z + (p0.z - p1.z) * S1 / (S1 + S2));
1347
1348     point->x = result_x;
1349     point->y = ROUTE_PLANE_HEIGHT;
1350     point->z = result_z;
1351     point->u = 0;
1352     point->v = 0;
1353 }
1354
1355 int length_comp(const void *pl0, const void *pl1) {
1356     PointLength point_length0 = *(PointLength *)pl0;
1357     PointLength point_length1 = *(PointLength *)pl1;
1358
1359     if (point_length0.length == point_length1.length) {
1360         return 0;
1361     } else if (point_length0.length < point_length1.length) {
1362         return 1;
1363     }
1364     return -1;
1365 }
1366
1367