server/ServerBase.cpp

Go to the documentation of this file.

//## begin module%37AB2C4702C6.cm preserve=no
//        %X% %Q% %Z% %W%
//## end module%37AB2C4702C6.cm

//## begin module%37AB2C4702C6.cp preserve=no
//## end module%37AB2C4702C6.cp

//## Module: ServerBase%37AB2C4702C6; Pseudo Package body
//## Source file: C:\project\mpk\code\MPKserver\ServerBase.cpp

//## begin module%37AB2C4702C6.additionalIncludes preserve=no
//## end module%37AB2C4702C6.additionalIncludes

//## begin module%37AB2C4702C6.includes preserve=yes
#pragma warning( disable : 4786 )
#include "debug/debug.h"
#include <collisionDetectors/CD_Swiftpp.h>
//#define CD_Swiftpp  CD_Vcollide

#include <collisionDetectors/CD_Vcollide.h>
#include <collisionDetectors/CD_Simple.h>
#include <collisionDetectors/CD_Solid.h>
#include <collisionDetectors/CD_Icollide.h>
#include <collisionDetectors/CD_rangesensor\CD_Range_Sensor.h>
#include <collisionDetectors/collisiondetectortype.h>
#include <planners/plannertype.h>
#include <planners/prm/pl_prmijg.h>
#include <planners/obsolete/pl_linear.h>
#include <planners/obsolete/pl_ianrandom.h>
#include <planners/obsolete/pl_SimplexSubdivision.h>
#include <planners/fortest/pl_ForTest.h>
#include <planners/neural/pl_neural.h>
#include <planners/rrt/pl_RRT.h>
#include <planners/rrt/pl_RRT_Connect.h>
#include <planners/aca/pl_ACA_Connect.h>
#include <planners/rangesensor/PL_Range_Sensor.h>
#include <planners/ik_mpep/pl_mpep.h>
#include <planners/atace/pl_atace.h>
#include <smoothers/SM_PathSmoothingSuccessiveNodePair.h>
#include <smoothers/SM_PathSmoothingRandomNodePair.h>
#include <smoothers/SM_SuccessiveAndRandomSmoothing.h>
#include <smoothers/SM_Trisection.h>
#include <robots/robotbase.h>
#include <serializable/MPK_Dir.h>


#include <string/string2.h>

#ifdef WIN32
    #include <direct.h>
#endif
//## end module%37AB2C4702C6.includes

// CD_BasicStyle
#include "CollisionDetectors/CD_BasicStyle.h"
// ObjectBase
#include "geometry/ObjectBase.h"
// VRML_20_Reader
#include "geometry/VRML_20_Reader.h"
// VRML_Reader
#include "geometry/VRML_Reader.h"
// DH_Link
#include "Kinematics/DH_Link.h"
// R_OpenChain
#include "robots/R_OpenChain.h"
// ServerBase
#include "server/ServerBase.h"
// IfstreamWithComments
#include "streams/IfstreamWithComments.h"
// Jacobian-based Inverse Kinematics Planner
#include "planners/ik_mpep/IK_Jacobian.h"
// Inverse Kinematics Planners
#include "planners/inversekin/IK_InvKinBase.h"
// PL_Juan
#include "Planners/aca/PL_Juan.h"
// PL_Sequential
#include "Planners/sequential/PL_Sequential.h"
// PL_Sandros
#include "Planners/sandros/PL_Sandros.h"


// IMPROVE:  These header files should be included last because the LEDA libraries they include, redefines 'std' class and causes
//                       compatablilty errors with the other header files.
// PL_PRM
#include "Planners/prm/pl_prm.h"
// PL_Astar
#include "Planners/astar/PL_Astar.h"
// IK_ACA
#include "planners/ik_aca/IK_ACA.h"

//## begin module%37AB2C4702C6.additionalDeclarations preserve=yes
//## end module%37AB2C4702C6.additionalDeclarations

#include "planners/closedchain/PL_prm_closedchain.h"
#include "planners/closedchain/PL_rrt_closedchain.h"
#include "planners/atace/PL_rgd_constraint.h"

// Class ServerBase 












ServerBase::ServerBase ()
  //## begin ServerBase::ServerBase%934854390.hasinit preserve=no
: 
        collisionDetectorType(CD_NONE), 
        planPassed(false), 
        plannerType(PLANNER_NONE), 
        collisionDetector(NULL), 
        planner(NULL),
        smoother(NULL)
  //## end ServerBase::ServerBase%934854390.hasinit
  //## begin ServerBase::ServerBase%934854390.initialization preserve=yes
  //## end ServerBase::ServerBase%934854390.initialization
{
        strcpy( filename, "NONE" );
        strcpy( obstacleFilename, "NONE" );
    strcpy( rootpath, "" );
        this->SetCollisionDetector( CD_VCOLLIDE );
        this->SetPlanner( PLANNER_NONE );
        this->SetSmoother(SMOOTHER_NONE);
//      P_Aca();
  //## end ServerBase::ServerBase%934854390.body
}


ServerBase::~ServerBase()
{
  //## begin ServerBase::~ServerBase%.body preserve=yes
        delete collisionDetector;
        collisionDetector = NULL;
        delete planner;
        planner = NULL;
        delete smoother;
        smoother = NULL;
  //## end ServerBase::~ServerBase%.body
}


