1 // location.cxx -- class for determining model location in the flightgear world.
3 // Written by Jim Wilson, David Megginson, started April 2002.
4 // Based largely on code by Curtis Olson and Norman Vine.
6 // Copyright (C) 2002 Curtis L. Olson - curt@flightgear.org
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.
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.
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.
25 #include <simgear/compiler.h>
28 # include <simgear_config.h>
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>
38 #include "location.hxx"
42 * make model transformation Matrix - based on optimizations by NHV
44 static void MakeTRANS( sgMat4 dst, const double Theta,
45 const double Phi, const double Psi,
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) ;
57 tmp[0][0] = cosPhi * cosTheta;
58 tmp[0][1] = sinPhi * cosPsi + cosPhi * -sinTheta * -sinPsi;
59 tmp[0][2] = sinPhi * sinPsi + cosPhi * -sinTheta * cosPsi;
61 tmp[1][0] = -sinPhi * cosTheta;
62 tmp[1][1] = cosPhi * cosPsi + -sinPhi * -sinTheta * -sinPsi;
63 tmp[1][2] = cosPhi * sinPsi + -sinPhi * -sinTheta * cosPsi;
66 tmp[2][1] = cosTheta * -sinPsi;
67 tmp[2][2] = cosTheta * cosPsi;
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]) ;
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]) ;
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]) ;
100 ////////////////////////////////////////////////////////////////////////
101 // Implementation of SGLocation.
102 ////////////////////////////////////////////////////////////////////////
105 SGLocation::SGLocation( void ):
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 );
125 SGLocation::~SGLocation( void ) {
139 SGLocation::unbind ()
144 SGLocation::setPosition (double lon_deg, double lat_deg, double alt_ft)
153 SGLocation::setOrientation (double roll_deg, double pitch_deg, double heading_deg)
156 _roll_deg = roll_deg;
157 _pitch_deg = pitch_deg;
158 _heading_deg = heading_deg;
162 SGLocation::get_absolute_view_pos( const Point3D scenery_center )
165 recalc( scenery_center );
167 return _absolute_view_pos;
171 SGLocation::getRelativeViewPos( const Point3D scenery_center )
174 recalc( scenery_center );
176 return _relative_view_pos;
180 SGLocation::getZeroElevViewPos( const Point3D scenery_center )
183 recalc( scenery_center );
185 return _zero_elev_view_pos;
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
193 SGLocation::recalc( const Point3D scenery_center )
196 recalcPosition( _lon_deg, _lat_deg, _alt_ft, scenery_center );
198 // Make the world up rotation matrix for eye positioin...
199 sgMakeRotMat4( UP, _lon_deg, 0.0, -_lat_deg );
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] );
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,
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".
216 sgSetVec3( minus_z, 0.0, 0.0, -1.0 );
218 sgmap_vec_onto_cur_surface_plane(_world_up, _relative_view_pos, minus_z,
220 sgNormalizeVec3(_surface_south);
222 // now calculate the surface east vector
224 sgNegateVec3(world_down, _world_up);
225 sgVectorProductVec3(_surface_east, _surface_south, world_down);
231 SGLocation::recalcPosition( double lon_deg, double lat_deg, double alt_ft,
232 const Point3D scenery_center ) const
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;
238 sgGeodToCart(lat, lon, alt, _absolute_view_pos);
242 sgGeodToCart(lat, lon, 0, ground);
244 _zero_elev_view_pos[i] = ground[i] - _tile_center[i];
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.
250 _relative_view_pos[i] = _absolute_view_pos[i] - scenery_center[i];
254 SGLocation::update (int dt)