|
[DPRG] offroad robot exercises -- 11 Nov 06
Subject: [DPRG] offroad robot exercises -- 11 Nov 06
From: David P. Anderson
dpa at io.isem.smu.edu
Date: Thu Nov 2 02:20:02 CST 2006
Hi
robodave wrote:
> Yes, that was my point, my math was "graph paper" math, standard rectangular
> to polar, not taking into account changing distance meaning of degree
> measure at different latitudes.
Ah.
Here is some C code from Circuit Cellar and modified a bit by me to
calculate azimuth and distance from a pair of lat/lon coordinates:
regards
dpa
/* ---------------------------------------------------------------- */
/* dst.c from Circuit Cellar: http://www.circuitcellar.com/library/print/1000/Stefan123/5.htm
*
* 12 Sep 05 dpa
*
* d = acos(sin(Lat1) × sin(Lat2) + cos(Lat1) × cos(Lat2) × cos(Lon1 - Lon2))
* returns d in radians.
*
* c = acos( (sin(Lat2) - sin(Lat1) * cos(d)) / (cos(Lat1) * sin(d)) )
* returns c in radians
*/
#include <stdio.h>
#include <math.h>
#define VERSION "1.0"
#define RADS (180/M_PI)
#define NAUTICALMILES 3437.7387
#define FEET_PER_NM 6076.11549
#define METERS_PER_NM 1852
int main(argc,argv)
int argc;
char **argv;
{
int i;
double lat1, lon1, lat2, lon2, distance, distance_NM;
double azimuth, azimuth_deg;
if (argc < 5) {
fprintf(stderr,"%s %s\n",argv[0],VERSION);
fprintf(stderr,"USEAGE: %s lat1 lon1 lat2 lon2 > azimuth distance\n",argv[0]);
exit (-1);
}
i = sscanf(argv[1],"%lf",&lat1);
i += sscanf(argv[2],"%lf",&lon1);
i += sscanf(argv[3],"%lf",&lat2);
i += sscanf(argv[4],"%lf",&lon2);
// printf("%f %f %f %f\n",lat1,lon1,lat2,lon2);
/* calculate great circle distance */
distance = acos(sin(lat1/RADS)*sin(lat2/RADS)+cos(lat1/RADS)*cos(lat2/RADS)*cos((lon1-lon2)/RADS));
distance_NM = distance * NAUTICALMILES;
/*
printf("%f NM, %f m, %f f, %f in\n",
distance,distance_NM*METERS_PER_NM,distance_NM*FEET_PER_NM,distance_NM*FEET_PER_NM*12);
*/
/* calculate azimuth */
azimuth = acos((sin(lat2/RADS) - sin(lat1/RADS) * cos(distance))
/ (cos(lat1/RADS) * sin(distance)));
azimuth_deg = azimuth * RADS;
if (sin((lon2/RADS) - (lon1/RADS)) < 0.0) {
azimuth_deg = 360.0 - azimuth_deg;
}
/*
printf("azimuth = %f, %f degrees\n",azimuth,azimuth_deg);
*/
printf("%.2f deg %.2f meters %.2f feet %.0f inches\n",
azimuth_deg,distance_NM*METERS_PER_NM,distance_NM*FEET_PER_NM,
distance_NM*FEET_PER_NM*12);
exit (0);
}
/* ------------------------------------------------------------------ */
/* EOF */
More information about the DPRG mailing list
|