void *ServerBase::GetPlannerParameters()
{
        if (planner)
                return planner->GetParameters();
        else
                return NULL;
}

bool ServerBase::SetPlannerParameters(const void *param)
{
        if (planner)
                return planner->SetParameters(param);
        else
                return false;
}

//## Other Operations (implementation)
void ServerBase::R_ThreeCylinderRobot ()
{
  //## begin ServerBase::R_ThreeCylinderRobot%932600045.body preserve=yes
        assert( false );                //IMPROVE: ian doesn't want this function called any more
        universe.Empty();
        //IMPROVE: check to see if this is valid file first
        this->ParseRobotFile( "..\\robots\\threeCylinderRobot.txt" );
  //## end ServerBase::R_ThreeCylinderRobot%932600045.body
}

bool ServerBase::ParseRobotFile (const char* filename)
{
  //## begin ServerBase::ParseRobotFile%935034310.body preserve=yes
        if( strcmp( filename, "NONE" ) == 0 )
        {
                return true;
        }

    char file[ _MAX_PATH ] = "";
    IJG_Assert( strcmp( rootpath, "NONE" ) != 0 );
    //strcat( file, rootpath );
    //strcat( file, filename );
        GetAbsoluteFileName(file, _MAX_PATH, rootpath, filename);
        IfstreamWithComments is;
        is.open( file );
        if( is.is_open() == false )
        {
                IJG_AssertMsg( false, "Could not open file" );
                return false;
        }

        char line[ 1000 ] = "";

        //here comes the robot part
        bool success = true;
        FrameManager* theFrameManager = this->universe.GetFrameManager();
        RobotBase* robot = RobotBase::DeserializeAbstract( is, theFrameManager );
        if( robot != NULL )
        {
                universe.AddEntity( robot );
                delete robot;
        }
        else
        {
                success = false;
        }

        is.close();

        strcpy( this->filename, filename ); 
        this->RefreshCollisionDetector();
        return success;
  //## end ServerBase::ParseRobotFile%935034310.body
}

void ServerBase::ParseRobotOpenChain (IfstreamWithComments& is)
{
  //## begin ServerBase::ParseRobotOpenChain%935084508.body preserve=yes
        assert( false ); //this should never be called
  //## end ServerBase::ParseRobotOpenChain%935084508.body
}

bool ServerBase::ParseObstacleFile (const char* filename)
{
  //## begin ServerBase::ParseObstacleFile%936229638.body preserve=yes
        if( stricmp( filename, "none" ) == 0 )
        {
                return false;
        }

        strcpy( this->obstacleFilename, filename );
        VRML_20_Reader reader;
        char newFilename[ 1000 ] = "";
        //strcpy( newFilename, filename );
        //MPKstring::TrimLeadingWhitespace( newFilename );
        GetAbsoluteFileName(newFilename, _MAX_PATH, rootpath, filename);
        reader.SetFilename( newFilename );
        
        bool success = reader.Read();
        if( success )
        {
                //ObjectBase* environment = dynamic_cast< ObjectBase* >( reader.GetObjectA()->Clone() );
                ObjectBase* environment = dynamic_cast< ObjectBase* >( reader.GetTheObject()->Clone() );
                universe.AddEntity( environment );
                delete environment;
                environment = NULL;
        }
        return success;
  //## end ServerBase::ParseObstacleFile%936229638.body
}

bool ServerBase::SetCollisionDetector (CollisionDetectorType collisionDetectorType)
{
  //## begin ServerBase::SetCollisionDetector%950474924.body preserve=yes
        //update the collision detector only if we need to
        if( this->collisionDetectorType != collisionDetectorType )
        {
                this->collisionDetectorType = collisionDetectorType;
                this->RefreshCollisionDetector();
        }
        return true;
  //## end ServerBase::SetCollisionDetector%950474924.body
}

bool ServerBase::RefreshCollisionDetector ()
{
  //## begin ServerBase::RefreshCollisionDetector%950474925.body preserve=yes
        delete collisionDetector;
        collisionDetector = NULL;
        GL_Universe& mpkUniverse = universe;
        switch( collisionDetectorType )
        {
                case CD_SIMPLE :
                {
                        collisionDetector = new CD_Simple( mpkUniverse );
                        break;
                }
                case CD_VCOLLIDE :
                {
                        collisionDetector = new CD_Vcollide( mpkUniverse );

            // this is to test link scaling
            RobotBase* robot = collisionDetector->GetRobot( 0 );
            if( robot != NULL )
            {
                LinkBase* link = robot->GetLink( 0 );
            }
                        break;
                }
                case CD_SOLID :
                {
                        collisionDetector = new CD_Solid( mpkUniverse );
                        break;
                }
                case CD_ICOLLIDE :
                {
                        collisionDetector = new CD_Icollide( mpkUniverse );
                        break;
                }
                case CD_RANGE_SENSOR :
                {
                        collisionDetector = new CD_Range_Sensor( mpkUniverse );
                        break;
                }
                case CD_SWIFTPP :
                {
                        collisionDetector = new CD_Swiftpp( mpkUniverse );

            // this is to test link scaling
            //RobotBase* robot = collisionDetector->GetRobot( 0 );
            //if( robot != NULL )
            //{
            //    LinkBase* link = robot->GetLink( 0 );
            //}
                        break;
                }
                default :
                {
                        bool noCollisionDetectorTypeSpecified = false;
//                      assert( noCollisionDetectorTypeSpecified );
                        break;
                }
        }

        this->RefreshPlanner();

        if( collisionDetector == NULL )
        {
                return false;
        }
        else
        {
                return true;
        }
  //## end ServerBase::RefreshCollisionDetector%950474925.body
}

bool ServerBase::RefreshRobotFile ()
{
  //## begin ServerBase::RefreshRobotFile%950480128.body preserve=yes
        universe.Empty();
        return ParseRobotFile( filename );
  //## end ServerBase::RefreshRobotFile%950480128.body
}

const RobotBase* ServerBase::GetRobot (const char* const filename) const
{
  //## begin ServerBase::GetRobot%950480129.body preserve=yes
        const Entity* entity = universe.GetEntity( filename );  //IMPROVE: it's not a filename, its a robot name
        const RobotBase* returnMe = dynamic_cast< const RobotBase* >( entity );
        return( returnMe );     //this will be null if the name is bad or the robot is not found
  //## end ServerBase::GetRobot%950480129.body
}

const ObjectBase* ServerBase::GetMPKObject (const char* const filename) const
{
  //## begin ServerBase::GetMPKObject%950480130.body preserve=yes
        assert( false );
        return( NULL );
  //## end ServerBase::GetMPKObject%950480130.body
}

const Frame ServerBase::GetFrame (const char* const name) const
{
  //## begin ServerBase::GetFrame%950480131.body preserve=yes
        assert( false );
        Frame returnMe;
        return( returnMe );
  //## end ServerBase::GetFrame%950480131.body
}

bool ServerBase::SetPlanner (PlannerType plannerType)
{
  //## begin ServerBase::SetPlanner%958847982.body preserve=yes

        //if the planner has changed then update it
        if( this->plannerType != plannerType )
        {
                this->plannerType = plannerType;
                bool success = this->RefreshPlanner();
                return success;
        }
        return true;
  //## end ServerBase::SetPlanner%958847982.body
}

