simgear/scene/model/SGTrackToAnimation.cxx

   1 // TrackTo animation
   2 //
   3 // Copyright (C) 2013  Thomas Geymayer <tomgey@gmail.com>
   4 //
   5 // This library is free software; you can redistribute it and/or
   6 // modify it under the terms of the GNU Library General Public
   7 // License as published by the Free Software Foundation; either
   8 // version 2 of the License, or (at your option) any later version.
   9 //
  10 // This library is distributed in the hope that it will be useful,
  11 // but WITHOUT ANY WARRANTY; without even the implied warranty of
  12 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  13 // Library General Public License for more details.
  14 //
  15 // You should have received a copy of the GNU Library General Public
  16 // License along with this library; if not, write to the Free Software
  17 // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301, USA
  18
  19 #include "SGRotateTransform.hxx"
  20 #include "SGTrackToAnimation.hxx"
  21
  22 #include <simgear/scene/util/OsgMath.hxx>
  23 #include <osg/MatrixTransform>
  24 #include <cassert>
  25
  26 /**
  27  *
  28  */
  29 static osg::NodePath requireNodePath( osg::Node* node,
  30                                       osg::Node* haltTraversalAtNode = 0 )
  31 {
  32   const osg::NodePathList node_paths =
  33     node->getParentalNodePaths(haltTraversalAtNode);
  34   return node_paths.at(0);
  35 }
  36
  37 /**
  38  * Get a subpath of an osg::NodePath
  39  *
  40  * @param path  Path to extract subpath from
  41  * @param start Number of elements to skip from start of #path
  42  *
  43  * @return Subpath starting with node at position #start
  44  */
  45 static osg::NodePath subPath( const osg::NodePath& path,
  46                               size_t start )
  47 {
  48   if( start >= path.size() )
  49     return osg::NodePath();
  50
  51   osg::NodePath np(path.size() - start);
  52   for(size_t i = start; i < path.size(); ++i)
  53     np[i - start] = path[i];
  54
  55   return np;
  56 }
  57
  58 /**
  59  * Visitor to find a group by its name.
  60  */
  61 class FindGroupVisitor:
  62   public osg::NodeVisitor
  63 {
  64   public:
  65
  66     FindGroupVisitor(const std::string& name):
  67         osg::NodeVisitor(osg::NodeVisitor::TRAVERSE_ALL_CHILDREN),
  68         _name(name),
  69         _group(0)
  70     {
  71       if( name.empty() )
  72         SG_LOG(SG_IO, SG_WARN, "FindGroupVisitor: empty name provided");
  73     }
  74
  75     osg::Group* getGroup() const
  76     {
  77       return _group;
  78     }
  79
  80     virtual void apply(osg::Group& group)
  81     {
  82       if( _name != group.getName() )
  83         return traverse(group);
  84
  85       if( !_group )
  86         _group = &group;
  87       else
  88         SG_LOG
  89         (
  90           SG_IO,
  91           SG_WARN,
  92           "FindGroupVisitor: name not unique '" << _name << "'"
  93         );
  94     }
  95
  96   protected:
  97
  98     std::string _name;
  99     osg::Group *_group;
 100 };
 101
 102 /**
 103  * Get angle of a triangle given by three side lengths
 104  *
 105  * @return Angle enclosed by @c side1 and @c side2
 106  */
 107 double angleFromSSS(double side1, double side2, double opposite)
 108 {
 109   double c = ( SGMiscd::pow<2>(side1)
 110              + SGMiscd::pow<2>(side2)
 111              - SGMiscd::pow<2>(opposite)
 112              ) / (2 * side1 * side2);
 113
 114   if( c <= -1 )
 115     return M_PI;
 116   else if( c >= 1 )
 117     return 0;
 118   else
 119     return std::acos(c);
 120 }
 121
 122 //------------------------------------------------------------------------------
 123 class SGTrackToAnimation::UpdateCallback:
 124   public osg::NodeCallback
 125 {
 126   public:
 127     UpdateCallback( osg::Group* target,
 128                     const SGTrackToAnimation* anim,
 129                     osg::MatrixTransform* slave_tf ):
 130       _target(target),
 131       _slave_tf(slave_tf),
 132       _root_length(0),
 133       _slave_length(0),
 134       _root_initial_angle(0),
 135       _slave_dof(slave_tf ? anim->getConfig()->getIntValue("slave-dof", 1) : 0),
 136       _condition(anim->getCondition()),
 137       _root_disabled_value(
 138         anim->readOffsetValue("disabled-rotation-deg")
 139       ),
 140       _slave_disabled_value(
 141         anim->readOffsetValue("slave-disabled-rotation-deg")
 142       )
 143     {
 144       setName("SGTrackToAnimation::UpdateCallback");
 145
 146       _node_center = toOsg( anim->readVec3("center", "-m") );
 147       _slave_center = toOsg( anim->readVec3("slave-center", "-m") );
 148       _target_center = toOsg( anim->readVec3("target-center", "-m") );
 149       _lock_axis = toOsg( anim->readVec3("lock-axis") );
 150       _track_axis = toOsg( anim->readVec3("track-axis") );
 151
 152       if( _lock_axis.normalize() == 0.0 )
 153       {
 154         anim->log(SG_WARN, "invalid lock-axis");
 155         _lock_axis.set(0, 1, 0);
 156       }
 157
 158       if( _slave_center != osg::Vec3() )
 159       {
 160         _root_length = (_slave_center - _node_center).length();
 161         _slave_length = (_target_center - _slave_center).length();
 162         double dist = (_target_center - _node_center).length();
 163
 164         _root_initial_angle = angleFromSSS(_root_length, dist, _slave_length);
 165
 166         // If no rotation should be applied to the slave element it is looking
 167         // in the same direction then the root node. Inside the triangle given
 168         // by the root length, slave length and distance from the root node to
 169         // the target node, this equals an angle of 180 degrees.
 170         _slave_initial_angle = angleFromSSS(_root_length, _slave_length, dist)
 171                              - SGMiscd::pi();
 172
 173         _track_axis = _target_center - _node_center;
 174       }
 175
 176       for(;;)
 177       {
 178         float proj = _lock_axis * _track_axis;
 179         if( proj != 0.0 )
 180         {
 181           anim->log(SG_WARN, "track-axis not perpendicular to lock-axis");
 182
 183           // Make tracking axis perpendicular to locked axis
 184           _track_axis -= _lock_axis * proj;
 185         }
 186
 187         if( _track_axis.normalize() == 0.0 )
 188         {
 189           anim->log(SG_WARN, "invalid track-axis");
 190           if( std::fabs(_lock_axis.x()) < 0.1 )
 191             _track_axis.set(1, 0, 0);
 192           else
 193             _track_axis.set(0, 1, 0);
 194         }
 195         else
 196           break;
 197       }
 198
 199       _up_axis = _lock_axis ^ _track_axis;
 200       _slave_axis2 = (_target_center - _slave_center) ^ _lock_axis;
 201       _slave_axis2.normalize();
 202
 203       double target_offset = _lock_axis * (_target_center - _node_center);
 204       _slave_initial_angle2 = std::asin(target_offset / _slave_length);
 205     }
 206
 207     virtual void operator()(osg::Node* node, osg::NodeVisitor* nv)
 208     {
 209       // TODO prevent invalid animation from being added?
 210
 211       SGRotateTransform* tf = static_cast<SGRotateTransform*>(node);
 212
 213       // We need to wait with this initialization steps until the first update
 214       // as this allows us to be sure all animations have already been installed
 215       // and are therefore also accounted for calculating the animation.
 216       if( _target )
 217       {
 218         // Get path to animated node and calculated simplified paths to the
 219         // nearest common parent node of both the animated node and the target
 220         // node
 221
 222                                   // start at parent to not get false results by
 223                                   // also including animation transformation
 224         osg::NodePath node_path = requireNodePath(node->getParent(0)),
 225                       target_path = requireNodePath(_target);
 226         size_t tp_size = target_path.size(),
 227                np_size = node_path.size();
 228         size_t common_parents = 0;
 229
 230         for(; common_parents < std::min(tp_size, np_size); ++common_parents)
 231         {
 232           if( target_path[common_parents] != node_path[common_parents] )
 233             break;
 234         }
 235
 236         _node_path = subPath(node_path, common_parents);
 237         _target_path = subPath(target_path, common_parents);
 238         _target = 0;
 239
 240         tf->setCenter( toSG(_node_center) );
 241         tf->setAxis( toSG(_lock_axis) );
 242       }
 243
 244       double root_angle = 0,
 245              slave_angle = 0,
 246              slave_angle2 = 0;
 247
 248       if( !_condition || _condition->test() )
 249       {
 250         root_angle = -_root_initial_angle;
 251         slave_angle = -_slave_initial_angle;
 252
 253         osg::Vec3 target_pos = ( osg::computeLocalToWorld(_target_path)
 254                              * osg::computeWorldToLocal(_node_path)
 255                              ).preMult(_target_center),
 256                   dir = target_pos - _node_center;
 257
 258         // Ensure direction is perpendicular to lock axis
 259         osg::Vec3 lock_proj = _lock_axis * (_lock_axis * dir);
 260         dir -= lock_proj;
 261
 262         // Track to target
 263         float x = dir * _track_axis,
 264               y = dir * _up_axis;
 265         root_angle += std::atan2(y, x);
 266
 267         if( _root_length > 0 )
 268         {
 269           // Handle scissor/ik rotation
 270           double dist = dir.length(),
 271                  slave_target_dist = 0;
 272           if( dist < _root_length + _slave_length )
 273           {
 274             root_angle += angleFromSSS(_root_length, dist, _slave_length);
 275
 276             if( _slave_tf )
 277             {
 278               slave_angle += angleFromSSS(_root_length, _slave_length, dist)
 279                            - SGMiscd::pi();
 280             }
 281             if( _slave_dof >= 2 )
 282               slave_target_dist = _slave_length;
 283           }
 284           else if( _slave_dof >= 2 )
 285           {
 286             // If the distance is too large we need to manually calculate the
 287             // distance of the slave to the target, as it is not possible
 288             // anymore to rotate the objects to reach the target.
 289             osg::Vec3 root_dir = target_pos - _node_center;
 290             root_dir.normalize();
 291             osg::Vec3 slave_pos = _node_center + root_dir * _root_length;
 292             slave_target_dist = (target_pos - slave_pos).length();
 293           }
 294
 295           if( slave_target_dist > 0 )
 296             // Calculate angle to rotate "out of the plane" to point towards the
 297             // target object.
 298             slave_angle2 = std::asin( (_lock_axis * lock_proj)
 299                                     / slave_target_dist )
 300                          - _slave_initial_angle2;
 301         }
 302       }
 303       else
 304       {
 305         root_angle = _root_disabled_value
 306                    ? SGMiscd::deg2rad(_root_disabled_value->getValue())
 307                    : 0;
 308         slave_angle = _slave_disabled_value
 309                     ? SGMiscd::deg2rad(_slave_disabled_value->getValue())
 310                     : 0;
 311       }
 312
 313       tf->setAngleRad( root_angle );
 314       if( _slave_tf )
 315       {
 316         osg::Matrix mat_tf;
 317         SGRotateTransform::set_rotation
 318         (
 319           mat_tf,
 320           slave_angle,
 321           SGVec3d(toSG(_slave_center)),
 322           SGVec3d(toSG(_lock_axis))
 323         );
 324
 325         // Slave rotation around second axis
 326         if( slave_angle2 != 0 )
 327         {
 328           osg::Matrix mat_tf2;
 329           SGRotateTransform::set_rotation
 330           (
 331             mat_tf2,
 332             slave_angle2,
 333             SGVec3d(toSG(_slave_center)),
 334             SGVec3d(toSG(_slave_axis2))
 335           );
 336           mat_tf.preMult(mat_tf2);
 337         }
 338
 339         _slave_tf->setMatrix(mat_tf);
 340       }
 341
 342       traverse(node, nv);
 343     }
 344   protected:
 345
 346     osg::Vec3           _node_center,
 347                         _slave_center,
 348                         _target_center,
 349                         _lock_axis,
 350                         _track_axis,
 351                         _up_axis,
 352                         _slave_axis2; ///!< 2nd axis for 2dof slave
 353     osg::Group         *_target;
 354     osg::MatrixTransform *_slave_tf;
 355     osg::NodePath       _node_path,
 356                         _target_path;
 357     double              _root_length,
 358                         _slave_length,
 359                         _root_initial_angle,
 360                         _slave_initial_angle,
 361                         _slave_initial_angle2;
 362     int                 _slave_dof;
 363
 364     SGSharedPtr<SGCondition const>      _condition;
 365     SGExpressiond_ref   _root_disabled_value,
 366                         _slave_disabled_value;
 367 };
 368
 369 //------------------------------------------------------------------------------
 370 SGTrackToAnimation::SGTrackToAnimation( osg::Node* node,
 371                                         const SGPropertyNode* configNode,
 372                                         SGPropertyNode* modelRoot ):
 373   SGAnimation(configNode, modelRoot),
 374   _target_group(0),
 375   _slave_group(0)
 376 {
 377   std::string target = configNode->getStringValue("target-name");
 378   FindGroupVisitor target_finder(target);
 379   node->accept(target_finder);
 380
 381   if( !(_target_group = target_finder.getGroup()) )
 382     log(SG_ALERT, "target not found: '" + target + '\'');
 383
 384   std::string slave = configNode->getStringValue("slave-name");
 385   if( !slave.empty() )
 386   {
 387     FindGroupVisitor slave_finder(slave);
 388     node->accept(slave_finder);
 389     _slave_group = slave_finder.getGroup();
 390   }
 391 }
 392
 393 //------------------------------------------------------------------------------
 394 osg::Group* SGTrackToAnimation::createAnimationGroup(osg::Group& parent)
 395 {
 396   if( !_target_group )
 397     return 0;
 398
 399   osg::MatrixTransform* slave_tf = 0;
 400   if( _slave_group )
 401   {
 402     slave_tf = new osg::MatrixTransform;
 403     slave_tf->setName("locked-track slave animation");
 404
 405     osg::Group* parent = _slave_group->getParent(0);
 406     slave_tf->addChild(_slave_group);
 407     parent->removeChild(_slave_group);
 408     parent->addChild(slave_tf);
 409   }
 410
 411   SGRotateTransform* tf = new SGRotateTransform;
 412   tf->setName("locked-track animation");
 413   tf->setUpdateCallback(new UpdateCallback(_target_group, this, slave_tf));
 414   parent.addChild(tf);
 415
 416   return tf;
 417 }
 418
 419 //------------------------------------------------------------------------------
 420 void SGTrackToAnimation::log(sgDebugPriority p, const std::string& msg) const
 421 {
 422   SG_LOG
 423   (
 424     SG_IO,
 425     p,
 426     // TODO handle multiple object-names?
 427     "SGTrackToAnimation(" << getConfig()->getStringValue("object-name") << "): "
 428     << msg
 429   );
 430 }