/** saturates the magnitude of a vector */
static Geometry2d::Point saturate(Geometry2d::Point value, float max) {
float mag = value.mag();
if (mag > fabs(max)) {
return value.normalized() * fabs(max);
}
return value;
}
//// Fixed Step Tree ////
Tree::Point* FixedStepTree::extend(Geometry2d::Point pt, Tree::Point* base) {
// if we don't have a base point, try to find a close point
if (!base) {
base = nearest(pt);
if (!base) {
return nullptr;
}
}
Geometry2d::Point delta = pt - base->pos;
float d = delta.mag();
Geometry2d::Point pos;
if (d < step) {
pos = pt;
} else {
pos = base->pos + delta / d * step;
}
// Check for obstacles.
// moveHit is the set of obstacles that this move touches.
// If this move touches any obstacles that the starting point didn't already
// touch,
// it has entered an obstacle and will be rejected.
std::set<shared_ptr<Geometry2d::Shape>> moveHit =
_obstacles->hitSet(Geometry2d::Segment(pos, base->pos));
if (!moveHit.empty()) {
// We only care if there are any items in moveHit that are not in
// point->hit, so
// we don't store the result of set_difference.
try {
set_difference(
moveHit.begin(), moveHit.end(), base->hit.begin(),
base->hit.end(),
ExceptionIterator<std::shared_ptr<Geometry2d::Shape>>());
} catch (exception& e) {
// We hit a new obstacle
return nullptr;
}
}
// Allow this point to be added to the tree
Point* p = new Point(pos, base);
p->hit = std::move(moveHit);
points.push_back(p);
return p;
}
/**
* Generates a Cubic Bezier Path based on Albert's random Bezier Velocity Path
* Algorithm
*/
std::vector<InterpolatedPath::Entry> RRTPlanner::generateVelocityPath(
const std::vector<CubicBezierControlPoints>& controlPoints,
const MotionConstraints& motionConstraints, Geometry2d::Point vi,
Geometry2d::Point vf, int interpolations) {
// Interpolate Through Bezier Path
vector<Point> newPoints, newPoints1stDerivative, newPoints2ndDerivative;
vector<float> newPointsCurvature, newPointsDistance, newPointsSpeed;
float totalDistance = 0;
const float maxAceleration = motionConstraints.maxAcceleration;
for (const CubicBezierControlPoints& controlPoint : controlPoints) {
Point p0 = controlPoint.p0;
Point p1 = controlPoint.p1;
Point p2 = controlPoint.p2;
Point p3 = controlPoint.p3;
for (int j = 0; j < interpolations; j++) {
float t = (((float)j / (float)(interpolations)));
Geometry2d::Point pos =
pow(1.0 - t, 3) * p0 + 3.0 * pow(1.0 - t, 2) * t * p1 +
3 * (1.0 - t) * pow(t, 2) * p2 + pow(t, 3) * p3;
// Derivitive 1
// 3 k (-(A (-1 + k t)^2) + k t (2 C - 3 C k t + D k t) + B (1 - 4 k
// t + 3 k^2 t^2))
Geometry2d::Point d1 = 3 * pow(1 - t, 2) * (p1 - p0) +
6 * (1 - t) * t * (p2 - p1) +
3 * pow(t, 2) * (p3 - p2);
// Derivitive 2
// https://en.wikipedia.org/wiki/B%C3%A9zier_curve#Cubic_B.C3.A9zier_curves
// B''(t) = 6(1-t)(P2 - 2*P1 + P0) + 6*t(P3 - 2 * P2 + P1)
Geometry2d::Point d2 =
6 * (1 - t) * (p2 - 2 * p1 + p0) + 6 * t * (p3 - 2 * p2 + p1);
// https://en.wikipedia.org/wiki/Curvature#Local_expressions
// K = |x'*y'' - y'*x''| / (x'^2 + y'^2)^(3/2)
float curvature =
std::abs(d1.x * d2.y - d1.y * d2.x) /
std::pow(std::pow(d1.x, 2) + std::pow(d1.y, 2), 1.5);
// Handle 0 velocity case
if (isnan(curvature)) {
curvature = 0;
}
assert(curvature >= 0);
if (!newPoints.empty()) {
float distance = pos.distTo(newPoints.back());
totalDistance += distance;
}
newPointsDistance.push_back(totalDistance);
newPoints.push_back(pos);
newPoints1stDerivative.push_back(d1);
newPoints2ndDerivative.push_back(d2);
newPointsCurvature.push_back(curvature);
// Isolated maxSpeed based on Curvature
// Curvature = 1/Radius of Curvature
// vmax = sqrt(acceleartion/abs(Curvature))
float constantMaxSpeed = std::sqrt(maxAceleration / curvature);
newPointsSpeed.push_back(constantMaxSpeed);
}
}
// Get last point in Path
CubicBezierControlPoints lastControlPoint = controlPoints.back();
Point p0 = lastControlPoint.p0;
Point p1 = lastControlPoint.p1;
Point p2 = lastControlPoint.p2;
Point p3 = lastControlPoint.p3;
Point pos = p3;
Point d1 = vf;
Geometry2d::Point d2 = 6 * (1) * (p3 - 2 * p2 + p1);
float curvature = std::abs(d1.x * d2.y - d1.y * d2.x) /
std::pow(std::pow(d1.x, 2) + std::pow(d1.y, 2), 1.5);
// handle 0 velcoity case
if (isnan(curvature)) {
curvature = 0;
}
totalDistance += pos.distTo(newPoints.back());
newPoints.push_back(pos);
newPoints1stDerivative.push_back(vf);
newPoints2ndDerivative.push_back(d2);
newPointsCurvature.push_back(curvature);
newPointsDistance.push_back(totalDistance);
newPointsSpeed[0] = vi.mag();
newPointsSpeed.push_back(vf.mag());
// Velocity Profile Generation
// Forward Smoothing
const float size = newPoints.size();
assert(size == newPoints.size());
assert(size == newPoints1stDerivative.size());
assert(size == newPoints2ndDerivative.size());
assert(size == newPointsDistance.size());
assert(size == newPointsSpeed.size());
assert(size == newPointsCurvature.size());
// Generate Velocity Profile from Interpolation of Bezier Path
// Forward Constraints
for (int i = 1; i < size; i++) {
int i1 = i - 1;
int i2 = i;
newPointsSpeed[i2] = oneStepLimitAcceleration(
maxAceleration, newPointsDistance[i1], newPointsSpeed[i1],
newPointsCurvature[i1], newPointsDistance[i2], newPointsSpeed[i2],
newPointsCurvature[i2]);
}
// Backwards Constraints
for (int i = size - 2; i >= 0; i--) {
int i1 = i + 1;
int i2 = i;
newPointsSpeed[i2] = oneStepLimitAcceleration(
maxAceleration, newPointsDistance[i1], newPointsSpeed[i1],
newPointsCurvature[i1], newPointsDistance[i2], newPointsSpeed[i2],
newPointsCurvature[i2]);
}
float totalTime = 0;
vector<InterpolatedPath::Entry> entries;
for (int i = 0; i < size; i++) {
float currentSpeed = newPointsSpeed[i];
float distance = newPointsDistance[i];
if (i != 0) {
distance -= newPointsDistance[i - 1];
float averageSpeed = (currentSpeed + newPointsSpeed[i - 1]) / 2.0;
float deltaT = distance / averageSpeed;
totalTime += deltaT;
}
Point point = newPoints[i];
Point vel = newPoints1stDerivative[i].normalized();
entries.emplace_back(MotionInstant(point, vel * currentSpeed),
totalTime);
}
return entries;
}
vector<CubicBezierControlPoints> RRTPlanner::generateCubicBezierPath(
const vector<Geometry2d::Point>& points,
const MotionConstraints& motionConstraints, Geometry2d::Point vi,
Geometry2d::Point vf, const boost::optional<vector<float>>& times) {
size_t length = points.size();
size_t curvesNum = length - 1;
vector<double> pointsX(length);
vector<double> pointsY(length);
vector<double> ks(length - 1);
vector<double> ks2(length - 1);
for (int i = 0; i < length; i++) {
pointsX[i] = points[i].x;
pointsY[i] = points[i].y;
}
const float startSpeed = vi.mag();
const float endSpeed = vf.mag();
if (times) {
assert(times->size() == points.size());
for (int i = 0; i < curvesNum; i++) {
ks[i] = 1.0 / (times->at(i + 1) - times->at(i));
ks2[i] = ks[i] * ks[i];
if (std::isnan(ks[i])) {
debugThrow(
"Something went wrong. Points are too close to each other "
"probably");
return vector<CubicBezierControlPoints>();
}
}
} else {
for (int i = 0; i < curvesNum; i++) {
ks[i] = 1.0 / (getTime(points, i + 1, motionConstraints, startSpeed,
endSpeed) -
getTime(points, i, motionConstraints, startSpeed,
endSpeed));
ks2[i] = ks[i] * ks[i];
if (std::isnan(ks[i])) {
debugThrow(
"Something went wrong. Points are too close to each other "
"probably");
return vector<CubicBezierControlPoints>();
}
}
}
VectorXd solutionX =
RRTPlanner::cubicBezierCalc(vi.x, vf.x, pointsX, ks, ks2);
VectorXd solutionY =
RRTPlanner::cubicBezierCalc(vi.y, vf.y, pointsY, ks, ks2);
vector<CubicBezierControlPoints> path;
for (int i = 0; i < curvesNum; i++) {
Point p0 = points[i];
Point p1 = Geometry2d::Point(solutionX(i * 2), solutionY(i * 2));
Point p2 =
Geometry2d::Point(solutionX(i * 2 + 1), solutionY(i * 2 + 1));
Point p3 = points[i + 1];
path.emplace_back(p0, p1, p2, p3);
}
return path;
}
WorldBall::WorldBall(RJ::Time calcTime, std::list<KalmanBall> kalmanBalls)
: isValid(true), time(calcTime){
Geometry2d::Point posAvg = Geometry2d::Point(0, 0);
Geometry2d::Point velAvg = Geometry2d::Point(0, 0);
double totalPosWeight = 0;
double totalVelWeight = 0;
// Below 1 would invert the ratio of scaling
// Above 2 would just be super noisy
if (*ball_merger_power < 1 || *ball_merger_power > 2) {
std::cout
<< "WARN: ball_merger_power should be between 1 and 2"
<< std::endl;
}
if (kalmanBalls.size() == 0) {
std::cout
<< "ERROR: Zero balls are given to the WorldBall constructor"
<< std::endl;
isValid = false;
pos = posAvg;
vel = velAvg;
posCov = 0;
velCov = 0;
return;
}
for (KalmanBall& ball : kalmanBalls) {
// Get the covariance of everything
// AKA how well we can predict the next measurement
Geometry2d::Point posCov = ball.getPosCov();
Geometry2d::Point velCov = ball.getVelCov();
// Std dev of each state
// Lower std dev gives better idea of true values
Geometry2d::Point posStdDev;
Geometry2d::Point velStdDev;
posStdDev.x() = std::sqrt(posCov.x());
posStdDev.y() = std::sqrt(posCov.y());
velStdDev.x() = std::sqrt(velCov.x());
velStdDev.y() = std::sqrt(velCov.y());
// Inversely proportional to how much the filter has been updated
double filterUncertantity = 1.0 / ball.getHealth();
// How good of pos/vel estimation in total
// (This is less efficient than just doing the sqrt(x_cov + y_cov),
// but it's a little more clear math-wise)
double posUncertantity = posStdDev.mag();
double velUncertantity = velStdDev.mag();
// Weight better estimates higher
double filterPosWeight = std::pow(posUncertantity * filterUncertantity,
-*ball_merger_power);
double filterVelWeight = std::pow(velUncertantity * filterUncertantity,
-*ball_merger_power);
posAvg += filterPosWeight * ball.getPos();
velAvg += filterVelWeight * ball.getVel();
totalPosWeight += filterPosWeight;
totalVelWeight += filterVelWeight;
}
posAvg /= totalPosWeight;
velAvg /= totalVelWeight;
pos = posAvg;
vel = velAvg;
posCov = totalPosWeight / kalmanBalls.size();
velCov = totalVelWeight / kalmanBalls.size();
ballComponents = kalmanBalls;
}
void FieldView::drawTeamSpace(QPainter& p) {
// Get the latest LogFrame
const LogFrame* frame = _history->at(0).get();
if (showTeamNames) {
// Draw Team Names
QFont savedFont = p.font();
QFont fontstyle = p.font();
fontstyle.setPointSize(20);
p.setFont(fontstyle);
p.setPen(bluePen);
drawText(p, QPointF(0, 4.75), QString(frame->team_name_blue().c_str()),
true); // Blue
p.setPen(yellowPen);
drawText(p, QPointF(0, 1.75),
QString(frame->team_name_yellow().c_str()),
true); // Yellow
p.setFont(savedFont);
}
// Block off half the field
if (!frame->use_our_half()) {
const float FX = Field_Dimensions::Current_Dimensions.FloorWidth() / 2;
const float FY1 = -Field_Dimensions::Current_Dimensions.Border();
const float FY2 = Field_Dimensions::Current_Dimensions.Length() / 2;
p.fillRect(QRectF(QPointF(-FX, FY1), QPointF(FX, FY2)),
QColor(0, 0, 0, 128));
}
if (!frame->use_opponent_half()) {
const float FX = Field_Dimensions::Current_Dimensions.FloorWidth() / 2;
const float FY1 = Field_Dimensions::Current_Dimensions.Length() / 2;
const float FY2 = Field_Dimensions::Current_Dimensions.Length() +
Field_Dimensions::Current_Dimensions.Border();
p.fillRect(QRectF(QPointF(-FX, FY1), QPointF(FX, FY2)),
QColor(0, 0, 0, 128));
}
if (showCoords) {
drawCoords(p);
}
// History
p.setBrush(Qt::NoBrush);
QPainterPath ballTrail;
for (unsigned int i = 0; i < 200 && i < _history->size(); ++i) {
const LogFrame* oldFrame = _history->at(i).get();
if (oldFrame && oldFrame->has_ball()) {
QPointF pos = qpointf(oldFrame->ball().pos());
if (i == 0)
ballTrail.moveTo(pos);
else
ballTrail.lineTo(pos);
}
}
QPen ballTrailPen(ballColor, 0.03);
ballTrailPen.setCapStyle(Qt::RoundCap);
p.setPen(ballTrailPen);
p.drawPath(ballTrail);
// Debug lines
for (const DebugPath& path : frame->debug_paths()) {
if (path.layer() < 0 || layerVisible(path.layer())) {
tempPen.setColor(qcolor(path.color()));
p.setPen(tempPen);
std::vector<QPointF> pts;
for (int i = 0; i < path.points_size(); ++i) {
pts.push_back(qpointf(path.points(i)));
}
p.drawPolyline(pts.data(), pts.size());
}
}
for (const DebugRobotPath& path : frame->debug_robot_paths()) {
if (path.layer() < 0 || layerVisible(path.layer())) {
for (int i = 0; i < path.points_size() - 1; ++i) {
const DebugRobotPath::DebugRobotPathPoint& from =
path.points(i);
const DebugRobotPath::DebugRobotPathPoint& to =
path.points(i + 1);
Geometry2d::Point avgVel =
(Geometry2d::Point(path.points(i).vel()) +
Geometry2d::Point(path.points(i + 1).vel())) /
2;
float pcntMaxSpd =
avgVel.mag() / MotionConstraints::defaultMaxSpeed();
QColor mixedColor(std::min((int)(255 * pcntMaxSpd), 255), 0,
std::min((int)(255 * (1 - pcntMaxSpd)), 255));
QPen pen(mixedColor);
pen.setCapStyle(Qt::RoundCap);
pen.setWidthF(0.03);
p.setPen(pen);
const Geometry2d::Point fromPos = Geometry2d::Point(from.pos());
const Geometry2d::Point toPos = Geometry2d::Point(to.pos());
p.drawLine(fromPos.toQPointF(), toPos.toQPointF());
}
}
}
// Debug circles
for (const DebugCircle& c : frame->debug_circles()) {
if (c.layer() < 0 || layerVisible(c.layer())) {
tempPen.setColor(c.color());
p.setPen(tempPen);
p.drawEllipse(qpointf(c.center()), c.radius(), c.radius());
}
}
// Debug arcs
for (const DebugArc& a : frame->debug_arcs()) {
if (a.layer() < 0 || layerVisible(a.layer())) {
tempPen.setColor(a.color());
p.setPen(tempPen);
auto c = a.center();
auto t1 = a.start();
auto t2 = a.end();
auto R = a.radius();
QRectF rect;
rect.setX(-R + c.x());
rect.setY(-R + c.y());
rect.setWidth(R * 2);
rect.setHeight(R * 2);
t1 *= -(180 / M_PI) * 16;
t2 *= -(180 / M_PI) * 16;
p.drawArc(rect, t1, t2 - t1);
}
}
// Debug text
for (const DebugText& text : frame->debug_texts()) {
if (text.layer() < 0 || layerVisible(text.layer())) {
tempPen.setColor(text.color());
p.setPen(tempPen);
drawText(p, qpointf(text.pos()),
QString::fromStdString(text.text()), text.center());
}
}
// Debug polygons
p.setPen(Qt::NoPen);
for (const DebugPath& path : frame->debug_polygons()) {
if (path.layer() < 0 || layerVisible(path.layer())) {
if (path.points_size() < 3) {
fprintf(stderr, "Ignoring DebugPolygon with %d points\n",
path.points_size());
continue;
}
QColor color = qcolor(path.color());
color.setAlpha(64);
p.setBrush(color);
std::vector<QPointF> pts;
for (int i = 0; i < path.points_size(); ++i) {
pts.push_back(qpointf(path.points(i)));
}
p.drawConvexPolygon(pts.data(), pts.size());
}
}
p.setBrush(Qt::NoBrush);
// maps robots to their comet trails, so we can draw a path of where each
// robot has been over the past X frames the pair used as a key is of the
// form (team, robot_id). Blue team = 1, yellow = 2. we only draw trails
// for robots that exist in the current frame
map<pair<int, int>, QPainterPath> cometTrails;
/// populate @cometTrails with the past locations of each robot
int pastLocationCount = 40; // number of past locations to show
for (int i = 0; i < pastLocationCount + 1 && i < _history->size(); i++) {
const LogFrame* oldFrame = _history->at(i).get();
if (oldFrame) {
for (const LogFrame::Robot& r : oldFrame->self()) {
pair<int, int> key(1, r.shell());
if (cometTrails.find(key) != cometTrails.end() || i == 0) {
QPointF pt = qpointf(r.pos());
if (i == 0)
cometTrails[key].moveTo(pt);
else
cometTrails[key].lineTo(pt);
}
}
for (const LogFrame::Robot& r : oldFrame->self()) {
pair<int, int> key(2, r.shell());
if (cometTrails.find(key) != cometTrails.end() || i == 0) {
QPointF pt = qpointf(r.pos());
if (i == 0)
cometTrails[key].moveTo(pt);
else
cometTrails[key].lineTo(pt);
}
}
}
}
// draw robot comet trails
const float cometTrailPenSize = 0.07;
for (auto& kv : cometTrails) {
QColor color = kv.first.first == 1 ? Qt::blue : Qt::yellow;
QPen pen(color, cometTrailPenSize);
pen.setCapStyle(Qt::RoundCap);
p.setPen(pen);
p.drawPath(kv.second);
}
// Text positioning vectors
QPointF rtX = qpointf(Geometry2d::Point(0, 1).rotated(-_rotate * 90));
QPointF rtY = qpointf(Geometry2d::Point(-1, 0).rotated(-_rotate * 90));
// Opponent robots
for (const LogFrame::Robot& r : frame->opp()) {
drawRobot(p, !frame->blue_team(), r.shell(), qpointf(r.pos()),
r.angle(), r.ball_sense_status() == HasBall);
}
// Our robots
int manualID = frame->manual_id();
for (const LogFrame::Robot& r : frame->self()) {
QPointF center = qpointf(r.pos());
bool faulty = false;
if (r.has_ball_sense_status() && (r.ball_sense_status() == Dazzled ||
r.ball_sense_status() == Failed)) {
faulty = true;
}
if (r.has_kicker_works() && !r.kicker_works()) {
// faulty = true;
}
for (int i = 0; i < r.motor_status().size(); ++i) {
if (r.motor_status(i) != Good) {
faulty = true;
}
}
if (r.has_battery_voltage() && r.battery_voltage() <= 14.3f) {
faulty = true;
}
drawRobot(p, frame->blue_team(), r.shell(), center, r.angle(),
r.ball_sense_status() == HasBall, faulty);
// Highlight the manually controlled robot
if (manualID == r.shell()) {
p.setPen(greenPen);
const float r = Robot_Radius + .05;
p.drawEllipse(center, r, r);
}
// Robot text
QPointF textPos = center - rtX * 0.2 - rtY * (Robot_Radius + 0.1);
for (const DebugText& text : r.text()) {
if (text.layer() < 0 || layerVisible(text.layer())) {
tempPen.setColor(text.color());
p.setPen(tempPen);
drawText(p, textPos, QString::fromStdString(text.text()),
false);
textPos -= rtY * 0.1;
}
}
}
// Current ball position and velocity
if (frame->has_ball()) {
QPointF pos = qpointf(frame->ball().pos());
QPointF vel = qpointf(frame->ball().vel());
p.setPen(ballPen);
p.setBrush(ballColor);
p.drawEllipse(QRectF(-Ball_Radius + pos.x(), -Ball_Radius + pos.y(),
Ball_Diameter, Ball_Diameter));
if (!vel.isNull()) {
p.drawLine(pos, QPointF(pos.x() + vel.x(), pos.y() + vel.y()));
}
}
}
bool Gameplay::Behaviors::GoalDefender::run()
{
if (!assigned() || !allVisible())
{
return false;
}
Geometry2d::Point ballFuture = ball().pos + ball().vel*0.1;
const float radius = .7;
//goal line, for intersection detection
Geometry2d::Segment goalLine(Geometry2d::Point(-Constants::Field::GoalWidth / 2.0f, 0),
Geometry2d::Point(Constants::Field::GoalWidth / 2.0f, 0));
// Update the target window
_winEval->exclude.clear();
for (Robot *r : _robots)
{
_winEval->exclude.push_back(r->pos());
}
_winEval->run(ballFuture, goalLine);
Window* best = 0;
float bestDist = 0;
// finds the closest segment to the ball
for (Window* window : _winEval->windows)
{
Geometry2d::Segment seg(window->segment.center(), ball().pos);
float newDist = seg.distTo(Behavior::robot()->pos());
if (seg.length() > Constants::Robot::Radius && (!best || newDist < bestDist))
{
best = window;
bestDist = newDist;
}
}
if (!best)
{
//FIXME - What if there are no windows?
return true;
}
Geometry2d::Circle arc(Geometry2d::Point(), radius);
Geometry2d::Line ballTravel(ball().pos, ballFuture);
Geometry2d::Point dest[2];
bool ballTravelIntersects = ballTravel.intersects(arc, &dest[0], &dest[1]);
Geometry2d::Point blockPoint = (dest[0].y > 0 ? dest[0] : dest[1]);
bool movingTowardsGoal = ballFuture.mag() < ball().pos.mag();
if(ballTravelIntersects && blockPoint.y > 0 && movingTowardsGoal)
{
// If ball is traveling towards the goal, block it.
state()->drawText("YY", blockPoint, Qt::magenta);
}
else
{ // Stand in the largest open window
Geometry2d::Line bestWindowLine(best->segment.center(), Geometry2d::Point(0,0));
Geometry2d::Point blockPoint = (best->segment.center().normalized()) * radius;
state()->drawText("XX", blockPoint, Qt::white);
}
/*
_winEval->exclude.push_back(Behavior::robot()->pos());
//exclude robots that arn't the defender
//_winEval->run(ball().pos, goalLine);
for (Defender *f : otherDefenders)
{
if (f->robot())
{
_winEval->exclude.push_back(f->robot()->pos());
}
}
_winEval->run(ballFuture, goalLine);
Window* best = 0;
Behavior* goalie = _gameplay->goalie();
bool needTask = false;
//pick biggest window on appropriate side
if (goalie && goalie->robot())
{
for (Window* window : _winEval->windows)
{
if (_side == Left)
{
if (!best || window->segment.center().x < goalie->robot()->pos().x)
{
best = window;
}
}
else if (_side == Right)
{
if (!best || window->segment.center().x > goalie->robot()->pos().x)
{
best = window;
}
}
}
}
else
{
//if no side parameter...stay in the middle
float bestDist = 0;
for (Window* window : _winEval->windows)
{
Geometry2d::Segment seg(window->segment.center(), ball().pos);
float newDist = seg.distTo(Behavior::robot()->pos());
if (!best || newDist < bestDist)
{
best = window;
bestDist = newDist;
}
}
}
if (best)
{
Geometry2d::Segment shootLine(ball().pos, ball().pos + ball().vel.normalized() * 7.0);
Geometry2d::Segment& winSeg = best->segment;
if (ball().vel.magsq() > 0.03 && winSeg.intersects(shootLine))
{
robot()->move(shootLine.nearestPoint(Behavior::robot()->pos()));
robot()->faceNone();
}
else
{
const float winSize = winSeg.length();
if (winSize < Constants::Ball::Radius)
{
needTask = true;
}
else
{
const float radius = .7;
Geometry2d::Circle arc(Geometry2d::Point(), radius);
Geometry2d::Line shot(winSeg.center(), ballFuture);
Geometry2d::Point dest[2];
bool intersected = shot.intersects(arc, &dest[0], &dest[1]);
if (intersected)
{
if (dest[0].y > 0)
{
Behavior::robot()->move(dest[0]);
}
else
{
Behavior::robot()->move(dest[1]);
}
Behavior::robot()->face(ballFuture);
}
else
{
needTask = true;
}
}
}
}
else
{
needTask = true;
}*/
return false;
}