basic/kinematics/FrameManager.cpp

Go to the documentation of this file.

//## begin module%3717CCD90370.cm preserve=no
//        %X% %Q% %Z% %W%
//## end module%3717CCD90370.cm

//## begin module%3717CCD90370.cp preserve=no
//## end module%3717CCD90370.cp

//## Module: FrameManager%3717CCD90370; Pseudo Package body
//## Source file: E:\mpk\code\Kinematics\FrameManager.cpp

//## begin module%3717CCD90370.additionalIncludes preserve=no
//## end module%3717CCD90370.additionalIncludes

//## begin module%3717CCD90370.includes preserve=yes
#pragma warning( disable : 4786 )
#include <set>
#include <list>
#include <algorithm>
#include <assert.h>
//## end module%3717CCD90370.includes

// Matrix4x4
#include <math\Matrix4x4.h>
// Frame
#include "geometry\Frame.h"
// FrameManager
#include "Kinematics\FrameManager.h"
//## begin module%3717CCD90370.additionalDeclarations preserve=yes
//## end module%3717CCD90370.additionalDeclarations


// Class FrameManager 








FrameManager::FrameManager ()
  //## begin FrameManager::FrameManager%924391854.hasinit preserve=no
  //## end FrameManager::FrameManager%924391854.hasinit
  //## begin FrameManager::FrameManager%924391854.initialization preserve=yes
  //## end FrameManager::FrameManager%924391854.initialization
{
  //## begin FrameManager::FrameManager%924391854.body preserve=yes
        assert( allFrames.size() == allWorldFrames.size() ) ;
        //the 0 frame is set to the identity matrix
        Frame identity ;
        allFrames.push_back( identity ) ;
        allWorldFrames.push_back( identity ) ;
        worldFrameValid.push_back( true ) ;
        assert( allFrames.size() == allWorldFrames.size() ) ;
  //## end FrameManager::FrameManager%924391854.body
}

FrameManager::FrameManager (const FrameManager& right)
  //## begin FrameManager::FrameManager%924978291.hasinit preserve=no
  //## end FrameManager::FrameManager%924978291.hasinit
  //## begin FrameManager::FrameManager%924978291.initialization preserve=yes
  : allFrames( right.allFrames ),
  allWorldFrames( right.allWorldFrames ),
  worldFrameValid( right.worldFrameValid )
  //## end FrameManager::FrameManager%924978291.initialization
{
  //## begin FrameManager::FrameManager%924978291.body preserve=yes
        assert( allFrames.size() == allWorldFrames.size() ) ;
        Clear() ;

        //copy all the values in the frame manager 
        for( int i = 0 ; i < right.allFrames.size() ; i++ )
        {
                allFrames.push_back( right.allFrames[ i ] ) ;
                allWorldFrames.push_back( right.allWorldFrames[ i ] ) ;
                worldFrameValid.push_back( right.worldFrameValid[ i ] ) ;
        }
        assert( allFrames.size() == allWorldFrames.size() ) ;
  //## end FrameManager::FrameManager%924978291.body
}


FrameManager::~FrameManager()
{
  //## begin FrameManager::~FrameManager%.body preserve=yes
        assert( allFrames.size() == allWorldFrames.size() ) ;
  //## end FrameManager::~FrameManager%.body
}



//## Other Operations (implementation)
int FrameManager::AddFrame ()
{
        //ensure that the sets of frames are the same size
        int allFramesSize = allFrames.size() ;
        int allWorldFramesSize = allWorldFrames.size() ;
        assert( allFramesSize == allWorldFramesSize ) ;

        Frame blank ;
        allFrames.push_back( blank ) ;
        allWorldFrames.push_back( blank ) ;
        worldFrameValid.push_back( false ) ;    //frame does not initally start valid
        int afs = allFrames.size() ;
        int awfs= allWorldFrames.size() ;
        int wfv = worldFrameValid.size() ;
        assert( allFrames.size() == allWorldFrames.size() ) ;
        int returnMe = allFrames.size() - 1;
        return( returnMe ) ;
  //## end FrameManager::AddFrame%924306824.body
}

unsigned int FrameManager::GetNumberOfFrames () const
{
  //## begin FrameManager::GetNumberOfFrames%924391848.body preserve=yes
        return allFrames.size() ;
  //## end FrameManager::GetNumberOfFrames%924391848.body
}

//=============================================================================
// GetToolFrame
//
// Purpose: allows access to the tool frame
//=============================================================================
Matrix4x4 FrameManager::GetToolFrame( const int index ) const
{
        assert( false ); //this is never going to work
        assert( index == 0 );
        int numberOfFrames = GetNumberOfFrames();
        Matrix4x4 returnme = GetWorldFrame( numberOfFrames - 1 );
        return returnme;
}

