Advertisement
mkiefer

Untitled

Apr 10th, 2020
621
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C++ 1.54 KB | None | 0 0
  1. struct affine3d {
  2.  
  3.    rotor3 rotor_;
  4.    vector3 trans_;
  5.  
  6.    //
  7.    // constructors
  8.    //
  9.  
  10.    affine3d() = default;
  11.  
  12.    affine3d( const rotor3& rotor, const vector3& trans )
  13.       : rotor_( rotor ), trans_( trans ) {
  14.    }
  15.  
  16.    affine3d( const rotor3& rotor )
  17.       : rotor_( rotor ), trans_() {
  18.    }
  19.  
  20.  
  21.    //
  22.    // TODO figure out how to not do this...
  23.    //
  24.    vector3 map1( const vector3& xx ) const {
  25.       vector3 translated = xx + trans_;
  26.       vector3 rotated = rotor_.rotate( translated );
  27.       return rotated;
  28.    }
  29.  
  30.    //
  31.    // apply the full affine transform to the given vector
  32.    //
  33.    vector3 map( const vector3& xx ) const {
  34.       vector3 rotated = rotor_.rotate( xx );
  35.       vector3 translated = rotated + trans_;
  36.       return translated;
  37.    }
  38.  
  39. };
  40.  
  41. //
  42. // create an affine transformation from ECR coordinates to ENU
  43. // coordinates where the origin of the ENU coordinate system is
  44. // at the given geodetic point
  45. //
  46. static inline affine3d ecr_to_enu( const geodetic_t& lla ) {
  47.  
  48.    const vector3 center = lla_to_ecr( lla );
  49.  
  50.    const double lon = M_PI / 2.0 + lla.lon * deg_to_rad;
  51.    const double lat = M_PI / 2.0 - lla.lat * deg_to_rad;
  52.  
  53.    // build a rotor for the z axis...
  54.  
  55.    vector3 zz( 0.0, 0.0, 1.0 );
  56.    rotor3 zrot( zz, lon );
  57.  
  58.    // build a rotor for the x axis...
  59.  
  60.    vector3 xx( 1.0, 0.0, 0.0 );
  61.    rotor3 xrot( xx, lat );
  62.  
  63.    // compose the rotors...
  64.  
  65.    rotor3 total_rot = xrot * zrot;
  66.    // create the transform...
  67.  
  68.    affine3d affine( total_rot, -center );
  69.  
  70.    return affine;
  71. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement