OMKTrajectory.cpp

Go to the documentation of this file.

#include "OMKTrajectory.h"
#include "OMKParametersAccessor.inl"
#include "OMKAttribute.inl"
#include "OMKTracer.h"
#include "Wm4BSplineCurve3.h"
#include "OMKTransformType.h"

using namespace OMK ;
using namespace OMK::Type ;

//-------------------------------------------------------------------------
REGISTER_OBJECT_FACTORY( Trajectory, "Trajectory" ) ;
//-----------------------------------------------------------------------------

Trajectory::Trajectory( Controller& ctrl, const ObjectDescriptor& objectDescriptor )
: OMK::SimplePoint( ctrl, objectDescriptor ),
  _bSpline( 0 ),
  _positionBetweenTargets( 1.0f ), 
  _currentTarget( 0 ),
  _followOrientation( false ), 
  _step( "Step", 0.0f )
{ 
  addAttribute( _step ) ;
}

//-----------------------------------------------------------------------------

Trajectory::~Trajectory( void ) 
{
}
//-----------------------------------------------------------------------------

bool Trajectory::loadParameters( const ConfigurationParameterDescriptor * node )
{
  SimplePoint::loadParameters( node ) ;
  if( !ParametersAccessor::get( node, "Targets", _targets ) )
  {
    _targets.push_back( Transform( Wm4::Vector3f(  10, 0,  10 ) ) ) ;
    _targets.push_back( Transform( Wm4::Vector3f(  10, 0, -10 ) ) ) ;
    _targets.push_back( Transform( Wm4::Vector3f( -10, 0, -10 ) ) ) ;
    _targets.push_back( Transform( Wm4::Vector3f( -10, 0,  10 ) ) ) ;
  }
  ParametersAccessor::get( node, "Orientation", _followOrientation ) ;
  bool bspline = false ;
  ParametersAccessor::get( node, "BSpline", bspline ) ;
  if( bspline && 2 <= _targets.size() )
  { // At least 2 points to define a B-spline
    int degree = 2 ; // Default value
    ParametersAccessor::get( node, "Degree", degree ) ;
    degree = degree < (int)_targets.size() ? degree : _targets.size() - 1 ;
    Wm4::Vector3f* targets = new Wm4::Vector3f[ _targets.size() ] ;
    for( unsigned int i = 0 ; i < _targets.size() ; i++ )
    {
      targets[ i ] = _targets[ i ].getTranslate() ;
    }
    _bSpline = new Wm4::BSplineCurve3f( _targets.size(), targets, degree, true, false ) ;
    delete[] targets ;
    _refOrientation = _bSpline->GetFirstDerivative( 0.0f ) ;
    _refOrientation.Normalize() ;
  }
  else
  {
    _refOrientation = _targets[ 1 ].getTranslate() - _targets[ 0 ].getTranslate() ;
    _refOrientation.Normalize() ;
  }
  
  _refPosition = _position.get() ;
  OMK::Type::Transform position( _refPosition ) ;
  position.setTranslate( _targets[ 0 ].getTranslate() ) ;
  _position.set( position ) ;
  _position.setValueToOutput() ;
  return true ; 
}
//-----------------------------------------------------------------------------

void Trajectory::computeParameters() 
{
  float step = _step.get() * ( _bSpline ? 1.0f : _targets.size() ) ;
  // Get the increment
  if ( 1.0f <= _positionBetweenTargets ) 
  { // step > 0
    _currentTarget = ( _currentTarget + 1 ) % _targets.size() ;
    _positionBetweenTargets = step ;
  }
  else if ( _positionBetweenTargets <= 0.0f ) 
  { // step < 0
    _currentTarget = ( _currentTarget + _targets.size() - 1 ) % _targets.size() ;
    _positionBetweenTargets = 1.0f + step ;
  }
  else
  {
    _positionBetweenTargets += step ;
  }
  
  // Get the translate and the rotate
  OMK::Type::Transform position( _refPosition ) ;
  Wm4::Vector3f orientation ;
  if( _bSpline )
  { // For B-spline
    if( _followOrientation ) 
    {
      Wm4::Vector3f translate ;
      _bSpline->Get( _positionBetweenTargets, &translate, &orientation, 0, 0 ) ;
      position.setTranslate( translate ) ;
    }
    else
    {
      position.setTranslate( _bSpline->GetPosition( _positionBetweenTargets ) ) ;
    }
  }
  else
  { // For segments
    int previousTarget( ( _currentTarget + _targets.size() - 1 ) % _targets.size() ) ;
    orientation = _targets[ _currentTarget ].getTranslate() - _targets[ previousTarget ].getTranslate() ;
    position.setTranslate( _targets[previousTarget].getTranslate() + orientation * _positionBetweenTargets ) ;
  }
  
  if( _followOrientation ) 
  { // Calculate the orientation
    orientation.Normalize() ;
    Wm4::Vector3f axis( orientation.Cross( _refOrientation ) ) ;
    axis.Normalize() ;
    float angle = -Wm4::Math<float>::ACos( orientation.Dot( _refOrientation ) ) ;
    Wm4::Matrix3f rotationMatrix( axis, angle ) ;
    position.setRotate( position.getRotate() * rotationMatrix ) ;
  }
  // Set the new position
  _position.set( position ) ;
}

---