bool ServerBase::RefreshPlanner ()
{
        collisionDetector->ResetStats();
        switch( this->plannerType )
        {
        case PLANNER_LINEAR:
                {
                        PL_Linear* tempPlanner = new( PL_Linear );
                        //IMPROVE: does this cause a memory leak?
                        CD_BasicStyle* cd = dynamic_cast< CD_BasicStyle* >( collisionDetector );
                        if( cd != NULL )
                        {
                                tempPlanner->SetCollisionDetector( cd );        //IMPROVE: do the dynamic casting inside the function
                                delete planner;
                                planner = tempPlanner;
                        }
                        else
                        {
                                delete tempPlanner;
                                delete planner;
                                planner = NULL;
                                return false;
                        }
                        break;
                };
        case PLANNER_SEQUENTIAL:
                {
                        PL_Sequential* tempPlanner = new( PL_Sequential );
                        tempPlanner->initialize_parameters();
                        CD_BasicStyle* cd = dynamic_cast< CD_BasicStyle* >( collisionDetector );
                        if( cd != NULL )
                        {
                                tempPlanner->SetCollisionDetector( cd );
                                delete planner;
                                planner = tempPlanner;
                        }
                        else
                        {
                                delete tempPlanner;
                                delete planner;
                                planner = NULL;
                                return false;
                        }
                        break;
                }
        case PLANNER_ACA:
                {
                        PL_Juan* tempPlanner = new( PL_Juan );
                        CD_BasicStyle* cd = dynamic_cast< CD_BasicStyle* >( collisionDetector );
                        if( cd != NULL )
                        {
                                tempPlanner->SetCollisionDetector( cd );
                                delete planner;
                                planner = tempPlanner;
                        }
                        else
                        {
                                delete tempPlanner;
                                delete planner;
                                planner = NULL;
                                return false;
                        }
                        break;
                }
        case PLANNER_RANDOM:
                {
                        PL_IanRandom* tempPlanner = new( PL_IanRandom );
                        CD_BasicStyle* cd = dynamic_cast< CD_BasicStyle* >( collisionDetector );
                        if( cd != NULL )
                        {
                                tempPlanner->SetCollisionDetector( cd );
                                delete planner;
                                planner = tempPlanner;
                        }
                        else
                        {
                                delete tempPlanner;
                                delete planner;
                                planner = NULL;
                                return false ;
                        }
                        break;
                }
        case PLANNER_JONATHAN:
                {
                        PL_Range_Sensor* tempPlanner = new( PL_Range_Sensor );
                        CD_Range_Sensor* cd = dynamic_cast< CD_Range_Sensor* >( collisionDetector );
                        if( cd != NULL )
                        {
                                tempPlanner->SetCollisionDetector( cd );
                                delete planner;
                                planner = tempPlanner;
                        }
                        else
                        {
                                delete tempPlanner;
                                delete planner;
                                planner = NULL;
                                return false;
                        }
                        break;
                }
        case PLANNER_SIMPLEX_SUBDIVISION:
                {
                        PL_SimplexSubdivision* tempPlanner = new( PL_SimplexSubdivision );
                        CD_BasicStyle* cd = dynamic_cast< CD_BasicStyle* >( collisionDetector );        //IAN IMPROVE: fix the collision detector hierarchy so that things properly derive from linear, etc
                        if( cd != NULL )
                        {
                                tempPlanner->SetCollisionDetector( cd );
                                delete planner;
                                planner = tempPlanner;
                        }
                        else
                        {
                                delete tempPlanner;
                                delete planner;
                                planner = NULL;
                                return false;
                        }
                        break;
                }

        case PLANNER_PRM:
                {
                        PL_PRM* tempPlanner = new( PL_PRM );
                        tempPlanner->CopySettings( planner );

                        CD_BasicStyle* cd = dynamic_cast< CD_BasicStyle* >( collisionDetector );        //IAN IMPROVE: fix the collision detector hierarchy so that things properly derive from linear, etc
                        if( cd != NULL )
                        {
                                // Assign the Collision Detector
                                tempPlanner->SetCollisionDetector( cd );

                                delete planner;
                                planner = tempPlanner;
                        }
                        else
                        {
                                delete tempPlanner;
                                delete planner;
                                planner = NULL;
                                return false;
                        }
                        break;
                }
        case PLANNER_PRM_CLOSEDCHAIN:
                {
                        PL_PRM_ClosedChain* tempPlanner = new( PL_PRM_ClosedChain );
                        tempPlanner->CopySettings( planner );

                        CD_BasicStyle* cd = dynamic_cast< CD_BasicStyle* >( collisionDetector );        //IAN IMPROVE: fix the collision detector hierarchy so that things properly derive from linear, etc
                        if( cd != NULL )
                        {
                                // Assign the Collision Detector
                                tempPlanner->SetCollisionDetector( cd );

                                delete planner;
                                planner = tempPlanner;
                        }
                        else
                        {
                                delete tempPlanner;
                                delete planner;
                                planner = NULL;
                                return false;
                        }
                        break;
                }
        case PLANNER_RRT_CLOSEDCHAIN:
                {
                        PL_RRT_ClosedChain* tempPlanner = new( PL_RRT_ClosedChain );
                        tempPlanner->CopySettings( planner );

                        CD_BasicStyle* cd = dynamic_cast< CD_BasicStyle* >( collisionDetector );        //IAN IMPROVE: fix the collision detector hierarchy so that things properly derive from linear, etc
                        if( cd != NULL )
                        {
                                // Assign the Collision Detector
                                tempPlanner->SetCollisionDetector( cd );

                                delete planner;
                                planner = tempPlanner;
                        }
                        else
                        {
                                delete tempPlanner;
                                delete planner;
                                planner = NULL;
                                return false;
                        }
                        break;
                }
        case PLANNER_RGD_CONSTRAINT:
                {
                        PL_RGD_Constraint* tempPlanner = new( PL_RGD_Constraint );
                        tempPlanner->CopySettings( planner );

                        CD_BasicStyle* cd = dynamic_cast< CD_BasicStyle* >( collisionDetector );        //IAN IMPROVE: fix the collision detector hierarchy so that things properly derive from linear, etc
                        if( cd != NULL )
                        {
                                // Assign the Collision Detector
                                tempPlanner->SetCollisionDetector( cd );

                                delete planner;
                                planner = tempPlanner;
                        }
                        else
                        {
                                delete tempPlanner;
                                delete planner;
                                planner = NULL;
                                return false;
                        }
                        break;
                }
        case PLANNER_ACA_CONNECT:
                {
                        PL_ACA_CONNECT* tempPlanner = new( PL_ACA_CONNECT );
                        CD_BasicStyle* cd = dynamic_cast< CD_BasicStyle* >( collisionDetector );
                        if( cd != NULL )
                        {
                                tempPlanner->SetCollisionDetector( cd );
                                delete planner;
                                planner = tempPlanner;
                        }
                        else
                        {
                                delete tempPlanner;
                                delete planner;
                                planner = NULL;
                                return false;
                        }
                        break;
                }
        case PLANNER_RRT:
                {
                        PL_RRT* tempPlanner = new( PL_RRT );
                        CD_BasicStyle* cd = dynamic_cast< CD_BasicStyle* >( collisionDetector );
                        if( cd != NULL )
                        {
                                tempPlanner->SetCollisionDetector( cd );
                                delete planner;
                                planner = tempPlanner;
                        }
                        else
                        {
                                delete tempPlanner;
                                delete planner;
                                planner = NULL;
                                return false;
                        }
                        break;
                }
        case PLANNER_RRT_CONNECT:
                {
                        PL_RRT_CONNECT* tempPlanner = new( PL_RRT_CONNECT);
                        CD_BasicStyle* cd = dynamic_cast< CD_BasicStyle* >( collisionDetector );
                        if( cd != NULL )
                        {
                                tempPlanner->SetCollisionDetector( cd );
                                delete planner;
                                planner = tempPlanner;
                        }
                        else
                        {
                                delete tempPlanner;
                                delete planner;
                                planner = NULL;
                                return false;
                        }
                        break;
                }
        case PLANNER_PRM_IJG:
                {
                        PL_PrmIjg* tempPlanner = new PL_PrmIjg;
                        //tempPlanner->CopySettings( planner );

                        CD_BasicStyle* cd = dynamic_cast< CD_BasicStyle* >( collisionDetector );        //IAN IMPROVE: fix the collision detector hierarchy so that things properly derive from linear, etc
                        if( cd != NULL )
                        {
                                // Assign the Collision Detector
                                tempPlanner->SetCollisionDetector( cd );

                                delete planner;
                                planner = tempPlanner;
                        }
                        else
                        {
                                delete tempPlanner;
                                delete planner;
                                planner = NULL;
                                return false;
                        }
                        break;
                }
        case PLANNER_SANDROS:
                {
                        PL_Sandros* tempPlanner = new( PL_Sandros );
                        CD_BasicStyle* cd = dynamic_cast< CD_BasicStyle* >( collisionDetector );        //IAN IMPROVE: fix the collision detector hierarchy so that things properly derive from linear, etc
                        if( cd != NULL )
                        {
                                // Assign the Collision Detector
                                tempPlanner->SetCollisionDetector( cd );

                                delete planner;
                                planner = tempPlanner;
                        }
                        else
                        {
                                delete tempPlanner;
                                delete planner;
                                planner = NULL;
                                return false;
                        }
                        break;
                }
        case PLANNER_ASTAR:
                {
                        PL_Astar* tempPlanner = new( PL_Astar );
                        tempPlanner->CopySettings( planner );
                        CD_BasicStyle* cd = dynamic_cast< CD_BasicStyle* >( collisionDetector );        //IAN IMPROVE: fix the collision detector hierarchy so that things properly derive from linear, etc
                        if( cd != NULL )
                        {
                                // Assign the Collision Detector
                                tempPlanner->SetCollisionDetector( cd );

                                delete planner;
                                planner = tempPlanner;
                        }
                        else
                        {
                                delete tempPlanner;
                                delete planner;
                                planner = NULL;
                                return false;
                        }
                        break;
                }
        case PLANNER_IK_ACA:
                {
                        IK_ACA* tempPlanner = new( IK_ACA );
                        tempPlanner->CopySettings( planner );
                        CD_BasicStyle* cd = dynamic_cast< CD_BasicStyle* >( collisionDetector );        //IAN IMPROVE: fix the collision detector hierarchy so that things properly derive from linear, etc
                        if( cd != NULL )
                        {
                                // Assign the Collision Detector
                                tempPlanner->SetCollisionDetector( cd );

                                delete planner;
                                planner = tempPlanner;
                        }
                        else
                        {
                                delete tempPlanner;
                                delete planner;
                                planner = NULL;
                                return false;
                        }
                        break;
                }
        case PLANNER_FORTEST:
                {
                        PL_ForTest* tempPlanner = new( PL_ForTest);
                        CD_BasicStyle* cd = dynamic_cast< CD_BasicStyle* >( collisionDetector );        //IAN IMPROVE: fix the collision detector hierarchy so that things properly derive from linear, etc
                        if( cd != NULL )
                        {
                                // Assign the Collision Detector
                                tempPlanner->SetCollisionDetector( cd );

                                delete planner;
                                planner = tempPlanner;
                        }
                        else
                        {
                                delete tempPlanner;
                                delete planner;
                                planner = NULL;
                                return false;
                        }
                        break;
                }
        case PLANNER_MPEP:
                {
                        PL_MPEP* tempPlanner = new( PL_MPEP );
                        CD_BasicStyle* cd = dynamic_cast< CD_BasicStyle* >( collisionDetector );        //IAN IMPROVE: fix the collision detector hierarchy so that things properly derive from linear, etc
                        if( cd != NULL )
                        {
                                // Assign the Collision Detector
                                tempPlanner->SetCollisionDetector( cd );

                                delete planner;
                                planner = tempPlanner;
                        }
                        else
                        {
                                delete tempPlanner;
                                delete planner;
                                planner = NULL;
                                return false;
                        }
                        break;
                }
        case PLANNER_IK_JACOBIAN:
                {
                        IK_Jacobian* tempPlanner = new( IK_Jacobian );
                        tempPlanner->CopySettings( planner );
                        CD_BasicStyle* cd = dynamic_cast< CD_BasicStyle* >( collisionDetector );        //IAN IMPROVE: fix the collision detector hierarchy so that things properly derive from linear, etc
                        if( cd != NULL )
                        {
                                // Assign the Collision Detector
                                tempPlanner->SetCollisionDetector( cd );

                                delete planner;
                                planner = tempPlanner;
                        }
                        else
                        {
                                delete tempPlanner;
                                delete planner;
                                planner = NULL;
                                return false;
                        }
                        break;
                }
        case PLANNER_ATACE:
                {
                        PL_ATACE* tempPlanner = new( PL_ATACE );
                        CD_BasicStyle* cd = dynamic_cast< CD_BasicStyle* >( collisionDetector );        //IAN IMPROVE: fix the collision detector hierarchy so that things properly derive from linear, etc
                        if( cd != NULL )
                        {
                                // Assign the Collision Detector
                                tempPlanner->SetCollisionDetector( cd );

                                delete planner;
                                planner = tempPlanner;
                        }
                        else
                        {
                                delete tempPlanner;
                                delete planner;
                                planner = NULL;
                                return false;
                        }
                        break;
                }
        case PLANNER_NEURAL:
                {
                        PL_Neural* tempPlanner = new PL_Neural;
                        CD_BasicStyle* cd = dynamic_cast< CD_BasicStyle* >( collisionDetector );        //IAN IMPROVE: fix the collision detector hierarchy so that things properly derive from linear, etc
                        if( cd != NULL )
                        {
                                // Assign the Collision Detector
                                tempPlanner->SetCollisionDetector( cd );

                                delete planner;
                                planner = tempPlanner;
                        }
                        else
                        {
                                delete tempPlanner;
                                delete planner;
                                planner = NULL;
                                return false;
                        }
                        break;
                }
    case PLANNER_NONE:
                {
                        this->plannerType = PLANNER_LINEAR;
                        return this->RefreshPlanner();
                }
        default:
                {
                        KickOut();
                        delete planner;
                        planner = new PL_Linear;
                        planner->SetCollisionDetector( dynamic_cast< CD_BasicStyle*>( collisionDetector ) );
                        return false;
                        break;
                }
        }
        planner->SetStartConfig( this->m_StartConfig );
        planner->SetGoalConfig( this->m_GoalConfig );
        RefreshSmoother();
        return true;
}

