OMKTransform.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 "OMKTransform.h" 
#include "OMKTransformType.h"

using namespace OMK ;
using namespace OMK::Type ;

const Transform Transform::sk_identity;

//-------------------------------------------------------------------------
// constructor
//-------------------------------------------------------------------------
// default transform is the identity transformation
Transform::Transform()
: _matrix   ( 1.0f, 0.0f, 0.0f, 
              0.0f, 1.0f, 0.0f, 
              0.0f, 0.0f, 1.0f ),
  _translate( 0.0f, 0.0f, 0.0f ),
  _scale    ( 1.0f, 1.0f, 1.0f ),
  _identity    ( true ),
  _uniformScale( true )
{
} 

//-------------------------------------------------------------------------
// constructor
//-------------------------------------------------------------------------
Transform::Transform( const Wm4::Vector3f& translate, 
                      const Wm4::Matrix3f& rotate,// = Wm4::Matrix3f( 1.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 1.0f ), 
                      float scale ) // = 1.0f )
: _matrix   ( rotate              ),
  _translate( translate           ),
  _scale    ( scale, scale, scale ),
  _identity    ( false     ),
  _uniformScale( true      )
{
  OMASSERTM( scale != 0.0f, "scale cannot be null" ) ;
  updateFlags() ;
} 

//-------------------------------------------------------------------------
// constructor
//-------------------------------------------------------------------------
Transform::Transform( const Wm4::Vector3f& translate,
                      const Wm4::Quaternionf& rotate, 
                      float scale ) // = 1.0f )
: _matrix   (),
  _translate( translate           ),
  _scale    ( scale, scale, scale ),
  _identity    ( false     ),
  _uniformScale( true      )
{
  rotate.ToRotationMatrix( _matrix ) ;
  OMASSERTM( scale != 0.0f, "scale cannot be null" ) ;
  updateFlags() ;
} 
   
//-------------------------------------------------------------------------
// constructor
//-------------------------------------------------------------------------
Transform::Transform( const Wm4::Vector3f& translate, 
                      const Wm4::Matrix3f& rotate, 
                      const Wm4::Vector3f& scale ) 
: _matrix   ( rotate           ),
  _translate( translate        ),
  _scale    ( scale            ),
  _identity    ( false     ),
  _uniformScale( false     )
{                
  OMASSERTM( ( scale.X() != 0.0f 
            && scale.Y() != 0.0f
            && scale.Z() != 0.0f ), "scale cannot be a null vector" ) ;
  updateFlags() ;
} 

//-------------------------------------------------------------------------
// constructor
//-------------------------------------------------------------------------
Transform::Transform( const Wm4::Vector3f& translate, 
                      const Wm4::Quaternionf& rotate, 
                      const Wm4::Vector3f& scale ) 
: _matrix   (),
  _translate( translate        ),
  _scale    ( scale            ),
  _identity    ( false     ),
  _uniformScale( false     )
{         
  rotate.ToRotationMatrix( _matrix ) ;
  OMASSERTM( ( scale.X() != 0.0f 
            && scale.Y() != 0.0f
            && scale.Z() != 0.0f ), "scale cannot be a null vector" ) ;
  updateFlags() ;
} 

//-------------------------------------------------------------------------
// destructor
//-------------------------------------------------------------------------
Transform::~Transform()
{
}

//-------------------------------------------------------------------------
// resetUnitScale
//-------------------------------------------------------------------------
void 
Transform::resetUnitScale ()
{
  _scale = Wm4::Vector3f( 1.0f, 1.0f, 1.0f );
  _uniformScale = true;
}

//-------------------------------------------------------------------------
// getNorm
//-------------------------------------------------------------------------
float 
Transform::getNorm() const
{
  float sX = (float)::fabs( _scale.X() ) ;
  float sY = (float)::fabs( _scale.Y() ) ;
  float sZ = (float)::fabs( _scale.Z() ) ;
  return sX < sY ? ( sY < sZ ? sZ : sY ) : ( sX < sZ ? sZ : sX ) ;
}

