/* "nav.c" global navigation program August 10, 1991 */ // This program is copyright (c) 2016, P. Lutus and is released // under the GPL (http://www.gnu.org/licenses/gpl-3.0.en.html). #include <stdio.h> #include <math.h> #define PI2 6.283185307179586 #define PD2 1.570796326794897 #define DTOR 1.74532925199433e-2 #define RTOD 5.729577951308232e1 #define isalpha(c) (((c >= 'A') && (c <= 'Z')) || ((c >= 'a') && (c <= 'z'))) #define isnum(c) (((c>='0') && (c<='9')) || (c=='-') || (c=='+') || (c=='.')) #define toupper(c) (((c >= 'a') && (c <= 'z'))?c-32:c) struct COMPLX { double rad; double lat; double lng; double x; double y; double z; }; int matherr(struct exception *p) /* dummy matherr routine to suppress printed message */ //struct exception *p; { return(1); } double myatan2(double a, double b) // double a,b; { if((a != 0.0) || (b != 0.0)) a = atan2(a,b); return(a); } struct COMPLX rect_to_pol(struct COMPLX p) //struct COMPLX p; { p.lng = myatan2(p.x,p.z); p.rad = hypot(p.x,p.z); p.lat = myatan2(p.y,p.rad); return(p); } struct COMPLX comp_pr(struct COMPLX b, struct COMPLX a) //struct COMPLX b,a; { double dlo,cdl,sbl,cbl; dlo = a.lng - b.lng; cdl = cos(dlo); sbl = sin(a.lat); cbl = cos(a.lat); a.rad = acos(sbl * sin(b.lat) + cbl * cos(b.lat) * cdl); a.rad *= RTOD * 60; a.lat = myatan2(sin(dlo),(cbl * tan(b.lat) - sbl * cdl)); if (a.lat < 0) a.lat = PI2 + a.lat; return(a); } struct COMPLX comp_rp(struct COMPLX b,struct COMPLX a) //struct COMPLX b,a; { struct COMPLX c; double r,sr,ang; r = b.rad / (RTOD * 60.0); sr = sin(r); c.x = -sin(b.lat) * sr; c.y = cos(b.lat) * sr; c.z = cos(r); r = hypot(c.y,c.z); ang = myatan2(c.y,c.z) + a.lat; c.z = r * cos(ang); c.y = r * sin(ang); c = rect_to_pol(c); c.lng += a.lng; return(c); } void disp_lat_lng2(struct COMPLX q) //struct COMPLX q; { int lat_s,lng_s; int lat_d,lng_d; int lat_m,lng_m; double lat_m2,lng_m2; q.lat *= RTOD; q.lng *= RTOD; if(q.lng > 180.0) q.lng -= 360.0; lat_s = (q.lat >= 0); lng_s = (q.lng >= 0); q.lat = fabs(q.lat) + .0000083333; q.lng = fabs(q.lng) + .0000083333; lat_d = (int) q.lat; lat_m = (int) ((q.lat - lat_d) * 6000.0); lat_m2 = (int) (lat_m * 1e-2); lng_d = (int) q.lng; lng_m = (int) ((q.lng - lng_d) * 6000.0); lng_m2 = (int) (lng_m * 1e-2); printf("lat %3d deg %5.02lf min %c lng %3d deg %5.02lf min %c" ,lat_d,lat_m2,(lat_s)?'N':'S',lng_d,lng_m2,(lng_s)?'W':'E'); } void disp_lat_lng(struct COMPLX q) //struct COMPLX q; { disp_lat_lng2(q); printf("\n"); } void disp_dist_brg(struct COMPLX q) //struct COMPLX q; { q.rad += .0005; q.lat *= RTOD; q.lat += .0005; printf("%8.02lf nm, %6.02lf deg true\n",q.rad,q.lat); } int llscan(char *argv[], double *q, int i, int argc) //char *argv[]; //double *q; //int i,argc; { double x; int c; if(i <= argc) { sscanf(*(argv+i),"%lf",q); i++; } if(i <= argc) { sscanf(*(argv+i),"%lf",&x); *q += (x/60.0); i++; } if(i <= argc) { c = toupper(*(argv+i)[0]); if(isalpha(c)) { if ((c == 'S') || (c == 'E')) *q = -*q; i++; } } *q *= DTOR; return(i); } int dbscan(char *argv[], double *q, int i,int argc) //char *argv[]; //double *q; //int i,argc; { if(i <= argc) { sscanf(*(argv+i),"%lf",q); i++; } return(i); } int timescan(char *argv[], double *q, int i,int argc) //char *argv[]; //double *q; //int i,argc; { double h,m; if(i <= argc) { sscanf(*(argv+i),"%lf:%lf",&h,&m); i++; *q = h + (m / 60); } return(i); } int numflds(int argc,char *argv[]) //int argc; //char *argv[]; { int count = 0,i; for(i = 1;i < argc;i++) { if(isnum(*(argv+i)[0])) count++; } return(count); } int istime(char *string) //char *string; { int i = 0; while((*string+i) && ((*string+i) != ':')) i++; return((*string+i) == ':'); } char llstring[] = "(lat) ddd mm.mm [n/s] (lng) ddd mm.mm [w/e]"; void help() { printf( "usage: [-f(ull)] (1) result distance & bearing for 2 positions:\n"); printf( "(from) %s\n",llstring); printf( "(to) %s\n\n",llstring); printf( " (2) result speed, distance & bearing for 2 positions & times:\n"); printf( "(from) (time) hh:mm %s\n",llstring); printf( "(to) (time) hh:mm %s\n\n",llstring); printf( " (3) result position for position, distance & bearing\n"); printf( "(from) %s\n",llstring); printf( "(to) (dist) nm (bearing true) ddd.dd\n\n"); printf( " (4) track intercept distance & bearing for 3 positions\n"); printf( "(track begin position) %s\n",llstring); printf( "(track end position) %s\n",llstring); printf( "(vessel position) %s\n",llstring); } main(int argc,char *argv[]) //int argc; //char *argv[]; { int i = 1,count,listmode,full = 0; struct COMPLX p1,p2,p3,p4,p5; double intpos,steps,atime,btime; if((argc > 1) && (argv[1][0] == '-') && (toupper(argv[1][1]) == 'F')) { argc--; argv++; full++; } count = numflds(argc,argv); if(count == 8) { i = llscan(argv,&p1.lat,i,argc); i = llscan(argv,&p1.lng,i,argc); i = llscan(argv,&p2.lat,i,argc); i = llscan(argv,&p2.lng,i,argc); p3 = comp_pr(p2,p1); if(full) { printf("from "); disp_lat_lng(p1); printf("to "); disp_lat_lng(p2); printf("is "); } disp_dist_brg(p3); } else if(count == 6) { i = llscan(argv,&p1.lat,i,argc); i = llscan(argv,&p1.lng,i,argc); i = dbscan(argv,&p2.rad,i,argc); i = dbscan(argv,&p2.lat,i,argc); p2.lat *= DTOR; p3 = comp_rp(p2,p1); if(full) { printf("from "); disp_lat_lng(p1); printf("to "); disp_dist_brg(p2); printf("is "); } disp_lat_lng(p3); } else if(count == 10) { i = timescan(argv,&atime,i,argc); i = llscan(argv,&p1.lat,i,argc); i = llscan(argv,&p1.lng,i,argc); i = timescan(argv,&btime,i,argc); i = llscan(argv,&p2.lat,i,argc); i = llscan(argv,&p2.lng,i,argc); p3 = comp_pr(p2,p1); if(full) { printf("from "); disp_lat_lng(p1); printf("to "); disp_lat_lng(p2); printf("is "); } disp_dist_brg(p3); btime -= atime; if(btime <= 0.0) btime += 24.0; atime = p3.rad / btime; if(atime > 40.0) btime += 24.0; atime = p3.rad / btime; printf("time %.2lf hours -- %.2lf knots\n",btime,atime); } else if(count == 12) { i = llscan(argv,&p1.lat,i,argc); i = llscan(argv,&p1.lng,i,argc); i = llscan(argv,&p2.lat,i,argc); i = llscan(argv,&p2.lng,i,argc); i = llscan(argv,&p3.lat,i,argc); i = llscan(argv,&p3.lng,i,argc); p4 = comp_pr(p2,p1); /* bearing to track end */ p5 = comp_pr(p3,p1); /* distance to vessel from track start */ p5.lat = p4.lat; /* now p5 points to track intercept */ p4 = comp_rp(p5,p1); /* track intercept pos */ p5 = comp_pr(p4,p3); /* dist, brg to intercept */ if(full) { printf("track begin "); disp_lat_lng(p1); printf("track end "); disp_lat_lng(p2); printf("vessel position "); disp_lat_lng(p3); printf("track intercept "); disp_lat_lng(p4); } printf("vessel to intercept "); disp_dist_brg(p5); } else help(); }