Move2DPathFollowerActPosVarRel.cpp
Go to the documentation of this file.
00001 #include "Move2DPathFollowerActPosVarRel.h"
00002 #include <cfloat>
00003 
00004 Move2DPathFollowerActPosVarRel::Move2DPathFollowerActPosVarRel(SharedPointer<Map2DPosVar> robotPosVar, SharedPointer<Map2DPath> path)
00005 {
00006     this->path = path;
00007     this->posVar = robotPosVar;
00008 
00009         this->addSourceVar(this->posVar);
00010     this->setDestVariable(this->posVar);
00011 }
00012 
00013 Move2DPathFollowerActPosVarRel::~Move2DPathFollowerActPosVarRel(void)
00014 {
00015 }
00016 
00017 vector<SharedPointer<RelEntry> > Move2DPathFollowerActPosVarRel::getProb(map<string, SharedPointer<IVariableValue> > sourceVals)
00018 {
00019         //string action = sourceVals[destVar->getVariableName()];
00020         
00021         SharedPointer<Map2DPosValue> actPos = dynamic_pointer_cast<Map2DPosValue>(sourceVals[posVar->getVariableName()]);
00022 
00023         vector<SharedPointer<Map2DPosValue> >  steps = path->getPath();
00024     double minDistance = DBL_MAX;
00025     
00026         SharedPointer<Map2DPosValue> minPosVal;
00027 
00028     for (int i = 0; i < steps.size(); i++)
00029     {
00030         SharedPointer<Map2DPosValue>& curStep = steps[i];
00031         if (actPos->isSamePos(curStep))
00032         {
00033             // on path, should move to next step if possible
00034             SharedPointer<Map2DPosValue> nextStep = curStep;
00035             if (i + 1 < steps.size())
00036             {
00037                 nextStep = steps[i + 1];
00038             }
00039 
00040             minPosVal = nextStep;
00041             break;
00042         }
00043 
00044         
00045         double dist = curStep->distanceTo(actPos);
00046         if (dist <= minDistance)
00047         {
00048             minDistance = dist;
00049             minPosVal = curStep;
00050         }
00051     }
00052 
00053     SharedPointer<Map2DPosValue> destPosVar = actPos->duplicate();
00054     if (minPosVal->x > destPosVar->x)
00055     {
00056         destPosVar->x++;
00057     }
00058         else if (minPosVal->x < destPosVar->x)
00059     {
00060         destPosVar->x--;
00061     }
00062     else if (minPosVal->y < destPosVar->y)
00063     {
00064         destPosVar->y--;
00065     }
00066     else if (minPosVal->y > destPosVar->y)
00067     {
00068         destPosVar->y++;
00069     }
00070 
00071         vector<SharedPointer<RelEntry> > result;
00072 
00073         SharedPointer<RelEntry> relEntry (new RelEntry());
00074         //relEntry->sourceValues[posVar->getVariableName()] = actPos;
00075         relEntry->destValues[this->getDestVariable()->getVariableName()] = destPosVar;
00076         relEntry->prob = 1.0;
00077         result.push_back(relEntry);
00078 
00079 
00080         return result;
00081 }


appl
Author(s): petercai
autogenerated on Tue Jan 7 2014 11:02:29