//-------------------------------------------------------------------------
// setTranslate
//-------------------------------------------------------------------------
void 
Transform::setTranslate( const Wm4::Vector3f& translate )
{
    _translate = translate ;
    _identity  = false     ;
}

//-------------------------------------------------------------------------
// setRotate
//-------------------------------------------------------------------------
void 
Transform::setRotate( const Wm4::Matrix3f& rotate )
{
  _matrix   = rotate ;
  _identity = false  ;
}

//-------------------------------------------------------------------------
// setRotate
//-------------------------------------------------------------------------
void 
Transform::setRotate( const Wm4::Quaternionf& rotate )
{
  rotate.ToRotationMatrix( _matrix ) ;
  _identity = false  ;
}

//-------------------------------------------------------------------------
// setScale
//-------------------------------------------------------------------------
void 
Transform::setScale( const Wm4::Vector3f& scale )
{
  OMASSERTM( ( scale.X() != 0.0f 
            && scale.Y() != 0.0f
            && scale.Z() != 0.0f ), "The scale vector cannot be null" ) ;
  _scale        = scale ;
  _identity     = false ;
  _uniformScale = false ;
}

//-------------------------------------------------------------------------
// setUniformScale
//-------------------------------------------------------------------------
void 
Transform::setUniformScale( float scale )
{
  OMASSERTM( scale != 0.0f, "The scale cannot be null" );
  _scale        = Wm4::Vector3f( scale, scale, scale ) ;
  _identity     = false ;
  _uniformScale = true  ;
}

//-------------------------------------------------------------------------
// applyForward
//-------------------------------------------------------------------------
Wm4::Vector3f 
Transform::applyForward( const Wm4::Vector3f& input ) const
{
  Wm4::Vector3f output ;
  if( isIdentity() )
  { // Y = X
    output = input;
  }
  else
  {
    // Y = R*S*X + T
    output = Wm4::Vector3f( _scale.X() * input.X(), 
                            _scale.Y() * input.Y(),
                            _scale.Z() * input.Z() );
    output = _matrix * output + _translate;
  }
  return output;
}

//-------------------------------------------------------------------------
// applyForward
//-------------------------------------------------------------------------
std::vector<Wm4::Vector3f> 
Transform::applyForward( const std::vector<Wm4::Vector3f>& input ) const
{
  std::vector<Wm4::Vector3f> output ;
  if ( isIdentity() )
  {
    // Y = X
    output = input ;
  }
  else
  {
    // Y = R*S*X + T
    output.resize( input.size() ) ;
    for( unsigned int i( 0 ) ; i < input.size() ; ++i )
  {
        output[i].X() = _scale.X() * input[i].X() ;
        output[i].Y() = _scale.Y() * input[i].Y() ;
        output[i].Z() = _scale.Z() * input[i].Z() ;
        output[i] = _matrix * output[i] + _translate;
    }
  }
  return output ;
}

//-------------------------------------------------------------------------
// applyInverse
//-------------------------------------------------------------------------
Wm4::Vector3f 
Transform::applyInverse( const Wm4::Vector3f& input ) const
{
    Wm4::Vector3f output ;
    if (_identity)
    {
      // X = Y
      output = input;
    }
    else
    {
      output = input - _translate ;
      // X = S^{-1}*R^t*(Y - T)
      output = output * _matrix;
      if( isUniformScale() )
      {
        output /= getUniformScale();
      }
      else
      {
        // The direct inverse scaling is
        //   output.X() /= _scale.X();
        //   output.Y() /= _scale.Y();
        //   output.Z() /= _scale.Z();
        // When division is much more expensive than multiplication,
        // three divisions are replaced by one division and ten
        // multiplications.
        float SXY = _scale.X() * _scale.Y() ;
        float SXZ = _scale.X() * _scale.Z() ;
        float SYZ = _scale.Y() * _scale.Z() ;
        float invDet = 1.0f / ( SXY * _scale.Z() ) ;
        output.X() *= invDet * SYZ ;
        output.Y() *= invDet * SXZ ;
        output.Z() *= invDet * SXY ;
      }
    }
    return output;
}

