bool Consultant::CleanUp(DOContainer &cDO, IactContainer &cIact)
{
SystemParameter* csp = pDOWorld->GetSystemParameter();
if (csp->GetZoneOfInterest().Active())
{
std::vector<_uint_t> RedundantElement;
std::map<_uint_t, _uint_t> ElementMappingJump;
_uint_t numberDO = csp->GetDONumber();
const DOStatus* pdos = 0;
const DOModel* pdoml = 0;
_uint_t ul;
_uint_t ulDead;
for (ul=0, ulDead=0; ul<numberDO; ul++)
{
pdos = pDOWorld->GetDOStatus(ul);
pdoml = pDOWorld->GetDOModel(pdos->GetDOName());
DOMap pm = (DOMap(ul, pdos, pdoml, 0.0));
if (DOMap::ISMobile(pm))
{
if (! (InBoundary(ul)) )
{
RedundantElement.push_back(ul);
ulDead--;
}
}
ElementMappingJump[ul] = ulDead;
}
if (!RedundantElement.empty())
{
#ifdef _DEBUG
std::cout << "Number of redundant elements = " << RedundantElement.size() << " / " << numberDO << std::endl;
#endif // _DEBUG
cDO.Erase(RedundantElement);
pIRTbl->ModifyPair(ElementMappingJump);
pDOWorld->EraseDOStatus(RedundantElement);
vcIactMaster.clear();
vcIactSlave.clear();
IactPairTab.clear();
cIact.Clear();
return true;
}
else
{
return false;
}
}
else
{
return false;
}
}
void Planner::GetSuccessors(int nid, std::vector<float> &successors)
{
Point coord = NodeIDtoGridCoord((float)nid);
std::cout<<coord.x<<" "<<coord.y<<"\n";
coord.x = coord.x ;
coord.y = coord.y + 1;
if(GridCoordtoNodeID(coord)>0 && InBoundary(GridCoordtoConfiguration(coord)) && !InCollision(GridCoordtoConfiguration(coord))){ successors.push_back(GridCoordtoNodeID(coord));}
coord.x = coord.x;
coord.y = coord.y - 2; // = y-1
if(GridCoordtoNodeID(coord)>0 && InBoundary(GridCoordtoConfiguration(coord)) && !InCollision(GridCoordtoConfiguration(coord))) { successors.push_back(GridCoordtoNodeID(coord));}
coord.x = coord.x + 1;
coord.y = coord.y+1; // = y
if(GridCoordtoNodeID(coord)>0 && InBoundary(GridCoordtoConfiguration(coord)) && !InCollision(GridCoordtoConfiguration(coord))) { successors.push_back(GridCoordtoNodeID(coord));}
coord.x = coord.x -2 ;
coord.y = coord.y; // = y
if(GridCoordtoNodeID(coord)>0 && InBoundary(GridCoordtoConfiguration(coord)) && !InCollision(GridCoordtoConfiguration(coord))) { successors.push_back(GridCoordtoNodeID(coord));}
}
