#include "SGTrackToAnimation.hxx"
#include <simgear/scene/util/OsgMath.hxx>
-#include <osg/Transform>
+#include <osg/MatrixTransform>
#include <cassert>
/**
if( !_group )
_group = &group;
else
- SG_LOG(SG_IO, SG_WARN, "FindGroupVisitor: name not unique");
+ SG_LOG
+ (
+ SG_IO,
+ SG_WARN,
+ "FindGroupVisitor: name not unique '" << _name << "'"
+ );
}
protected:
osg::Group *_group;
};
+/**
+ * Get angle of a triangle given by three side lengths
+ *
+ * @return Angle enclosed by @c side1 and @c side2
+ */
+double angleFromSSS(double side1, double side2, double opposite)
+{
+ double c = ( SGMiscd::pow<2>(side1)
+ + SGMiscd::pow<2>(side2)
+ - SGMiscd::pow<2>(opposite)
+ ) / (2 * side1 * side2);
+
+ if( c <= -1 )
+ return M_PI;
+ else if( c >= 1 )
+ return 0;
+ else
+ return std::acos(c);
+}
+
//------------------------------------------------------------------------------
class SGTrackToAnimation::UpdateCallback:
public osg::NodeCallback
{
public:
UpdateCallback( osg::Group* target,
- const SGTrackToAnimation* anim ):
- _target(target)
+ const SGTrackToAnimation* anim,
+ osg::MatrixTransform* slave_tf ):
+ _target(target),
+ _slave_tf(slave_tf),
+ _root_length(0),
+ _slave_length(0),
+ _root_initial_angle(0),
+ _slave_dof(slave_tf ? anim->getConfig()->getIntValue("slave-dof", 1) : 0),
+ _condition(anim->getCondition()),
+ _root_disabled_value(
+ anim->readOffsetValue("disabled-rotation-deg")
+ ),
+ _slave_disabled_value(
+ anim->readOffsetValue("slave-disabled-rotation-deg")
+ )
{
setName("SGTrackToAnimation::UpdateCallback");
_node_center = toOsg( anim->readVec3("center", "-m") );
+ _slave_center = toOsg( anim->readVec3("slave-center", "-m") );
_target_center = toOsg( anim->readVec3("target-center", "-m") );
_lock_axis = toOsg( anim->readVec3("lock-axis") );
_track_axis = toOsg( anim->readVec3("track-axis") );
_lock_axis.set(0, 1, 0);
}
+ if( _slave_center != osg::Vec3() )
+ {
+ _root_length = (_slave_center - _node_center).length();
+ _slave_length = (_target_center - _slave_center).length();
+ double dist = (_target_center - _node_center).length();
+
+ _root_initial_angle = angleFromSSS(_root_length, dist, _slave_length);
+
+ // If no rotation should be applied to the slave element it is looking
+ // in the same direction then the root node. Inside the triangle given
+ // by the root length, slave length and distance from the root node to
+ // the target node, this equals an angle of 180 degrees.
+ _slave_initial_angle = angleFromSSS(_root_length, _slave_length, dist)
+ - SGMiscd::pi();
+
+ _track_axis = _target_center - _node_center;
+ }
+
for(;;)
{
float proj = _lock_axis * _track_axis;
}
_up_axis = _lock_axis ^ _track_axis;
+ _slave_axis2 = (_target_center - _slave_center) ^ _lock_axis;
+ _slave_axis2.normalize();
+
+ double target_offset = _lock_axis * (_target_center - _node_center);
+ _slave_initial_angle2 = std::asin(target_offset / _slave_length);
}
virtual void operator()(osg::Node* node, osg::NodeVisitor* nv)
{
+ if( _root_length <= 0 )
+ // TODO prevent invalid animation from being added?
+ return;
+
SGRotateTransform* tf = static_cast<SGRotateTransform*>(node);
+ // We need to wait with this initialization steps until the first update
+ // as this allows us to be sure all animations have already been installed
+ // and are therefore also accounted for calculating the animation.
if( _target )
{
// Get path to animated node and calculated simplified paths to the
tf->setAxis( toSG(_lock_axis) );
}
- osg::Vec3 target_pos = ( osg::computeLocalToWorld(_target_path)
+ double root_angle = 0,
+ slave_angle = 0,
+ slave_angle2 = 0;
+
+ if( !_condition || _condition->test() )
+ {
+ root_angle = -_root_initial_angle;
+ slave_angle = -_slave_initial_angle;
+
+ osg::Vec3 target_pos = ( osg::computeLocalToWorld(_target_path)
* osg::computeWorldToLocal(_node_path)
).preMult(_target_center),
- dir = target_pos - _node_center;
+ dir = target_pos - _node_center;
+
+ // Ensure direction is perpendicular to lock axis
+ osg::Vec3 lock_proj = _lock_axis * (_lock_axis * dir);
+ dir -= lock_proj;
+
+ // Track to target
+ float x = dir * _track_axis,
+ y = dir * _up_axis;
+ root_angle += std::atan2(y, x);
- // Ensure direction is perpendicular to lock axis
- float proj = _lock_axis * dir;
- if( proj != 0.0 )
- dir -= _lock_axis * proj;
+ // Handle scissor/ik rotation
+ double dist = dir.length(),
+ slave_target_dist = 0;
+ if( dist < _root_length + _slave_length )
+ {
+ root_angle += angleFromSSS(_root_length, dist, _slave_length);
+
+ if( _slave_tf )
+ {
+ slave_angle += angleFromSSS(_root_length, _slave_length, dist)
+ - SGMiscd::pi();
+ }
+ if( _slave_dof >= 2 )
+ slave_target_dist = _slave_length;
+ }
+ else if( _slave_dof >= 2 )
+ {
+ // If the distance is too large we need to manually calculate the
+ // distance of the slave to the target, as it is not possible anymore
+ // to rotate the objects to reach the target.
+ osg::Vec3 root_dir = target_pos - _node_center;
+ root_dir.normalize();
+ osg::Vec3 slave_pos = _node_center + root_dir * _root_length;
+ slave_target_dist = (target_pos - slave_pos).length();
+ }
+
+ if( slave_target_dist > 0 )
+ // Calculate angle to rotate "out of the plane" to point towards the
+ // target object.
+ slave_angle2 = std::asin((_lock_axis * lock_proj) / slave_target_dist)
+ - _slave_initial_angle2;
+ }
+ else
+ {
+ root_angle = _root_disabled_value
+ ? SGMiscd::deg2rad(_root_disabled_value->getValue())
+ : 0;
+ slave_angle = _slave_disabled_value
+ ? SGMiscd::deg2rad(_slave_disabled_value->getValue())
+ : 0;
+ }
+
+ tf->setAngleRad( root_angle );
+ if( _slave_tf )
+ {
+ osg::Matrix mat_tf;
+ SGRotateTransform::set_rotation
+ (
+ mat_tf,
+ slave_angle,
+ SGVec3d(toSG(_slave_center)),
+ SGVec3d(toSG(_lock_axis))
+ );
+
+ // Slave rotation around second axis
+ if( slave_angle2 != 0 )
+ {
+ osg::Matrix mat_tf2;
+ SGRotateTransform::set_rotation
+ (
+ mat_tf2,
+ slave_angle2,
+ SGVec3d(toSG(_slave_center)),
+ SGVec3d(toSG(_slave_axis2))
+ );
+ mat_tf.preMult(mat_tf2);
+ }
- float x = dir * _track_axis,
- y = dir * _up_axis;
- tf->setAngleRad( std::atan2(y, x) );
+ _slave_tf->setMatrix(mat_tf);
+ }
traverse(node, nv);
}
protected:
- osg::Vec3 _node_center,
- _target_center,
- _lock_axis,
- _track_axis,
- _up_axis;
- osg::Group *_target;
- osg::NodePath _node_path,
- _target_path;
+
+ osg::Vec3 _node_center,
+ _slave_center,
+ _target_center,
+ _lock_axis,
+ _track_axis,
+ _up_axis,
+ _slave_axis2; ///!< 2nd axis for 2dof slave
+ osg::Group *_target;
+ osg::MatrixTransform *_slave_tf;
+ osg::NodePath _node_path,
+ _target_path;
+ double _root_length,
+ _slave_length,
+ _root_initial_angle,
+ _slave_initial_angle,
+ _slave_initial_angle2;
+ int _slave_dof;
+
+ SGSharedPtr<SGCondition const> _condition;
+ SGExpressiond_ref _root_disabled_value,
+ _slave_disabled_value;
};
//------------------------------------------------------------------------------
const SGPropertyNode* configNode,
SGPropertyNode* modelRoot ):
SGAnimation(configNode, modelRoot),
- _target_group(0)
+ _target_group(0),
+ _slave_group(0)
{
std::string target = configNode->getStringValue("target-name");
- std::cout << "track " << target << std::endl;
- configNode->printOn(std::cout);
-
FindGroupVisitor target_finder(target);
node->accept(target_finder);
if( !(_target_group = target_finder.getGroup()) )
log(SG_ALERT, "target not found: '" + target + '\'');
+
+ std::string slave = configNode->getStringValue("slave-name");
+ if( !slave.empty() )
+ {
+ FindGroupVisitor slave_finder(slave);
+ node->accept(slave_finder);
+ _slave_group = slave_finder.getGroup();
+ }
}
//------------------------------------------------------------------------------
if( !_target_group )
return 0;
- SGRotateTransform* transform = new SGRotateTransform;
- transform->setName("locked-track animation");
- transform->setUpdateCallback(new UpdateCallback(_target_group, this));
- parent.addChild(transform);
- return transform;
+ osg::MatrixTransform* slave_tf = 0;
+ if( _slave_group )
+ {
+ slave_tf = new osg::MatrixTransform;
+ slave_tf->setName("locked-track slave animation");
+
+ osg::Group* parent = _slave_group->getParent(0);
+ slave_tf->addChild(_slave_group);
+ parent->removeChild(_slave_group);
+ parent->addChild(slave_tf);
+ }
+
+ SGRotateTransform* tf = new SGRotateTransform;
+ tf->setName("locked-track animation");
+ tf->setUpdateCallback(new UpdateCallback(_target_group, this, slave_tf));
+ parent.addChild(tf);
+
+ return tf;
}
//------------------------------------------------------------------------------
projects / simgear.git / blobdiff