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