2 * Copyright (c) 2013, TOYOTA MOTOR CORPORATION.
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
10 * @brief library which displays 3D field of view
17 #include "samplenavi.h"
19 #include "ico_apf_log.h"
21 /*============================================================================*/
22 /* Define fixed parameters */
23 /*============================================================================*/
24 /* Get route from websocket */
25 #define ROUTE_START_TAG "<route>"
26 #define ROUTE_END_TAG "</route>"
29 /*============================================================================*/
30 /* Define data types */
31 /*============================================================================*/
32 /* Landmark Points and Cubes */
33 typedef struct _PointLength {
39 /*============================================================================*/
40 /* Function prototype for static(internal) functions */
41 /*============================================================================*/
42 static void load_route_from_websocket(char *p_in, size_t len);
44 /* Local Functions for Route. */
45 static void calc_route_coord_from_lonlat();
47 /* Local Functions for Landmark. */
48 static void load_landmarks_from_csv();
49 static void calc_landmarks_coord_from_lonlat();
51 void init_e3d(Evas *e3d, void *p_in, size_t len);
53 /* Landmark Points and Cubes */
54 static const char *landmark_img_path(int i);
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);
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();
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();
89 /* Local Functions for Landmark. */
90 void calc_landmarks_coord_by_camera();
91 void draw_landmark(Evas *e3d);
93 /*============================================================================*/
94 /* Tables and Valiables */
95 /*============================================================================*/
97 CameraGeocode camera_geocode;
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];
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];
118 double route_distance[MAX_DRAW_ROUTE_POINTS - 1];
119 Evas_Object *goal_object;
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];
128 /*============================================================================*/
130 /*============================================================================*/
131 /*--------------------------------------------------------------------------*/
133 * @brief load_route_from_websocket
134 * route information acquisition.
136 * @param[in] p_in receive message
137 * @param[in] len message size[BYTE]
140 /*--------------------------------------------------------------------------*/
142 load_route_from_websocket(char *p_in, size_t len)
147 route_data_count = 0;
150 while( index < len ) {
152 if( ( strlen( &p_in[index] ) >= strlen( ROUTE_START_TAG ) ) &&
153 ( strncmp( &p_in[index], ROUTE_START_TAG, strlen( ROUTE_START_TAG ) ) == 0 ) )
155 index += strlen( ROUTE_START_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 ) )
164 else if( ( strlen( &p_in[index] ) >= strlen( BR_TAG ) ) &&
165 ( strncmp( &p_in[index], BR_TAG, strlen( BR_TAG ) ) == 0 ) )
167 index += strlen( BR_TAG );
172 memset( str, 0, sizeof( str ) );
173 while( ( p_in[index] != ',' ) && ( index < len ) ) {
174 str[strlen( str )] = p_in[index];
177 csv_route[route_data_count].lon = atof( str );
181 memset( str, 0, sizeof( str ) );
182 while( ( p_in[index] != '<' ) && ( index < len ) ) {
183 str[strlen( str )] = p_in[index];
186 csv_route[route_data_count].lat = atof( str );
190 if(route_data_count > 0){
195 /*--------------------------------------------------------------------------*/
197 * @brief calc_route_coord_from_lonlat
198 * The coordinates of a route are calculated from
199 * latitude and longitude.
204 /*--------------------------------------------------------------------------*/
206 calc_route_coord_from_lonlat()
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);
217 /*--------------------------------------------------------------------------*/
219 * @brief load_landmarks_from_csv
220 * The information on a landmark is loaded from a CSV file.
225 /*--------------------------------------------------------------------------*/
227 load_landmarks_from_csv()
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;
239 while(fscanf(fp, "%lf,%lf,%hhd",
240 &csv_landmark[i].lon, &csv_landmark[i].lat, &csv_landmark[i].id) != EOF) {
243 landmark_data_count = i;
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);
253 uim_debug("landmark data read. count = %d\n", landmark_data_count);
258 /*--------------------------------------------------------------------------*/
260 * @brief calc_landmarks_coord_from_lonlat
261 * The coordinates of a landmark are calculated from
262 * latitude and longitude.
267 /*--------------------------------------------------------------------------*/
268 static void calc_landmarks_coord_from_lonlat()
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);
279 /*--------------------------------------------------------------------------*/
281 * @brief load_route_from_websocket
282 * route information acquisition.
284 * @param[in] e3d 3D view Evas object
285 * @param[in] p_in receive message
286 * @param[in] len message size[BYTE]
289 /*--------------------------------------------------------------------------*/
291 init_e3d( Evas *e3d, void *p_in, size_t len )
295 goal_square_length = 1000000000000000.0;
297 /***********************
299 ***********************/
301 /* load route data */
302 load_route_from_websocket( (char *)p_in, len );
304 calc_route_coord_from_lonlat();
305 route_draw_num = route_data_count;
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);
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);
317 goal_object_new(e3d);
319 #ifndef _USE_Z_LIMIT_FIX_
320 calc_route_distance();
323 calc_route_coord_by_camera();
324 calc_base_box_points_all();
325 calc_fixed_box_points_all();
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);
338 /***********************
339 * Landmark Initialize *
340 ***********************/
342 /* load landmark data */
343 load_landmarks_from_csv();
344 calc_landmarks_coord_from_lonlat();
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));
351 /*****************************************************************************/
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);
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;
368 /***********************
369 * Map present location Initialize *
370 ***********************/
371 map_pos.lon = csv_route[0].lon;
372 map_pos.lat = csv_route[0].lat;
379 /*--------------------------------------------------------------------------*/
381 * @brief landmark_img_path
382 * Image file path acquisition of a landmark.
384 * @param[in] i landmark image file path identifier
385 * @return file path address
386 * @retval > 0 success
389 /*--------------------------------------------------------------------------*/
391 landmark_img_path(int i)
393 const char *img_path;
396 img_path = landmark_img_path_0;
399 img_path = landmark_img_path_1;
402 img_path = landmark_img_path_2;
405 img_path = landmark_img_path_3;
408 img_path = landmark_img_path_4;
411 img_path = landmark_img_path_5;
414 img_path = landmark_img_path_6;
417 img_path = landmark_img_path_7;
420 img_path = landmark_img_path_8;
423 img_path = landmark_img_path_9;
426 img_path = landmark_img_path_10;
429 img_path = landmark_img_path_11;
432 img_path = landmark_img_path_12;
435 img_path = landmark_img_path_13;
438 img_path = landmark_img_path_14;
441 img_path = landmark_img_path_15;
447 /*--------------------------------------------------------------------------*/
450 * The information on a landmark is loaded from a CSV file.
455 /*--------------------------------------------------------------------------*/
461 for (i = 0; i < route_draw_num - 1; i++) {
462 _plane_free(base_plane[i]);
463 _plane_free(fixed_plane[i]);
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]);
472 for (i = 0; i < landmark_data_count; i++) {
473 _cube_free(landmark_cube[i]);
479 void draw_route(Evas *e3d) {
487 calc_route_coord_by_camera();
488 calc_base_box_points_all();
489 calc_fixed_box_points_all();
491 // ----------------------------------------------------------------------------
493 if (enable_navi == FALSE || set_route == FALSE) {
494 evas_object_hide(goal_object);
498 for (i = 0; i < route_draw_num - 1; i++) {
499 if (fixed_plane[i]->drawable == TRUE) {
500 _plane_draw(fixed_plane[i]);
502 evas_object_hide(fixed_plane[i]->o);
507 for (i = 0; i < route_draw_num - 2; i++) {
508 if (fixed_plane[i]->drawable == TRUE) {
511 for (j = 0; j < ROUTE_DIVISION_NUMBER; j++) {
512 evas_object_hide(division_plane[i][j]->o);
515 #ifdef _E3D_H_OUTPUT_PRINT_
516 uim_debug("curve draw. \n");
520 #ifdef _E3D_H_OUTPUT_PRINT_
521 uim_debug("route draw comp.\n");
524 goal_object_draw(fixed_plane[route_draw_num - 2]);
528 void draw_landmark(Evas *e3d) {
529 static unsigned long frame_count = 0;
530 PointLength draw_order[landmark_data_count];
534 double culling_length;
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);
544 calc_landmarks_coord_by_camera();
546 camera_coord.x = camera.x;
547 camera_coord.z = camera.z;
549 /* calc landmark rotation */
550 landmark_angle = (int)((conf_data[LANDMARK_ROTATION] / (1.0 / TIME_INTERVAL_AR)) * frame_count) % 360;
552 culling_length = CULLING_LENGTH_LANDMARKS * CULLING_LENGTH_LANDMARKS;
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);
560 if (culling_length < draw_order[i].length) {
561 draw_order[i].drawable = FALSE;
563 draw_order[i].drawable = TRUE;
567 qsort(draw_order, landmark_data_count, sizeof(PointLength), length_comp);
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);
575 for (k = 0; k < 6; k++) {
576 evas_object_hide(landmark_cube[j]->side[k].o);
585 _camera_pos(Evas_Coord x, Evas_Coord y, Evas_Coord z, double angle)
587 // uim_debug("camera_pos x:%d y:%d z:%d angle:%d", x, y, z, angle);
591 camera.angle = angle;
594 void _camera_move(double speed, double angle) {
598 rad = (-angle) * M_PI / 180.0;
600 x = camera.x + speed * sin(rad);
602 z = camera.z + speed * cos(rad);
604 _camera_pos(x, y, z, angle);
607 void calc_camera_coord() {
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;
613 _camera_pos(x, camera.y, z, (360.0 - (double)camera_geocode.dir));
617 /******************************************************************************
618 * Functions for Drawing on Evas
619 ******************************************************************************/
620 Plane* _plane_new(Evas *e3d, Evas_Coord w, Evas_Coord d, Evas_Coord h)
625 plane = calloc(1, sizeof(Plane));
630 o = evas_object_rectangle_add(e3d);
633 evas_object_resize(o, 256, 256);
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);
641 evas_object_layer_set(o, LAYER_ROUTE);
643 plane->drawable = TRUE;
648 void _plane_draw(Plane *plane) {
649 static Evas_Map *m2 = NULL;
654 if (!m2) m2 = evas_map_new(4);
655 evas_map_smooth_set(m2, 0);
657 #ifdef _USE_Z_LIMIT_FIX_
658 if(plane->drawable == FALSE) {
659 evas_object_hide(plane->o);
664 evas_object_color_set(plane->o, 51, 178, 0, 179);
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));
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);
680 evas_map_util_3d_perspective(m2, (W_WIDTH / 2), (W_NAVI_HEIGHT / 2) - 100,
681 Z0_POSITION, FOCUS_LENGTH);
683 evas_object_map_enable_set(plane->o, 1);
684 evas_object_map_set(plane->o, m2);
686 evas_object_show(plane->o);
694 void _plane_free(Plane *plane) {
695 evas_object_del(plane->o);
699 Cube *_cube_new(Evas *e3d, Evas_Coord w, Evas_Coord h, Evas_Coord d, const char *img_path) {
706 c = calloc(1, sizeof(Cube));
707 for (i = 0; i < 6; i++)
715 o = evas_object_image_add(e3d);
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);
722 evas_object_layer_set(o, LAYER_LANDMARK);
726 o = evas_object_rectangle_add(e3d);
728 evas_object_color_set(o, 0, 34, 119, 255);
729 evas_object_resize(o, 256, 256);
731 evas_object_layer_set(o, LAYER_LANDMARK);
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);
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);
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);
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);
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);
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);
774 void _cube_draw(Cube *c, Evas_Coord x, Evas_Coord y, Evas_Coord z,
775 double dx, double dy, double dz)
777 static Evas_Map *m = NULL;
778 int i, j, k, order[6], sorted, drawable, tmp_z;
784 if (!m) m = evas_map_new(4);
785 // smooth rendering disable
786 evas_map_smooth_set(m, 0);
788 for (i = 0; i < 6; i++)
792 for (j = 0; j < 4; j++)
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,
804 evas_map_point_color_set(m, j, 255, 255, 255, 255);
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);
810 evas_map_util_3d_rotate(m, 0, -camera.angle, 0,
811 (W_WIDTH / 2), (W_NAVI_HEIGHT / 2), FRONT_SIDE_Z);
813 evas_map_util_3d_perspective(m, (W_WIDTH / 2), (W_NAVI_HEIGHT / 2) , Z0_POSITION, FOCUS_LENGTH);
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)) {
823 if (evas_map_util_clockwise_get(m))
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) {
830 evas_object_show(c->side[i].o);
834 #ifdef _USE_Z_LIMIT_FIX_
836 evas_object_hide(c->side[i].o);
841 evas_object_hide(c->side[i].o);
846 for (j = 0; j < 4; j++) {
847 evas_map_point_coord_get(m, j, NULL, NULL, &(tz[j]));
850 mz[i] = (tz[0] + tz[1] + tz[2] + tz[3]) / 4;
853 evas_object_hide(c->side[i].o);
862 for (i = 0; i < 5; i++)
864 if (mz[order[i]] > mz[order[i + 1]])
867 order[i] = order[i + 1];
875 evas_object_raise(c->side[order[0]].o);
877 for (i = 1; i < 6; i++) {
878 evas_object_stack_below(c->side[order[i]].o, c->side[order[i - 1]].o);
882 void _cube_free(Cube *c)
886 for (i = 0; i < 6; i++) evas_object_del(c->side[i].o);
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);
895 evas_object_resize(goal_object, 256, 256);
896 evas_object_image_smooth_scale_set(goal_object, 0);
897 evas_object_hide(goal_object);
900 void goal_object_draw(Plane *plane) {
901 static Evas_Map *m3 = NULL;
904 if (plane->drawable == FALSE) {
905 evas_object_hide(goal_object);
909 if (!m3) m3 = evas_map_new(4);
910 evas_map_smooth_set(m3, 0);
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));
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);
926 evas_map_util_3d_perspective(m3, (W_WIDTH / 2), (W_NAVI_HEIGHT / 2) - 100,
927 Z0_POSITION, FOCUS_LENGTH);
929 evas_object_map_enable_set(goal_object, 1);
930 evas_object_map_set(goal_object, m3);
931 evas_object_show(goal_object);
933 evas_object_raise(goal_object);
936 void goal_object_free() {
937 evas_object_del(goal_object);
941 /******************************************************************************
942 * Functions for Calculate Route Points
943 ******************************************************************************/
945 #ifdef _USE_Z_LIMIT_FIX_
946 void check_route_darawable_by_z_limit_fix() {
948 Point limit_p0, limit_p1;
950 POINT(limit_p0, -100, ROUTE_PLANE_HEIGHT, Z_LIMIT, 0, 0);
951 POINT(limit_p1, 100, ROUTE_PLANE_HEIGHT, Z_LIMIT, 0, 0);
953 for (i = 0; i < route_draw_num - 1; i++) {
954 route[i].status = NONE;
955 route[i].enable_fix = TRUE;
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]);
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;
971 route[i].x = route_pre_fix[i].x;
972 route[i].z = route_pre_fix[i].z;
974 if (route_pre_fix[i].z < 0) {
975 if (i > 0 && (fixed_plane[i - 1]->drawable == FALSE)) {
976 route[i].enable_fix = FALSE;
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;
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;
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]);
993 route[i+1].x = route_pre_fix[i+1].x;
994 route[i+1].z = route_pre_fix[i+1].z;
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]);
1001 route[i+1].x = route_pre_fix[i+1].x;
1002 route[i+1].z = route_pre_fix[i+1].z;
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]);
1009 route[i].status = BOTH_POINT;
1013 route[i+1].x = route_pre_fix[i+1].x;
1014 route[i+1].z = route_pre_fix[i+1].z;
1017 if (route[i].status == BOTH_POINT) {
1018 fixed_plane[i]->drawable = FALSE;
1020 fixed_plane[i]->drawable = TRUE;
1029 void calc_route_coord_by_camera() {
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);
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);
1049 #ifdef _USE_Z_LIMIT_FIX_
1050 goal_square_length = calc_square_length(orizin, route_pre_fix[route_draw_num - 1]);
1052 goal_square_length = calc_square_length(orizin, route[route_draw_num - 1]);
1055 for (i = 0; i < route_draw_num - 1; i++) {
1056 fixed_plane[i]->drawable = TRUE;
1059 #ifdef _USE_Z_LIMIT_FIX_
1060 check_route_darawable_by_z_limit_fix();
1063 calc_route_distance();
1064 calc_route_distance_to_camera();
1065 check_route_drawable_by_distance_to_camera();
1068 void calc_route_distance_to_camera() {
1075 for ( i = 0; i < route_draw_num; i++) {
1076 route_square_length_camera[i] = calc_square_length(route[i], origin);
1081 void check_route_drawable_by_distance_to_camera() {
1082 int i, culling_route_count;
1083 double culling_length;
1085 culling_route_count = 0;
1086 culling_length = CULLING_LENGTH_ROUTE * CULLING_LENGTH_ROUTE;
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++;
1098 void calc_fixed_box_points_all() {
1100 for (i = 0; i < route_draw_num - 1; i++) {
1101 calc_fixed_box_points(i);
1105 void calc_fixed_box_points(int routenum) {
1106 #ifdef _USE_Z_LIMIT_FIX_
1107 if (routenum == 0 || route[routenum].enable_fix == FALSE) {
1109 if (routenum == 0) {
1111 fixed_plane[routenum]->pt[1] = base_plane[routenum]->pt[1];
1112 fixed_plane[routenum]->pt[2] = base_plane[routenum]->pt[2];
1114 fixed_plane[routenum]->pt[1] = calc_fix_point(routenum, 1);
1115 fixed_plane[routenum]->pt[2] = calc_fix_point(routenum, 2);
1118 #ifdef _USE_Z_LIMIT_FIX_
1119 if (routenum == route_draw_num - 2 || (route[routenum].status == ENDING_POINT || route[routenum].status == BOTH_POINT)) {
1121 if (routenum == route_draw_num - 2) {
1123 fixed_plane[routenum]->pt[0] = base_plane[routenum]->pt[0];
1124 fixed_plane[routenum]->pt[3] = base_plane[routenum]->pt[3];
1126 fixed_plane[routenum]->pt[0] = calc_fix_point(routenum, 0);
1127 fixed_plane[routenum]->pt[3] = calc_fix_point(routenum, 3);
1131 Point calc_fix_point(int routenum, int point_index) {
1133 Point result, p0, p1;
1135 if (route_distance[routenum] > (BOX_FIXED_LENGTH * 2)) {
1136 m = (route_distance[routenum] - BOX_FIXED_LENGTH) / route_distance[routenum];
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);
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;
1156 p0.x = base_plane[routenum]->pt[2].x;
1157 p0.z = base_plane[routenum]->pt[2].z;
1160 result.x = (m * (p1.x - p0.x)) + p0.x;
1161 result.z = (m * (p1.z - p0.z)) + p0.z;
1166 void draw_curve(int routenum) {
1171 Point curve_point1[ROUTE_DIVISION_NUMBER+1];
1172 Point curve_point2[ROUTE_DIVISION_NUMBER+1];
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);
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);
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);
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;
1191 if (curve_point2[i].z < Z_LIMIT) {
1192 curve_point2[i].z = Z_LIMIT;
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);
1202 _plane_draw(division_plane[routenum][i]);
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");
1217 void calc_bezier_points(Point p0, Point p1, Point p2, Point *points) {
1222 for(i = 0; i < ROUTE_DIVISION_NUMBER + 1; i++) {
1223 b = (double)i / ROUTE_DIVISION_NUMBER;
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;
1229 POINT(p, x, ROUTE_PLANE_HEIGHT, z, 0, 0);
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);
1238 void calc_base_box_points_all() {
1240 for (i = 0; i < route_draw_num - 1; i++) {
1241 calc_base_box_points(i);
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];
1253 #ifdef _E3D_H_OUTPUT_PRINT_
1254 uim_debug("th = %f\n", (th*180/M_PI));
1257 PLANE_POINT(base_plane[routenum], 0,
1258 (cos(th)*dis - sin(th)*(-ROUTE_PLANE_WIDTH)) + route[routenum].x,
1260 (sin(th)*dis + cos(th)*(-ROUTE_PLANE_WIDTH)) + route[routenum].z, 0, 0);
1262 PLANE_POINT(base_plane[routenum], 1,
1263 (cos(th)*0 - sin(th)*(-ROUTE_PLANE_WIDTH)) + route[routenum].x,
1265 (sin(th)*0 + cos(th)*(-ROUTE_PLANE_WIDTH)) + route[routenum].z, 256, 0);
1267 PLANE_POINT(base_plane[routenum], 2,
1268 (cos(th)*0 - sin(th)*(ROUTE_PLANE_WIDTH)) + route[routenum].x,
1270 (sin(th)*0 + cos(th)*(ROUTE_PLANE_WIDTH)) + route[routenum].z, 256, 256);
1272 PLANE_POINT(base_plane[routenum], 3,
1273 (cos(th)*dis - sin(th)*(ROUTE_PLANE_WIDTH)) + route[routenum].x,
1275 (sin(th)*dis + cos(th)*(ROUTE_PLANE_WIDTH)) + route[routenum].z, 0, 256);
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);
1286 /******************************************************************************
1287 * Functions for Calculate Landmark Points
1288 ******************************************************************************/
1289 void calc_landmarks_coord_by_camera() {
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;
1300 /******************************************************************************
1301 * Functions for Calculate (Common)
1302 ******************************************************************************/
1303 void calc_route_distance() {
1305 for (i = 0; i < route_draw_num - 1; i++) {
1306 route_distance[i] = calc_length(route[i], route[i+1]);
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) );
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));
1320 /* rotate point (center is orizin, xz-plane) */
1321 void rotate_xz(Point src, Point *dist, double angle) {
1322 double rx, rz, sine, cosine;
1325 rad = (-angle) * M_PI / 180.0;
1330 rx = cosine * src.x - sine * src.z;
1331 rz = sine * src.x + cosine * src.z;
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;
1345 result_x = (p1.x + (p0.x - p1.x) * S1 / (S1 + S2));
1346 result_z = (p1.z + (p0.z - p1.z) * S1 / (S1 + S2));
1348 point->x = result_x;
1349 point->y = ROUTE_PLANE_HEIGHT;
1350 point->z = result_z;
1355 int length_comp(const void *pl0, const void *pl1) {
1356 PointLength point_length0 = *(PointLength *)pl0;
1357 PointLength point_length1 = *(PointLength *)pl1;
1359 if (point_length0.length == point_length1.length) {
1361 } else if (point_length0.length < point_length1.length) {