C++ CVector2::Set方法代码示例

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;
	}
//.........这里部分代码省略.........