diff --git a/Core/src/Utils/utils.cpp b/Core/src/Utils/utils.cpp index 3858408..75ef332 100644 --- a/Core/src/Utils/utils.cpp +++ b/Core/src/Utils/utils.cpp @@ -28,33 +28,29 @@ namespace Utils { // 没写完 START - /** * 初始化擬合類 * * */ - int InitFitFunction(const CVisionModule *pVision){ + int InitFitFunction(const CVisionModule *pVision) + { UpdataTickMessage(pVision); fitFunction.GetFitData(Tick); -// if (Tick[1].ball.vel > 0 && Tick[0].ball.vel == 0) -// { -// for (int i = 0; i < PARAM::Tick::TickLength;i++) -// { + // if (Tick[1].ball.vel > 0 && Tick[0].ball.vel == 0) + // { + // for (int i = 0; i < PARAM::Tick::TickLength;i++) + // { -// } -// } + // } + // } return 0; - } - - - /** * 计算全局位置 * @param {CVisionModule*} pVision : 视觉模块 @@ -64,7 +60,7 @@ namespace Utils string GlobalComputingPos(const CVisionModule *pVision) { - CGeoPoint touchpos = GetTouchPos(pVision,pVision -> ourPlayer(0).Pos(),true); + CGeoPoint touchpos = GetTouchPos(pVision, pVision->ourPlayer(0).Pos(), true); return to_string(1); // FIXME: 字符串可能还是抽象了点，到时候看看修一下 } @@ -73,85 +69,85 @@ namespace Utils * @param {CVisionModule*} pVision : 视觉模块 */ - - GlobalTick UpdataTickMessage(const CVisionModule *pVision,int defend_player_num1,int defend_player_num2){ + GlobalTick UpdataTickMessage(const CVisionModule *pVision, int defend_player_num1, int defend_player_num2) + { CWorldModel RobotSensor; int oldest = 0; double our_min_dist = 9999; double their_min_dist = 9999; - ///记录帧信息 - for (int i = oldest; i < PARAM::Tick::TickLength -1;i++){ - Tick[i] = Tick[i+1]; + /// 记录帧信息 + for (int i = oldest; i < PARAM::Tick::TickLength - 1; i++) + { + Tick[i] = Tick[i + 1]; } - ///更新帧信息 + /// 更新帧信息 Tick[now].our.defend_player_num1 = defend_player_num1; Tick[now].our.defend_player_num2 = defend_player_num2; Tick[now].time.time = std::chrono::high_resolution_clock::now(); Tick[now].time.delta_time = (double)std::chrono::duration_cast(Tick[now].time.time - Tick[last].time.time).count() / 1000000; - Tick[now].time.tick_count+=1; - Tick[now].ball.pos = pVision ->ball().Pos(); - Tick[now].ball.vel = pVision ->ball().Vel().mod() / 1000; - Tick[now].ball.vel_dir = pVision ->ball().Vel().dir(); + Tick[now].time.tick_count += 1; + Tick[now].ball.pos = pVision->ball().Pos(); + Tick[now].ball.vel = pVision->ball().Vel().mod() / 1000; + Tick[now].ball.vel_dir = pVision->ball().Vel().dir(); Tick[now].ball.acc = (Tick[now].ball.vel - Tick[last].ball.vel) / Tick[now].time.delta_time; - Tick[now].our.player_num = pVision -> getValidNum() + 1; - Tick[now].their.player_num = pVision ->getTheirValidNum(); - ///获取场上机器人信息 - for(int i = 0;i < PARAM::Field::MAX_PLAYER;i++) + Tick[now].our.player_num = pVision->getValidNum() + 1; + Tick[now].their.player_num = pVision->getTheirValidNum(); + /// 获取场上机器人信息 + for (int i = 0; i < PARAM::Field::MAX_PLAYER; i++) { - if(pVision ->ourPlayer(i).Valid()) + if (pVision->ourPlayer(i).Valid()) { - //获取有效机器人 + // 获取有效机器人 Tick[now].our.player[i] = i; // 我方距离球最近的车号 - double to_ball_dist = pVision ->ourPlayer(i).Pos().dist(Tick[now].ball.pos); - if(our_min_dist > to_ball_dist)our_min_dist = to_ball_dist,Tick[now].our.to_balldist_min_num = i; - //获取我方守门员 - if(InExclusionZone(pVision ->ourPlayer(Tick[now].our.player[i]).Pos())) Tick[now].our.goalie_num = i; + double to_ball_dist = pVision->ourPlayer(i).Pos().dist(Tick[now].ball.pos); + if (our_min_dist > to_ball_dist) + our_min_dist = to_ball_dist, Tick[now].our.to_balldist_min_num = i; + // 获取我方守门员 + if (InExclusionZone(pVision->ourPlayer(Tick[now].our.player[i]).Pos())) + Tick[now].our.goalie_num = i; } - if(pVision ->theirPlayer(i).Valid()) + if (pVision->theirPlayer(i).Valid()) { - //获取有效机器人 + // 获取有效机器人 Tick[now].their.player[i] = i; // 敌方距离球最近的车号 - double to_ball_dist = pVision ->theirPlayer(i).Pos().dist(Tick[now].ball.pos); - if(their_min_dist > to_ball_dist)their_min_dist = to_ball_dist,Tick[now].their.to_balldist_min_num = i; - //获取敌方守门员 - if(InExclusionZone(pVision ->theirPlayer(Tick[now].their.player[i]).Pos())) Tick[now].their.goalie_num = i; + double to_ball_dist = pVision->theirPlayer(i).Pos().dist(Tick[now].ball.pos); + if (their_min_dist > to_ball_dist) + their_min_dist = to_ball_dist, Tick[now].their.to_balldist_min_num = i; + // 获取敌方守门员 + if (InExclusionZone(pVision->theirPlayer(Tick[now].their.player[i]).Pos())) + Tick[now].their.goalie_num = i; } - } - - - - - ///球权判断 - //球权一定是我方的情况 - if(RobotSensor.IsInfraredOn(Tick[now].our.to_balldist_min_num) || (our_min_dist < PARAM::Player::playerBallRightsBuffer && their_min_dist > PARAM::Player::playerBallRightsBuffer)) + /// 球权判断 + // 球权一定是我方的情况 + if (RobotSensor.IsInfraredOn(Tick[now].our.to_balldist_min_num) || (our_min_dist < PARAM::Player::playerBallRightsBuffer && their_min_dist > PARAM::Player::playerBallRightsBuffer)) { Tick[now].ball.rights = 1; Tick[now].our.dribbling_num = Tick[now].our.to_balldist_min_num; Tick[now].their.dribbling_num = -1; } - //球权一定是敌方的情况 - else if(!RobotSensor.IsInfraredOn(Tick[now].our.to_balldist_min_num) && our_min_dist > PARAM::Player::playerBallRightsBuffer && their_min_dist < PARAM::Player::playerBallRightsBuffer) + // 球权一定是敌方的情况 + else if (!RobotSensor.IsInfraredOn(Tick[now].our.to_balldist_min_num) && our_min_dist > PARAM::Player::playerBallRightsBuffer && their_min_dist < PARAM::Player::playerBallRightsBuffer) { Tick[now].ball.rights = -1; Tick[now].their.dribbling_num = Tick[now].their.to_balldist_min_num; -// Tick[now].our.dribbling_num = -1; + // Tick[now].our.dribbling_num = -1; } - //传球或射门失误导致的双方都无球权的情况 + // 传球或射门失误导致的双方都无球权的情况 else Tick[now].ball.rights = 0; // 顶牛 或 抢球对抗 if (our_min_dist < PARAM::Player::playerBallRightsBuffer && their_min_dist < PARAM::Player::playerBallRightsBuffer) Tick[now].ball.rights = 2; - Tick[now].their.dribbling_num = -1; -// Tick[now].our.dribbling_num = -1; + Tick[now].their.dribbling_num = -1; + // Tick[now].our.dribbling_num = -1; - //球静止状态 + // 球静止状态 if (Tick[now].ball.vel < 0.01 || (abs(Tick[last].ball.vel_dir - Tick[now].ball.vel_dir) > 0.006 && abs(Tick[last].ball.vel_dir - Tick[now].ball.vel_dir) < 6)) { Tick[now].ball.pos_move_befor = Tick[now].ball.pos; @@ -161,7 +157,6 @@ namespace Utils return Tick[now]; } - /** * 坐标安全性评分计算 * @param {CVisionModule*} pVision : pVision @@ -169,7 +164,7 @@ namespace Utils * @param {CGeoPoint} end : * @return {double} : */ - double PosSafetyGrade(const CVisionModule *pVision, CGeoPoint start, CGeoPoint end,string model) + double PosSafetyGrade(const CVisionModule *pVision, CGeoPoint start, CGeoPoint end, string model) { /* SHOOT 模式考虑守门员 @@ -179,27 +174,31 @@ namespace Utils if (model == "SHOOT") { - //ConfidencePass() + // ConfidencePass() CGeoSegment BallLine(start, end); - //model SHOOT + // model SHOOT double dist = 0; double min_dist = 99999; int min_num = 0; double grade = 0; - for(int i = 0;i < Tick[now].their.player_num;i++) + for (int i = 0; i < Tick[now].their.player_num; i++) { - if(i == Tick[now].their.goalie_num) continue; + if (i == Tick[now].their.goalie_num) + continue; CGeoPoint VildPos = pVision->theirPlayer(Tick[now].their.player[i]).Pos(); - if(VildPos.x() < start.x()) continue; + if (VildPos.x() < start.x()) + continue; dist = VildPos.dist(BallLine.projection(VildPos)); - if(min_dist > dist) min_dist = dist,min_num = i; + if (min_dist > dist) + min_dist = dist, min_num = i; } - double goalie_dist = pVision ->theirPlayer(Tick[now].their.goalie_num).Pos().dist(end); - if(goalie_dist > min_dist) - grade = 0.7 * NumberNormalize(pVision -> theirPlayer(min_num).Pos().dist(BallLine.projection(pVision -> theirPlayer(min_num).Pos())),1500,0) + - 0.3 * NumberNormalize(pVision -> theirPlayer(Tick[now].their.goalie_num).Pos().dist(end),1500,0); - else grade = NumberNormalize( pVision -> theirPlayer(Tick[now].their.goalie_num).Pos().dist(end),1000,0); - grade = grade > 1?1:grade; + double goalie_dist = pVision->theirPlayer(Tick[now].their.goalie_num).Pos().dist(end); + if (goalie_dist > min_dist) + grade = 0.7 * NumberNormalize(pVision->theirPlayer(min_num).Pos().dist(BallLine.projection(pVision->theirPlayer(min_num).Pos())), 1500, 0) + + 0.3 * NumberNormalize(pVision->theirPlayer(Tick[now].their.goalie_num).Pos().dist(end), 1500, 0); + else + grade = NumberNormalize(pVision->theirPlayer(Tick[now].their.goalie_num).Pos().dist(end), 1000, 0); + grade = grade > 1 ? 1 : grade; return grade; } else @@ -210,44 +209,45 @@ namespace Utils double their_min_time = 9999; double their_min_num = 0; double enemy_to_ball_time = 0; - CGeoSegment ball_line(start,end); + CGeoSegment ball_line(start, end); int count = 0; // 获取敌方距离截球点最近的车，过滤在球线以后的车 - for(int i = 0;i< Tick[now].their.player_num;i++) + for (int i = 0; i < Tick[now].their.player_num; i++) { - //如果有车在球后 计数 - if (Tick[now].their.goalie_num == Tick[now].their.player[i] ||!ball_line.IsPointOnLineOnSegment(ball_line.projection(pVision -> theirPlayer(Tick[now].their.player[i]).Pos()))) + // 如果有车在球后 计数 + if (Tick[now].their.goalie_num == Tick[now].their.player[i] || !ball_line.IsPointOnLineOnSegment(ball_line.projection(pVision->theirPlayer(Tick[now].their.player[i]).Pos()))) { count++; continue; } - double dist = ball_line.projection(pVision -> theirPlayer(Tick[now].their.player[i]).Pos()).dist(pVision -> theirPlayer(Tick[now].their.player[i]).Pos()); - //当截球点在敌方禁区的时候，新构造一条垂直X轴的线段，求新的截球点 - if(InExclusionZone(ball_line.projection(pVision -> theirPlayer(Tick[now].their.player[i]).Pos()))) + double dist = ball_line.projection(pVision->theirPlayer(Tick[now].their.player[i]).Pos()).dist(pVision->theirPlayer(Tick[now].their.player[i]).Pos()); + // 当截球点在敌方禁区的时候，新构造一条垂直X轴的线段，求新的截球点 + if (InExclusionZone(ball_line.projection(pVision->theirPlayer(Tick[now].their.player[i]).Pos()))) { - //新构造一条垂直X轴的线段 - CGeoSegment Segment1(CGeoPoint(pVision ->theirPlayer(Tick[now].their.player[i]).Pos().x(),PARAM::Field::PITCH_WIDTH / 2), - CGeoPoint(pVision ->theirPlayer(Tick[now].their.player[i]).Pos().x(),-1 * PARAM::Field::PITCH_WIDTH / 2)); - //新的截球点 - CGeoPoint newInterPos = ball_line.segmentsIntersectPoint(Segment1); - dist = newInterPos.dist(pVision -> theirPlayer(Tick[now].their.player[i]).Pos()); + // 新构造一条垂直X轴的线段 + CGeoSegment Segment1(CGeoPoint(pVision->theirPlayer(Tick[now].their.player[i]).Pos().x(), PARAM::Field::PITCH_WIDTH / 2), + CGeoPoint(pVision->theirPlayer(Tick[now].their.player[i]).Pos().x(), -1 * PARAM::Field::PITCH_WIDTH / 2)); + // 新的截球点 + CGeoPoint newInterPos = ball_line.segmentsIntersectPoint(Segment1); + dist = newInterPos.dist(pVision->theirPlayer(Tick[now].their.player[i]).Pos()); } enemy_to_ball_time = dist / robot_max_speed / 1000; - if(their_min_time > enemy_to_ball_time) their_min_time = enemy_to_ball_time,their_min_num = Tick[now].their.player[i]; + if (their_min_time > enemy_to_ball_time) + their_min_time = enemy_to_ball_time, their_min_num = Tick[now].their.player[i]; count = 0; } - //如果敌方车子都在球后面，认为安全 + // 如果敌方车子都在球后面，认为安全 if (count == Tick[now].their.player_num) { safty_grade = 1; return safty_grade; } - //球到截球点的时间 - double ball_to_interpos_time = ball_line.projection(pVision -> theirPlayer(their_min_num).Pos()).dist(start) / ball_max_speed / 1000; + // 球到截球点的时间 + double ball_to_interpos_time = ball_line.projection(pVision->theirPlayer(their_min_num).Pos()).dist(start) / ball_max_speed / 1000; safty_grade = their_min_time - ball_to_interpos_time; - safty_grade = NumberNormalize(safty_grade,0.25,-0.15); + safty_grade = NumberNormalize(safty_grade, 0.25, -0.15); -// GDebugEngine::Instance() -> gui_debug_msg(CGeoPoint(-3000,2000),to_string(safty_grade) + " " + to_string(their_min_num),3); + // GDebugEngine::Instance() -> gui_debug_msg(CGeoPoint(-3000,2000),to_string(safty_grade) + " " + to_string(their_min_num),3); return safty_grade; } } @@ -258,14 +258,15 @@ namespace Utils * @param pVision * @return */ - double GetBallMaxDist(const CVisionModule *pVision){ + double GetBallMaxDist(const CVisionModule *pVision) + { double a = PARAM::Field::V_DECAY_RATE; - double v = pVision ->ball().Vel().mod(); + double v = pVision->ball().Vel().mod(); double maxT = v / a; double maxDist = a * maxT * maxT; - GDebugEngine::Instance()->gui_debug_msg(CGeoPoint(1000, 1500), "v:"+to_string(v)); - GDebugEngine::Instance()->gui_debug_msg(CGeoPoint(1000, 1000), "d:"+to_string(maxDist)); + GDebugEngine::Instance()->gui_debug_msg(CGeoPoint(1000, 1500), "v:" + to_string(v)); + GDebugEngine::Instance()->gui_debug_msg(CGeoPoint(1000, 1000), "d:" + to_string(maxDist)); return maxDist; } @@ -276,16 +277,16 @@ namespace Utils * @param dist * @return */ - double GetBallToDistTime(const CVisionModule *pVision, double dist){ + double GetBallToDistTime(const CVisionModule *pVision, double dist) + { double a = PARAM::Field::V_DECAY_RATE; - double v = pVision ->ball().Vel().mod(); - double t = sqrt((2*a*dist + v*v) / a*a) - v/a; + double v = pVision->ball().Vel().mod(); + double t = sqrt((2 * a * dist + v * v) / a * a) - v / a; -// GDebugEngine::Instance()->gui_debug_msg(CGeoPoint(1000, 2000), "t:"+to_string(t)); + // GDebugEngine::Instance()->gui_debug_msg(CGeoPoint(1000, 2000), "t:"+to_string(t)); return t; } - /** * 获取相对某坐标最佳截球点（动态：球在运动过程中） * @param {CVisionModule*} pVision : pVision @@ -297,39 +298,37 @@ namespace Utils { double maxDist = GetBallMaxDist(pVision); GetBallToDistTime(pVision, maxDist); - for(int dist=0;distgui_debug_msg(pVision->ball().Pos()+Polar2Vector(dist, pVision->ball().Vel().dir()), to_string(GetBallToDistTime(pVision, dist)),1,10); - GDebugEngine::Instance()->gui_debug_x(pVision->ball().Pos()+Polar2Vector(dist, pVision->ball().Vel().dir())); + for (int dist = 0; dist < maxDist; dist += 100) + { + // GetBallToDistTime(pVision, dist); + GDebugEngine::Instance()->gui_debug_msg(pVision->ball().Pos() + Polar2Vector(dist, pVision->ball().Vel().dir()), to_string(GetBallToDistTime(pVision, dist)), 1, 10); + GDebugEngine::Instance()->gui_debug_x(pVision->ball().Pos() + Polar2Vector(dist, pVision->ball().Vel().dir())); } - return pVision ->ball().Pos() + Polar2Vector(maxDist, pVision ->ball().Vel().dir()); + return pVision->ball().Pos() + Polar2Vector(maxDist, pVision->ball().Vel().dir()); } - /** - * 坐标到坐标之间的时间 - * @param {CGeoPoint} start_pos : 起始位置 - * @param {CGeoPoint} end_pos : 终点位置 - * @param {double} velocity : 速度 - * @return {double} : 时间 - */ - double PosToPosTime(CGeoPoint start_pos, CGeoPoint end_pos, double velocity) - { - return (start_pos - end_pos).mod() / velocity; - } + /** + * 坐标到坐标之间的时间 + * @param {CGeoPoint} start_pos : 起始位置 + * @param {CGeoPoint} end_pos : 终点位置 + * @param {double} velocity : 速度 + * @return {double} : 时间 + */ + double PosToPosTime(CGeoPoint start_pos, CGeoPoint end_pos, double velocity) + { + return (start_pos - end_pos).mod() / velocity; + } - /** - * 预测球运动的线段 - * @param {CVisionModule*} pVision : pVision - * @return {CGeoSegment} : 球运动轨迹的线段 - */ + /** + * 预测球运动的线段 + * @param {CVisionModule*} pVision : pVision + * @return {CGeoSegment} : 球运动轨迹的线段 + */ CGeoSegment PredictBallLine(const CVisionModule *pVision) { - - - } - double GlobalConfidence(const CVisionModule *pVision,int attack_flag) + double GlobalConfidence(const CVisionModule *pVision, int attack_flag) { /// /// /// OUR BALL RIGHTS /// /// /// /// @@ -347,11 +346,14 @@ namespace Utils int status = 0; double confidence_dribbling = 0; double max_confidence_pass = 0; - for(int i = 0;i < Tick[now].our.player_num;i++){ + for (int i = 0; i < Tick[now].our.player_num; i++) + { // 跳过守门员 - if(Tick[now]. our.goalie_num == i) continue; + if (Tick[now].our.goalie_num == i) + continue; // 如果是传统模式跳过后卫 - if(attack_flag == 0 && (Tick[now].our.defend_player_num1 == i || Tick[now].our.defend_player_num2 == i) )continue; + if (attack_flag == 0 && (Tick[now].our.defend_player_num1 == i || Tick[now].our.defend_player_num2 == i)) + continue; int num = Tick[now].our.player[i]; Tick[now].task[num].player_num = Tick[now].our.player[i]; Tick[now].task[num].confidence_shoot = 0; @@ -361,33 +363,33 @@ namespace Utils Tick[now].task[num].confidence_defend = 0; Tick[now].task[num].confidence_getball = 0; // 球权是我方的情况 - if(Tick[now].ball.rights == 1) + if (Tick[now].ball.rights == 1) { // 带球机器人状态 -> [射门，传球，带球，主动造犯规] - if(Tick[now].our.dribbling_num == num) + if (Tick[now].our.dribbling_num == num) { // 获取带球机器人的射门置信度 - Tick[now].task[num].confidence_shoot = ConfidenceShoot(pVision,Tick[now].our.player[i]); - Tick[now].task[num].confidence_shoot = Tick[now].task[num].confidence_shoot - 0.8 * (1 - NumberNormalize(pVision ->ourPlayer(num).Pos().x(),1200,0)); + Tick[now].task[num].confidence_shoot = ConfidenceShoot(pVision, Tick[now].our.player[i]); + Tick[now].task[num].confidence_shoot = Tick[now].task[num].confidence_shoot - 0.8 * (1 - NumberNormalize(pVision->ourPlayer(num).Pos().x(), 1200, 0)); } // 非带球机器人状态 -> [跑位，接球] else { Tick[now].task[num].confidence_pass = 0; // 如果无法被传球的非持球机器人 只能进行跑位 - if(!isValidPass(pVision,pVision ->ourPlayer(Tick[now].our.dribbling_num).Pos(),pVision -> ourPlayer(Tick[now].our.player[i]).Pos(),PARAM::Player::playerBuffer) || !isValidPass(pVision,pVision ->ourPlayer(Tick[now].our.dribbling_num).Pos(),pVision -> ourPlayer(Tick[now].our.player[i]).Pos(),PARAM::Player::playerBuffer)) + if (!isValidPass(pVision, pVision->ourPlayer(Tick[now].our.dribbling_num).Pos(), pVision->ourPlayer(Tick[now].our.player[i]).Pos(), PARAM::Player::playerBuffer) || !isValidPass(pVision, pVision->ourPlayer(Tick[now].our.dribbling_num).Pos(), pVision->ourPlayer(Tick[now].our.player[i]).Pos(), PARAM::Player::playerBuffer)) { Tick[now].task[num].confidence_run = 1; Tick[now].task[num].status = "Run"; continue; } // 获取非带球机器人的被传球概率 - Tick[now].task[num].confidence_shoot = ConfidenceShoot(pVision,pVision->ourPlayer(Tick[now].our.player[i]).Pos()); - Tick[now].task[num].confidence_shoot = Tick[now].task[num].confidence_shoot - 0.8 * (1 - NumberNormalize(pVision ->ourPlayer(num).Pos().x(),1200,0)); - Tick[now].task[num].confidence_pass = ConfidencePass(pVision,Tick[now].our.dribbling_num,Tick[now].our.player[i],Tick[now].task[num].confidence_shoot); - //如果友方位置太靠后，酌情扣分 - if(pVision ->ourPlayer(num).Pos().x() < 1000) - Tick[now].task[num].confidence_pass = Tick[now].task[num].confidence_pass - 0.4 * (1 - NumberNormalize(pVision ->ourPlayer(num).Pos().x(),1000,-2000)); + Tick[now].task[num].confidence_shoot = ConfidenceShoot(pVision, pVision->ourPlayer(Tick[now].our.player[i]).Pos()); + Tick[now].task[num].confidence_shoot = Tick[now].task[num].confidence_shoot - 0.8 * (1 - NumberNormalize(pVision->ourPlayer(num).Pos().x(), 1200, 0)); + Tick[now].task[num].confidence_pass = ConfidencePass(pVision, Tick[now].our.dribbling_num, Tick[now].our.player[i], Tick[now].task[num].confidence_shoot); + // 如果友方位置太靠后，酌情扣分 + if (pVision->ourPlayer(num).Pos().x() < 1000) + Tick[now].task[num].confidence_pass = Tick[now].task[num].confidence_pass - 0.4 * (1 - NumberNormalize(pVision->ourPlayer(num).Pos().x(), 1000, -2000)); Tick[now].task[num].confidence_run = 1; Tick[now].task[num].status = "Run"; // 保存最大的被传球自信度给带球机器人 @@ -404,7 +406,7 @@ namespace Utils { Tick[now].task[num].confidence_pass = 0; // 距离球最近的机器人去抢球，其余人去防守 - if(Tick[now].our.to_balldist_min_num == num) + if (Tick[now].our.to_balldist_min_num == num) { Tick[now].task[num].confidence_getball = 1; Tick[now].task[num].status = "Getball"; @@ -420,7 +422,7 @@ namespace Utils { Tick[now].task[num].confidence_pass = 0; // 距离球最近的机器人去抢球，其余人跑位 - if(Tick[now].our.to_balldist_min_num == num) + if (Tick[now].our.to_balldist_min_num == num) { Tick[now].task[num].confidence_getball = 1; Tick[now].task[num].status = "Getball"; @@ -436,66 +438,65 @@ namespace Utils return 0; } - std::string GlobalStatus(const CVisionModule *pVision,int attack_flag) + std::string GlobalStatus(const CVisionModule *pVision, int attack_flag) { - GlobalConfidence(pVision,attack_flag); + GlobalConfidence(pVision, attack_flag); double dribbling_threshold = 0.40; double pass_threshold = 0; string global_status = ""; - //如果是我方球权，那么先决定带球机器人状态 - if(Tick[now].ball.rights == 1) + // 如果是我方球权，那么先决定带球机器人状态 + if (Tick[now].ball.rights == 1) { double confidence_shoot = Tick[now].task[Tick[now].our.dribbling_num].confidence_shoot; double confidence_pass = Tick[now].task[Tick[now].our.dribbling_num].confidence_pass; - //如果传球、射门的概率都 > 阈值，那么从中选择一项作为状态 - if(confidence_shoot > dribbling_threshold && confidence_pass > dribbling_threshold) + // 如果传球、射门的概率都 > 阈值，那么从中选择一项作为状态 + if (confidence_shoot > dribbling_threshold && confidence_pass > dribbling_threshold) { - //如果 传球概率 > 射门概率 阈值以上，那么才会传球 因为射门的收益会更高，所以条件要宽裕一点 + // 如果 传球概率 > 射门概率 阈值以上，那么才会传球 因为射门的收益会更高，所以条件要宽裕一点 if (confidence_pass - confidence_shoot > pass_threshold) Tick[now].task[Tick[now].our.dribbling_num].status = "passToPlayer" + to_string(Tick[now].task[Tick[now].our.dribbling_num].max_confidence_pass_num); else Tick[now].task[Tick[now].our.dribbling_num].status = "Shoot"; } - //如果某一项大于阈值某一项小于阈值，那么就按照大于阈值的概率来 - if(confidence_shoot > dribbling_threshold && confidence_pass < dribbling_threshold) + // 如果某一项大于阈值某一项小于阈值，那么就按照大于阈值的概率来 + if (confidence_shoot > dribbling_threshold && confidence_pass < dribbling_threshold) Tick[now].task[Tick[now].our.dribbling_num].status = "Shoot"; - else if(confidence_shoot < dribbling_threshold && confidence_pass > dribbling_threshold) + else if (confidence_shoot < dribbling_threshold && confidence_pass > dribbling_threshold) Tick[now].task[Tick[now].our.dribbling_num].status = "passToPlayer" + to_string(Tick[now].task[Tick[now].our.dribbling_num].max_confidence_pass_num); // 如果两项都小于阈值，那么就带球找机会 - if(confidence_shoot < dribbling_threshold && confidence_pass < dribbling_threshold) + if (confidence_shoot < dribbling_threshold && confidence_pass < dribbling_threshold) { Tick[now].task[Tick[now].our.dribbling_num].confidence_dribbling = 1; Tick[now].task[Tick[now].our.dribbling_num].status = "Dribbling"; } } - //Debug - GDebugEngine::Instance()->gui_debug_msg(CGeoPoint(0,0), "testmsg"); + // Debug + GDebugEngine::Instance()->gui_debug_msg(CGeoPoint(0, 0), "testmsg"); GDebugEngine::Instance()->gui_debug_x(BestGetBallPos(pVision)); - for(int i = 0;i < PARAM::Field::MAX_PLAYER;i++) + for (int i = 0; i < PARAM::Field::MAX_PLAYER; i++) { - if(pVision ->ourPlayer(i).Valid() && - (attack_flag == 0 && - (Tick[now].task[i].player_num != -1 && Tick[now].task[i].player_num != Tick[now].our.goalie_num))) + if (pVision->ourPlayer(i).Valid() && + (attack_flag == 0 && + (Tick[now].task[i].player_num != -1 && Tick[now].task[i].player_num != Tick[now].our.goalie_num))) { global_status = global_status + "[" + to_string(Tick[now].task[i].player_num) + "," + Tick[now].task[i].status + "]"; - GDebugEngine::Instance()->gui_debug_msg(CGeoPoint(pVision ->ourPlayer(i).Pos().x(),pVision ->ourPlayer(i).Pos().y() - 160),"Number: " + to_string(Tick[now].task[i].player_num),4,0,80); - GDebugEngine::Instance()->gui_debug_msg(CGeoPoint(pVision ->ourPlayer(i).Pos().x(),pVision ->ourPlayer(i).Pos().y() - 250),"shoot: " + to_string(Tick[now].task[i].confidence_shoot),8,0,80); - GDebugEngine::Instance()->gui_debug_msg(CGeoPoint(pVision ->ourPlayer(i).Pos().x(),pVision ->ourPlayer(i).Pos().y() - 340),"Pass: " + to_string(Tick[now].task[i].confidence_pass),2,0,80); - GDebugEngine::Instance()->gui_debug_msg(CGeoPoint(pVision ->ourPlayer(i).Pos().x(),pVision ->ourPlayer(i).Pos().y() - 430),"Dribbling: " + to_string(Tick[now].task[i].confidence_dribbling),1,0,80); - GDebugEngine::Instance()->gui_debug_msg(CGeoPoint(pVision ->ourPlayer(i).Pos().x(),pVision ->ourPlayer(i).Pos().y() - 520),"Getball: " + to_string(Tick[now].task[i].confidence_getball),5,0,80); - GDebugEngine::Instance()->gui_debug_msg(CGeoPoint(pVision ->ourPlayer(i).Pos().x(),pVision ->ourPlayer(i).Pos().y() - 610),"Defene: " + to_string(Tick[now].task[i].confidence_defend),6,0,80); - GDebugEngine::Instance()->gui_debug_msg(CGeoPoint(pVision ->ourPlayer(i).Pos().x(),pVision ->ourPlayer(i).Pos().y() - 700),"Run: " + to_string(Tick[now].task[i].confidence_run),7,0,80); - GDebugEngine::Instance()->gui_debug_msg(CGeoPoint(pVision ->ourPlayer(i).Pos().x(),pVision ->ourPlayer(i).Pos().y() - 790),"Status: " + Tick[now].task[i].status,3,0,80); + GDebugEngine::Instance()->gui_debug_msg(CGeoPoint(pVision->ourPlayer(i).Pos().x(), pVision->ourPlayer(i).Pos().y() - 160), "Number: " + to_string(Tick[now].task[i].player_num), 4, 0, 80); + GDebugEngine::Instance()->gui_debug_msg(CGeoPoint(pVision->ourPlayer(i).Pos().x(), pVision->ourPlayer(i).Pos().y() - 250), "shoot: " + to_string(Tick[now].task[i].confidence_shoot), 8, 0, 80); + GDebugEngine::Instance()->gui_debug_msg(CGeoPoint(pVision->ourPlayer(i).Pos().x(), pVision->ourPlayer(i).Pos().y() - 340), "Pass: " + to_string(Tick[now].task[i].confidence_pass), 2, 0, 80); + GDebugEngine::Instance()->gui_debug_msg(CGeoPoint(pVision->ourPlayer(i).Pos().x(), pVision->ourPlayer(i).Pos().y() - 430), "Dribbling: " + to_string(Tick[now].task[i].confidence_dribbling), 1, 0, 80); + GDebugEngine::Instance()->gui_debug_msg(CGeoPoint(pVision->ourPlayer(i).Pos().x(), pVision->ourPlayer(i).Pos().y() - 520), "Getball: " + to_string(Tick[now].task[i].confidence_getball), 5, 0, 80); + GDebugEngine::Instance()->gui_debug_msg(CGeoPoint(pVision->ourPlayer(i).Pos().x(), pVision->ourPlayer(i).Pos().y() - 610), "Defene: " + to_string(Tick[now].task[i].confidence_defend), 6, 0, 80); + GDebugEngine::Instance()->gui_debug_msg(CGeoPoint(pVision->ourPlayer(i).Pos().x(), pVision->ourPlayer(i).Pos().y() - 700), "Run: " + to_string(Tick[now].task[i].confidence_run), 7, 0, 80); + GDebugEngine::Instance()->gui_debug_msg(CGeoPoint(pVision->ourPlayer(i).Pos().x(), pVision->ourPlayer(i).Pos().y() - 790), "Status: " + Tick[now].task[i].status, 3, 0, 80); } - } return global_status; } - double ConfidencePass(const CVisionModule *pVision, int dribbling_player_num,int getball_player_num,double getball_player_confidence_shoot) + double ConfidencePass(const CVisionModule *pVision, int dribbling_player_num, int getball_player_num, double getball_player_confidence_shoot) { // 传球路径安全评分 double pass_safty_grade = 0; @@ -507,17 +508,17 @@ namespace Utils double pass_shoot_grade; double pass_grade; double grade; - CGeoPoint dribbling_player_pos = pVision ->ourPlayer(dribbling_player_num).Pos(); - CGeoPoint getball_player_pos = pVision ->ourPlayer(getball_player_num).Pos(); - pass_safty_grade = PosSafetyGrade(pVision,dribbling_player_pos,getball_player_pos,"PASS"); - pos_to_pos_dist_grade = PosToPosDistGrade(dribbling_player_pos.x(),getball_player_pos.y(),getball_player_pos.x(),getball_player_pos.y()); - robot_to_pos_dir_grade = PosToPosDirGrade(dribbling_player_pos.x(),dribbling_player_pos.y(),getball_player_pos.x(),getball_player_pos.y(),1); - pass_grade = 0.5 * pos_to_pos_dist_grade+ 0.5 * robot_to_pos_dir_grade; + CGeoPoint dribbling_player_pos = pVision->ourPlayer(dribbling_player_num).Pos(); + CGeoPoint getball_player_pos = pVision->ourPlayer(getball_player_num).Pos(); + pass_safty_grade = PosSafetyGrade(pVision, dribbling_player_pos, getball_player_pos, "PASS"); + pos_to_pos_dist_grade = PosToPosDistGrade(dribbling_player_pos.x(), getball_player_pos.y(), getball_player_pos.x(), getball_player_pos.y()); + robot_to_pos_dir_grade = PosToPosDirGrade(dribbling_player_pos.x(), dribbling_player_pos.y(), getball_player_pos.x(), getball_player_pos.y(), 1); + pass_grade = 0.5 * pos_to_pos_dist_grade + 0.5 * robot_to_pos_dir_grade; grade = 0.4 * pass_grade + 0.2 * pass_safty_grade + 0.4 * getball_player_confidence_shoot; -// GDebugEngine::Instance() ->gui_debug_msg(CGeoPoint(-3000,3000),to_string(pass_grade)); -// GDebugEngine::Instance() ->gui_debug_msg(CGeoPoint(-3000,2000),to_string(pass_safty_grade)); -// GDebugEngine::Instance() ->gui_debug_msg(CGeoPoint(-3000,1000),to_string(getball_player_confidence_shoot)); -// GDebugEngine::Instance() ->gui_debug_msg(CGeoPoint(-3000,0),to_string(grade)); + // GDebugEngine::Instance() ->gui_debug_msg(CGeoPoint(-3000,3000),to_string(pass_grade)); + // GDebugEngine::Instance() ->gui_debug_msg(CGeoPoint(-3000,2000),to_string(pass_safty_grade)); + // GDebugEngine::Instance() ->gui_debug_msg(CGeoPoint(-3000,1000),to_string(getball_player_confidence_shoot)); + // GDebugEngine::Instance() ->gui_debug_msg(CGeoPoint(-3000,0),to_string(grade)); return grade; } @@ -530,57 +531,59 @@ namespace Utils double grade; double their_min_dist = 9999; double their_min_num = 0; - CGeoPoint player_pos = pVision ->ourPlayer(dribbling_num).Pos(); - //获取射门点 - CGeoPoint shoot_pos = GetShootPoint(pVision,dribbling_num); + CGeoPoint player_pos = pVision->ourPlayer(dribbling_num).Pos(); + // 获取射门点 + CGeoPoint shoot_pos = GetShootPoint(pVision, dribbling_num); Tick[now].task[dribbling_num].shoot_pos = shoot_pos; grade_shoot = Tick[now].globalData.confidence_shoot; - //如果算不到射门点直接返回 0 - if (shoot_pos.y() == -999 || player_pos.x() > 4000) return 0; - CGeoSegment ball_line(player_pos,shoot_pos); + // 如果算不到射门点直接返回 0 + if (shoot_pos.y() == -999 || player_pos.x() > 4000) + return 0; + CGeoSegment ball_line(player_pos, shoot_pos); int count = 0; // 获取敌方距离截球点最近的车，过滤在球线以后的车 - for(int i = 0;i< Tick[now].their.player_num;i++) + for (int i = 0; i < Tick[now].their.player_num; i++) { - //如果有车在球后 计数 - if (Tick[now].their.goalie_num == Tick[now].their.player[i] ||!ball_line.IsPointOnLineOnSegment(ball_line.projection(pVision -> theirPlayer(Tick[now].their.player[i]).Pos()))) + // 如果有车在球后 计数 + if (Tick[now].their.goalie_num == Tick[now].their.player[i] || !ball_line.IsPointOnLineOnSegment(ball_line.projection(pVision->theirPlayer(Tick[now].their.player[i]).Pos()))) { count++; continue; } - double dist = ball_line.projection(pVision -> theirPlayer(Tick[now].their.player[i]).Pos()).dist(pVision -> theirPlayer(Tick[now].their.player[i]).Pos()); - //当截球点在敌方禁区的时候，新构造一条垂直X轴的线段，求新的截球点 - if(InExclusionZone(ball_line.projection(pVision -> theirPlayer(Tick[now].their.player[i]).Pos()))) + double dist = ball_line.projection(pVision->theirPlayer(Tick[now].their.player[i]).Pos()).dist(pVision->theirPlayer(Tick[now].their.player[i]).Pos()); + // 当截球点在敌方禁区的时候，新构造一条垂直X轴的线段，求新的截球点 + if (InExclusionZone(ball_line.projection(pVision->theirPlayer(Tick[now].their.player[i]).Pos()))) { - //新构造一条垂直X轴的线段 - CGeoSegment Segment1(CGeoPoint(pVision ->theirPlayer(Tick[now].their.player[i]).Pos().x(),PARAM::Field::PITCH_WIDTH / 2), - CGeoPoint(pVision ->theirPlayer(Tick[now].their.player[i]).Pos().x(),-1 * PARAM::Field::PITCH_WIDTH / 2)); - //新的截球点 - CGeoPoint newInterPos = ball_line.segmentsIntersectPoint(Segment1); - dist = newInterPos.dist(pVision -> theirPlayer(Tick[now].their.player[i]).Pos()); + // 新构造一条垂直X轴的线段 + CGeoSegment Segment1(CGeoPoint(pVision->theirPlayer(Tick[now].their.player[i]).Pos().x(), PARAM::Field::PITCH_WIDTH / 2), + CGeoPoint(pVision->theirPlayer(Tick[now].their.player[i]).Pos().x(), -1 * PARAM::Field::PITCH_WIDTH / 2)); + // 新的截球点 + CGeoPoint newInterPos = ball_line.segmentsIntersectPoint(Segment1); + dist = newInterPos.dist(pVision->theirPlayer(Tick[now].their.player[i]).Pos()); } - if(their_min_dist > dist) their_min_dist = dist,their_min_num = Tick[now].their.player[i]; + if (their_min_dist > dist) + their_min_dist = dist, their_min_num = Tick[now].their.player[i]; count = 0; } - //如果敌方车子都在球后面，认为安全 + // 如果敌方车子都在球后面，认为安全 if (count == Tick[now].their.player_num) { safty_grade = 1; grade = 0.75 * grade_shoot + 0.25 * safty_grade; -// GDebugEngine::Instance() -> gui_debug_msg(CGeoPoint(-3000,2000),to_string(grade_shoot) + " " + to_string(safty_grade) + " " + to_string(grade),3); -// GDebugEngine::Instance() -> gui_debug_x(shoot_pos,3); + // GDebugEngine::Instance() -> gui_debug_msg(CGeoPoint(-3000,2000),to_string(grade_shoot) + " " + to_string(safty_grade) + " " + to_string(grade),3); + // GDebugEngine::Instance() -> gui_debug_x(shoot_pos,3); return grade; } - //敌方到截球点的时间 + // 敌方到截球点的时间 double enemy_to_ball_time = their_min_dist / robot_max_speed / 1000; - //球到截球点的时间 - double ball_to_interpos_time = ball_line.projection(pVision -> theirPlayer(their_min_num).Pos()).dist(player_pos) / ball_max_speed / 1000; + // 球到截球点的时间 + double ball_to_interpos_time = ball_line.projection(pVision->theirPlayer(their_min_num).Pos()).dist(player_pos) / ball_max_speed / 1000; safty_grade = enemy_to_ball_time - ball_to_interpos_time; - safty_grade = NumberNormalize(safty_grade,0.15,0); + safty_grade = NumberNormalize(safty_grade, 0.15, 0); grade = 0.75 * grade_shoot + 0.25 * safty_grade; - grade = grade - 0.5 * (1 - NumberNormalize(player_pos.x(),1700,0)); -// GDebugEngine::Instance() -> gui_debug_msg(CGeoPoint(-3000,2000),to_string(grade_shoot) + " " + to_string(safty_grade) + " " + to_string(grade),3); -// GDebugEngine::Instance() -> gui_debug_x(shoot_pos,3); + grade = grade - 0.5 * (1 - NumberNormalize(player_pos.x(), 1700, 0)); + // GDebugEngine::Instance() -> gui_debug_msg(CGeoPoint(-3000,2000),to_string(grade_shoot) + " " + to_string(safty_grade) + " " + to_string(grade),3); + // GDebugEngine::Instance() -> gui_debug_x(shoot_pos,3); return grade; } double ConfidenceShoot(const CVisionModule *pVision, CGeoPoint player_pos) @@ -592,55 +595,57 @@ namespace Utils double grade; double their_min_dist = 9999; double their_min_num = 0; - //获取射门点 - CGeoPoint shoot_pos = GetShootPoint(pVision,player_pos.x(),player_pos.y()); + // 获取射门点 + CGeoPoint shoot_pos = GetShootPoint(pVision, player_pos.x(), player_pos.y()); grade_shoot = Tick[now].globalData.confidence_shoot; - //如果算不到射门点直接返回 0 - if (shoot_pos.y() == -999 || player_pos.x() > 4000) return 0; - CGeoSegment ball_line(player_pos,shoot_pos); + // 如果算不到射门点直接返回 0 + if (shoot_pos.y() == -999 || player_pos.x() > 4000) + return 0; + CGeoSegment ball_line(player_pos, shoot_pos); int count = 0; // 获取敌方距离截球点最近的车，过滤在球线以后的车 - for(int i = 0;i< Tick[now].their.player_num;i++) + for (int i = 0; i < Tick[now].their.player_num; i++) { - //如果有车在球后 计数 - if (Tick[now].their.goalie_num == Tick[now].their.player[i] ||!ball_line.IsPointOnLineOnSegment(ball_line.projection(pVision -> theirPlayer(Tick[now].their.player[i]).Pos()))) + // 如果有车在球后 计数 + if (Tick[now].their.goalie_num == Tick[now].their.player[i] || !ball_line.IsPointOnLineOnSegment(ball_line.projection(pVision->theirPlayer(Tick[now].their.player[i]).Pos()))) { count++; continue; } - double dist = ball_line.projection(pVision -> theirPlayer(Tick[now].their.player[i]).Pos()).dist(pVision -> theirPlayer(Tick[now].their.player[i]).Pos()); - //当截球点在敌方禁区的时候，新构造一条垂直X轴的线段，求新的截球点 - if(InExclusionZone(ball_line.projection(pVision -> theirPlayer(Tick[now].their.player[i]).Pos()))) + double dist = ball_line.projection(pVision->theirPlayer(Tick[now].their.player[i]).Pos()).dist(pVision->theirPlayer(Tick[now].their.player[i]).Pos()); + // 当截球点在敌方禁区的时候，新构造一条垂直X轴的线段，求新的截球点 + if (InExclusionZone(ball_line.projection(pVision->theirPlayer(Tick[now].their.player[i]).Pos()))) { - //新构造一条垂直X轴的线段 - CGeoSegment Segment1(CGeoPoint(pVision ->theirPlayer(Tick[now].their.player[i]).Pos().x(),PARAM::Field::PITCH_WIDTH / 2), - CGeoPoint(pVision ->theirPlayer(Tick[now].their.player[i]).Pos().x(),-1 * PARAM::Field::PITCH_WIDTH / 2)); - //新的截球点 - CGeoPoint newInterPos = ball_line.segmentsIntersectPoint(Segment1); - dist = newInterPos.dist(pVision -> theirPlayer(Tick[now].their.player[i]).Pos()); + // 新构造一条垂直X轴的线段 + CGeoSegment Segment1(CGeoPoint(pVision->theirPlayer(Tick[now].their.player[i]).Pos().x(), PARAM::Field::PITCH_WIDTH / 2), + CGeoPoint(pVision->theirPlayer(Tick[now].their.player[i]).Pos().x(), -1 * PARAM::Field::PITCH_WIDTH / 2)); + // 新的截球点 + CGeoPoint newInterPos = ball_line.segmentsIntersectPoint(Segment1); + dist = newInterPos.dist(pVision->theirPlayer(Tick[now].their.player[i]).Pos()); } - if(their_min_dist > dist) their_min_dist = dist,their_min_num = Tick[now].their.player[i]; + if (their_min_dist > dist) + their_min_dist = dist, their_min_num = Tick[now].their.player[i]; count = 0; } - //如果敌方车子都在球后面，认为安全 + // 如果敌方车子都在球后面，认为安全 if (count == Tick[now].their.player_num) { safty_grade = 1; grade = 0.75 * grade_shoot + 0.25 * safty_grade; -// GDebugEngine::Instance() -> gui_debug_msg(CGeoPoint(-3000,2000),to_string(grade_shoot) + " " + to_string(safty_grade) + " " + to_string(grade),3); -// GDebugEngine::Instance() -> gui_debug_x(shoot_pos,3); + // GDebugEngine::Instance() -> gui_debug_msg(CGeoPoint(-3000,2000),to_string(grade_shoot) + " " + to_string(safty_grade) + " " + to_string(grade),3); + // GDebugEngine::Instance() -> gui_debug_x(shoot_pos,3); return grade; } - //敌方到截球点的时间 + // 敌方到截球点的时间 double enemy_to_ball_time = their_min_dist / robot_max_speed / 1000; - //球到截球点的时间 - double ball_to_interpos_time = ball_line.projection(pVision -> theirPlayer(their_min_num).Pos()).dist(player_pos) / ball_max_speed / 1000; + // 球到截球点的时间 + double ball_to_interpos_time = ball_line.projection(pVision->theirPlayer(their_min_num).Pos()).dist(player_pos) / ball_max_speed / 1000; safty_grade = enemy_to_ball_time - ball_to_interpos_time; - safty_grade = NumberNormalize(safty_grade,0.15,0); + safty_grade = NumberNormalize(safty_grade, 0.15, 0); grade = 0.75 * grade_shoot + 0.25 * safty_grade; - grade = grade - 0.5 * (1 - NumberNormalize(player_pos.x(),2200,500)); -// GDebugEngine::Instance() -> gui_debug_msg(CGeoPoint(-3000,2000),to_string(grade_shoot) + " " + to_string(safty_grade) + " " + to_string(grade),3); -// GDebugEngine::Instance() -> gui_debug_x(shoot_pos,3); + grade = grade - 0.5 * (1 - NumberNormalize(player_pos.x(), 2200, 500)); + // GDebugEngine::Instance() -> gui_debug_msg(CGeoPoint(-3000,2000),to_string(grade_shoot) + " " + to_string(safty_grade) + " " + to_string(grade),3); + // GDebugEngine::Instance() -> gui_debug_x(shoot_pos,3); return grade; } /** @@ -650,7 +655,7 @@ namespace Utils * @param {double} TICK_LAST_grade : * @return {double} : (x,y)关于最佳跑位点的评分 */ - double GetAttackGrade(const CVisionModule *pVision, double x, double y,CGeoPoint player_pos,CGeoPoint shoot_pos) + double GetAttackGrade(const CVisionModule *pVision, double x, double y, CGeoPoint player_pos, CGeoPoint shoot_pos) { // 射门评分 double shoot_grade; @@ -664,21 +669,21 @@ namespace Utils double pass_dir_grade; // 传球距离评分 double pass_dist_grade; - //传球安全度评分 + // 传球安全度评分 double pass_safty_grade; double grade = 0.0; - shoot_dir_grade = PosToPosDirGrade(x,y,shoot_pos.x(),shoot_pos.y(),4 / PARAM::Math::RADIAN * PARAM::Math::PI); - shoot_dist_grade = PosToPosDistGrade(x,y,shoot_pos.x(),shoot_pos.y(),-1,"NORMAL"); + shoot_dir_grade = PosToPosDirGrade(x, y, shoot_pos.x(), shoot_pos.y(), 4 / PARAM::Math::RADIAN * PARAM::Math::PI); + shoot_dist_grade = PosToPosDistGrade(x, y, shoot_pos.x(), shoot_pos.y(), -1, "NORMAL"); shoot_grade = 0.2 * shoot_dir_grade + 0.8 * shoot_dist_grade; - pass_dir_grade = PosToPosDirGrade(x,y,player_pos.x(),player_pos.y(),4 / PARAM::Math::RADIAN * PARAM::Math::PI); - pass_dist_grade = PosToPosDistGrade(x,y,player_pos.x(),player_pos.y()); - pass_safty_grade = PosSafetyGrade(pVision,player_pos,CGeoPoint(x,y)); + pass_dir_grade = PosToPosDirGrade(x, y, player_pos.x(), player_pos.y(), 4 / PARAM::Math::RADIAN * PARAM::Math::PI); + pass_dist_grade = PosToPosDistGrade(x, y, player_pos.x(), player_pos.y()); + pass_safty_grade = PosSafetyGrade(pVision, player_pos, CGeoPoint(x, y)); pass_grade = 0.5 * pass_dir_grade + 0.5 * pass_dist_grade; grade = 0.2 * pass_grade + 0.5 * shoot_grade + 0.3 * pass_safty_grade; return grade; } - double GetTouchGrade(const CVisionModule *pVision, double x, double y,CGeoPoint player_pos,CGeoPoint shoot_pos) + double GetTouchGrade(const CVisionModule *pVision, double x, double y, CGeoPoint player_pos, CGeoPoint shoot_pos) { // 射门评分 double shoot_grade; @@ -692,86 +697,76 @@ namespace Utils double pass_dir_grade; // 传球距离评分 double pass_dist_grade; - //传球安全度评分 + // 传球安全度评分 double pass_safty_grade; double grade = 0.0; - shoot_dir_grade = PosToPosDirGrade(x,y,shoot_pos.x(),shoot_pos.y(),4 / PARAM::Math::RADIAN * PARAM::Math::PI); - shoot_dist_grade = PosToPosDistGrade(x,y,shoot_pos.x(),shoot_pos.y(),-1,"NORMAL"); + shoot_dir_grade = PosToPosDirGrade(x, y, shoot_pos.x(), shoot_pos.y(), 4 / PARAM::Math::RADIAN * PARAM::Math::PI); + shoot_dist_grade = PosToPosDistGrade(x, y, shoot_pos.x(), shoot_pos.y(), -1, "NORMAL"); shoot_grade = 0.1 * shoot_dir_grade + 0.9 * shoot_dist_grade; - pass_dir_grade = PosToPosDirGrade(x,y,player_pos.x(),player_pos.y(),4 / PARAM::Math::RADIAN * PARAM::Math::PI); - pass_dist_grade = PosToPosDistGrade(x,y,player_pos.x(),player_pos.y()); - pass_safty_grade = PosSafetyGrade(pVision,player_pos,CGeoPoint(x,y)); + pass_dir_grade = PosToPosDirGrade(x, y, player_pos.x(), player_pos.y(), 4 / PARAM::Math::RADIAN * PARAM::Math::PI); + pass_dist_grade = PosToPosDistGrade(x, y, player_pos.x(), player_pos.y()); + pass_safty_grade = PosSafetyGrade(pVision, player_pos, CGeoPoint(x, y)); pass_grade = 0.2 * pass_dir_grade + 0.8 * pass_dist_grade; grade = 0.4 * shoot_grade + 0.3 * pass_grade + 0.3 * pass_safty_grade; return grade; } - - CGeoPoint GetTouchPos(const CVisionModule *pVision,CGeoPoint player_pos,bool double_flag) + CGeoPoint GetTouchPos(const CVisionModule *pVision, CGeoPoint player_pos, bool double_flag) { int LENGTH = (PARAM::Field::PITCH_LENGTH / 2) - 400; int WIDTH = (PARAM::Field::PITCH_WIDTH / 2) - 350; int step = 230; double grade = 0; double max_grade = -999; - CGeoPoint max_grade_point = CGeoPoint(0,0); - CGeoPoint max_shoot_point = CGeoPoint(0,0); + CGeoPoint max_grade_point = CGeoPoint(0, 0); + CGeoPoint max_shoot_point = CGeoPoint(0, 0); - for(int x = 1800; x < LENGTH;x += step) + for (int x = 1800; x < LENGTH; x += step) { - for(int y = -1 * WIDTH; y < WIDTH;y += step) + for (int y = -1 * WIDTH; y < WIDTH; y += step) { double touch_dir_grade = 0; - CGeoPoint now_pos = CGeoPoint(x,y); - CGeoPoint shoot_pos = GetShootPoint(pVision,x,y); + CGeoPoint now_pos = CGeoPoint(x, y); + CGeoPoint shoot_pos = GetShootPoint(pVision, x, y); - if( shoot_pos.y() == -999 || InExclusionZone(now_pos) || !isValidPass(pVision,now_pos,shoot_pos)) + if (shoot_pos.y() == -999 || InExclusionZone(now_pos) || !isValidPass(pVision, now_pos, shoot_pos)) continue; // 一传一touch的射门位计算 if (!double_flag) { double shootdir = 90 + (shoot_pos - now_pos).dir() * PARAM::Math::RADIAN; double passdir = 90 + (now_pos - player_pos).dir() * PARAM::Math::RADIAN; - double touch_dir = 180 - (passdir-shootdir); - touch_dir = now_pos.y() < player_pos.y()?360-touch_dir:touch_dir; - if(touch_dir > PARAM::Player::playerTouchAngle || !isValidPass(pVision,player_pos,now_pos)) + double touch_dir = 180 - (passdir - shootdir); + touch_dir = now_pos.y() < player_pos.y() ? 360 - touch_dir : touch_dir; + if (touch_dir > PARAM::Player::playerTouchAngle || !isValidPass(pVision, player_pos, now_pos)) continue; - touch_dir_grade = 1 - NumberNormalize(touch_dir_grade,PARAM::Player::playerTouchAngle,0); - grade = 0.8 * GetTouchGrade(pVision,x,y,player_pos,shoot_pos) + 0.2 * touch_dir_grade; - + touch_dir_grade = 1 - NumberNormalize(touch_dir_grade, PARAM::Player::playerTouchAngle, 0); + grade = 0.8 * GetTouchGrade(pVision, x, y, player_pos, shoot_pos) + 0.2 * touch_dir_grade; } // 一传二touch的射门位计算 else { - - grade = ConfidenceShoot(pVision,CGeoPoint(x,y)); + grade = ConfidenceShoot(pVision, CGeoPoint(x, y)); } - if(max_grade < grade) + if (max_grade < grade) { max_grade = grade; max_grade_point = now_pos; max_shoot_point = shoot_pos; } - GDebugEngine::Instance() ->gui_debug_x(now_pos); + GDebugEngine::Instance()->gui_debug_x(now_pos); } } -// GDebugEngine::Instance() ->gui_debug_msg(CGeoPoint(-2500,2000),to_string(shootdir) + " " + to_string(passdir) + " " + to_string(dir)); - GDebugEngine::Instance() ->gui_debug_x(max_grade_point,3); - GDebugEngine::Instance() ->gui_debug_x(max_shoot_point,3); + // GDebugEngine::Instance() ->gui_debug_msg(CGeoPoint(-2500,2000),to_string(shootdir) + " " + to_string(passdir) + " " + to_string(dir)); + GDebugEngine::Instance()->gui_debug_x(max_grade_point, 3); + GDebugEngine::Instance()->gui_debug_x(max_shoot_point, 3); return max_grade_point; } - - - - - - - - CGeoPoint GetAttackPos(const CVisionModule *pVision,int num) + CGeoPoint GetAttackPos(const CVisionModule *pVision, int num) { // 圆的半径 int radius = 800; @@ -788,7 +783,7 @@ namespace Utils double pass_dir_grade; // 传球距离评分 double pass_dist_grade; - //传球安全度评分 + // 传球安全度评分 double pass_safty_grade; double grade = 0.0; double max_grade = -999; @@ -799,68 +794,62 @@ namespace Utils // 圆心 CGeoPoint player_pos = pVision->ourPlayer(num).Pos(); CGeoPoint dribbling_player_pos = pVision->ourPlayer(Tick[now].our.dribbling_num).Pos(); - //洒点 - for (int x = player_pos.x() - radius; x <= player_pos.x() + radius; x+=step) + // 洒点 + for (int x = player_pos.x() - radius; x <= player_pos.x() + radius; x += step) { - for (int y = player_pos.y() - radius; y <= player_pos.y() + radius; y+=step) + for (int y = player_pos.y() - radius; y <= player_pos.y() + radius; y += step) { - CGeoPoint pos(x,y); - CGeoPoint shoot_pos = GetShootPoint(pVision, x,y); + CGeoPoint pos(x, y); + CGeoPoint shoot_pos = GetShootPoint(pVision, x, y); // 如果 无有效射门点 或 点位在禁区 或 传球路径被挡住 或 射门路径被挡住 跳过该点 - if(!InField(pos) || shoot_pos.y() == -999 || InExclusionZone(pos) || (!isValidPass(pVision,dribbling_player_pos,CGeoPoint(x,y),PARAM::Player::playerBuffer)) || !isValidPass(pVision,pos,shoot_pos,PARAM::Player::playerBuffer)) continue; + if (!InField(pos) || shoot_pos.y() == -999 || InExclusionZone(pos) || (!isValidPass(pVision, dribbling_player_pos, CGeoPoint(x, y), PARAM::Player::playerBuffer)) || !isValidPass(pVision, pos, shoot_pos, PARAM::Player::playerBuffer)) + continue; if (pos.dist2(player_pos) < radius * radius) { - shoot_dir_grade = PosToPosDirGrade(x,y,shoot_pos.x(),shoot_pos.y(),4 / PARAM::Math::RADIAN * PARAM::Math::PI); - shoot_dist_grade = PosToPosDistGrade(x,y,shoot_pos.x(),shoot_pos.y(),-1,"NORMAL"); + shoot_dir_grade = PosToPosDirGrade(x, y, shoot_pos.x(), shoot_pos.y(), 4 / PARAM::Math::RADIAN * PARAM::Math::PI); + shoot_dist_grade = PosToPosDistGrade(x, y, shoot_pos.x(), shoot_pos.y(), -1, "NORMAL"); shoot_grade = 0.2 * shoot_dir_grade + 0.8 * shoot_dist_grade; - pass_dir_grade = PosToPosDirGrade(x,y,dribbling_player_pos.x(),dribbling_player_pos.y(),4 / PARAM::Math::RADIAN * PARAM::Math::PI,1); - pass_dist_grade = PosToPosDistGrade(x,y,dribbling_player_pos.x(),dribbling_player_pos.y()); - pass_safty_grade = PosSafetyGrade(pVision,dribbling_player_pos,CGeoPoint(x,y)); + pass_dir_grade = PosToPosDirGrade(x, y, dribbling_player_pos.x(), dribbling_player_pos.y(), 4 / PARAM::Math::RADIAN * PARAM::Math::PI, 1); + pass_dist_grade = PosToPosDistGrade(x, y, dribbling_player_pos.x(), dribbling_player_pos.y()); + pass_safty_grade = PosSafetyGrade(pVision, dribbling_player_pos, CGeoPoint(x, y)); pass_grade = 0.5 * pass_dir_grade + 0.5 * pass_dist_grade; - if(x < 1000) - pass_grade = pass_grade - 0.4 * (1 - NumberNormalize(pVision ->ourPlayer(num).Pos().x(),1000,-500)); + if (x < 1000) + pass_grade = pass_grade - 0.4 * (1 - NumberNormalize(pVision->ourPlayer(num).Pos().x(), 1000, -500)); grade = 0.2 * pass_grade + 0.5 * shoot_grade + 0.3 * pass_safty_grade; - for(int j = 0;j < Tick[now].our.player_num;j++) + for (int j = 0; j < Tick[now].our.player_num; j++) { - if(Tick[now].our.player[j] == Tick[now].our.goalie_num || Tick[now].our.player[j] == num) continue; + if (Tick[now].our.player[j] == Tick[now].our.goalie_num || Tick[now].our.player[j] == num) + continue; double dist = pos.dist(pVision->ourPlayer(Tick[now].our.player[j]).Pos()); - if(min_dist_to_player > dist) + if (min_dist_to_player > dist) { min_dist_to_player = dist; } } - grade = grade - 0.3 * (1 - NumberNormalize(min_dist_to_player,1300,500)); -// std::ostringstream stream; -// stream << std::fixed << std::setprecision(2) << grade; -// std::string a_str = stream.str(); -// GDebugEngine::Instance() ->gui_debug_x(CGeoPoint(x,y),3); -// GDebugEngine::Instance() ->gui_debug_msg(CGeoPoint(x,y),a_str,3,0,80); + grade = grade - 0.3 * (1 - NumberNormalize(min_dist_to_player, 1300, 500)); + // std::ostringstream stream; + // stream << std::fixed << std::setprecision(2) << grade; + // std::string a_str = stream.str(); + // GDebugEngine::Instance() ->gui_debug_x(CGeoPoint(x,y),3); + // GDebugEngine::Instance() ->gui_debug_msg(CGeoPoint(x,y),a_str,3,0,80); if (max_grade < grade) { max_grade = grade; - max_grade_pos = CGeoPoint (x,y); + max_grade_pos = CGeoPoint(x, y); } GDebugEngine::Instance()->gui_debug_x(pos); } } - } - GDebugEngine::Instance() -> gui_debug_x(max_grade_pos,3); + GDebugEngine::Instance()->gui_debug_x(max_grade_pos, 3); return max_grade_pos; } - - - - - - - /** * 坐标点关于最佳射门点的评分 * @param {CVisionModule*} pVision : pVsion @@ -884,8 +873,8 @@ namespace Utils if (!isValidPass(pVision, CGeoPoint(x, y), CGeoPoint(PARAM::Field::PITCH_LENGTH / 2, y1), PARAM::Player::playerBuffer)) continue; pos_to_pos_dist_grade = PosToPosDistGrade(x, y, x1, y1, -1, "NORMAL"); - pos_to_pos_dir_grade = PosToPosDirGrade(x, y, x1, y1,1); - pos_safety_grade = PosSafetyGrade(pVision,CGeoPoint(x,y),CGeoPoint(x1,y1)); + pos_to_pos_dir_grade = PosToPosDirGrade(x, y, x1, y1, 1); + pos_safety_grade = PosSafetyGrade(pVision, CGeoPoint(x, y), CGeoPoint(x1, y1)); grade = 0.3 * pos_to_pos_dist_grade + 0.3 * pos_to_pos_dir_grade + 0.4 * pos_safety_grade; if (grade > max_grade) { @@ -899,7 +888,7 @@ namespace Utils } CGeoPoint GetShootPoint(const CVisionModule *pVision, int num) { - CGeoPoint player_pos = pVision -> ourPlayer(num).Pos(); + CGeoPoint player_pos = pVision->ourPlayer(num).Pos(); double x = player_pos.x(); double y = player_pos.y(); double pos_to_pos_dist_grade = 0; @@ -915,9 +904,9 @@ namespace Utils if (!isValidPass(pVision, CGeoPoint(x, y), CGeoPoint(PARAM::Field::PITCH_LENGTH / 2, y1), PARAM::Player::playerBuffer)) continue; pos_to_pos_dist_grade = PosToPosDistGrade(x, y, x1, y1, -1, "NORMAL"); - pos_to_pos_dir_grade = PosToPosDirGrade(x, y, x1, y1,1); - player_to_pos_dir_grade = RobotToPosDirGrade(pVision,num,player_pos,CGeoPoint(x1,y1)); - pos_safety_grade = PosSafetyGrade(pVision,CGeoPoint(x,y),CGeoPoint(x1,y1)); + pos_to_pos_dir_grade = PosToPosDirGrade(x, y, x1, y1, 1); + player_to_pos_dir_grade = RobotToPosDirGrade(pVision, num, player_pos, CGeoPoint(x1, y1)); + pos_safety_grade = PosSafetyGrade(pVision, CGeoPoint(x, y), CGeoPoint(x1, y1)); grade = 0.3 * pos_to_pos_dist_grade + 0.25 * pos_to_pos_dir_grade + 0.35 * pos_safety_grade + 0.1 * player_to_pos_dir_grade; if (grade > max_grade) { @@ -927,7 +916,7 @@ namespace Utils } Tick[now].globalData.confidence_shoot = max_grade; CGeoPoint ShootPoint(PARAM::Field::PITCH_LENGTH / 2, max_y); - GDebugEngine::Instance() ->gui_debug_x(ShootPoint,3); + GDebugEngine::Instance()->gui_debug_x(ShootPoint, 3); return ShootPoint; } /** @@ -942,8 +931,8 @@ namespace Utils */ bool isValidPass(const CVisionModule *pVision, CGeoPoint start, CGeoPoint end, double buffer) { - CGeoSegment Line(start,end); - for(int i = 0;i < Tick[now].their.player_num;i++) + CGeoSegment Line(start, end); + for (int i = 0; i < Tick[now].their.player_num; i++) { CGeoPoint player_pos(pVision->theirPlayer(i).Pos()); CGeoPoint player_pos(pVision->theirPlayer(i).Pos()); @@ -994,12 +983,12 @@ namespace Utils return grade_dir; } - double RobotToPosDirGrade(const CVisionModule *pVision,int num,CGeoPoint start,CGeoPoint end) + double RobotToPosDirGrade(const CVisionModule *pVision, int num, CGeoPoint start, CGeoPoint end) { - CGeoSegment new_segment (start,end); - double robot_dir = PARAM::Math::RADIAN * pVision ->ourPlayer(num).Dir(); + CGeoSegment new_segment(start, end); + double robot_dir = PARAM::Math::RADIAN * pVision->ourPlayer(num).Dir(); double target_dir = PARAM::Math::RADIAN * (end - start).dir(); - double grade = 1 - NumberNormalize(abs(robot_dir-target_dir),180,0); + double grade = 1 - NumberNormalize(abs(robot_dir - target_dir), 180, 0); return grade; } /** @@ -1137,8 +1126,10 @@ namespace Utils double NumberNormalize(double data, double max_data, double min_data) { double res = (data - min_data) / (max_data - min_data); - if(res > 1)res = 1; - if(res < 0)res = 0; + if (res > 1) + res = 1; + if (res < 0) + res = 0; return res; } @@ -1162,13 +1153,13 @@ namespace Utils * @param {double} y : * @return {bool} : */ - bool InExclusionZone(CGeoPoint Point,double buffer) + bool InExclusionZone(CGeoPoint Point, double buffer) { double x = Point.x(); double y = Point.y(); if (((x < (-1 * PARAM::Field::PITCH_LENGTH / 2) + PARAM::Field::PENALTY_AREA_DEPTH + buffer) || - (x > (PARAM::Field::PITCH_LENGTH / 2) - PARAM::Field::PENALTY_AREA_DEPTH - buffer))&& - (y > -1 * PARAM::Field::PENALTY_AREA_WIDTH / 2 - buffer&& y < PARAM::Field::PENALTY_AREA_WIDTH / 2 +buffer)) + (x > (PARAM::Field::PITCH_LENGTH / 2) - PARAM::Field::PENALTY_AREA_DEPTH - buffer)) && + (y > -1 * PARAM::Field::PENALTY_AREA_WIDTH / 2 - buffer && y < PARAM::Field::PENALTY_AREA_WIDTH / 2 + buffer)) { return true; } diff --git a/Core/tactics/skill/Defender.cpp b/Core/tactics/skill/Defender.cpp index 2d5ee72..a6d807e 100644 --- a/Core/tactics/skill/Defender.cpp +++ b/Core/tactics/skill/Defender.cpp @@ -1,5 +1,123 @@ #include "Defender.h" -void Defender::plan(const CVisionModule* pVision){ - setSubTask("SmartGoto",task()); +#include "utils.h" + +namespace +{ + /* 球场信息 */ + /* 己方半场信息 */ + const int FIELD_X_MIN = -PARAM::Field::PITCH_LENGTH / 2 + PARAM::Field::PENALTY_AREA_DEPTH; + const int FIELD_X_MAX = 0; + const int FIELD_Y_MIN = -PARAM::Field::PITCH_WIDTH / 2; + const int FIELD_Y_MAX = PARAM::Field::PITCH_WIDTH / 2; + + const CGeoLine FIELD_BOR[3] = { + {{FIELD_X_MIN, PARAM::Field::PENALTY_AREA_WIDTH / 2}, {FIELD_X_MIN, -PARAM::Field::PENALTY_AREA_WIDTH / 2}}, + {{PARAM::Field::PITCH_LENGTH / 2, PARAM::Field::PENALTY_AREA_WIDTH / 2}, {FIELD_X_MIN, PARAM::Field::PENALTY_AREA_WIDTH / 2}}, + {{-PARAM::Field::PITCH_LENGTH / 2, PARAM::Field::PENALTY_AREA_WIDTH / 2}, {FIELD_X_MIN, PARAM::Field::PENALTY_AREA_WIDTH / 2}}}; + const CGeoLine FIELD_PENALTYBOR({FIELD_X_MIN, PARAM::Field::PENALTY_AREA_WIDTH / 2}, {FIELD_X_MIN, -PARAM::Field::PENALTY_AREA_WIDTH / 2}); // 禁区所在直线 + + /* 禁区信息 */ + + /* 球员默认站位信息 */ + const CGeoPoint DEFAULT_STAND_POS(FIELD_X_MIN, PARAM::Field::PENALTY_AREA_WIDTH / 2); + const double DEFAULT_STAND_DIR = 0; + const double DEFAULT_DISTANCE_MAX = PARAM::FILED::PENALTY_AREA_WIDTH; // 两个后卫之间的最大距离 + const double DEFAULT_DISTANCE_MIN = 200.0; // 两个后卫之间的最小距离 +} + +void Defender::plan(const CVisionModule *pVision, const GlobalTick tick) +{ + auto ball = pVision->ball().pos(); + double their_angle = pVision->theirPlayer(tick.their.to_balldist_min_num); + + /* ====================================================================== */ + + /* STAND - 默认状态站在位置等待 */ + CGeoPoint stand_pos = DEFAULT_STAND_POS; + double stand_dir = -pVision->ball.Vel().dir(); + // MIND: 考虑是要恢复站位还是直接不动了 + + /* PRE - 准备态 */ + bool state_pre = pVision->ball().pos().x() > FIELD_X_MIN && pVision->ball().pos().x() < FIELD_X_MAX && pVision->ball().pos().y() > FIELD_Y_MIN && pVision->ball().pos().y() < FIELD_Y_MAX; + CGeoPoint hitPoint = ComputeCrossPENALTY(pVision->ball()); + if (hitPoint == {NULL, NULL}) // 朝向我方则防守 + state_pre = false; + + /* GET - 抢夺态 */ + bool state_get = pVision->ball.pos().dist() < 500; + // TODO: 预测滚动距离截球 + + /* WALL - 阻止态 */ + bool state_wall = pVision->theirPlayer(tick.their.to_balldist_min_num).Pos().dist(hitPoint) < 1000; + + /* ====================================================================== */ + + TaskT newTask(task()); + + if (NULL) + { + // 这里只是为了切换代码顺序（操作优先级）方便 + } + else if (1 == tick.ball.rights && state_wall) + { + _state = WALL; + newTask.player.pos = {FIELD_X_MIN, ComputeCrossPENALTY(pVision->ball()).y() + PARAM::Vehicle::V2::PLAYER_SIZE / 2}; + newTask.player.angle = stand_dir; + setSubTask("SmartGoto", newTask); + } + else if (1 == tick.ball.rights && state_get) + { + _state = GET; + setSubTask("Touch", newTask); + } + else if (state_pre) + { + // NOTE: 基本就是说车的间距跟着球走然后对球的跟随运动 + _state = PRE; + newTask.player.pos = {FIELD_X_MIN, ComputeCrossPENALTY(pVision->ball()).y() + ComputeDistance(pVision->ball()) / 2, hitPoint}; + newTask.player.angle = stand_dir; + setSubTask("SmartGoto", newTask); + } + else + { + newTask.player.pos = stand_pos; + newTask.player.angle = stand_dir; + setSubTask("SmartGoto", newTask); + } + Skill::plan(pVision); -} \ No newline at end of file +} + +/** + * 根据球的位置动态调整后卫间距离 + * @param {MobileVisionT} ball : 球 + * @param {CGeoPoint} hitPoint : 交点 + * @return {double} : 两后卫之间距离 + */ +double ComputeDistance(MobileVisionT ball, CGeoPoint hitPoint) +{ + double ballDis = ball.Pos().dist(hitPoint); + if (ballDis > PARAM::Field::PITCH_WIDTH / 2) + return DEFAULT_DISTANCE_MAX; + else if (ballDis < PARAM::Field::PENALTY_AREA_DEPTH) + return DEFAULT_DISTANCE_MIN; + else + return DEFAULT_DISTANCE_MIN + (DEFAULT_DISTANCE_MAX - DEFAULT_DISTANCE_MIN) * (ballDis / (PARAM::Field::PITCH_WIDTH / 2 - PARAM::Field::PENALTY_AREA_DEPTH)); +} + +/** + * 球方向与禁区边的交点 + * @param {MobileVisionT} ball : 球 + * @return {CGeoPoint} : {NULL, NULL} 时表示无交点 + */ +CGeoPoint ComputeCrossPENALTY(MobileVisionT ball) +{ + CGeoLineLineIntersection intersection(FIELD_PENALTYBOR, {ball.Pos(), ball.Vel().dir()}); // 获取球运动姿态的交点 + if (true == intersection.Intersectant) + { + return intersection.IntersectPoint; + } + return {NULL, NULL}; +} + +// TODO: 写个接口方便上层调用 diff --git a/Core/tactics/skill/Defender.h b/Core/tactics/skill/Defender.h index c85dead..c7aec3d 100644 --- a/Core/tactics/skill/Defender.h +++ b/Core/tactics/skill/Defender.h @@ -1,11 +1,25 @@ #pragma once #include "skill_registry.h" -class Defender : public Skill{ +class Defender : public Skill +{ public: Defender() = default; - virtual void plan(const CVisionModule* pVision) override; - virtual void toStream(std::ostream& os) const override{ os << "Defender"; } + virtual void plan(const CVisionModule *pVision) override; + virtual void toStream(std::ostream &os) const override { os << "Defender"; } + +private: + enum STATE + { + ERROR = -1, // 错误 + STAND = 0, // 等待 + PRE, // 准备态 + GET, // 抢夺态 + WALL, // 阻止态 + // SHOOT + }; + STATE _state; + std::string debug_state; }; REGISTER_SKILL(Defender, Defender); \ No newline at end of file