This is geometry.c in view mode; [Download] [Up]
/****************************************************************
* *
* I C A O M a p - *
* *
* A n A v i a t i o n U t i l i t y *
* *
* *
* Copyright (C) 1993, 1994 Martin Pauly *
* *
* N e x t V e r s i o n *
* *
* Copyright (C) 1994 Stefan Leuker & Oliver Meyer *
* *
* This file may be freely distributed only if it includes *
* the above copyright notice. *
* *
****************************************************************/
#import "geometry.h"
double unitChange[]={ 0, 6076.115, 1852, 1.852, 1 };
char *unitName[]={NULL, "ft", "m", "km", "NM"};
double
distance(LOCATION a, LOCATION b)
{
struct
{
double x, y, z;
} start, dest;
double chord;
double angle;
/* transfer to Cartesian system */
start.x = EARTH_RADIUS * cos(internal2rad(a.longitude))
* cos(internal2rad(a.latitude));
start.y = EARTH_RADIUS * sin(internal2rad(a.longitude))
* cos(internal2rad(a.latitude));
start.z = EARTH_RADIUS * sin(internal2rad(a.latitude));
dest.x = EARTH_RADIUS * cos(internal2rad(b.longitude))
* cos(internal2rad(b.latitude));
dest.y = EARTH_RADIUS * sin(internal2rad(b.longitude))
* cos(internal2rad(b.latitude));
dest.z = EARTH_RADIUS * sin(internal2rad(b.latitude));
/* length of the chord between start and destination */
chord = sqrt((start.x - dest.x) * (start.x - dest.x)
+ (start.y - dest.y) * (start.y - dest.y)
+ (start.z - dest.z) * (start.z - dest.z));
angle = acos(1 - (chord * chord) / (2 * EARTH_RADIUS * EARTH_RADIUS));
return EARTH_RADIUS * angle / 1.852; /* result in NM */
}
double
distance2(LOCATION a, LOCATION b)
{
double lat1, lat2, long1, long2, dl;
double alpha = 1 / 297.0;
double s, s0, sdiva, stemp, cosquad;
double ae = 6378388;
/*
* L. Kiefer: Die Beruecksichtigung der Ellipsoidgestalt der Erde in der
* Flugnavigation
*/
lat1 = internal2rad(a.latitude);
long1 = internal2rad(a.longitude);
lat2 = internal2rad(b.latitude);
long2 = internal2rad(b.longitude);
dl = internal2rad(a.longitude - b.longitude);
if ((lat1 == lat2) && (long1 == long2))
return 0;
if (lat1 == lat2)
lat1 += 0.000000001;
if (long1 == long2)
long1 += 0.000000001;
cosquad = cos((lat2 - lat1) / 2);
cosquad *= cosquad;
stemp = (2 * cosquad - 1) * cos(dl / 2) * cos(dl / 2) - (2 * cosquad - 1) * sin(dl / 2) * sin(dl / 2);
/* this made problems, I suspect the pow function of my library:
stemp = (2*pow(cos((lat2-lat1)/2),2)-1)*pow(cos(dl/2),2) -
(2*pow(cos((lat2+lat1)/2),2)-1)*pow(sin(dl/2),2);
*/
s0 = acos(stemp);
/* again, we should avoid use of the power function
sdiva = s0 - alpha *
(pow(sin((lat2+lat1)/2),2)*pow(cos((lat2-lat1)/2),2)*(s0-3*sin(s0))/(1+cos(s0)) +
pow(cos((lat2+lat1)/2),2)*pow(sin((lat2-lat1)/2),2)*(s0+3*sin(s0))/(1-cos(s0)));
*/
sdiva = s0 - alpha *
(sin((lat2 + lat1) / 2) * sin((lat2 + lat1) / 2) * cos((lat2 - lat1) / 2) *
cos((lat2 - lat1) / 2) * (s0 - 3 * sin(s0)) / (1 + cos(s0)) +
cos((lat2 + lat1) / 2) * cos((lat2 + lat1) / 2) * sin((lat2 - lat1) / 2) *
sin((lat2 - lat1) / 2) * (s0 + 3 * sin(s0)) / (1 - cos(s0)));
s = sdiva * ae;
return s / 1852;
}
double
truetrack(LOCATION a, LOCATION b)
{
double tt = 0;
long dlat, dlong;
int offset, factor;
LOCATION p1, p2, p3;
p1 = a;
p2.latitude = b.latitude;
p2.longitude = a.longitude;
p3 = b;
dlat = b.latitude - a.latitude;
dlong = b.longitude - a.longitude;
if (dlat == 0)
{
if (dlong > 0)
tt = 90;
if (dlong < 0)
tt = 270;
}
else
{
offset = 0;
factor = 1;
if ((dlat < 0) && (dlong > 0))
{
offset = 90;
factor = -1;
}
if ((dlat < 0) && (dlong < 0))
offset = 180;
if ((dlat > 0) && (dlong < 0))
{
offset = 270;
factor = -1;
}
dlat *= (dlat >= 0) ? 1 : -1;
dlong *= (dlong >= 0) ? 1 : -1;
/* tt = factor * (180 * atan((double) dlong / (double) dlat) / PI); */
tt = factor * (180 * atan(distance(p2, p3) / distance(p1, p2)) / PI);
if (tt < 0)
tt += 90;
tt += offset;
}
return tt; /* not implemented either... */
}
double
truetrack1(LOCATION a, LOCATION b)
{
double tt;
double dlat;
double d;
d = distance(a, b);
dlat = (b.latitude - a.latitude) / 360000;
if (d)
{
tt = 180 * acos(dlat * ONE_DEGREE / d) / PI;
}
else
tt = 0; /* default value if a==b */
return tt;
}
/* convert a string 99.99'99"d into the internal degrees representation */
/* bug fixed Jan-16-94: format 999.99'99"d also accepted */
long
deg2num(char *string)
{
int deg, min, sec;
long result=0; /* to avoid Compiler Warning */
if (string[2] == '.')
{
deg = atoi(string);
min = atoi(string + 3);
sec = atoi(string + 6);
result = (((deg * 60) + min) * 60 + sec) * 100;
if ((string[9] == 'W') || (string[9] == 'S'))
result *= -1;
}
else
if (string[3] == '.')
{
deg = atoi(string);
min = atoi(string + 4);
sec = atoi(string + 7);
result = (((deg * 60) + min) * 60 + sec) * 100;
if ((string[10] == 'W') || (string[10] == 'S'))
result *= -1;
}
return result;
}
/* convert internal representation to human readable string */
char *
internal2string(LOCATION pos)
{
static char build[25];
sprintf(build, "%2d.%2d\047%2d\042%c %2d.%2d\047%2d\042%c",
(int)(abs(pos.latitude) / 360000),
(int)((abs(pos.latitude) / 6000) % 60),
(int)((abs(pos.latitude) / 100) % 60),
(pos.latitude < 0) ? 'S' : 'N',
(int)(abs(pos.longitude) / 360000),
(int)((abs(pos.longitude) / 6000) % 60),
(int)((abs(pos.longitude) / 100) % 60),
(pos.longitude < 0) ? 'W' : 'E');
if (build[0] == ' ')
build[0] = '0';
if (build[3] == ' ')
build[3] = '0';
if (build[6] == ' ')
build[6] = '0';
if (build[11] == ' ')
build[11] = '0';
if (build[14] == ' ')
build[14] = '0';
if (build[15] == ' ')
build[15] = '0'; /* also handle 3-digit longitude */
if (build[17] == ' ')
build[17] = '0';
if (build[18] == ' ')
build[18] = '0';
return build;
}
/* convert degrees into radians */
double
rad(double deg)
{
return (deg * PI / 180);
}
/* convert radians into degrees */
double
deg(double rad)
{
return (rad * 180 / PI);
}
/* convert internal repr. into radians */
double
internal2rad(long internal)
{
return (((double)internal) * PI / 64800000);
}
/* convert radians into internal rep. */
long
rad2internal(double rad)
{
return (long)(rad * 64800000 / PI);
}
/*
conformal conical projection with two standard parallels (Lambert)
The following two routines require a geographical loaction,
given in geographical coordinates (radian).
They return a coordinate pair (x,y) for the position on a two-dimensional
map on which the given location should be displayed.
-1 < x < 1; -1 < y < 1.
The zero-meridian (the one going through London) will be the line from
(0,0) to (0, -1).
If anything goes wrong with these two functions: the mathematical background
can be found in Josef Hoschek: "Mathematische Grundlagen der Kartographie".
note: the latitudes of the two standard parallels are stored in global vars
v1 and v2. It doesn't matter which one is which.
*/
double
proj_x_lambert(double latitude, double longitude)
{
double x;
x = 2 * (sqrt(cos(v1) * cos(v1) + (sin(v1) - sin(latitude))
* (sin(v1) + sin(v2))) / (sin(v1) + sin(v2)))
* sin((sin(v1) + sin(v2)) * longitude / 2);
return x;
}
double
proj_y_lambert(double latitude, double longitude)
{
double y;
y = -2 * (sqrt(cos(v1) * cos(v1) + (sin(v1) - sin(latitude))
* (sin(v1) + sin(v2))) / (sin(v1) + sin(v2)))
* cos((sin(v1) + sin(v2)) * longitude / 2);
return y;
}
double
proj_x_mercator(double latitude, double longitude)
{
return longitude * mercatorscale;
}
double
proj_y_mercator(double latitude, double longitude)
{
return log(tan(PI / 4 + latitude / 2)) * mercatorscale; /* log is natural log */
}
double
proj_x(double latitude, double longitude)
{
switch (map_projection)
{
case PROJECT_LAMBERT:
return proj_x_lambert(latitude, longitude);
break;
case PROJECT_MERCATOR:
return proj_x_mercator(latitude, longitude);
break;
}
return 0.0;
}
double
proj_y(double latitude, double longitude)
{
switch (map_projection)
{
case PROJECT_LAMBERT:
return proj_y_lambert(latitude, longitude);
break;
case PROJECT_MERCATOR:
return proj_y_mercator(latitude, longitude);
break;
}
return 0.0;
}
/*
Now, we also need the other way around, i.e. from a given map coordinate
we'd like to find out what the "real world" coordinate is. The code for
the reverse lambert projection looks a bit ugly - it was created
automatically by Maple...
*/
void
plane2sphere_mercator(double x, double y, double *latitude, double *longitude)
{
*latitude = 2 * atan(exp(y / mercatorscale)) - PI / 2;
*longitude = x / mercatorscale;
}
void
plane2sphere_lambert(double x, double y, double *latitude, double *longitude)
{
*latitude = -asin((-4.0 * pow(cos(v1), 2.0) - 4.0 * pow(sin(v1), 2.0) - 4.0 * sin(v1) * sin(v2)
+ y * y * pow(sin(v1), 2.0) + pow(sin(v1), 2.0) * x * x + 2.0 * y * y * sin(v1) * sin(v2) + 2.0 * sin(v1)
* sin(v2) * x * x + y * y * pow(sin(v2), 2.0) + pow(sin(v2), 2.0) * x * x) / (sin(v1) + sin(v2)) / 4);
*longitude = -2.0 * atan(x / y) / (sin(v1) + sin(v2));
}
void
plane2sphere(double x, double y, double *latitude, double *longitude)
{
switch (map_projection)
{
case PROJECT_LAMBERT:
plane2sphere_lambert(x, y, latitude, longitude);
break;
case PROJECT_MERCATOR:
plane2sphere_mercator(x, y, latitude, longitude);
break;
}
}
/* convert position x,y (in cm) to human readable string */
char *
cm2string(double mapx, double mapy)
{
double x, y;
double latitude, longitude;
LOCATION temp;
x = mapx / scalefactor + originx;
y = mapy / scalefactor + originy;
plane2sphere(x, y, &latitude, &longitude);
temp.latitude = rad2internal(latitude);
temp.longitude = rad2internal(longitude) + map_zeromeridian;
return internal2string(temp);
}
/* convert position x,y (in cm) to internal format */
LOCATION
cm2internal(double mapx, double mapy)
{
double x, y;
double latitude, longitude;
LOCATION temp;
x = mapx / scalefactor + originx;
y = mapy / scalefactor + originy;
plane2sphere(x, y, &latitude, &longitude);
temp.latitude = rad2internal(latitude);
temp.longitude = rad2internal(longitude) + map_zeromeridian;
return temp;
}
/* convert internal format to map position in cm */
void
internal2cm(LOCATION position, double *mapx, double *mapy)
{
double x, y;
x = proj_x(internal2rad(position.latitude),
internal2rad(position.longitude - map_zeromeridian));
y = proj_y(internal2rad(position.latitude),
internal2rad(position.longitude - map_zeromeridian));
*mapx = (x - originx) * scalefactor;
*mapy = (y - originy) * scalefactor;
}
/*
drawing to the map, it may be necessary to find out what offset has to added
to a true track at a given position to properly draw that angle on the map
(shifts by zeromeridian etc.):
*/
int
truenorthoffset(LOCATION pos)
{
if (map_projection == PROJECT_LAMBERT)
return (int)((map_zeromeridian - pos.longitude) / 360000);
else
return 0; /* naturally, true north is always up in
* Mercator style maps */
}
/*
geometry primitives
*/
/* test for counter-clock wise movement from p0 via p1 to p2 */
/* from: Sedgewick: Algorithms */
int
ccw(POINT p0, POINT p1, POINT p2)
{
int dx1, dx2, dy1, dy2, result = -1;
dx1 = p1.x - p0.x;
dy1 = p1.y - p0.y;
dx2 = p2.x - p0.x;
dy2 = p2.y - p0.y;
if (dx1 * dy2 > dy1 * dx2)
result = 1;
if (dx1 * dy2 < dy1 * dx2)
result = -1;
if (dx1 * dy2 == dy1 * dx2)
{
if ((dx1 * dx2 < 0) || (dy1 * dy2 < 0))
result = -1;
else
{
if (dx1 * dx1 + dy1 * dy1 >= dx2 * dx2 + dy2 * dy2)
result = 0;
else
result = 1;
}
}
return result;
}
/* test for intersection of two lines */
/* from: Sedgewick: Algorithms */
int
intersect(LINE l1, LINE l2)
{
return ((ccw(l1.p1, l1.p2, l2.p1) * ccw(l1.p1, l1.p2, l2.p2)) <= 0) &&
((ccw(l2.p1, l2.p2, l1.p1) * ccw(l2.p1, l2.p2, l1.p2)) <= 0);
}
/* test if point t is contained in polygon p */
/* points in p[1]..p[numpoints], p[0] and p[numpoints+1] must be available */
/* from: Sedgewick: Algorithms */
int
ppcontains(POINT t, POINT * p, int numpoints)
{
int count, i, j;
LINE lt, lp;
POINT comp;
/* first make p[1] the point with lowest x among points with lowest y */
i = 1;
comp = p[1];
for (j = 2; j <= numpoints; j++) /* search */
if ((p[j].y < comp.y) || ((p[j].y == comp.y) && (p[j].x < comp.x)))
{
i = j;
comp = p[j];
}
while (i > 1) /* reorganie */
{
comp = p[1];
for (j = 1; j < numpoints; j++)
p[j] = p[j + 1];
p[numpoints] = comp;
i--;
}
count = 0;
j = 0;
p[0] = p[numpoints];
p[numpoints + 1] = p[1];
lt.p1 = lt.p2 = t;
lt.p2.x += 9998; /* make a very long line */
lt.p2.y += 9991;
for (i = 1; i <= numpoints; i++)
{
lp.p1 = p[i];
lp.p2 = p[i];
if (!intersect(lp, lt))
{
lp.p2 = p[j];
j = i;
if (intersect(lp, lt))
count++;
}
}
return count % 2;
}
These are the contents of the former NiCE NeXT User Group NeXTSTEP/OpenStep software archive, currently hosted by Netfuture.ch.