//-------------------------------------------------------------------------
// applyInverse
//-------------------------------------------------------------------------
std::vector<Wm4::Vector3f> 
Transform::applyInverse( const std::vector<Wm4::Vector3f>& input ) const
{
  std::vector<Wm4::Vector3f> output ;
  if (_identity)
  {
    output = input ;
  }
  else
  {
    Wm4::Vector3f diff;
    // X = S^{-1}*R^t*(Y - T)
    if ( isUniformScale() )
    {
      float invScale = 1.0f / getUniformScale() ;
      for( unsigned int i( 0 ) ; i < input.size() ; ++i )
      {
        output.push_back( invScale * ( ( input[i] - _translate ) * _matrix ) );
      }
    }
    else
    {
      // The direct inverse scaling is
      //   invXScale = 1.0f/_scale.X();
      //   invYScale = 1.0f/_scale.Y();
      //   invZScale = 1.0f/_scale.Z();
      // When division is much more expensive than multiplication, three
      // divisions are replaced by one division and ten multiplications.
      float SXY = _scale.X() * _scale.Y() ;
      float SXZ = _scale.X() * _scale.Z() ;
      float SYZ = _scale.Y() * _scale.Z() ;
      float invDet = 1.0f / ( SXY * _scale.Z() ) ;
      float invXScale = invDet * SYZ ;
      float invYScale = invDet * SXZ ;
      float invZScale = invDet * SXY ;
      for( unsigned int i( 0 ) ; i < input.size() ; ++i )
      {
        output.push_back( ( input[i] - _translate ) * _matrix );
        output[i].X() *= invXScale;
        output[i].Y() *= invYScale;
        output[i].Z() *= invZScale;
      }
    }
  }
  return output ;
}

//-------------------------------------------------------------------------
// invertVector
//-------------------------------------------------------------------------
Wm4::Vector3f 
Transform::invertVector( const Wm4::Vector3f& input ) const
{
  Wm4::Vector3f output;
  if( isIdentity() )
  {
    // X = Y
    output = input ;
  }
  else
  {
    // X = S^{-1}*R^t*Y
    output = input * _matrix ;
    if ( isUniformScale() )
    {
      output /= getUniformScale() ;
    }
    else
    {
      // The direct inverse scaling is
      //   output.X() /= _scale.X();
      //   output.Y() /= _scale.Y();
      //   output.Z() /= _scale.Z();
      // When division is much more expensive than multiplication,
      // three divisions are replaced by one division and ten
      // multiplications.
      float SXY = _scale.X() * _scale.Y() ;
      float SXZ = _scale.X() * _scale.Z() ;
      float SYZ = _scale.Y() * _scale.Z() ;
      float invDet = 1.0f/( SXY * _scale.Z() ) ;
      output.X() *= invDet * SYZ ;
      output.Y() *= invDet * SXZ ;
      output.Z() *= invDet * SXY ;
    }
  }
  return output;
}