PlannerType ServerBase::GetPlannerType ()
{
  //## begin ServerBase::GetPlannerType%958953409.body preserve=yes
        return this->plannerType;
  //## end ServerBase::GetPlannerType%958953409.body
}

bool ServerBase::SetSmoother (SmootherType smootherType)
{
        //if the planner has changed then update it
        if( this->smootherType != smootherType )
        {
                this->smootherType = smootherType;
                bool success = this->RefreshSmoother();
                return success;
        }
        return true;
}

SmootherType ServerBase::GetSmootherType ()
{
        return this->smootherType;
}

//=============================================================================
// RefreshSmoother
//
// Description: sets the smoothing algorithm that the planner will use
//=============================================================================
bool ServerBase::RefreshSmoother()
{       
        switch( this->smootherType )
        {
                case SMOOTHER_NONE :
                {
                        if (smoother != NULL)
                                delete smoother;
                        smoother = NULL;
                        break;
                }
                case SMOOTHER_SUCCESSIVE_NODE_PAIR :
                {
                        SM_PathSmoothingSuccessiveNodePair* tempSmoother = new( SM_PathSmoothingSuccessiveNodePair );
                        if (smoother != NULL)
                                delete smoother;
                        smoother = tempSmoother;
                        break;
                }
                case SMOOTHER_RANDOM_NODE_PAIR :
                {
                        SM_PathSmoothingRandomNodePair* tempSmoother = new( SM_PathSmoothingRandomNodePair );
                        if (smoother != NULL)
                                delete smoother;
                        smoother = tempSmoother;
                        break;
                }
                case SMOOTHER_SUCCESSIVE_AND_RANDOM:
                {                       
                        SM_SuccessiveAndRandomSmoothing* tempSmoother = new ( SM_SuccessiveAndRandomSmoothing );
                        if (smoother != NULL)
                                delete smoother;
                        smoother = tempSmoother;
                        break;
                }
                case SMOOTHER_TRISECTION :
                {
                        SM_Trisection* tempSmoother = new ( SM_Trisection );
                        if (smoother != NULL)
                                delete smoother;
                        smoother = tempSmoother;
                        break;
                }
                default :
                        break;
        }

        PL_Boolean_Output* booleanPlanner = dynamic_cast< PL_Boolean_Output* >( planner );
        if( booleanPlanner != NULL )
        {
                booleanPlanner->SetSmoother( smoother );
                return true;
        }
        else
                return false;
}

