Updated package changelog.
[profile/ivi/ico-uxf-homescreen-sample-apps.git] / 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