//-------------------------------------------------------------------------
// applyForward
//-------------------------------------------------------------------------
Wm4::Plane3f 
Transform::applyForward( const Wm4::Plane3f& input ) const
{
  Wm4::Plane3f output ;
  if( isIdentity() )
  {
    output = input;
  }
  // Let X represent points in model space and Y = R*S*X+T represent
  // points in world space where S are the world scales, R is the world
  // rotation, and T is the world translation.  The inverse transform is
  // X = S^{-1}*R^t*(Y-T).  The model plane is Dot(N0,X) = C0.
  // Replacing the formula for X in it and applying some algebra leads
  // to the world plane Dot(N1,Y) = C1 where N1 = R*S^{-1}*N0 and
  // C1 = C0+Dot(N1,T).  If S is not the identity, then N1 is not unit
  // length.  We need to normalize N1 and adjust C1:  N1' = N1/|N1| and
  // C1' = C1/|N1|.
  else if ( isUniformScale() )
  {
    output.Normal   = _matrix * input.Normal ;
    output.Constant = getUniformScale() * input.Constant +
                      output.Normal.Dot( _translate );
  }
  else 
  {
    output.Normal = input.Normal ;

    // The direct inverse scaling is
    //   output.X() /= _scale.X();
    //   output.Y() /= _scale.Y();
    //   output.Z() /= _scale.Z();
    // When division is much more expensive than multiplication,
    // three divisions are replaced by one division and ten
    // multiplications.
    float SXY = _scale.X() * _scale.Y() ;
    float SXZ = _scale.X() * _scale.Z() ;
    float SYZ = _scale.Y() * _scale.Z() ;
    float invDet = 1.0f / ( SXY * _scale.Z() ) ;
    output.Normal.X() *= invDet * SYZ;
    output.Normal.Y() *= invDet * SXZ;
    output.Normal.Z() *= invDet * SXY;
    output.Normal = _matrix * output.Normal;
  
    float fInvLength = 1.0f / output.Normal.Length() ;
    output.Normal *= fInvLength ;
    output.Constant = fInvLength * input.Constant +
                      output.Normal.Dot( _translate );
  }
  return output;
}

//-------------------------------------------------------------------------
// product
//-------------------------------------------------------------------------
Transform 
OMK::Type::product( const Transform& A, const Transform& B )
{
  Transform output ;
  if( A.isIdentity() )
  {
    output = B ;
  }
  else if( B.isIdentity() )
  {
    output = A ;
  }
  else
  {
    if( !A.isUniformScale() )
    {
      // set correctly "_uniformScale" boolean in A if needed
      const_cast< Transform& >( A )._uniformScale = ( A._scale[0] == A._scale[1] && A._scale[1] == A._scale[2] ) ;
    }

    output.setRotate( A._matrix * B._matrix ) ;

    if( A.isUniformScale() )
    {
      output.setTranslate( A.getUniformScale() * ( A._matrix * B._translate ) + A._translate ) ;

      if ( B.isUniformScale() )
      {
        output.setUniformScale( A.getUniformScale() * B.getUniformScale() ) ;
      }
      else
      {
        output.setScale( A.getUniformScale() * B.getScale() ) ;
      }
    }
    else
    {
      Wm4::Vector3f v0_translate( A._matrix * B._translate ) ;
      Wm4::Vector3f v1_translate( A.getScale()[0] * v0_translate[0],
                                  A.getScale()[1] * v0_translate[1],
                                  A.getScale()[2] * v0_translate[2] ) ;
      output.setTranslate( v1_translate + A._translate ) ;

      if ( B.isUniformScale() )
      {
        output.setScale( A.getScale() * B.getUniformScale() ) ;
      }
      else
      {
        Wm4::Vector3f v0_scale( A._matrix * B._scale ) ;
        Wm4::Vector3f v1_scale( A.getScale()[0] * v0_scale[0],
                                A.getScale()[1] * v0_scale[1],
                                A.getScale()[2] * v0_scale[2] ) ;
        output.setScale( v1_scale ) ;
      }
    }
  }
  return output ;
}

//-------------------------------------------------------------------------
// inverse
//-------------------------------------------------------------------------
Transform 
Transform::inverse() const
{ 
  Transform inverse ;
  if( _identity )
  {
    inverse = *this;
  }
  else 
  {
    if( _uniformScale )
    {
      inverse._matrix = getRotate().Transpose() / _scale[0];
      inverse._uniformScale = _scale[0] != 1.0 ;
    }
    else
    {
      Wm4::Matrix3f RS = _matrix.TimesDiagonal( _scale ) ;
      inverse._matrix = RS.Inverse() ;
      inverse._uniformScale = false;
    }

    inverse._translate = -( inverse._matrix * _translate );
    inverse._identity     = false;
  }
  return inverse ;
}