Frame FrameManager::operator [] (const unsigned int index) const
{
  //## begin FrameManager::operator[]%924391849.body preserve=yes
        assert( false ) ;
        return( allFrames[ index ] ) ;
  //## end FrameManager::operator[]%924391849.body
}

bool FrameManager::ValidateFrame (const unsigned int frame) const
{
  //## begin FrameManager::ValidateFrame%924391855.body preserve=yes
        // Detached code regions:
        //check all the frames beneath this one to see if any of them are the root
        std::set< unsigned int > frames ;
        frames.insert( frame ) ;

        unsigned int lastChecked = frame ;
        while( lastChecked != 0 )
        {
                if( frames.find( lastChecked ) != frames.end() ) 
                {
                        assert( false ) ;
                        return false ;
                }
                frames.insert( lastChecked ) ;
                lastChecked = GetFrame(lastChecked).BaseFrameNum() ;
        }
        return true ;
  //## end FrameManager::ValidateFrame%924391855.body
}

Frame* FrameManager::GetFrameRef (const unsigned int number)
{
  //## begin FrameManager::GetFrameRef%925158794.body preserve=yes
        int allFramesSize = allFrames.size() ;
        assert( number < allFramesSize ) ;
        assert( number >= 0 ) ;
        return( &allFrames[ number ] ) ;
  //## end FrameManager::GetFrameRef%925158794.body
}

FrameManager& FrameManager::operator = (const FrameManager& right)
{
  //## begin FrameManager::operator=%925235860.body preserve=yes
        assert( allFrames.size() == allWorldFrames.size() ) ;
        if( &right == this )
        { 
                return *this ;
        }
        
        //copy all the frames ;
        allFrames.clear() ;
        allWorldFrames.clear() ;
        worldFrameValid.clear() ;
        for( int i = 0 ; i < right.allFrames.size() ; i++ )
        {
                allFrames.push_back( right.allFrames[ i ] ) ;
                allWorldFrames.push_back( right.allWorldFrames[ i ] ) ;
                worldFrameValid.push_back( right.worldFrameValid[ i ] ) ;
        }
        assert( allFrames.size() == allWorldFrames.size() ) ;
        return *this ;
  //## end FrameManager::operator=%925235860.body
}

Matrix4x4 FrameManager::GetTransformRelative (const unsigned int frame, const unsigned int base) const
{
  //## begin FrameManager::GetTransformRelative%925324832.body preserve=yes
        assert( allFrames.size() == allWorldFrames.size() ) ;
        const Matrix4x4& mf = GetWorldFrame( frame ) ;
        if( base == 0 )
        {
                return mf ;
        }
        else
        {
                Matrix4x4 mb = GetWorldFrame( base ) ;  
                return mf * mb.Inverse();               //IMPROVE: order of multiplication
        }
        
/*      
        //find a common base frame to both these frames
        unsigned int commonBase = 0 ;
        std::list< unsigned int > frameHistory ;
        unsigned int addme = frame ;
        do
        {
                frameHistory.insert( frameHistory.begin(), addme ) ;
                addme = BaseFrame( addme ) ;
        } while( addme != 0 ) ;
        frameHistory.insert( frameHistory.begin(), 0 ) ;
        
        //check the base frame, and all its parents for commonality
        std::list< unsigned int > baseFrameList ;
        unsigned int root = base ;
        while( std::find( frameHistory.begin(), frameHistory.end(), root ) == frameHistory.end() ) 
        {
                baseFrameList.insert( baseFrameList.begin(), root ) ;
                root = BaseFrame( root ) ;      //IMPROVE: given 3 and 1 this doesn't work right
                                                                        //IMPROVE: given 1 and 0 this doesn't work right either
        }
        baseFrameList.insert( baseFrameList.begin(), root ) ;
        commonBase = root ;

        //now we know that there's a common frame that we can use to relate these frames in 
        
        Frame relative ;        //this is the frame we're looking at
        //multiply back to the common frame by the inverse of frames from the frame backwards
        unsigned int frameNum = frame ;
        while( frameNum != commonBase )
        {
                Frame multiplyMe = allFrames[ frameNum ] ;
                relative = multiplyMe * relative ;
                frameNum = BaseFrame( frameNum ) ;
        }
        
        //multiply out to the base frame
        Frame toBase ;  //this represents the base frame
        unsigned int baseNum = base ;
        while( baseNum != commonBase )
        {
                Frame multiplyMe = allFrames[ baseNum ] ;
                toBase = multiplyMe * toBase ;
                baseNum = BaseFrame( baseNum ) ;
        }

        return toBase.Inverse() * relative ;
*/
  //## end FrameManager::GetTransformRelative%925324832.body
}

