collisiondetectors/CD_JointLimits.cpp

Go to the documentation of this file.

//## begin module%37542C1B0011.cm preserve=no
//        %X% %Q% %Z% %W%
//## end module%37542C1B0011.cm

//## begin module%37542C1B0011.cp preserve=no
//## end module%37542C1B0011.cp

//## Module: CD_JointLimits%37542C1B0011; Pseudo Package body
//## Source file: C:\project\mpk\code\CollisionDetectors\CD_JointLimits.cpp

//## begin module%37542C1B0011.additionalIncludes preserve=no
//## end module%37542C1B0011.additionalIncludes

//## begin module%37542C1B0011.includes preserve=yes
#include <math.h>
#include <math/math2.h>
//## end module%37542C1B0011.includes

// Configuration
#include "kinematics\Configuration.h"
// CD_JointLimits
#include "CollisionDetectors\CD_JointLimits.h"
//## begin module%37542C1B0011.additionalDeclarations preserve=yes
//## end module%37542C1B0011.additionalDeclarations


// Class CD_JointLimits 

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



//## Other Operations (implementation)
double CD_JointLimits::DistanceBetween (const Configuration& c1, const Configuration& c2) const
{
  //## begin CD_JointLimits::DistanceBetween%930083383.body preserve=yes
        double distance = 0 ;
        for( int i = 0; i < c1.DOF(); i++ )
        {
                double addme = 0 ;
                double jmax = JointMax( i ) ;
                double jmin = JointMin( i ) ;
                double a1 = fabs( c2[ i ] - c1[ i ] ) ;
                if( JointWraps( i ) )
                {
                        double a2 = fabs( jmax - c1[ i ] ) + fabs( c2[ i ] - jmin ) ;
                        double a3 = fabs( jmax - c2[ i ] ) + fabs( c1[ i ] - jmin ) ;
                        addme = Min( a1, a2, a3 ) ;             //IMPROVE: this is a rapid funciton
                }
                else
                {
                        addme = a1 ;
                }
                distance += addme ;
        }
        return distance ;
  //## end CD_JointLimits::DistanceBetween%930083383.body
}

VectorN CD_JointLimits::JointDisplacement( const Configuration& c1, const Configuration& c2) const
{
        int dof = c1.DOF();
        VectorN displacement;

        displacement.SetLength( dof );

        double diff;

        for (int i=0; i < dof; i++ )
        {
                // Direct Linear Difference
                double diff1 = c2[i] - c1[i];

                double jrange = JointMax( i ) - JointMin( i );
                
                // If the joint wraps, it may be shorter to go over the joint limits and adjust them.
                if ( JointWraps( i ) )
                {
                        double diff2 = ( jrange + c2[i] ) - c1[i];
                        double diff3 = c2[i] - ( jrange + c1[i] );
                        diff = MinMag ( diff1, diff2, diff3 );
                }
                else
                {
                        diff = diff1;
                }

                // Assign the smallest displacment as the actual displacement between the two joint configs.
                displacement[i] = diff;

        }

        return displacement;
}


// Additional Declarations
  //## begin CD_JointLimits%37542C1B0011.declarations preserve=yes
  //## end CD_JointLimits%37542C1B0011.declarations

//## begin module%37542C1B0011.epilog preserve=yes
//## end module%37542C1B0011.epilog

---