src/Main/location.cxx

   1 // location.hxx -- class for determining model location in the flightgear world.
   2 //
   3 // Written by Jim Wilson,  David Megginson, started April 2002.
   4 //                          overhaul started October 2000.
   5 //
   6 // This file is in the Public Domain, and comes with no warranty.
   7
   8
   9
  10 #include <simgear/compiler.h>
  11
  12 #ifdef HAVE_CONFIG_H
  13 #  include <config.h>
  14 #endif
  15
  16 #include <simgear/debug/logstream.hxx>
  17 #include <simgear/constants.h>
  18 #include <simgear/math/point3d.hxx>
  19 #include <simgear/math/polar3d.hxx>
  20 #include <simgear/math/sg_geodesy.hxx>
  21
  22 #include <Scenery/scenery.hxx>
  23
  24 #include <simgear/math/vector.hxx>
  25 /*
  26 #include "globals.hxx"
  27 */
  28
  29 #include "location.hxx"
  30
  31
  32 /**
  33  * make model transformation Matrix - based on optimizations by NHV
  34  */
  35 static void MakeTRANS( sgMat4 dst, const double Theta,
  36                         const double Phi, const double Psi,
  37                         const sgMat4 UP)
  38 {
  39     SGfloat cosTheta = (SGfloat) cos(Theta);
  40     SGfloat sinTheta = (SGfloat) sin(Theta);
  41     SGfloat cosPhi   = (SGfloat) cos(Phi);
  42     SGfloat sinPhi   = (SGfloat) sin(Phi);
  43     SGfloat sinPsi   = (SGfloat) sin(Psi) ;
  44     SGfloat cosPsi   = (SGfloat) cos(Psi) ;
  45
  46     sgMat4 tmp;
  47
  48     tmp[0][0] = cosPhi * cosTheta;
  49     tmp[0][1] = sinPhi * cosPsi + cosPhi * -sinTheta * -sinPsi;
  50     tmp[0][2] = sinPhi * sinPsi + cosPhi * -sinTheta * cosPsi;
  51
  52     tmp[1][0] = -sinPhi * cosTheta;
  53     tmp[1][1] = cosPhi * cosPsi + -sinPhi * -sinTheta * -sinPsi;
  54     tmp[1][2] = cosPhi * sinPsi + -sinPhi * -sinTheta * cosPsi;
  55
  56     tmp[2][0] = sinTheta;
  57     tmp[2][1] = cosTheta * -sinPsi;
  58     tmp[2][2] = cosTheta * cosPsi;
  59
  60     float a = UP[0][0];
  61     float b = UP[1][0];
  62     float c = UP[2][0];
  63     dst[2][0] = a*tmp[0][0] + b*tmp[0][1] + c*tmp[0][2] ;
  64     dst[1][0] = a*tmp[1][0] + b*tmp[1][1] + c*tmp[1][2] ;
  65     dst[0][0] = -(a*tmp[2][0] + b*tmp[2][1] + c*tmp[2][2]) ;
  66     dst[3][0] = SG_ZERO ;
  67
  68     a = UP[0][1];
  69     b = UP[1][1];
  70     c = UP[2][1];
  71     dst[2][1] = a*tmp[0][0] + b*tmp[0][1] + c*tmp[0][2] ;
  72     dst[1][1] = a*tmp[1][0] + b*tmp[1][1] + c*tmp[1][2] ;
  73     dst[0][1] = -(a*tmp[2][0] + b*tmp[2][1] + c*tmp[2][2]) ;
  74     dst[3][1] = SG_ZERO ;
  75
  76     a = UP[0][2];
  77     c = UP[2][2];
  78     dst[2][2] = a*tmp[0][0] + c*tmp[0][2] ;
  79     dst[1][2] = a*tmp[1][0] + c*tmp[1][2] ;
  80     dst[0][2] = -(a*tmp[2][0] + c*tmp[2][2]) ;
  81     dst[3][2] = SG_ZERO ;
  82
  83     dst[2][3] = SG_ZERO ;
  84     dst[1][3] = SG_ZERO ;
  85     dst[0][3] = SG_ZERO ;
  86     dst[3][3] = SG_ONE ;
  87
  88 }
  89
  90
  91 ////////////////////////////////////////////////////////////////////////
  92 // Implementation of FGLocation.
  93 ////////////////////////////////////////////////////////////////////////
  94
  95 // Constructor
  96 FGLocation::FGLocation( void ):
  97     _dirty(true),
  98     _lon_deg(0),
  99     _lat_deg(0),
 100     _alt_ft(0),
 101     _roll_deg(0),
 102     _pitch_deg(0),
 103     _heading_deg(0)
 104 {
 105     sgdZeroVec3(_absolute_view_pos);
 106 }
 107
 108
 109 // Destructor
 110 FGLocation::~FGLocation( void ) {
 111 }
 112
 113 void
 114 FGLocation::init ()
 115 {
 116 }
 117
 118 void
 119 FGLocation::bind ()
 120 {
 121 }
 122
 123 void
 124 FGLocation::unbind ()
 125 {
 126 }
 127
 128 void
 129 FGLocation::setPosition (double lon_deg, double lat_deg, double alt_ft)
 130 {
 131   _dirty = true;
 132   _lon_deg = lon_deg;
 133   _lat_deg = lat_deg;
 134   _alt_ft = alt_ft;
 135 }
 136
 137 void
 138 FGLocation::setOrientation (double roll_deg, double pitch_deg, double heading_deg)
 139 {
 140   _dirty = true;
 141   _roll_deg = roll_deg;
 142   _pitch_deg = pitch_deg;
 143   _heading_deg = heading_deg;
 144 }
 145
 146 double *
 147 FGLocation::get_absolute_view_pos ()
 148 {
 149   if (_dirty)
 150     recalc();
 151   return _absolute_view_pos;
 152 }
 153
 154 float *
 155 FGLocation::getRelativeViewPos ()
 156 {
 157   if (_dirty)
 158     recalc();
 159   return _relative_view_pos;
 160 }
 161
 162 float *
 163 FGLocation::getZeroElevViewPos ()
 164 {
 165   if (_dirty)
 166     recalc();
 167   return _zero_elev_view_pos;
 168 }
 169
 170
 171 // recalc() is done every time one of the setters is called (making the
 172 // cached data "dirty") on the next "get".  It calculates all the outputs
 173 // for viewer.
 174 void
 175 FGLocation::recalc ()
 176 {
 177
 178   recalcPosition( _lon_deg, _lat_deg, _alt_ft );
 179
 180   // Make the world up rotation matrix for eye positioin...
 181   sgMakeRotMat4( UP, _lon_deg, 0.0, -_lat_deg );
 182
 183
 184   // get the world up radial vector from planet center for output
 185   sgSetVec3( _world_up, UP[0][0], UP[0][1], UP[0][2] );
 186
 187   // Creat local matrix with current geodetic position.  Converting
 188   // the orientation (pitch/roll/heading) to vectors.
 189   MakeTRANS( TRANS, _pitch_deg * SG_DEGREES_TO_RADIANS,
 190                       _roll_deg * SG_DEGREES_TO_RADIANS,
 191                       -_heading_deg * SG_DEGREES_TO_RADIANS,
 192                       UP);
 193
 194   // Given a vector pointing straight down (-Z), map into onto the
 195   // local plane representing "horizontal".  This should give us the
 196   // local direction for moving "south".
 197   sgVec3 minus_z;
 198   sgSetVec3( minus_z, 0.0, 0.0, -1.0 );
 199
 200   sgmap_vec_onto_cur_surface_plane(_world_up, _relative_view_pos, minus_z,
 201                                      _surface_south);
 202   sgNormalizeVec3(_surface_south);
 203
 204   // now calculate the surface east vector
 205   sgVec3 world_down;
 206   sgNegateVec3(world_down, _world_up);
 207   sgVectorProductVec3(_surface_east, _surface_south, world_down);
 208
 209   set_clean();
 210 }
 211
 212 void
 213 FGLocation::recalcPosition (double lon_deg, double lat_deg, double alt_ft) const
 214 {
 215   double sea_level_radius_m;
 216   double lat_geoc_rad;
 217
 218
 219                                 // Convert from geodetic to geocentric
 220                                 // coordinates.
 221   sgGeodToGeoc(lat_deg * SGD_DEGREES_TO_RADIANS,
 222                alt_ft * SG_FEET_TO_METER,
 223                &sea_level_radius_m,
 224                &lat_geoc_rad);
 225
 226                                 // Calculate the cartesian coordinates
 227                                 // of point directly below at sea level.
 228                                 // aka Zero Elevation Position
 229   Point3D p = Point3D(lon_deg * SG_DEGREES_TO_RADIANS,
 230                       lat_geoc_rad,
 231                       sea_level_radius_m);
 232   Point3D tmp = sgPolarToCart3d(p) - scenery.get_next_center();
 233   sgSetVec3(_zero_elev_view_pos, tmp[0], tmp[1], tmp[2]);
 234
 235                                 // Calculate the absolute view position
 236                                 // in fgfs coordinates.
 237                                 // aka Absolute View Position
 238   p.setz(p.radius() + alt_ft * SG_FEET_TO_METER);
 239   tmp = sgPolarToCart3d(p);
 240   sgdSetVec3(_absolute_view_pos, tmp[0], tmp[1], tmp[2]);
 241
 242                                 // Calculate the relative view position
 243                                 // from the scenery center.
 244                                 // aka Relative View Position
 245   sgdVec3 scenery_center;
 246   sgdSetVec3(scenery_center,
 247              scenery.get_next_center().x(),
 248              scenery.get_next_center().y(),
 249              scenery.get_next_center().z());
 250   sgdVec3 view_pos;
 251   sgdSubVec3(view_pos, _absolute_view_pos, scenery_center);
 252   sgSetVec3(_relative_view_pos, view_pos);
 253
 254 }
 255
 256 void
 257 FGLocation::update (int dt)
 258 {
 259 }
 260
 261
 262