Matrix4x4 FrameManager::GetTransformRelative (const Matrix4x4& frame, const Matrix4x4& base )
{
        Matrix4x4 baseInverse = base.Inverse();
        return baseInverse * frame;             // Shane:  This is now the correct order of multiplication.  DO NOT CHANGE!
                                                                        //  Tba = Tbc * Tca = (Tcb)^-1 * Tca
                                                                        //  "aWRTb" = "cWRTb" * "aWRTb"
}

unsigned int FrameManager::BaseFrame (const unsigned int frame) const
{
  //## begin FrameManager::BaseFrame%925324835.body preserve=yes
        assert( frame >= 0 ) ;
        assert( frame < allFrames.size() ) ;
        Frame frameInQuestion = allFrames[ frame ] ;
        return frameInQuestion.BaseFrameNum() ;
  //## end FrameManager::BaseFrame%925324835.body
}

void FrameManager::SetBaseFrame (const unsigned int frame, const unsigned int baseFrame)
{
  //## begin FrameManager::SetBaseFrame%925703339.body preserve=yes
        allFrames[ frame ].SetBaseFrame( baseFrame ) ;
  //## end FrameManager::SetBaseFrame%925703339.body
}

void FrameManager::Clear ()
{
  //## begin FrameManager::Clear%927844452.body preserve=yes
        allFrames.clear() ;
        allWorldFrames.clear() ;
        worldFrameValid.clear() ;

        //the 0 frame is set to the identity matrix             //IMPROVE: this is duplicated in the constructor
        Frame identity ;                
        allFrames.push_back( identity ) ;
        allWorldFrames.push_back( identity ) ;
        worldFrameValid.push_back( true ) ;

  //## end FrameManager::Clear%927844452.body
}

void FrameManager::MarkFrameChanged (unsigned int frameNum)
{
  //## begin FrameManager::MarkFrameChanged%932581525.body preserve=yes
        //this frame has changed, so the world frames associated with it also have to change
        if( frameNum != 0 )
        {
                worldFrameValid[ frameNum ] = false ;
        }

  //## end FrameManager::MarkFrameChanged%932581525.body
}

//=============================================================================
// MarkToolFrame
//
// Purpose: Marks a particular frame as a tool frame
//=============================================================================
void FrameManager::MarkToolFrame( const int index )
{
        m_ToolFrames.insert( index );
}

Frame FrameManager::GetFrame (const unsigned int frameNum) const
{
  //## begin FrameManager::GetFrame%932581526.body preserve=yes
        return ( allFrames[ frameNum ] ) ;      //IMPROVE: is it wise to return an object? reference better?
  //## end FrameManager::GetFrame%932581526.body
}

Matrix4x4 FrameManager::GetWorldFrame (const unsigned int frameNum) const
{
  //## begin FrameManager::GetWorldFrame%932581527.body preserve=yes
        if( frameNum == 0 )
        {
                assert( worldFrameValid[ frameNum ] == true );
        }
        if( worldFrameValid[ frameNum ] == false )
        {
                //recompute frames that are invalid
                unsigned int baseFrame      = allFrames[ frameNum ].BaseFrameNum() ;            
        Matrix4x4 baseFrameMatrix   = GetWorldFrame( baseFrame );
        Matrix4x4 frameMatrix       = allFrames[ frameNum ];
                allWorldFrames[ frameNum ]  =  baseFrameMatrix * frameMatrix;
                worldFrameValid[ frameNum ] = true ;
        }
        Matrix4x4 returnme = allWorldFrames[ frameNum ] ;
        return( allWorldFrames[ frameNum ] );

  //## end FrameManager::GetWorldFrame%932581527.body
}

void FrameManager::MarkAllFramesChanged ()
{
  //## begin FrameManager::MarkAllFramesChanged%932581528.body preserve=yes
        for( int i = 1; i < worldFrameValid.size(); i++ )
        {
                worldFrameValid[ i ] = false ;
        }
  //## end FrameManager::MarkAllFramesChanged%932581528.body
}

// Additional Declarations
  //## begin FrameManager%3717CCD90370.declarations preserve=yes
  //## end FrameManager%3717CCD90370.declarations

//## begin module%3717CCD90370.epilog preserve=yes
//## end module%3717CCD90370.epilog

---