Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- struct affine3d {
- rotor3 rotor_;
- vector3 trans_;
- //
- // constructors
- //
- affine3d() = default;
- affine3d( const rotor3& rotor, const vector3& trans )
- : rotor_( rotor ), trans_( trans ) {
- }
- affine3d( const rotor3& rotor )
- : rotor_( rotor ), trans_() {
- }
- //
- // TODO figure out how to not do this...
- //
- vector3 map1( const vector3& xx ) const {
- vector3 translated = xx + trans_;
- vector3 rotated = rotor_.rotate( translated );
- return rotated;
- }
- //
- // apply the full affine transform to the given vector
- //
- vector3 map( const vector3& xx ) const {
- vector3 rotated = rotor_.rotate( xx );
- vector3 translated = rotated + trans_;
- return translated;
- }
- };
- //
- // create an affine transformation from ECR coordinates to ENU
- // coordinates where the origin of the ENU coordinate system is
- // at the given geodetic point
- //
- static inline affine3d ecr_to_enu( const geodetic_t& lla ) {
- const vector3 center = lla_to_ecr( lla );
- const double lon = M_PI / 2.0 + lla.lon * deg_to_rad;
- const double lat = M_PI / 2.0 - lla.lat * deg_to_rad;
- // build a rotor for the z axis...
- vector3 zz( 0.0, 0.0, 1.0 );
- rotor3 zrot( zz, lon );
- // build a rotor for the x axis...
- vector3 xx( 1.0, 0.0, 0.0 );
- rotor3 xrot( xx, lat );
- // compose the rotors...
- rotor3 total_rot = xrot * zrot;
- // create the transform...
- affine3d affine( total_rot, -center );
- return affine;
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement