41 #include "./../KDefines.h"
43 #ifndef _USE_MATH_DEFINES
44 #define _USE_MATH_DEFINES
59 return Rad * ( 180.0 / M_PI );
65 return Deg * ( M_PI / 180.0 );
83 return Meters * 3.2808;
131 MajorAxis = 6377563.396;
132 MinorAxis = 6356256.909;
137 MajorAxis = 6377340.189;
138 MinorAxis = 6356034.448;
143 MajorAxis = 6378160.000;
144 MinorAxis = 6356774.719;
149 MajorAxis = 6377397.155;
150 MinorAxis = 6356078.963;
155 MajorAxis = 6377483.865;
156 MinorAxis = 6356078.963;
161 MajorAxis = 6378206.400;
162 MinorAxis = 6356583.800;
167 MajorAxis = 6378249.145;
168 MinorAxis = 6356514.870;
173 MajorAxis = 6377298.556;
174 MinorAxis = 6356097.550;
179 MajorAxis = 6377276.345;
180 MinorAxis = 6356075.413;
185 MajorAxis = 6377304.063;
186 MinorAxis = 6356103.039;
191 MajorAxis = 6377301.243;
192 MinorAxis = 6356100.228;
197 MajorAxis = 6377295.664;
198 MinorAxis = 6356094.668;
203 MajorAxis = 6378166.000;
204 MinorAxis = 6356784.284;
209 MajorAxis = 6378155.000;
210 MinorAxis = 6356773.320;
215 MajorAxis = 6378150.000;
216 MinorAxis = 6356768.337;
221 MajorAxis = 6378137.000;
222 MinorAxis = 6356752.314;
227 MajorAxis = 6378200.000;
228 MinorAxis = 6356818.170;
233 MajorAxis = 6378270.000;
234 MinorAxis = 6356794.343;
239 MajorAxis = 6378388.000;
240 MinorAxis = 6356911.946;
245 MajorAxis = 6378245.000;
246 MinorAxis = 6356863.019;
251 MajorAxis = 6378136.000;
252 MinorAxis = 6356751.302;
257 MajorAxis = 6378160.000;
258 MinorAxis = 6356774.719;
268 MajorAxis = 6378165.000;
269 MinorAxis = 6356783.287;
274 MajorAxis = 6378145.000;
275 MinorAxis = 6356759.769;
280 MajorAxis = 6378135.000;
281 MinorAxis = 6356750.520;
286 MajorAxis = 6378137.000;
287 MinorAxis = 6356752.314245;
296 inline void DecimalToDMS( Type Decimal, Type & DegOUT, Type & MinOUT, Type & SecOUT )
298 Decimal = abs( Decimal );
302 MinOUT = Decimal * 60.0;
304 Decimal = MinOUT - (
KUINT32 )MinOUT;
307 SecOUT = Decimal * 60.0;
315 return Deg + ( Min / 60.0 ) + ( Sec / 3600.0 );
334 Type & GeocentricX, Type & GeocentricY, Type & GeocentricZ,
337 GeodeticLat =
DegToRad( GeodeticLat );
338 GeodeticLon =
DegToRad( GeodeticLon );
340 Type MajorAxis, MinorAxis;
343 Type Esq = ( pow( MajorAxis, 2 ) - pow( MinorAxis, 2 ) ) / pow( MajorAxis, 2 );
344 Type V = MajorAxis / sqrt( 1 - ( Esq * pow( sin( GeodeticLat ), 2 ) ) );
346 GeocentricX = ( V + GeodeticHeight ) * cos( GeodeticLat ) * cos( GeodeticLon );
347 GeocentricY = ( V + GeodeticHeight ) * cos( GeodeticLat ) * sin( GeodeticLon );
348 GeocentricZ = ( ( 1 - Esq ) * V + GeodeticHeight ) * sin( GeodeticLat );
373 Type
const e2 = 1.0 - ( b * b ) / ( a * a ) ;
374 Type
const ed2 = ( a * a ) / ( b * b ) - 1.0 ;
375 Type
const a2 = a * a ;
376 Type
const b2 = b * b ;
377 Type
const z2 = z * z ;
378 Type
const e4 = e2 * e2 ;
379 Type
const r2 = x * x + y * y ;
380 Type
const r = sqrt( r2 );
384 Type F = 54.0 * b2 * z2 ;
386 Type G = r2 + ( 1.0 - e2 ) * z2 - e2 * E2 ;
388 Type C = e4 * F * r2 / ( G * G * G );
390 Type S = pow( 1.0 + C + sqrt( C * C + 2.0 * C ) , 1.0 / 3.0 );
392 t = S + 1.0 / S + 1.0 ;
394 Type P = F / ( 3.0 * t * t * G * G );
396 Type Q = sqrt( 1.0 + 2.0 * e4 * P );
398 Type r0 = -( P * e2 * r ) / ( 1.0 + Q ) + sqrt( 0.5 * a2 * ( 1.0 + 1.0 / Q ) - ( P * ( 1 - e2 ) * z2 ) / ( Q * ( 1.0 + Q ) ) - 0.5 * P * r2 );
401 Type U = sqrt( t * t + z2 );
402 Type V = sqrt( t * t + ( 1.0 - e2 ) * z2 );
406 alt = U * ( 1.0 - t );
407 lat = atan2( z + ed2 * t * z , r );
420 double st = sin( t );
421 double ct = cos( t );
423 d[0] = (1.0-ct)*(n[0]*n[0]*s[0] + n[0]*n[1]*s[1] + n[0]*n[2]*s[2]) + ct*s[0] + st*(n[1]*s[2]-n[2]*s[1]);
424 d[1] = (1.0-ct)*(n[0]*n[1]*s[0] + n[1]*n[1]*s[1] + n[1]*n[2]*s[2]) + ct*s[1] + st*(n[2]*s[0]-n[0]*s[2]);
425 d[2] = (1.0-ct)*(n[0]*n[2]*s[0] + n[1]*n[2]*s[1] + n[2]*n[2]*s[2]) + ct*s[2] + st*(n[0]*s[1]-n[1]*s[0]);
429 inline void Cross( Type d[3] ,Type
const a[3] ,Type
const b[3] )
431 d[0] = a[1] * b[2] - b[1] * a[2] ;
432 d[1] = b[0] * a[2] - a[0] * b[2] ;
433 d[2] = a[0] * b[1] - b[0] * a[1] ;
437 inline double Dot( Type
const a[3] ,Type
const b[3] )
439 return a[0]*b[0] + a[1]*b[1] + a[2]*b[2];
460 Type
const D0[3] = { 1.0 , 0.0 , 0.0 };
461 Type
const E0[3] = { 0.0 , 1.0 , 0.0 };
462 Type
const N0[3] = { 0.0 , 0.0 , 1.0 };
480 Type N1[3] , E1[3] , D1[3] ;
483 memcpy( D1 , D ,
sizeof( Type[3] ) );
485 Type N2[3] , E2[3] , D2[3] ;
487 memcpy( E2 , E1 ,
sizeof( Type[3] ) );
490 Type N3[3] , E3[3] , D3[3] ;
491 memcpy( N3 , N2 ,
sizeof( Type[3] ) );
496 Type x0[3] = { 1.0 , 0.0 , 0.0 };
497 Type y0[3] = { 0.0 , 1.0 , 0.0 };
498 Type z0[3] = { 0.0 , 0.0 , 1.0 };
501 Psi = atan2(
Dot( N3 , y0 ) ,
Dot( N3 , x0 ) );
502 Theta = atan2( -
Dot( N3 , z0 ) , sqrt( pow(
Dot( N3 , x0 ), 2) + pow(
Dot( N3 , y0 ), 2) ) );
505 Phi = atan2(
Dot( E3 , z2 ) ,
Dot( E3 , y2 ) );
533 Type
const D0[3] = { 1. , 0. , 0. };
534 Type
const E0[3] = { 0. , 1. , 0. };
535 Type
const N0[3] = { 0. , 0. , 1. };
552 Type X[]= {1.,0.,0.};
553 Type Y[]= {0.,1.,0.};
554 Type Z[]= {0.,0.,1.};
555 Type X1[3] , Y1[3] , Z1[3] ;
558 memcpy( Z1 , Z ,
sizeof( Type[3] ) );
560 Type X2[3] , Y2[3] , Z2[3] ;
562 memcpy( Y2 , Y1 ,
sizeof( Type[3] ) );
565 Type X3[3] , Y3[3] , Z3[3] ;
566 memcpy( X3 , X2 ,
sizeof( Type[3] ) );
571 memcpy( x0 , N ,
sizeof( Type[3] ) );
573 memcpy( y0 , E ,
sizeof( Type[3] ) );
575 memcpy( z0 , D ,
sizeof( Type[3] ) );
578 H = atan2(
Dot( X3 , y0 ) ,
Dot( X3 , x0 ) );
579 P = atan2( -
Dot( X3 , z0 ) , sqrt( pow(
Dot( X3 , x0 ), 2) + pow(
Dot( X3 , y0 ), 2) ) );
582 R = atan2(
Dot( Y3 , z2 ) ,
Dot( Y3 , y2 ) );
void GetEllipsoidAxis(RefEllipsoid R, Type &MajorAxis, Type &MinorAxis)
Definition: KConversions.h:126
unsigned int KUINT32
Definition: KDefines.h:103
Definition: KConversions.h:100
Definition: KConversions.h:95
Type DegToRad(Type Deg)
Definition: KConversions.h:63
Definition: KConversions.h:94
Definition: KConversions.h:110
Definition: KConversions.h:119
void Cross(Type d[3], Type const a[3], Type const b[3])
Definition: KConversions.h:429
void GeocentricToGeodetic(Type x, Type y, Type z, Type &lat, Type &lon, Type &alt, RefEllipsoid R)
Description: Converts Geocentric Cartesian coords to Geodetic.
Definition: KConversions.h:367
Definition: KConversions.h:109
Definition: KConversions.h:106
void DecimalToDMS(Type Decimal, Type &DegOUT, Type &MinOUT, Type &SecOUT)
Definition: KConversions.h:296
Definition: KConversions.h:102
RefEllipsoid
Definition: KConversions.h:92
Definition: KConversions.h:101
Definition: KConversions.h:108
Type DMSToDecimal(Type Deg, Type Min, Type Sec)
Definition: KConversions.h:313
Definition: KConversions.h:107
Definition: KConversions.h:104
double Dot(Type const a[3], Type const b[3])
Definition: KConversions.h:437
Definition: KConversions.h:116
Definition: KConversions.h:115
Type FeetToMeters(Type Feet)
Definition: KConversions.h:73
void HeadingPitchRollToEuler(Type H, Type P, Type R, Type Lat, Type Lon, Type &Psi, Type &Theta, Type &Phi)
Description: Converts Heading, Pitch and Roll to Euler for DIS.
Definition: KConversions.h:456
void EulerToHeadingPitchRoll(Type Lat, Type Lon, Type Psi, Type Theta, Type Phi, Type &H, Type &P, Type &R)
Description: Converts Euler to Heading, Pitch and Roll.
Definition: KConversions.h:524
Definition: KConversions.h:118
Definition: KConversions.h:117
Definition: KConversions.h:99
Definition: KConversions.h:105
Definition: KConversions.h:113
Type RadToDeg(Type Rad)
Definition: KConversions.h:57
void RotateAboutAxis(Type d[3], Type const s[3], Type const n[3], Type t)
Definition: KConversions.h:418
Definition: KConversions.h:111
Type MetersToFeet(Type Meters)
Definition: KConversions.h:81
Definition: KConversions.h:120
Definition: KConversions.h:96
Definition: KConversions.h:114
Definition: KConversions.h:98
void GeodeticToGeocentric(Type GeodeticLat, Type GeodeticLon, Type GeodeticHeight, Type &GeocentricX, Type &GeocentricY, Type &GeocentricZ, RefEllipsoid R)
Description: Converts Geodetic coords to Geocentric Cartesian.
Definition: KConversions.h:333
Definition: KConversions.h:112
Definition: KConversions.h:103
Definition: KConversions.h:97