static Geometry2d::Point fromOursToTheirs(Geometry2d::Point& pt) {
Geometry2d::Point c;
c.y() = Field_Dimensions::Current_Dimensions.Length() - pt.y();
c.x() = -pt.x();
return c;
}
/** 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;
}
float Planning::Path::length(const Geometry2d::Point &pt) const
{
float dist = -1;
float length = 0;
if (points.empty())
{
return 0;
}
for (unsigned int i = 0; i < (points.size() - 1); ++i)
{
Geometry2d::Segment s(points[i], points[i+1]);
//add the segment length
length += s.length();
const float d = s.distTo(pt);
//if point closer to this segment
if (dist < 0 || d < dist)
{
//closest point on segment
Geometry2d::Point p = s.nearestPoint(pt);
//new best distance
dist = d;
//reset running length count
length = 0;
length += p.distTo(s.pt[1]);
}
}
return length;
}
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;
}
void SimFieldView::drawTeamSpace(QPainter& p) {
FieldView::drawTeamSpace(p);
// Simulator drag-to-shoot
std::shared_ptr<LogFrame> frame = currentFrame();
if (_dragMode == DRAG_SHOOT && frame) {
p.setPen(QPen(Qt::white, 0.1f));
Geometry2d::Point ball = frame->ball().pos();
p.drawLine(ball.toQPointF(), _dragTo.toQPointF());
if (ball != _dragTo) {
p.setPen(QPen(Qt::gray, 0.1f));
_shot = (ball - _dragTo) * ShootScale;
float speed = _shot.mag();
Geometry2d::Point shotExtension = ball + _shot / speed * 8;
p.drawLine(ball.toQPointF(), shotExtension.toQPointF());
p.setPen(Qt::black);
drawText(p, _dragTo.toQPointF(),
QString("%1 m/s").arg(speed, 0, 'f', 1));
}
}
}
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 Environment::sendVision() {
SSL_WrapperPacket wrapper;
SSL_DetectionFrame* det = wrapper.mutable_detection();
det->set_frame_number(_frameNumber++);
det->set_camera_id(0);
struct timeval tv;
gettimeofday(&tv, nullptr);
det->set_t_capture(tv.tv_sec + (double)tv.tv_usec * 1.0e-6);
det->set_t_sent(det->t_capture());
for (Robot* robot : _yellow) {
if ((rand() % 100) < robot->visibility) {
SSL_DetectionRobot* out = det->add_robots_yellow();
convert_robot(robot, out);
}
}
for (Robot* robot : _blue) {
if ((rand() % 100) < robot->visibility) {
SSL_DetectionRobot* out = det->add_robots_blue();
convert_robot(robot, out);
}
}
for (const Ball* b : _balls) {
Geometry2d::Point ballPos = b->getPosition();
// bool occ;
// if (ballPos.x < 0)
// {
// occ = occluded(ballPos, cam0);
// } else {
// occ = occluded(ballPos, cam1);
// }
if ((rand() % 100) < ballVisibility) {
SSL_DetectionBall* out = det->add_balls();
out->set_confidence(1);
out->set_x(ballPos.x() * 1000);
out->set_y(ballPos.y() * 1000);
out->set_pixel_x(ballPos.x() * 1000);
out->set_pixel_y(ballPos.y() * 1000);
}
}
std::string buf;
wrapper.SerializeToString(&buf);
if (sendShared) {
_visionSocket.writeDatagram(&buf[0], buf.size(), MulticastAddress,
SharedVisionPort);
} else {
_visionSocket.writeDatagram(&buf[0], buf.size(), LocalAddress,
SimVisionPort);
_visionSocket.writeDatagram(&buf[0], buf.size(), LocalAddress,
SimVisionPort + 1);
}
}
void Environment::addBall(Geometry2d::Point pos) {
Ball* b = new Ball(this);
b->initPhysics();
b->position(pos.x(), pos.y());
_balls.append(b);
printf("New Ball: %f %f\n", pos.x(), pos.y());
}
Geometry2d::Point gaussianPoint(int n, float scale) {
Geometry2d::Point pt;
for (int i = 0; i < n; ++i) {
pt.x() += drand48() - 0.5;
pt.y() += drand48() - 0.5;
}
pt *= scale / n;
return pt;
}
void Environment::convert_robot(const Robot* robot, SSL_DetectionRobot* out) {
Geometry2d::Point pos = robot->getPosition();
out->set_confidence(1);
out->set_robot_id(robot->shell);
out->set_x(pos.x() * 1000);
out->set_y(pos.y() * 1000);
out->set_orientation(robot->getAngle());
out->set_pixel_x(pos.x() * 1000);
out->set_pixel_y(pos.y() * 1000);
}
void OurRobot::pivot(Geometry2d::Point pivotTarget) {
_rotationCommand = std::make_unique<Planning::EmptyAngleCommand>();
const float radius = Robot_Radius * 1;
Geometry2d::Point pivotPoint = _state->ball.pos;
// reset other conflicting motion commands
_motionCommand = std::make_unique<Planning::PivotCommand>(
pivotPoint, pivotTarget, radius);
*_cmdText << "pivot(" << pivotTarget.x() << ", " << pivotTarget.y() << ")"
<< endl;
}
void OurRobot::moveDirect(Geometry2d::Point goal, float endSpeed) {
if (!visible) return;
// sets flags for future movement
if (verbose)
cout << " in OurRobot::moveDirect(goal): adding a goal (" << goal.x()
<< ", " << goal.y() << ")" << endl;
_motionCommand = std::make_unique<Planning::DirectPathTargetCommand>(
MotionInstant(goal, (goal - pos).normalized() * endSpeed));
*_cmdText << "moveDirect(" << goal << ")" << endl;
*_cmdText << "endSpeed(" << endSpeed << ")" << endl;
}
//// 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;
}
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::Behaviors::Idle::run()
{
if (!ball().valid || robots.empty())
{
// If we can't find the ball, leave the robots where they are
return false;
}
// Arrange all robots close together around the ball
// We really should exclude parts of the circle that aren't accessible due to field or rules,
// but I think it doesn't matter when there are only four robots.
// Radius of the circle that will contain the robot centers
float radius = Field_CenterRadius + Robot_Radius + .01;
// Angle between robots, as seen from the ball
float perRobot = (Robot_Diameter * 1.25) / radius * RadiansToDegrees;
// Direction from the ball to the first robot.
// Center the robots around the line from the ball to our goal
Geometry2d::Point dir = (Geometry2d::Point() - ball().pos).normalized() * radius;
dir.rotate(Geometry2d::Point(), -perRobot * (robots.size() - 1) / 2);
state()->drawLine(ball().pos, Geometry2d::Point());
for (OurRobot *r : robots)
{
if (r->visible)
{
state()->drawLine(ball().pos, ball().pos + dir);
r->move(ball().pos + dir);
r->face(ball().pos);
// Avoid the ball even on our restart
r->avoidBallRadius(Field_CenterRadius);
// Don't leave gaps
r->avoidAllTeammates(true);
}
// Move to the next robot's position
dir.rotate(Geometry2d::Point(), perRobot);
}
return false;
}
void Gameplay::WindowEvaluator::obstacleRobot(Geometry2d::Point pos)
{
Geometry2d::Point n = (pos - _origin).normalized();
Geometry2d::Point t = n.perpCCW();
const float r = Robot_Radius + Ball_Radius;
Geometry2d::Segment seg(pos - n * Robot_Radius + t * r,
pos - n * Robot_Radius - t * r);
if (debug)
{
// _state->debugLines.push_back(seg);
}
Geometry2d::Point intersection[2];
float extent[2] = {0, _end};
bool valid[2] = {false, false};
for (int i = 0; i < 2; ++i)
{
Geometry2d::Line edge(_origin, seg.pt[i]);
float d = edge.delta().magsq();
if (edge.intersects(_target, &intersection[i]) && (intersection[i] - _origin).dot(edge.delta()) > d)
{
valid[i] = true;
float t = (intersection[i] - _target.pt[0]).dot(_target.delta());
if (t < 0)
{
extent[i] = 0;
} else if (t > _end)
{
extent[i] = _end;
} else {
extent[i] = t;
}
}
}
if (!valid[0] || !valid[1])
{
// Obstacle has no effect
return;
}
obstacleRange(extent[0], extent[1]);
}
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;
}
Robot::Robot(Environment* env, unsigned int id, Robot::RobotRevision rev,
Geometry2d::Point startPos)
: Entity(env),
shell(id),
_rev(rev),
_robotChassis(nullptr),
_robotVehicle(nullptr),
_wheelShape(nullptr),
_controller(nullptr),
_brakingForce(0),
_targetVel(0, 0, 0),
_targetRot(0),
_simEngine(env->getSimEngine()) {
visibility = 100;
_startTransform.setIdentity();
_startTransform.setOrigin(btVector3(startPos.y(), 0, startPos.x()) *
scaling);
setEngineForce(0);
}
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;
}
// Returns the index of the point in this path nearest to pt.
int Planning::Path::nearestIndex(const Geometry2d::Point &pt) const
{
if (points.size() == 0)
{
return -1;
}
int index = 0;
float dist = pt.distTo(points[0]);
for (unsigned int i=1 ; i<points.size(); ++i)
{
float d = pt.distTo(points[i]);
if (d < dist)
{
dist = d;
index = i;
}
}
return index;
}
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::ShapeSet createRobotObstacles(const std::vector<ROBOT*>& robots,
const RobotMask& mask,
Geometry2d::Point currentPosition,
float checkRadius) const {
Geometry2d::ShapeSet result;
for (size_t i = 0; i < mask.size(); ++i)
if (mask[i] > 0 && robots[i] && robots[i]->visible) {
if (currentPosition.distTo(robots[i]->pos) <= checkRadius) {
result.add(std::shared_ptr<Geometry2d::Shape>(
new Geometry2d::Circle(robots[i]->pos, mask[i])));
}
}
return result;
}
TEST(WorldRobot, one_robot) {
RJ::Time t = RJ::now();
Geometry2d::Point p = Geometry2d::Point(1,1);
double th = 1;
int rID = 1;
CameraRobot b = CameraRobot(t, p, th, rID);
int cID = 1;
WorldRobot w;
KalmanRobot kb = KalmanRobot(cID, t, b, w);
std::list<KalmanRobot> kbl;
kbl.push_back(kb);
WorldRobot wb = WorldRobot(t, WorldRobot::Team::BLUE, rID, kbl);
Geometry2d::Point rp = wb.getPos();
double rt = wb.getTheta();
Geometry2d::Point rv = wb.getVel();
double ro = wb.getOmega();
double rpc = wb.getPosCov();
double rvc = wb.getVelCov();
std::list<KalmanRobot> list = wb.getRobotComponents();
EXPECT_TRUE(wb.getIsValid());
EXPECT_EQ(wb.getRobotID(), rID);
EXPECT_EQ(rp.x(), p.x());
EXPECT_EQ(rp.y(), p.y());
EXPECT_EQ(rt, th);
EXPECT_EQ(rv.x(), 0);
EXPECT_EQ(rv.y(), 0);
EXPECT_EQ(ro, 0);
EXPECT_GT(rpc, 0);
EXPECT_GT(rvc, 0);
EXPECT_LT(rpc, 10000);
EXPECT_LT(rvc, 10000);
EXPECT_EQ(list.size(), 1);
}
bool Circle::tangentPoints(const Geometry2d::Point &src,
Geometry2d::Point* p1, Geometry2d::Point* p2) const
{
if (!p1 && !p2)
{
return false;
}
const float dist = src.distTo(center);
if (dist < _r)
{
return false;
}
else if (dist == _r)
{
if (p1)
{
*p1 = src;
}
if (p2)
{
*p2 = src;
}
}
else
{
//source is outside of circle
const float theta = asin(_r/dist);
const float degT = theta * 180.0f / M_PI;
if (p1)
{
Point final = center;
final.rotate(src, degT);
*p1 = final;
}
if (p2)
{
Point final = center;
final.rotate(src, -degT);
*p2 = final;
}
void OurRobot::move(Geometry2d::Point goal, Geometry2d::Point endVelocity) {
if (!visible) return;
// sets flags for future movement
if (verbose)
cout << " in OurRobot::move(goal): adding a goal (" << goal.x() << ", "
<< goal.y() << ")" << std::endl;
_motionCommand = std::make_unique<Planning::PathTargetCommand>(
MotionInstant(goal, endVelocity));
*_cmdText << "move(" << goal.x() << ", " << goal.y() << ")" << endl;
*_cmdText << "endVelocity(" << endVelocity.x() << ", " << endVelocity.y()
<< ")" << endl;
}
TEST(WorldRobot, two_robot) {
RJ::Time t = RJ::now();
Geometry2d::Point p1 = Geometry2d::Point(1,1);
Geometry2d::Point p2 = Geometry2d::Point(2,2);
double th1 = 1;
double th2 = 2;
int rID = 1;
CameraRobot b1 = CameraRobot(t, p1, th1, rID);
CameraRobot b2 = CameraRobot(t, p2, th2, rID);
int cID = 1;
WorldRobot w;
KalmanRobot kb1 = KalmanRobot(cID, t, b1, w);
KalmanRobot kb2 = KalmanRobot(cID, t, b2, w);
std::list<KalmanRobot> kbl;
kbl.push_back(kb1);
kbl.push_back(kb2);
WorldRobot wb = WorldRobot(t, WorldRobot::Team::BLUE, rID, kbl);
Geometry2d::Point rp = wb.getPos();
double rt = wb.getTheta();
Geometry2d::Point rv = wb.getVel();
double ro = wb.getOmega();
double rpc = wb.getPosCov();
double rvc = wb.getVelCov();
std::list<KalmanRobot> list = wb.getRobotComponents();
EXPECT_TRUE(wb.getIsValid());
EXPECT_EQ(rp.x(), (p1.x() + p2.x()) / 2);
EXPECT_EQ(rp.y(), (p1.y() + p2.y()) / 2);
EXPECT_EQ(rt, (th1 + th2) / 2);
EXPECT_EQ(rv.x(), 0);
EXPECT_EQ(rv.y(), 0);
EXPECT_EQ(ro, 0);
EXPECT_GT(rpc, 0);
EXPECT_GT(rvc, 0);
EXPECT_LT(rpc, 10000);
EXPECT_LT(rvc, 10000);
EXPECT_EQ(list.size(), 2);
}
/** Checks whether or not the given ball is in the defense area. */
static inline bool ballIsInGoalieBox(Geometry2d::Point point) {
if (std::abs(point.x()) <
Field_Dimensions::Current_Dimensions.GoalFlat() / 2.0f) {
// Ball is in center (rectangular) portion of defensive bubble
return point.y() > 0 &&
point.y() < Field_Dimensions::Current_Dimensions.ArcRadius();
} else if (std::abs(point.x()) <
(Field_Dimensions::Current_Dimensions.ArcRadius() +
Field_Dimensions::Current_Dimensions.GoalFlat() / 2.0f)) {
// Ball is in one of the side (arc) portions of defensive bubble
double adjusted_x =
std::abs(point.x()) -
(Field_Dimensions::Current_Dimensions.GoalFlat() / 2.0f);
double max_y = sqrt((Field_Dimensions::Current_Dimensions.ArcRadius() *
Field_Dimensions::Current_Dimensions.ArcRadius()) -
(adjusted_x * adjusted_x));
return point.y() > 0 && point.y() <= max_y;
}
return false;
}
void Environment::addRobot(bool blue, int id, Geometry2d::Point pos,
Robot::RobotRevision rev) {
Robot* r = new Robot(this, id, rev, pos);
r->initPhysics(blue);
if (blue) {
_blue.insert(id, r);
} else {
_yellow.insert(id, r);
}
Geometry2d::Point actPos = r->getPosition();
switch (rev) {
case Robot::rev2008:
printf("New 2008 Robot: %d : %f %f\n", id, actPos.x(), actPos.y());
break;
case Robot::rev2011:
printf("New 2011 Robot: %d : %f %f\n", id, actPos.x(), actPos.y());
}
QVector3D pos3(pos.x(), pos.y(), 0.0);
QVector3D axis(0.0, 0.0, 1.0);
}
void OurRobot::face(Geometry2d::Point pt) {
_rotationCommand = std::make_unique<Planning::FacePointCommand>(pt);
*_cmdText << "face(" << pt.x() << ", " << pt.y() << ")" << endl;
}
void OurRobot::worldVelocity(Geometry2d::Point v) {
_motionCommand = std::make_unique<Planning::WorldVelTargetCommand>(v);
setPath(nullptr);
*_cmdText << "worldVel(" << v.x() << ", " << v.y() << ")" << endl;
}