//-------------------------------------------------------------------------
// updateFlags
//-------------------------------------------------------------------------
void Transform::updateFlags() 
{
  if( !_identity )
  { // Not declared identity
    if( ( _matrix == Wm4::Matrix3f::IDENTITY ) 
     && ( _translate == Wm4::Vector3f::ZERO )
     && ( _scale == Wm4::Vector3f::ONE ) )
    { // Test identity
      _identity = true ;
      _uniformScale = true ;
    }
    else
    {
#if !defined NDEBUG      
      // is it a pure rotation
      if( ( ( _matrix != Wm4::Matrix3f::IDENTITY ) 
        && ( ( _matrix * _matrix.Transpose() != Wm4::Matrix3f::IDENTITY )
          || ( _matrix.Determinant() != 1.0f ) ) ) )
      {
        Wm4::Matrix3f m( _matrix * _matrix.Transpose() ) ;
        OMTRACEID( OMK_DEBUG_OMK_TYPE,
                   "This transform is not a pure RS one, may be you will encounter some problem with it" << std::endl << *this ) ;
      }
#endif 
      // is it a uniform scale
      _uniformScale = ( _scale[0] == _scale[1] && _scale[1] == _scale[2] ) ;
    }
  }
}

//-------------------------------------------------------------------------
// getHomogeneous
//-------------------------------------------------------------------------
Wm4::Matrix4f 
Transform::getHomogeneous() const
{
  Wm4::Matrix4f hMatrix ;
  hMatrix[0][0] = _scale[0] * _matrix[0][0] ;
  hMatrix[0][1] = _scale[0] * _matrix[1][0] ;
  hMatrix[0][2] = _scale[0] * _matrix[2][0] ;
  hMatrix[0][3] = 0.0f ;
  hMatrix[1][0] = _scale[1] * _matrix[0][1] ;
  hMatrix[1][1] = _scale[1] * _matrix[1][1] ;
  hMatrix[1][2] = _scale[1] * _matrix[2][1] ;
  hMatrix[1][3] = 0.0f ;
  hMatrix[2][0] = _scale[2] * _matrix[0][2] ;
  hMatrix[2][1] = _scale[2] * _matrix[1][2] ;
  hMatrix[2][2] = _scale[2] * _matrix[2][2] ;
  hMatrix[2][3] = 0.0f ;
  hMatrix[3][0] = _translate[0] ;
  hMatrix[3][1] = _translate[1] ;
  hMatrix[3][2] = _translate[2] ;
  hMatrix[3][3] = 1.0f ;
  
  return hMatrix ;
}

//-------------------------------------------------------------------------
// operator ==
//-------------------------------------------------------------------------
bool
OMK::Type::operator == ( const Transform& t0, const Transform& t1 )
{
  return (  t0._identity == t1._identity &&
            t0._uniformScale == t1._uniformScale && 
            t0._translate == t1._translate && 
            t0._scale == t1._scale && 
            t0._matrix == t1._matrix ) ;
}

//-------------------------------------------------------------------------
// operator !=
//-------------------------------------------------------------------------
bool
OMK::Type::operator != ( const Transform& t0, const Transform& t1 )
{
  return !( t0 == t1 ) ;
}
//-------------------------------------------------------------------------
// operator <<
//-------------------------------------------------------------------------
std::ostream& operator << ( std::ostream& out, const Transform& q )
{
  out << q.isIdentity() << " " ;
  if( !q.isIdentity() )
  {
    out << q.isUniformScale()    << " "
        << q.getTranslate().X()  << " "
        << q.getTranslate().Y()  << " "
        << q.getTranslate().Z()  << " "
        << q.getRotate()( 0, 0 ) << " "
        << q.getRotate()( 0, 1 ) << " "
        << q.getRotate()( 0, 2 ) << " "
        << q.getRotate()( 1, 0 ) << " "
        << q.getRotate()( 1, 1 ) << " "
        << q.getRotate()( 1, 2 ) << " "
        << q.getRotate()( 2, 0 ) << " "
        << q.getRotate()( 2, 1 ) << " "
        << q.getRotate()( 2, 2 ) << " "
        << q.getScale().X()      << " "
        << q.getScale().Y()      << " "
        << q.getScale().Z()      << " "  ;
  }
  out << " " ;
  return out;
}

