00001
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027 static char rcsid[] = "$Id: gpsutil.c,v 1.10 2004/01/27 19:59:00 clavelei Exp $";
00028
00029 #include <PalmOS.h>
00030 #include <CharAttr.h>
00031 #include <stdio.h>
00032 #include "MathLib.h"
00033
00034 #include "Garmin.h"
00035 #include "types.h"
00036 #include "log.h"
00037 #include "gpsutil.h"
00038 #include <TxtGlue.h>
00039
00040
00041 #define min(a, b) (((a) < (b)) ? (a) : (b))
00042 #undef TxtGlueCharIsDigit // TxtGlueCharIsDigit is buggy on some PalmOS
00043 #define TxtGlueCharIsDigit(c) (((c)<='9') && ((c)>='0'))
00044
00045 int DebugLevel;
00046 int DebugIndentLevel;
00047 static LogProcPtr LogOutput;
00048 void DebugPrintf (char * formatStr, ...);
00049
00050 void
00051 SetMathLibRef (UInt16 ref)
00052 {
00053 MathLibRef = ref;
00054 }
00055
00056
00057
00058
00059
00060
00073 void
00074 StrNCatSafe (char *dstp, char *srcp, UInt16 dstsize, UInt16 srcsize)
00075 {
00076 debug (GPSUDBG_CALLS, ("<->StrNCatSafe(dstsize=%d, srcsize=%d", dstsize, srcsize));
00077 if (dstsize > (StrLen (dstp) + srcsize))
00078 StrNCat (dstp, srcp, StrLen (dstp) + srcsize + 1);
00079 else
00080 StrNCat (dstp, srcp, dstsize - 1);
00081 dstp[dstsize - 1] = 0;
00082 }
00083
00084
00096 char *
00097 StrNCopySafe (char *dstp, char *srcp, UInt16 dstsize)
00098 {
00099 debug (GPSUDBG_CALLS, ("StrNCopySafe(dstsize=%d)", dstsize));
00100 if (dstsize)
00101 {
00102 for (; dstsize; dstsize--)
00103 if (!(*dstp++ = *srcp++))
00104 break;
00105 *(dstp - 1) = 0;
00106 }
00107 return (dstp);
00108 }
00109
00110
00111
00121 void
00122 StrNToField (char *str, UInt16 length, UInt16 fieldid)
00123 {
00124 FieldPtr fdPtr = (FieldPtr) (GetObjPtr (fieldid));
00125 MemHandle OldH,
00126 NewH;
00127 FieldAttrType fdattr;
00128
00129 debug (GPSUDBG_CALLS, ("<->StrNToField(l=%d, fieldid=%d)", length, fieldid));
00130 OldH = FldGetTextHandle (fdPtr);
00131 NewH = MemHandleNew (length + 2);
00132 if (!NewH)
00133 return;
00134 StrNCopySafe (MemHandleLock (NewH), str, length + 1);
00135 MemHandleUnlock (NewH);
00136
00137 FldSetTextHandle (fdPtr, NewH);
00138 FldGetAttributes (fdPtr, &fdattr);
00139 if (fdattr.visible)
00140 FldDrawField (fdPtr);
00141 if (OldH)
00142 MemHandleFree (OldH);
00143 }
00144
00145
00158 void
00159 DrawCharsInWidth (char *string, Int16 x, Int16 y, UInt16 w, Boolean RightAligned)
00160 {
00161 Int16 width = w,
00162 dotw,
00163 len = StrLen (string);
00164 Boolean fitWithinWidth;
00165
00166 FntCharsInWidth (string, &width, &len, &fitWithinWidth);
00167 if (fitWithinWidth)
00168 WinDrawChars (string, len, RightAligned ? x + w - FntCharsWidth (string, len) : x, y);
00169 else
00170 {
00171 dotw = 3 * FntCharWidth ('.');
00172 width = w - dotw;
00173 FntCharsInWidth (string, &width, &len, &fitWithinWidth);
00174 WinDrawChars (string, len, RightAligned ? x + w - width - dotw : x, y);
00175 WinDrawChars ("...", 3, RightAligned ? x + w - dotw : x + width, y);
00176 }
00177 }
00178
00179
00188 void
00189 Canonicalize (char *p, UInt16 l)
00190 {
00191 Int16 i;
00192 for (i = 0; i < l; i++, p++)
00193 if ((*p >= 'a') && (*p <= 'z'))
00194 *p -= 'a' - 'A';
00195 else if (((*p < 'A') || (*p > 'Z')) && (*p != '-') && (*p != ' ')
00196 && ((*p < '0') || (*p > '9')))
00197 *p = ' ';
00198 }
00199
00200
00201
00202
00210 void *
00211 GetObjPtr (UInt16 ObjID)
00212 {
00213 FormPtr frm = FrmGetActiveForm ();
00214 debug (GPSUDBG_CALLS, ("<->GetObjPtr(%d)", ObjID));
00215 return (FrmGetObjectPtr (frm, (FrmGetObjectIndex (frm, ObjID))));
00216 }
00217
00218
00226 void
00227 HideObject (UInt16 objID)
00228 {
00229 FormPtr frm = FrmGetActiveForm ();
00230 FrmHideObject (frm, FrmGetObjectIndex (frm, objID));
00231 }
00232
00233
00241 void
00242 ShowObject (UInt16 objID)
00243 {
00244 FormPtr frm = FrmGetActiveForm ();
00245 FrmShowObject (frm, FrmGetObjectIndex (frm, objID));
00246 }
00247
00248
00257 void
00258 SetControlValue (UInt16 objID, short value)
00259 {
00260 FormPtr frm = FrmGetActiveForm ();
00261 FrmSetControlValue (frm, FrmGetObjectIndex (frm, objID), value);
00262 }
00263
00264
00272 short
00273 GetControlValue (UInt16 objID)
00274 {
00275 FormPtr frm = FrmGetActiveForm ();
00276 return (FrmGetControlValue (frm, FrmGetObjectIndex (frm, objID)));
00277 }
00278
00279
00289 void
00290 GetObjectBounds (UInt16 objID, RectanglePtr rp)
00291 {
00292 FormPtr frm = FrmGetActiveForm ();
00293 FrmGetObjectBounds (frm, FrmGetObjectIndex (frm, objID), rp);
00294 }
00295
00296
00297
00298
00299
00311 Int16
00312 RecIdToPosition (DmOpenRef DB, UInt16 category, UInt32 recid, UInt16 * indexp)
00313 {
00314 UInt16 i = 0;
00315 UInt16 pos = 0;
00316 UInt32 l;
00317 UInt16 attr;
00318
00319 debug (GPSUDBG_CALLS, ("RecIdToPosition(recid=%ld)", recid));
00320 while (DmQueryNextInCategory (DB, &i, category))
00321 {
00322 DmRecordInfo (DB, i, &attr, &l, NULL);
00323 if (l == recid)
00324 {
00325 if (indexp)
00326 *indexp = i;
00327 return (pos);
00328 }
00329 pos++;
00330 i++;
00331 }
00332 return (-1);
00333
00334 }
00335
00336
00345 UInt32
00346 GetRecordID (DmOpenRef DB, UInt16 index)
00347 {
00348 UInt32 recid = 0;
00349 DmRecordInfo (DB, index, NULL, &recid, NULL);
00350 debug (GPSUDBG_CALLS, ("GetRecordID(%d)->%ld", index, recid));
00351 return (recid);
00352 }
00353
00354
00355
00356
00357
00369 char *
00370 DToA (double x, UInt16 digits, UInt16 precision)
00371 {
00372 UInt32 l,
00373 k;
00374 UInt16 i;
00375 Boolean sign = 1;
00376 static char buf[21];
00377 char *p,
00378 *first;
00379
00380 debug_enter_func (GPSUDBG_CALLS, "(digits=%d,prec=%d)", digits, precision);
00381 MemSet (buf, sizeof (buf), '0');
00382 buf[9] = '.';
00383 if (x < 0.0)
00384 x = -x;
00385 else
00386 sign = 0;
00387 digits = min (digits, 9);
00388 precision = min (precision, 9);
00389 buf[9 + precision + 1] = 0;
00390 for (i = precision, k = 1; i; i--)
00391 k *= 10;
00392 x += (double) (1. / (2. * (double) k));
00393 l = (long) x;
00394 p = buf + 8;
00395 for (; l; l /= 10, p--)
00396 *p = (l % 10) + '0';
00397 first = min (p + 1, buf + 8);
00398 x -= (long) x;
00399 l = x * (double) k;
00400 p = buf + 9 + precision;
00401
00402 for (; l; l /= 10, p--)
00403 *p = (l % 10) + '0';
00404 p = digits ? buf + 9 - digits : first;
00405 if (sign)
00406 *--p = '-';
00407 debug_exit_func (GPSUDBG_CALLS, "=\"%s\"", p);
00408 return (p);
00409 }
00410
00411
00420 double
00421 StrToD (char *p, char **endp)
00422 {
00423 double d = 0.;
00424 enum
00425 { get_mantisse, get_frac, get_exp }
00426 state = get_mantisse;
00427 Boolean empty = 1,
00428 m_sign = 0,
00429 e_sign = 0;
00430 Int16 i,
00431 stop = 0;
00432 UInt32 coeff = 10,
00433 ecoeff = 10;
00434
00435
00436 debug_enter_func (GPSUDBG_CALLS, "(\"%s\"), test:%d", p,TxtGlueCharIsDigit ('°'));
00437 while (*p && TxtGlueCharIsSpace (*p))
00438 p++;
00439
00440 for (; *p && !stop; p++)
00441 {
00442 switch (state)
00443 {
00444 case get_mantisse:
00445 if (TxtGlueCharIsDigit (*p))
00446 {
00447 d = d * 10. + (*p - '0');
00448 empty = 0;
00449 }
00450 else if (*p == '.')
00451 state = get_frac;
00452
00453
00454 else if (*p == '-')
00455 m_sign = 1;
00456 else if (*p == '+');
00457 else if (*p == 'e')
00458 state = get_exp;
00459 else
00460 stop = 1;
00461 break;
00462 case get_frac:
00463 if (TxtGlueCharIsDigit (*p))
00464 d = d + (double) (*p - '0') / coeff;
00465 else if (*p == 'e')
00466 state = get_exp;
00467 else
00468 stop = 1;
00469 coeff *= 10;
00470 break;
00471 case get_exp:
00472 if (TxtGlueCharIsDigit (*p))
00473 {
00474 for (i = 0; i < (*p - '0'); i++)
00475 d = e_sign ? d / ecoeff : d * ecoeff;
00476 ecoeff = e_sign ? ecoeff / 10 : ecoeff * 10;
00477 }
00478 else if (*p == '-')
00479 e_sign = 1;
00480 else if (*p == '+');
00481 else
00482 stop = 1;
00483 }
00484 }
00485 if (stop)
00486 p--;
00487 if (endp)
00488 *endp = p;
00489 debug_exit_func (GPSUDBG_CALLS, "(\"%s\")=%s",endp?*endp:"",DToA (m_sign?-d:d,0,3));
00490 return (m_sign?-d:d);
00491 }
00492
00493
00504 double
00505 ReadCoord (char *string, Boolean * longitude)
00506 {
00507 char *stop = string + StrLen (string);
00508 enum
00509 { deg = 1, min = 2, sec = 4, latlon = 8 }
00510 got = 0;
00511 double t,
00512 r = 0.,
00513 coef = 1;
00514 char *dep = string;
00515
00516 debug (GPSUDBG_CALLS, ("<->ReadCoord(\"%s\",\"%s\")", string, longitude ? "longitude" : "latitude"));
00517 while (string < stop)
00518 {
00519 dep = string;
00520 t = StrToD (string, &string);
00521
00522 while (*string == ' ')
00523 string++;
00524 switch (*string)
00525 {
00526 case '°':
00527 if (got & deg)
00528 return (BadCoordinate);
00529 got |= deg;
00530 r += t;
00531 break;
00532 case '\'':
00533 if (got & min)
00534 return (BadCoordinate);
00535 got |= min;
00536 r += t / 60.;
00537 break;
00538 case '"':
00539 if (got & sec)
00540 return (BadCoordinate);
00541 got |= sec;
00542 r += t / 3600.;
00543 break;
00544 case 'N':
00545 case 'n':
00546 if (got & latlon)
00547 return (BadCoordinate);
00548 got |= latlon;
00549 *longitude = 0;
00550 break;
00551 case 'S':
00552 case 's':
00553 if (got & latlon)
00554 return (BadCoordinate);
00555 got |= latlon;
00556 *longitude = 0;
00557 coef = -1;
00558 break;
00559 case 'W':
00560 case 'w':
00561 if (got & latlon)
00562 return (BadCoordinate);
00563 got |= latlon;
00564 *longitude = 1;
00565 coef = -1;
00566 break;
00567 case 'E':
00568 case 'e':
00569 if (got & latlon)
00570 return (BadCoordinate);
00571 got |= latlon;
00572 *longitude = 1;
00573 break;
00574 default:
00575 if (!(got & latlon) || !(got & 7))
00576 return (BadCoordinate);
00577 else
00578 stop = string;
00579 }
00580 string++;
00581 }
00582 if (!*longitude && (r > 90.))
00583 return (BadCoordinate);
00584 if (*longitude && (r > 180.))
00585 return (BadCoordinate);
00586 return (r * coef);
00587 }
00588
00589
00590
00591
00601 char *
00602 DegreToChars (double coord, Boolean longitude, DisplayFormat DisplayMode)
00603 {
00604 switch (DisplayMode)
00605 {
00606 case DMM:
00607 return (DegreToDM (coord, longitude, 0));
00608 break;
00609 case DMS:
00610 return (DegreToDMS (coord, longitude));
00611 break;
00612 case DDD:
00613 default:
00614 return (DegreToD (coord, longitude));
00615 }
00616 }
00617
00618
00629 char *
00630 DegreToDM (double coord, Boolean longitude, Boolean nmea)
00631 {
00632 Int16 d,
00633 m,
00634 mm;
00635 static char buf[16];
00636 char c,
00637 deg[2],
00638 e;
00639
00640 c = longitude ? coord < 0 ? 'W' : 'E' : coord < 0 ? 'S' : 'N';
00641 if (coord < 0)
00642 coord = -coord;
00643 d = (Int16) coord;
00644 coord -= d;
00645 coord *= 60.0;
00646 m = (Int16) coord;
00647 coord -= m;
00648 coord *= 1000;
00649 mm = (Int16) (coord + 0.5);
00650 if (mm >= 1000)
00651 {
00652 mm = 0;
00653 if (++m == 60)
00654 {
00655 m = 0;
00656 d++;
00657 }
00658 }
00659 deg[0] = nmea ? 0 : '°';
00660 deg[1] = 0;
00661 e = nmea ? ',' : '\'';
00662 StrPrintF (buf, "%s%d%s%s%d.%s%d%c%c",
00663 longitude ? d > 99 ? "" : d > 9 ? "0" : "00" : d > 9 ? "" : "0",
00664 d, deg, m > 9 ? "" : "0", m, mm > 99 ? "" : mm > 9 ? "0" : "00", mm, e, c);
00665 debug (GPSUDBG_CALLS, ("<->DegreToDM()=\"%s\"", buf));
00666 return (buf);
00667 }
00668
00669
00678 char *
00679 DegreToDMS (double coord, Boolean longitude)
00680 {
00681 Int16 d,
00682 m,
00683 s,
00684 ss;
00685 static char buf[16];
00686 char c;
00687 double precision = (1. / 3600.) / 10.;
00688
00689 c = longitude ? coord < 0 ? 'W' : 'E' : coord < 0 ? 'S' : 'N';
00690 if (coord < 0)
00691 coord = -coord;
00692 coord += precision / 2.;
00693 d = (Int16) coord;
00694 m = (Int16) (fmod (coord, 1) * 60.);
00695 s = (Int16) (fmod (coord, 1. / 60.) * 3600.);
00696 ss = (Int16) (fmod (coord, 1. / 3600.) * 36000.);
00697 StrPrintF (buf, "%s%d°%s%d'%s%d.%d\"%c",
00698 longitude ? d > 99 ? "" : d > 9 ? "0" : "00" : d > 9 ? "" : "0",
00699 d, m > 9 ? "" : "0", m, s > 9 ? "" : "0", s, ss, c);
00700 debug (GPSUDBG_CALLS, ("<->DegreToDMS()=\"%s\"", buf));
00701 return (buf);
00702 }
00703
00704
00713 char *
00714 DegreToD (double coord, Boolean longitude)
00715 {
00716 Int16 d;
00717 UInt32 dd;
00718 static char buf[16];
00719 char conv[8],
00720 fill[] = "000000";
00721 char c;
00722 double precision = 0.00001;
00723
00724 c = longitude ? coord < 0 ? 'W' : 'E' : coord < 0 ? 'S' : 'N';
00725 if (coord < 0)
00726 coord = -coord;
00727 coord += precision / 2.;
00728 d = (Int16) coord;
00729 dd = (UInt32) (fmod (coord, 1) * 100000.);
00730 StrIToA (conv, dd);
00731 StrPrintF (fill + (5 - StrLen (conv)), "%s", conv);
00732 StrPrintF (buf, "%s%d.%s°%c",
00733 longitude ? d > 99 ? "" : d > 9 ? "0" : "00" : d > 9 ? "" : "0", d, fill, c);
00734 debug (GPSUDBG_CALLS, ("<->DegreToD()=\"%s\"", buf));
00735 return (buf);
00736 }
00737
00738
00748 void
00749 DegreToSemi (double latitude, double longitude, Semicircle_Type * p)
00750 {
00751 const double d2s = ((long) 1 << 30) / 90.0;
00752
00753 debug (GPSUDBG_CALLS, ("<->DegreToSemi()"));
00754 p->lat = latitude * d2s + 0.5;
00755 p->lon = longitude * d2s + 0.5;
00756 }
00757
00758
00768 void
00769 SemiToDegre (Semicircle_Type * p, double *latitude, double *longitude)
00770 {
00771 const double s2d = 90.0 / ((long) 1 << 30);
00772
00773 debug (GPSUDBG_CALLS, ("SemiToDegre()"));
00774 *latitude = p->lat * s2d;
00775 *longitude = p->lon * s2d;
00776 }
00777
00778
00789 void
00790 DistBearToSemi (double dist, UInt16 bearing, Semicircle_Type * p)
00791 {
00792 double const a = PI / OnePower31;
00793 double lat,
00794 lon,
00795 lat1 = p->lat * a,
00796 lon1 = -p->lon * a,
00797 tc = bearing * PI / 180.,
00798 d = PI / (180. * 60.) * dist;
00799
00800 debug (GPSUDBG_CALLS, ("DistBearToSemi(bearing=%d)", bearing));
00801 lat = asin (sin (lat1) * cos (d) + cos (lat1) * sin (d) * cos (tc));
00802 if (cos (lat) == 0)
00803 lon = lon1;
00804 else
00805 lon = fmod (lon1 - asin (sin (tc) * sin (d) / cos (lat)) + PI, 2. * PI) - PI;
00806 p->lat = round (lat / a);
00807 p->lon = -round (lon / a);
00808 }
00809
00810
00821 UInt16
00822 Bearing (long slat1, long slon1, long slat2, long slon2)
00823 {
00824 double const a = PI / OnePower31;
00825 double lat1 = slat1 * a,
00826 lon1 = -slon1 * a,
00827 lat2 = slat2 * a,
00828 lon2 = -slon2 * a,
00829 b;
00830 Int16 r;
00831
00832 b = fmod (atan2 (sin (lon1 - lon2) * cos (lat2),
00833 cos (lat1) * sin (lat2) - sin (lat1) * cos (lat2) * cos (lon1 - lon2)),
00834 2 * PI);
00835 r = round (180. / PI * b);
00836 debug (GPSUDBG_CALLS, ("Bearing()=%d", r >= 0 ? r : r + 360));
00837 return (r >= 0 ? r : r + 360);
00838 }
00839
00840
00853 Int16
00854 DistComp (long lat1, long lon1, long lat2, long lon2, long reflat, long reflon)
00855 {
00856 double d1,
00857 d2;
00858 d1 = PDistance (reflat, reflon, lat1, lon1);
00859 d2 = PDistance (reflat, reflon, lat2, lon2);
00860 return (d1 > d2 ? 1 : d1 == d2 ? 0 : -1);
00861 }
00862
00863
00875 double
00876 Distance (long lat1, long lon1, long lat2, long lon2, DistanceUnit unit)
00877 {
00878 double lat,
00879 lon,
00880 rlat,
00881 rlon,
00882 u,
00883 v,
00884 d;
00885 static double const a = PI / OnePower31,
00886 bnm = 3437.746771,
00887
00888 bm = 6366.707019,
00889
00890 bs = 3956.098164;
00891
00892 debug (GPSUDBG_CALLS, ("<->Distance(%ld,%ld,%ld,%ld,%d)", lat1, lon1, lat2, lon2, unit));
00893 rlat = lat1 * a;
00894 rlon = lon1 * a;
00895 lat = lat2 * a;
00896 lon = lon2 * a;
00897 u = sin ((rlat - lat) / 2.);
00898 v = sin ((rlon - lon) / 2.);
00899 d = 2. * asin (sqrt (u * u + cos (rlat) * cos (lat) * v * v));
00900 if (unit == metric)
00901 return (d * bm);
00902 else if (unit == nautical)
00903 return (d * bnm);
00904 else
00905 return (d * bs);
00906 }
00907
00908
00920 double
00921 PDistance (long lat1, long lon1, long lat2, long lon2)
00922 {
00923 double dx,
00924 dy;
00925 long l;
00926
00927 static double Pcos[33] =
00928 { 1.000000, 0.998795, 0.995185, 0.989176, 0.980785, 0.970031, 0.956940, 0.941544, 0.923879,
00929 0.903989, 0.881921, 0.857728, 0.831469, 0.803207, 0.773009, 0.740950, 0.707105,
00930 0.671558, 0.634392, 0.595698, 0.555568, 0.514101, 0.471395, 0.427553, 0.382681,
00931 0.336887, 0.290282, 0.242977, 0.195087, 0.146727, 0.098014, 0.049064, 0
00932 };
00933
00934 debug (GPSUDBG_CALLS, ("PDistance()"));
00935 dx = lon1 - lon2;
00936 l = (lat1 + lat2) / 2;
00937 if (l < 0)
00938 l = -l;
00939 dx *= Pcos[l / ((long) 1 << 25)];
00940 dy = lat1 - lat2;
00941 return (dx * dx + dy * dy);
00942 }
00943
00944
00945
00946
00954 void
00955 GpsutilSetDebugLevel (UInt16 debuglevel, LogProcPtr logprocp)
00956 {
00957 #ifdef DEBUG
00958 char debuf[256];
00959 DebugLevel = debuglevel;
00960 LogOutput = logprocp;
00961 DebugIndentLevel = 0;
00962 StrPrintF (debuf, "%s, debugging level set to %d", rcsid, DebugLevel);
00963 DebugPrintf (debuf);
00964 #endif
00965 }
00966
00967
00968
00978 void
00979 DebugPrintf (char * formatStr, ...)
00980 {
00981 #ifdef DEBUG
00982 va_list args;
00983 char buf[256];
00984
00985 if (DebugIndentLevel < 0)
00986 DebugIndentLevel = 0;
00987 else if (DebugIndentLevel > 20)
00988 DebugIndentLevel = 20;
00989 MemSet (buf, DebugIndentLevel, ' ');
00990 va_start (args, formatStr);
00991 StrVPrintF (buf + DebugIndentLevel, formatStr, args);
00992 va_end (args);
00993 if (LogOutput)
00994 LogOutput (buf);
00995 #endif
00996 }
00997
00998
01007 Boolean
01008 WptHit (Custom_Wpt_Type * wptp, Semicircle_Type * position)
01009 {
01010
01011 if (!wptp->dst)
01012 return (0);
01013 if (Distance (wptp->posn.lat, wptp->posn.lon, position->lat, position->lon, metric) <=
01014 (wptp->dst / 1000.))
01015 return (1);
01016 else
01017 return (0);
01018 }