1 // viewer_rph.cxx -- class for managing a Roll/Pitch/Heading viewer in
2 // the flightgear world.
4 // Written by Curtis Olson, started August 1997.
5 // overhaul started October 2000.
7 // Copyright (C) 1997 - 2000 Curtis L. Olson - curt@flightgear.org
9 // This program is free software; you can redistribute it and/or
10 // modify it under the terms of the GNU General Public License as
11 // published by the Free Software Foundation; either version 2 of the
12 // License, or (at your option) any later version.
14 // This program is distributed in the hope that it will be useful, but
15 // WITHOUT ANY WARRANTY; without even the implied warranty of
16 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
17 // General Public License for more details.
19 // You should have received a copy of the GNU General Public License
20 // along with this program; if not, write to the Free Software
21 // Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
26 #include <simgear/compiler.h>
32 #include <plib/ssg.h> // plib include
34 #include <simgear/constants.h>
35 #include <simgear/debug/logstream.hxx>
36 #include <simgear/math/point3d.hxx>
37 #include <simgear/math/polar3d.hxx>
38 #include <simgear/math/sg_geodesy.hxx>
39 #include <simgear/math/vector.hxx>
41 #include <Scenery/scenery.hxx>
43 #include "globals.hxx"
44 #include "viewer_rph.hxx"
48 FGViewerRPH::FGViewerRPH( void )
50 set_reverse_view_offset(false);
54 // VIEW_ROT = LARC_TO_SSG * ( VIEWo * VIEW_OFFSET )
55 // This takes advantage of the fact that VIEWo and VIEW_OFFSET
56 // only have entries in the upper 3x3 block
57 // and that LARC_TO_SSG is just a shift of rows NHV
58 inline static void fgMakeViewRot( sgMat4 dst, const sgMat4 m1, const sgMat4 m2 )
60 for ( int j = 0 ; j < 3 ; j++ ) {
61 dst[2][j] = m2[0][0] * m1[0][j] +
65 dst[0][j] = m2[1][0] * m1[0][j] +
69 dst[1][j] = m2[2][0] * m1[0][j] +
83 inline static void fgMakeLOCAL( sgMat4 dst, const double Theta,
84 const double Phi, const double Psi)
86 SGfloat cosTheta = (SGfloat) cos(Theta);
87 SGfloat sinTheta = (SGfloat) sin(Theta);
88 SGfloat cosPhi = (SGfloat) cos(Phi);
89 SGfloat sinPhi = (SGfloat) sin(Phi);
90 SGfloat sinPsi = (SGfloat) sin(Psi) ;
91 SGfloat cosPsi = (SGfloat) cos(Psi) ;
93 dst[0][0] = cosPhi * cosTheta;
94 dst[0][1] = sinPhi * cosPsi + cosPhi * -sinTheta * -sinPsi;
95 dst[0][2] = sinPhi * sinPsi + cosPhi * -sinTheta * cosPsi;
98 dst[1][0] = -sinPhi * cosTheta;
99 dst[1][1] = cosPhi * cosPsi + -sinPhi * -sinTheta * -sinPsi;
100 dst[1][2] = cosPhi * sinPsi + -sinPhi * -sinTheta * cosPsi;
101 dst[1][3] = SG_ZERO ;
103 dst[2][0] = sinTheta;
104 dst[2][1] = cosTheta * -sinPsi;
105 dst[2][2] = cosTheta * cosPsi;
115 // Update the view parameters
116 void FGViewerRPH::update() {
117 sgVec3 minus_z, right, forward, tilt;
120 view_point.setPosition(geod_view_pos[0] * SGD_RADIANS_TO_DEGREES,
121 geod_view_pos[1] * SGD_RADIANS_TO_DEGREES,
122 geod_view_pos[2] * SG_METER_TO_FEET);
123 sgCopyVec3(zero_elev, view_point.getZeroElevViewPos());
124 sgdCopyVec3(abs_view_pos, view_point.getAbsoluteViewPos());
125 sgCopyVec3(view_pos, view_point.getRelativeViewPos());
127 // code to calculate LOCAL matrix calculated from Phi, Theta, and
128 // Psi (roll, pitch, yaw) in case we aren't running LaRCsim as our
131 fgMakeLOCAL( LOCAL, rph[1], rph[0], -rph[2] );
134 geod_view_pos[0] * SGD_RADIANS_TO_DEGREES,
136 -geod_view_pos[1] * SGD_RADIANS_TO_DEGREES );
138 sgSetVec3( world_up, UP[0][0], UP[0][1], UP[0][2] );
139 sgCopyMat4( VIEWo, LOCAL );
140 sgPostMultMat4( VIEWo, UP );
142 // generate the sg view up and forward vectors
143 sgSetVec3( view_up, VIEWo[0][0], VIEWo[0][1], VIEWo[0][2] );
144 sgSetVec3( right, VIEWo[1][0], VIEWo[1][1], VIEWo[1][2] );
145 sgSetVec3( forward, VIEWo[2][0], VIEWo[2][1], VIEWo[2][2] );
147 // generate the pilot offset vector in world coordinates
148 sgVec3 pilot_offset_world;
149 sgSetVec3( pilot_offset_world,
150 pilot_offset[2], pilot_offset[1], -pilot_offset[0] );
151 sgXformVec3( pilot_offset_world, pilot_offset_world, VIEWo );
153 // generate the view offset matrix
154 sgMakeRotMat4( VIEW_OFFSET, view_offset * SGD_RADIANS_TO_DEGREES, view_up );
157 sgMakeRotMat4( VIEW_TILT, view_tilt * SGD_RADIANS_TO_DEGREES, right );
158 sgPreMultMat4(VIEW_OFFSET, VIEW_TILT);
159 // cout << "VIEW_OFFSET matrix" << endl;
160 // print_sgMat4( VIEW_OFFSET );
161 sgXformVec3( view_forward, forward, VIEW_OFFSET );
162 SG_LOG( SG_VIEW, SG_DEBUG, "(RPH) view forward = "
163 << view_forward[0] << "," << view_forward[1] << ","
164 << view_forward[2] );
166 // VIEW_ROT = LARC_TO_SSG * ( VIEWo * VIEW_OFFSET )
167 fgMakeViewRot( VIEW_ROT, VIEW_OFFSET, VIEWo );
170 sgAddVec3( trans_vec, view_pos, pilot_offset_world );
172 // VIEW = VIEW_ROT * TRANS
173 sgCopyMat4( VIEW, VIEW_ROT );
174 sgPostMultMat4ByTransMat4( VIEW, trans_vec );
176 //!!!!!!!!!!!!!!!!!!!
177 // THIS IS THE EXPERIMENTAL VIEWING ANGLE SHIFTER
178 // THE MAJORITY OF THE WORK IS DONE IN GUI.CXX
179 // this in gui.cxx for now just testing
180 extern float GuiQuat_mat[4][4];
181 sgPreMultMat4( VIEW, GuiQuat_mat);
182 // !!!!!!!!!! testing
184 // Given a vector pointing straight down (-Z), map into onto the
185 // local plane representing "horizontal". This should give us the
186 // local direction for moving "south".
187 sgSetVec3( minus_z, 0.0, 0.0, -1.0 );
189 sgmap_vec_onto_cur_surface_plane(world_up, view_pos, minus_z,
191 sgNormalizeVec3(surface_south);
192 // cout << "Surface direction directly south " << surface_south[0] << ","
193 // << surface_south[1] << "," << surface_south[2] << endl;
195 // now calculate the surface east vector
197 sgNegateVec3(world_down, world_up);
198 sgVectorProductVec3(surface_east, surface_south, world_down);
205 FGViewerRPH::~FGViewerRPH( void ) {