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
40 /*============================================================================*/
41 /* Function prototype for static(internal) functions */
42 /*============================================================================*/
43 static void load_route_from_websocket(char *p_in, size_t len);
44 static void calc_route_coord_from_lonlat();
45 static void load_landmarks_from_csv();
46 static void calc_landmarks_coord_from_lonlat();
47 static const char *landmark_img_path(int i);
48 static void _camera_pos(Evas_Coord x, Evas_Coord y, Evas_Coord z,
50 static Plane *_plane_new(Evas *e3d, Evas_Coord w, Evas_Coord d,
52 static void _plane_draw(Plane *plane);
53 static void _plane_free(Plane *plane);
54 static Cube *_cube_new(Evas *e3d, Evas_Coord w, Evas_Coord h, Evas_Coord d,
55 const char *img_path);
56 static void _cube_draw(Cube *c, Evas_Coord x, Evas_Coord y, Evas_Coord z,
57 double dx, double dy, double dz);
58 static void _cube_free(Cube *c);
59 static void goal_object_new(Evas *e3d);
60 static void goal_object_draw();
61 static void goal_object_free();
62 static void check_route_drawable_by_z_limit_fix();
63 static void calc_route_coord_by_camera();
64 static void calc_route_distance_to_camera();
65 static void check_route_drawable_by_distance_to_camera();
66 static void calc_fixed_box_points_all();
67 static void calc_fixed_box_points(int routenum);
68 static Point calc_fix_point(int routenum, int point_index);
69 static void draw_curve(int routenum);
70 static void calc_bezier_points(Point p0, Point p1, Point p2, Point *points);
71 static void calc_base_box_points_all();
72 static void calc_base_box_points(int routenum);
73 static void calc_landmarks_coord_by_camera();
74 static void calc_route_distance();
75 static double calc_length(Point p0, Point p1);
76 static double calc_square_length(Point p0, Point p1);
77 static void rotate_xz(Point src, Point *dist, double angle);
78 static void calc_intersection(Point p0, Point p1, Point p2, Point p3,
80 static int length_comp(const void *pl0, const void *pl1);
82 /*============================================================================*/
83 /* Tables and Valiables */
84 /*============================================================================*/
86 CameraGeocode camera_geocode;
94 /* Route Points and Planes */
96 CsvRoute csv_route[MAX_ROUTE_POINTS];
97 Point route_raw[MAX_DRAW_ROUTE_POINTS];
98 Point route[MAX_DRAW_ROUTE_POINTS];
99 double route_square_length_camera[MAX_DRAW_ROUTE_POINTS];
100 #ifdef _USE_Z_LIMIT_FIX_
101 Point route_pre_fix[MAX_DRAW_ROUTE_POINTS];
103 Plane *base_plane[MAX_DRAW_ROUTE_POINTS - 1];
104 Plane *fixed_plane[MAX_DRAW_ROUTE_POINTS - 1];
105 Plane *division_plane[MAX_DRAW_ROUTE_POINTS - 2][ROUTE_DIVISION_NUMBER];
107 double route_distance[MAX_DRAW_ROUTE_POINTS - 1];
108 Evas_Object *goal_object;
109 double goal_square_length;
111 /* Landmark Points and Cubes */
112 int landmark_data_count;
113 CsvLandmark csv_landmark[MAX_LANDMARK_POINTS];
114 Point landmark_raw[MAX_LANDMARK_POINTS];
115 Point landmark[MAX_LANDMARK_POINTS];
116 Cube *landmark_cube[MAX_LANDMARK_POINTS];
118 static const char *goal_img_path = IMAGES_DIR "/na_goal.png";
119 static const char *landmark_img_path_0 = IMAGES_DIR "/sg_m_01.png";
120 static const char *landmark_img_path_1 = IMAGES_DIR "/sg_m_02.png";
121 static const char *landmark_img_path_2 = IMAGES_DIR "/sg_m_03.png";
122 static const char *landmark_img_path_3 = IMAGES_DIR "/sg_m_04.png";
123 static const char *landmark_img_path_4 = IMAGES_DIR "/sg_m_05.png";
124 static const char *landmark_img_path_5 = IMAGES_DIR "/sg_m_06.png";
125 static const char *landmark_img_path_6 = IMAGES_DIR "/sg_m_07.png";
126 static const char *landmark_img_path_7 = IMAGES_DIR "/sg_m_08.png";
127 static const char *landmark_img_path_8 = IMAGES_DIR "/sg_l_01.png";
128 static const char *landmark_img_path_9 = IMAGES_DIR "/sg_l_02.png";
129 static const char *landmark_img_path_10 = IMAGES_DIR "/sg_l_03.png";
130 static const char *landmark_img_path_11 = IMAGES_DIR "/sg_l_04.png";
131 static const char *landmark_img_path_12 = IMAGES_DIR "/sg_l_05.png";
132 static const char *landmark_img_path_13 = IMAGES_DIR "/sg_l_06.png";
133 static const char *landmark_img_path_14 = IMAGES_DIR "/sg_l_07.png";
134 static const char *landmark_img_path_15 = IMAGES_DIR "/sg_l_08.png";
135 static const char *csv_landmark_path = RESOURCE_DIR "/landmark.csv";
137 /*============================================================================*/
139 /*============================================================================*/
140 /*--------------------------------------------------------------------------*/
143 * Initialization of 3D field-of-view information.
145 * @param[in] e3d 3D view Evas object
146 * @param[in] p_in receive message
147 * @param[in] len message size[BYTE]
150 /*--------------------------------------------------------------------------*/
151 void init_e3d(Evas *e3d, void *p_in, size_t len)
155 goal_square_length = 1000000000000000.0;
157 /***********************
159 ***********************/
161 /* load route data */
162 load_route_from_websocket((char *) p_in, len);
164 calc_route_coord_from_lonlat();
165 route_draw_num = route_data_count;
167 for (i = 0; i < route_draw_num - 1; i++) {
168 base_plane[i] = _plane_new(e3d, 100, 400, 170);
169 fixed_plane[i] = _plane_new(e3d, 100, 400, 170);
171 for (i = 0; i < route_draw_num - 2; i++) {
172 for (j = 0; j < ROUTE_DIVISION_NUMBER; j++) {
173 division_plane[i][j] = _plane_new(e3d, 10, 10, 10);
177 goal_object_new(e3d);
179 #ifndef _USE_Z_LIMIT_FIX_
180 calc_route_distance();
183 calc_route_coord_by_camera();
184 calc_base_box_points_all();
185 calc_fixed_box_points_all();
187 #ifdef _E3D_H_OUTPUT_PRINT_
188 uim_debug("Base Plane point0 = %f, %f, %f \n", base_plane[0]->pt[0].x,
189 base_plane[0]->pt[0].y, base_plane[0]->pt[0].z);
190 uim_debug("Base Plane point1 = %f, %f, %f \n", base_plane[0]->pt[1].x,
191 base_plane[0]->pt[1].y, base_plane[0]->pt[1].z);
192 uim_debug("Base Plane point2 = %f, %f, %f \n", base_plane[0]->pt[2].x,
193 base_plane[0]->pt[2].y, base_plane[0]->pt[2].z);
194 uim_debug("Base Plane point3 = %f, %f, %f \n", base_plane[0]->pt[3].x,
195 base_plane[0]->pt[3].y, base_plane[0]->pt[3].z);
198 /***********************
199 * Landmark Initialize *
200 ***********************/
202 /* load landmark data */
203 load_landmarks_from_csv();
204 calc_landmarks_coord_from_lonlat();
206 for (i = 0; i < landmark_data_count; i++) {
207 landmark_cube[i] = _cube_new(e3d, LANDMARK_WIDTH,
208 (LANDMARK_WIDTH / 3) * 4, LANDMARK_WIDTH,
209 landmark_img_path(csv_landmark[i].id));
212 /***********************************************************************/
215 uim_debug("Route CSV x = %f, z = %f\n",
216 csv_route[1].lon, csv_route[1].lat);
217 uim_debug("Route x = %f, z = %f\n", route_raw[1].x, route_raw[1].z);
218 uim_debug("Landmark CSV x = %f, z = %f\n",
219 csv_landmark[0].lon, csv_landmark[0].lat);
220 uim_debug("Landmark x = %f, z = %f\n",
221 landmark_raw[0].x, landmark_raw[0].z);
223 /***********************
224 * Camera Initialize *
225 ***********************/
226 _camera_pos(0, 0, 0, 0);
227 camera_geocode.lon = csv_route[0].lon;
228 camera_geocode.lat = csv_route[0].lat;
229 camera_geocode.dir = 0;
232 /***********************
233 * Map present location Initialize *
234 ***********************/
235 map_pos.lon = csv_route[0].lon;
236 map_pos.lat = csv_route[0].lat;
243 /*--------------------------------------------------------------------------*/
246 * 3D field-of-view object cleaning.
251 /*--------------------------------------------------------------------------*/
256 for (i = 0; i < route_draw_num - 1; i++) {
257 _plane_free(base_plane[i]);
258 _plane_free(fixed_plane[i]);
261 for (i = 0; i < route_draw_num - 2; i++) {
262 for (j = 0; j < ROUTE_DIVISION_NUMBER; j++) {
263 _plane_free(division_plane[i][j]);
267 for (i = 0; i < landmark_data_count; i++) {
268 _cube_free(landmark_cube[i]);
274 /*--------------------------------------------------------------------------*/
277 * 3D field-of-view route drawing.
279 * @param[in] e3d 3D view Evas object
282 /*--------------------------------------------------------------------------*/
283 void draw_route(Evas *e3d)
292 calc_route_coord_by_camera();
293 calc_base_box_points_all();
294 calc_fixed_box_points_all();
296 /***********************************************************************/
298 if (enable_navi == FALSE || set_route == FALSE) {
299 evas_object_hide(goal_object);
303 for (i = 0; i < route_draw_num - 1; i++) {
304 if (fixed_plane[i]->drawable == TRUE) {
305 _plane_draw(fixed_plane[i]);
308 evas_object_hide(fixed_plane[i]->o);
313 for (i = 0; i < route_draw_num - 2; i++) {
314 if (fixed_plane[i]->drawable == TRUE) {
318 for (j = 0; j < ROUTE_DIVISION_NUMBER; j++) {
319 evas_object_hide(division_plane[i][j]->o);
322 #ifdef _E3D_H_OUTPUT_PRINT_
323 uim_debug("curve draw. \n");
327 #ifdef _E3D_H_OUTPUT_PRINT_
328 uim_debug("route draw comp.\n");
331 goal_object_draw(fixed_plane[route_draw_num - 2]);
334 /*--------------------------------------------------------------------------*/
336 * @brief draw_landmark
337 * 3D field-of-view landmark drawing.
339 * @param[in] e3d 3D view Evas object
342 /*--------------------------------------------------------------------------*/
343 void draw_landmark(Evas *e3d)
345 static unsigned long frame_count = 0;
346 PointLength draw_order[landmark_data_count];
350 double culling_length;
352 if (enable_navi == FALSE) {
353 for (i = 0; i < landmark_data_count; i++) {
354 for (j = 0; j < 6; j++) {
355 evas_object_hide(landmark_cube[i]->side[j].o);
360 calc_landmarks_coord_by_camera();
362 camera_coord.x = camera.x;
363 camera_coord.z = camera.z;
365 /* calc landmark rotation */
367 (int) ((conf_data[LANDMARK_ROTATION] / (1.0 / TIME_INTERVAL_AR)) *
370 culling_length = CULLING_LENGTH_LANDMARKS * CULLING_LENGTH_LANDMARKS;
372 /* In order to draw from distant landmark. */
373 for (i = 0; i < landmark_data_count; i++) {
374 draw_order[i].index = i;
375 draw_order[i].length = calc_square_length(landmark[i], camera_coord);
378 if (culling_length < draw_order[i].length) {
379 draw_order[i].drawable = FALSE;
382 draw_order[i].drawable = TRUE;
386 qsort(draw_order, landmark_data_count, sizeof(PointLength), length_comp);
388 for (i = 0; i < landmark_data_count; i++) {
389 j = draw_order[i].index;
390 if (draw_order[i].drawable == TRUE) {
391 _cube_draw(landmark_cube[j], landmark[j].x, landmark[j].y,
392 landmark[j].z, 0, landmark_angle, 0);
395 for (k = 0; k < 6; k++) {
396 evas_object_hide(landmark_cube[j]->side[k].o);
404 /*--------------------------------------------------------------------------*/
406 * @brief calc_camera_coord
407 * Calculation of the coordinates of a camera.
412 /*--------------------------------------------------------------------------*/
413 void calc_camera_coord()
417 x = (camera_geocode.lon -
418 csv_route[0].lon) * LON_CONVERT * COORD_CONVERT_COEFFICIENT;
419 z = (camera_geocode.lat -
420 csv_route[0].lat) * LAT_CONVERT * COORD_CONVERT_COEFFICIENT;
422 _camera_pos(x, camera.y, z, (360.0 - (double) camera_geocode.dir));
426 /*--------------------------------------------------------------------------*/
428 * @brief load_route_from_websocket
429 * route information acquisition.
431 * @param[in] p_in receive message
432 * @param[in] len message size[BYTE]
435 /*--------------------------------------------------------------------------*/
436 static void load_route_from_websocket(char *p_in, size_t len)
441 route_data_count = 0;
444 while (index < len) {
446 if ((strlen(&p_in[index]) >= strlen(ROUTE_START_TAG)) &&
447 (strncmp(&p_in[index], ROUTE_START_TAG, strlen(ROUTE_START_TAG))
449 index += strlen(ROUTE_START_TAG);
452 else if ((strlen(&p_in[index]) >= strlen(ROUTE_END_TAG)) &&
454 (&p_in[index], ROUTE_START_TAG,
455 strlen(ROUTE_END_TAG)) == 0)) {
459 else if ((strlen(&p_in[index]) >= strlen(BR_TAG)) &&
460 (strncmp(&p_in[index], BR_TAG, strlen(BR_TAG)) == 0)) {
461 index += strlen(BR_TAG);
466 memset(str, 0, sizeof(str));
467 while ((p_in[index] != ',') && (index < len)) {
468 str[strlen(str)] = p_in[index];
471 csv_route[route_data_count].lon = atof(str);
475 memset(str, 0, sizeof(str));
476 while ((p_in[index] != '<') && (index < len)) {
477 str[strlen(str)] = p_in[index];
480 csv_route[route_data_count].lat = atof(str);
484 if (route_data_count > 0) {
489 /*--------------------------------------------------------------------------*/
491 * @brief calc_route_coord_from_lonlat
492 * The coordinates of a route are calculated from
493 * latitude and longitude.
498 /*--------------------------------------------------------------------------*/
499 static void calc_route_coord_from_lonlat()
503 for (i = 0; i < route_data_count; i++) {
506 csv_route[0].lon) * LON_CONVERT * COORD_CONVERT_COEFFICIENT;
509 csv_route[0].lat) * LAT_CONVERT * COORD_CONVERT_COEFFICIENT;
510 POINT(route_raw[i], x, ROUTE_PLANE_HEIGHT, z, 0, 0);
514 /*--------------------------------------------------------------------------*/
516 * @brief load_landmarks_from_csv
517 * The information on a landmark is loaded from a CSV file.
522 /*--------------------------------------------------------------------------*/
523 static void load_landmarks_from_csv()
528 if ((fp = fopen(csv_landmark_path, "r")) == NULL) {
529 fprintf(stderr, "%s\n", "Error : can't open file.(landmark.csv)\n");
530 landmark_data_count = 0;
535 while (fscanf(fp, "%lf,%lf,%hhd",
536 &csv_landmark[i].lon, &csv_landmark[i].lat,
537 &csv_landmark[i].id) != EOF) {
540 landmark_data_count = i;
545 for (i = 0; i < landmark_data_count; i++) {
546 uim_debug("landmark data %d : %.4f, %.4f, %d\n",
547 i, csv_landmark[i].lon, csv_landmark[i].lat,
551 uim_debug("landmark data read. count = %d\n", landmark_data_count);
556 /*--------------------------------------------------------------------------*/
558 * @brief calc_landmarks_coord_from_lonlat
559 * The coordinates of a landmark are calculated from
560 * latitude and longitude.
565 /*--------------------------------------------------------------------------*/
566 static void calc_landmarks_coord_from_lonlat()
570 for (i = 0; i < landmark_data_count; i++) {
572 (csv_landmark[i].lon -
573 csv_route[0].lon) * LON_CONVERT * COORD_CONVERT_COEFFICIENT;
575 (csv_landmark[i].lat -
576 csv_route[0].lat) * LAT_CONVERT * COORD_CONVERT_COEFFICIENT;
577 POINT(landmark_raw[i], x, conf_data[LANDMARK_POSITION], z, 0, 0);
581 /*--------------------------------------------------------------------------*/
583 * @brief landmark_img_path
584 * Image file path acquisition of a landmark.
586 * @param[in] landmark image file path identifier
587 * @return file path address
588 * @retval > 0 success
591 /*--------------------------------------------------------------------------*/
592 static const char *landmark_img_path(int i)
594 const char *img_path;
597 img_path = landmark_img_path_0;
600 img_path = landmark_img_path_1;
603 img_path = landmark_img_path_2;
606 img_path = landmark_img_path_3;
609 img_path = landmark_img_path_4;
612 img_path = landmark_img_path_5;
615 img_path = landmark_img_path_6;
618 img_path = landmark_img_path_7;
621 img_path = landmark_img_path_8;
624 img_path = landmark_img_path_9;
627 img_path = landmark_img_path_10;
630 img_path = landmark_img_path_11;
633 img_path = landmark_img_path_12;
636 img_path = landmark_img_path_13;
639 img_path = landmark_img_path_14;
642 img_path = landmark_img_path_15;
649 /*--------------------------------------------------------------------------*/
652 * The setup of camera position.
654 * @param[in] x x coordinates
655 * @param[in] y y coordinates
656 * @param[in] z z coordinates
657 * @param[in] angle angle
660 /*--------------------------------------------------------------------------*/
662 _camera_pos(Evas_Coord x, Evas_Coord y, Evas_Coord z, double angle)
664 // uim_debug("camera_pos x:%d y:%d z:%d angle:%d", x, y, z, angle);
668 camera.angle = angle;
671 /******************************************************************************
672 * Functions for Drawing on Evas
673 ******************************************************************************/
674 /*--------------------------------------------------------------------------*/
679 * @param[in] e3d 3D view Evas object
682 * @param[in] h Height
683 * @return plane address
684 * @retval > 0 success
687 /*--------------------------------------------------------------------------*/
688 static Plane *_plane_new(Evas *e3d, Evas_Coord w, Evas_Coord d, Evas_Coord h)
693 plane = calloc(1, sizeof(Plane));
698 o = evas_object_rectangle_add(e3d);
701 evas_object_resize(o, 256, 256);
704 SIDE_POINT(0, -w, -h, d, 0, 0);
705 SIDE_POINT(1, w, -h, d, 256, 0);
706 SIDE_POINT(2, w, -h, -d, 256, 256);
707 SIDE_POINT(3, -w, -h, -d, 0, 256);
709 evas_object_layer_set(o, LAYER_ROUTE);
711 plane->drawable = TRUE;
716 /*--------------------------------------------------------------------------*/
721 * @param[in] plane plane address
724 /*--------------------------------------------------------------------------*/
725 static void _plane_draw(Plane *plane)
727 static Evas_Map *m2 = NULL;
731 m2 = evas_map_new(4);
732 evas_map_smooth_set(m2, 0);
734 #ifdef _USE_Z_LIMIT_FIX_
735 if (plane->drawable == FALSE) {
736 evas_object_hide(plane->o);
741 evas_object_color_set(plane->o, 51, 178, 0, 179);
744 evas_map_point_coord_set(m2, 0,
745 (Evas_Coord) (plane->pt[0].x + ((W_WIDTH) / 4)),
746 (Evas_Coord) (plane->pt[0].y +
747 ((W_NAVI_HEIGHT) / 4)),
748 (Evas_Coord) (plane->pt[0].z + FRONT_SIDE_Z));
749 evas_map_point_coord_set(m2, 1,
750 (Evas_Coord) (plane->pt[1].x + ((W_WIDTH) / 4)),
751 (Evas_Coord) (plane->pt[1].y +
752 ((W_NAVI_HEIGHT) / 4)),
753 (Evas_Coord) (plane->pt[1].z + FRONT_SIDE_Z));
754 evas_map_point_coord_set(m2, 2,
755 (Evas_Coord) (plane->pt[2].x + ((W_WIDTH) / 4)),
756 (Evas_Coord) (plane->pt[2].y +
757 ((W_NAVI_HEIGHT) / 4)),
758 (Evas_Coord) (plane->pt[2].z + FRONT_SIDE_Z));
759 evas_map_point_coord_set(m2, 3,
760 (Evas_Coord) (plane->pt[3].x + ((W_WIDTH) / 4)),
761 (Evas_Coord) (plane->pt[3].y +
762 ((W_NAVI_HEIGHT) / 4)),
763 (Evas_Coord) (plane->pt[3].z + FRONT_SIDE_Z));
765 for (i = 0; i < 4; i++) {
766 evas_map_point_image_uv_set(m2, i, plane->pt[i].u, plane->pt[i].v);
767 evas_map_point_color_set(m2, i, 255, 255, 255, 255);
769 evas_map_util_3d_perspective(m2, (W_WIDTH / 4), (W_NAVI_HEIGHT / 4) - 100,
770 Z0_POSITION, FOCUS_LENGTH / 2);
771 evas_object_map_enable_set(plane->o, 1);
772 evas_object_map_set(plane->o, m2);
774 evas_object_show(plane->o);
782 /*--------------------------------------------------------------------------*/
785 * plane object release.
787 * @param[in] plane plane address
790 /*--------------------------------------------------------------------------*/
791 static void _plane_free(Plane *plane)
793 evas_object_del(plane->o);
797 /*--------------------------------------------------------------------------*/
802 * @param[in] e3d 3D view Evas object
804 * @param[in] h Height
806 * @param[in] img_path image file path address
807 * @return cube address
808 * @retval > 0 success
811 /*--------------------------------------------------------------------------*/
812 static Cube *_cube_new(Evas *e3d, Evas_Coord w, Evas_Coord h, Evas_Coord d,
813 const char *img_path)
821 c = calloc(1, sizeof(Cube));
822 for (i = 0; i < 6; i++) {
829 o = evas_object_image_add(e3d);
831 evas_object_image_file_set(o, img_path, NULL);
832 evas_object_image_fill_set(o, 0, 0, 256, 256);
833 evas_object_resize(o, 256, 256);
834 evas_object_image_smooth_scale_set(o, 0);
836 evas_object_layer_set(o, LAYER_LANDMARK);
840 o = evas_object_rectangle_add(e3d);
842 evas_object_color_set(o, 0, 34, 119, 255);
843 evas_object_resize(o, 256, 256);
845 evas_object_layer_set(o, LAYER_LANDMARK);
850 /* First Plane (Front) */
851 CUBE_POINT(0, 0, -w, -h, -d, 0, 0);
852 CUBE_POINT(0, 1, w, -h, -d, 600, 0);
853 CUBE_POINT(0, 2, w, h, -d, 600, 800);
854 CUBE_POINT(0, 3, -w, h, -d, 0, 800);
856 /* Second Plane (Right side) */
857 CUBE_POINT(1, 0, w, -h, -d, 0, 0);
858 CUBE_POINT(1, 1, w, -h, d, 600, 0);
859 CUBE_POINT(1, 2, w, h, d, 600, 800);
860 CUBE_POINT(1, 3, w, h, -d, 0, 800);
862 /* Third Plane (Back) */
863 CUBE_POINT(2, 0, w, -h, d, 0, 0);
864 CUBE_POINT(2, 1, -w, -h, d, 600, 0);
865 CUBE_POINT(2, 2, -w, h, d, 600, 800);
866 CUBE_POINT(2, 3, w, h, d, 0, 800);
868 /* Fourth Plane (Left side) */
869 CUBE_POINT(3, 0, -w, -h, d, 0, 0);
870 CUBE_POINT(3, 1, -w, -h, -d, 600, 0);
871 CUBE_POINT(3, 2, -w, h, -d, 600, 800);
872 CUBE_POINT(3, 3, -w, h, d, 0, 800);
874 /* Fifth Plane (Top side) */
875 CUBE_POINT(4, 0, -w, -h, d, 0, 0);
876 CUBE_POINT(4, 1, w, -h, d, 256, 0);
877 CUBE_POINT(4, 2, w, -h, -d, 256, 256);
878 CUBE_POINT(4, 3, -w, -h, -d, 0, 256);
880 /* Sixth Plane (Under side) */
881 CUBE_POINT(5, 0, -w, h, -d, 0, 0);
882 CUBE_POINT(5, 1, w, h, -d, 256, 0);
883 CUBE_POINT(5, 2, w, h, d, 256, 256);
884 CUBE_POINT(5, 3, -w, h, d, 0, 256);
889 /*--------------------------------------------------------------------------*/
894 * @param[in] c cube address
895 * @param[in] x x coordinates
896 * param[in] y y coordinates
897 * @param[in] z z coordinates
898 * @param[in] dx amount of degrees from 0.0 to 360.0 to rotate around X axis.
899 * @param[in] dy amount of degrees from 0.0 to 360.0 to rotate around Y axis.
900 * @param[in] dz amount of degrees from 0.0 to 360.0 to rotate around Z axis.
903 /*--------------------------------------------------------------------------*/
905 _cube_draw(Cube *c, Evas_Coord x, Evas_Coord y, Evas_Coord z,
906 double dx, double dy, double dz)
908 static Evas_Map *m = NULL;
909 int i, j, order[6], sorted, drawable, tmp_z;
917 /* smooth rendering disable */
918 evas_map_smooth_set(m, 0);
920 for (i = 0; i < 6; i++) {
923 for (j = 0; j < 4; j++) {
925 evas_map_point_coord_set(m, j,
926 c->side[i].pt[j].x + ((W_WIDTH) / 4) +
929 (W_NAVI_HEIGHT / 2) + y - camera.y,
930 c->side[i].pt[j].z + z - camera.z +
932 if (!(i == 4 || i == 5)) {
933 evas_map_point_image_uv_set(m, j,
937 evas_map_point_color_set(m, j, 255, 255, 255, 255);
939 evas_map_util_3d_rotate(m, dx, dy, dz,
940 ((W_WIDTH) / 4) + x - camera.x,
941 (W_NAVI_HEIGHT / 2) + y - camera.y,
942 z - camera.z + FRONT_SIDE_Z);
944 /* for camera angle */
945 evas_map_util_3d_rotate(m, 0, -camera.angle, 0,
946 ((W_WIDTH) / 4), (W_NAVI_HEIGHT / 2),
949 evas_map_util_3d_perspective(m, ((W_WIDTH) / 4), (W_NAVI_HEIGHT / 2),
950 Z0_POSITION, FOCUS_LENGTH / 2);
952 #ifdef _USE_Z_LIMIT_FIX_
953 for (j = 0; j < 4; j++) {
954 evas_map_point_coord_get(m, j, NULL, NULL, &tmp_z);
955 if (tmp_z < (Z_LIMIT + FRONT_SIDE_Z)) {
960 if (evas_map_util_clockwise_get(m)) {
961 evas_object_map_enable_set(c->side[i].o, 1);
962 evas_object_map_set(c->side[i].o, m);
963 #ifdef _USE_Z_LIMIT_FIX_
964 if (drawable == TRUE) {
966 evas_object_show(c->side[i].o);
970 #ifdef _USE_Z_LIMIT_FIX_
973 evas_object_hide(c->side[i].o);
978 evas_object_hide(c->side[i].o);
983 for (j = 0; j < 4; j++) {
984 evas_map_point_coord_get(m, j, NULL, NULL, &(tz[j]));
987 mz[i] = (tz[0] + tz[1] + tz[2] + tz[3]) / 4;
990 evas_object_hide(c->side[i].o);
998 for (i = 0; i < 5; i++) {
999 if (mz[order[i]] > mz[order[i + 1]]) {
1001 order[i] = order[i + 1];
1009 evas_object_raise(c->side[order[0]].o);
1011 for (i = 1; i < 6; i++) {
1012 evas_object_stack_below(c->side[order[i]].o, c->side[order[i - 1]].o);
1016 /*--------------------------------------------------------------------------*/
1019 * cube object release.
1021 * @param[in] c cube address
1024 /*--------------------------------------------------------------------------*/
1025 static void _cube_free(Cube *c)
1029 for (i = 0; i < 6; i++)
1030 evas_object_del(c->side[i].o);
1034 /*--------------------------------------------------------------------------*/
1036 * @brief goal_object_new
1039 * @param[in] e3d 3D view Evas object
1042 /*--------------------------------------------------------------------------*/
1043 static void goal_object_new(Evas *e3d)
1045 goal_object = evas_object_image_add(e3d);
1046 evas_object_image_file_set(goal_object, goal_img_path, NULL);
1047 evas_object_image_fill_set(goal_object, 0, 0, 600, 600);
1049 evas_object_resize(goal_object, 256, 256);
1050 evas_object_image_smooth_scale_set(goal_object, 0);
1051 evas_object_hide(goal_object);
1054 /*--------------------------------------------------------------------------*/
1056 * @brief goal_object_draw
1059 * @param[in] plane plane address
1062 /*--------------------------------------------------------------------------*/
1063 static void goal_object_draw(Plane *plane)
1065 static Evas_Map *m3 = NULL;
1067 if (plane->drawable == FALSE) {
1068 evas_object_hide(goal_object);
1073 m3 = evas_map_new(4);
1074 evas_map_smooth_set(m3, 0);
1076 evas_map_point_coord_set(m3, 0,
1077 (Evas_Coord) (plane->pt[0].x + ((W_WIDTH) / 4)),
1078 (Evas_Coord) (plane->pt[0].y +
1079 ((W_NAVI_HEIGHT) / 2)),
1080 (Evas_Coord) (plane->pt[0].z + FRONT_SIDE_Z));
1081 evas_map_point_coord_set(m3, 1,
1082 (Evas_Coord) (plane->pt[0].x + ((W_WIDTH) / 4)),
1083 (Evas_Coord) ((-1) * plane->pt[0].y +
1084 ((W_NAVI_HEIGHT) / 2)),
1085 (Evas_Coord) (plane->pt[0].z + FRONT_SIDE_Z));
1086 evas_map_point_coord_set(m3, 2,
1087 (Evas_Coord) (plane->pt[3].x + ((W_WIDTH) / 4)),
1088 (Evas_Coord) ((-1) * plane->pt[3].y +
1089 ((W_NAVI_HEIGHT) / 2)),
1090 (Evas_Coord) (plane->pt[3].z + FRONT_SIDE_Z));
1091 evas_map_point_coord_set(m3, 3,
1092 (Evas_Coord) (plane->pt[3].x + ((W_WIDTH) / 4)),
1093 (Evas_Coord) (plane->pt[3].y +
1094 ((W_NAVI_HEIGHT) / 2)),
1095 (Evas_Coord) (plane->pt[3].z + FRONT_SIDE_Z));
1097 evas_map_point_image_uv_set(m3, 0, 600, 0);
1098 evas_map_point_image_uv_set(m3, 1, 600, 600);
1099 evas_map_point_image_uv_set(m3, 2, 0, 600);
1100 evas_map_point_image_uv_set(m3, 3, 0, 0);
1102 evas_map_util_3d_perspective(m3, (W_WIDTH / 4), (W_NAVI_HEIGHT / 2) - 100,
1103 Z0_POSITION, FOCUS_LENGTH / 2);
1105 evas_object_map_enable_set(goal_object, 1);
1106 evas_object_map_set(goal_object, m3);
1107 evas_object_show(goal_object);
1109 evas_object_raise(goal_object);
1112 /*--------------------------------------------------------------------------*/
1114 * @brief goal_object_free
1115 * goal object release.
1120 /*--------------------------------------------------------------------------*/
1121 static void goal_object_free()
1123 evas_object_del(goal_object);
1126 /******************************************************************************
1127 * Functions for Calculate Route Points
1128 ******************************************************************************/
1130 #ifdef _USE_Z_LIMIT_FIX_
1131 /*--------------------------------------------------------------------------*/
1133 * @brief check_route_drawable_by_z_limit_fix
1134 * route check drawable by z limit fix.
1139 /*--------------------------------------------------------------------------*/
1140 static void check_route_drawable_by_z_limit_fix()
1143 Point limit_p0, limit_p1;
1145 POINT(limit_p0, -100, ROUTE_PLANE_HEIGHT, Z_LIMIT, 0, 0);
1146 POINT(limit_p1, 100, ROUTE_PLANE_HEIGHT, Z_LIMIT, 0, 0);
1148 for (i = 0; i < route_draw_num - 1; i++) {
1149 route[i].status = NONE;
1150 route[i].enable_fix = TRUE;
1152 if (route_pre_fix[i].z < Z_LIMIT) {
1153 route[i].status = STARTING_POINT;
1154 route[i].enable_fix = FALSE;
1155 if (route_pre_fix[i + 1].z >= Z_LIMIT) {
1156 calc_intersection(limit_p0, limit_p1, route_pre_fix[i],
1157 route_pre_fix[i + 1], &route[i]);
1160 if (!(i > 0 && (route[i - 1].status == ENDING_POINT))) {
1161 route[i].status = BOTH_POINT;
1162 route[i].x = route_pre_fix[i].x;
1163 route[i].z = route_pre_fix[i].z;
1169 route[i].x = route_pre_fix[i].x;
1170 route[i].z = route_pre_fix[i].z;
1172 if (route_pre_fix[i].z < 0) {
1173 if (i > 0 && (fixed_plane[i - 1]->drawable == FALSE)) {
1174 route[i].enable_fix = FALSE;
1179 if (route_pre_fix[i + 1].z < Z_LIMIT) {
1180 if ((route[i].status == STARTING_POINT)
1181 || (route[i].status == BOTH_POINT)) {
1182 route[i].status = BOTH_POINT;
1184 if ((i + 1) == route_draw_num - 1) {
1185 route[i + 1].x = route_pre_fix[i + 1].x;
1186 route[i + 1].z = route_pre_fix[i + 1].z;
1189 if (route_pre_fix[i + 2].z >= Z_LIMIT) {
1190 calc_intersection(limit_p0, limit_p1,
1191 route_pre_fix[i + 1],
1192 route_pre_fix[i + 2],
1197 route[i + 1].x = route_pre_fix[i + 1].x;
1198 route[i + 1].z = route_pre_fix[i + 1].z;
1202 if ((i < route_draw_num - 1)
1203 && (route_pre_fix[i + 2].z > Z_LIMIT)) {
1204 calc_intersection(limit_p0, limit_p1,
1205 route_pre_fix[i + 1],
1206 route_pre_fix[i + 2], &route[i + 1]);
1209 route[i + 1].x = route_pre_fix[i + 1].x;
1210 route[i + 1].z = route_pre_fix[i + 1].z;
1214 if (route_pre_fix[i].z >= Z_LIMIT) {
1215 route[i].status = ENDING_POINT;
1216 calc_intersection(limit_p0, limit_p1, route_pre_fix[i],
1217 route_pre_fix[i + 1], &route[i + 1]);
1220 route[i].status = BOTH_POINT;
1225 route[i + 1].x = route_pre_fix[i + 1].x;
1226 route[i + 1].z = route_pre_fix[i + 1].z;
1229 if (route[i].status == BOTH_POINT) {
1230 fixed_plane[i]->drawable = FALSE;
1233 fixed_plane[i]->drawable = TRUE;
1240 /*--------------------------------------------------------------------------*/
1242 * @brief calc_route_coord_by_camera
1243 * The coordinates of a route are calculated with a camera.
1248 /*--------------------------------------------------------------------------*/
1249 static void calc_route_coord_by_camera()
1254 for (i = 0; i < route_draw_num; i++) {
1255 #ifdef _USE_Z_LIMIT_FIX_
1256 route_pre_fix[i].x = route_raw[i].x - camera.x;
1257 route_pre_fix[i].z = route_raw[i].z - camera.z;
1258 rotate_xz(route_pre_fix[i], &route_pre_fix[i], camera.angle);
1260 route[i].x = route_raw[i].x - camera.x;
1261 route[i].z = route_raw[i].z - camera.z;
1262 rotate_xz(route[i], &route[i], camera.angle);
1268 #ifdef _USE_Z_LIMIT_FIX_
1269 goal_square_length =
1270 calc_square_length(orizin, route_pre_fix[route_draw_num - 1]);
1272 goal_square_length =
1273 calc_square_length(orizin, route[route_draw_num - 1]);
1276 for (i = 0; i < route_draw_num - 1; i++) {
1277 fixed_plane[i]->drawable = TRUE;
1280 #ifdef _USE_Z_LIMIT_FIX_
1281 check_route_drawable_by_z_limit_fix();
1284 calc_route_distance();
1285 calc_route_distance_to_camera();
1286 check_route_drawable_by_distance_to_camera();
1289 /*--------------------------------------------------------------------------*/
1291 * @brief calc_route_distance_to_camera
1292 * The route distance to a camera is calculated.
1297 /*--------------------------------------------------------------------------*/
1298 static void calc_route_distance_to_camera()
1306 for (i = 0; i < route_draw_num; i++) {
1307 route_square_length_camera[i] = calc_square_length(route[i], origin);
1311 /*--------------------------------------------------------------------------*/
1313 * @brief check_route_drawable_by_distance_to_camera
1314 * route check drawable by distance to camera.
1319 /*--------------------------------------------------------------------------*/
1320 static void check_route_drawable_by_distance_to_camera()
1322 int i, culling_route_count;
1323 double culling_length;
1325 culling_route_count = 0;
1326 culling_length = CULLING_LENGTH_ROUTE * CULLING_LENGTH_ROUTE;
1328 for (i = 0; i < route_draw_num - 1; i++) {
1329 if ((route_square_length_camera[i] > culling_length) &&
1330 (route_square_length_camera[i + 1] > culling_length)) {
1331 fixed_plane[i]->drawable = FALSE;
1332 culling_route_count++;
1337 /*--------------------------------------------------------------------------*/
1339 * @brief calc_fixed_box_points_all
1340 * All fixed box points are calculated.
1345 /*--------------------------------------------------------------------------*/
1346 static void calc_fixed_box_points_all()
1349 for (i = 0; i < route_draw_num - 1; i++) {
1350 calc_fixed_box_points(i);
1354 /*--------------------------------------------------------------------------*/
1356 * @brief calc_fixed_box_points
1357 * fixed box points are calculated.
1359 * @param[in] routenum route number
1362 /*--------------------------------------------------------------------------*/
1363 static void calc_fixed_box_points(int routenum)
1365 #ifdef _USE_Z_LIMIT_FIX_
1366 if (routenum == 0 || route[routenum].enable_fix == FALSE) {
1368 if (routenum == 0) {
1370 fixed_plane[routenum]->pt[1] = base_plane[routenum]->pt[1];
1371 fixed_plane[routenum]->pt[2] = base_plane[routenum]->pt[2];
1374 fixed_plane[routenum]->pt[1] = calc_fix_point(routenum, 1);
1375 fixed_plane[routenum]->pt[2] = calc_fix_point(routenum, 2);
1378 #ifdef _USE_Z_LIMIT_FIX_
1379 if (routenum == route_draw_num - 2
1380 || (route[routenum].status == ENDING_POINT
1381 || route[routenum].status == BOTH_POINT)) {
1383 if (routenum == route_draw_num - 2) {
1385 fixed_plane[routenum]->pt[0] = base_plane[routenum]->pt[0];
1386 fixed_plane[routenum]->pt[3] = base_plane[routenum]->pt[3];
1389 fixed_plane[routenum]->pt[0] = calc_fix_point(routenum, 0);
1390 fixed_plane[routenum]->pt[3] = calc_fix_point(routenum, 3);
1394 /*--------------------------------------------------------------------------*/
1396 * @brief calc_fix_point
1397 * fixed point is calculated.
1399 * @param[in] routenum route number
1400 * @param[in] point_index point index
1403 /*--------------------------------------------------------------------------*/
1404 static Point calc_fix_point(int routenum, int point_index)
1407 Point result, p0, p1;
1409 if (route_distance[routenum] > (BOX_FIXED_LENGTH * 2)) {
1410 m = (route_distance[routenum] -
1411 BOX_FIXED_LENGTH) / route_distance[routenum];
1417 POINT(result, 0, ROUTE_PLANE_HEIGHT, 0, 0, 0);
1418 POINT(p0, 0, ROUTE_PLANE_HEIGHT, 0, 0, 0);
1419 POINT(p1, base_plane[routenum]->pt[point_index].x, ROUTE_PLANE_HEIGHT,
1420 base_plane[routenum]->pt[point_index].z, 0, 0);
1422 if (point_index == 0) {
1423 p0.x = base_plane[routenum]->pt[1].x;
1424 p0.z = base_plane[routenum]->pt[1].z;
1426 else if (point_index == 1) {
1427 p0.x = base_plane[routenum]->pt[0].x;
1428 p0.z = base_plane[routenum]->pt[0].z;
1430 else if (point_index == 2) {
1431 p0.x = base_plane[routenum]->pt[3].x;
1432 p0.z = base_plane[routenum]->pt[3].z;
1435 p0.x = base_plane[routenum]->pt[2].x;
1436 p0.z = base_plane[routenum]->pt[2].z;
1439 result.x = (m * (p1.x - p0.x)) + p0.x;
1440 result.z = (m * (p1.z - p0.z)) + p0.z;
1445 /*--------------------------------------------------------------------------*/
1450 * @param[in] routenum route number
1453 /*--------------------------------------------------------------------------*/
1454 static void draw_curve(int routenum)
1460 Point curve_point1[ROUTE_DIVISION_NUMBER + 1];
1461 Point curve_point2[ROUTE_DIVISION_NUMBER + 1];
1463 calc_intersection(base_plane[routenum]->pt[0],
1464 base_plane[routenum]->pt[1],
1465 base_plane[routenum + 1]->pt[0],
1466 base_plane[routenum + 1]->pt[1], &con_point1);
1467 calc_intersection(base_plane[routenum]->pt[3],
1468 base_plane[routenum]->pt[2],
1469 base_plane[routenum + 1]->pt[3],
1470 base_plane[routenum + 1]->pt[2], &con_point2);
1472 #ifdef _E3D_H_OUTPUT_PRINT_
1473 uim_debug("con_point1 x = %f, z = %f \n", con_point1.x, con_point1.z);
1474 uim_debug("con_point2 x = %f, z = %f \n", con_point2.x, con_point2.z);
1477 calc_bezier_points(fixed_plane[routenum]->pt[0], con_point1,
1478 fixed_plane[routenum + 1]->pt[1], curve_point1);
1479 calc_bezier_points(fixed_plane[routenum]->pt[3], con_point2,
1480 fixed_plane[routenum + 1]->pt[2], curve_point2);
1482 for (i = 0; i < (ROUTE_DIVISION_NUMBER) + 1; i++) {
1483 if (curve_point1[i].z < Z_LIMIT) {
1484 curve_point1[i].z = Z_LIMIT;
1487 if (curve_point2[i].z < Z_LIMIT) {
1488 curve_point2[i].z = Z_LIMIT;
1492 for (i = 0; i < ROUTE_DIVISION_NUMBER; i++) {
1493 PLANE_POINT(division_plane[routenum][i], 0, curve_point1[i + 1].x,
1494 ROUTE_PLANE_HEIGHT, curve_point1[i + 1].z, 0, 0);
1495 PLANE_POINT(division_plane[routenum][i], 1, curve_point1[i].x,
1496 ROUTE_PLANE_HEIGHT, curve_point1[i].z, 256, 0);
1497 PLANE_POINT(division_plane[routenum][i], 2, curve_point2[i].x,
1498 ROUTE_PLANE_HEIGHT, curve_point2[i].z, 256, 256);
1499 PLANE_POINT(division_plane[routenum][i], 3, curve_point2[i + 1].x,
1500 ROUTE_PLANE_HEIGHT, curve_point2[i + 1].z, 0, 256);
1502 _plane_draw(division_plane[routenum][i]);
1504 #ifdef _E3D_H_OUTPUT_PRINT_
1506 ("Division Plane %d : x0 = %f, z0 = %f, x1 = %f, z1 = %f, x2 = %f, z2 = %f, x3 = %f, z3 = %f\n",
1507 routenum, division_plane[routenum][i]->pt[0].x,
1508 division_plane[routenum][i]->pt[0].z,
1509 division_plane[routenum][i]->pt[1].x,
1510 division_plane[routenum][i]->pt[1].z,
1511 division_plane[routenum][i]->pt[2].x,
1512 division_plane[routenum][i]->pt[2].z,
1513 division_plane[routenum][i]->pt[3].x,
1514 division_plane[routenum][i]->pt[3].z);
1515 uim_debug("Draw division plane.\n");
1521 /*--------------------------------------------------------------------------*/
1523 * @brief calc_bezier_points
1524 * bezier point is calculated.
1526 * @param[in] p0 point 0
1527 * @param[in] p1 point 1
1528 * @param[in] p2 point 2
1529 * @param[out] points point address
1532 /*--------------------------------------------------------------------------*/
1533 static void calc_bezier_points(Point p0, Point p1, Point p2, Point *points)
1539 for (i = 0; i < ROUTE_DIVISION_NUMBER + 1; i++) {
1540 b = (double) i / ROUTE_DIVISION_NUMBER;
1543 x = a * a * p0.x + 2 * a * b * p1.x + b * b * p2.x;
1544 z = a * a * p0.z + 2 * a * b * p1.z + b * b * p2.z;
1546 POINT(p, x, ROUTE_PLANE_HEIGHT, z, 0, 0);
1549 #ifdef _E3D_H_OUTPUT_PRINT_
1550 uim_debug("bezier_points %d : x = %f, y = %f, z = %f\n", i,
1551 points[i].x, points[i].y, points[i].z);
1556 /*--------------------------------------------------------------------------*/
1558 * @brief calc_base_box_points_all
1559 * All base box points are calculated.
1564 /*--------------------------------------------------------------------------*/
1565 static void calc_base_box_points_all()
1568 for (i = 0; i < route_draw_num - 1; i++) {
1569 calc_base_box_points(i);
1573 /*--------------------------------------------------------------------------*/
1575 * @brief calc_base_box_points
1576 * base box points are calculated.
1578 * @param[in] routenum route number
1581 /*--------------------------------------------------------------------------*/
1582 static void calc_base_box_points(int routenum)
1584 /* Determine the angle. */
1585 double th = atan2((route[routenum + 1].z - route[routenum].z),
1586 (route[routenum + 1].x - route[routenum].x));
1587 /* Determine the distance. */
1588 double dis = route_distance[routenum];
1590 #ifdef _E3D_H_OUTPUT_PRINT_
1591 uim_debug("th = %f\n", (th * 180 / M_PI));
1594 PLANE_POINT(base_plane[routenum], 0,
1595 (cos(th) * dis - sin(th) * (-ROUTE_PLANE_WIDTH)) +
1596 route[routenum].x, ROUTE_PLANE_HEIGHT,
1597 (sin(th) * dis + cos(th) * (-ROUTE_PLANE_WIDTH)) +
1598 route[routenum].z, 0, 0);
1600 PLANE_POINT(base_plane[routenum], 1,
1601 (cos(th) * 0 - sin(th) * (-ROUTE_PLANE_WIDTH)) +
1602 route[routenum].x, ROUTE_PLANE_HEIGHT,
1603 (sin(th) * 0 + cos(th) * (-ROUTE_PLANE_WIDTH)) +
1604 route[routenum].z, 256, 0);
1606 PLANE_POINT(base_plane[routenum], 2,
1607 (cos(th) * 0 - sin(th) * (ROUTE_PLANE_WIDTH)) +
1608 route[routenum].x, ROUTE_PLANE_HEIGHT,
1609 (sin(th) * 0 + cos(th) * (ROUTE_PLANE_WIDTH)) +
1610 route[routenum].z, 256, 256);
1612 PLANE_POINT(base_plane[routenum], 3,
1613 (cos(th) * dis - sin(th) * (ROUTE_PLANE_WIDTH)) +
1614 route[routenum].x, ROUTE_PLANE_HEIGHT,
1615 (sin(th) * dis + cos(th) * (ROUTE_PLANE_WIDTH)) +
1616 route[routenum].z, 0, 256);
1618 #ifdef _E3D_H_OUTPUT_PRINT_
1620 ("Plane %d : x0 = %f, z0 = %f, x1 = %f, z1 = %f, x2 = %f, z2 = %f, x3 = %f, z3 = %f\n",
1621 routenum, base_plane[routenum]->pt[0].x,
1622 base_plane[routenum]->pt[0].z, base_plane[routenum]->pt[1].x,
1623 base_plane[routenum]->pt[1].z, base_plane[routenum]->pt[2].x,
1624 base_plane[routenum]->pt[2].z, base_plane[routenum]->pt[3].x,
1625 base_plane[routenum]->pt[3].z);
1629 /******************************************************************************
1630 * Functions for Calculate Landmark Points
1631 ******************************************************************************/
1632 /*--------------------------------------------------------------------------*/
1634 * @brief calc_landmarks_coord_by_camera
1635 * The coordinates of landmarks are calculated with a camera.
1640 /*--------------------------------------------------------------------------*/
1641 static void calc_landmarks_coord_by_camera()
1645 for (i = 0; i < landmark_data_count; i++) {
1646 landmark[i].x = landmark_raw[i].x;
1647 landmark[i].y = landmark_raw[i].y;
1648 landmark[i].z = landmark_raw[i].z;
1652 /******************************************************************************
1653 * Functions for Calculate (Common)
1654 ******************************************************************************/
1655 /*--------------------------------------------------------------------------*/
1657 * @brief calc_route_distance
1658 * The route distance is calculated.
1663 /*--------------------------------------------------------------------------*/
1664 static void calc_route_distance()
1667 for (i = 0; i < route_draw_num - 1; i++) {
1668 route_distance[i] = calc_length(route[i], route[i + 1]);
1672 /** Calculate the distance between two points.(only x-z plane) */
1673 /*--------------------------------------------------------------------------*/
1675 * @brief calc_length
1676 * The distance between two points is calculated.
1678 * @param[in] p0 point 0
1679 * @param[in] p1 point 1
1680 * @return distance between two points
1682 /*--------------------------------------------------------------------------*/
1683 static double calc_length(Point p0, Point p1)
1685 return sqrt((p1.x - p0.x) * (p1.x - p0.x) +
1686 (p1.z - p0.z) * (p1.z - p0.z));
1689 /** Calculate the square of distance between two points.(only x-z plane) */
1690 /*--------------------------------------------------------------------------*/
1692 * @brief calc_square_length
1693 * The square of the distance between two points is calculated.
1695 * @param[in] p0 point 0
1696 * @param[in] p1 point 1
1697 * @return square of distance between two points
1699 /*--------------------------------------------------------------------------*/
1700 static double calc_square_length(Point p0, Point p1)
1702 return ((p1.x - p0.x) * (p1.x - p0.x) + (p1.z - p0.z) * (p1.z - p0.z));
1705 /* rotate point (center is orizin, xz-plane) */
1706 /*--------------------------------------------------------------------------*/
1709 * rotate point is calculated.
1711 * @param[in] str point
1712 * @param[out] dist point address
1713 * @param[in] angle angle
1716 /*--------------------------------------------------------------------------*/
1717 static void rotate_xz(Point src, Point *dist, double angle)
1719 double rx, rz, sine, cosine;
1722 rad = (-angle) * M_PI / 180.0;
1727 rx = cosine * src.x - sine * src.z;
1728 rz = sine * src.x + cosine * src.z;
1737 /*--------------------------------------------------------------------------*/
1739 * @brief calc_intersection
1740 * intersection is calculated.
1742 * @param[in] p0 point 0
1743 * @param[in] p1 point 1
1744 * @param[in] p2 point 2
1745 * @param[in] p3 point 3
1746 * @param[out] point point address
1749 /*--------------------------------------------------------------------------*/
1751 calc_intersection(Point p0, Point p1, Point p2, Point p3, Point *point)
1753 double S1, S2, result_x, result_z;
1754 S1 = ((p2.x - p3.x) * (p1.z - p3.z) - (p2.z - p3.z) * (p1.x - p3.x)) / 2;
1755 S2 = ((p2.x - p3.x) * (p3.z - p0.z) - (p2.z - p3.z) * (p3.x - p0.x)) / 2;
1757 result_x = (p1.x + (p0.x - p1.x) * S1 / (S1 + S2));
1758 result_z = (p1.z + (p0.z - p1.z) * S1 / (S1 + S2));
1760 point->x = result_x;
1761 point->y = ROUTE_PLANE_HEIGHT;
1762 point->z = result_z;
1767 /*--------------------------------------------------------------------------*/
1769 * @brief length_comp
1770 * Comparison of length of point.
1772 * @param[in] pl0 point length 0 address
1773 * @param[in] pl1 point length 1 address
1775 * @retval =0 length coincidence
1776 * @retval =1 point length 0 < point length 1
1777 * @retval =-1 point length 0 > point length 1
1779 /*--------------------------------------------------------------------------*/
1780 static int length_comp(const void *pl0, const void *pl1)
1782 PointLength point_length0 = *(PointLength *) pl0;
1783 PointLength point_length1 = *(PointLength *) pl1;
1785 if (point_length0.length == point_length1.length) {
1788 else if (point_length0.length < point_length1.length) {