include/KDIS/Extras/KConversions.hpp

   1 /*********************************************************************
   2 Copyright 2013 Karl Jones
   3 All rights reserved.
   4
   5 Redistribution and use in source and binary forms, with or without
   6 modification, are permitted provided that the following conditions are met:
   7
   8 1. Redistributions of source code must retain the above copyright notice, this
   9    list of conditions and the following disclaimer.
  10 2. Redistributions in binary form must reproduce the above copyright notice,
  11    this list of conditions and the following disclaimer in the documentation
  12    and/or other materials provided with the distribution.
  13
  14 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
  15 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
  16 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
  17 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
  18 ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
  19 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
  20 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
  21 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
  22 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
  23 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  24
  25 For Further Information Please Contact me at
  26 Karljj1@yahoo.com
  27 http://p.sf.net/kdis/UserGuide
  28 *********************************************************************/
  29
  30 /********************************************************************
  31     KConversions
  32     created:    18/12/2008
  33     author:     Karl Jones
  34
  35     purpose:    Holds all common conversions, Datum conversion,
  36                 unit conversion etc
  37 *********************************************************************/
  38
  39 #pragma once
  40
  41 #include "./../KDefines.h"
  42 #include <cmath>
  43
  44 #ifndef KDIS_PI
  45     #define KDIS_PI 3.14159265358979323846
  46 #endif
  47
  48 namespace KDIS {
  49 namespace UTILS {
  50
  51 /************************************************************************/
  52 /* Conversions to and from radians / degrees                            */
  53 /************************************************************************/
  54
  55 template<class Type>
  56 inline Type RadToDeg( Type Rad )
  57 {
  58     return static_cast<Type>(Rad * ( 180.0 / KDIS_PI ));
  59 };
  60
  61 template<class Type>
  62 inline Type DegToRad( Type Deg )
  63 {
  64     return static_cast<Type>(Deg * ( KDIS_PI / 180.0 ));
  65 };
  66
  67 /************************************************************************/
  68 /* Conversions to and from feet / meters                                */
  69 /************************************************************************/
  70
  71 template<class Type>
  72 inline Type FeetToMeters( Type Feet )
  73 {
  74     return static_cast<Type>(Feet / 3.2808);
  75 };
  76
  77 //////////////////////////////////////////////////////////////////////////
  78
  79 template<class Type>
  80 inline Type MetersToFeet( Type Meters )
  81 {
  82     return static_cast<Type>(Meters * 3.2808);
  83 };
  84
  85 /************************************************************************/
  86 /* Conversions to and from Geocentric / Geodetic - ECEF / LLA           */
  87 /************************************************************************/
  88
  89 // Reference Ellipsoids, data taken from Wikipedia and DoD, WGS84, DMA TR 8350.2-B,1 Sept. 1991
  90
  91 enum RefEllipsoid
  92 {
  93     Airy,
  94     Airy_Modified,
  95     Australian_National,
  96     Bessel_1841,
  97     Bessel_1841_Namibia,
  98     Clarke_1866,
  99     Clarke_1880,
 100     Everest_Sabah_Sarawak,
 101     Everest_1830,
 102     Everest_1948,
 103     Everest_1956,
 104     Everest_1969,
 105     Fischer_1960,
 106     Fischer_1960_Modified,
 107     Fischer_1968,
 108     GRS_1980,
 109     Helmert_1906,
 110     Hough,
 111     International_1924,
 112     Karsovsky_1940,
 113     SGS_1985,
 114     South_American_1969,
 115     Sphere_6371km,
 116     WGS_1960,
 117     WGS_1966,
 118     WGS_1972,
 119     WGS_1984
 120 };
 121
 122 //////////////////////////////////////////////////////////////////////////
 123
 124 template<class Type>
 125 inline void GetEllipsoidAxis( RefEllipsoid R, Type & MajorAxis, Type & MinorAxis )
 126 {
 127     switch( R )
 128     {
 129     case Airy:
 130         MajorAxis = static_cast<Type>(6377563.396);
 131         MinorAxis = static_cast<Type>(6356256.909);
 132         // 1/F 299.324965
 133         break;
 134
 135     case Airy_Modified:
 136         MajorAxis = static_cast<Type>(6377340.189);
 137         MinorAxis = static_cast<Type>(6356034.448);
 138         // 1/F 299.324965
 139         break;
 140
 141     case Australian_National:
 142         MajorAxis = static_cast<Type>(6378160.000);
 143         MinorAxis = static_cast<Type>(6356774.719);
 144         // 1/F 298.250000
 145         break;
 146
 147     case Bessel_1841:
 148         MajorAxis = static_cast<Type>(6377397.155);
 149         MinorAxis = static_cast<Type>(6356078.963);
 150         // 1/F 299.152813
 151         break;
 152
 153     case Bessel_1841_Namibia:
 154         MajorAxis = static_cast<Type>(6377483.865);
 155         MinorAxis = static_cast<Type>(6356078.963);
 156         // 1/F 299.152813
 157         break;
 158
 159     case Clarke_1866:
 160         MajorAxis = static_cast<Type>(6378206.400);
 161         MinorAxis = static_cast<Type>(6356583.800);
 162         // 1/F 294.978698
 163         break;
 164
 165     case Clarke_1880:
 166         MajorAxis = static_cast<Type>(6378249.145);
 167         MinorAxis = static_cast<Type>(6356514.870);
 168         // 1/F 293.465000
 169         break;
 170
 171     case Everest_Sabah_Sarawak:
 172         MajorAxis = static_cast<Type>(6377298.556);
 173         MinorAxis = static_cast<Type>(6356097.550);
 174         // 1/F 300.801700
 175         break;
 176
 177     case Everest_1830:
 178         MajorAxis = static_cast<Type>(6377276.345);
 179         MinorAxis = static_cast<Type>(6356075.413);
 180         // 1/F 300.801700
 181         break;
 182
 183     case Everest_1948:
 184         MajorAxis = static_cast<Type>(6377304.063);
 185         MinorAxis = static_cast<Type>(6356103.039);
 186         // 1/F 300.801700
 187         break;
 188
 189     case Everest_1956:
 190         MajorAxis = static_cast<Type>(6377301.243);
 191         MinorAxis = static_cast<Type>(6356100.228);
 192         // 1/F 300.801700
 193         break;
 194
 195     case Everest_1969:
 196         MajorAxis = static_cast<Type>(6377295.664);
 197         MinorAxis = static_cast<Type>(6356094.668);
 198         // 1/F 300.801700
 199         break;
 200
 201     case Fischer_1960:
 202         MajorAxis = static_cast<Type>(6378166.000);
 203         MinorAxis = static_cast<Type>(6356784.284);
 204         // 1/F 298.300000
 205         break;
 206
 207     case Fischer_1960_Modified:
 208         MajorAxis = static_cast<Type>(6378155.000);
 209         MinorAxis = static_cast<Type>(6356773.320);
 210         // 1/F 298.300000
 211         break;
 212
 213     case Fischer_1968:
 214         MajorAxis = static_cast<Type>(6378150.000);
 215         MinorAxis = static_cast<Type>(6356768.337);
 216         // 1/F 298.300000
 217         break;
 218
 219     case GRS_1980:
 220         MajorAxis = static_cast<Type>(6378137.000);
 221         MinorAxis = static_cast<Type>(6356752.314);
 222         // 1/F 298.257222
 223         break;
 224
 225     case Helmert_1906:
 226         MajorAxis = static_cast<Type>(6378200.000);
 227         MinorAxis = static_cast<Type>(6356818.170);
 228         // 1/F 298.300000
 229         break;
 230
 231     case Hough:
 232         MajorAxis = static_cast<Type>(6378270.000);
 233         MinorAxis = static_cast<Type>(6356794.343);
 234         // 1/F 297.000000
 235         break;
 236
 237     case International_1924:
 238         MajorAxis = static_cast<Type>(6378388.000);
 239         MinorAxis = static_cast<Type>(6356911.946);
 240         // 1/F 297.000000
 241         break;
 242
 243     case Karsovsky_1940:
 244         MajorAxis = static_cast<Type>(6378245.000);
 245         MinorAxis = static_cast<Type>(6356863.019);
 246         // 1/F 298.300000
 247         break;
 248
 249     case SGS_1985:
 250         MajorAxis = static_cast<Type>(6378136.000);
 251         MinorAxis = static_cast<Type>(6356751.302);
 252         // 1/F 298.257000
 253         break;
 254
 255     case South_American_1969:
 256         MajorAxis = static_cast<Type>(6378160.000);
 257         MinorAxis = static_cast<Type>(6356774.719);
 258         // 1/F 298.250000
 259         break;
 260
 261     case Sphere_6371km:
 262         MajorAxis = static_cast<Type>(6371000);
 263         MinorAxis = static_cast<Type>(6371000);
 264         break;
 265
 266     case WGS_1960:
 267         MajorAxis = static_cast<Type>(6378165.000);
 268         MinorAxis = static_cast<Type>(6356783.287);
 269         // 1/F 298.300000
 270         break;
 271
 272     case WGS_1966:
 273         MajorAxis = static_cast<Type>(6378145.000);
 274         MinorAxis = static_cast<Type>(6356759.769);
 275         // 1/F 298.250000
 276         break;
 277
 278     case WGS_1972:
 279         MajorAxis = static_cast<Type>(6378135.000);
 280         MinorAxis = static_cast<Type>(6356750.520);
 281         // 1/F 298.260000
 282         break;
 283
 284     case WGS_1984:
 285         MajorAxis = static_cast<Type>(6378137.000);
 286         MinorAxis = static_cast<Type>(6356752.314245);
 287         // 1/F 298.257224
 288         break;
 289     }
 290 };
 291
 292 //////////////////////////////////////////////////////////////////////////
 293
 294 template<class Type>
 295 inline void DecimalToDMS( Type Decimal, Type & DegOUT, Type & MinOUT, Type & SecOUT )
 296 {
 297     Decimal = std::abs( Decimal ); // Make sure the value is not negative
 298     DegOUT = static_cast<Type>(( KUINT32 )Decimal);
 299     Decimal -= DegOUT; // Degrees
 300
 301     MinOUT = static_cast<Type>(Decimal * 60.0);
 302
 303     Decimal = MinOUT - static_cast<Type>(( KUINT32 )MinOUT);
 304
 305     MinOUT = static_cast<Type>(( KUINT32 )MinOUT); // Minutes
 306     SecOUT = static_cast<Type>(Decimal * 60.0); // Seconds
 307 };
 308
 309 //////////////////////////////////////////////////////////////////////////
 310
 311 template<class Type>
 312 inline Type DMSToDecimal( Type Deg, Type Min, Type Sec )
 313 {
 314     return static_cast<Type>(Deg + ( Min / 60.0 ) + ( Sec / 3600.0 ));
 315 };
 316
 317 //////////////////////////////////////////////////////////////////////////
 318
 319 //************************************
 320 // FullName:    KDIS::UTILS<Type>::GeodeticToGeocentric
 321 // Description: Converts Geodetic coords to Geocentric Cartesian.
 322 // Parameter:   Type GeodeticLat - in degrees
 323 // Parameter:   Type GeodeticLon - in degrees
 324 // Parameter:   Type GeodeticHeight - in meters
 325 // Parameter:   Type & GeocentricX
 326 // Parameter:   Type & GeocentricY
 327 // Parameter:   Type & GeocentricZ
 328 // Parameter:   RefEllipsoid R
 329 //************************************
 330
 331 template<class Type>
 332 inline void GeodeticToGeocentric( Type GeodeticLat, Type GeodeticLon, Type GeodeticHeight,
 333                                   Type & GeocentricX, Type & GeocentricY, Type & GeocentricZ,
 334                                   RefEllipsoid R )
 335 {
 336     GeodeticLat = DegToRad( GeodeticLat );
 337     GeodeticLon = DegToRad( GeodeticLon );
 338
 339     Type MajorAxis, MinorAxis;
 340     GetEllipsoidAxis( R, MajorAxis, MinorAxis );
 341
 342     Type Esq = ( pow( MajorAxis, 2 ) - pow( MinorAxis, 2 ) ) / pow( MajorAxis, 2 );
 343     Type V = MajorAxis / sqrt( 1 - ( Esq * pow( sin( GeodeticLat ), 2 ) ) );
 344
 345     GeocentricX = ( V + GeodeticHeight ) * cos( GeodeticLat ) * cos( GeodeticLon );
 346     GeocentricY = ( V + GeodeticHeight ) * cos( GeodeticLat ) * sin( GeodeticLon );
 347     GeocentricZ = ( ( 1 - Esq ) * V + GeodeticHeight ) * sin( GeodeticLat );
 348 };
 349
 350 //////////////////////////////////////////////////////////////////////////
 351
 352 //************************************
 353 // FullName:    KDIS::UTILS<Type>::GeocentricToGeodetic
 354 // Description: Converts Geocentric Cartesian coords to Geodetic.
 355 // Parameter:   Type x - in radians
 356 // Parameter:   Type y - in radians
 357 // Parameter:   Type z - in radians
 358 // Parameter:   Type & lat
 359 // Parameter:   Type & lon
 360 // Parameter:   Type & alt
 361 // Parameter:   RefEllipsoid R
 362 //************************************
 363
 364 template<class Type>
 365 inline void GeocentricToGeodetic( Type x, Type y, Type z,Type & lat, Type & lon, Type & alt, RefEllipsoid R )
 366 {
 367     // This is the 'closed form solution'
 368     // equations described by https://microem.ru/files/2012/08/GPS.G1-X-00006.pdf
 369     // and many other places on the web. start at wikipedia, "ECEF"
 370
 371     Type  a;   // semi-major axis
 372     Type  b;   // semi-minor axis
 373     GetEllipsoidAxis( R, a, b );
 374
 375     Type  const e2       = (a*a-b*b) / (a*a); // eccentricity (first)
 376     Type  const e_prime2 = (a*a-b*b) / (b*b); // eccentricity (second)
 377     Type  const f        =  1 - b/a ;   // flattening
 378
 379     // 'auxiliary values'
 380     Type const p = sqrt(x*x + y*y);
 381     Type const theta = static_cast<Type>(atan2 ( (z*a) , (p*b)));
 382
 383     // latitude
 384     lat = static_cast<Type>(atan2 ( ( z + (e_prime2) * b * pow (sin(theta),3) ),
 385                                     ( p - (e2*a*pow(cos(theta),3)))));
 386
 387     // Radius of curvature
 388     Type const N = a / sqrt ( 1 - e2*sin(lat)*sin(lat));
 389
 390     // altitude
 391     Type cosLat = cos(lat);
 392     Type const COS_THRESHOLD = 0.0000001;
 393     if( (cosLat > -COS_THRESHOLD) && (cosLat < COS_THRESHOLD) ) // Very near the poles
 394         alt = std::abs( z ) - b;
 395     else
 396         alt = p / cosLat - N;
 397
 398     // longitude
 399     lon = static_cast<Type>(atan2(y,x));
 400
 401     lon = RadToDeg( lon );
 402     lat = RadToDeg( lat );
 403 };
 404
 405 //////////////////////////////////////////////////////////////////////////
 406
 407 template<class Type>
 408 inline void RotateAboutAxis( Type d[3] ,Type const s[3] ,Type const n[3] ,Type  t )
 409 {
 410     double  st = sin( t );
 411     double  ct = cos( t );
 412
 413     d[0] = static_cast<Type>((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]));
 414     d[1] = static_cast<Type>((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]));
 415     d[2] = static_cast<Type>((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]));
 416 };
 417
 418 template<class Type>
 419 inline void Cross( Type  d[3] ,Type const  a[3] ,Type const b[3] )
 420 {
 421     d[0] = a[1] * b[2] - b[1] * a[2] ;
 422     d[1] = b[0] * a[2] - a[0] * b[2] ;
 423     d[2] = a[0] * b[1] - b[0] * a[1] ;
 424 };
 425
 426 template<class Type>
 427 inline Type Dot( Type const a[3] ,Type const b[3] )
 428 {
 429     return  a[0]*b[0] + a[1]*b[1] + a[2]*b[2];
 430 };
 431
 432 //************************************
 433 // FullName:    KDIS::UTILS<Type>::HeadingPitchRollToEuler
 434 // Description: Converts Heading, Pitch and Roll to Euler for DIS.
 435 // Parameter:   Type H - Heading in radians
 436 // Parameter:   Type P - Pitch in radians
 437 // Parameter:   Type R - Roll in radians
 438 // Parameter:   Type Lat - Geodetic Latitude in radians
 439 // Parameter:   Type Lon - Geodetic Longitude in radians
 440 // Parameter:   Type & Psi - Euler angle out
 441 // Parameter:   Type & Theta - Euler angle out
 442 // Parameter:   Type & Phi - Euler angle out
 443 //************************************
 444
 445 template<class Type>
 446 inline void HeadingPitchRollToEuler( Type H, Type P, Type R, Type Lat, Type Lon, Type & Psi, Type & Theta,
 447                                      Type & Phi )
 448 {
 449     // local NED
 450     //Type const  D0[3] = { 1.0 , 0.0 , 0.0 };
 451     Type const  E0[3] = { 0.0 , 1.0 , 0.0 };
 452     Type const  N0[3] = { 0.0 , 0.0 , 1.0 };
 453     Type        me[3];
 454     Type        N[3];
 455     Type        E[3];
 456     Type        D[3];
 457     // 'E'
 458     RotateAboutAxis( E , E0 , N0 , Lon );
 459     me[0] = -E[0] ;
 460     me[1] = -E[1] ;
 461     me[2] = -E[2] ;
 462     // 'N'
 463     RotateAboutAxis( N , N0 , me , Lat );
 464     // 'D'
 465     Cross( D , N , E );
 466     /*
 467     *  Orientation
 468     */
 469     // rotate about D by heading
 470     Type  N1[3] , E1[3] , D1[3] ;
 471     RotateAboutAxis( N1 , N  , D , H );
 472     RotateAboutAxis( E1 , E  , D , H );
 473     memcpy( D1 , D  , sizeof( Type[3] ) );
 474     // rotate about E1 vector by pitch
 475     Type  N2[3] , E2[3] , D2[3] ;
 476     RotateAboutAxis( N2 , N1 , E1 , P );
 477     memcpy( E2 , E1 , sizeof( Type[3] ) );
 478     RotateAboutAxis( D2 , D1 , E1 , P );
 479     // rotate about N2 by roll
 480     Type  N3[3] , E3[3] , D3[3] ;
 481     memcpy( N3 , N2 , sizeof( Type[3] ) );
 482     RotateAboutAxis( E3 , E2 , N2 , R );
 483     RotateAboutAxis( D3 , D2 , N2 , R );
 484
 485     // calculate angles from vectors
 486     Type  x0[3] = { 1.0 , 0.0 , 0.0 };   // == D0
 487     Type  y0[3] = { 0.0 , 1.0 , 0.0 };   // == E0
 488     Type  z0[3] = { 0.0 , 0.0 , 1.0 };   // == Z0
 489     Type  y2[3] ;
 490     Type  z2[3] ;
 491     Psi = static_cast<Type>(atan2(  Dot( N3 , y0 ) , Dot( N3 , x0 ) ));
 492     Theta = static_cast<Type>(atan2( -Dot( N3 , z0 ) , sqrt( pow(Dot( N3 , x0 ), 2) + pow(Dot( N3 , y0 ), 2) ) ));
 493     RotateAboutAxis( y2 , y0 , z0 , Psi );
 494     RotateAboutAxis( z2 , z0 , y2 , Theta );
 495     Phi = static_cast<Type>(atan2(  Dot( E3 , z2 ) , Dot( E3 , y2 ) ));
 496 };
 497
 498 //////////////////////////////////////////////////////////////////////////
 499
 500 //************************************
 501 // FullName:    KDIS::UTILS<Type>::EulerToHeadingPitchRoll
 502 // Description: Converts Euler to Heading, Pitch and Roll.
 503 // Parameter:   Type Lat - Geodetic Latitude in radians
 504 // Parameter:   Type Lon - Geodetic Longitude in radians
 505 // Parameter:   Type Psi - Euler angle
 506 // Parameter:   Type Theta - Euler angle
 507 // Parameter:   Type Phi - Euler angle
 508 // Parameter:   Type & H - Heading in radians out
 509 // Parameter:   Type & P - Pitch in radians out
 510 // Parameter:   Type & R - Roll in radians out
 511 //************************************
 512
 513 template<class Type>
 514 void EulerToHeadingPitchRoll( Type Lat, Type Lon, Type Psi, Type Theta, Type Phi, Type & H, Type & P, Type & R )
 515 {
 516     // local NED vectors in ECEF coordinate frame
 517     Type  N[3] ;
 518     Type  E[3] ;
 519     Type  D[3] ;
 520
 521     // Calculate NED from lat and lon
 522     // local NED
 523     //Type const  D0[3] = { 1. , 0. , 0. };
 524     Type const  E0[3] = { 0. , 1. , 0. };
 525     Type const  N0[3] = { 0. , 0. , 1. };
 526     Type        me[3] ;
 527     // 'E'
 528     RotateAboutAxis( E , E0 , N0 , Lon );
 529     me[0] = -E[0] ;
 530     me[1] = -E[1] ;
 531     me[2] = -E[2] ;
 532     // 'N'
 533     RotateAboutAxis( N , N0 , me , Lat );
 534     // 'D'
 535     Cross( D , N , E );
 536     /*
 537      *  Orientation:
 538      *    input : (x0,y0,z0)=(N,E,D) and (Psi,Theta,Phi Euler angles)
 539      *    output: (x3,y3,z3)=body vectors in local frame
 540      */
 541     // rotate about Z by Psi
 542     Type  X[]= {1.,0.,0.};
 543     Type  Y[]= {0.,1.,0.};
 544     Type  Z[]= {0.,0.,1.};
 545     Type  X1[3] , Y1[3] , Z1[3] ;
 546     RotateAboutAxis( X1 , X  , Z , Psi );
 547     RotateAboutAxis( Y1 , Y  , Z , Psi );
 548     memcpy(          Z1 , Z  , sizeof( Type[3] ) );
 549     // rotate about Y1 vector by Theta
 550     Type  X2[3] , Y2[3] , Z2[3] ;
 551     RotateAboutAxis( X2 , X1 , Y1 , Theta );
 552     memcpy(          Y2 , Y1 , sizeof( Type[3] ) );
 553     RotateAboutAxis( Z2 , Z1 , Y1 , Theta );
 554     // rotate about X2 by Phi
 555     Type  X3[3] , Y3[3] , Z3[3] ;
 556     memcpy(          X3 , X2 , sizeof( Type[3] ) );
 557     RotateAboutAxis( Y3 , Y2 , X2 , Phi );
 558     RotateAboutAxis( Z3 , Z2 , X2 , Phi );
 559     // calculate angles from vectors
 560     Type x0[3];
 561     memcpy( x0 , N , sizeof( Type[3] ) );
 562     Type y0[3];
 563     memcpy( y0 , E , sizeof( Type[3] ) );
 564     Type z0[3];
 565     memcpy( z0 , D , sizeof( Type[3] ) );
 566     Type  y2[3] ;
 567     Type  z2[3] ;
 568     H = static_cast<Type>(atan2(  Dot( X3 , y0 ) , Dot( X3 , x0 ) ));
 569     P = static_cast<Type>(atan2( -Dot( X3 , z0 ) , sqrt( pow(Dot( X3 , x0 ), 2) + pow(Dot( X3 , y0 ), 2) ) ));
 570     RotateAboutAxis( y2 , y0 , z0 , H );
 571     RotateAboutAxis( z2 , z0 , y2 , P );
 572     R = static_cast<Type>(atan2(  Dot( Y3 , z2 ) , Dot( Y3 , y2 ) ));
 573 }
 574
 575 } // END namespace UTILS
 576 } // END namespace KDIS
 577
 578 #undef KDIS_PI