#ident "@(#) lc_math.c         v1.0.0"/* * Copyright (C) 2002 Ricardo Arroyo <ricardo.arroyo@eresmas.net> * * This code may be used under the terms of Version 2 of the GPL, * read the file COPYING for details. * * This program 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. * * This program 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 this program; if not, write to the Free Software * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA. ******************************************************************************/#include <stdio.h>#include <errno.h>#include <stdlib.h>#include <math.h>#include "gps.h"#include "gpsdatum.h"#define EPS	0.00000000005#define MAXITER	100//Original source from: http://williams.best.vwh.net/gccalc.htm//used with author permisioninline double atan2(double y, double x){double out=0.0;  if (x <0)            { out= atan(y/x) + M_PI;}  if ((x >0) && (y>=0)){ out= atan(y/x);}  if ((x >0) && (y<0)) { out= atan(y/x) + 2.0 * M_PI;}  if ((x==0) && (y>0)) { out= M_PI_2;}  if ((x==0) && (y<0)) { out= 3.0 * M_PI_2;}  if ((x==0) && (y==0)) {out= 0.0;}  return out;}//Original source from: http://williams.best.vwh.net/gccalc.htm//used with author permision/*----------------------------------------------------------------------------*//*  FUNCTION :   dDist2Points (double dLat1, double dLon1,                    *//*                 double dLat2, double dLon2);                               *//*                                                                            *//*          .-   Calculate distance between 2 points (lat, lon)               *//*                                                                            *//* INPUT PARAMETERS        dLat1       Latitute of point 1 in degrees         *//*                         dLon1       Longitude of point 1 in degrees        *//*                                                                            *//*                         dLat2       Latitude of point 2 in degrees         *//*                         dLon2       Longitude of point 2 in degrees        *//*                                                                            *//*                         iDatumNo    No. of datum in GPS_datum (jeeps lib.) *//*                                                                            *//* OUTPUT PARAMETERS       distance in metres                                 *//* 			                                                      *//*----------------------------------------------------------------------------*/double dDist2Points (double dLat1, double dLon1, double dLat2, double dLon2, int iDatumNo){double a, f;double r, tu1, tu2, cu1, su1, cu2, s1, b1, f1;double x, sx, cx, sy, cy,y, sa, c2a, cz, e, c, d;double s;int iter=1;double rlat1, rlon1, rlat2, rlon2; /* data in radians */   sy = cy = y = c2a = cz = e = 0.; /*to avoid uninitialized message */    rlat1 = GPS_Math_Deg_To_Rad(dLat1);    rlon1 = GPS_Math_Deg_To_Rad(dLon1);    rlat2 = GPS_Math_Deg_To_Rad(dLat2);    rlon2 = GPS_Math_Deg_To_Rad(dLon2);    a = GPS_Ellipse[GPS_Datum[iDatumNo].ellipse].a;    f = 1.0/GPS_Ellipse[GPS_Datum[iDatumNo].ellipse].invf;    if ((rlat1+rlat2 == 0.0) && (abs(rlon1-rlon2) == M_PI))    {/* antipodal points */        rlat1 = rlat1 + 0.00001; // allow algorithm to complete    }    if (rlat1 == rlat2 && (rlon1 == rlon2 || abs(abs(rlon1-rlon2)-2.0 * M_PI) <  EPS))    { /* Points identical or too near */    	return 0.0;    }    r = 1 - f;    tu1 = r * tan (rlat1);    tu2 = r * tan (rlat2);    cu1 = 1.0 / sqrt (1.0 + tu1 * tu1);    su1 = cu1 * tu1;    cu2 = 1. / sqrt (1. + tu2 * tu2);    s1 = cu1 * cu2;    b1 = s1 * tu2;    f1 = b1 * tu1;    x = rlon2 - rlon1;    d = x + 1.0; // force one pass    while ((abs(d - x) > EPS) && (iter < MAXITER))    {        iter=iter + 1;        sx = sin (x);        cx = cos (x);        tu1 = cu2 * sx;        tu2 = b1 - su1 * cu2 * cx;        sy = sqrt(tu1 * tu1 + tu2 * tu2);	cy = s1 * cx + f1;        y = atan2 (sy, cy);        sa = s1 * sx / sy;        c2a = 1.0 - sa * sa;        cz = f1 + f1;        if (c2a > 0.0);        cz = cy - cz / c2a;        e = cz * cz * 2.0 - 1.0;        c = ((-3. * c2a + 4.) * f + 4.) * c2a * f / 16.;        d = x;        x = ((e * cy * c + cz) * sy * c + y) * sa;        x = (1. - c) * x * f + rlon2 - rlon1;    }    x = sqrt ((1. / (r * r) - 1.) * c2a + 1.);    x += 1.;    x = (x - 2.) / x;    c = 1. - x;    c = (x * x / 4. + 1.) / c;    d = (0.375 * x * x - 1.) * x;    x = e * cy;    s = ((((sy*sy*4.-3.)*(1.-e-e)*cz*d/6.-x)*d/4.+cz)*sy*d+y)*c*a*r;      return s;}/*----------------------------------------------------------------------------*//*  FUNCTION :   dBearing (float fEastVel, float fNorthVel)                   *//*                                                                            *//*          .-   Calculate bearing from North and East Speed                  *//*                                                                            *//* INPUT PARAMETERS        fEastVel    East component of Speed                *//*                         fNorthVel   North component of speed               *//*                                                                            *//* OUTPUT PARAMETERS       Bearing of movement in degrees                     *//* 			                                                      *//*----------------------------------------------------------------------------*/double dBearing (float fEastVel, float fNorthVel){double dOut;    dOut = atan2 ((double)fEastVel, (double) fNorthVel);    dOut = GPS_Math_Rad_To_Deg (dOut);    return dOut;}/*----------------------------------------------------------------------------*//*  FUNCTION :   dUnixtime2Windatetime(time_t tTime)                          *//*                                                                            *//*          .-   Conversts UnixTime to Windows(Excel) date/time               *//*               (integer part = number of days from 31/12/1899,              *//*                decimal part = decimal fracion of day                       *//*                00:00 is .0, 6:00 is .25, 12:00 is .5 and 18:00 is .75 )    *//*                                                                            *//* INPUT PARAMETERS        tTime	Unix time                             *//*                                                                            *//* OUTPUT PARAMETERS       double with date/time in Excel 1900 date system    *//* 			                                                      *//*----------------------------------------------------------------------------*/double dUnixtime2Windatetime(time_t tTime){double dOut;    dOut = tTime / 86400.0 + 25569.0;    return dOut;}/*----------------------------------------------------------------------------*//*  FUNCTION :   tWindatetime2Unixtime(double dWinTime)                       *//*                                                                            *//*          .-   Conversts Windows(Excel) date/time to Unixtime               *//*                                                                            *//* INPUT PARAMETERS        dWinTime	Windows time                          *//*                                                                            *//* OUTPUT PARAMETERS       time_t with Unixtime                               *//* 			                                                      *//*----------------------------------------------------------------------------*/time_t tWindatetime2Unixtime(double dWinTime){time_t tOut;    tOut = (dWinTime - 25569.0) * 86400.0 ;    return tOut;}