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
  88       // Different paths can exists for example with a picking animation (pick
  89       // render group)
  90       else if( _group != &group )
  91         SG_LOG
  92         (
  93           SG_IO,
  94           SG_WARN,
  95           "FindGroupVisitor: name not unique '" << _name << "'"
  96         );
  97     }
  98
  99   protected:
 100
 101     std::string _name;
 102     osg::Group *_group;
 103 };
 104
 105 /**
 106  * Get angle of a triangle given by three side lengths
 107  *
 108  * @return Angle enclosed by @c side1 and @c side2
 109  */
 110 double angleFromSSS(double side1, double side2, double opposite)
 111 {
 112   double c = ( SGMiscd::pow<2>(side1)
 113              + SGMiscd::pow<2>(side2)
 114              - SGMiscd::pow<2>(opposite)
 115              ) / (2 * side1 * side2);
 116
 117   if( c <= -1 )
 118     return M_PI;
 119   else if( c >= 1 )
 120     return 0;
 121   else
 122     return std::acos(c);
 123 }
 124
 125 //------------------------------------------------------------------------------
 126 class SGTrackToAnimation::UpdateCallback:
 127   public osg::NodeCallback
 128 {
 129   public:
 130     UpdateCallback( osg::Group* target,
 131                     const SGTrackToAnimation* anim,
 132                     osg::MatrixTransform* slave_tf ):
 133       _target(target),
 134       _slave_tf(slave_tf),
 135       _root_length(0),
 136       _slave_length(0),
 137       _root_initial_angle(0),
 138       _slave_dof(slave_tf ? anim->getConfig()->getIntValue("slave-dof", 1) : 0),
 139       _condition(anim->getCondition()),
 140       _root_disabled_value(
 141         anim->readOffsetValue("disabled-rotation-deg")
 142       ),
 143       _slave_disabled_value(
 144         anim->readOffsetValue("slave-disabled-rotation-deg")
 145       )
 146     {
 147       setName("SGTrackToAnimation::UpdateCallback");
 148
 149       _node_center = toOsg( anim->readVec3("center", "-m") );
 150       _slave_center = toOsg( anim->readVec3("slave-center", "-m") );
 151       _target_center = toOsg( anim->readVec3("target-center", "-m") );
 152       _lock_axis = toOsg( anim->readVec3("lock-axis") );
 153       _track_axis = toOsg( anim->readVec3("track-axis") );
 154
 155       if( _lock_axis.normalize() == 0.0 )
 156       {
 157         anim->log(SG_WARN, "invalid lock-axis");
 158         _lock_axis.set(0, 1, 0);
 159       }
 160
 161       if( _slave_center != osg::Vec3() )
 162       {
 163         _root_length = (_slave_center - _node_center).length();
 164         _slave_length = (_target_center - _slave_center).length();
 165         double dist = (_target_center - _node_center).length();
 166
 167         _root_initial_angle = angleFromSSS(_root_length, dist, _slave_length);
 168
 169         // If no rotation should be applied to the slave element it is looking
 170         // in the same direction then the root node. Inside the triangle given
 171         // by the root length, slave length and distance from the root node to
 172         // the target node, this equals an angle of 180 degrees.
 173         _slave_initial_angle = angleFromSSS(_root_length, _slave_length, dist)
 174                              - SGMiscd::pi();
 175
 176         _track_axis = _target_center - _node_center;
 177       }
 178
 179       for(;;)
 180       {
 181         float proj = _lock_axis * _track_axis;
 182         if( proj != 0.0 )
 183         {
 184           if( _slave_dof < 2 )
 185             // Without slave_dof >= 2, can not rotate out of plane normal to
 186             // lock axis.
 187             anim->log(SG_WARN, "track-axis not perpendicular to lock-axis");
 188
 189           // Make tracking axis perpendicular to locked axis
 190           _track_axis -= _lock_axis * proj;
 191         }
 192
 193         if( _track_axis.normalize() == 0.0 )
 194         {
 195           anim->log(SG_WARN, "invalid track-axis");
 196           if( std::fabs(_lock_axis.x()) < 0.1 )
 197             _track_axis.set(1, 0, 0);
 198           else
 199             _track_axis.set(0, 1, 0);
 200         }
 201         else
 202           break;
 203       }
 204
 205       _up_axis = _lock_axis ^ _track_axis;
 206       _slave_axis2 = (_target_center - _slave_center) ^ _lock_axis;
 207       _slave_axis2.normalize();
 208
 209       double target_offset = _lock_axis * (_target_center - _node_center);
 210       _slave_initial_angle2 = std::asin(target_offset / _slave_length);
 211     }
 212
 213     virtual void operator()(osg::Node* node, osg::NodeVisitor* nv)
 214     {
 215       // TODO prevent invalid animation from being added?
 216
 217       SGRotateTransform* tf = static_cast<SGRotateTransform*>(node);
 218
 219       // We need to wait with this initialization steps until the first update
 220       // as this allows us to be sure all animations have already been installed
 221       // and are therefore also accounted for calculating the animation.
 222       if( _target )
 223       {
 224         // Get path to animated node and calculated simplified paths to the
 225         // nearest common parent node of both the animated node and the target
 226         // node
 227
 228                                   // start at parent to not get false results by
 229                                   // also including animation transformation
 230         osg::NodePath node_path = requireNodePath(node->getParent(0)),
 231                       target_path = requireNodePath(_target);
 232         size_t tp_size = target_path.size(),
 233                np_size = node_path.size();
 234         size_t common_parents = 0;
 235
 236         for(; common_parents < std::min(tp_size, np_size); ++common_parents)
 237         {
 238           if( target_path[common_parents] != node_path[common_parents] )
 239             break;
 240         }
 241
 242         _node_path = subPath(node_path, common_parents);
 243         _target_path = subPath(target_path, common_parents);
 244         _target = 0;
 245
 246         tf->setCenter( toSG(_node_center) );
 247         tf->setAxis( toSG(_lock_axis) );
 248       }
 249
 250       double root_angle = 0,
 251              slave_angle = 0,
 252              slave_angle2 = 0;
 253
 254       if( !_condition || _condition->test() )
 255       {
 256         root_angle = -_root_initial_angle;
 257         slave_angle = -_slave_initial_angle;
 258
 259         osg::Vec3 target_pos = ( osg::computeLocalToWorld(_target_path)
 260                              * osg::computeWorldToLocal(_node_path)
 261                              ).preMult(_target_center),
 262                   dir = target_pos - _node_center;
 263
 264         // Ensure direction is perpendicular to lock axis
 265         osg::Vec3 lock_proj = _lock_axis * (_lock_axis * dir);
 266         dir -= lock_proj;
 267
 268         // Track to target
 269         float x = dir * _track_axis,
 270               y = dir * _up_axis;
 271         root_angle += std::atan2(y, x);
 272
 273         if( _root_length > 0 )
 274         {
 275           // Handle scissor/ik rotation
 276           double dist = dir.length(),
 277                  slave_target_dist = 0;
 278           if( dist < _root_length + _slave_length )
 279           {
 280             root_angle += angleFromSSS(_root_length, dist, _slave_length);
 281
 282             if( _slave_tf )
 283             {
 284               slave_angle += angleFromSSS(_root_length, _slave_length, dist)
 285                            - SGMiscd::pi();
 286             }
 287             if( _slave_dof >= 2 )
 288               slave_target_dist = _slave_length;
 289           }
 290           else if( _slave_dof >= 2 )
 291           {
 292             // If the distance is too large we need to manually calculate the
 293             // distance of the slave to the target, as it is not possible
 294             // anymore to rotate the objects to reach the target.
 295             osg::Vec3 root_dir = target_pos - _node_center;
 296             root_dir.normalize();
 297             osg::Vec3 slave_pos = _node_center + root_dir * _root_length;
 298             slave_target_dist = (target_pos - slave_pos).length();
 299           }
 300
 301           if( slave_target_dist > 0 )
 302             // Calculate angle to rotate "out of the plane" to point towards the
 303             // target object.
 304             slave_angle2 = std::asin( (_lock_axis * lock_proj)
 305                                     / slave_target_dist )
 306                          - _slave_initial_angle2;
 307         }
 308       }
 309       else
 310       {
 311         root_angle = _root_disabled_value
 312                    ? SGMiscd::deg2rad(_root_disabled_value->getValue())
 313                    : 0;
 314         slave_angle = _slave_disabled_value
 315                     ? SGMiscd::deg2rad(_slave_disabled_value->getValue())
 316                     : 0;
 317       }
 318
 319       tf->setAngleRad( root_angle );
 320       if( _slave_tf )
 321       {
 322         osg::Matrix mat_tf;
 323         SGRotateTransform::set_rotation
 324         (
 325           mat_tf,
 326           slave_angle,
 327           SGVec3d(toSG(_slave_center)),
 328           SGVec3d(toSG(_lock_axis))
 329         );
 330
 331         // Slave rotation around second axis
 332         if( slave_angle2 != 0 )
 333         {
 334           osg::Matrix mat_tf2;
 335           SGRotateTransform::set_rotation
 336           (
 337             mat_tf2,
 338             slave_angle2,
 339             SGVec3d(toSG(_slave_center)),
 340             SGVec3d(toSG(_slave_axis2))
 341           );
 342           mat_tf.preMult(mat_tf2);
 343         }
 344
 345         _slave_tf->setMatrix(mat_tf);
 346       }
 347
 348       traverse(node, nv);
 349     }
 350   protected:
 351
 352     osg::Vec3           _node_center,
 353                         _slave_center,
 354                         _target_center,
 355                         _lock_axis,
 356                         _track_axis,
 357                         _up_axis,
 358                         _slave_axis2; ///!< 2nd axis for 2dof slave
 359     osg::Group         *_target;
 360     osg::MatrixTransform *_slave_tf;
 361     osg::NodePath       _node_path,
 362                         _target_path;
 363     double              _root_length,
 364                         _slave_length,
 365                         _root_initial_angle,
 366                         _slave_initial_angle,
 367                         _slave_initial_angle2;
 368     int                 _slave_dof;
 369
 370     SGSharedPtr<SGCondition const>      _condition;
 371     SGExpressiond_ref   _root_disabled_value,
 372                         _slave_disabled_value;
 373 };
 374
 375 //------------------------------------------------------------------------------
 376 SGTrackToAnimation::SGTrackToAnimation( osg::Node* node,
 377                                         const SGPropertyNode* configNode,
 378                                         SGPropertyNode* modelRoot ):
 379   SGAnimation(configNode, modelRoot),
 380   _target_group(0),
 381   _slave_group(0)
 382 {
 383   std::string target = configNode->getStringValue("target-name");
 384   FindGroupVisitor target_finder(target);
 385   node->accept(target_finder);
 386
 387   if( !(_target_group = target_finder.getGroup()) )
 388     log(SG_ALERT, "target not found: '" + target + '\'');
 389
 390   std::string slave = configNode->getStringValue("slave-name");
 391   if( !slave.empty() )
 392   {
 393     FindGroupVisitor slave_finder(slave);
 394     node->accept(slave_finder);
 395     _slave_group = slave_finder.getGroup();
 396   }
 397 }
 398
 399 //------------------------------------------------------------------------------
 400 osg::Group* SGTrackToAnimation::createAnimationGroup(osg::Group& parent)
 401 {
 402   if( !_target_group )
 403     return 0;
 404
 405   osg::MatrixTransform* slave_tf = 0;
 406   if( _slave_group )
 407   {
 408     slave_tf = new osg::MatrixTransform;
 409     slave_tf->setName("locked-track slave animation");
 410
 411     osg::Group* parent = _slave_group->getParent(0);
 412     slave_tf->addChild(_slave_group);
 413     parent->removeChild(_slave_group);
 414     parent->addChild(slave_tf);
 415   }
 416
 417   SGRotateTransform* tf = new SGRotateTransform;
 418   tf->setName("locked-track animation");
 419   tf->setUpdateCallback(new UpdateCallback(_target_group, this, slave_tf));
 420   parent.addChild(tf);
 421
 422   return tf;
 423 }
 424
 425 //------------------------------------------------------------------------------
 426 void SGTrackToAnimation::log(sgDebugPriority p, const std::string& msg) const
 427 {
 428   SG_LOG
 429   (
 430     SG_IO,
 431     p,
 432     // TODO handle multiple object-names?
 433     "SGTrackToAnimation(" << getConfig()->getStringValue("object-name") << "): "
 434     << msg
 435   );
 436 }