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);
92 "FindGroupVisitor: name not unique '" << _name << "'"
103 * Get angle of a triangle given by three side lengths
105 * @return Angle enclosed by @c side1 and @c side2
107 double angleFromSSS(double side1, double side2, double opposite)
109 double c = ( SGMiscd::pow<2>(side1)
110 + SGMiscd::pow<2>(side2)
111 - SGMiscd::pow<2>(opposite)
112 ) / (2 * side1 * side2);
122 //------------------------------------------------------------------------------
123 class SGTrackToAnimation::UpdateCallback:
124 public osg::NodeCallback
127 UpdateCallback( osg::Group* target,
128 const SGTrackToAnimation* anim,
129 osg::MatrixTransform* slave_tf ):
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")
140 _slave_disabled_value(
141 anim->readOffsetValue("slave-disabled-rotation-deg")
144 setName("SGTrackToAnimation::UpdateCallback");
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") );
152 if( _lock_axis.normalize() == 0.0 )
154 anim->log(SG_WARN, "invalid lock-axis");
155 _lock_axis.set(0, 1, 0);
158 if( _slave_center != osg::Vec3() )
160 _root_length = (_slave_center - _node_center).length();
161 _slave_length = (_target_center - _slave_center).length();
162 double dist = (_target_center - _node_center).length();
164 _root_initial_angle = angleFromSSS(_root_length, dist, _slave_length);
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)
173 _track_axis = _target_center - _node_center;
178 float proj = _lock_axis * _track_axis;
181 anim->log(SG_WARN, "track-axis not perpendicular to lock-axis");
183 // Make tracking axis perpendicular to locked axis
184 _track_axis -= _lock_axis * proj;
187 if( _track_axis.normalize() == 0.0 )
189 anim->log(SG_WARN, "invalid track-axis");
190 if( std::fabs(_lock_axis.x()) < 0.1 )
191 _track_axis.set(1, 0, 0);
193 _track_axis.set(0, 1, 0);
199 _up_axis = _lock_axis ^ _track_axis;
200 _slave_axis2 = (_target_center - _slave_center) ^ _lock_axis;
201 _slave_axis2.normalize();
203 double target_offset = _lock_axis * (_target_center - _node_center);
204 _slave_initial_angle2 = std::asin(target_offset / _slave_length);
207 virtual void operator()(osg::Node* node, osg::NodeVisitor* nv)
209 if( _root_length <= 0 )
210 // TODO prevent invalid animation from being added?
213 SGRotateTransform* tf = static_cast<SGRotateTransform*>(node);
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.
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
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;
232 for(; common_parents < std::min(tp_size, np_size); ++common_parents)
234 if( target_path[common_parents] != node_path[common_parents] )
238 _node_path = subPath(node_path, common_parents);
239 _target_path = subPath(target_path, common_parents);
242 tf->setCenter( toSG(_node_center) );
243 tf->setAxis( toSG(_lock_axis) );
246 double root_angle = 0,
250 if( !_condition || _condition->test() )
252 root_angle = -_root_initial_angle;
253 slave_angle = -_slave_initial_angle;
255 osg::Vec3 target_pos = ( osg::computeLocalToWorld(_target_path)
256 * osg::computeWorldToLocal(_node_path)
257 ).preMult(_target_center),
258 dir = target_pos - _node_center;
260 // Ensure direction is perpendicular to lock axis
261 osg::Vec3 lock_proj = _lock_axis * (_lock_axis * dir);
265 float x = dir * _track_axis,
267 root_angle += std::atan2(y, x);
269 // Handle scissor/ik rotation
270 double dist = dir.length(),
271 slave_target_dist = 0;
272 if( dist < _root_length + _slave_length )
274 root_angle += angleFromSSS(_root_length, dist, _slave_length);
278 slave_angle += angleFromSSS(_root_length, _slave_length, dist)
281 if( _slave_dof >= 2 )
282 slave_target_dist = _slave_length;
284 else if( _slave_dof >= 2 )
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();
295 if( slave_target_dist > 0 )
296 // Calculate angle to rotate "out of the plane" to point towards the
298 slave_angle2 = std::asin((_lock_axis * lock_proj) / slave_target_dist)
299 - _slave_initial_angle2;
303 root_angle = _root_disabled_value
304 ? SGMiscd::deg2rad(_root_disabled_value->getValue())
306 slave_angle = _slave_disabled_value
307 ? SGMiscd::deg2rad(_slave_disabled_value->getValue())
311 tf->setAngleRad( root_angle );
315 SGRotateTransform::set_rotation
319 SGVec3d(toSG(_slave_center)),
320 SGVec3d(toSG(_lock_axis))
323 // Slave rotation around second axis
324 if( slave_angle2 != 0 )
327 SGRotateTransform::set_rotation
331 SGVec3d(toSG(_slave_center)),
332 SGVec3d(toSG(_slave_axis2))
334 mat_tf.preMult(mat_tf2);
337 _slave_tf->setMatrix(mat_tf);
344 osg::Vec3 _node_center,
350 _slave_axis2; ///!< 2nd axis for 2dof slave
352 osg::MatrixTransform *_slave_tf;
353 osg::NodePath _node_path,
358 _slave_initial_angle,
359 _slave_initial_angle2;
362 SGSharedPtr<SGCondition const> _condition;
363 SGExpressiond_ref _root_disabled_value,
364 _slave_disabled_value;
367 //------------------------------------------------------------------------------
368 SGTrackToAnimation::SGTrackToAnimation( osg::Node* node,
369 const SGPropertyNode* configNode,
370 SGPropertyNode* modelRoot ):
371 SGAnimation(configNode, modelRoot),
375 std::string target = configNode->getStringValue("target-name");
376 FindGroupVisitor target_finder(target);
377 node->accept(target_finder);
379 if( !(_target_group = target_finder.getGroup()) )
380 log(SG_ALERT, "target not found: '" + target + '\'');
382 std::string slave = configNode->getStringValue("slave-name");
385 FindGroupVisitor slave_finder(slave);
386 node->accept(slave_finder);
387 _slave_group = slave_finder.getGroup();
391 //------------------------------------------------------------------------------
392 osg::Group* SGTrackToAnimation::createAnimationGroup(osg::Group& parent)
397 osg::MatrixTransform* slave_tf = 0;
400 slave_tf = new osg::MatrixTransform;
401 slave_tf->setName("locked-track slave animation");
403 osg::Group* parent = _slave_group->getParent(0);
404 slave_tf->addChild(_slave_group);
405 parent->removeChild(_slave_group);
406 parent->addChild(slave_tf);
409 SGRotateTransform* tf = new SGRotateTransform;
410 tf->setName("locked-track animation");
411 tf->setUpdateCallback(new UpdateCallback(_target_group, this, slave_tf));
417 //------------------------------------------------------------------------------
418 void SGTrackToAnimation::log(sgDebugPriority p, const std::string& msg) const
424 // TODO handle multiple object-names?
425 "SGTrackToAnimation(" << getConfig()->getStringValue("object-name") << "): "