simgear/scene/model/location.cxx

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