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 <osgDB/Registry>
  27 #include <osgDB/Input>
  28 #include <osgDB/Output>
  29
  30 #include "SGRotateTransform.hxx"
  31
  32 static void
  33 set_rotation (osg::Matrix &matrix, double position_rad,
  34               const SGVec3d &center, const SGVec3d &axis)
  35 {
  36   double temp_angle = -position_rad;
  37
  38   double s = sin(temp_angle);
  39   double c = cos(temp_angle);
  40   double t = 1 - c;
  41
  42   // axis was normalized at load time
  43   // hint to the compiler to put these into FP registers
  44   double x = axis[0];
  45   double y = axis[1];
  46   double z = axis[2];
  47
  48   matrix(0, 0) = t * x * x + c ;
  49   matrix(0, 1) = t * y * x - s * z ;
  50   matrix(0, 2) = t * z * x + s * y ;
  51   matrix(0, 3) = 0;
  52
  53   matrix(1, 0) = t * x * y + s * z ;
  54   matrix(1, 1) = t * y * y + c ;
  55   matrix(1, 2) = t * z * y - s * x ;
  56   matrix(1, 3) = 0;
  57
  58   matrix(2, 0) = t * x * z - s * y ;
  59   matrix(2, 1) = t * y * z + s * x ;
  60   matrix(2, 2) = t * z * z + c ;
  61   matrix(2, 3) = 0;
  62
  63   // hint to the compiler to put these into FP registers
  64   x = center[0];
  65   y = center[1];
  66   z = center[2];
  67
  68   matrix(3, 0) = x - x*matrix(0, 0) - y*matrix(1, 0) - z*matrix(2, 0);
  69   matrix(3, 1) = y - x*matrix(0, 1) - y*matrix(1, 1) - z*matrix(2, 1);
  70   matrix(3, 2) = z - x*matrix(0, 2) - y*matrix(1, 2) - z*matrix(2, 2);
  71   matrix(3, 3) = 1;
  72 }
  73
  74 SGRotateTransform::SGRotateTransform() :
  75   _center(0, 0, 0),
  76   _axis(0, 0, 0),
  77   _angleRad(0)
  78 {
  79   setReferenceFrame(RELATIVE_RF);
  80 }
  81
  82 SGRotateTransform::SGRotateTransform(const SGRotateTransform& rot,
  83                                      const osg::CopyOp& copyop) :
  84     osg::Transform(rot, copyop),
  85     _center(rot._center),
  86     _axis(rot._axis),
  87     _angleRad(rot._angleRad)
  88 {
  89 }
  90
  91 bool
  92 SGRotateTransform::computeLocalToWorldMatrix(osg::Matrix& matrix,
  93                                              osg::NodeVisitor* nv) const
  94 {
  95   // This is the fast path, optimize a bit
  96   if (_referenceFrame == RELATIVE_RF) {
  97     // FIXME optimize
  98     osg::Matrix tmp;
  99     set_rotation(tmp, _angleRad, _center, _axis);
 100     matrix.preMult(tmp);
 101   } else {
 102     osg::Matrix tmp;
 103     set_rotation(tmp, _angleRad, _center, _axis);
 104     matrix = tmp;
 105   }
 106   return true;
 107 }
 108
 109 bool
 110 SGRotateTransform::computeWorldToLocalMatrix(osg::Matrix& matrix,
 111                                              osg::NodeVisitor* nv) const
 112 {
 113   if (_referenceFrame == RELATIVE_RF) {
 114     // FIXME optimize
 115     osg::Matrix tmp;
 116     set_rotation(tmp, -_angleRad, _center, _axis);
 117     matrix.postMult(tmp);
 118   } else {
 119     // FIXME optimize
 120     osg::Matrix tmp;
 121     set_rotation(tmp, -_angleRad, _center, _axis);
 122     matrix = tmp;
 123   }
 124   return true;
 125 }
 126
 127 osg::BoundingSphere
 128 SGRotateTransform::computeBound() const
 129 {
 130   osg::BoundingSphere bs = osg::Group::computeBound();
 131   osg::BoundingSphere centerbs(toOsg(_center), bs.radius());
 132   centerbs.expandBy(bs);
 133   return centerbs;
 134 }
 135
 136 // Functions to read/write SGRotateTransform from/to a .osg file.
 137
 138 namespace {
 139
 140 bool RotateTransform_readLocalData(osg::Object& obj, osgDB::Input& fr)
 141 {
 142     SGRotateTransform& rot = static_cast<SGRotateTransform&>(obj);
 143     if (fr[0].matchWord("center")) {
 144         ++fr;
 145         osg::Vec3d center;
 146         if (fr.readSequence(center))
 147             fr += 3;
 148         else
 149             return false;
 150         rot.setCenter(toSG(center));
 151     }
 152     if (fr[0].matchWord("axis")) {
 153         ++fr;
 154         osg::Vec3d axis;
 155         if (fr.readSequence(axis))
 156             fr += 3;
 157         else
 158             return false;
 159         rot.setCenter(toSG(axis));
 160     }
 161     if (fr[0].matchWord("angle")) {
 162         ++fr;
 163         double angle;
 164         if (fr[0].getFloat(angle))
 165             ++fr;
 166         else
 167             return false;
 168         rot.setAngleRad(angle);
 169     }
 170     return true;
 171 }
 172
 173 bool RotateTransform_writeLocalData(const osg::Object& obj, osgDB::Output& fw)
 174 {
 175     const SGRotateTransform& rot = static_cast<const SGRotateTransform&>(obj);
 176     const SGVec3d& center = rot.getCenter();
 177     const SGVec3d& axis = rot.getAxis();
 178     const double angle = rot.getAngleRad();
 179     int prec = fw.precision();
 180     fw.precision(15);
 181     fw.indent() << "center ";
 182     for (int i = 0; i < 3; i++) {
 183         fw << center(i) << " ";
 184     }
 185     fw << std::endl;
 186     fw.precision(prec);
 187     fw.indent() << "axis ";
 188     for (int i = 0; i < 3; i++) {
 189         fw << axis(i) << " ";
 190     }
 191     fw << std::endl;
 192     fw.indent() << "angle ";
 193     fw << angle << std::endl;
 194     return true;
 195 }
 196 }
 197
 198 osgDB::RegisterDotOsgWrapperProxy g_SGRotateTransformProxy
 199 (
 200     new SGRotateTransform,
 201     "SGRotateTransform",
 202     "Object Node Transform SGRotateTransform Group",
 203     &RotateTransform_readLocalData,
 204     &RotateTransform_writeLocalData
 205 );