void RangeData::merge(const RangeData& other, bool removeDups)
{
RangeData mergedData;
mergedData.resize(size() + other.size());
std::merge(begin(), end(),
other.begin(), other.end(),
mergedData.begin(), earlier);
RangeData::iterator newEnd = mergedData.end();
if (removeDups)
{
newEnd = std::unique(mergedData.begin(), mergedData.end());
}
m_data.resize(std::distance(mergedData.begin(), newEnd));
std::copy(mergedData.begin(), newEnd, m_data.begin());
}
int obstacleAvoidance(Robot *robot, Point *at, Vector *velocity) {
RangeData *rdata = robot->GetRangeData();
RangeData::iterator it;
Vector rForce = (Vector){ 0.0, 0.0 }, force;
const Vector currentVelocity = *robot->GetVelocity();
double ignoreAngle = dtor(45);
if ( robot->Ranger() ) {
ignoreAngle = dtor(35);
}
// Calculate a repelling force (rForce) to push our robot away
// from obstacles.
for ( it = rdata->begin(); it < rdata->end(); it++ ) {
if ( it->magnitude == it->magnitude && it->magnitude > 0.0 &&
it->magnitude < 1.0 && it->direction > ignoreAngle &&
it->direction < (dtor(180) - ignoreAngle) ) {
//std::cout << "Obstacle: ";
//it->print();
if (rForce.magnitude == 0.0) rForce = *it;
else rForce = rForce + (*it);
}
}
rForce.direction -= (.5 * PI);
rForce.direction += robot->GetVelocity()->direction;
force = (-rForce) + *robot->GetVelocity();
if ( fabs(force.direction - robot->GetVelocity()->direction) > MIN_TURNRATE &&
velocity->magnitude > 0.0 && rForce.magnitude > 0.0) {
velocity->direction = force.direction;
turning = true;
}
}
