]> git.mxchange.org Git - flightgear.git/blob - src/Main/viewer.cxx
Added static port system and a new altimeter model connected to it.
[flightgear.git] / src / Main / viewer.cxx
1 // viewer.cxx -- class for managing a viewer in the flightgear world.
2 //
3 // Written by Curtis Olson, started August 1997.
4 //                          overhaul started October 2000.
5 //   partially rewritten by Jim Wilson jim@kelcomaine.com using interface
6 //                          by David Megginson March 2002
7 //
8 // Copyright (C) 1997 - 2000  Curtis L. Olson  - curt@flightgear.org
9 //
10 // This program is free software; you can redistribute it and/or
11 // modify it under the terms of the GNU General Public License as
12 // published by the Free Software Foundation; either version 2 of the
13 // License, or (at your option) any later version.
14 //
15 // This program is distributed in the hope that it will be useful, but
16 // WITHOUT ANY WARRANTY; without even the implied warranty of
17 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
18 // General Public License for more details.
19 //
20 // You should have received a copy of the GNU General Public License
21 // along with this program; if not, write to the Free Software
22 // Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
23 //
24 // $Id$
25
26
27 #include <simgear/compiler.h>
28
29 #include "fg_props.hxx"
30
31 #ifdef HAVE_CONFIG_H
32 #  include <config.h>
33 #endif
34
35 #include <simgear/debug/logstream.hxx>
36 #include <simgear/constants.h>
37 #include <simgear/math/point3d.hxx>
38 #include <simgear/math/polar3d.hxx>
39 #include <simgear/math/sg_geodesy.hxx>
40
41 #include <Scenery/scenery.hxx>
42
43 #include <simgear/math/vector.hxx>
44 #include <Main/globals.hxx>
45 #include <Model/acmodel.hxx>
46 #include <Model/model.hxx>
47
48 #include "viewer.hxx"
49
50 \f
51 //////////////////////////////////////////////////////////////////
52 // Norman's Optimized matrix rotators!                          //
53 //////////////////////////////////////////////////////////////////
54
55
56 // Since these are pure rotation matrices we can save some bookwork
57 // by considering them to be 3x3 until the very end -- NHV
58 static void MakeVIEW_OFFSET( sgMat4 dst,
59                       const float angle1, const sgVec3 axis1,
60                       const float angle2, const sgVec3 axis2 )
61 {
62     // make rotmatrix1 from angle and axis
63     float s = (float) sin ( angle1 ) ;
64     float c = (float) cos ( angle1 ) ;
65     float t = SG_ONE - c ;
66
67     sgMat3 mat1;
68     float tmp = t * axis1[0];
69     mat1[0][0] = tmp * axis1[0] + c ;
70     mat1[0][1] = tmp * axis1[1] + s * axis1[2] ;
71     mat1[0][2] = tmp * axis1[2] - s * axis1[1] ;
72
73     tmp = t * axis1[1];
74     mat1[1][0] = tmp * axis1[0] - s * axis1[2] ;
75     mat1[1][1] = tmp * axis1[1] + c ;
76     mat1[1][2] = tmp * axis1[2] + s * axis1[0] ;
77
78     tmp = t * axis1[2];
79     mat1[2][0] = tmp * axis1[0] + s * axis1[1] ;
80     mat1[2][1] = tmp * axis1[1] - s * axis1[0] ;
81     mat1[2][2] = tmp * axis1[2] + c ;
82
83     // make rotmatrix2 from angle and axis
84     s = (float) sin ( angle2 ) ;
85     c = (float) cos ( angle2 ) ;
86     t = SG_ONE - c ;
87
88     sgMat3 mat2;
89     tmp = t * axis2[0];
90     mat2[0][0] = tmp * axis2[0] + c ;
91     mat2[0][1] = tmp * axis2[1] + s * axis2[2] ;
92     mat2[0][2] = tmp * axis2[2] - s * axis2[1] ;
93
94     tmp = t * axis2[1];
95     mat2[1][0] = tmp * axis2[0] - s * axis2[2] ;
96     mat2[1][1] = tmp * axis2[1] + c ;
97     mat2[1][2] = tmp * axis2[2] + s * axis2[0] ;
98
99     tmp = t * axis2[2];
100     mat2[2][0] = tmp * axis2[0] + s * axis2[1] ;
101     mat2[2][1] = tmp * axis2[1] - s * axis2[0] ;
102     mat2[2][2] = tmp * axis2[2] + c ;
103
104     // multiply matrices
105     for ( int j = 0 ; j < 3 ; j++ ) {
106         dst[0][j] = mat2[0][0] * mat1[0][j] +
107                     mat2[0][1] * mat1[1][j] +
108                     mat2[0][2] * mat1[2][j];
109
110         dst[1][j] = mat2[1][0] * mat1[0][j] +
111                     mat2[1][1] * mat1[1][j] +
112                     mat2[1][2] * mat1[2][j];
113
114         dst[2][j] = mat2[2][0] * mat1[0][j] +
115                     mat2[2][1] * mat1[1][j] +
116                     mat2[2][2] * mat1[2][j];
117     }
118     // fill in 4x4 matrix elements
119     dst[0][3] = SG_ZERO; 
120     dst[1][3] = SG_ZERO; 
121     dst[2][3] = SG_ZERO;
122     dst[3][0] = SG_ZERO;
123     dst[3][1] = SG_ZERO;
124     dst[3][2] = SG_ZERO;
125     dst[3][3] = SG_ONE;
126 }
127
128
129 ////////////////////////////////////////////////////////////////////////
130 // Implementation of FGViewer.
131 ////////////////////////////////////////////////////////////////////////
132
133 // Constructor
134 FGViewer::FGViewer( fgViewType Type, bool from_model, int from_model_index, bool at_model, int at_model_index, double x_offset_m, double y_offset_m, double z_offset_m, double near_m ):
135     _dirty(true),
136     _lon_deg(0),
137     _lat_deg(0),
138     _alt_ft(0),
139     _target_lon_deg(0),
140     _target_lat_deg(0),
141     _target_alt_ft(0),
142     _roll_deg(0),
143     _pitch_deg(0),
144     _heading_deg(0),
145     _roll_offset_deg(0),
146     _pitch_offset_deg(0),
147     _heading_offset_deg(0),
148     _goal_pitch_offset_deg(0.0),
149     _goal_heading_offset_deg(0.0),
150     _scaling_type(FG_SCALING_MAX),
151     _fov_deg(55.0)
152 {
153     sgdZeroVec3(_absolute_view_pos);
154     _type = Type;
155     _from_model = from_model;
156     _from_model_index = from_model_index;
157     _at_model = at_model;
158     _at_model_index = at_model_index;
159     _x_offset_m = x_offset_m;
160     _y_offset_m = y_offset_m;
161     _z_offset_m = z_offset_m;
162     _ground_level_nearplane_m = near_m;
163     //a reasonable guess for init, so that the math doesn't blow up
164 }
165
166
167 // Destructor
168 FGViewer::~FGViewer( void ) {
169 }
170
171 void
172 FGViewer::init ()
173 {
174   if ( _from_model )
175     _location = (FGLocation *) globals->get_aircraft_model()->get3DModel()->getFGLocation();
176   else
177     _location = (FGLocation *) new FGLocation;
178
179   if ( _type == FG_LOOKAT ) {
180     if ( _at_model )
181       _target_location = (FGLocation *) globals->get_aircraft_model()->get3DModel()->getFGLocation();
182     else
183       _target_location = (FGLocation *) new FGLocation;
184   }
185 }
186
187 void
188 FGViewer::bind ()
189 {
190 }
191
192 void
193 FGViewer::unbind ()
194 {
195 }
196
197 void
198 FGViewer::setType ( int type )
199 {
200   if (type == 0)
201     _type = FG_LOOKFROM;
202   if (type == 1)
203     _type = FG_LOOKAT;
204 }
205
206 void
207 FGViewer::setLongitude_deg (double lon_deg)
208 {
209   _dirty = true;
210   _lon_deg = lon_deg;
211 }
212
213 void
214 FGViewer::setLatitude_deg (double lat_deg)
215 {
216   _dirty = true;
217   _lat_deg = lat_deg;
218 }
219
220 void
221 FGViewer::setAltitude_ft (double alt_ft)
222 {
223   _dirty = true;
224   _alt_ft = alt_ft;
225 }
226
227 void
228 FGViewer::setPosition (double lon_deg, double lat_deg, double alt_ft)
229 {
230   _dirty = true;
231   _lon_deg = lon_deg;
232   _lat_deg = lat_deg;
233   _alt_ft = alt_ft;
234 }
235
236 void
237 FGViewer::setTargetLongitude_deg (double lon_deg)
238 {
239   _dirty = true;
240   _target_lon_deg = lon_deg;
241 }
242
243 void
244 FGViewer::setTargetLatitude_deg (double lat_deg)
245 {
246   _dirty = true;
247   _target_lat_deg = lat_deg;
248 }
249
250 void
251 FGViewer::setTargetAltitude_ft (double alt_ft)
252 {
253   _dirty = true;
254   _target_alt_ft = alt_ft;
255 }
256
257 void
258 FGViewer::setTargetPosition (double lon_deg, double lat_deg, double alt_ft)
259 {
260   _dirty = true;
261   _target_lon_deg = lon_deg;
262   _target_lat_deg = lat_deg;
263   _target_alt_ft = alt_ft;
264 }
265
266 void
267 FGViewer::setRoll_deg (double roll_deg)
268 {
269   _dirty = true;
270   _roll_deg = roll_deg;
271 }
272
273 void
274 FGViewer::setPitch_deg (double pitch_deg)
275 {
276   _dirty = true;
277   _pitch_deg = pitch_deg;
278 }
279
280 void
281 FGViewer::setHeading_deg (double heading_deg)
282 {
283   _dirty = true;
284   _heading_deg = heading_deg;
285 }
286
287 void
288 FGViewer::setOrientation (double roll_deg, double pitch_deg, double heading_deg)
289 {
290   _dirty = true;
291   _roll_deg = roll_deg;
292   _pitch_deg = pitch_deg;
293   _heading_deg = heading_deg;
294 }
295
296 void
297 FGViewer::setTargetRoll_deg (double target_roll_deg)
298 {
299   _dirty = true;
300   _target_roll_deg = target_roll_deg;
301 }
302
303 void
304 FGViewer::setTargetPitch_deg (double target_pitch_deg)
305 {
306   _dirty = true;
307   _target_pitch_deg = target_pitch_deg;
308 }
309
310 void
311 FGViewer::setTargetHeading_deg (double target_heading_deg)
312 {
313   _dirty = true;
314   _target_heading_deg = target_heading_deg;
315 }
316
317 void
318 FGViewer::setTargetOrientation (double target_roll_deg, double target_pitch_deg, double target_heading_deg)
319 {
320   _dirty = true;
321   _target_roll_deg = target_roll_deg;
322   _target_pitch_deg = target_pitch_deg;
323   _target_heading_deg = target_heading_deg;
324 }
325
326 void
327 FGViewer::setXOffset_m (double x_offset_m)
328 {
329   _dirty = true;
330   _x_offset_m = x_offset_m;
331 }
332
333 void
334 FGViewer::setYOffset_m (double y_offset_m)
335 {
336   _dirty = true;
337   _y_offset_m = y_offset_m;
338 }
339
340 void
341 FGViewer::setZOffset_m (double z_offset_m)
342 {
343   _dirty = true;
344   _z_offset_m = z_offset_m;
345 }
346
347 void
348 FGViewer::setPositionOffsets (double x_offset_m, double y_offset_m, double z_offset_m)
349 {
350   _dirty = true;
351   _x_offset_m = x_offset_m;
352   _y_offset_m = y_offset_m;
353   _z_offset_m = z_offset_m;
354 }
355
356 void
357 FGViewer::setRollOffset_deg (double roll_offset_deg)
358 {
359   _dirty = true;
360   _roll_offset_deg = roll_offset_deg;
361 }
362
363 void
364 FGViewer::setPitchOffset_deg (double pitch_offset_deg)
365 {
366   _dirty = true;
367   _pitch_offset_deg = pitch_offset_deg;
368 }
369
370 void
371 FGViewer::setHeadingOffset_deg (double heading_offset_deg)
372 {
373   _dirty = true;
374   _heading_offset_deg = heading_offset_deg;
375 }
376
377 void
378 FGViewer::setGoalRollOffset_deg (double goal_roll_offset_deg)
379 {
380   _dirty = true;
381   _goal_roll_offset_deg = goal_roll_offset_deg;
382 }
383
384 void
385 FGViewer::setGoalPitchOffset_deg (double goal_pitch_offset_deg)
386 {
387   _dirty = true;
388   _goal_pitch_offset_deg = goal_pitch_offset_deg;
389   if ( _goal_pitch_offset_deg < -90 ) {
390     _goal_pitch_offset_deg = -90.0;
391   }
392   if ( _goal_pitch_offset_deg > 90.0 ) {
393     _goal_pitch_offset_deg = 90.0;
394   }
395
396 }
397
398 void
399 FGViewer::setGoalHeadingOffset_deg (double goal_heading_offset_deg)
400 {
401   _dirty = true;
402   _goal_heading_offset_deg = goal_heading_offset_deg;
403   while ( _goal_heading_offset_deg < 0.0 ) {
404     _goal_heading_offset_deg += 360;
405   }
406   while ( _goal_heading_offset_deg > 360 ) {
407     _goal_heading_offset_deg -= 360;
408   }
409 }
410
411 void
412 FGViewer::setOrientationOffsets (double roll_offset_deg, double pitch_offset_deg, double heading_offset_deg)
413 {
414   _dirty = true;
415   _roll_offset_deg = roll_offset_deg;
416   _pitch_offset_deg = pitch_offset_deg;
417   _heading_offset_deg = heading_offset_deg;
418 }
419
420 double *
421 FGViewer::get_absolute_view_pos () 
422 {
423   if (_dirty)
424     recalc();
425   return _absolute_view_pos;
426 }
427
428 float *
429 FGViewer::getRelativeViewPos () 
430 {
431   if (_dirty)
432     recalc();
433   return _relative_view_pos;
434 }
435
436 float *
437 FGViewer::getZeroElevViewPos () 
438 {
439   if (_dirty)
440     recalc();
441   return _zero_elev_view_pos;
442 }
443
444 void
445 FGViewer::updateFromModelLocation (FGLocation * location)
446 {
447   sgCopyMat4(LOCAL, location->getCachedTransformMatrix());
448 }
449
450 void
451 FGViewer::recalcOurOwnLocation (FGLocation * location, double lon_deg, double lat_deg, double alt_ft, 
452                         double roll_deg, double pitch_deg, double heading_deg)
453 {
454   // update from our own data...
455   location->setPosition( lon_deg, lat_deg, alt_ft );
456   location->setOrientation( roll_deg, pitch_deg, heading_deg );
457   sgCopyMat4(LOCAL, location->getTransformMatrix());
458 }
459
460 // recalc() is done every time one of the setters is called (making the 
461 // cached data "dirty") on the next "get".  It calculates all the outputs 
462 // for viewer.
463 void
464 FGViewer::recalc ()
465 {
466   if (_type == FG_LOOKFROM) {
467     recalcLookFrom();
468   } else {
469     recalcLookAt();
470   }
471
472   set_clean();
473 }
474
475 // recalculate for LookFrom view type...
476 void
477 FGViewer::recalcLookFrom ()
478 {
479
480   sgVec3 right, forward;
481   sgVec3 eye_pos;
482   sgVec3 position_offset; // eye position offsets (xyz)
483
484   // LOOKFROM mode...
485
486   // Update location data...
487   if ( _from_model ) {
488     // update or data from model location
489     updateFromModelLocation(_location);
490   } else {
491     // update from our own data...
492     recalcOurOwnLocation( _location, _lon_deg, _lat_deg, _alt_ft, 
493           _roll_deg, _pitch_deg, _heading_deg );
494   }
495
496   // copy data from location class to local items...
497   copyLocationData();
498
499   // make sg vectors view up, right and forward vectors from LOCAL
500   sgSetVec3( _view_up, LOCAL[2][0], LOCAL[2][1], LOCAL[2][2] );
501   sgSetVec3( right, LOCAL[1][0], LOCAL[1][1], LOCAL[1][2] );
502   sgSetVec3( forward, -LOCAL[0][0], -LOCAL[0][1], -LOCAL[0][2] );
503
504
505   // Note that when in "lookfrom" view the "view up" vector is always applied
506   // to the viewer.  View up is based on verticle of the aircraft itself. (see
507   // "LOCAL" matrix above)
508
509   // Orientation Offsets matrix
510   MakeVIEW_OFFSET( VIEW_OFFSET,
511     _heading_offset_deg  * SG_DEGREES_TO_RADIANS, _view_up,
512     _pitch_offset_deg  * SG_DEGREES_TO_RADIANS, right );
513
514   // Make the VIEW matrix.
515   sgSetVec4(VIEW[0], right[0], right[1], right[2],SG_ZERO);
516   sgSetVec4(VIEW[1], forward[0], forward[1], forward[2],SG_ZERO);
517   sgSetVec4(VIEW[2], _view_up[0], _view_up[1], _view_up[2],SG_ZERO);
518   sgSetVec4(VIEW[3], SG_ZERO, SG_ZERO, SG_ZERO,SG_ONE);
519
520   // rotate matrix to get a matrix to apply Eye Position Offsets
521   sgMat4 VIEW_UP; // L0 forward L1 right L2 up
522   sgCopyVec4(VIEW_UP[0], LOCAL[1]); 
523   sgCopyVec4(VIEW_UP[1], LOCAL[2]);
524   sgCopyVec4(VIEW_UP[2], LOCAL[0]);
525   sgZeroVec4(VIEW_UP[3]);
526
527   // Eye Position Offsets to vector
528   sgSetVec3( position_offset, _x_offset_m, _y_offset_m, _z_offset_m );
529   sgXformVec3( position_offset, position_offset, VIEW_UP);
530
531   // add the offsets including rotations to the translation vector
532   sgAddVec3( _view_pos, position_offset );
533
534   // multiply the OFFSETS (for heading and pitch) into the VIEW
535   sgPostMultMat4(VIEW, VIEW_OFFSET);
536
537   // add the position data to the matrix
538   sgSetVec4(VIEW[3], _view_pos[0], _view_pos[1], _view_pos[2],SG_ONE);
539
540 }
541
542 void
543 FGViewer::recalcLookAt ()
544 {
545
546   sgVec3 right;
547   sgVec3 eye_pos, at_pos;
548   sgVec3 position_offset; // eye position offsets (xyz)
549
550   // The position vectors originate from the view point or target location
551   // depending on the type of view.
552
553   // LOOKAT mode...
554
555   // Update location data for target...
556   if ( _at_model ) {
557     // update or data from model location
558     updateFromModelLocation(_target_location);
559   } else {
560     // if not model then calculate our own target position...
561     recalcOurOwnLocation( _target_location, _target_lon_deg, _target_lat_deg, _target_alt_ft, 
562           _target_roll_deg, _target_pitch_deg, _target_heading_deg );
563   }
564   // calculate the "at" target object positon relative to eye or view's tile center...
565   sgdVec3 dVec3;
566   sgdSetVec3(dVec3,  _location->get_tile_center()[0], _location->get_tile_center()[1], _location->get_tile_center()[2]);
567   sgdSubVec3(dVec3, _target_location->get_absolute_view_pos(), dVec3 );
568   sgSetVec3(at_pos, dVec3[0], dVec3[1], dVec3[2]);
569
570   // Update location data for eye...
571   if ( _from_model ) {
572     // update or data from model location
573     updateFromModelLocation(_location);
574   } else {
575     // update from our own data, just the rotation here...
576     recalcOurOwnLocation( _location, _lon_deg, _lat_deg, _alt_ft, 
577           _roll_deg, _pitch_deg, _heading_deg );
578   }
579   // save the eye positon...
580   sgCopyVec3(eye_pos,  _location->get_view_pos());
581
582   // copy data from location class to local items...
583   copyLocationData();
584
585   // make sg vectors view up, right and forward vectors from LOCAL
586   sgSetVec3( _view_up, LOCAL[2][0], LOCAL[2][1], LOCAL[2][2] );
587   sgSetVec3( right, LOCAL[1][0], LOCAL[1][1], LOCAL[1][2] );
588
589   // Note that when in "lookat" view the "world up" vector is always applied
590   // to the viewer.  World up is based on verticle at a given lon/lat (see
591   // matrix "UP" above).
592
593   // Orientation Offsets matrix
594   MakeVIEW_OFFSET( VIEW_OFFSET,
595     (_heading_offset_deg -_heading_deg) * SG_DEGREES_TO_RADIANS, _world_up,
596     _pitch_offset_deg * SG_DEGREES_TO_RADIANS, right );
597    
598   // add in the Orientation Offsets here
599   sgSetVec3( position_offset, _x_offset_m, _y_offset_m, _z_offset_m );
600   sgXformVec3( position_offset, position_offset, UP);
601
602   sgXformVec3( position_offset, position_offset, VIEW_OFFSET );
603
604   // add the Position offsets from object to the eye position
605   sgAddVec3( eye_pos, eye_pos, position_offset );
606
607   // Make the VIEW matrix for a "LOOKAT".
608   sgMakeLookAtMat4( VIEW, eye_pos, at_pos, _view_up );
609
610 }
611
612 // copy results from location class to viewer...
613 // FIXME: some of these should be changed to reference directly to FGLocation...
614 void
615 FGViewer::copyLocationData()
616 {
617   // Get our friendly vectors from the eye location...
618   sgCopyVec3(_zero_elev_view_pos,  _location->get_zero_elev());
619   sgCopyVec3(_relative_view_pos, _location->get_view_pos());
620   sgdCopyVec3(_absolute_view_pos, _location->get_absolute_view_pos());
621   sgCopyMat4(UP, _location->getCachedUpMatrix());
622   sgCopyVec3(_world_up, _location->get_world_up());
623   // these are the vectors that the sun and moon code like to get...
624   sgCopyVec3(_surface_east, _location->get_surface_east());
625   sgCopyVec3(_surface_south, _location->get_surface_south());
626
627   // Update viewer's postion data for the eye location...
628   _lon_deg = _location->getLongitude_deg();
629   _lat_deg = _location->getLatitude_deg();
630   _alt_ft = _location->getAltitudeASL_ft();
631   _roll_deg = _location->getRoll_deg();
632   _pitch_deg = _location->getPitch_deg();
633   _heading_deg = _location->getHeading_deg();
634
635   // Update viewer's postion data for the target (at object) location
636   if (_type == FG_LOOKAT) {
637     _target_lon_deg = _target_location->getLongitude_deg();
638     _target_lat_deg = _target_location->getLatitude_deg();
639     _target_alt_ft = _target_location->getAltitudeASL_ft();
640     _target_roll_deg = _target_location->getRoll_deg();
641     _target_pitch_deg = _target_location->getPitch_deg();
642     _target_heading_deg = _target_location->getHeading_deg();
643   }
644
645   // copy coordinates to outputs for viewer...
646   sgCopyVec3(_zero_elev, _zero_elev_view_pos);
647   sgCopyVec3(_view_pos, _relative_view_pos);
648 }
649
650 double
651 FGViewer::get_h_fov()
652 {
653     switch (_scaling_type) {
654     case FG_SCALING_WIDTH:  // h_fov == fov
655         return _fov_deg;
656     case FG_SCALING_MAX:
657         if (_aspect_ratio < 1.0) {
658             // h_fov == fov
659             return _fov_deg;
660         } else {
661             // v_fov == fov
662             return atan(tan(_fov_deg/2 * SG_DEGREES_TO_RADIANS) / _aspect_ratio) *
663                 SG_RADIANS_TO_DEGREES * 2;
664         }
665     default:
666         assert(false);
667     }
668     return 0.0;
669 }
670
671
672
673 double
674 FGViewer::get_v_fov()
675 {
676     switch (_scaling_type) {
677     case FG_SCALING_WIDTH:  // h_fov == fov
678         return atan(tan(_fov_deg/2 * SG_DEGREES_TO_RADIANS) * _aspect_ratio) *
679             SG_RADIANS_TO_DEGREES * 2;
680     case FG_SCALING_MAX:
681         if (_aspect_ratio < 1.0) {
682             // h_fov == fov
683             return atan(tan(_fov_deg/2 * SG_DEGREES_TO_RADIANS) * _aspect_ratio) *
684                 SG_RADIANS_TO_DEGREES * 2;
685         } else {
686             // v_fov == fov
687             return _fov_deg;
688         }
689     default:
690         assert(false);
691     }
692     return 0.0;
693 }
694
695 void
696 FGViewer::update (double dt)
697 {
698   int i;
699   int dt_ms = int(dt * 1000);
700   for ( i = 0; i < dt_ms; i++ ) {
701     if ( fabs( _goal_heading_offset_deg - _heading_offset_deg) < 1 ) {
702       setHeadingOffset_deg( _goal_heading_offset_deg );
703       break;
704     } else {
705       // move current_view.headingoffset towards
706       // current_view.goal_view_offset
707       if ( _goal_heading_offset_deg > _heading_offset_deg )
708         {
709           if ( _goal_heading_offset_deg - _heading_offset_deg < 180 ){
710             incHeadingOffset_deg( 0.5 );
711           } else {
712             incHeadingOffset_deg( -0.5 );
713           }
714         } else {
715           if ( _heading_offset_deg - _goal_heading_offset_deg < 180 ){
716             incHeadingOffset_deg( -0.5 );
717           } else {
718             incHeadingOffset_deg( 0.5 );
719           }
720         }
721       if ( _heading_offset_deg > 360 ) {
722         incHeadingOffset_deg( -360 );
723       } else if ( _heading_offset_deg < 0 ) {
724         incHeadingOffset_deg( 360 );
725       }
726     }
727   }
728
729   for ( i = 0; i < dt_ms; i++ ) {
730     if ( fabs( _goal_pitch_offset_deg - _pitch_offset_deg ) < 1 ) {
731       setPitchOffset_deg( _goal_pitch_offset_deg );
732       break;
733     } else {
734       // move current_view.pitch_offset_deg towards
735       // current_view.goal_pitch_offset
736       if ( _goal_pitch_offset_deg > _pitch_offset_deg )
737         {
738           incPitchOffset_deg( 1.0 );
739         } else {
740             incPitchOffset_deg( -1.0 );
741         }
742       if ( _pitch_offset_deg > 90 ) {
743         setPitchOffset_deg(90);
744       } else if ( _pitch_offset_deg < -90 ) {
745         setPitchOffset_deg( -90 );
746       }
747     }
748   }
749 }