CollisionDetectorType ServerBase::GetCollisionDetectorType ()
{
  //## begin ServerBase::GetCollisionDetectorType%958953410.body preserve=yes
        return this->collisionDetectorType;
  //## end ServerBase::GetCollisionDetectorType%958953410.body
}

// Additional Declarations

//=============================================================================
// OpenExampleFile
//
// comment - opens up a complete example
//=============================================================================
bool ServerBase::OpenExampleFile( const char* filename )
{
        //
        // Change directories to the path of the example file
        //
        char drive[ 256 ] = "";
        char dir[ 256 ] = "";
        char fname[ 256 ] = "";
        char ext[ 256 ] = "";
    //assert( false );
        //::_splitpath( filename, drive, dir, fname, ext );
        char rootDirectory[ 256 ] = "";
        strcat( rootDirectory, drive );
        strcat( rootDirectory, dir );
        if( strcmp( rootDirectory, "" ) == 0 )
        {
                strcat( rootDirectory, "." );
        }

//  int cdSuccess = ::chdir( rootDirectory );
//      assert( cdSuccess == 0 );

#ifdef _DEBUG
#ifdef WIN32
        char currentDirectory[ 256 ] = "";
        ::_getcwd( currentDirectory, 256 );
#endif //WIN32
#endif //_DEBUG

        char totalFileName[ 256 ] = "";
        strcat( totalFileName, fname );
        strcat( totalFileName, ext );
        IfstreamWithComments infile;
        infile.open( filename );
        IJG_Assert( infile.good() );

        //read the file header
        char header[ 300 ] = "";
        infile.getline( header, 300 );
        assert( strcmp( header, "#MPK save file version 0.1" ) == 0 );

        //read start, goal and current configurations
        char marker[ 300 ] = "";
        eatwhite( infile );
        infile.getline( marker, 300, ' ' );
        assert( strcmp( marker, "#start_config" ) == 0 );
        infile >> m_StartConfig;
#ifdef _DEBUG
        int size;
        size = m_StartConfig.DOF();
        assert( size != 0 );
#endif

        eatwhite( infile );
        infile.getline( marker, 300, ' ' );
        assert( strcmp( marker, "#goal_config" ) == 0 );
        infile >> m_GoalConfig;
#ifdef _DEBUG
        size = m_GoalConfig.DOF();
        assert( size != 0 );
#endif


        eatwhite( infile );
        infile.getline( marker, 300, ' ' );
        assert( strcmp( marker, "#current_config" ) == 0 );
        infile >> m_CurrentConfig;

        //read the camera positions
        eatwhite( infile );
        infile.getline( marker, 300 );
        assert( strcmp( marker, "#camera" ) == 0 );
        infile >> m_CameraPosition;
        infile >> m_CameraLookat;
        infile >> m_CameraUp;

        //read the camera positions
        eatwhite( infile );
        infile.getline( marker, 300 );
    MPKstring::TrimTrailingWhitespace( marker );
    if( strcmp( marker, "#cspacecamera" ) == 0 )
    {
        ParseCspaceCamera( infile );
        eatwhite( infile );
            infile.getline( marker, 300 );
        MPKstring::TrimTrailingWhitespace( marker );
    }

        //read the planner type
        IJG_Assert( strcmp( marker, "#planner_type" ) == 0 );
        int plType = -1;
        infile >> plType;

        //read the collision detector type
        eatwhite( infile );
        infile.getline( marker, 300, ' ' );
        IJG_Assert( strcmp( marker, "#collisionDetectorType" ) == 0 );
        int cdType = -1;
        infile >> cdType;
        
        //input the filename
        eatwhite( infile );
        infile.getline( marker, 300 );
        IJG_Assert( stricmp( marker, "#robotfile" ) == 0 );
        infile.getline( marker, 300 );
        this->universe.Empty();
        bool success = this->ParseRobotFile( marker );
        if( success == false )
        {
                //Could not open robot file
                IJG_Assert( false );
                //MessageBox( NULL, "Could not open robot file", "ERROR", MB_OK );
                return false;
        }

        //input the obstacle filename
        eatwhite( infile );
        infile.getline( marker, 300 );
        IJG_Assert( stricmp( marker, "#obstacle" ) == 0 );
        infile.getline( marker, 300 );
        this->ParseObstacleFile( marker );

        //refresh the collision detector
        SetCollisionDetector( static_cast< CollisionDetectorType >( cdType ) );
        RefreshCollisionDetector();

        //this must be done after the collision detector is opened
        SetPlanner( static_cast< PlannerType >( plType ) );
        planner->SetStartConfig( this->m_StartConfig );
        planner->SetGoalConfig( this->m_GoalConfig );

        //apply the planner type 
        bool pSuccess = this->planner->Load( filename );
        if( pSuccess == false )
        {
                this->RefreshPlanner();
        }

        //read the next marker - goalframes
        eatwhite( infile );
        infile.getline( marker, 300 );
    if( stricmp( marker, "#goalFrames") == 0 )
    {
        ParseGoalFrames( infile );
            eatwhite( infile );
            infile.getline( marker, 300 );
    }

        IJG_Assert( strcmp( marker, "#path" ) == 0 );
        infile >> m_Path;

        return true;
}

