simgear/scene/model/SGRotateTransform.cxx

   1 /* -*-c++-*-
   2  *
   3  * Copyright (C) 2006-2007 Mathias Froehlich
   4  *
   5  * This program is free software; you can redistribute it and/or
   6  * modify it under the terms of the GNU General Public License as
   7  * published by the Free Software Foundation; either version 2 of the
   8  * License, or (at your option) any later version.
   9  *
  10  * This program is distributed in the hope that it will be useful, but
  11  * WITHOUT ANY WARRANTY; without even the implied warranty of
  12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  13  * General Public License for more details.
  14  *
  15  * You should have received a copy of the GNU General Public License
  16  * along with this program; if not, write to the Free Software
  17  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
  18  * MA 02110-1301, USA.
  19  *
  20  */
  21
  22 #ifdef HAVE_CONFIG_H
  23 #  include <simgear_config.h>
  24 #endif
  25
  26 #include "SGRotateTransform.hxx"
  27
  28 static void
  29 set_rotation (osg::Matrix &matrix, double position_rad,
  30               const SGVec3d &center, const SGVec3d &axis)
  31 {
  32   double temp_angle = -position_rad;
  33
  34   double s = sin(temp_angle);
  35   double c = cos(temp_angle);
  36   double t = 1 - c;
  37
  38   // axis was normalized at load time
  39   // hint to the compiler to put these into FP registers
  40   double x = axis[0];
  41   double y = axis[1];
  42   double z = axis[2];
  43
  44   matrix(0, 0) = t * x * x + c ;
  45   matrix(0, 1) = t * y * x - s * z ;
  46   matrix(0, 2) = t * z * x + s * y ;
  47   matrix(0, 3) = 0;
  48
  49   matrix(1, 0) = t * x * y + s * z ;
  50   matrix(1, 1) = t * y * y + c ;
  51   matrix(1, 2) = t * z * y - s * x ;
  52   matrix(1, 3) = 0;
  53
  54   matrix(2, 0) = t * x * z - s * y ;
  55   matrix(2, 1) = t * y * z + s * x ;
  56   matrix(2, 2) = t * z * z + c ;
  57   matrix(2, 3) = 0;
  58
  59   // hint to the compiler to put these into FP registers
  60   x = center[0];
  61   y = center[1];
  62   z = center[2];
  63
  64   matrix(3, 0) = x - x*matrix(0, 0) - y*matrix(1, 0) - z*matrix(2, 0);
  65   matrix(3, 1) = y - x*matrix(0, 1) - y*matrix(1, 1) - z*matrix(2, 1);
  66   matrix(3, 2) = z - x*matrix(0, 2) - y*matrix(1, 2) - z*matrix(2, 2);
  67   matrix(3, 3) = 1;
  68 }
  69
  70 SGRotateTransform::SGRotateTransform() :
  71   _center(0, 0, 0),
  72   _axis(0, 0, 0),
  73   _angleRad(0)
  74 {
  75   setReferenceFrame(RELATIVE_RF);
  76 }
  77
  78 bool
  79 SGRotateTransform::computeLocalToWorldMatrix(osg::Matrix& matrix,
  80                                              osg::NodeVisitor* nv) const
  81 {
  82   // This is the fast path, optimize a bit
  83   if (_referenceFrame == RELATIVE_RF) {
  84     // FIXME optimize
  85     osg::Matrix tmp;
  86     set_rotation(tmp, _angleRad, _center, _axis);
  87     matrix.preMult(tmp);
  88   } else {
  89     osg::Matrix tmp;
  90     set_rotation(tmp, _angleRad, _center, _axis);
  91     matrix = tmp;
  92   }
  93   return true;
  94 }
  95
  96 bool
  97 SGRotateTransform::computeWorldToLocalMatrix(osg::Matrix& matrix,
  98                                              osg::NodeVisitor* nv) const
  99 {
 100   if (_referenceFrame == RELATIVE_RF) {
 101     // FIXME optimize
 102     osg::Matrix tmp;
 103     set_rotation(tmp, -_angleRad, _center, _axis);
 104     matrix.postMult(tmp);
 105   } else {
 106     // FIXME optimize
 107     osg::Matrix tmp;
 108     set_rotation(tmp, -_angleRad, _center, _axis);
 109     matrix = tmp;
 110   }
 111   return true;
 112 }
 113
 114 osg::BoundingSphere
 115 SGRotateTransform::computeBound() const
 116 {
 117   osg::BoundingSphere bs = osg::Group::computeBound();
 118   osg::BoundingSphere centerbs(_center.osg(), bs.radius());
 119   centerbs.expandBy(bs);
 120   return centerbs;
 121 }
 122