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