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 - http://www.flightgear.org/~curt
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., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, 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]) ;
99 ////////////////////////////////////////////////////////////////////////
100 // Implementation of SGLocation.
101 ////////////////////////////////////////////////////////////////////////
104 SGLocation::SGLocation( void ):
105 _orientation_dirty(true),
106 _position_dirty(true),
115 sgdZeroVec3(_absolute_view_pos);
116 sgMakeRotMat4( UP, 0.0, 0.0, 0.0 );
117 sgMakeRotMat4( TRANS, 0.0, 0.0, 0.0 );
122 SGLocation::~SGLocation( void ) {
126 SGLocation::setPosition (double lon_deg, double lat_deg, double alt_ft)
128 _position_dirty = true;
135 SGLocation::setOrientation (double roll_deg, double pitch_deg, double heading_deg)
137 _orientation_dirty = true;
138 _roll_deg = roll_deg;
139 _pitch_deg = pitch_deg;
140 _heading_deg = heading_deg;
144 SGLocation::get_absolute_view_pos()
146 recalcAbsolutePosition();
147 return _absolute_view_pos;
151 SGLocation::get_view_pos( const Point3D& scenery_center )
153 recalcAbsolutePosition();
154 for (int i = 0; i < 3; i++)
155 _relative_view_pos[i] = _absolute_view_pos[i] - scenery_center[i];
156 return _relative_view_pos;
160 SGLocation::recalcOrientation() const
162 if (_orientation_dirty) {
163 // Make sure UP matrix is up-to-date.
164 recalcAbsolutePosition();
166 // Create local matrix with current geodetic position. Converting
167 // the orientation (pitch/roll/heading) to vectors.
168 MakeTRANS( TRANS, _pitch_deg * SG_DEGREES_TO_RADIANS,
169 _roll_deg * SG_DEGREES_TO_RADIANS,
170 -_heading_deg * SG_DEGREES_TO_RADIANS,
172 _orientation_dirty = false;
177 * Update values derived from the longitude, latitude and altitude parameters
178 * of this instance. This encompasses absolute position in cartesian
179 * coordinates, the local up, east and south vectors and the UP Matrix.
182 SGLocation::recalcAbsolutePosition() const
184 if (_position_dirty) {
185 double lat = _lat_deg * SGD_DEGREES_TO_RADIANS;
186 double lon = _lon_deg * SGD_DEGREES_TO_RADIANS;
187 double alt = _alt_ft * SG_FEET_TO_METER;
189 sgGeodToCart(lat, lon, alt, _absolute_view_pos);
191 // Make the world up rotation matrix for eye positioin...
192 sgMakeRotMat4( UP, _lon_deg, 0.0, -_lat_deg );
194 // get the world up radial vector from planet center for output
195 sgSetVec3( _world_up, UP[0][0], UP[0][1], UP[0][2] );
197 // Calculate the surface east and south vectors using the (normalized)
198 // partial derivatives of the up vector Could also be fetched and
199 // normalized from the UP rotation matrix, but I doubt this would
200 // be more efficient.
201 float sin_lon = sin(_lon_deg * SGD_DEGREES_TO_RADIANS);
202 float sin_lat = sin(_lat_deg * SGD_DEGREES_TO_RADIANS);
203 float cos_lon = cos(_lon_deg * SGD_DEGREES_TO_RADIANS);
204 float cos_lat = cos(_lat_deg * SGD_DEGREES_TO_RADIANS);
206 _surface_south[0] = (sin_lat*cos_lon);
207 _surface_south[1] = (sin_lat*sin_lon);
208 _surface_south[2] = - cos_lat;
210 _surface_east[0] = -sin_lon;
211 _surface_east[1] = cos_lon;
212 _surface_east[2] = 0.f;
214 _position_dirty = false;