//-------------------------------------------------------------------------
// operator >>
//-------------------------------------------------------------------------
std::istream& operator >> ( std::istream& in, Transform& q ) 
{
  bool isIdentity = false ;
  in >> isIdentity ;
  if( isIdentity )
  {
    q.resetToIdentity() ;
  }
  else
  {
    bool isUniformScale ;
    Wm4::Vector3f translate ;
    Wm4::Matrix3f matrix ;
    Wm4::Vector3f scale ;
    in >> isUniformScale 
       >> translate.X()
       >> translate.Y()
       >> translate.Z() 
       >> matrix( 0, 0 ) 
       >> matrix( 0, 1 ) 
       >> matrix( 0, 2 ) 
       >> matrix( 1, 0 ) 
       >> matrix( 1, 1 ) 
       >> matrix( 1, 2 ) 
       >> matrix( 2, 0 ) 
       >> matrix( 2, 1 ) 
       >> matrix( 2, 2 ) 
       >> scale.X()  
       >> scale.Y()  
       >> scale.Z() ;
    q.setTranslate( translate ) ;
    q.setRotate( matrix ) ;
    if( isUniformScale ) q.setUniformScale( scale.X() ) ; else q.setScale( scale ) ;
  }
  return in;
}

//-------------------------------------------------------------------------
// operator <<
//-------------------------------------------------------------------------
OutgoingSynchronisationMessage& operator << ( OutgoingSynchronisationMessage& out, const Transform& q ) 
{
  out << q.isIdentity() ;
  if( !q.isIdentity() )
  {
    out << q.isUniformScale()    
        << q.getTranslate().X()  
        << q.getTranslate().Y()  
        << q.getTranslate().Z()  
        << q.getRotate()( 0, 0 ) 
        << q.getRotate()( 0, 1 ) 
        << q.getRotate()( 0, 2 ) 
        << q.getRotate()( 1, 0 ) 
        << q.getRotate()( 1, 1 ) 
        << q.getRotate()( 1, 2 ) 
        << q.getRotate()( 2, 0 ) 
        << q.getRotate()( 2, 1 ) 
        << q.getRotate()( 2, 2 ) 
        << q.getScale().X()      
        << q.getScale().Y()      
        << q.getScale().Z()      ;
  }
  return out;
}

//-------------------------------------------------------------------------
// operator >>
//-------------------------------------------------------------------------
IncomingSynchronisationMessage& operator >> ( IncomingSynchronisationMessage& in, Transform& q ) 
{
  bool isIdentity = false ;
  in >> isIdentity ;
  if( isIdentity )
  {
    q.resetToIdentity() ;
  }
  else
  {
    bool isUniformScale ;
    Wm4::Vector3f translate ;
    Wm4::Matrix3f matrix ;
    Wm4::Vector3f scale ;
    in >> isUniformScale 
       >> translate.X()
       >> translate.Y()
       >> translate.Z() 
       >> matrix( 0, 0 ) 
       >> matrix( 0, 1 ) 
       >> matrix( 0, 2 ) 
       >> matrix( 1, 0 ) 
       >> matrix( 1, 1 ) 
       >> matrix( 1, 2 ) 
       >> matrix( 2, 0 ) 
       >> matrix( 2, 1 ) 
       >> matrix( 2, 2 ) 
       >> scale.X()  
       >> scale.Y()  
       >> scale.Z() ;
    q.setTranslate( translate ) ;
    q.setRotate( matrix ) ;
    if( isUniformScale ) q.setUniformScale( scale.X() ) ; else q.setScale( scale ) ;
  }
  return in;
}

---