bool ServerBase::SaveExampleFile( const char* filename )
{
        std::ofstream outfile( filename, std::ios::out );
        if( outfile.good() == false )
        {
                return false;
        }

        //output the file header
        outfile << "#MPK save file version 0.1" << endl;

        //output start, goal and current configurations
        outfile << "#start_config "     << m_StartConfig        << endl;
        outfile << "#goal_config "      << m_GoalConfig << endl;
        outfile << "#current_config " << m_CurrentConfig << endl;

        //output camera values
        //need to get the first view
        outfile << "#camera" << endl;
        outfile << m_CameraPosition << endl;
        outfile << m_CameraLookat   << endl;
        outfile << m_CameraUp       << endl;

        outfile << "#cspacecamera" << endl;
        outfile << m_CspaceCameraPosition << endl;
        outfile << m_CspaceCameraLookat   << endl;
        outfile << m_CspaceCameraUp       << endl;

        //output the planner type
        outfile << "#planner_type " << endl << plannerType << endl;

        //output the collision detector type
        outfile << "#collisionDetectorType " << endl << collisionDetectorType << endl;
        planner->Save( filename );

        //output the robot filename
        outfile << "#robotfile" << endl;
        outfile << this->filename << endl;

        //output the obstacle filename
        outfile << "#obstacle" << endl;
        outfile << obstacleFilename << endl;

        //output the current path
        outfile << "#path" << endl;
        outfile << m_Path << endl;
        
        return true;
}

const char* ServerBase::GetRobotFilename() const
{
        return filename;
};
  
const char* ServerBase::GetObstacleFilename() const
{
        return obstacleFilename;
}

