void CCameraData::GetPixelLengthVectors( const CVector3& Pos, const CVector2& vp, CVector3& OutX, CVector3& OutY ) const { ///////////////////////////////////////////////////////////////// int ivpw = int(vp.GetX()); int ivph = int(vp.GetY()); ///////////////////////////////////////////////////////////////// CVector4 va = Pos; CVector4 va_xf = va.Transform(mVPMatrix); va_xf.PerspectiveDivide(); va_xf = va_xf*CVector4(vp.GetX(),vp.GetY(),0.0f); ///////////////////////////////////////////////////////////////// CVector4 vdx = Pos+mCamXNormal; CVector4 vdx_xf = vdx.Transform(mVPMatrix); vdx_xf.PerspectiveDivide(); vdx_xf = vdx_xf*CVector4(vp.GetX(),vp.GetY(),0.0f); float MagX = (vdx_xf-va_xf).Mag(); // magnitude in pixels of mBillboardRight ///////////////////////////////////////////////////////////////// CVector4 vdy = Pos+mCamYNormal; CVector4 vdy_xf = vdy.Transform(mVPMatrix); vdy_xf.PerspectiveDivide(); vdy_xf = vdy_xf*CVector4(vp.GetX(),vp.GetY(),0.0f); float MagY = (vdy_xf-va_xf).Mag(); // magnitude in pixels of mBillboardUp ///////////////////////////////////////////////////////////////// OutX = mCamXNormal*(2.0f/MagX); OutY = mCamYNormal*(2.0f/MagY); ///////////////////////////////////////////////////////////////// }
void CCameraData::ProjectDepthRay( const CVector2& v2d, CVector3& vdir, CVector3& vori ) const { const Frustum& camfrus = mFrustum; CVector3 near_xt_lerp; near_xt_lerp.Lerp( camfrus.mNearCorners[0], camfrus.mNearCorners[1], v2d.GetX() ); CVector3 near_xb_lerp; near_xb_lerp.Lerp( camfrus.mNearCorners[3], camfrus.mNearCorners[2], v2d.GetX() ); CVector3 near_lerp; near_lerp.Lerp( near_xt_lerp, near_xb_lerp, v2d.GetY() ); CVector3 far_xt_lerp; far_xt_lerp.Lerp( camfrus.mFarCorners[0], camfrus.mFarCorners[1], v2d.GetX() ); CVector3 far_xb_lerp; far_xb_lerp.Lerp( camfrus.mFarCorners[3], camfrus.mFarCorners[2], v2d.GetX() ); CVector3 far_lerp; far_lerp.Lerp( far_xt_lerp, far_xb_lerp, v2d.GetY() ); vdir=(far_lerp-near_lerp).Normal(); vori=near_lerp; }
void CFootBotMotorGroundSensor::Update() { /* We make the assumption that the foot-bot is rotated only wrt to Z */ CFloorEntity& cFloorEntity = m_cSpace.GetFloorEntity(); const CVector3& cEntityPos = GetEntity().GetEmbodiedEntity().GetPosition(); const CQuaternion& cEntityRot = GetEntity().GetEmbodiedEntity().GetOrientation(); CRadians cRotZ, cRotY, cRotX; cEntityRot.ToEulerAngles( cRotZ, cRotY, cRotX ); CVector2 cCenterPos(cEntityPos.GetX(), cEntityPos.GetY()); CVector2 cSensorPos; for(UInt32 i = 0; i < CCI_FootBotMotorGroundSensor::NUM_READINGS; ++i) { cSensorPos = m_tReadings[i].Offset; cSensorPos.Rotate(cRotZ); cSensorPos *= 0.01; cSensorPos += cCenterPos; const CColor& cColor = cFloorEntity.GetColorAtPoint(cSensorPos.GetX(),cSensorPos.GetY()); m_tReadings[i].Value = cColor.ToGrayScale()/255*FOOTBOT_MOTOR_GROUND_SENSOR_READING_RANGE.GetSpan(); if( m_fNoiseLevel > 0.0f ) { AddNoise(i); } /* Normalize reading between 0 and 1, only if calibration has been performed */ if( m_bCalibrated ) { m_tReadings[i].Value = FOOTBOT_MOTOR_GROUND_SENSOR_READING_RANGE.NormalizeValue(m_tReadings[i].Value); } } }
void CFootBotBaseGroundRotZOnlySensor::Update() { /* * We make the assumption that the robot is rotated only wrt to Z */ /* Get robot position and orientation */ const CVector3& cEntityPos = m_pcEmbodiedEntity->GetPosition(); const CQuaternion& cEntityRot = m_pcEmbodiedEntity->GetOrientation(); CRadians cRotZ, cRotY, cRotX; cEntityRot.ToEulerAngles(cRotZ, cRotY, cRotX); /* Set robot center */ CVector2 cCenterPos(cEntityPos.GetX(), cEntityPos.GetY()); /* Position of sensor on the ground after rototranslation */ CVector2 cSensorPos; /* Go through the sensors */ for(UInt32 i = 0; i < m_tReadings.size(); ++i) { /* Calculate sensor position on the ground */ cSensorPos = m_pcGroundSensorEntity->GetSensor(i+4).Offset; cSensorPos.Rotate(cRotZ); cSensorPos += cCenterPos; /* Get the color */ const CColor& cColor = m_pcFloorEntity->GetColorAtPoint(cSensorPos.GetX(), cSensorPos.GetY()); /* Set the reading */ m_tReadings[i].Value = cColor.ToGrayScale() / 255.0f; /* Apply noise to the sensor */ if(m_bAddNoise) { m_tReadings[i].Value += m_pcRNG->Uniform(m_cNoiseRange); } /* Set the final reading */ m_tReadings[i].Value = m_tReadings[i].Value < 0.5f ? 0.0f : 1.0f; } }
void iAnt_controller::SetTargetInBounds(CVector2 t) { /* Bound the X value based on the forage range. */ if(t.GetX() > data->ForageRangeX.GetMax()) t = CVector2(data->ForageRangeX.GetMax(), t.GetY()); if(t.GetX() < data->ForageRangeX.GetMin()) t = CVector2(data->ForageRangeX.GetMin(), t.GetY()); /* Bound the Y value based on the forage range. */ if(t.GetY() > data->ForageRangeY.GetMax()) t = CVector2(t.GetX(), data->ForageRangeY.GetMax()); if(t.GetY() < data->ForageRangeY.GetMin()) t = CVector2(t.GetX(), data->ForageRangeY.GetMin()); /* Set the robot's target to the bounded t position. */ target = t; }
void HRVOAgent::addObstacleAtPoint(CVector2 p,CVector2 v,Real r) { HRVO::Agent *a=new HRVO::Agent(); a->velocity_=HRVO::Vector2((float)v.GetX(),(float)v.GetY()); a->position_=HRVO::Vector2((float)p.GetX(),(float)p.GetY()); Real distance; CVector2 relativePosition=relativePositionOfObstacleAt(p,r,distance); //a->radius_=r+marginForObstacleAtDistance(distance,r,safetyMargin,socialMargin); a->radius_=r+fmin(distance-r-_HRVOAgent->radius_-0.001,marginForObstacleAtDistance(distance,r,safetyMargin,socialMargin)); // printf("Obstacle radius %.3f\n",a->radius_); //a->radius_=r+marginForObstacleAtDistance(p.Length(),r,safetyMargin,socialMargin); a->prefVelocity_=a->velocity_; _HRVOAgent->agents_.push_back(a); _HRVOAgent->insertAgentNeighbor(agentIndex, rangeSq); agentIndex++; }
/** * Returns true if the given vector is near the arena end. */ bool CCombinedLoopFunctions::CloseToArenaEnd(const CVector2& pos){ if(pos.GetX() > arenaSize.GetX()/2.0f - CLOSE_TO_ARENA_END_PARAMETER || pos.GetX() < -arenaSize.GetX()/2.0f + CLOSE_TO_ARENA_END_PARAMETER || pos.GetY() > arenaSize.GetY()/2.0f - CLOSE_TO_ARENA_END_PARAMETER || pos.GetY() < -arenaSize.GetY()/2.0f + CLOSE_TO_ARENA_END_PARAMETER) { return true; } return false; }
bool CKinematics2DEngine::CheckCollisions( const CKinematics2DCollisionCircle* pc_circle, const CKinematics2DCollisionRectangle* pc_rectangle ) { /* Rototranslate the plane so that the rectangle is axis aligned and centered in O */ CVector2 center = pc_circle->GetPosition() - pc_rectangle->GetPosition(); center.Rotate(pc_rectangle->GetOrientation() ); center.Absolute(); /* Find the Voronoi Region that the circle is in, exploiting the symmetries */ CVector2 c_half_size = pc_rectangle->GetHalfSize(); Real f_radius = pc_circle->GetRadius(); if( center.GetX() <= c_half_size.GetX() ) { /* The circle is in the top or bottom region */ return (center.GetY() <= c_half_size.GetY() + f_radius); } if( center.GetY() <= c_half_size.GetY() ) { /* The circle is in the left or right region */ return (center.GetX() <= c_half_size.GetX() + f_radius); } /* The circle is in one of the four corner regions */ return (SquareDistance( c_half_size, center ) <= f_radius*f_radius); }
CColor CLandmarks::GetFloorColor(const CVector2& c_position_on_plane) { if((c_position_on_plane.GetX() >= -2.0f && c_position_on_plane.GetX() <= 2.0f) && (c_position_on_plane.GetY() >= -5.0f && c_position_on_plane.GetY() <= -1.0f)) { return CColor::GRAY90; } for(size_t i = 0; i < TARGETS; ++i) { if(SquareDistance(c_position_on_plane, m_vecTargets[i]) < TARGET_RADIUS2) { return CColor::BLACK; } } return CColor::WHITE; }
bool iAnt_controller::checkDistanceX() { cout << "checkDistanceX\n"; CVector2 curPosition = GetPosition(); //cout << "line 87\n"; cout << curPosition << "curPositionX\n"; bool stop = false; //cout << "nest:" << data ->NestPosition; float distanceX = curPosition.GetY()- data->NestPosition.GetY(); cout << "distance: " << distanceX << "\n"; if (distanceX > 0.3) { cout << "STOP!"; stop = true; } return stop; }
void Texture::SetTexel( const CColor4 & color, const CVector2 & ST ) { CReal Mul(255.0f); U8* pu8data = (U8*) mpImageData; u8 ur = (u8) (color.GetX()*Mul); u8 ug = (u8) (color.GetY()*Mul); u8 ub = (u8) (color.GetZ()*Mul); u8 ua = (u8) (color.GetW()*Mul); int ix = (int) (miWidth * fmod( (float) ST.GetY(), 1.0f )); int iy = (int) (miHeight * fmod( (float) ST.GetX(), 1.0f )); int idx = (int) (4 * (ix*miHeight + iy)); pu8data[idx+0] = ub; pu8data[idx+1] = ug; pu8data[idx+2] = ur; pu8data[idx+3] = ua; SetDirty(true); }
void CProprioceptiveFeatureVector::ComputeFeatureValues() { CProprioceptiveFeatureVector::FEATURE_RANGE = 30.0f; unsigned unCloseRangeNbrCount = CountNeighbors(FEATURE_RANGE/2.0f); unsigned unFarRangeNbrCount = CountNeighbors(FEATURE_RANGE) - unCloseRangeNbrCount; bool neighbours_present = ((unCloseRangeNbrCount + unFarRangeNbrCount) > 0) ? true : false; int CurrentStepNumber = m_sSensoryData.m_rTime; // Feature (from LS to MS bits in FV) // Sensors //1st: set if bot has atleast one neighbor in range 0-30cm in the majority of of past X time-steps //2nd: set if bot has atleast one neighbor in range 30-60cm in the majority of of past X time-steps if(CurrentStepNumber >= m_iEventSelectionTimeWindow) { // decision based on the last X time-steps if(m_unSumTimeStepsNbrsRange0to30 > (unsigned)(0.5*(double)m_iEventSelectionTimeWindow)) m_pfAllFeatureValues[0] = 1.0; else m_pfAllFeatureValues[0] = 0.0; if(m_unSumTimeStepsNbrsRange30to60 > (unsigned)(0.5*(double)m_iEventSelectionTimeWindow)) m_pfAllFeatureValues[1] = 1.0; else m_pfAllFeatureValues[1] = 0.0; // removing the fist entry of the moving time window from the sum m_unSumTimeStepsNbrsRange0to30 -= m_punNbrsRange0to30AtTimeStep[m_unNbrsCurrQueueIndex]; m_unSumTimeStepsNbrsRange30to60 -= m_punNbrsRange30to60AtTimeStep[m_unNbrsCurrQueueIndex]; } // adding new values into the queue if (unCloseRangeNbrCount > 0) { m_punNbrsRange0to30AtTimeStep[m_unNbrsCurrQueueIndex] = 1; m_unSumTimeStepsNbrsRange0to30++; } else m_punNbrsRange0to30AtTimeStep[m_unNbrsCurrQueueIndex] = 0; if (unFarRangeNbrCount > 0) { m_punNbrsRange30to60AtTimeStep[m_unNbrsCurrQueueIndex] = 1; m_unSumTimeStepsNbrsRange30to60++; } else m_punNbrsRange30to60AtTimeStep[m_unNbrsCurrQueueIndex] = 0; m_unNbrsCurrQueueIndex = (m_unNbrsCurrQueueIndex + 1) % m_iEventSelectionTimeWindow; // Sensors-motor interactions // Set if the occurance of the following event, atleast once in time window X // 3rd: distance to nbrs 0-6 && change in angular acceleration // 4th: no neighbors detected && change in angular acceleration if(neighbours_present && (m_sSensoryData.AngularAcceleration > m_tAngularAccelerationThreshold || m_sSensoryData.AngularAcceleration < -m_tAngularAccelerationThreshold)) { m_piLastOccuranceEvent[2] = CurrentStepNumber; } if(!neighbours_present && (m_sSensoryData.AngularAcceleration > m_tAngularAccelerationThreshold || m_sSensoryData.AngularAcceleration < -m_tAngularAccelerationThreshold)) { m_piLastOccuranceEvent[3] = CurrentStepNumber; } for(unsigned int featureindex = 2; featureindex <=3; featureindex++) { if ((CurrentStepNumber - m_piLastOccuranceEvent[featureindex]) <= m_iEventSelectionTimeWindow) { m_pfAllFeatureValues[featureindex] = 1.0; } else { m_pfAllFeatureValues[featureindex] = 0.0; } } // Motors //5th: distance travelled by bot in past 100 time-steps. Higher than 5% of max-possible distance travelled is accepted as feature=1. CVector2 vecAgentPos = m_sSensoryData.pos; m_fCumulativeDistTravelled += m_sSensoryData.dist; if(CurrentStepNumber >= m_iDistTravelledTimeWindow) { // distance travelled in last 100 time-steps m_fSquaredDistTravelled = (vecAgentPos.GetX() - m_pvecCoordAtTimeStep[m_unCoordCurrQueueIndex].GetX()) * (vecAgentPos.GetX() - m_pvecCoordAtTimeStep[m_unCoordCurrQueueIndex].GetX()) + (vecAgentPos.GetY() - m_pvecCoordAtTimeStep[m_unCoordCurrQueueIndex].GetY()) * (vecAgentPos.GetY() - m_pvecCoordAtTimeStep[m_unCoordCurrQueueIndex].GetY()); //pos_ref = m_pvecCoordAtTimeStep[m_unCoordCurrQueueIndex]; // removing the distance travelled in the first time-step of the moving time-window from the queue m_fCumulativeDistTravelled -= m_pfDistAtTimeStep[m_unCoordCurrQueueIndex]; // decision based on distance travelled in the last 100 time-steps /*if(m_fCumulativeDistTravelled >= m_fCumulativeDistThreshold) m_pfAllFeatureValues[4] = 1.0; else m_pfAllFeatureValues[4] = 0.0;*/ /*if(m_fSquaredDistTravelled >= m_fSquaredDistThreshold) m_pfAllFeatureValues[4] = 1.0; else m_pfAllFeatureValues[4] = 0.0;*/ } m_fEstimated_Dist_LongTimeWindow = TrackRobotDisplacement(CurrentStepNumber, vec_RobPos_LongRangeTimeWindow); m_pfAllFeatureValues[4] = (m_fEstimated_Dist_LongTimeWindow > sqrt(m_fSquaredDistThreshold)) ? 1.0f: 0.0f; //std::cout << " cumulative distance " << m_fCumulativeDistTravelled << std::endl; //std::cout << " LinearSpeed " << m_sSensoryData.LinearSpeed << " angular speed " << m_sSensoryData.AngularSpeed << std::endl; //std::cout << " LinearAcceleration " << m_sSensoryData.LinearAcceleration << " angular acceleration " << m_sSensoryData.AngularAcceleration << std::endl; // adding new coordinate values into the queue m_pvecCoordAtTimeStep[m_unCoordCurrQueueIndex] = vecAgentPos; // adding distance travelled at last time-step into queue m_pfDistAtTimeStep[m_unCoordCurrQueueIndex] = m_sSensoryData.dist; m_unCoordCurrQueueIndex = (m_unCoordCurrQueueIndex + 1) % m_iDistTravelledTimeWindow; if((m_sSensoryData.AngularAcceleration > m_tAngularAccelerationThreshold || m_sSensoryData.AngularAcceleration < -m_tAngularAccelerationThreshold)) { m_piLastOccuranceEvent[5] = CurrentStepNumber; } // 6th: set if the robot changed its angular speed (per control cycle) atleast once in the past time-window. if ((CurrentStepNumber - m_piLastOccuranceEvent[5]) <= m_iEventSelectionTimeWindow) { m_pfAllFeatureValues[5] = 1.0; } else { m_pfAllFeatureValues[5] = 0.0; } //6th: linear speed, higher than 5% of max. speed is accepted as feature=1 OR angular speed higher than 5% of max. angular speed is accepted as feature=1 /*m_pfAllFeatureValues[5] = (m_sSensoryData.LinearSpeed >= m_fVelocityThreshold) ? 1.0:0.0;*/ // adding the selected features into the feature vector for(size_t i = 0; i < NUMBER_OF_FEATURES; ++i) m_pfFeatureValues[i] = m_pfAllFeatureValues[i]; }
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"; } }
/***** * Sets target West of the robot's current target. *****/ void iAnt_controller::SetTargetW(char x) { CVector2 position = GetTarget(); target = CVector2(position.GetX(),position.GetY()+stepSize); }
bool CCombinedLoopFunctions::InNest(const CVector2& pos){ if(pos.GetX() > -(nestSize/2.0f) && pos.GetX() < (nestSize/2.0f) && pos.GetY() > -(nestSize/2.0f) && pos.GetY() < (nestSize/2.0f)) { return true; } return false; }
bool CRecruitmentLoopFunctions::InNest(const CVector2& pos){ if(pos.GetX() > -1.0f && pos.GetX() < 1.0f && pos.GetY() > -5.0f && pos.GetY() < -3.0f) { return true; } return false; }
/***** * Required by ARGoS. This function initializes global variables using * values from the XML configuration file supplied when ARGoS is run. *****/ void DSA_loop_functions::Init(TConfigurationNode& node) { /* Temporary variables. */ CSimulator *simulator = &GetSimulator(); CPhysicsEngine *physicsEngine = &simulator->GetPhysicsEngine("default"); CVector3 ArenaSize = GetSpace().GetArenaSize(); CVector2 rangeX = CVector2(-ArenaSize.GetX()/2.0, ArenaSize.GetX()/2.0); CVector2 rangeY = CVector2(-ArenaSize.GetY()/2.0, ArenaSize.GetY()/2.0); CDegrees USV_InDegrees; /* Get each tag in the loop functions section of the XML file. */ TConfigurationNode simNode = GetNode(node, "simulation"); TConfigurationNode random = GetNode(node, "_0_FoodDistribution_Random"); TConfigurationNode cluster = GetNode(node, "_1_FoodDistribution_Cluster"); TConfigurationNode powerLaw = GetNode(node, "_2_FoodDistribution_PowerLaw"); GetNodeAttribute(simNode, "MaxSimCounter", MaxSimCounter); GetNodeAttribute(simNode, "VariableSeed", VariableSeed); GetNodeAttribute(simNode, "OutputData", OutputData); GetNodeAttribute(simNode, "NestPosition", NestPosition); GetNodeAttribute(simNode, "NestRadius", NestRadius); GetNodeAttribute(simNode, "FoodRadius", FoodRadius); GetNodeAttribute(simNode, "FoodDistribution", FoodDistribution); GetNodeAttribute(random, "FoodItemCount", FoodItemCount); GetNodeAttribute(cluster, "NumberOfClusters", NumberOfClusters); GetNodeAttribute(cluster, "ClusterWidthX", ClusterWidthX); GetNodeAttribute(cluster, "ClusterLengthY", ClusterLengthY); GetNodeAttribute(powerLaw, "PowerRank", PowerRank); /* Convert and calculate additional values. */ NestRadiusSquared = (NestRadius) * (NestRadius); FoodRadiusSquared = (FoodRadius + 0.04) * (FoodRadius + 0.04); ForageRangeX.Set(rangeX.GetX() + (2.0 * FoodRadius), rangeX.GetY() - (2.0 * FoodRadius)); ForageRangeY.Set(rangeY.GetX() + (2.0 * FoodRadius), rangeY.GetY() - (2.0 * FoodRadius)); RNG = CRandom::CreateRNG("argos"); /* Store the iAnts in a more friendly, human-readable structure. */ CSpace::TMapPerType& footbots = GetSpace().GetEntitiesByType("foot-bot"); CSpace::TMapPerType::iterator it; ReadFile(); size_t STOP = 0; size_t robots = 0; for(it = footbots.begin(); it != footbots.end(); it++) { CFootBotEntity& footBot = *any_cast<CFootBotEntity*>(it->second); DSA_controller& c = dynamic_cast<DSA_controller&>(footBot.GetControllableEntity().GetController()); DSAnts.push_back(&c); c.SetData(&data); c.SetStop(STOP); STOP += 10; /* adds 10 seconds extra pause for each robot */ if(robots <= N_robots) { c.GetPattern(robotPattern[robots]); robots++; //attempt to add mutiple colors for each robot // if(robots==2) // { // DrawTargetRays = 2; // } // else if(robots ==3) // { // DrawTargetRays = 3; // } } } /* Set up the food distribution based on the XML file. */ SetFoodDistribution(); }
/** * Returns true if the given vector is near the nest, relative to food patch size. */ bool CCombinedLoopFunctions::CloseToNest(const CVector2& pos){ /*if(pos.GetX() > -(nestSize/2.0f)-foodPatchSize/1 && pos.GetX() < (nestSize/2.0f) + foodPatchSize/1 && pos.GetY() > -(nestSize/2.0f) - foodPatchSize/1 && pos.GetY() < (nestSize/2.0f) + foodPatchSize/1) { return true; } return false;*/ if(pos.GetX() > -(nestSize/2.0f) - CLOSE_TO_NEST_PARAMETER && pos.GetX() < (nestSize/2.0f) + CLOSE_TO_NEST_PARAMETER && pos.GetY() > -(nestSize/2.0f) - CLOSE_TO_NEST_PARAMETER && pos.GetY() < (nestSize/2.0f) + CLOSE_TO_NEST_PARAMETER) { return true; } return false; }
void CDynamics2DVelocityControl::SetLinearVelocity(const CVector2& c_velocity) { m_ptControlBody->v.x = c_velocity.GetX(); m_ptControlBody->v.y = c_velocity.GetY(); }
void CCameraDefaultSensor::Init(TConfigurationNode& t_tree) { try { /* Parent class init */ CCI_CameraSensor::Init(t_tree); /* Show the frustums */ GetNodeAttributeOrDefault(t_tree, "show_frustum", m_bShowFrustum, m_bShowFrustum); /* For each camera */ TConfigurationNodeIterator itCamera("camera"); for(itCamera = itCamera.begin(&t_tree); itCamera != itCamera.end(); ++itCamera) { /* Get camera indentifier */ std::string strId; GetNodeAttribute(*itCamera, "id", strId); /* Parse and look up the anchor */ std::string strAnchorId; GetNodeAttribute(*itCamera, "anchor", strAnchorId); SAnchor& sAnchor = m_pcEmbodiedEntity->GetAnchor(strAnchorId); /* parse the offset */ CVector3 cOffsetPosition; CQuaternion cOffsetOrientation; GetNodeAttribute(*itCamera, "position", cOffsetPosition); GetNodeAttribute(*itCamera, "orientation", cOffsetOrientation); CTransformationMatrix3 cOffset(cOffsetOrientation, cOffsetPosition); /* parse the range */ CRange<Real> cRange; GetNodeAttribute(*itCamera, "range", cRange); /* create the projection matrix */ CSquareMatrix<3> cProjectionMatrix; cProjectionMatrix.SetIdentityMatrix(); /* set the focal length */ CVector2 cFocalLength; GetNodeAttribute(*itCamera, "focal_length", cFocalLength); cProjectionMatrix(0,0) = cFocalLength.GetX(); // Fx cProjectionMatrix(1,1) = cFocalLength.GetY(); // Fy /* set the principal point */ CVector2 cPrincipalPoint; GetNodeAttribute(*itCamera, "principal_point", cPrincipalPoint); cProjectionMatrix(0,2) = cPrincipalPoint.GetX(); // Px cProjectionMatrix(1,2) = cPrincipalPoint.GetY(); // Py /* set the distortion parameters */ /* CMatrix<1,5> cDistortionParameters; std::string strDistortionParameters; Real pfDistortionParameters[3]; GetNodeAttribute(*itCamera, "distortion_parameters", strDistortionParameters); ParseValues<Real>(strDistortionParameters, 3, pfDistortionParameters, ','); cDistortionParameters(0,0) = pfDistortionParameters[0]; // K1 cDistortionParameters(0,1) = pfDistortionParameters[1]; // K2 cDistortionParameters(0,4) = pfDistortionParameters[2]; // K3 */ /* parse the resolution */ CVector2 cResolution; GetNodeAttribute(*itCamera, "resolution", cResolution); /* create and initialise the algorithms */ std::vector<CCameraSensorSimulatedAlgorithm*> vecSimulatedAlgorithms; std::vector<CCI_CameraSensorAlgorithm*> vecAlgorithms; TConfigurationNodeIterator itAlgorithm; for(itAlgorithm = itAlgorithm.begin(&(*itCamera)); itAlgorithm != itAlgorithm.end(); ++itAlgorithm) { /* create the algorithm */ CCameraSensorSimulatedAlgorithm* pcAlgorithm = CFactory<CCameraSensorSimulatedAlgorithm>::New(itAlgorithm->Value()); /* check that algorithm inherits from a control interface */ CCI_CameraSensorAlgorithm* pcCIAlgorithm = dynamic_cast<CCI_CameraSensorAlgorithm*>(pcAlgorithm); if(pcCIAlgorithm == nullptr) { THROW_ARGOSEXCEPTION("Algorithm \"" << itAlgorithm->Value() << "\" does not inherit from CCI_CameraSensorAlgorithm"); } /* initialize the algorithm's control interface */ pcCIAlgorithm->Init(*itAlgorithm); /* store pointers to the algorithms */ vecSimulatedAlgorithms.push_back(pcAlgorithm); vecAlgorithms.push_back(pcCIAlgorithm); } /* create the simulated sensor */ m_vecSensors.emplace_back(sAnchor, cOffset, cRange, cProjectionMatrix, cResolution, vecSimulatedAlgorithms); /* create the sensor's control interface */ m_vecInterfaces.emplace_back(strId, vecAlgorithms); } } catch(CARGoSException& ex) { THROW_ARGOSEXCEPTION_NESTED("Error initializing camera sensor", ex); } Update(); }
/***** * Sets target East of the robot's current target. *****/ void DSA_controller::SetTargetE(char x){ CVector2 position = GetTarget(); target = CVector2(position.GetX(),position.GetY()-stepSize); }