DPRG
DPRG List  



[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