bool Environment::occluded(Geometry2d::Point ball, Geometry2d::Point camera) { float camZ = 4; float ballZ = Ball_Radius; float intZ = Robot_Height; // FIXME: use actual physics engine to determine line of sight // Find where the line from the camera to the ball intersects the // plane at the top of the robots. // // intZ = (ballZ - camZ) * t + camZ float t = (intZ - camZ) / (ballZ - camZ); Geometry2d::Point intersection; intersection.x = (ball.x - camera.x) * t + camera.x; intersection.y = (ball.y - camera.y) * t + camera.y; // Return true if the intersection point is inside any robot for (const Robot* r : _blue) { if (intersection.nearPoint(r->getPosition(), Robot_Radius)) { return true; } } for (const Robot* r : _yellow) { if (intersection.nearPoint(r->getPosition(), Robot_Radius)) { return true; } } return false; }
void SimFieldView::mousePressEvent(QMouseEvent* me) { Geometry2d::Point pos = _worldToTeam * _screenToWorld * me->pos(); std::shared_ptr<LogFrame> frame = currentFrame(); if (me->button() == Qt::LeftButton && frame) { _dragRobot = -1; for (const LogFrame::Robot& r : frame->self()) { if (pos.nearPoint(r.pos(), Robot_Radius)) { _dragRobot = r.shell(); _dragRobotBlue = frame->blue_team(); break; } } for (const LogFrame::Robot& r : frame->opp()) { if (pos.nearPoint(r.pos(), Robot_Radius)) { _dragRobot = r.shell(); _dragRobotBlue = !frame->blue_team(); break; } } if (_dragRobot < 0) { placeBall(me->pos()); } _dragMode = DRAG_PLACE; } else if (me->button() == Qt::RightButton && frame) { if (frame->has_ball() && pos.nearPoint(frame->ball().pos(), Ball_Radius)) { // Drag to shoot the ball _dragMode = DRAG_SHOOT; _dragTo = pos; } else { // Look for a robot selection int newID = -1; for (int i = 0; i < frame->self_size(); ++i) { if (pos.distTo(frame->self(i).pos()) < Robot_Radius) { newID = frame->self(i).shell(); break; } } if (newID != frame->manual_id()) { robotSelected(newID); } } } }
bool Gameplay::Plays::DemoAttack::run() { Geometry2d::Point ballPos = ball().pos; set<OurRobot *> available = _gameplay->playRobots(); assignNearest(_kicker.robot, available, ballPos); // if we have kicked, we want to reset if (_kicker.done() && ball().valid && (!ballPos.nearPoint(Geometry2d::Point(0, Field_Length), Field_ArcRadius))) { _kicker.restart(); } // set flags from parameters if (_calculateChipPower->value()) _kicker.calculateChipPower(ball().pos.distTo(_kicker.target().center())); _kicker.use_chipper = *_useChip; _kicker.minChipRange = *_minChipRange; _kicker.maxChipRange = *_maxChipRange; _kicker.dribbler_speed = *_dribblerSpeed; _kicker.kick_power = *_kickPower; _kicker.use_line_kick = *_useLineKick; _kicker.run(); return true; }
bool Gameplay::Plays::DemoBump::run() { Geometry2d::Point ballPos = ball().pos; set<OurRobot *> available = _gameplay->playRobots(); assignNearest(_bump.robot, available, ballPos); // if we have kicked, we want to reset if (_bump.done() && ball().valid && (!ballPos.nearPoint(Geometry2d::Point(0, Field_Length), Field_ArcRadius))) { _bump.restart(); } _bump.run(); return true; }
bool Gameplay::Plays::DemoTouchKick::run() { set<OurRobot *> available = _gameplay->playRobots(); assignNearest(_kicker.robot, available, Geometry2d::Point()); assignNearest(_passer.robot, available, Geometry2d::Point()); Geometry2d::Point ballPos = ball().pos; _passer.setTarget(_kicker.robot->kickerBar()); _passer.use_line_kick = true; // if we have kicked, we want to reset if (_kicker.done() && ball().valid && (!ballPos.nearPoint(Geometry2d::Point(0, Field_Length), Field_ArcRadius))) { _kicker.restart(); _passer.restart(); } if (_passer.done() && ball().valid) { } if (_kicker.robot->pos.distTo(_passTarget) < 0.5) { _kicker.run(); } else { _kicker.robot->move(_passTarget); } _kicker.use_chipper = *_use_chipper; _kicker.kick_power = *_kick_power; _passer.run(); return true; }
bool Gameplay::Plays::DemoYank::run() { Geometry2d::Point ballPos = ball().pos; set<OurRobot *> available = _gameplay->playRobots(); assignNearest(_yank.robot, available, ballPos); // if we have kicked, we want to reset if (_yank.done() && ball().valid && (!ballPos.nearPoint(Geometry2d::Point(0, Field_Length), Field_ArcRadius))) { _yank.restart(); } // set flags from parameters _yank.dribble_speed = *_dribblerSpeed; _yank.enable_bump = *_enableBump; _yank.run(); return true; }
Geometry2d::Point OurRobot::findGoalOnPath(const Geometry2d::Point& pose, const Planning::Path& path, const ObstacleGroup& obstacles) { const bool blend_verbose = false; // empty path case - leave robot stationary if (path.empty()) return pose; // find closest point on path to pose float max = path.points[0].distTo(pose); unsigned int ip = 0; for (unsigned i=0; i<path.points.size(); i++) { if (path.points[i].distTo(pose) < max) { max = path.points[i].distTo(pose); ip = i; } } if (blend_verbose) addText(QString("cur pt %1=(%2,%3)").arg(ip).arg(path.points[ip].x).arg(path.points[ip].y)); // go to nearest point if only point or closest point is the goal if (path.size() == 1) { if (blend_verbose) addText(QString("blend:simple_path")); return path.points[0]; } // can't mix, just go to endpoint if (path.size() == 2) { if (blend_verbose) addText(QString("blend:size2")); return path.points[1]; } // FIXME: does not blend the last segment // All other cases: proportionally blend the next two points together for a smoother // path, so long as it is still viable if (blend_verbose) addText(QString("blend:segments=%1").arg(path.points.size()-1)); // pull out relevant points Point p0 = pos; Point p1; Point p2; if (path.size() > ip+2) { p1 = path.points[ip+1]; p2 = path.points[ip+2]; } else if (path.size() > ip+1) { p1 = path.points[ip]; p2 = path.points[ip+1]; } else { p1 = path.points[ip-1]; p2 = path.points[ip]; } Geometry2d::Segment target_seg(p1, p2); if (blend_verbose) addText(QString("pos=(%1,%2)").arg(pos.x,5).arg(pos.y,5)); if (blend_verbose) addText(QString("path[0]=(%1,%2)").arg(path.points[0].x).arg(path.points[0].y)); if (blend_verbose) addText(QString("p1=(%1,%2)").arg(p1.x,5).arg(p1.y,5)); if (blend_verbose) addText(QString("p2=(%1,%2)").arg(p2.x,5).arg(p2.y,5)); // final endpoint handling if (target_seg.nearPointPerp(p0, 0.02) && p0.nearPoint(p2, 0.03)) { if (2 == path.size()-1) { if (blend_verbose) addText(QString("blend:at_end")); return p2; } else { // reset this segment to next one if (blend_verbose) addText(QString("blend:reset_segment")); Point temp(p1); p1 = p2; p2 = temp; target_seg = Geometry2d::Segment(p1, p2); } } float dist1 = p0.distTo(p1), dist2 = p1.distTo(p2); if (blend_verbose) addText(QString("blend:d1=%1,d2=%2").arg(dist1).arg(dist2)); // endpoint handling if (dist1 < 0.02) { if (blend_verbose) addText(QString("blend:dist1small=%1").arg(dist1)); return p2; /// just go to next point } // short segment handling if (p1.distTo(p2) < 0.05) { if (blend_verbose) addText(QString("blend:dist2small=%1").arg(dist2)); return p2; /// just go to next point } // close to segment - go to end of segment if (target_seg.nearPoint(p0, 0.03)) { if (blend_verbose) addText(QString("blend:closeToSegment")); return p2; } // mix the next point between the first and second point // if we are far away from p1, want scale to be closer to p1 // if we are close to p1, want scale to be closer to p2 float scale = 1 - clamp(dist1/dist2, 0.0f, 1.0f); Geometry2d::Point targetPos = p1 + (p2-p1)*scale; if (blend_verbose) { addText(QString("blend:scale=%1").arg(scale)); addText(QString("blend:dist1=%1").arg(dist1)); addText(QString("blend:dist2=%1").arg(dist2)); } // check for collisions on blended path Geometry2d::Segment shortcut(p0, targetPos); Geometry2d::Point result = p1; if (!obstacles.hit(shortcut)) { if (blend_verbose) addText(QString("blend:shortcut_succeed")); result = targetPos; } else if (result.nearPoint(pose, 0.05)) { if (blend_verbose) addText(QString("blend:shortcut_failed")); result = result + (result-pose).normalized() * 0.10; } if (blend_verbose) addText(QString("point (%1, %2)").arg(result.x).arg(result.y)); return result; }