void CPFA_loop_functions::UpdatePheromoneList() {
// Return if this is not a tick that lands on a 0.5 second interval
if ((int)(GetSpace().GetSimulationClock()) % ((int)(GetSimulator().GetPhysicsEngine("dyn2d").GetInverseSimulationClockTick()) / 2) != 0) return;
std::vector<Pheromone> new_p_list;
argos::Real t = GetSpace().GetSimulationClock() / GetSimulator().GetPhysicsEngine("dyn2d").GetInverseSimulationClockTick();
//ofstream log_output_stream;
//log_output_stream.open("time.txt", ios::app);
//log_output_stream << t << ", " << GetSpace().GetSimulationClock() << ", " << GetSimulator().GetPhysicsEngine("default").GetInverseSimulationClockTick() << endl;
//log_output_stream.close();
for(size_t i = 0; i < PheromoneList.size(); i++) {
PheromoneList[i].Update(t);
if(PheromoneList[i].IsActive() == true) {
new_p_list.push_back(PheromoneList[i]);
}
}
PheromoneList = new_p_list;
}
void CPFA_loop_functions::Reset() {
if(VariableFoodPlacement == 0) {
RNG->Reset();
}
GetSpace().Reset();
GetSpace().GetFloorEntity().Reset();
MaxSimCounter = SimCounter;
SimCounter = 0;
FoodList.clear();
FoodColoringList.clear();
PheromoneList.clear();
FidelityList.clear();
TargetRayList.clear();
// SetFoodDistribution();
argos::CSpace::TMapPerType& footbots = GetSpace().GetEntitiesByType("foot-bot");
argos::CSpace::TMapPerType::iterator it;
for(it = footbots.begin(); it != footbots.end(); it++) {
argos::CFootBotEntity& footBot = *argos::any_cast<argos::CFootBotEntity*>(it->second);
BaseController& c = dynamic_cast<BaseController&>(footBot.GetControllableEntity().GetController());
CPFA_controller& c2 = dynamic_cast<CPFA_controller&>(c);
MoveEntity(footBot.GetEmbodiedEntity(), c2.GetStartPosition(), argos::CQuaternion(), false);
}
}
void CPFA_loop_functions::Init(argos::TConfigurationNode &node) {
argos::CDegrees USV_InDegrees;
argos::TConfigurationNode CPFA_node = argos::GetNode(node, "CPFA");
argos::GetNodeAttribute(CPFA_node, "ProbabilityOfSwitchingToSearching", ProbabilityOfSwitchingToSearching);
argos::GetNodeAttribute(CPFA_node, "ProbabilityOfReturningToNest", ProbabilityOfReturningToNest);
argos::GetNodeAttribute(CPFA_node, "UninformedSearchVariation", USV_InDegrees);
argos::GetNodeAttribute(CPFA_node, "RateOfInformedSearchDecay", RateOfInformedSearchDecay);
argos::GetNodeAttribute(CPFA_node, "RateOfSiteFidelity", RateOfSiteFidelity);
argos::GetNodeAttribute(CPFA_node, "RateOfLayingPheromone", RateOfLayingPheromone);
argos::GetNodeAttribute(CPFA_node, "RateOfPheromoneDecay", RateOfPheromoneDecay);
argos::GetNodeAttribute(CPFA_node, "PrintFinalScore", PrintFinalScore);
UninformedSearchVariation = ToRadians(USV_InDegrees);
/****************************************************************************************************************************/
argos::TConfigurationNode settings_node = argos::GetNode(node, "settings");
argos::GetNodeAttribute(settings_node, "MaxSimTimeInSeconds", MaxSimTime);
MaxSimTime *= GetSimulator().GetPhysicsEngine("dyn2d").GetInverseSimulationClockTick();
argos::GetNodeAttribute(settings_node, "MaxSimCounter", MaxSimCounter);
argos::GetNodeAttribute(settings_node, "VariableFoodPlacement", VariableFoodPlacement);
argos::GetNodeAttribute(settings_node, "OutputData", OutputData);
argos::GetNodeAttribute(settings_node, "DrawIDs", DrawIDs);
argos::GetNodeAttribute(settings_node, "DrawTrails", DrawTrails);
argos::GetNodeAttribute(settings_node, "DrawTargetRays", DrawTargetRays);
argos::GetNodeAttribute(settings_node, "FoodDistribution", FoodDistribution);
argos::GetNodeAttribute(settings_node, "FoodItemCount", FoodItemCount);
argos::GetNodeAttribute(settings_node, "NumberOfClusters", NumberOfClusters);
argos::GetNodeAttribute(settings_node, "ClusterWidthX", ClusterWidthX);
argos::GetNodeAttribute(settings_node, "ClusterLengthY", ClusterLengthY);
argos::GetNodeAttribute(settings_node, "PowerRank", PowerRank);
argos::GetNodeAttribute(settings_node, "FoodRadius", FoodRadius);
argos::GetNodeAttribute(settings_node, "NestElevation", NestElevation);
FoodRadiusSquared = FoodRadius*FoodRadius;
// calculate the forage range and compensate for the robot's radius of 0.085m
argos::CVector3 ArenaSize = GetSpace().GetArenaSize();
argos::Real rangeX = (ArenaSize.GetX() / 2.0) - 0.085;
argos::Real rangeY = (ArenaSize.GetY() / 2.0) - 0.085;
ForageRangeX.Set(-rangeX, rangeX);
ForageRangeY.Set(-rangeY, rangeY);
// Send a pointer to this loop functions object to each controller.
argos::CSpace::TMapPerType& footbots = GetSpace().GetEntitiesByType("foot-bot");
argos::CSpace::TMapPerType::iterator it;
for(it = footbots.begin(); it != footbots.end(); it++) {
argos::CFootBotEntity& footBot = *argos::any_cast<argos::CFootBotEntity*>(it->second);
BaseController& c = dynamic_cast<BaseController&>(footBot.GetControllableEntity().GetController());
CPFA_controller& c2 = dynamic_cast<CPFA_controller&>(c);
c2.SetLoopFunctions(this);
}
SetFoodDistribution();
}
void CRecruitmentLoopFunctions::Init(TConfigurationNode& t_node) {
try {
/* Get a pointer to the floor entity */
m_pcFloor = &(GetSpace().GetFloorEntity());
//m_pcFloor = &m_cSpace.GetFloorEntity();
/* Create a new RNG */
m_pcRNG = CRandom::CreateRNG("argos");
/* Get the number of food items we want to be scattered from XML */
TConfigurationNode& tForaging = GetNode(t_node, "foraging");
GetNodeAttribute(tForaging, "output", m_strOutput);
UInt32 NbFoodItems;
GetNodeAttribute(tForaging, "items", NbFoodItems);
GetNodeAttribute(tForaging, "radius", m_fFoodSquareRadius);
m_fFoodSquareRadius *= m_fFoodSquareRadius;
/* Distribute uniformly the items in the environment */
for(UInt32 i = 0; i < NbFoodItems; ++i) {
m_cFoodPos.push_back(
CVector2(m_pcRNG->Uniform(m_cForagingArenaSideX),
m_pcRNG->Uniform(m_cForagingArenaSideY)));
}
for(UInt32 i = 0; i < NbFoodItems; ++i) {
m_cFoodPos.push_back(
CVector2(m_pcRNG->Uniform(m_cForagingArena2SideX),
m_pcRNG->Uniform(m_cForagingArena2SideY)));
}
}
catch(CARGoSException& ex) {
THROW_ARGOSEXCEPTION_NESTED("Error parsing loop functions!", ex);
}
m_cOutput.open(m_strOutput.c_str(), std::ios_base::trunc | std::ios_base::out);
m_cOutput << "# clock\tcollected_food\tavarage per 100 steps" << std::endl;
}
bool Slider::GetNewValue (const Point& cursor, int* vout)
{
bool ret = false;
int offset = 0;
int move_space = GetSpace();
if (move_space == 0) return false;
if (orientation() == Horizontal) {
offset = cursor.x() - last_cursor_.x();
} else {
offset = cursor.y() - last_cursor_.y();
}
int val = last_value_ + (offset * (maximum() - minimum())) / move_space;
if (val > maximum()) {
*vout = maximum();
} else if (val < minimum()) {
*vout = minimum();
} else {
*vout = val;
ret = true;
}
return ret;
}
double CPulseAEStream::GetCacheTime()
{
if (!m_Initialized)
return 0.0;
return (double)(m_cacheSize - GetSpace()) / (double)(m_sampleRate * m_frameSize);
}
ILSerializeWriter *ILSerializeWriterInit(void)
{
ILSerializeWriter *writer;
unsigned char *buf;
/* Allocate space for the writer */
writer = (ILSerializeWriter *)ILMalloc(sizeof(ILSerializeWriter));
if(!writer)
{
return 0;
}
/* Initialize the writer */
writer->blob = 0;
writer->blobLen = 0;
writer->blobMax = 0;
writer->outOfMemory = 0;
/* Write the version number to the blob */
buf = GetSpace(writer, 2);
if(buf)
{
IL_WRITE_UINT16(buf, 1);
}
/* The writer is ready to go */
return writer;
}
/*****
* This function resets all iAnts and restarts the simulation based on initial
* conditions set in the XML file.
*****/
void DSA_loop_functions::Reset() {
if(VariableSeed == 1) GetSimulator().SetRandomSeed(++RandomSeed);
//GetSimulator().Reset();
GetSpace().Reset();
SimTime = 0;
ResourceDensityDelay = 0;
FoodList.clear();
//PheromoneList.clear();
//FidelityList.clear();
TargetRayList.clear();
SetFoodDistribution();
size_t STOP = 0;
size_t robots = 0;
for(size_t i = 0; i < iAnts.size(); i++) {
iAnts[i]->Reset();
ReadFile();
iAnts[i]->SetStop(STOP);
STOP += 10;
if(robots <= N_robots)
{
iAnts[i]->GetPattern(robotPattern[robots]);
robots++;
}
}
}
//***********************************************************************************************
DWORD CoreAudioRenderer::AddPackets(unsigned char *data, DWORD len)
{
// sanity
if (!audioUnit && !IsValid())
{
CLog::Log(LOGERROR, "No audio output unit found!");
return -1;
}
if (len == 0) return len;
int samplesPassedIn, byteFactor;
uint8_t *pcmPtr = data;
byteFactor = m_uiChannels * m_uiBitsPerSample/8;
samplesPassedIn = len / byteFactor;
// Find out how much space we have available and clip to the amount we got passed in.
DWORD samplesToWrite = GetSpace();
if (samplesToWrite == 0) return samplesToWrite;
if (samplesToWrite > samplesPassedIn)
{
samplesToWrite = samplesPassedIn;
}
if (!m_bPassthrough)
m_amp.DeAmplifyInt16((int16_t *)pcmPtr, samplesToWrite * m_uiChannels, g_guiSettings.GetBool("audiooutput.normalisevolume"), true);
audioUnit->WriteStream(pcmPtr, samplesToWrite);
return samplesToWrite * byteFactor;
}
void FormatItems()
{
ExAssert(m_Ctrl);
// 获得属性
items_t* items = (items_t*)GetItems();
if (items->Empty()) m_FocItm = NULL;
LONG space = GetSpace();
LONG offst = (LONG)(LONG_PTR)m_Ctrl->GetState(_T("offset"));
CRect rect;
m_Ctrl->GetClientRect(rect);
CPoint pt;
LONG w = 0;
// 遍历列表项
items_t::iterator_t ite = items->Head();
items_t::ite_list_t ite_lst = ite->Children();
items_t::ite_list_t::iterator_t ite_ite = ite_lst.Head();
for(; ite_ite != ite_lst.Tail(); ++ite_ite)
FormatItems(*ite_ite, space, offst, pt, w);
// 设置滚动区域
m_Ctrl->SetAllRect(CSize(w, pt.y));
m_Ctrl->SetFraRect(CSize(rect.Width(), rect.Height()));
}
void ILSerializeWriterSetInt64(ILSerializeWriter *writer, ILInt64 value)
{
unsigned char *buf = GetSpace(writer, 8);
if(buf)
{
IL_WRITE_INT64(buf, value);
}
}
//***********************************************************************************************
unsigned int CWin32DirectSound::AddPackets(const void* data, unsigned int len)
{
CSingleLock lock (m_critSection);
DWORD total = len;
unsigned char* pBuffer = (unsigned char*)data;
DWORD bufferStatus = 0;
m_pBuffer->GetStatus(&bufferStatus);
if (bufferStatus & DSBSTATUS_BUFFERLOST)
{
CLog::Log(LOGDEBUG, __FUNCTION__ ": Buffer allocation was lost. Restoring buffer.");
m_pBuffer->Restore();
}
while (len >= m_dwDataChunkSize && GetSpace() >= m_dwDataChunkSize) // We want to write at least one chunk at a time
{
LPVOID start = NULL, startWrap = NULL;
DWORD size = 0, sizeWrap = 0;
if (m_BufferOffset >= m_dwBufferLen) // Wrap-around manually
m_BufferOffset = 0;
HRESULT res = m_pBuffer->Lock(m_BufferOffset, m_dwChunkSize, &start, &size, &startWrap, &sizeWrap, 0);
if (DS_OK != res)
{
CLog::Log(LOGERROR, __FUNCTION__ ": Unable to lock buffer at offset %u. HRESULT: 0x%08x", m_BufferOffset, res);
break;
}
// Remap the data to the correct channels into the buffer
if (m_remap.CanRemap())
m_remap.Remap((void*)pBuffer, start, size / m_uiBytesPerFrame, m_drc);
else
memcpy(start, pBuffer, size);
pBuffer += size * m_uiDataBytesPerFrame / m_uiBytesPerFrame;
len -= size * m_uiDataBytesPerFrame / m_uiBytesPerFrame;
m_BufferOffset += size;
if (startWrap) // Write-region wraps to beginning of buffer
{
// Remap the data to the correct channels into the buffer
if (m_remap.CanRemap())
m_remap.Remap((void*)pBuffer, startWrap, sizeWrap / m_uiBytesPerFrame, m_drc);
else
memcpy(startWrap, pBuffer, sizeWrap);
m_BufferOffset = sizeWrap;
pBuffer += sizeWrap * m_uiDataBytesPerFrame / m_uiBytesPerFrame;
len -= sizeWrap * m_uiDataBytesPerFrame / m_uiBytesPerFrame;
}
m_CacheLen += size + sizeWrap; // This data is now in the cache
m_pBuffer->Unlock(start, size, startWrap, sizeWrap);
}
CheckPlayStatus();
return total - len; // Bytes used
}
void ILSerializeWriterSetBoxedPrefix(ILSerializeWriter *writer,int type)
{
unsigned char *buf;
buf = GetSpace(writer, 1);
if(buf)
{
*buf = type;
}
}
unsigned int CAESinkDirectSound::AddPackets(uint8_t *data, unsigned int frames)
{
if (!m_initialized)
return 0;
DWORD total = m_dwFrameSize * frames;
DWORD len = total;
unsigned char* pBuffer = (unsigned char*)data;
DWORD bufferStatus = 0;
m_pBuffer->GetStatus(&bufferStatus);
if (bufferStatus & DSBSTATUS_BUFFERLOST)
{
CLog::Log(LOGDEBUG, __FUNCTION__ ": Buffer allocation was lost. Restoring buffer.");
m_pBuffer->Restore();
}
while (GetSpace() < total)
Sleep(total * 1000 / m_AvgBytesPerSec);
while (len)
{
LPVOID start = NULL, startWrap = NULL;
DWORD size = 0, sizeWrap = 0;
if (m_BufferOffset >= m_dwBufferLen) // Wrap-around manually
m_BufferOffset = 0;
HRESULT res = m_pBuffer->Lock(m_BufferOffset, m_dwChunkSize, &start, &size, &startWrap, &sizeWrap, 0);
if (DS_OK != res)
{
CLog::Log(LOGERROR, __FUNCTION__ ": Unable to lock buffer at offset %u. HRESULT: 0x%08x", m_BufferOffset, res);
break;
}
memcpy(start, pBuffer, size);
pBuffer += size;
len -= size;
m_BufferOffset += size;
if (startWrap) // Write-region wraps to beginning of buffer
{
memcpy(startWrap, pBuffer, sizeWrap);
m_BufferOffset = sizeWrap;
pBuffer += sizeWrap;
len -= sizeWrap;
}
m_CacheLen += size + sizeWrap; // This data is now in the cache
m_pBuffer->Unlock(start, size, startWrap, sizeWrap);
}
CheckPlayStatus();
return (total - len) / m_dwFrameSize; // Frames used
}
void
PrintSpace(char *name)
{
#ifdef DEBUG
Space *s;
s = GetSpace(XrmStringToQuark(name));
if (s != NULL)
print_space_windows(s);
#endif
}
void ILSerializeWriterSetFloat64(ILSerializeWriter *writer, ILDouble value)
{
#if IL_CONFIG_FP_SUPPORTED
unsigned char *buf = GetSpace(writer, 8);
if(buf)
{
IL_WRITE_DOUBLE(buf, value);
}
#endif
}
void ILSerializeWriterSetFloat32(ILSerializeWriter *writer, ILFloat value)
{
#if IL_CONFIG_FP_SUPPORTED
unsigned char *buf = GetSpace(writer, 4);
if(buf)
{
IL_WRITE_FLOAT(buf, value);
}
#endif
}
void ILSerializeWriterSetInt32(ILSerializeWriter *writer,
ILInt32 value, int type)
{
unsigned char *buf;
switch(type)
{
case IL_META_SERIALTYPE_BOOLEAN:
case IL_META_SERIALTYPE_I1:
case IL_META_SERIALTYPE_U1:
{
buf = GetSpace(writer, 1);
if(buf)
{
*buf = (unsigned char)value;
}
}
break;
case IL_META_SERIALTYPE_I2:
case IL_META_SERIALTYPE_U2:
case IL_META_SERIALTYPE_CHAR:
{
buf = GetSpace(writer, 2);
if(buf)
{
IL_WRITE_UINT16(buf, (ILUInt16)value);
}
}
break;
case IL_META_SERIALTYPE_I4:
case IL_META_SERIALTYPE_U4:
{
buf = GetSpace(writer, 4);
if(buf)
{
IL_WRITE_INT32(buf, value);
}
}
break;
}
}
void ILSerializeWriterSetProperty(ILSerializeWriter *writer,
const char *name, int type)
{
unsigned char *buf = GetSpace(writer, 2);
if(buf)
{
buf[0] = IL_META_SERIALTYPE_PROPERTY;
buf[1] = (unsigned char)type;
ILSerializeWriterSetString(writer, name, strlen(name));
}
}
void ILSerializeWriterSetString(ILSerializeWriter *writer,
const char *str, int len)
{
if(str)
{
unsigned char header[IL_META_COMPRESS_MAX_SIZE];
int headerLen = ILMetaCompressData(header, len);
unsigned char *buf = GetSpace(writer, headerLen + len);
if(buf)
{
ILMemCpy(buf, header, headerLen);
ILMemCpy(buf + headerLen, str, len);
}
}
else
{
/* Encode the null string in the output */
unsigned char *buf = GetSpace(writer, 1);
*buf = (unsigned char)0xFF;
}
}
void CLandmarks::PostStep() {
/* Reset flags for robot at target to false */
m_vecAtTarget = std::vector<bool>(TARGETS, false);
/* Index for the target on which a foot-bot is located */
SInt32 nAtTarget;
/* Counter for the robots currently on a target */
size_t unOnTarget = 0;
/* Counter for the robots currently in a chain */
size_t unInChain = 0;
/*
* Get a map of all the foot-bots
* The map is organized by robot id
*/
CSpace::TMapPerType& cFBMap = GetSpace().GetEntitiesByType("foot-bot");
/* Go through each foot-bot */
for(CSpace::TMapPerType::iterator it = cFBMap.begin(); it != cFBMap.end(); ++it) {
/* Create a reference for easier coding */
CFootBotEntity& cFB = *any_cast<CFootBotEntity*>(it->second);
/* Check whether this foot-bot is on a target */
nAtTarget = AtTarget(cFB);
if(nAtTarget >= 0) {
/* Yes, set corresponding flag to true */
m_vecAtTarget[nAtTarget] = true;
/* Increase counter */
++unOnTarget;
}
/* Check whether foot-bot is part of a chain */
if(InChain(cFB)) {
/* Yes, increase counter */
++unInChain;
}
}
/* Output a line for this step */
m_cOutFile << GetSpace().GetSimulationClock() << "\t"
<< unInChain << "\t"
<< unOnTarget
<< std::endl;
}
bool SimpleBuf::Write(const void* pBuf,int nSize )
{
int nSpace = GetSpace();
if (nSpace >= nSize)
{
memcpy(GetTailBuf(),pBuf,nSize);
nTail += nSize;
assert(nHead <= nTail);
return true;
}
else
return false;
}
void CPFA_loop_functions::PreStep() {
UpdatePheromoneList();
if(GetSpace().GetSimulationClock() > ResourceDensityDelay) {
for(size_t i = 0; i < FoodColoringList.size(); i++) {
FoodColoringList[i] = argos::CColor::BLACK;
}
}
if(FoodList.size() == 0) {
FidelityList.clear();
//TargetRayList.clear();
PheromoneList.clear();
}
}
//***********************************************************************************************
void CWin32DirectSound::WaitCompletion()
{
CSingleLock lock (m_critSection);
DWORD status;
unsigned int timeout;
unsigned char* silence;
if (!m_pBuffer)
return ;
if(FAILED(m_pBuffer->GetStatus(&status)) || (status & DSBSTATUS_PLAYING) == 0)
return; // We weren't playing anyway
// The drain should complete in the time occupied by the cache
timeout = (unsigned int)(1000 * GetDelay());
unsigned int startTime = XbmcThreads::SystemClockMillis();
silence = (unsigned char*)calloc(1,m_dwChunkSize); // Initialize 'silence' to zero...
while(AddPackets(silence, m_dwChunkSize) == 0)
{
if(FAILED(m_pBuffer->GetStatus(&status)) || (status & DSBSTATUS_PLAYING) == 0)
break;
if((XbmcThreads::SystemClockMillis() - startTime) > timeout)
{
CLog::Log(LOGWARNING, __FUNCTION__ ": timeout adding silence to buffer");
break;
}
}
free(silence);
while(m_CacheLen)
{
if(FAILED(m_pBuffer->GetStatus(&status)) || (status & DSBSTATUS_PLAYING) == 0)
break;
if((XbmcThreads::SystemClockMillis() - startTime) > timeout)
{
CLog::Log(LOGDEBUG, "CWin32DirectSound::WaitCompletion - timeout waiting for silence");
break;
}
else
Sleep(1);
GetSpace();
}
m_pBuffer->Stop();
}
Market::Market(
double B_,
double TradingFee_,
const std::string &OwnerAd_,
const std::string &Title_,
const std::string &Description_,
const std::vector<std::string> &Tags_,
time_t MaturationTime_,
std::vector<std::vector<std::string> > &D_State_
)
: B(B_),
TradingFee(TradingFee_),
OwnerAd(OwnerAd_),
Title(Title_),
Description(Description_),
Tags(Tags_),
MaturationTime(MaturationTime_),
D_State(D_State_),
State(1) /* 1 indicates active (ie "trading"), 2 = "contested", 3 = "redeemable" */
{
char B_str[16];
sprintf(B_str, "%.0f", B);
char TradingFee_str[16];
sprintf(TradingFee_str, "%.2f", TradingFee);
char MaturationTime_str[16];
sprintf(MaturationTime_str, "%u", (uint32_t)MaturationTime);
std::string str = "";
str += std::string(B_str);
str += std::string(TradingFee_str);
str += OwnerAd;
str += Title;
str += Description;
for(uint32_t i=0; i < Tags.size(); i++)
str += Tags[i];
str += std::string(MaturationTime_str);
for(uint32_t i=0; i < D_State.size(); i++)
for(uint32_t j=0; j < D_State[i].size(); j++)
str += D_State[i][j];
ID = tc_digest("md5", str.c_str(), strlen(str.c_str()));
MaturationTime = GetEndingDate();
Space S = GetSpace();
Shares = std::vector<double>(S.data.size(), 0.0);
uint32_t Nstates = S.data.size();
Balance = B * log((double)Nstates);
}
bool CPFA_loop_functions::IsExperimentFinished() {
bool isFinished = false;
if(FoodList.size() == 0 || GetSpace().GetSimulationClock() >= MaxSimTime) {
isFinished = true;
}
if(isFinished == true && MaxSimCounter > 1) {
size_t newSimCounter = SimCounter + 1;
size_t newMaxSimCounter = MaxSimCounter - 1;
PostExperiment();
Reset();
SimCounter = newSimCounter;
MaxSimCounter = newMaxSimCounter;
isFinished = false;
}
return isFinished;
}
void CFloorPowerFunctions::ResetPosition(){
CSpace::TMapPerType& m_cFootbots = GetSpace().GetEntitiesByType("efootbot");
for(CSpace::TMapPerType::iterator it = m_cFootbots.begin(); it != m_cFootbots.end(); ++it) {
CEFootBotEntity& cFootBot = *any_cast<CEFootBotEntity*>(it->second);
CQuaternion qOrientation = cFootBot.GetEmbodiedEntity().GetOriginAnchor().Orientation;
CVector3 vPosition = cFootBot.GetEmbodiedEntity().GetOriginAnchor().Position;
// check up/down robots
if(vPosition.GetX() >= 7.7 || vPosition.GetX() <= -7.7){ // map edge
vPosition.SetX(vPosition.GetX() - (vPosition.GetX() * 2));
}
// check left/right robots
if(vPosition.GetY()>= 7.7 || vPosition.GetY() <= -7.7){
vPosition.SetY(vPosition.GetY() - (vPosition.GetY() * 2));
}
cFootBot.GetEmbodiedEntity().MoveTo(vPosition, qOrientation, false);
m_pcController = &dynamic_cast<CEFootBotDiffusion&>(cFootBot.GetControllableEntity().GetController());
//LOG << "Current floor color: " << m_pcFloor->GetColorAtPoint(vPosition.GetX(), vPosition.GetY())<<std::endl;
m_pcController->m_clrBottomColor = m_pcFloor->GetColorAtPoint(vPosition.GetX(), vPosition.GetY());
}
}
void CFloorPowerFunctions::Init(TConfigurationNode& t_node) {
m_pcFloor = &GetSpace().GetFloorEntity();
//m_pcEFootBot = dynamic_cast<CEFootBotEntity*>(&GetSpace().GetEntity("fb1"));
//m_pcController = &dynamic_cast<CEFootBotDiffusion&>(m_pcEFootBot->GetControllableEntity().GetController());
}
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";
}
}
/**
* @brief
*/
inline float get_slider_position ()
{
return value() * GetSpace() / ((float) maximum() - (float) minimum());
}
