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       if( _root_length <= 0 )
 210         // TODO prevent invalid animation from being added?
 211         return;
 212
 213       SGRotateTransform* tf = static_cast<SGRotateTransform*>(node);
 214
 215       // We need to wait with this initialization steps until the first update
 216       // as this allows us to be sure all animations have already been installed
 217       // and are therefore also accounted for calculating the animation.
 218       if( _target )
 219       {
 220         // Get path to animated node and calculated simplified paths to the
 221         // nearest common parent node of both the animated node and the target
 222         // node
 223
 224                                   // start at parent to not get false results by
 225                                   // also including animation transformation
 226         osg::NodePath node_path = requireNodePath(node->getParent(0)),
 227                       target_path = requireNodePath(_target);
 228         size_t tp_size = target_path.size(),
 229                np_size = node_path.size();
 230         size_t common_parents = 0;
 231
 232         for(; common_parents < std::min(tp_size, np_size); ++common_parents)
 233         {
 234           if( target_path[common_parents] != node_path[common_parents] )
 235             break;
 236         }
 237
 238         _node_path = subPath(node_path, common_parents);
 239         _target_path = subPath(target_path, common_parents);
 240         _target = 0;
 241
 242         tf->setCenter( toSG(_node_center) );
 243         tf->setAxis( toSG(_lock_axis) );
 244       }
 245
 246       double root_angle = 0,
 247              slave_angle = 0,
 248              slave_angle2 = 0;
 249
 250       if( !_condition || _condition->test() )
 251       {
 252         root_angle = -_root_initial_angle;
 253         slave_angle = -_slave_initial_angle;
 254
 255         osg::Vec3 target_pos = ( osg::computeLocalToWorld(_target_path)
 256                              * osg::computeWorldToLocal(_node_path)
 257                              ).preMult(_target_center),
 258                   dir = target_pos - _node_center;
 259
 260         // Ensure direction is perpendicular to lock axis
 261         osg::Vec3 lock_proj = _lock_axis * (_lock_axis * dir);
 262         dir -= lock_proj;
 263
 264         // Track to target
 265         float x = dir * _track_axis,
 266               y = dir * _up_axis;
 267         root_angle += std::atan2(y, x);
 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 anymore
 288           // 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) / slave_target_dist)
 299                        - _slave_initial_angle2;
 300       }
 301       else
 302       {
 303         root_angle = _root_disabled_value
 304                    ? SGMiscd::deg2rad(_root_disabled_value->getValue())
 305                    : 0;
 306         slave_angle = _slave_disabled_value
 307                     ? SGMiscd::deg2rad(_slave_disabled_value->getValue())
 308                     : 0;
 309       }
 310
 311       tf->setAngleRad( root_angle );
 312       if( _slave_tf )
 313       {
 314         osg::Matrix mat_tf;
 315         SGRotateTransform::set_rotation
 316         (
 317           mat_tf,
 318           slave_angle,
 319           SGVec3d(toSG(_slave_center)),
 320           SGVec3d(toSG(_lock_axis))
 321         );
 322
 323         // Slave rotation around second axis
 324         if( slave_angle2 != 0 )
 325         {
 326           osg::Matrix mat_tf2;
 327           SGRotateTransform::set_rotation
 328           (
 329             mat_tf2,
 330             slave_angle2,
 331             SGVec3d(toSG(_slave_center)),
 332             SGVec3d(toSG(_slave_axis2))
 333           );
 334           mat_tf.preMult(mat_tf2);
 335         }
 336
 337         _slave_tf->setMatrix(mat_tf);
 338       }
 339
 340       traverse(node, nv);
 341     }
 342   protected:
 343
 344     osg::Vec3           _node_center,
 345                         _slave_center,
 346                         _target_center,
 347                         _lock_axis,
 348                         _track_axis,
 349                         _up_axis,
 350                         _slave_axis2; ///!< 2nd axis for 2dof slave
 351     osg::Group         *_target;
 352     osg::MatrixTransform *_slave_tf;
 353     osg::NodePath       _node_path,
 354                         _target_path;
 355     double              _root_length,
 356                         _slave_length,
 357                         _root_initial_angle,
 358                         _slave_initial_angle,
 359                         _slave_initial_angle2;
 360     int                 _slave_dof;
 361
 362     SGSharedPtr<SGCondition const>      _condition;
 363     SGExpressiond_ref   _root_disabled_value,
 364                         _slave_disabled_value;
 365 };
 366
 367 //------------------------------------------------------------------------------
 368 SGTrackToAnimation::SGTrackToAnimation( osg::Node* node,
 369                                         const SGPropertyNode* configNode,
 370                                         SGPropertyNode* modelRoot ):
 371   SGAnimation(configNode, modelRoot),
 372   _target_group(0),
 373   _slave_group(0)
 374 {
 375   std::string target = configNode->getStringValue("target-name");
 376   FindGroupVisitor target_finder(target);
 377   node->accept(target_finder);
 378
 379   if( !(_target_group = target_finder.getGroup()) )
 380     log(SG_ALERT, "target not found: '" + target + '\'');
 381
 382   std::string slave = configNode->getStringValue("slave-name");
 383   if( !slave.empty() )
 384   {
 385     FindGroupVisitor slave_finder(slave);
 386     node->accept(slave_finder);
 387     _slave_group = slave_finder.getGroup();
 388   }
 389 }
 390
 391 //------------------------------------------------------------------------------
 392 osg::Group* SGTrackToAnimation::createAnimationGroup(osg::Group& parent)
 393 {
 394   if( !_target_group )
 395     return 0;
 396
 397   osg::MatrixTransform* slave_tf = 0;
 398   if( _slave_group )
 399   {
 400     slave_tf = new osg::MatrixTransform;
 401     slave_tf->setName("locked-track slave animation");
 402
 403     osg::Group* parent = _slave_group->getParent(0);
 404     slave_tf->addChild(_slave_group);
 405     parent->removeChild(_slave_group);
 406     parent->addChild(slave_tf);
 407   }
 408
 409   SGRotateTransform* tf = new SGRotateTransform;
 410   tf->setName("locked-track animation");
 411   tf->setUpdateCallback(new UpdateCallback(_target_group, this, slave_tf));
 412   parent.addChild(tf);
 413
 414   return tf;
 415 }
 416
 417 //------------------------------------------------------------------------------
 418 void SGTrackToAnimation::log(sgDebugPriority p, const std::string& msg) const
 419 {
 420   SG_LOG
 421   (
 422     SG_IO,
 423     p,
 424     // TODO handle multiple object-names?
 425     "SGTrackToAnimation(" << getConfig()->getStringValue("object-name") << "): "
 426     << msg
 427   );
 428 }