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 #ifdef HAVE_CONFIG_H
  30 #  include <config.h>
  31 #endif
  32
  33 #include <simgear/debug/logstream.hxx>
  34 #include <simgear/constants.h>
  35 #include <simgear/math/point3d.hxx>
  36 #include <simgear/math/polar3d.hxx>
  37 #include <simgear/math/sg_geodesy.hxx>
  38
  39 #include <Scenery/scenery.hxx>
  40
  41 /* from lookat */
  42 #include <simgear/math/vector.hxx>
  43 #include "globals.hxx"
  44 /* end from lookat */
  45
  46 #include "viewer.hxx"
  47
  48 \f
  49 ////////////////////////////////////////////////////////////////////////
  50 // Implementation of FGViewer.
  51 ////////////////////////////////////////////////////////////////////////
  52
  53 // Constructor
  54 FGViewer::FGViewer( void ):
  55     scalingType(FG_SCALING_MAX),
  56     fov(55.0),
  57     goal_view_offset(0.0),
  58     goal_view_tilt(0.0),
  59     _dirty(true),
  60     _lon_deg(0),
  61     _lat_deg(0),
  62     _alt_ft(0),
  63     _target_lon_deg(0),
  64     _target_lat_deg(0),
  65     _target_alt_ft(0),
  66     _roll_deg(0),
  67     _pitch_deg(0),
  68     _heading_deg(0),
  69     _x_offset_m(0),
  70     _y_offset_m(0),
  71     _z_offset_m(0),
  72     _heading_offset_deg(0),
  73     _pitch_offset_deg(0),
  74     _roll_offset_deg(0)
  75 {
  76     sgdZeroVec3(_absolute_view_pos);
  77     sea_level_radius = SG_EQUATORIAL_RADIUS_M;
  78     //a reasonable guess for init, so that the math doesn't blow up
  79 }
  80
  81
  82 // Destructor
  83 FGViewer::~FGViewer( void ) {
  84 }
  85
  86 void
  87 FGViewer::init ()
  88 {
  89   if ( _type == FG_LOOKAT ) {
  90       set_reverse_view_offset(true);
  91   }
  92
  93   if ( _type == FG_RPH ) {
  94       set_reverse_view_offset(false);
  95   }
  96 }
  97
  98 void
  99 FGViewer::bind ()
 100 {
 101 }
 102
 103 void
 104 FGViewer::unbind ()
 105 {
 106 }
 107
 108 void
 109 FGViewer::setType ( int type )
 110 {
 111   if (type == 0)
 112     _type = FG_RPH;
 113   if (type == 1)
 114     _type = FG_LOOKAT;
 115 }
 116
 117 void
 118 FGViewer::setLongitude_deg (double lon_deg)
 119 {
 120   _dirty = true;
 121   _lon_deg = lon_deg;
 122 }
 123
 124 void
 125 FGViewer::setLatitude_deg (double lat_deg)
 126 {
 127   _dirty = true;
 128   _lat_deg = lat_deg;
 129 }
 130
 131 void
 132 FGViewer::setAltitude_ft (double alt_ft)
 133 {
 134   _dirty = true;
 135   _alt_ft = alt_ft;
 136 }
 137
 138 void
 139 FGViewer::setPosition (double lon_deg, double lat_deg, double alt_ft)
 140 {
 141   _dirty = true;
 142   _lon_deg = lon_deg;
 143   _lat_deg = lat_deg;
 144   _alt_ft = alt_ft;
 145 }
 146
 147 void
 148 FGViewer::setTargetLongitude_deg (double lon_deg)
 149 {
 150   _dirty = true;
 151   _target_lon_deg = lon_deg;
 152 }
 153
 154 void
 155 FGViewer::setTargetLatitude_deg (double lat_deg)
 156 {
 157   _dirty = true;
 158   _target_lat_deg = lat_deg;
 159 }
 160
 161 void
 162 FGViewer::setTargetAltitude_ft (double alt_ft)
 163 {
 164   _dirty = true;
 165   _target_alt_ft = alt_ft;
 166 }
 167
 168 void
 169 FGViewer::setTargetPosition (double lon_deg, double lat_deg, double alt_ft)
 170 {
 171   _dirty = true;
 172   _target_lon_deg = lon_deg;
 173   _target_lat_deg = lat_deg;
 174   _target_alt_ft = alt_ft;
 175 }
 176
 177 void
 178 FGViewer::setRoll_deg (double roll_deg)
 179 {
 180   _dirty = true;
 181   _roll_deg = roll_deg;
 182 }
 183
 184 void
 185 FGViewer::setPitch_deg (double pitch_deg)
 186 {
 187   _dirty = true;
 188   _pitch_deg = pitch_deg;
 189 }
 190
 191 void
 192 FGViewer::setHeading_deg (double heading_deg)
 193 {
 194   _dirty = true;
 195   _heading_deg = heading_deg;
 196 }
 197
 198 void
 199 FGViewer::setOrientation (double roll_deg, double pitch_deg, double heading_deg)
 200 {
 201   _dirty = true;
 202   _roll_deg = roll_deg;
 203   _pitch_deg = pitch_deg;
 204   _heading_deg = heading_deg;
 205 }
 206
 207 void
 208 FGViewer::setXOffset_m (double x_offset_m)
 209 {
 210   _dirty = true;
 211   _x_offset_m = x_offset_m;
 212 }
 213
 214 void
 215 FGViewer::setYOffset_m (double y_offset_m)
 216 {
 217   _dirty = true;
 218   _y_offset_m = y_offset_m;
 219 }
 220
 221 void
 222 FGViewer::setZOffset_m (double z_offset_m)
 223 {
 224   _dirty = true;
 225   _z_offset_m = z_offset_m;
 226 }
 227
 228 void
 229 FGViewer::setPositionOffsets (double x_offset_m, double y_offset_m, double z_offset_m)
 230 {
 231   _dirty = true;
 232   _x_offset_m = x_offset_m;
 233   _y_offset_m = y_offset_m;
 234   _z_offset_m = z_offset_m;
 235 }
 236
 237 void
 238 FGViewer::setRollOffset_deg (double roll_offset_deg)
 239 {
 240   _dirty = true;
 241   _roll_offset_deg = roll_offset_deg;
 242 }
 243
 244 void
 245 FGViewer::setPitchOffset_deg (double pitch_offset_deg)
 246 {
 247   _dirty = true;
 248   _pitch_offset_deg = pitch_offset_deg;
 249 }
 250
 251 void
 252 FGViewer::setHeadingOffset_deg (double heading_offset_deg)
 253 {
 254   _dirty = true;
 255   _heading_offset_deg = heading_offset_deg;
 256 }
 257
 258 void
 259 FGViewer::setOrientationOffsets (double roll_offset_deg, double pitch_offset_deg, double heading_offset_deg)
 260 {
 261   _dirty = true;
 262   _roll_offset_deg = roll_offset_deg;
 263   _pitch_offset_deg = pitch_offset_deg;
 264   _heading_offset_deg = heading_offset_deg;
 265 }
 266
 267 double *
 268 FGViewer::get_absolute_view_pos ()
 269 {
 270   if (_dirty)
 271     recalc();
 272   return _absolute_view_pos;
 273 }
 274
 275 float *
 276 FGViewer::getRelativeViewPos ()
 277 {
 278   if (_dirty)
 279     recalc();
 280   return _relative_view_pos;
 281 }
 282
 283 float *
 284 FGViewer::getZeroElevViewPos ()
 285 {
 286   if (_dirty)
 287     recalc();
 288   return _zero_elev_view_pos;
 289 }
 290
 291
 292 // recalc() is done every time one of the setters is called (making the
 293 // cached data "dirty").  It calculates all the outputs for viewer.
 294 void
 295 FGViewer::recalc ()
 296 {
 297   sgVec3 minus_z, right, forward, tilt;
 298   sgMat4 VIEWo;
 299   sgMat4 tmpROT;  // temp rotation work matrices
 300   sgVec3 tmpVec3;  // temp work vector (3)
 301
 302
 303   // The position vectors originate from the view point or target location
 304   // depending on the type of view.
 305
 306   if (_type == FG_RPH) {
 307     recalcPositionVectors( _lon_deg, _lat_deg, _alt_ft );
 308   } else {
 309     recalcPositionVectors( _target_lon_deg, _target_lat_deg, _target_alt_ft );
 310   }
 311
 312   sgCopyVec3(zero_elev, _zero_elev_view_pos);
 313   sgCopyVec3(view_pos, _relative_view_pos);
 314
 315   if (_type == FG_LOOKAT) {
 316
 317     // Make the world up rotation matrix for lookat
 318     sgMakeRotMat4( UP, _target_lon_deg, 0.0, -_target_lat_deg );
 319
 320     // get the world up verctor from the worldup rotation matrix
 321     sgSetVec3( world_up, UP[0][0], UP[0][1], UP[0][2] );
 322
 323     sgCopyVec3( view_up, world_up );
 324
 325
 326     // create offset vector
 327     sgVec3 lookat_offset;
 328     sgSetVec3( lookat_offset, _x_offset_m, _y_offset_m, _z_offset_m );
 329
 330     // Apply heading orientation and orientation offset to lookat_offset...
 331     sgMakeRotMat4( tmpROT, _heading_offset_deg -_heading_deg, world_up);
 332     sgXformVec3( lookat_offset, lookat_offset, UP );
 333     sgXformVec3( lookat_offset, lookat_offset, tmpROT );
 334
 335     // Apply orientation offset tilt...
 336     // FIXME: Need to get and use a "right" vector instead of 1-0-0
 337     sgSetVec3 (tmpVec3, 1, 0, 0);
 338     sgMakeRotMat4( tmpROT, _pitch_offset_deg, tmpVec3 );
 339     sgXformPnt3( lookat_offset, lookat_offset, tmpROT );
 340
 341     // add the offsets including rotations to the coordinates
 342     sgAddVec3( view_pos, lookat_offset );
 343
 344     // Make the VIEW matrix.
 345     fgMakeLookAtMat4( VIEW, view_pos, view_forward, view_up );
 346
 347
 348     // the VIEW matrix includes both rotation and translation.  Let's
 349     // knock out the translation part to make the VIEW_ROT matrix
 350     sgCopyMat4( VIEW_ROT, VIEW );
 351     VIEW_ROT[3][0] = VIEW_ROT[3][1] = VIEW_ROT[3][2] = 0.0;
 352
 353   }
 354
 355   if (_type == FG_RPH) {
 356
 357     // code to calculate LOCAL matrix calculated from Phi, Theta, and
 358     // Psi (roll, pitch, yaw) in case we aren't running LaRCsim as our
 359     // flight model
 360
 361     fgMakeLOCAL( LOCAL, _pitch_deg * SG_DEGREES_TO_RADIANS,
 362                         _roll_deg * SG_DEGREES_TO_RADIANS,
 363                         -_heading_deg * SG_DEGREES_TO_RADIANS);
 364
 365     // Make the world up rotation matrix for pilot view
 366     sgMakeRotMat4( UP, _lon_deg, 0.0, -_lat_deg );
 367
 368     // get the world up verctor from the worldup rotation matrix
 369     sgSetVec3( world_up, UP[0][0], UP[0][1], UP[0][2] );
 370
 371     // VIEWo becomes the rotation matrix with world_up incorporated
 372     sgCopyMat4( VIEWo, LOCAL );
 373     sgPostMultMat4( VIEWo, UP );
 374
 375     // generate the sg view up and forward vectors
 376     sgSetVec3( view_up, VIEWo[0][0], VIEWo[0][1], VIEWo[0][2] );
 377     sgSetVec3( right, VIEWo[1][0], VIEWo[1][1], VIEWo[1][2] );
 378     sgSetVec3( forward, VIEWo[2][0], VIEWo[2][1], VIEWo[2][2] );
 379
 380     // apply the offsets in world coordinates
 381     sgVec3 pilot_offset_world;
 382     sgSetVec3( pilot_offset_world,
 383                _z_offset_m, _y_offset_m, -_x_offset_m );
 384     sgXformVec3( pilot_offset_world, pilot_offset_world, VIEWo );
 385
 386     // generate the view offset matrix using orientation offset (heading)
 387     sgMakeRotMat4( VIEW_OFFSET, _heading_offset_deg, view_up );
 388
 389     // create a tilt matrix using orientation offset (pitch)
 390     sgMat4 VIEW_TILT;
 391     sgMakeRotMat4( VIEW_TILT, _pitch_offset_deg, right );
 392     sgPreMultMat4(VIEW_OFFSET, VIEW_TILT);
 393     sgXformVec3( view_forward, forward, VIEW_OFFSET );
 394     SG_LOG( SG_VIEW, SG_DEBUG, "(RPH) view forward = "
 395             << view_forward[0] << "," << view_forward[1] << ","
 396             << view_forward[2] );
 397
 398     // VIEW_ROT = LARC_TO_SSG * ( VIEWo * VIEW_OFFSET )
 399     fgMakeViewRot( VIEW_ROT, VIEW_OFFSET, VIEWo );
 400
 401     sgVec3 trans_vec;
 402     sgAddVec3( trans_vec, view_pos, pilot_offset_world );
 403
 404     // VIEW = VIEW_ROT * TRANS
 405     sgCopyMat4( VIEW, VIEW_ROT );
 406     sgPostMultMat4ByTransMat4( VIEW, trans_vec );
 407
 408   }
 409
 410   // Given a vector pointing straight down (-Z), map into onto the
 411   // local plane representing "horizontal".  This should give us the
 412   // local direction for moving "south".
 413   sgSetVec3( minus_z, 0.0, 0.0, -1.0 );
 414
 415   sgmap_vec_onto_cur_surface_plane(world_up, view_pos, minus_z,
 416                                      surface_south);
 417   sgNormalizeVec3(surface_south);
 418
 419   // now calculate the surface east vector
 420   sgVec3 world_down;
 421   sgNegateVec3(world_down, world_up);
 422   sgVectorProductVec3(surface_east, surface_south, world_down);
 423
 424   set_clean();
 425 }
 426
 427 void
 428 FGViewer::recalcPositionVectors (double lon_deg, double lat_deg, double alt_ft) const
 429 {
 430   double sea_level_radius_m;
 431   double lat_geoc_rad;
 432
 433
 434                                 // Convert from geodetic to geocentric
 435                                 // coordinates.
 436   sgGeodToGeoc(lat_deg * SGD_DEGREES_TO_RADIANS,
 437                alt_ft * SG_FEET_TO_METER,
 438                &sea_level_radius_m,
 439                &lat_geoc_rad);
 440
 441                                 // Calculate the cartesian coordinates
 442                                 // of point directly below at sea level.
 443                                 // aka Zero Elevation Position
 444   Point3D p = Point3D(lon_deg * SG_DEGREES_TO_RADIANS,
 445                       lat_geoc_rad,
 446                       sea_level_radius_m);
 447   Point3D tmp = sgPolarToCart3d(p) - scenery.get_next_center();
 448   sgSetVec3(_zero_elev_view_pos, tmp[0], tmp[1], tmp[2]);
 449
 450                                 // Calculate the absolute view position
 451                                 // in fgfs coordinates.
 452                                 // aka Absolute View Position
 453   p.setz(p.radius() + alt_ft * SG_FEET_TO_METER);
 454   tmp = sgPolarToCart3d(p);
 455   sgdSetVec3(_absolute_view_pos, tmp[0], tmp[1], tmp[2]);
 456
 457                                 // Calculate the relative view position
 458                                 // from the scenery center.
 459                                 // aka Relative View Position
 460   sgdVec3 scenery_center;
 461   sgdSetVec3(scenery_center,
 462              scenery.get_next_center().x(),
 463              scenery.get_next_center().y(),
 464              scenery.get_next_center().z());
 465   sgdVec3 view_pos;
 466   sgdSubVec3(view_pos, _absolute_view_pos, scenery_center);
 467   sgSetVec3(_relative_view_pos, view_pos);
 468
 469 }
 470
 471 double
 472 FGViewer::get_h_fov()
 473 {
 474     switch (scalingType) {
 475     case FG_SCALING_WIDTH:  // h_fov == fov
 476         return fov;
 477     case FG_SCALING_MAX:
 478         if (aspect_ratio < 1.0) {
 479             // h_fov == fov
 480             return fov;
 481         } else {
 482             // v_fov == fov
 483             return atan(tan(fov/2 * SG_DEGREES_TO_RADIANS) / aspect_ratio) *
 484                 SG_RADIANS_TO_DEGREES * 2;
 485         }
 486     default:
 487         assert(false);
 488     }
 489 }
 490
 491 double
 492 FGViewer::get_v_fov()
 493 {
 494     switch (scalingType) {
 495     case FG_SCALING_WIDTH:  // h_fov == fov
 496         return atan(tan(fov/2 * SG_DEGREES_TO_RADIANS) * aspect_ratio) *
 497             SG_RADIANS_TO_DEGREES * 2;
 498     case FG_SCALING_MAX:
 499         if (aspect_ratio < 1.0) {
 500             // h_fov == fov
 501             return atan(tan(fov/2 * SG_DEGREES_TO_RADIANS) * aspect_ratio) *
 502                 SG_RADIANS_TO_DEGREES * 2;
 503         } else {
 504             // v_fov == fov
 505             return fov;
 506         }
 507     default:
 508         assert(false);
 509     }
 510 }
 511
 512 void
 513 FGViewer::update (int dt)
 514 {
 515   int i;
 516   for ( i = 0; i < dt; i++ ) {
 517     if ( fabs(get_goal_view_offset() - getHeadingOffset_deg()) < 1 ) {
 518       setHeadingOffset_deg( get_goal_view_offset() );
 519       break;
 520     } else {
 521       // move current_view.headingoffset towards
 522       // current_view.goal_view_offset
 523       if ( get_goal_view_offset() > getHeadingOffset_deg() )
 524         {
 525           if ( get_goal_view_offset() - getHeadingOffset_deg() < 180 ){
 526             inc_view_offset( 0.5 );
 527           } else {
 528             inc_view_offset( -0.5 );
 529           }
 530         } else {
 531           if ( getHeadingOffset_deg() - get_goal_view_offset() < 180 ){
 532             inc_view_offset( -0.5 );
 533           } else {
 534             inc_view_offset( 0.5 );
 535           }
 536         }
 537       if ( getHeadingOffset_deg() > 360 ) {
 538         inc_view_offset( -360 );
 539       } else if ( getHeadingOffset_deg() < 0 ) {
 540         inc_view_offset( 360 );
 541       }
 542     }
 543   }
 544
 545   for ( i = 0; i < dt; i++ ) {
 546     if ( fabs(get_goal_view_tilt() - getPitchOffset_deg()) < 1 ) {
 547       setPitchOffset_deg( get_goal_view_tilt() );
 548       break;
 549     } else {
 550       // move current_view.pitch_offset_deg towards
 551       // current_view.goal_view_tilt
 552       if ( get_goal_view_tilt() > getPitchOffset_deg() )
 553         {
 554           if ( get_goal_view_tilt() - getPitchOffset_deg() < 0 ){
 555             inc_view_tilt( 1.0 );
 556           } else {
 557             inc_view_tilt( -1.0 );
 558           }
 559         } else {
 560           if ( getPitchOffset_deg() - get_goal_view_tilt() < 0 ){
 561             inc_view_tilt( -1.0 );
 562           } else {
 563             inc_view_tilt( 1.0 );
 564           }
 565         }
 566       if ( getPitchOffset_deg() > 90 ) {
 567         setPitchOffset_deg(90);
 568       } else if ( getPitchOffset_deg() < -90 ) {
 569         setPitchOffset_deg( -90 );
 570       }
 571     }
 572   }
 573 }
 574
 575
 576 void FGViewer::fgMakeLookAtMat4 ( sgMat4 dst, const sgVec3 eye, const sgVec3 center,
 577                         const sgVec3 up )
 578 {
 579   // Caveats:
 580   // 1) In order to compute the line of sight, the eye point must not be equal
 581   //    to the center point.
 582   // 2) The up vector must not be parallel to the line of sight from the eye
 583   //    to the center point.
 584
 585   /* Compute the direction vectors */
 586   sgVec3 x,y,z;
 587
 588   /* Y vector = center - eye */
 589   sgSubVec3 ( y, center, eye ) ;
 590
 591   /* Z vector = up */
 592   sgCopyVec3 ( z, up ) ;
 593
 594   /* X vector = Y cross Z */
 595   sgVectorProductVec3 ( x, y, z ) ;
 596
 597   /* Recompute Z = X cross Y */
 598   sgVectorProductVec3 ( z, x, y ) ;
 599
 600   /* Normalize everything */
 601   sgNormaliseVec3 ( x ) ;
 602   sgNormaliseVec3 ( y ) ;
 603   sgNormaliseVec3 ( z ) ;
 604
 605   /* Build the matrix */
 606 #define M(row,col)  dst[row][col]
 607   M(0,0) = x[0];    M(0,1) = x[1];    M(0,2) = x[2];    M(0,3) = 0.0;
 608   M(1,0) = y[0];    M(1,1) = y[1];    M(1,2) = y[2];    M(1,3) = 0.0;
 609   M(2,0) = z[0];    M(2,1) = z[1];    M(2,2) = z[2];    M(2,3) = 0.0;
 610   M(3,0) = eye[0];  M(3,1) = eye[1];  M(3,2) = eye[2];  M(3,3) = 1.0;
 611 #undef M
 612 }
 613 /* end from lookat */
 614
 615 /* from rph */
 616 // VIEW_ROT = LARC_TO_SSG * ( VIEWo * VIEW_OFFSET )
 617 // This takes advantage of the fact that VIEWo and VIEW_OFFSET
 618 // only have entries in the upper 3x3 block
 619 // and that LARC_TO_SSG is just a shift of rows   NHV
 620 void FGViewer::fgMakeViewRot( sgMat4 dst, const sgMat4 m1, const sgMat4 m2 )
 621 {
 622     for ( int j = 0 ; j < 3 ; j++ ) {
 623         dst[2][j] = m2[0][0] * m1[0][j] +
 624             m2[0][1] * m1[1][j] +
 625             m2[0][2] * m1[2][j];
 626
 627         dst[0][j] = m2[1][0] * m1[0][j] +
 628             m2[1][1] * m1[1][j] +
 629             m2[1][2] * m1[2][j];
 630
 631         dst[1][j] = m2[2][0] * m1[0][j] +
 632             m2[2][1] * m1[1][j] +
 633             m2[2][2] * m1[2][j];
 634     }
 635     dst[0][3] =
 636         dst[1][3] =
 637         dst[2][3] =
 638         dst[3][0] =
 639         dst[3][1] =
 640         dst[3][2] = SG_ZERO;
 641     dst[3][3] = SG_ONE;
 642 }
 643
 644
 645 void FGViewer::fgMakeLOCAL( sgMat4 dst, const double Theta,
 646                                 const double Phi, const double Psi)
 647 {
 648     SGfloat cosTheta = (SGfloat) cos(Theta);
 649     SGfloat sinTheta = (SGfloat) sin(Theta);
 650     SGfloat cosPhi   = (SGfloat) cos(Phi);
 651     SGfloat sinPhi   = (SGfloat) sin(Phi);
 652     SGfloat sinPsi   = (SGfloat) sin(Psi) ;
 653     SGfloat cosPsi   = (SGfloat) cos(Psi) ;
 654
 655     dst[0][0] = cosPhi * cosTheta;
 656     dst[0][1] = sinPhi * cosPsi + cosPhi * -sinTheta * -sinPsi;
 657     dst[0][2] = sinPhi * sinPsi + cosPhi * -sinTheta * cosPsi;
 658     dst[0][3] = SG_ZERO;
 659
 660     dst[1][0] = -sinPhi * cosTheta;
 661     dst[1][1] = cosPhi * cosPsi + -sinPhi * -sinTheta * -sinPsi;
 662     dst[1][2] = cosPhi * sinPsi + -sinPhi * -sinTheta * cosPsi;
 663     dst[1][3] = SG_ZERO ;
 664
 665     dst[2][0] = sinTheta;
 666     dst[2][1] = cosTheta * -sinPsi;
 667     dst[2][2] = cosTheta * cosPsi;
 668     dst[2][3] = SG_ZERO;
 669
 670     dst[3][0] = SG_ZERO;
 671     dst[3][1] = SG_ZERO;
 672     dst[3][2] = SG_ZERO;
 673     dst[3][3] = SG_ONE ;
 674 }
 675
 676 /* end from rph */
 677