lc_math.c

open source for car navigation in linux

#ident "@(#) lc_math.c         v1.0.0"
/*
 * Copyright (C) 2002 Ricardo Arroyo <[e-mail]>
 *
 * 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 permision
inline 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;
}