//=============================================================================
// ServerBase::ParseCspaceCamera
//=============================================================================
void ServerBase::ParseCspaceCamera( IfstreamWithComments& is )
{
        is >> m_CspaceCameraPosition;
        is >> m_CspaceCameraLookat;
        is >> m_CspaceCameraUp;
}

//=============================================================================
// ServerBase::ParseGoalFrames
//=============================================================================
void ServerBase::ParseGoalFrames( IfstreamWithComments& is )
{
        int numFrames;
        is >> numFrames;
        IJG_Assert( numFrames == 0 );
    int frameCount;
    for( frameCount = 0; frameCount < numFrames; frameCount++ )
    {
            VectorN xyzrpy;
            is >> xyzrpy;
            IK_InvKinBase* ik = dynamic_cast< IK_InvKinBase* >( planner );
            if( NULL != ik )
            {
                    ik->SetGoalFrame
                    ( 
                            xyzrpy[ 0 ],
                            xyzrpy[ 1 ],
                            xyzrpy[ 2 ],
                            xyzrpy[ 3 ],
                            xyzrpy[ 4 ],
                            xyzrpy[ 5 ]
                    );
            }
    }
}

//sets the root path for loading files
void ServerBase::SetRootPath( const char* path )
{
    //VRML_20_Reader::SetRootPath( path );
    ::strcpy( rootpath, path );
}

void ServerBase::SetMPKWorkingDirectory( const char* path )
{
    //VRML_20_Reader::SetRootPath( path );
        char mpkpath[_MAX_PATH];
        strncpy(mpkpath, path, _MAX_PATH);

        int pathlen = strlen(mpkpath);
        if ((pathlen > 0) && (pathlen<(_MAX_PATH-1)))
        {
                if (mpkpath[pathlen-1] != '\\')
                {
                        strcat(mpkpath, "\\");
                }

        }

    ::SetMPKWorkingDirectory( mpkpath );
}

bool ServerBase::AddObstacle( double x, double y )
{
        char newFilename[ 1000 ] = "";
        GetAbsoluteFileName(newFilename, _MAX_PATH, GetMPKWorkingDirectory(), "vrml models\\cylinder.wrl");

        VRML_20_Reader reader;
        reader.SetFilename( newFilename );
        reader.Read();
        const ObjectBase* theObject = reader.GetTheObject();
        assert( theObject != NULL );
        if( theObject == NULL )
        {
                assert( false );
                return false;
        }
        ObjectBase* obstacle = dynamic_cast< ObjectBase* >( theObject->Clone() );
        assert( obstacle != NULL );
        obstacle->SetBaseFrame( 0 );
        Matrix4x4 transform;
        transform.Translate( x, y, 0 );
        obstacle->SetFrame( transform );
        universe.AddEntity( obstacle );
        delete obstacle;
        //RefreshCollisionDetector();
        userObstacles.push_back(transform);
        return true;
}

bool ServerBase::SaveUserObstacle( const char *obsfile )
{
        std::ofstream outfile( obsfile, std::ios::out );
        if( outfile.good() == false )
        {
                return false;
        }

        //output the file header
        outfile << "User-added obstacles" << endl;
        outfile << userObstacles.size() <<endl;

        //output the transofmration of the obstacles
        for (int k=0; k<userObstacles.size(); k++)
        {
                Matrix4x4 obsPos = userObstacles[k];
                outfile << obsPos;
        }
        return true;
}

bool ServerBase::LoadUserObstacle( const char *obsfile )
{
        std::ifstream infile( obsfile, std::ios::in );
        if( infile.good() == false )
        {
                return false;
        }

        //input the file header
        char marker[100] = "";
        infile.getline(marker, 100);
        int numObstacles;
        infile >> numObstacles;

        /*
        //check if the robot is the sphere/stick one
        GL_Universe& mpkUniverse = server->universe;
        char currentPath[_MAX_PATH];
        char fullFileName[_MAX_PATH];
        _getcwd(currentPath, _MAX_PATH);
        _makepath(fullFileName, NULL, currentPath, "vrml models\\cylinder.wrl", NULL);
        VRML_20_Reader reader;
        reader.SetFilename( fullFileName  );
        reader.Read();
        const ObjectBase* theObject = reader.GetTheObject();
        assert( theObject != NULL );
        if( theObject == NULL )
        {
                MessageBox( NULL, "Could not open filename specified", "ERROR", MB_OK );
                return;
        }

        ObjectBase* obstacle = dynamic_cast< ObjectBase* >( theObject->Clone() );
        assert( obstacle != NULL );
        obstacle->SetBaseFrame( 0 );
        //Matrix4x4 transform;
        //transform.Translate( x, y, 0 );
        //obstacle->SetFrame( transform );
        */

        //input the transofmration of the obstacles
        for (int k=0; k< numObstacles; k++)
        {
                Matrix4x4 obsPos;
                infile >> obsPos;
                //obstacle->SetFrame(obsPos);
                AddObstacle(obsPos(0, 3), obsPos(1, 3));
                //mpkUniverse.AddEntity( obstacle );
                //userObstacles.push_back(obsPos);
                //obstacle->SetFrame(obsPos.Inverse());
        }

        //RefreshCollisionDetector();
        return true;
}

//## end ServerBase%37AB2C4702C6.declarations
//## begin module%37AB2C4702C6.epilog preserve=yes
//## end module%37AB2C4702C6.epilog

---