3 // Copyright (C) 2013 Thomas Geymayer <tomgey@gmail.com>
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.
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.
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
19 #include "SGRotateTransform.hxx"
20 #include "SGTrackToAnimation.hxx"
22 #include <simgear/scene/util/OsgMath.hxx>
23 #include <osg/MatrixTransform>
29 static osg::NodePath requireNodePath( osg::Node* node,
30 osg::Node* haltTraversalAtNode = 0 )
32 const osg::NodePathList node_paths =
33 node->getParentalNodePaths(haltTraversalAtNode);
34 return node_paths.at(0);
38 * Get a subpath of an osg::NodePath
40 * @param path Path to extract subpath from
41 * @param start Number of elements to skip from start of #path
43 * @return Subpath starting with node at position #start
45 static osg::NodePath subPath( const osg::NodePath& path,
48 if( start >= path.size() )
49 return osg::NodePath();
51 osg::NodePath np(path.size() - start);
52 for(size_t i = start; i < path.size(); ++i)
53 np[i - start] = path[i];
59 * Visitor to find a group by its name.
61 class FindGroupVisitor:
62 public osg::NodeVisitor
66 FindGroupVisitor(const std::string& name):
67 osg::NodeVisitor(osg::NodeVisitor::TRAVERSE_ALL_CHILDREN),
72 SG_LOG(SG_IO, SG_WARN, "FindGroupVisitor: empty name provided");
75 osg::Group* getGroup() const
80 virtual void apply(osg::Group& group)
82 if( _name != group.getName() )
83 return traverse(group);
88 // Different paths can exists for example with a picking animation (pick
90 else if( _group != &group )
95 "FindGroupVisitor: name not unique '" << _name << "'"
106 * Get angle of a triangle given by three side lengths
108 * @return Angle enclosed by @c side1 and @c side2
110 double angleFromSSS(double side1, double side2, double opposite)
112 double c = ( SGMiscd::pow<2>(side1)
113 + SGMiscd::pow<2>(side2)
114 - SGMiscd::pow<2>(opposite)
115 ) / (2 * side1 * side2);
125 //------------------------------------------------------------------------------
126 class SGTrackToAnimation::UpdateCallback:
127 public osg::NodeCallback
130 UpdateCallback( osg::Group* target,
131 const SGTrackToAnimation* anim,
132 osg::MatrixTransform* slave_tf ):
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")
143 _slave_disabled_value(
144 anim->readOffsetValue("slave-disabled-rotation-deg")
147 setName("SGTrackToAnimation::UpdateCallback");
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") );
155 if( _lock_axis.normalize() == 0.0 )
157 anim->log(SG_WARN, "invalid lock-axis");
158 _lock_axis.set(0, 1, 0);
161 if( _slave_center != osg::Vec3() )
163 _root_length = (_slave_center - _node_center).length();
164 _slave_length = (_target_center - _slave_center).length();
165 double dist = (_target_center - _node_center).length();
167 _root_initial_angle = angleFromSSS(_root_length, dist, _slave_length);
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)
176 _track_axis = _target_center - _node_center;
181 float proj = _lock_axis * _track_axis;
185 // Without slave_dof >= 2, can not rotate out of plane normal to
187 anim->log(SG_WARN, "track-axis not perpendicular to lock-axis");
189 // Make tracking axis perpendicular to locked axis
190 _track_axis -= _lock_axis * proj;
193 if( _track_axis.normalize() == 0.0 )
195 anim->log(SG_WARN, "invalid track-axis");
196 if( std::fabs(_lock_axis.x()) < 0.1 )
197 _track_axis.set(1, 0, 0);
199 _track_axis.set(0, 1, 0);
205 _up_axis = _lock_axis ^ _track_axis;
206 _slave_axis2 = (_target_center - _slave_center) ^ _lock_axis;
207 _slave_axis2.normalize();
209 double target_offset = _lock_axis * (_target_center - _node_center);
210 _slave_initial_angle2 = std::asin(target_offset / _slave_length);
213 virtual void operator()(osg::Node* node, osg::NodeVisitor* nv)
215 // TODO prevent invalid animation from being added?
217 SGRotateTransform* tf = static_cast<SGRotateTransform*>(node);
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.
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
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;
236 for(; common_parents < std::min(tp_size, np_size); ++common_parents)
238 if( target_path[common_parents] != node_path[common_parents] )
242 _node_path = subPath(node_path, common_parents);
243 _target_path = subPath(target_path, common_parents);
246 tf->setCenter( toSG(_node_center) );
247 tf->setAxis( toSG(_lock_axis) );
250 double root_angle = 0,
254 if( !_condition || _condition->test() )
256 root_angle = -_root_initial_angle;
257 slave_angle = -_slave_initial_angle;
259 osg::Vec3 target_pos = ( osg::computeLocalToWorld(_target_path)
260 * osg::computeWorldToLocal(_node_path)
261 ).preMult(_target_center),
262 dir = target_pos - _node_center;
264 // Ensure direction is perpendicular to lock axis
265 osg::Vec3 lock_proj = _lock_axis * (_lock_axis * dir);
269 float x = dir * _track_axis,
271 root_angle += std::atan2(y, x);
273 if( _root_length > 0 )
275 // Handle scissor/ik rotation
276 double dist = dir.length(),
277 slave_target_dist = 0;
278 if( dist < _root_length + _slave_length )
280 root_angle += angleFromSSS(_root_length, dist, _slave_length);
284 slave_angle += angleFromSSS(_root_length, _slave_length, dist)
287 if( _slave_dof >= 2 )
288 slave_target_dist = _slave_length;
290 else if( _slave_dof >= 2 )
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();
301 if( slave_target_dist > 0 )
302 // Calculate angle to rotate "out of the plane" to point towards the
304 slave_angle2 = std::asin( (_lock_axis * lock_proj)
305 / slave_target_dist )
306 - _slave_initial_angle2;
311 root_angle = _root_disabled_value
312 ? SGMiscd::deg2rad(_root_disabled_value->getValue())
314 slave_angle = _slave_disabled_value
315 ? SGMiscd::deg2rad(_slave_disabled_value->getValue())
319 tf->setAngleRad( root_angle );
323 SGRotateTransform::set_rotation
327 SGVec3d(toSG(_slave_center)),
328 SGVec3d(toSG(_lock_axis))
331 // Slave rotation around second axis
332 if( slave_angle2 != 0 )
335 SGRotateTransform::set_rotation
339 SGVec3d(toSG(_slave_center)),
340 SGVec3d(toSG(_slave_axis2))
342 mat_tf.preMult(mat_tf2);
345 _slave_tf->setMatrix(mat_tf);
352 osg::Vec3 _node_center,
358 _slave_axis2; ///!< 2nd axis for 2dof slave
360 osg::MatrixTransform *_slave_tf;
361 osg::NodePath _node_path,
366 _slave_initial_angle,
367 _slave_initial_angle2;
370 SGSharedPtr<SGCondition const> _condition;
371 SGExpressiond_ref _root_disabled_value,
372 _slave_disabled_value;
375 //------------------------------------------------------------------------------
376 SGTrackToAnimation::SGTrackToAnimation( osg::Node* node,
377 const SGPropertyNode* configNode,
378 SGPropertyNode* modelRoot ):
379 SGAnimation(configNode, modelRoot),
383 std::string target = configNode->getStringValue("target-name");
384 FindGroupVisitor target_finder(target);
385 node->accept(target_finder);
387 if( !(_target_group = target_finder.getGroup()) )
388 log(SG_ALERT, "target not found: '" + target + '\'');
390 std::string slave = configNode->getStringValue("slave-name");
393 FindGroupVisitor slave_finder(slave);
394 node->accept(slave_finder);
395 _slave_group = slave_finder.getGroup();
399 //------------------------------------------------------------------------------
400 osg::Group* SGTrackToAnimation::createAnimationGroup(osg::Group& parent)
405 osg::MatrixTransform* slave_tf = 0;
408 slave_tf = new osg::MatrixTransform;
409 slave_tf->setName("locked-track slave animation");
411 osg::Group* parent = _slave_group->getParent(0);
412 slave_tf->addChild(_slave_group);
413 parent->removeChild(_slave_group);
414 parent->addChild(slave_tf);
417 SGRotateTransform* tf = new SGRotateTransform;
418 tf->setName("locked-track animation");
419 tf->setUpdateCallback(new UpdateCallback(_target_group, this, slave_tf));
425 //------------------------------------------------------------------------------
426 void SGTrackToAnimation::log(sgDebugPriority p, const std::string& msg) const
432 // TODO handle multiple object-names?
433 "SGTrackToAnimation(" << getConfig()->getStringValue("object-name") << "): "