collisiondetectors/CD_LinearDiscrete.cpp

Go to the documentation of this file.

//## begin module%378A7A6703AB.cm preserve=no
//        %X% %Q% %Z% %W%
//## end module%378A7A6703AB.cm

//## begin module%378A7A6703AB.cp preserve=no
//## end module%378A7A6703AB.cp

//## Module: CD_LinearDiscrete%378A7A6703AB; Pseudo Package body
//## Source file: C:\project\mpk\code\CollisionDetectors\CD_LinearDiscrete.cpp

//## begin module%378A7A6703AB.additionalIncludes preserve=no
//## end module%378A7A6703AB.additionalIncludes

//## begin module%378A7A6703AB.includes preserve=yes
#include <math.h>
//## end module%378A7A6703AB.includes

// CD_LinearDiscrete
#include "CollisionDetectors\CD_LinearDiscrete.h"
// Universe
#include "Universe\Universe.h"
//## begin module%378A7A6703AB.additionalDeclarations preserve=yes
//## end module%378A7A6703AB.additionalDeclarations


// Class CD_LinearDiscrete 


CD_LinearDiscrete::CD_LinearDiscrete (const CD_LinearDiscrete& right)
  //## begin CD_LinearDiscrete::CD_LinearDiscrete%931798972.hasinit preserve=no
  //## end CD_LinearDiscrete::CD_LinearDiscrete%931798972.hasinit
  //## begin CD_LinearDiscrete::CD_LinearDiscrete%931798972.initialization preserve=yes
  :     
        CollisionDetectorBase( right ),
        CD_Linear( right )
  //## end CD_LinearDiscrete::CD_LinearDiscrete%931798972.initialization
{
  //## begin CD_LinearDiscrete::CD_LinearDiscrete%931798972.body preserve=yes
  //## end CD_LinearDiscrete::CD_LinearDiscrete%931798972.body
}

CD_LinearDiscrete::CD_LinearDiscrete (const Universe& universe)
  //## begin CD_LinearDiscrete::CD_LinearDiscrete%931798973.hasinit preserve=no
  //## end CD_LinearDiscrete::CD_LinearDiscrete%931798973.hasinit
  //## begin CD_LinearDiscrete::CD_LinearDiscrete%931798973.initialization preserve=yes
  :     
        CollisionDetectorBase( universe ),
        CD_Linear( universe )
  //## end CD_LinearDiscrete::CD_LinearDiscrete%931798973.initialization
{
  //## begin CD_LinearDiscrete::CD_LinearDiscrete%931798973.body preserve=yes

        //set up the resolution properly
        resolution.SetLength( universe.DOF() ) ;
        for( int i = 0; i < universe.DOF(); i++ )
        {
                double max = universe.JointMax( i ) ;
                double min = universe.JointMin( i ) ;
                resolution[ i ] = ( max - min ) / 100.0 ;
        }
  //## end CD_LinearDiscrete::CD_LinearDiscrete%931798973.body
}


CD_LinearDiscrete::~CD_LinearDiscrete()
{
  //## begin CD_LinearDiscrete::~CD_LinearDiscrete%.body preserve=yes
  //## end CD_LinearDiscrete::~CD_LinearDiscrete%.body
}



//## Other Operations (implementation)
void CD_LinearDiscrete::SetResolution (const VectorN& resolution)
{
  //## begin CD_LinearDiscrete::SetResolution%931798968.body preserve=yes
        this->resolution = resolution ;
  //## end CD_LinearDiscrete::SetResolution%931798968.body
}

void CD_LinearDiscrete::SetResolution (const unsigned int joint, const double resolution)
{
  //## begin CD_LinearDiscrete::SetResolution%931798969.body preserve=yes
        this->resolution[ joint ] = resolution ;
  //## end CD_LinearDiscrete::SetResolution%931798969.body
}

bool CD_LinearDiscrete::IsInterferingLinear (const Configuration& c1, const Configuration& c2)
{
  //## begin CD_LinearDiscrete::IsInterferingLinear%931798974.body preserve=yes
        IncrementLinearStat();
        int steps = 1 ;
                
        Configuration offset = ( c2 - c1 ) ;
        for( int i = 0; i < offset.DOF(); i++ )
        {
                double offsetI = offset[ i ];
                double resolutionI = resolution[ i ];
                int newSteps = abs( ceil( offsetI / resolutionI ) );
                if( steps < newSteps )
                {
                        steps = newSteps ;
                }
        }

        offset = offset / steps ;
        Configuration current = c1 ;
        for( i = 0; i <= steps; i++ )
        {
                if( IsInterfering( current ) )
                {
                        lastIntersection = current ;
                        lastValid = lastIntersection - offset;
                        return true ;
                }
                current += offset ;
        }
        lastValid = current;
        lastIntersection = current ;
        return false ;
  //## end CD_LinearDiscrete::IsInterferingLinear%931798974.body
}

// Additional Declarations
  //## begin CD_LinearDiscrete%378A7A6703AB.declarations preserve=yes
  //## end CD_LinearDiscrete%378A7A6703AB.declarations

//## begin module%378A7A6703AB.epilog preserve=yes
//## end module%378A7A6703AB.epilog

---