void CFootBotUN::addNodesAsObstacles(map<UInt8, Node> listNodeObstacles) { //Aux variables CVector2 auxPosition; CVector2 auxVelocity; //Create a list of Nodes for (map<UInt8, Node>::iterator it = listNodeObstacles.begin(); it != listNodeObstacles.end(); it++) { //printf("Node %d\n", (it->second).getId()); // I'm not and obstacle for myself! if ((it->second).getId() != mySelf.getId()) { //printf("Agent \n"); auxPosition.Set((it->second).getPosition().GetX(), (it->second).getPosition().GetY()); auxVelocity.Set((it->second).getVelocity(), 0); // For a dynamic obstacle we need to use the addObstacleAtPoint(position,velocity,radius) //agent->addObstacleAtPoint(auxPosition, OBSTACLE_RADIUS); agent->addObstacleAtPoint(auxPosition, auxVelocity, OBSTACLE_RADIUS); printf("Adding obstacle : %d\n", (it->second).getId()); } } }
void CGroundSensorEquippedEntity::AddSensorRing(const CVector2& c_center, Real f_radius, const CRadians& c_start_angle, ESensorType e_type, UInt32 un_num_sensors, const SAnchor& s_anchor) { CRadians cSensorSpacing = CRadians::TWO_PI / un_num_sensors; CRadians cAngle; CVector2 cOffset; for(UInt32 i = 0; i < un_num_sensors; ++i) { cAngle = c_start_angle + i * cSensorSpacing; cAngle.SignedNormalize(); cOffset.Set(f_radius, 0.0f); cOffset.Rotate(cAngle); cOffset += c_center; AddSensor(cOffset, e_type, s_anchor); } }
bool CVector2::FromString(Engine::Containers::CString str, CVector2& a) { if (str.Length() <= 0 || // Needs a string :3 str[0] != '(' || str[str.Length() - 1] != ')' || // Needs the braces around it. str.Count(',') != 1) // Needs exactly 1 seperating comma. return false; str = str.Trim("() "); s32 idx = str.IndexOf(','); Engine::Containers::CString xs = str.SubString(0, idx).Trim(); Engine::Containers::CString ys = str.SubString(idx + 1).Trim(); a.Set(xs.ToFloat(), ys.ToFloat()); return true; }
virtual void SaveAsImage(const std::string& str_path) { fipImage cImage(FIT_BITMAP, m_unPixelsPerMeter * m_cHalfArenaSize.GetX()*2, m_unPixelsPerMeter * m_cHalfArenaSize.GetY()*2, 24); Real fFactor = 1.0f / static_cast<Real>(m_unPixelsPerMeter); CVector2 cFloorPos; CColor cARGoSPixel; RGBQUAD tFIPPixel; for(UInt32 y = 0; y < cImage.getHeight(); ++y) { for(UInt32 x = 0; x < cImage.getWidth(); ++x) { cFloorPos.Set(x * fFactor, y * fFactor); cFloorPos -= m_cHalfArenaSize; cARGoSPixel = m_cLoopFunctions.GetFloorColor(cFloorPos); tFIPPixel.rgbRed = cARGoSPixel.GetRed(); tFIPPixel.rgbGreen = cARGoSPixel.GetGreen(); tFIPPixel.rgbBlue = cARGoSPixel.GetBlue(); cImage.setPixelColor(x, y, &tFIPPixel); } } if(!cImage.save(str_path.c_str())) { THROW_ARGOSEXCEPTION("Cannot save image \"" << str_path << "\" for floor entity."); } }
void CRecruitmentLoopFunctions::PreStep() { /* Logic to pick and drop food items */ /* * If a robot is in the nest, drop the food item * If a robot is on a food item, pick it * Each robot can carry only one food item per time */ /* Check whether a robot is on a food item */ //CSpace::TMapPerType& m_cFootbots = m_cSpace.GetEntitiesByType("foot-bot"); CSpace::TMapPerType& m_cFootbots = GetSpace().GetEntitiesByType("foot-bot"); for(CSpace::TMapPerType::iterator it = m_cFootbots.begin(); it != m_cFootbots.end(); ++it) { /* Get handle to foot-bot entity and controller */ CFootBotEntity& cFootBot = *any_cast<CFootBotEntity*>(it->second); CBTFootbotRecruitmentController& cController = dynamic_cast<CBTFootbotRecruitmentController&>(cFootBot.GetControllableEntity().GetController()); /* Get the position of the foot-bot on the ground as a CVector2 */ CVector2 cPos; cPos.Set(cFootBot.GetEmbodiedEntity().GetPosition().GetX(), cFootBot.GetEmbodiedEntity().GetPosition().GetY()); /* Get state data */ CBTFootbotRecruitmentController::SStateData* sStateData = &cController.GetStateData(); sStateData->CurrentPosition = cPos; /* Get food data */ CBTFootbotRecruitmentController::SFoodData* sFoodData = &cController.GetFoodData(); /* The foot-bot has a food item */ if(sFoodData->HasFoodItem) { /* Check whether the foot-bot is in the nest */ if(InNest(cPos)) { /* Place a new food item on the ground */ CVector2 tmp = sFoodData->LastFoodPosition; m_cFoodPos[sFoodData->FoodItemIdx].Set(tmp.GetX(), tmp.GetY()); //m_cFoodPos[sFoodData->FoodItemIdx].Set(m_pcRNG->Uniform(m_cForagingArenaSideX), m_pcRNG->Uniform(m_cForagingArenaSideY)); /* Drop the food item */ sFoodData->HasFoodItem = false; sFoodData->FoodItemIdx = 0; ++sFoodData->TotalFoodItems; m_nbCollectedFood++; /* The floor texture must be updated */ m_pcFloor->SetChanged(); } } else { /* The foot-bot has no food item */ /* Check whether the foot-bot is out of the nest */ if(!InNest(cPos)) { /* Check whether the foot-bot is on a food item */ bool bDone = false; for(size_t i = 0; i < m_cFoodPos.size() && !bDone; ++i) { if((cPos - m_cFoodPos[i]).SquareLength() < m_fFoodSquareRadius) { /* If so, we move that item out of sight */ m_cFoodPos[i].Set(100.0f, 100.f); /* The foot-bot is now carrying an item */ sFoodData->HasFoodItem = true; sFoodData->FoodItemIdx = i; sFoodData->LastFoodPosition = cPos; /* The floor texture must be updated */ m_pcFloor->SetChanged(); /* We are done */ bDone = true; } } } } } if(GetSpace().GetSimulationClock() % 1000 == 0){ m_cOutput << GetSpace().GetSimulationClock() << "\t" << m_nbCollectedFood << "\t" << m_nbCollectedFood / (GetSpace().GetSimulationClock()/1000)<< "\n"; } }
void CCombinedLoopFunctions::PreStep() { if(--foodClock == 0){ AddOneFood(); foodClock = renewalRate; } /* Logic to pick and drop food items */ /* * If a robot is in the nest, drop the food item * If a robot is on a food item, pick it * Each robot can carry only one food item per time */ /* Check whether a robot is on a food item */ CSpace::TMapPerType& m_cFootbots = GetSpace().GetEntitiesByType("foot-bot"); for(CSpace::TMapPerType::iterator it = m_cFootbots.begin(); it != m_cFootbots.end(); ++it) { /* Get handle to foot-bot entity and controller */ CFootBotEntity& cFootBot = *any_cast<CFootBotEntity*>(it->second); CBTFootbotCombinedController& cController = dynamic_cast<CBTFootbotCombinedController&>(cFootBot.GetControllableEntity().GetController()); CBTFootbotCombinedRootBehavior* pcRootBehavior = cController.GetRootBehavior(); /* Get the position of the foot-bot on the ground as a CVector2 */ CVector2 cPos; cPos.Set(cFootBot.GetEmbodiedEntity().GetPosition().GetX(), cFootBot.GetEmbodiedEntity().GetPosition().GetY()); /* Get state data */ CBTFootbotCombinedRootBehavior::SStateData* sStateData = &(*pcRootBehavior).GetStateData(); sStateData->CurrentPosition = cPos; /* Get food data */ CBTFootbotCombinedRootBehavior::SFoodData* sFoodData = &(*pcRootBehavior).GetFoodData(); /* The foot-bot has a food item */ if(sFoodData->HasFoodItem) { /* Check whether the foot-bot is in the nest */ if(InNest(cPos)) { /* Drop the food item */ //foodPatches[sFoodData->FoodPatchIdx].erase(foodPatches[sFoodData->FoodPatchIdx].begin() + sFoodData->FoodItemIdx); sFoodData->HasFoodItem = false; sFoodData->FoodItemIdx = 0; sFoodData->FoodPatchIdx = 0; ++sFoodData->TotalFoodItems; m_nbCollectedFood++; } } else { /* The foot-bot has no food item */ /* Check whether the foot-bot is out of the nest */ if(!InNest(cPos)) { /* Check whether the foot-bot is on a food item */ bool bDone = false; for(UInt32 j = 0; j < foodPatches.size(); j++) for(size_t i = 0; i < foodPatches[j].size() && !bDone; ++i) { if((cPos - foodPatches[j][i]).SquareLength() < m_fFoodSquareRadius) { /* If so, we move that item out of sight */ foodPatches[j][i].Set(1000.0f, 1000.f); /* The foot-bot is now carrying an item */ sFoodData->HasFoodItem = true; sFoodData->FoodItemIdx = i; sFoodData->FoodPatchIdx = j; sFoodData->LastFoodPosition = cPos; /* We are done */ bDone = true; foodPatches[sFoodData->FoodPatchIdx].erase(foodPatches[j].begin() + i); /* The floor texture must be updated */ m_pcFloor->SetChanged(); } } } } } if(GetSpace().GetSimulationClock() % 1000 == 0){ int nbFoodSolitary = 0; int nbFoodRecruiter = 0; int nbFoodRecruitee = 0; int type = 0; CSpace::TMapPerType& m_cFootbots = GetSpace().GetEntitiesByType("foot-bot"); for(CSpace::TMapPerType::iterator it = m_cFootbots.begin(); it != m_cFootbots.end(); ++it) { CFootBotEntity& cFootBot = *any_cast<CFootBotEntity*>(it->second); CBTFootbotCombinedController& cController = dynamic_cast<CBTFootbotCombinedController&>(cFootBot.GetControllableEntity().GetController()); CBTFootbotCombinedRootBehavior* pcRootBehavior = cController.GetRootBehavior(); /* Get food data */ CBTFootbotCombinedRootBehavior::SFoodData* sFoodData = &(*pcRootBehavior).GetFoodData(); type = pcRootBehavior->GetRobotType(); switch(type){ case SOLITARY: {nbFoodSolitary += sFoodData->TotalFoodItems; break;} case RECRUITER: {nbFoodRecruiter += sFoodData->TotalFoodItems; break;} case RECRUITEE: {nbFoodRecruitee += sFoodData->TotalFoodItems; break;} default: { THROW_ARGOSEXCEPTION("unrecognized type when forming statistics"); } } } m_cOutput << GetSpace().GetSimulationClock() << "," << nbFoodSolitary << "," << nbFoodRecruiter << "," << nbFoodRecruitee << std::endl; } if(GetSpace().GetSimulationClock() % 10000 == 0){ LOG << "Clock at: " << GetSpace().GetSimulationClock() << std::endl; } }