bool Mbfo::isRoadClear(const CVector2& obstacleProximity) {
CDegrees obstacleAngle(ToDegrees(obstacleProximity.Angle()));
CDegrees safeAngle(150.0f);
return (minAngleFromObstacle.WithinMinBoundIncludedMaxBoundIncluded(obstacleAngle)
&& obstacleProximity.Length() < minDistanceFromObstacle) ||
(obstacleAngle < -safeAngle || obstacleAngle > safeAngle);
}
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 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;
}
}
bool CKinematics2DCollisionCircle::CheckIntersectionWithRay(Real& f_distance, const CRay& c_ray) const {
CSegment c_segment = c_ray.ProjectOntoXY();
CVector2 c_closest_point, c_closest_segment_point;
c_segment.GetMinimumDistancePoints( m_cPosition, c_closest_point, c_closest_segment_point );
Real f_min_segment_distance = Distance(m_cPosition,c_closest_segment_point);
if( f_min_segment_distance > m_fRadius ) {
return false;
}
// see http://local.wasp.uwa.edu.au/~pbourke/geometry/sphereline/
CVector2 dp = c_segment.GetEnd() - c_segment.GetStart();
Real a = dp.SquareLength();
Real b = 2 * dp.DotProduct(c_segment.GetStart() - m_cPosition);
Real c = (m_cPosition.SquareLength() +
c_segment.GetStart().SquareLength() -
2 * m_cPosition.DotProduct(c_segment.GetStart()) -
m_fRadius*m_fRadius);
Real bb4ac = b*b - 4*a*c;
Real mu1 = (-b + sqrt(bb4ac)) / (2 * a);
Real mu2 = (-b - sqrt(bb4ac)) / (2 * a);
Real mu = Min(mu1,mu2);
if( mu < 0 || mu > 1 ) mu = Max(mu1,mu2);
f_distance = mu * c_ray.GetLength();
return true;
}
void CBTFootbotRecruiterRootBehavior::GoToFood(){
CVector2 tmp = m_pcObstacleAvoidance->GetVector();
tmp += m_pcOdometry->GetReversedLocationVector();
tmp.Normalize();
GoToVector(tmp);
m_pcOdometry->Step(*c_robot_state);
}
/**
* 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;
}
/**
* 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;
}
CVector2 Agent::relativePositionOfObstacleAt(CVector2 &obstaclePosition, Real obstacleRadius, Real &distance) {
CVector2 relativePosition = obstaclePosition - position;
distance = relativePosition.Length();
Real minDistance = (radius + obstacleRadius) + 0.002;
if (distance < minDistance) {
//too near, cannot penetrate in an obstacle (footbot or human)
relativePosition = relativePosition / distance * minDistance;
obstaclePosition = position + relativePosition;
distance = minDistance;
}
return relativePosition;
}
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 CFootBotUN::updateDesideredVelocity() {
CVector2 agentToTarget = targetPosition - position;
CRadians a0 = agentToTarget.Angle() - angle;
DEBUG_CONTROLLER("updateDesideredVelocity to %.2f, state = %d \r\n",a0.GetValue(),state);
agent->targetPosition = targetPosition;
agent->updateDesideredVelocity();
printf("=> desidered speed %.2f, desidered angle %.2f \n", agent->desideredSpeed, agent->desideredAngle.GetValue());
}
bool iAnt_controller::checkDistanceY() {
cout << "checkDistanceY\n";
CVector2 curPosition = GetPosition();
cout << "curPositionY" << curPosition << '\n';
bool stop = false;
//cout << "curPosition" << curPosition.GetY() << '\n';
float distanceY = curPosition.GetX()- data->NestPosition.GetX();
cout << "distance: " << distanceY << "\n";
if (distanceY > 0.3)
{ cout << "line 115";
stop = true;
}
return stop;
}
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);
/////////////////////////////////////////////////////////////////
}
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;
}
CVector2 CFootBotFlocking::FlockingVector() {
/* Get the camera readings */
const CCI_ColoredBlobOmnidirectionalCameraSensor::SReadings& sReadings = m_pcCamera->GetReadings();
/* Go through the camera readings to calculate the flocking interaction vector */
if(! sReadings.BlobList.empty()) {
CVector2 cAccum;
Real fLJ;
size_t unBlobsSeen = 0;
for(size_t i = 0; i < sReadings.BlobList.size(); ++i) {
/*
* The camera perceives the light as a yellow blob
* The robots have their red beacon on
* So, consider only red blobs
* In addition: consider only the closest neighbors, to avoid
* attraction to the farthest ones. Taking 180% of the target
* distance is a good rule of thumb.
*/
if(sReadings.BlobList[i]->Color == CColor::RED &&
sReadings.BlobList[i]->Distance < m_sFlockingParams.TargetDistance * 1.80f) {
/*
* Take the blob distance and angle
* With the distance, calculate the Lennard-Jones interaction force
* Form a 2D vector with the interaction force and the angle
* Sum such vector to the accumulator
*/
/* Calculate LJ */
fLJ = m_sFlockingParams.GeneralizedLennardJones(sReadings.BlobList[i]->Distance);
/* Sum to accumulator */
cAccum += CVector2(fLJ,
sReadings.BlobList[i]->Angle);
/* Increment the blobs seen counter */
++unBlobsSeen;
}
}
/* Divide the accumulator by the number of blobs seen */
cAccum /= unBlobsSeen;
/* Clamp the length of the vector to the max speed */
if(cAccum.Length() > m_sWheelTurningParams.MaxSpeed) {
cAccum.Normalize();
cAccum *= m_sWheelTurningParams.MaxSpeed;
}
return cAccum;
}
else {
return CVector2();
}
}
CVector2 CFootBotFlocking::VectorToLight() {
/* Get light readings */
const CCI_FootBotLightSensor::TReadings& tReadings = m_pcLight->GetReadings();
/* Calculate a normalized vector that points to the closest light */
CVector2 cAccum;
for(size_t i = 0; i < tReadings.size(); ++i) {
cAccum += CVector2(tReadings[i].Value, tReadings[i].Angle);
}
if(cAccum.Length() > 0.0f) {
/* Make the vector long as 1/4 of the max speed */
cAccum.Normalize();
cAccum *= 0.25f * m_sWheelTurningParams.MaxSpeed;
}
return cAccum;
}
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;
}
CColor CFloorPowerFunctions::GetFloorColor(const CVector2& c_position_on_plane) {
if(c_position_on_plane.GetX() < -1.0f) {
return CColor::GRAY50;
}
return CColor::GRAY20;
}
CVector2 CFootBotForaging::CalculateVectorToLight() {
/* Get readings from light sensor */
const CCI_FootBotLightSensor::TReadings& tLightReads = m_pcLight->GetReadings();
/* Sum them together */
CVector2 cAccumulator;
for(size_t i = 0; i < tLightReads.size(); ++i) {
cAccumulator += CVector2(tLightReads[i].Value, tLightReads[i].Angle);
}
/* If the light was perceived, return the vector */
if(cAccumulator.Length() > 0.0f) {
return CVector2(1.0f, cAccumulator.Angle());
}
/* Otherwise, return zero */
else {
return CVector2();
}
}
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);
}
}
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++;
}
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);
}
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.");
}
}
CVector2 CFootBotForaging::DiffusionVector(bool& b_collision) {
/* Get readings from proximity sensor */
const CCI_FootBotProximitySensor::TReadings& tProxReads = m_pcProximity->GetReadings();
/* Sum them together */
CVector2 cDiffusionVector;
for(size_t i = 0; i < tProxReads.size(); ++i) {
cDiffusionVector += CVector2(tProxReads[i].Value, tProxReads[i].Angle);
}
/* If the angle of the vector is small enough and the closest obstacle
is far enough, ignore the vector and go straight, otherwise return
it */
if(m_sDiffusionParams.GoStraightAngleRange.WithinMinBoundIncludedMaxBoundIncluded(cDiffusionVector.Angle()) &&
cDiffusionVector.Length() < m_sDiffusionParams.Delta ) {
b_collision = false;
return CVector2::X;
}
else {
b_collision = true;
cDiffusionVector.Normalize();
return -cDiffusionVector;
}
}
static int BuzzGoToC(buzzvm_t vm) {
/* Push the vector components */
buzzvm_lload(vm, 1);
buzzvm_lload(vm, 2);
/* Create a new vector with that */
buzzobj_t tX = buzzvm_stack_at(vm, 2);
buzzobj_t tY = buzzvm_stack_at(vm, 1);
CVector2 cDir;
if(tX->o.type == BUZZTYPE_INT) cDir.SetX(tX->i.value);
else if(tX->o.type == BUZZTYPE_FLOAT) cDir.SetX(tX->f.value);
else {
buzzvm_seterror(vm,
BUZZVM_ERROR_TYPE,
"gotoc(x,y): expected %s, got %s in first argument",
buzztype_desc[BUZZTYPE_FLOAT],
buzztype_desc[tX->o.type]
);
return vm->state;
}
if(tY->o.type == BUZZTYPE_INT) cDir.SetY(tY->i.value);
else if(tY->o.type == BUZZTYPE_FLOAT) cDir.SetY(tY->f.value);
else {
buzzvm_seterror(vm,
BUZZVM_ERROR_TYPE,
"gotoc(x,y): expected %s, got %s in second argument",
buzztype_desc[BUZZTYPE_FLOAT],
buzztype_desc[tY->o.type]
);
return vm->state;
}
/* Get pointer to the controller */
buzzvm_pushs(vm, buzzvm_string_register(vm, "controller", 1));
buzzvm_gload(vm);
/* Call function */
reinterpret_cast<CBuzzControllerFootBot*>(buzzvm_stack_at(vm, 1)->u.value)->SetWheelSpeedsFromVector(cDir);
return buzzvm_ret0(vm);
}
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;
}
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;
}
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);
}
void CBuzzControllerEFootBot::SetWheelSpeedsFromVector(const CVector2& c_heading) {
/* Get the heading angle */
CRadians cHeadingAngle = c_heading.Angle().SignedNormalize();
/* Get the length of the heading vector */
Real fHeadingLength = c_heading.Length();
/* Clamp the speed so that it's not greater than MaxSpeed */
Real fBaseAngularWheelSpeed = Min<Real>(fHeadingLength, m_sWheelTurningParams.MaxSpeed);
/* Turning state switching conditions */
if(Abs(cHeadingAngle) <= m_sWheelTurningParams.NoTurnAngleThreshold) {
/* No Turn, heading angle very small */
m_sWheelTurningParams.TurningMechanism = SWheelTurningParams::NO_TURN;
}
else if(Abs(cHeadingAngle) > m_sWheelTurningParams.HardTurnOnAngleThreshold) {
/* Hard Turn, heading angle very large */
m_sWheelTurningParams.TurningMechanism = SWheelTurningParams::HARD_TURN;
}
else if(m_sWheelTurningParams.TurningMechanism == SWheelTurningParams::NO_TURN &&
Abs(cHeadingAngle) > m_sWheelTurningParams.SoftTurnOnAngleThreshold) {
/* Soft Turn, heading angle in between the two cases */
m_sWheelTurningParams.TurningMechanism = SWheelTurningParams::SOFT_TURN;
}
/* Wheel speeds based on current turning state */
Real fSpeed1, fSpeed2;
switch(m_sWheelTurningParams.TurningMechanism) {
case SWheelTurningParams::NO_TURN: {
/* Just go straight */
fSpeed1 = fBaseAngularWheelSpeed;
fSpeed2 = fBaseAngularWheelSpeed;
break;
}
case SWheelTurningParams::SOFT_TURN: {
/* Both wheels go straight, but one is faster than the other */
Real fSpeedFactor = (m_sWheelTurningParams.HardTurnOnAngleThreshold - Abs(cHeadingAngle)) / m_sWheelTurningParams.HardTurnOnAngleThreshold;
fSpeed1 = fBaseAngularWheelSpeed - fBaseAngularWheelSpeed * (1.0 - fSpeedFactor);
fSpeed2 = fBaseAngularWheelSpeed + fBaseAngularWheelSpeed * (1.0 - fSpeedFactor);
break;
}
case SWheelTurningParams::HARD_TURN: {
/* Opposite wheel speeds */
fSpeed1 = -m_sWheelTurningParams.MaxSpeed;
fSpeed2 = m_sWheelTurningParams.MaxSpeed;
break;
}
}
/* Apply the calculated speeds to the appropriate wheels */
Real fLeftWheelSpeed, fRightWheelSpeed;
if(cHeadingAngle > CRadians::ZERO) {
/* Turn Left */
fLeftWheelSpeed = fSpeed1;
fRightWheelSpeed = fSpeed2;
}
else {
/* Turn Right */
fLeftWheelSpeed = fSpeed2;
fRightWheelSpeed = fSpeed1;
}
/* Finally, set the wheel speeds */
m_pcWheels->SetLinearVelocity(fLeftWheelSpeed, fRightWheelSpeed);
}
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";
}
}