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));} }