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 - http://www.flightgear.org/~curt
   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., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, 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
  37 #include "location.hxx"
  38
  39
  40 /**
  41  * make model transformation Matrix - based on optimizations by NHV
  42  */
  43 static void MakeTRANS( sgMat4 dst, const double Theta,
  44                         const double Phi, const double Psi,
  45                         const sgMat4 UP)
  46 {
  47     SGfloat cosTheta = (SGfloat) cos(Theta);
  48     SGfloat sinTheta = (SGfloat) sin(Theta);
  49     SGfloat cosPhi   = (SGfloat) cos(Phi);
  50     SGfloat sinPhi   = (SGfloat) sin(Phi);
  51     SGfloat sinPsi   = (SGfloat) sin(Psi) ;
  52     SGfloat cosPsi   = (SGfloat) cos(Psi) ;
  53
  54     sgMat4 tmp;
  55
  56     tmp[0][0] = cosPhi * cosTheta;
  57     tmp[0][1] = sinPhi * cosPsi + cosPhi * -sinTheta * -sinPsi;
  58     tmp[0][2] = sinPhi * sinPsi + cosPhi * -sinTheta * cosPsi;
  59
  60     tmp[1][0] = -sinPhi * cosTheta;
  61     tmp[1][1] = cosPhi * cosPsi + -sinPhi * -sinTheta * -sinPsi;
  62     tmp[1][2] = cosPhi * sinPsi + -sinPhi * -sinTheta * cosPsi;
  63
  64     tmp[2][0] = sinTheta;
  65     tmp[2][1] = cosTheta * -sinPsi;
  66     tmp[2][2] = cosTheta * cosPsi;
  67
  68     float a = UP[0][0];
  69     float b = UP[1][0];
  70     float c = UP[2][0];
  71     dst[2][0] = a*tmp[0][0] + b*tmp[0][1] + c*tmp[0][2] ;
  72     dst[1][0] = a*tmp[1][0] + b*tmp[1][1] + c*tmp[1][2] ;
  73     dst[0][0] = -(a*tmp[2][0] + b*tmp[2][1] + c*tmp[2][2]) ;
  74     dst[3][0] = SG_ZERO ;
  75
  76     a = UP[0][1];
  77     b = UP[1][1];
  78     c = UP[2][1];
  79     dst[2][1] = a*tmp[0][0] + b*tmp[0][1] + c*tmp[0][2] ;
  80     dst[1][1] = a*tmp[1][0] + b*tmp[1][1] + c*tmp[1][2] ;
  81     dst[0][1] = -(a*tmp[2][0] + b*tmp[2][1] + c*tmp[2][2]) ;
  82     dst[3][1] = SG_ZERO ;
  83
  84     a = UP[0][2];
  85     c = UP[2][2];
  86     dst[2][2] = a*tmp[0][0] + c*tmp[0][2] ;
  87     dst[1][2] = a*tmp[1][0] + c*tmp[1][2] ;
  88     dst[0][2] = -(a*tmp[2][0] + c*tmp[2][2]) ;
  89     dst[3][2] = SG_ZERO ;
  90
  91     dst[2][3] = SG_ZERO ;
  92     dst[1][3] = SG_ZERO ;
  93     dst[0][3] = SG_ZERO ;
  94     dst[3][3] = SG_ONE ;
  95 }
  96
  97
  98 ////////////////////////////////////////////////////////////////////////
  99 // Implementation of SGLocation.
 100 ////////////////////////////////////////////////////////////////////////
 101
 102 // Constructor
 103 SGLocation::SGLocation( void ):
 104     _orientation_dirty(true),
 105     _position_dirty(true),
 106     _lon_deg(-1000),
 107     _lat_deg(0),
 108     _alt_ft(0),
 109     _roll_deg(0),
 110     _pitch_deg(0),
 111     _heading_deg(0),
 112     _cur_elev_m(0)
 113 {
 114     sgdZeroVec3(_absolute_view_pos);
 115     sgMakeRotMat4( UP, 0.0, 0.0, 0.0 );
 116     sgMakeRotMat4( TRANS, 0.0, 0.0, 0.0 );
 117 }
 118
 119
 120 // Destructor
 121 SGLocation::~SGLocation( void ) {
 122 }
 123
 124 void
 125 SGLocation::setPosition (double lon_deg, double lat_deg, double alt_ft)
 126 {
 127   _position_dirty = true;
 128   _lon_deg = lon_deg;
 129   _lat_deg = lat_deg;
 130   _alt_ft = alt_ft;
 131 }
 132
 133 void
 134 SGLocation::setOrientation (double roll_deg, double pitch_deg, double heading_deg)
 135 {
 136   _orientation_dirty = true;
 137   _roll_deg = roll_deg;
 138   _pitch_deg = pitch_deg;
 139   _heading_deg = heading_deg;
 140 }
 141
 142 double *
 143 SGLocation::get_absolute_view_pos()
 144 {
 145     recalcAbsolutePosition();
 146     return _absolute_view_pos;
 147 }
 148
 149 float *
 150 SGLocation::get_view_pos( const Point3D& scenery_center )
 151 {
 152     recalcAbsolutePosition();
 153     for (int i = 0; i < 3; i++)
 154         _relative_view_pos[i] = _absolute_view_pos[i] - scenery_center[i];
 155     return _relative_view_pos;
 156 }
 157
 158 void
 159 SGLocation::recalcOrientation() const
 160 {
 161   if (_orientation_dirty) {
 162     // Make sure UP matrix is up-to-date.
 163     recalcAbsolutePosition();
 164
 165     // Create local matrix with current geodetic position.  Converting
 166     // the orientation (pitch/roll/heading) to vectors.
 167     MakeTRANS( TRANS, _pitch_deg * SG_DEGREES_TO_RADIANS,
 168                       _roll_deg * SG_DEGREES_TO_RADIANS,
 169                      -_heading_deg * SG_DEGREES_TO_RADIANS,
 170                       UP );
 171     _orientation_dirty = false;
 172   }
 173 }
 174
 175 /*
 176  * Update values derived from the longitude, latitude and altitude parameters
 177  * of this instance. This encompasses absolute position in cartesian
 178  * coordinates, the local up, east and south vectors and the UP Matrix.
 179  */
 180 void
 181 SGLocation::recalcAbsolutePosition() const
 182 {
 183   if (_position_dirty) {
 184     double lat = _lat_deg * SGD_DEGREES_TO_RADIANS;
 185     double lon = _lon_deg * SGD_DEGREES_TO_RADIANS;
 186     double alt = _alt_ft * SG_FEET_TO_METER;
 187
 188     sgGeodToCart(lat, lon, alt, _absolute_view_pos);
 189
 190      // Make the world up rotation matrix for eye positioin...
 191     sgMakeRotMat4( UP, _lon_deg, 0.0, -_lat_deg );
 192
 193     // get the world up radial vector from planet center for output
 194     sgSetVec3( _world_up, UP[0][0], UP[0][1], UP[0][2] );
 195
 196     // Calculate the surface east and south vectors using the (normalized)
 197     // partial derivatives of the up vector Could also be fetched and
 198     // normalized from the UP rotation matrix, but I doubt this would
 199     // be more efficient.
 200     float sin_lon = sin(_lon_deg * SGD_DEGREES_TO_RADIANS);
 201     float sin_lat = sin(_lat_deg * SGD_DEGREES_TO_RADIANS);
 202     float cos_lon = cos(_lon_deg * SGD_DEGREES_TO_RADIANS);
 203     float cos_lat = cos(_lat_deg * SGD_DEGREES_TO_RADIANS);
 204
 205     _surface_south[0] = (sin_lat*cos_lon);
 206     _surface_south[1] = (sin_lat*sin_lon);
 207     _surface_south[2] = - cos_lat;
 208
 209     _surface_east[0] = -sin_lon;
 210     _surface_east[1] = cos_lon;
 211     _surface_east[2] = 0.f;
 212
 213     _position_dirty = false;
 214   }
 215 }