~csiro-asl/csiro-asl-ros-drivers/trunk

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
/*
 * Author Hendrik Erckens, Adrian Bonchis and Nick Hillier
 * 
 * Implementation of Redfearn's formula available from:
 * http://www.ga.gov.au/cedda/applications/458 (accessed 6/07/2011)
 * (Not sure how generic this is, or if it's Australia or Southern 
 * Hemisphere only)
 * 
 */
/*
 * 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 Lesser General Public License for more details.
 * 
 * You should have received a copy of the GNU Lesser General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>. 
 */


#include <math.h>                                                                                                                                            


/**
 * convert GPS latitude and longitude to Easting and Northing
 */
#define a_sma 6378137.00        /* semi-major axis */
#define f (1/298.257222101)        /* flattening, GRS80 */
#define ex2 (2.0*f - pow(f,2))	/* eccentricity powers (2, 4, 6) */
#define ex4 (ex2*ex2)
#define ex6 (ex4*ex2)
#define False_east 500000.0     /* false easting (m) */
#define False_north 10000000.0      /* false northing (m) */
#define K0 0.9996           /* central scale factor */


int latlong2eastnorth(double latitude, double longitude, double *easting, double *northing, int *zone)
{
	double phi = latitude;
	double lambda = longitude;
	double coslat1, coslat2, coslat3, coslat4, coslat5, coslat6, coslat7, coslat8;
	double A0, A2, A4, A6, m;
	double rho, niu, psi, psi1, psi2, psi3, psi4;
	double C23, C24, C25, C26, L4, C5, C6, C7, w;
	double w1, w2, w3, w4, w5, w6, w7, w8; 
	double tan1, tan2, tan4, tan6;
	double Term1, Term2, Term3, Term4, SumE, E1, SumN, N1;
	double Easting, Northing;

	
	phi = phi * M_PI / 180.0;

	coslat1 = cos(phi);
	coslat2 = coslat1*coslat1;
	coslat3 = coslat2*coslat1;
	coslat4 = coslat3*coslat1;
	coslat5 = coslat4*coslat1;
	coslat6 = coslat5*coslat1;
	coslat7 = coslat6*coslat1;
	coslat8 = coslat7*coslat1;


	// meridian distance
	A0 = 1.0 - (ex2 / 4.0) - (3.0 * ex4 / 64.0) - (5.0 * ex6 / 256.0);
	A2 = (3.0/8.0)*(ex2+ex4/4.0+15.0*ex6/128.0);
	A4 = (15.0/256.0)*(ex4+3.0*ex6/4.0);
	A6 = 35.0*ex6/3072.0;

	m = a_sma*(A0*phi - A2*sin(2.0*phi) + A4*sin(4.0*phi) - A6*sin(6.0*phi));

	// radius of curvature for given latitude
	rho = a_sma*(1.0-ex2)/pow((1.0-ex2*sin(phi)*sin(phi)),1.5);
	niu = a_sma/pow((1.0-ex2*sin(phi)*sin(phi)),0.5);
	psi = niu/rho;

	psi1 = psi;
	psi2 = psi1*psi1;
	psi3 = psi2*psi1;
	psi4 = psi3*psi1;

	C23 = 6.0; // Zone width (degrees)
	C24 = -177.0; // Meridian of zone 1 (degrees)
	C25 = C24 - 1.5*C23; // Longitude of Western Edge of Zone 0
	C26 = C25 + 0.5*C23; // Central Meridian of Zone 0
	L4 = lambda; // degrees in decimal format
	C5 = (L4-C25)/C23; // Zone as a real number
	C7 = floor(C5); // Zone number
	C6 = C7*C23+C26; // Central Meridian
	w = L4 - C6;

	w1 = w*M_PI/180.0;
	w2 = w1*w1;
	w3 = w2*w1;
	w4 = w3*w1;
	w5 = w4*w1;
	w6 = w5*w1;
	w7 = w6*w1;
	w8 = w7*w1;

	tan1 = tan(phi);
	tan2 = tan1*tan1;
	tan4 = tan2*tan2;
	tan6 = tan4*tan2;

	Term1 = niu*w1*coslat1;
	Term2 = niu*w1*coslat1*(w2/6.0)*coslat2*(psi1-tan2);
	Term3 = niu*w1*coslat1*(w4/120.0)*coslat4*(4.0*psi3*(1.0-6.0*tan2) + psi2*(1.0+8.0*tan2) - psi1*2.0*tan2 +tan4);
	Term4 = niu*w1*coslat1*(w6/5040.0)*coslat6*(61.0 - 479.0*tan2 + 179.0*tan4 - tan6);
	SumE = Term1+Term2+Term3+Term4;
	E1 = K0*SumE;
	Easting = E1 + False_east;

	Term1 = (w2/2.0)*niu*sin(phi)*coslat1;
	Term2 = (w4/24.0)*niu*sin(phi)*coslat3*(4.0*psi2+psi1-tan2);
	Term3 = (w6/720.0)*niu*sin(phi)*coslat5*(8.0*psi4*(11.0-24.0*tan2)-28.0*psi3*(1.0-6.0*tan2)+psi2*(1.0-32.0*tan2)-psi*2.0*tan2+tan4);
	Term4 = (w8/40320.0)*niu*sin(phi)*coslat7*(1385.0-3111.0*tan2+534.0*tan4-tan6);
	SumN = m+Term1+Term2+Term3+Term4;
	N1 = K0*SumN;
	Northing = N1+False_north;

	/* return calc values */
	*(northing) = Northing;
	*(easting) = Easting;
	*(zone) = C7;

	return 0;
}