OMKJoint.cpp

Go to the documentation of this file.

/************************************************************************/
/* This file is part of openMask(c) INRIA, CNRS, Universite de Rennes 1 */
/* 1993-2002, thereinafter the Software                                 */
/*                                                                      */
/* The Software has been developped within the Siames Project.          */
/* INRIA, the University of Rennes 1 and CNRS jointly hold intellectual */
/* property rights                                                      */
/*                                                                      */
/* The Software has been registered with the Agence pour la Protection  */
/* des Programmes (APP) under registration number                       */
/* IDDN.FR.001.510008.00.S.P.2001.000.41200                             */
/*                                                                      */
/* This file may be distributed under the terms of the Q Public License */
/* version 1.0 as defined by Trolltech AS of Norway and appearing in    */
/* the file LICENSE.QPL included in the packaging of this file.         */
/*                                                                      */
/* Licensees holding valid specific licenses issued by INRIA, CNRS or   */
/* Universite Rennes 1 for the software may use this file in            */
/* acordance with that specific license                                 */
/************************************************************************/
    
#include "OMKJointExtension.h"
#include "OMKJoint.h"
#include "OMKExtensibleSimulatedObject.h"
#include "OMKInteractorExtension.h"
#include "OMKParametersAccessor.inl"
#include "OMKInputNT.h"
#include "OMKSessionPrm.h"
#include "OMKTransformType.h"
#include "Wm4Plane3.h"
#include "Wm4IntrLine3Plane3.h"
#include "Wm4IntrRay3Sphere3.h"

