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 "location.hxx"
  39
  40
  41 /**
  42  * make model transformation Matrix - based on optimizations by NHV
  43  */
  44 static void MakeTRANS( sgMat4 dst, const double Theta,
  45                         const double Phi, const double Psi,
  46                         const sgMat4 UP)
  47 {
  48     SGfloat cosTheta = (SGfloat) cos(Theta);
  49     SGfloat sinTheta = (SGfloat) sin(Theta);
  50     SGfloat cosPhi   = (SGfloat) cos(Phi);
  51     SGfloat sinPhi   = (SGfloat) sin(Phi);
  52     SGfloat sinPsi   = (SGfloat) sin(Psi) ;
  53     SGfloat cosPsi   = (SGfloat) cos(Psi) ;
  54
  55     sgMat4 tmp;
  56
  57     tmp[0][0] = cosPhi * cosTheta;
  58     tmp[0][1] = sinPhi * cosPsi + cosPhi * -sinTheta * -sinPsi;
  59     tmp[0][2] = sinPhi * sinPsi + cosPhi * -sinTheta * cosPsi;
  60
  61     tmp[1][0] = -sinPhi * cosTheta;
  62     tmp[1][1] = cosPhi * cosPsi + -sinPhi * -sinTheta * -sinPsi;
  63     tmp[1][2] = cosPhi * sinPsi + -sinPhi * -sinTheta * cosPsi;
  64
  65     tmp[2][0] = sinTheta;
  66     tmp[2][1] = cosTheta * -sinPsi;
  67     tmp[2][2] = cosTheta * cosPsi;
  68
  69     float a = UP[0][0];
  70     float b = UP[1][0];
  71     float c = UP[2][0];
  72     dst[2][0] = a*tmp[0][0] + b*tmp[0][1] + c*tmp[0][2] ;
  73     dst[1][0] = a*tmp[1][0] + b*tmp[1][1] + c*tmp[1][2] ;
  74     dst[0][0] = -(a*tmp[2][0] + b*tmp[2][1] + c*tmp[2][2]) ;
  75     dst[3][0] = SG_ZERO ;
  76
  77     a = UP[0][1];
  78     b = UP[1][1];
  79     c = UP[2][1];
  80     dst[2][1] = a*tmp[0][0] + b*tmp[0][1] + c*tmp[0][2] ;
  81     dst[1][1] = a*tmp[1][0] + b*tmp[1][1] + c*tmp[1][2] ;
  82     dst[0][1] = -(a*tmp[2][0] + b*tmp[2][1] + c*tmp[2][2]) ;
  83     dst[3][1] = SG_ZERO ;
  84
  85     a = UP[0][2];
  86     c = UP[2][2];
  87     dst[2][2] = a*tmp[0][0] + c*tmp[0][2] ;
  88     dst[1][2] = a*tmp[1][0] + c*tmp[1][2] ;
  89     dst[0][2] = -(a*tmp[2][0] + c*tmp[2][2]) ;
  90     dst[3][2] = SG_ZERO ;
  91
  92     dst[2][3] = SG_ZERO ;
  93     dst[1][3] = SG_ZERO ;
  94     dst[0][3] = SG_ZERO ;
  95     dst[3][3] = SG_ONE ;
  96
  97 }
  98
  99
 100 ////////////////////////////////////////////////////////////////////////
 101 // Implementation of SGLocation.
 102 ////////////////////////////////////////////////////////////////////////
 103
 104 // Constructor
 105 SGLocation::SGLocation( void ):
 106     _dirty(true),
 107     _lon_deg(-1000),
 108     _lat_deg(0),
 109     _alt_ft(0),
 110     _roll_deg(0),
 111     _pitch_deg(0),
 112     _heading_deg(0),
 113     _cur_elev_m(0),
 114     _tile_center(0)
 115 {
 116     sgdZeroVec3(_absolute_view_pos);
 117     sgZeroVec3(_relative_view_pos);
 118     sgZeroVec3(_zero_elev_view_pos);
 119     sgMakeRotMat4( UP, 0.0, 0.0, 0.0 );
 120     sgMakeRotMat4( TRANS, 0.0, 0.0, 0.0 );
 121 }
 122
 123
 124 // Destructor
 125 SGLocation::~SGLocation( void ) {
 126 }
 127
 128 void
 129 SGLocation::init ()
 130 {
 131 }
 132
 133 void
 134 SGLocation::bind ()
 135 {
 136 }
 137
 138 void
 139 SGLocation::unbind ()
 140 {
 141 }
 142
 143 void
 144 SGLocation::setPosition (double lon_deg, double lat_deg, double alt_ft)
 145 {
 146   _dirty = true;
 147   _lon_deg = lon_deg;
 148   _lat_deg = lat_deg;
 149   _alt_ft = alt_ft;
 150 }
 151
 152 void
 153 SGLocation::setOrientation (double roll_deg, double pitch_deg, double heading_deg)
 154 {
 155   _dirty = true;
 156   _roll_deg = roll_deg;
 157   _pitch_deg = pitch_deg;
 158   _heading_deg = heading_deg;
 159 }
 160
 161 double *
 162 SGLocation::get_absolute_view_pos( const Point3D scenery_center )
 163 {
 164     if ( _dirty ) {
 165         recalc( scenery_center );
 166     }
 167     return _absolute_view_pos;
 168 }
 169
 170 float *
 171 SGLocation::getRelativeViewPos( const Point3D scenery_center )
 172 {
 173     if ( _dirty ) {
 174         recalc( scenery_center );
 175     }
 176     return _relative_view_pos;
 177 }
 178
 179 float *
 180 SGLocation::getZeroElevViewPos( const Point3D scenery_center )
 181 {
 182     if ( _dirty ) {
 183         recalc( scenery_center );
 184     }
 185     return _zero_elev_view_pos;
 186 }
 187
 188
 189 // recalc() is done every time one of the setters is called (making the
 190 // cached data "dirty") on the next "get".  It calculates all the outputs
 191 // for viewer.
 192 void
 193 SGLocation::recalc( const Point3D scenery_center )
 194 {
 195
 196   recalcPosition( _lon_deg, _lat_deg, _alt_ft, scenery_center );
 197
 198   // Make the world up rotation matrix for eye positioin...
 199   sgMakeRotMat4( UP, _lon_deg, 0.0, -_lat_deg );
 200
 201
 202   // get the world up radial vector from planet center for output
 203   sgSetVec3( _world_up, UP[0][0], UP[0][1], UP[0][2] );
 204
 205   // Creat local matrix with current geodetic position.  Converting
 206   // the orientation (pitch/roll/heading) to vectors.
 207   MakeTRANS( TRANS, _pitch_deg * SG_DEGREES_TO_RADIANS,
 208                       _roll_deg * SG_DEGREES_TO_RADIANS,
 209                       -_heading_deg * SG_DEGREES_TO_RADIANS,
 210                       UP);
 211
 212   // Given a vector pointing straight down (-Z), map into onto the
 213   // local plane representing "horizontal".  This should give us the
 214   // local direction for moving "south".
 215   sgVec3 minus_z;
 216   sgSetVec3( minus_z, 0.0, 0.0, -1.0 );
 217
 218   sgmap_vec_onto_cur_surface_plane(_world_up, _relative_view_pos, minus_z,
 219                                      _surface_south);
 220   sgNormalizeVec3(_surface_south);
 221
 222   // now calculate the surface east vector
 223   sgVec3 world_down;
 224   sgNegateVec3(world_down, _world_up);
 225   sgVectorProductVec3(_surface_east, _surface_south, world_down);
 226
 227   set_clean();
 228 }
 229
 230 void
 231 SGLocation::recalcPosition( double lon_deg, double lat_deg, double alt_ft,
 232                             const Point3D scenery_center ) const
 233 {
 234   double lat = lat_deg * SGD_DEGREES_TO_RADIANS;
 235   double lon = lon_deg * SGD_DEGREES_TO_RADIANS;
 236   double alt = alt_ft * SG_FEET_TO_METER;
 237
 238   sgGeodToCart(lat, lon, alt, _absolute_view_pos);
 239
 240   int i;
 241   double ground[3];
 242   sgGeodToCart(lat, lon, 0, ground);
 243   for(i=0; i<3; i++)
 244       _zero_elev_view_pos[i] = ground[i] - _tile_center[i];
 245
 246   // FIXME: view position should ONLY be calculated in the viewer...
 247   // Anything else should calculate their own positions relative to the
 248   // viewer's tile_center.
 249   for(i=0; i<3; i++)
 250     _relative_view_pos[i] = _absolute_view_pos[i] - scenery_center[i];
 251 }
 252
 253 void
 254 SGLocation::update (int dt)
 255 {
 256 }