1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
|
/*
* nav.c
*
* Copyright (C) 2006-2008 Simon Wunderlich <dotslash@packetmixer.de>
*
* This file is part of s3dosm, a gps card application for s3d.
* See http://s3d.berlios.de/ for more updates.
*
* s3dosm is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2 of the License, or
* (at your option) any later version.
*
* s3dosm is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with s3dosm; if not, write to the Free Software
* Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
*/
#include "s3dosm.h"
#include <s3d.h>
#include <string.h> /* strcmp() */
#include <stdlib.h> /* strtod() */
#include <stdio.h> /* printf() */
#include <math.h> /* fabs() */
static int oidx;
int oidy;
static float lat, lon, tlat, tlon;
/* load rotation centers */
void nav_init(void)
{
lat = lon = tlat = tlon = 0.0;
oidx = s3d_new_object();
oidy = s3d_new_object();
s3d_link(oidy, oidx);
s3d_translate(oidx, 0, -ESIZE*RESCALE - VIEWHEIGHT, 0);
s3d_scale(oidx, RESCALE);
}
/* center to given latitude longitude */
void nav_center(float la, float lo)
{
tlat = la;
tlon = lo;
}
void nav_campos(float campos[3], float earthpos[3])
{
float tmp1[3], tmp2[3];
float alpha;
tmp1[0] = campos[0];
tmp1[1] = campos[1] + ESIZE * RESCALE + VIEWHEIGHT;
tmp1[2] = campos[2];
alpha = (90 - lat) * M_PI / 180.0;
tmp2[0] = tmp1[0];
tmp2[1] = tmp1[1] * cos(alpha) - tmp1[2] * sin(alpha);
tmp2[2] = tmp1[1] * sin(alpha) + tmp1[2] * cos(alpha);
alpha = lon * M_PI / 180.0;
tmp1[0] = tmp2[0] * cos(alpha) + tmp2[2] * sin(alpha);
tmp1[1] = tmp2[1];
tmp1[2] = - tmp2[0] * sin(alpha) + tmp2[2] * cos(alpha);
earthpos[0] = tmp1[0];
earthpos[1] = tmp1[1];
earthpos[2] = tmp1[2];
}
void nav_main(void)
{
float x[3];
if ((fabs(tlat - lat) > 0.00001) && (fabs(tlon - lon) > 0.00001)) {
if (lat == 0.0 && lon == 0.0) {
lat = tlat;
lon = tlon;
} else {
lat = (tlat + lat * 15) / 16;
lon = (tlon + lon * 15) / 16;
}
} else {
lat = tlat;
lon = tlon;
}
s3d_rotate(oidy, 0, -lon, 0);
s3d_rotate(oidx, -(90 - lat), 0, 0);
calc_earth_to_eukl(lon, lat, 0, x);
}
static int get_center(void *data, int argc, char **argv, char **azColName)
{
float *med = (float *)data;
int i;
med[0] = 0;
med[1] = 0;
for (i = 0; i < argc; i++) {
if (argv[i]) {
if (0 == strcmp(azColName[i], "la")) med[0] = strtod(argv[i], NULL);
else if (0 == strcmp(azColName[i], "lo")) med[1] = strtod(argv[i], NULL);
}
}
return(0);
}
/* returns the heading in degress of position P1 -> P2 */
float get_heading(float la1, float lo1, float la2, float lo2)
{
float p1_north[3], p1[3], p2[3];
float dir[3], north[3];
float angle;
int i;
calc_earth_to_eukl(la1, lo1, 0, p1);
calc_earth_to_eukl(la2, lo2, 0, p2);
calc_earth_to_eukl(la1 + 1, lo1, 0, p1_north);
for (i = 0;i < 3;i++) north[i] = p1_north[i] - p1[i];
for (i = 0;i < 3;i++) dir[i] = p2[i] - p1[i];
angle = s3d_vector_angle(dir, north);
angle = angle * 180.0 / M_PI;
if ((lo2 > lo1) || (lo1 - lo2 > 180.0)) angle = 360 - angle;
return(angle);
}
/* find some good center on our own */
void nav_autocenter(void)
{
float med[2];
char query[] = "SELECT avg(longitude) as lo, avg(latitude) as la FROM node; ";
db_exec(query, get_center, med);
nav_center(med[0], med[1]);
printf("center to %f,%f\n", med[0], med[1]);
}
|
