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