collisiondetectors/CD_LinearContinuous.cpp

Go to the documentation of this file.

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

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

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

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

//## begin module%378A7A4B03DD.includes preserve=yes
//## end module%378A7A4B03DD.includes

#include <assert.h>
#include "CollisionDetectors\CD_LinearContinuous.h"
// Universe
#include "Universe\Universe.h"
//## begin module%378A7A4B03DD.additionalDeclarations preserve=yes
//## end module%378A7A4B03DD.additionalDeclarations

static const int maxSteps = 32;
static const double cspaceResolution = 0.5;

// Class CD_LinearContinuous 

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

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


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

bool CD_LinearContinuous::IsCompletelyWithinObstacles(const Configuration& c1, const Configuration& c2)
{
    return IsCompletelyWithinObstaclesNoWrapping( c1, c2 );
}

//## Other Operations (implementation)
bool CD_LinearContinuous::IsInterferingLinear (const Configuration& c1, const Configuration& c2)
{
        IncrementLinearStat();
        if ( useWrapping )
        {
                return IsInterferingLinearWrapping( c1, c2 );
        }
        else
        {
                return IsInterferingLinearNoWrapping( c1, c2 );
        }
}

bool CD_LinearContinuous::IsCompletelyWithinObstaclesNoWrapping (const Configuration& c1, const Configuration& c2)
{
        //const int steps = maxSteps ;          //IMPROVE: this is not reliable - put it elsewhere
        //Configuration offset = ( c2 - c1 ) / ( steps - 1 ) ;
        
        Configuration offset = c2 - c1;
        int steps = offset.Magnitude()/cspaceResolution + 2;
        if (steps > maxSteps)
                steps = maxSteps;
        offset /= (steps-1);    
        Configuration current = c1 ;
        for( int i = 0; i < steps; i++ )
        {
                if( !IsInterfering( current ) )
                {
                        lastIntersection = current ;
                        lastValid = lastIntersection - offset;
                        return false ;
                }
                current += offset ;
        }
        lastValid = current;
        lastIntersection = current ;
        return true ;
}

bool CD_LinearContinuous::IsCompletelyWithinObstaclesWrapping (const Configuration& c1, const Configuration& c2)
{
    assert( false ); //this doesn't work yet
    return false;
}


bool CD_LinearContinuous::IsInterferingLinearNoWrapping (const Configuration& c1, const Configuration& c2)
{
        //const int steps = maxSteps ;          //IMPROVE: this is not reliable - put it elsewhere
        //Configuration offset = ( c2 - c1 ) / ( steps - 1 ) ;
        Configuration offset = c2 - c1;
        int steps = offset.Magnitude()/cspaceResolution + 2;
        if (steps > maxSteps)
                steps = maxSteps;
        offset /= (steps-1);    
        Configuration current = c1 ;
        for( int i = 0; i < steps; i++ )
        {
                if( IsInterfering( current ) )
                {
                        lastIntersection = current ;
                        lastValid = lastIntersection - offset;
                        return true ;
                }
                current += offset ;
        }
        lastValid = current;
        lastIntersection = current ;
        return false ;
}

bool CD_LinearContinuous::IsInterferingLinearWrapping (const Configuration& c1, const Configuration& c2)
{
        assert( false ); //IAN needs to look into this some more
        VectorN stepdisp = JointDisplacement( c1, c2 ) / ( maxSteps ); 
        Configuration current = c1;

        int dof = c1.DOF();

        // Check starting configuration 
        if( IsInterfering( current ) )
        {
                lastIntersection = current;
                return true;
        }

        // Check all intermediate configurations and final config. ( c2 = c1 + stepdisp*maxsteps )
        for (int step = 0; step < maxSteps; step++ )
        {
                // advance configuration by the displacement
                current += stepdisp;

                // check if any of the joints went over the limits and correct.  (Limits will only
                // be exceeded if joints were wrapping.
                for (int i = 0; i < dof; i++)
                {
                        double jmin = JointMin( i );
                        double jmax = JointMax( i );
                        if ( current[i] < jmin )
                        {
                                current[i] += (jmax - jmin);    // add joint range ccw to have joint wrap over lower limit.
                        }
                        else if ( current[i] > jmax )
                        {
                                current[i] -= (jmax - jmin);    // add joint range cw to have joint wrap back under upper limit.
                        }
                }

                // check the new config for collision
                if( IsInterfering( current ) )
                {
                        lastIntersection = current;
                        return true;
                }

        }

        // If still in function, then we have checked all configuration between start and goal config
        // linearly, through the shortest wrapping displacement.

        return false;

}

// Additional Declarations
  //## begin CD_LinearContinuous%378A7A4B03DD.declarations preserve=yes
  //## end CD_LinearContinuous%378A7A4B03DD.declarations

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

---