namespace OMK  
{
  namespace Iii 
  {
//=================================================================
// Joint
//=================================================================

REGISTER_JOINT_FACTORY( Joint, "Lock" ) ;

//-----------------------------------------------------------------
Joint::Joint( JointExtension* owner, const ConfigurationParameterDescriptor * node ) 
: _jointOffset( OMK::Type::Transform::sk_identity ),
  _jointOffsetInverse( OMK::Type::Transform::sk_identity ),
  _objectOffset( OMK::Type::Transform::sk_identity ),
  _objectOffsetInverse( OMK::Type::Transform::sk_identity ),
  _owner( owner ),
  _objectPosition( owner->_objectPosition ),
  _referencePosition( &owner->_referencePosition ), 
  _jointPosition( &owner->_jointPosition )
{
}
//-----------------------------------------------------------------
Joint::~Joint()
{
}
//-----------------------------------------------------------------
void Joint::establishLink() 
{
  // Must calculate the new offset between the reference position and the joint position
  // It is the current offset
  _jointOffset = product( _referencePosition->get().inverse(), _jointPosition->get() ) ;
  _jointOffset.updateFlags() ;
  
  // Must calculate the new offset between the joint position and the object position
  // It is the current offset
  _objectOffset = product( _jointPosition->get().inverse(), _objectPosition->get() ) ;
  _objectOffset.updateFlags() ;
  
  // Calculates the inverse transforms for optimisation
  _jointOffsetInverse = _jointOffset.inverse() ;
  _jointOffsetInverse.updateFlags() ;
  
  _objectOffsetInverse = _objectOffset.inverse() ;
  _objectOffsetInverse.updateFlags() ;
}

//-----------------------------------------------------------------
void Joint::currentJointPosition() 
{
  // calculates the new joint position
  _jointPosition->set( product( _referencePosition->get(), _jointOffset ) ) ;
}
//-----------------------------------------------------------------
bool Joint::retroPropagation() 
{
  // Calculates the optimal position of the reference position to retro-propagates
  _referencePosition->set( product( product( _objectPosition->get(), _objectOffsetInverse ), _jointOffsetInverse ) ) ;
  return true ;
}
//-----------------------------------------------------------------
void Joint::currentObjectPosition() 
{
  // calculates the new object position
  _objectPosition->set( product( _jointPosition->get(), _objectOffset ) ) ;
}
//-----------------------------------------------------------------

//=================================================================
// BallJoint
//=================================================================
REGISTER_JOINT_FACTORY( BallJoint, "Ball" ) ;

//-----------------------------------------------------------------
BallJoint::BallJoint( JointExtension* owner, const ConfigurationParameterDescriptor * node ) 
: Joint( owner, node ),
  _newObjectPosition(),
  _alreadyKnow( false ),
  _radius( 0.0f )
{
}
//-----------------------------------------------------------------
BallJoint::~BallJoint()
{
}
//-----------------------------------------------------------------
void BallJoint::establishLink() 
{
  Joint::establishLink() ;
  _radius = _objectOffset.getTranslate().Length() ;
  
    // stores the reference offset
  _refObjectOffset = _objectOffset ;
  _newObjectPosition = _objectPosition->get() ;
  _reset = true ;
}
//-----------------------------------------------------------------
bool BallJoint::retroPropagation() 
{
  if( _reset )
  {
    _previousPosition = _objectPosition->get().getTranslate() ;
    _reset = false ;
  }
  // Get the joint axis and perpendicular plane
  Wm4::Vector3f axis( _refObjectOffset.getTranslate() ) ;
  axis.Normalize() ;
  
  // the reference position according to the position of the joint and the reference offset
  OMK::Type::Transform refObjectPosition( product( _jointPosition->get(), _refObjectOffset ) ) ;
  
  // The reference orientation
  Wm4::Matrix3f refOrientation = _jointPosition->get().getRotate() ;
  // get the new orientation
  Wm4::Matrix3f newOrientation = _objectPosition->get().getRotate() * ( refObjectPosition.getRotate().Transpose() * refOrientation ) ;
  
  // Only the translation
  bool isConstraint = _objectPosition->get().getTranslate() != refObjectPosition.getTranslate() ;
  
  // The new object position
  // = the position of the joint
  // + the position along the axis
  _newObjectPosition.setTranslate( _jointPosition->get().getTranslate() + ( newOrientation * axis ) * _radius ) ;
  _newObjectPosition.setRotate( newOrientation ) ;
  _alreadyKnow = true ;
  // update the current object offset
  _objectOffset = product( _jointPosition->get().inverse(), _newObjectPosition ) ;
  _objectOffset.updateFlags() ;
  _objectOffsetInverse = _objectOffset.inverse() ;
  _objectOffsetInverse.updateFlags() ;

  if( isConstraint )
  {
    // Calculates the optimal position of the reference position to retro-propagates
    OMK::Type::Transform newJointPosition( _jointPosition->get().getTranslate() 
                                        + _objectPosition->get().getTranslate() 
                                        - _previousPosition,
                                          _jointPosition->get().getRotate() ) ;
    _referencePosition->set( product( newJointPosition, _jointOffsetInverse ) ) ;
    _reset = true ;
  }

  return isConstraint ;
}
//-----------------------------------------------------------------
void BallJoint::currentObjectPosition() 
{
  if( _alreadyKnow )
  {
    _alreadyKnow = false ;
    _objectPosition->set( _newObjectPosition ) ;
  }
  else
  {
    // calculates the new object position
    _objectPosition->set( product( _jointPosition->get(), _objectOffset ) ) ;
  }
}


//=================================================================
// HingeSlideJoint
//=================================================================
REGISTER_JOINT_FACTORY( HingeSlideJoint, "Hinge" ) ;

//-----------------------------------------------------------------
HingeSlideJoint::HingeSlideJoint( JointExtension* owner, const ConfigurationParameterDescriptor * node ) 
: Joint( owner, node ),
  _newObjectPosition(),
  _refObjectOffset(),
  _alreadyKnow( false ),
  _radius( 0.0f ),
  _refSlide( 0.0f ),
  _minSlide( 0.0f ),
  _maxSlide( 0.0f ),
  _minSlideEnable( false ),
  _maxSlideEnable( false ),
  _minAngle( 0.0f ),
  _maxAngle( 0.0f ),
  _angleEnable( false ),
  _rotateOnly( false ),
  _ignoreTranslate( false )
                             
{
  _minSlideEnable = ParametersAccessor::get( node, "MinSlide", _minSlide ) ;
  _maxSlideEnable = ParametersAccessor::get( node, "MaxSlide", _maxSlide ) ;
  _angleEnable = ParametersAccessor::get( node, "MinAngle", _minAngle ) 
      && ParametersAccessor::get( node, "MaxAngle", _maxAngle ) ;
  ParametersAccessor::get( node, "RotateOnly", _rotateOnly ) ;
}
//-----------------------------------------------------------------
HingeSlideJoint::~HingeSlideJoint()
{
}
//-----------------------------------------------------------------
void HingeSlideJoint::establishLink() 
{
  Joint::establishLink() ;
  // Get the joint plane
  Wm4::Vector3f axis( _jointPosition->get().getUp() ) ;
  axis.Normalize() ;
  Wm4::Plane3f plane( axis, _jointPosition->get().getTranslate() ) ;
  // The reference slide position
    // projection of the current position over the plane perpendicular to the joint axis
    Wm4::IntrLine3Plane3f objectOnPlane( Wm4::Line3f( _objectPosition->get().getTranslate(), axis ), plane ) ;
    objectOnPlane.Find() ;
    // to get the reference slide position
    _refSlide = objectOnPlane.GetLineT() ;
    // to get the radius on the plane
    Wm4::Vector3f projection( _objectPosition->get().getTranslate() + _refSlide * axis ) ;
    Wm4::Vector3f radiusVector( projection - _jointPosition->get().getTranslate() ) ;
    _radius = radiusVector.Length() ;
    _ignoreTranslate = ( _radius < Wm4::Math<float>::ZERO_TOLERANCE ) || _rotateOnly ;

    // stores the reference offset
    _refObjectOffset = _objectOffset ;
}
//-----------------------------------------------------------------
bool HingeSlideJoint::retroPropagation() 
{
  // Get the joint axis and perpendicular plane
  Wm4::Vector3f axis( _jointPosition->get().getUp() ) ;
  axis.Normalize() ;
  Wm4::Plane3f plane( axis, _jointPosition->get().getTranslate() ) ;
  // the reference position according to the position of the joint and the reference offset
  OMK::Type::Transform refObjectPosition( product( _jointPosition->get(), _refObjectOffset ) ) ;
  float currentSlide = 0.0f ;
  Wm4::Vector3f refOrientation ;
  Wm4::Vector3f newOrientation ;
  
  if( _ignoreTranslate )
  { // The axis is on the object position, only rotate
    // get a reference position over the joint plane
    refOrientation = _jointPosition->get().getRight() ;
    // get the new orientation
    newOrientation = _objectPosition->get().getRotate() * ( refObjectPosition.getRotate().Transpose() * _jointPosition->get().getRight() ) ;
    
    // the new slide offset
    currentSlide = axis.Dot( _jointPosition->get().getTranslate() - _objectPosition->get().getTranslate() ) - _refSlide ;
    
  }
  else
  { // The axis is far of the object position, only translate
    
    // projection of the reference position over the joint plane
    Wm4::Vector3f refIntersection( refObjectPosition.getTranslate() + _refSlide * axis ) ;
    // reference orientation
    refOrientation = refIntersection - _jointPosition->get().getTranslate() ;
    refOrientation.Normalize() ;
    
    // projection of the current position over the joint plane
    Wm4::IntrLine3Plane3f objectOnPlane( Wm4::Line3f( _objectPosition->get().getTranslate(), axis ), plane ) ;
    objectOnPlane.Find() ;
    // the new slide offset
    float currentSlideOverThePlane = objectOnPlane.GetLineT() ;
    
    // The new orientation
    Wm4::Vector3f projection( _objectPosition->get().getTranslate() + currentSlideOverThePlane * axis ) ;
    newOrientation = projection - _jointPosition->get().getTranslate() ;
    newOrientation.Normalize() ;
    
    // the new slide offset
    currentSlide = currentSlideOverThePlane - _refSlide ;
  }  
  
  // Compute the angle between the reference and the new orientation
  float angle = Wm4::Math<float>::ACos( newOrientation.Dot( refOrientation ) ) 
      * ( axis.Dot( newOrientation.Cross( refOrientation ) ) < 0.0f ? 1.0f : -1.0f ) ;
  
  Wm4::Matrix3f overRotationMatrix( Wm4::Matrix3f::IDENTITY ) ;
  float overSlide = 0.0f ;
  bool isConstraint = computeConstraints( angle, currentSlide, 
                                          overRotationMatrix, overSlide, newOrientation, 
                                          refOrientation, axis ) ; 
  
  // The new object position
  // = the position of the joint
  // + the position on the plane of the joint
  // + the position along the axis
  _newObjectPosition.setTranslate( _jointPosition->get().getTranslate() + newOrientation * _radius - ( _refSlide + currentSlide ) * axis ) ;
  // the rotation of the object compared to the position of reference
  Wm4::Matrix3f rotationMatrix( axis, angle ) ;
  _newObjectPosition.setRotate( rotationMatrix * refObjectPosition.getRotate() ) ;
  _alreadyKnow = true ;
  // update the current object offset
  _objectOffset = product( _jointPosition->get().inverse(), _newObjectPosition ) ;
  _objectOffset.updateFlags() ;
  _objectOffsetInverse = _objectOffset.inverse() ;
  _objectOffsetInverse.updateFlags() ;

  if( isConstraint )
  {
    // Calculates the optimal position of the reference position to retro-propagates
    OMK::Type::Transform newJointPosition( _jointPosition->get().getTranslate() - overSlide * axis, overRotationMatrix * _jointPosition->get().getRotate() ) ;
    _referencePosition->set( product( newJointPosition, _jointOffsetInverse ) ) ;
  }

  return isConstraint ;
}
//-----------------------------------------------------------------
bool HingeSlideJoint::computeConstraints( float& angle, float& currentSlide,
                                     Wm4::Matrix3f& overRotationMatrix, float& overSlide, 
                                     Wm4::Vector3f& newOrientation, 
                                     const Wm4::Vector3f& refOrientation, const Wm4::Vector3f& axis ) 
{
  bool isConstraint = false ; 
  overRotationMatrix.MakeIdentity() ;
  overSlide = 0.0f ;
  // Compute the constraint
  if( _angleEnable )
  {
    if( angle < _minAngle )
    {
      overRotationMatrix.FromAxisAngle( axis, angle - _minAngle ) ;
      angle = _minAngle ;
      isConstraint = true ;
    }
    else if( _maxAngle < angle )
    {
      overRotationMatrix.FromAxisAngle( axis, angle - _maxAngle ) ;
      angle = _maxAngle ;
      isConstraint = true ;
    }
    if( isConstraint )
    {
      newOrientation = Wm4::Matrix3f( axis, angle ) * refOrientation ;
    }
  }
    
  // Compute the constraint
  if( _minSlideEnable && ( currentSlide < _minSlide ) )
  {
    overSlide = currentSlide - _minSlide ;
    currentSlide = _minSlide ;
    isConstraint = true ;
  }
  else if( _maxSlideEnable && ( _maxSlide < currentSlide ) )
  {
    overSlide = currentSlide - _maxSlide ;
    currentSlide = _maxSlide ;
    isConstraint = true ;
  }
  return isConstraint ;
}
//-----------------------------------------------------------------
void HingeSlideJoint::currentObjectPosition() 
{
  if( _alreadyKnow )
  {
    _alreadyKnow = false ;
    _objectPosition->set( _newObjectPosition ) ;
  }
  else
  {
    // calculates the new object position
    _objectPosition->set( product( _jointPosition->get(), _objectOffset ) ) ;
  }
}



//=================================================================
// ScrewJoint
//=================================================================
REGISTER_JOINT_FACTORY( ScrewJoint, "Screw" ) ;

//-----------------------------------------------------------------
ScrewJoint::ScrewJoint( JointExtension* owner, const ConfigurationParameterDescriptor * node ) 
: HingeSlideJoint( owner, node ),
  _pitch( 0.0f ),
  _reverse( false ),
  _state( NOSCREW ),
  _stateMin( NOSCREW ),
  _stateMax( NOSCREW ),
  _previousAngle( 0.0f ),
  _turns( 0.0f )
{
  ParametersAccessor::get( node, "Pitch", _pitch ) ;
  _stateMin = ( 0.0f < _pitch ) ? UNSCREW : SCREW ;
  _stateMax = ( 0.0f < _pitch ) ? SCREW : UNSCREW ;
  OMASSERTM( _pitch != 0.0f, "The screw pitch cannot be null" ) ;
  ParametersAccessor::get( node, "Reverse", _reverse ) ;
  _angleEnable = false ;
}
//-----------------------------------------------------------------
ScrewJoint::~ScrewJoint()
{
}
//-----------------------------------------------------------------
void ScrewJoint::establishLink() 
{
  _turns = 0.0f ;
  _previousAngle = 0.0f ;
  _state = NOSCREW ;
  HingeSlideJoint::establishLink() ;
  _previousOrientation = _jointPosition->get().getRight() ;
}
//-----------------------------------------------------------------
bool ScrewJoint::computeConstraints( float& angle, float& currentSlide,
                                     Wm4::Matrix3f& overRotationMatrix, float& overSlide, 
                                     Wm4::Vector3f& newOrientation, 
                                     const Wm4::Vector3f& refOrientation, const Wm4::Vector3f& axis ) 
{
  overRotationMatrix.MakeIdentity() ;
  overSlide = 0.0f ;
  bool isConstraint = false ;
  if( _reverse )
  { // angle is a function of the slide
    // Compute the constraint
    if( _minSlideEnable && ( currentSlide < _minSlide ) )
    {
      overSlide = currentSlide - _minSlide ;
      currentSlide = _minSlide ;
      isConstraint = true ;
    }
    else if( _maxSlideEnable && ( _maxSlide < currentSlide ) )
    {
      overSlide = currentSlide - _maxSlide ;
      currentSlide = _maxSlide ;
      isConstraint = true ;
    }
    // The angle is only a function of the slide offset
    angle = currentSlide / _pitch * -Wm4::Math<float>::TWO_PI ;
    newOrientation = Wm4::Matrix3f( axis, angle ) * refOrientation ;
  }
  else
  { // slide is a function of the angle
     // Computes the angle between the previous and the new orientation to determine if it screws or unscrews
    float screw = axis.Dot( newOrientation.Cross( _previousOrientation ) ) ;
    switch( _state )
    {
      case NOSCREW :
        if( Wm4::Math<float>::ZERO_TOLERANCE < screw ) _state = SCREW ;
        else if( screw < -Wm4::Math<float>::ZERO_TOLERANCE ) _state = UNSCREW ;
        // Calculates the slide
        currentSlide = ( _turns + angle * -Wm4::Math<float>::INV_TWO_PI ) * _pitch ;
        break ;
      case SCREW :
      case UNSCREW :
        if( Wm4::Math<float>::ZERO_TOLERANCE < screw )
        {
          _state = SCREW ;
          if( _previousAngle < angle ) _turns += 1.0f ;
        }
        else if( screw < -Wm4::Math<float>::ZERO_TOLERANCE )
        {
          _state = UNSCREW ;
          if( angle < _previousAngle ) _turns -= 1.0f ;
        }
        else _state = NOSCREW ;
        // Calculates the slide
        currentSlide = ( _turns + angle * -Wm4::Math<float>::INV_TWO_PI ) * _pitch ;
        // Applies the constraintes
        if( _state == _stateMin && _minSlideEnable && ( currentSlide < _minSlide ) )
        {
          _state = IS_MIN ;
          computeConstraintAngle( angle, _minSlide, newOrientation, refOrientation, axis ) ;
        }
        else if( _state == _stateMax && _maxSlideEnable && ( _maxSlide < currentSlide ) )
        {
          _state = IS_MAX ;
          computeConstraintAngle( angle, _maxSlide, newOrientation, refOrientation, axis ) ;
        }
        break ;
      case IS_MIN :
        {
          if( -Wm4::Math<float>::ZERO_TOLERANCE < screw ) _state = NOSCREW ;
          // The angle is only a function of the slide offset
          currentSlide = _minSlide ;
          overRotationMatrix.FromAxisAngle( axis, angle - computeConstraintAngle( angle, currentSlide, newOrientation, refOrientation, axis ) ) ;
          isConstraint = true ;
        }
        break ;    
      case IS_MAX :
        {
          if( screw < Wm4::Math<float>::ZERO_TOLERANCE) _state = NOSCREW ;
          // The angle is only a function of the slide offset
          currentSlide = _maxSlide ;
          overRotationMatrix.FromAxisAngle( axis, angle - computeConstraintAngle( angle, currentSlide, newOrientation, refOrientation, axis ) ) ;
          isConstraint = true ;
        }
        break ;    
    }
    // min and max slide saturation
    if( _minSlideEnable && ( currentSlide < _minSlide ) )
    {
      currentSlide = _minSlide ;
    }
    else if( _maxSlideEnable && ( _maxSlide < currentSlide ) )
    {
      currentSlide = _maxSlide ;
    }
    _turns = currentSlide / _pitch + angle * Wm4::Math<float>::INV_TWO_PI ;
    // Stores current values which will be the previous ones of the next step
    _previousAngle = angle ;
    _previousOrientation = newOrientation ;
  }
    
  return isConstraint ;
}
float ScrewJoint::computeConstraintAngle( float& angle, const float& currentSlide,
                                         Wm4::Vector3f& newOrientation, 
                                         const Wm4::Vector3f& refOrientation, const Wm4::Vector3f& axis ) 
{
  float constraintAngle = currentSlide / _pitch * -Wm4::Math<float>::TWO_PI ;
  if( constraintAngle < 0.0f )
  {
    constraintAngle = Wm4::Math<float>::FMod( -constraintAngle + Wm4::Math<float>::TWO_PI + Wm4::Math<float>::PI, Wm4::Math<float>::TWO_PI ) - Wm4::Math<float>::PI ;
  }
  else
  {
    constraintAngle = Wm4::Math<float>::FMod( constraintAngle + Wm4::Math<float>::PI, Wm4::Math<float>::TWO_PI ) - Wm4::Math<float>::PI ;            
  }
  newOrientation = Wm4::Matrix3f( axis, constraintAngle ) * refOrientation ;
  angle = constraintAngle ;
  return constraintAngle ;
}

  }// namespace Iii
}// namespace OMK

---