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;
}