/**
* 主要是为了调整power用，为了避免power过大而跑过可
* 目标点。原则视为了更高层协调，power只可能减小不可
* 能增大。为了不必要到dash，power也可可能为零。也就
* dash不执行
* power 的正负由外面给定
* This funcition is mainly used to adjust ( usually decrease ) the dash power
* to avoid run over the target position by using a big power. The final power
* could be 0 to avoid an unnecessary dash.
* The sign of power is given outside the function.
* @param player the player to caculate.
* @param target the target position to go to.
* @param buffer
* @param power orignal power given.
* @return power adjusted.
*/
double Dasher::AdjustPowerForDash(const PlayerState & player, Vector target, double buffer, double power)
{
const double & speedmax = player.GetEffectiveSpeedMax();
const double & effort = player.GetEffort();
const double & accrate = player.GetDashPowerRate();
const Vector & pos = player.GetPos();
const Vector & vel = player.GetVel();
const double & facing = player.GetBodyDir();
if (pos.Dist(target) > speedmax + buffer){ //怎么跑的都不会跑过
return power;
}
if((pos + vel).Dist(target) < buffer) { //不跑也能到到达，就不用跑了
return 0.0;
}
Line dash_line(Ray(pos, player.GetBodyDir()));
Vector projective_target = dash_line.GetProjectPoint(target);
Vector acc = projective_target - pos - vel;
double dash_power = acc.Mod() / (effort * accrate);
if (GetAngleDegDiffer(acc.Dir(), facing) > 90.0){
dash_power = -dash_power;
}
if (power > 0){
return MinMax(ServerParam::instance().minDashPower(), dash_power, power);
}
else {
return MinMax(power, dash_power, ServerParam::instance().maxDashPower());
}
}
/**
* 通过传入kick的参数，计算kick后球的位置和速度
* Calculate ball position and velocity after a kick action.
* \param kick_power.
* \param kick_angle.
* \param player_state state of the player who is performing kick.
* \param ball_state ball state before the kick action is performed.
* \param ball_pos will be set to ball position after kick.
* \param ball_vel will be set to ball velocity after kick.
* \param is_self if the action is performed by the agent this process represents.
*/
void ActionEffector::ComputeInfoAfterKick(const double kick_power, const double kick_angle,
const PlayerState &player_state, const BallState &ball_state, Vector &ball_pos, Vector &ball_vel, bool is_self)
{
double power = GetNormalizeKickPower(kick_power);
double dir = GetNormalizeMoment(kick_angle);
Vector ball_2_player = (ball_state.GetPos() - player_state.GetPos()).Rotate(-player_state.GetBodyDir());
double eff_power = power *
(is_self ? player_state.GetKickRate() : GetKickRate(ball_2_player, player_state.GetPlayerType()));
Vector accel = Polar2Vector(eff_power, player_state.GetBodyDir() + dir);
ball_vel = ball_state.GetVel() + accel;
ball_pos = ball_state.GetPos() + ball_vel;
ball_vel *= ServerParam::instance().ballDecay();
}
void BehaviorHoldPlanner::Plan(std::list<ActiveBehavior> & behavior_list)
{
if (!mSelfState.IsKickable()) return;
if (mSelfState.IsGoalie()) return;
if(mStrategy.GetSureOppInterCycle() <= 2 &&
mStrategy.GetSureOppInterCycle() != 0 ){
ActiveBehavior hold(mAgent, BT_Hold);
double dDist;
Vector posAgent = mSelfState.GetPos();
PlayerState objOpp = mWorldState.GetOpponent(mPositionInfo.GetClosestOpponentToBall());
Vector posOpp = objOpp.GetPos();
dDist = (posOpp - posAgent).Mod();
AngleDeg angOpp = objOpp.GetBodyDir();
AngleDeg ang = 0.0;
if(dDist < 5 )
{
ang = ( posAgent - posOpp ).Dir();
int iSign = (GetNormalizeAngleDeg( angOpp - ang )) >0 ? -1:1;
ang += iSign*45 - mSelfState.GetBodyDir();
ang = GetNormalizeAngleDeg( ang );
}
if( mBallState.GetPos().Dist(posAgent + Polar2Vector(0.7,ang))< 0.3 )
{
Vector posBallPred = mBallState.GetPredictedPos(1);
Vector posPred = mSelfState.GetPredictedPos(1);
if( posPred.Dist( posBallPred )< 0.85 * mSelfState.GetKickableArea() )
{
hold.mDetailType = BDT_Hold_Turn;
hold.mEvaluation = 1.0 + FLOAT_EPS;
hold.mAngle = (Vector(ServerParam::instance().PITCH_LENGTH / 2.0, 0.0) - mSelfState.GetPos()).Dir();
mActiveBehaviorList.push_back(hold);
}
}
else
{
hold.mDetailType = BDT_Hold_Kick;
hold.mAngle = ang;
hold.mEvaluation = 1.0 + FLOAT_EPS;
mActiveBehaviorList.push_back(hold);
}
if (!mActiveBehaviorList.empty()) {
mActiveBehaviorList.sort(std::greater<ActiveBehavior>());
behavior_list.push_back(mActiveBehaviorList.front());
}
}
}
/**
* 通过传入dash的参数，计算dash后球员的位置和速度
* Calculate player position and velocity after a dash action. Conflicts or forbidden areas are not
* considered.
* \param dash_power.
* \param dash_dir.
* \param player_state state of the player who is dashing.
* \param player_pos will be set to player position after dash.
* \param player_vel will be set to player velocity after dash.
*/
void ActionEffector::ComputeInfoAfterDash(const double dash_power, double dash_dir,
const PlayerState &player_state, Vector &player_pos, Vector &player_vel)
{
double dir_rate = GetDashDirRate(dash_dir);
if (dash_power < 0.0) {
dash_dir += 180.0;
}
double eff_dash_power = fabs(dash_power) * player_state.GetEffort() * player_state.GetDashPowerRate() * dir_rate;
Vector accel = Polar2Vector(eff_dash_power, GetNormalizeAngleDeg(player_state.GetBodyDir() + dash_dir));
player_vel = player_state.GetVel() + accel;
player_pos = player_state.GetPos() + player_vel;
player_vel *= player_state.GetPlayerDecay();
}
/**
* player 跑到 target 的所需的最小周期数
* This function returns the minimum cycles for a player to go to a target position.
* @param player the player to caculate.
* @param target the target position to go to.
* @param can_inverse true means consider running backwards.
* @param buffer
* @return an integer to show the minimum cycles caculated.
*/
int Dasher::CycleNeedToPoint(const PlayerState & player, Vector target, bool can_inverse, double *buf)
{
const Vector & pos = player.GetPos();
const Vector & vel = player.GetVel();
const double dir = (target - pos).Dir();
double facing;
if (player.IsBodyDirValid()) {
facing = player.GetBodyDir();
}
else if (vel.Mod() > 0.26){
facing = vel.Dir();
}
else {
facing = dir; //认为不用转身
}
double diffang = fabs(GetNormalizeAngleDeg(dir - facing));
if (diffang < 90.0){
can_inverse = false; //没有必要倒着跑
}
if (can_inverse){
if (buf != 0) {
double tmp;
int cycle1 = CycleNeedToPointWithCertainPosture(player, target, true, buf);
int cycle2 = CycleNeedToPointWithCertainPosture(player, target, false, &tmp);
if (cycle2 <= cycle1) {
*buf = tmp;
}
return Min(cycle1, cycle2);
}
else {
int cycle1 = CycleNeedToPointWithCertainPosture(player, target, true);
int cycle2 = CycleNeedToPointWithCertainPosture(player, target, false);
return Min(cycle1, cycle2);
}
}
else {
return CycleNeedToPointWithCertainPosture(player, target, false, buf);
}
}
/*
* This function is used to correct the target position when near the ball.
* @param player the player to consider.
* @param target the target position to go to.
* @param fix the distance error to fix.
* @return position corrected.
*/
Vector Dasher::CorrectTurnForDash(const PlayerState & player, const Vector & target, double fix)
{
if (player.GetPosConf() > 0.99 - FLOAT_EPS && player.GetBodyDirConf() > 0.99 - FLOAT_EPS) {
Ray body_ray(player.GetPos(), player.GetBodyDir());
Line body_line(body_ray);
Vector closest_pt = body_line.GetProjectPoint(target);
double buffer = 0.05 + ServerParam::instance().ballSize();
if (closest_pt.Dist(player.GetPos()) > player.GetKickableArea() + buffer - fix){
if (closest_pt.Dist(target)
< player.GetKickableArea() - buffer + fix){
return closest_pt;
}
}
}
return target;
}
/**
* 计算用同一dash_power到达某点需要的时间
* Caculate cycles needed to a target position with a certain dash power.
* @param player the player to caculate.
* @param target the target position to go to.
* @param dash_power the power for dash to predict.
* @return an integer to show the cycles caculated.
*/
int Dasher::CyclePredictedToPoint(const PlayerState& player , Vector target , double dash_power)
{
double corrected_dash_power = dash_power;
double effective_power;
double predicted_stamina = player.GetStamina();
double predicted_effort = player.GetEffort();
double predicted_recovery = player.GetRecovery();
double predicted_capacity = player.GetCapacity();
double myang = player.GetBodyDir();
Vector position = player.GetPos();
Vector velocity;
if (player.GetVelConf() < FLOAT_EPS)
{
velocity = Vector(0,0);
}
else
{
velocity = player.GetVel();
}
Vector last_position = position;
int max_cyc = (int)(position.Dist(target) / player.GetEffectiveSpeedMax() + 100);
int last_action = 0; /* 0 -- turn , 1 -- dash*/
for (int i = 0;i < max_cyc;i++)
{
if (position.Dist(target) < PlayerParam::instance().AtPointBuffer())
{
return i;
}
if (position.Dist(target) > last_position.Dist(target) + FLOAT_EPS && last_action == 1)
{
return i;
}
last_position = position;
/*decide if we should turn*/
double targ_ang;
if (dash_power > 0)
{
targ_ang = (target - position).Dir() - myang;
}
else
{
targ_ang = (target - position).Dir() - GetNormalizeAngleDeg(myang + 180);
}
double max_go_to_point_angle_err = 4; //见08CP_max_go_to_point_angle_err
if (fabs(GetNormalizeAngleDeg(targ_ang)) > max_go_to_point_angle_err)
{
/*turnint*/
double this_turn = MinMax(-player.GetMaxTurnAngle(), GetNormalizeAngleDeg(targ_ang) , player.GetMaxTurnAngle());
myang += this_turn;
corrected_dash_power = 0;
last_action = 0;
}
else
{
corrected_dash_power = player.CorrectDashPowerForStamina(dash_power);
if (fabs(corrected_dash_power) > predicted_stamina)
{
effective_power = Sign(corrected_dash_power) * predicted_stamina;
}
else
{
effective_power = corrected_dash_power;
}
effective_power *= predicted_effort;
effective_power *= player.GetDashPowerRate();
velocity += Polar2Vector(effective_power , myang);
last_action = 1;
}
if (velocity.Mod() > player.GetEffectiveSpeedMax())
{
velocity *= (player.GetEffectiveSpeedMax()/ velocity.Mod());
}
position += velocity;
velocity *= player.GetPlayerDecay();
//08 内UpdatePredictStaminaWithDash函数 直接写入此函数
if (dash_power > 0)
{
predicted_stamina -= dash_power;
}
else
{
predicted_stamina -= 2 * dash_power;
}
if (predicted_stamina < 0)
{
predicted_stamina = 0;
}
if (predicted_stamina < ServerParam::instance().recoverDecStamina() && predicted_recovery > ServerParam::instance().recoverMin())
{
predicted_recovery -= ServerParam::instance().recoverDec();
}
if (predicted_stamina < ServerParam::instance().effortDecStamina() && predicted_effort > player.GetEffortMin())
{
predicted_effort -= ServerParam::instance().effortDec();
}
if (predicted_stamina > ServerParam::instance().effortIncStamina() && predicted_effort < player.GetEffortMax())
{
predicted_effort += ServerParam::instance().effortDec();
}
//增加capacity改进
double stamina_inc = Min(predicted_recovery * player.GetStaminaIncMax() , predicted_capacity);
predicted_stamina += stamina_inc;
if (predicted_stamina > ServerParam::instance().staminaMax())
{
predicted_stamina = ServerParam::instance().staminaMax();
}
predicted_capacity -= stamina_inc;
if (predicted_capacity < 0)
{
predicted_capacity = 0;
}
}
return max_cyc;
}
/**
* player 以确定得姿势（指倒着跑和正跑），跑到 target 所需要的周期数
* This function returns the minimum cycles for a player to go to a target position with
* a certain posture, forward or backward.
* @param player the player to caculate.
* @param target the target position to go to.
* @param inverse true means running backwards.
* @param buffer
* @return an integer to show the minimum cycles caculated.
*/
int Dasher::CycleNeedToPointWithCertainPosture(const PlayerState & player, Vector target, const bool inverse, double *buf)
{
int cycle = 0; //用的周期
const double & decay = player.GetPlayerDecay();
const double & speedmax = player.GetEffectiveSpeedMax();
const double & stamina = player.GetStamina();
const double & stamina_inc_max = player.GetStaminaIncMax();
const double & dash_max = ServerParam::instance().maxDashPower();
const Vector & pos = player.GetPos();
const Vector & vel = player.GetVel();
const double accrate = player.GetDashPowerRate() * player.GetEffort();
double speed = vel.Mod();
const Vector predict_pos_1 = pos + vel;
const Vector predict_pos_2 = predict_pos_1 + vel * decay;
const double dir = (target - pos).Dir();
double dis = (target - predict_pos_1).Mod();
const double kick_area = player.IsGoalie()? ServerParam::instance().catchAreaLength(): (player.GetKickableArea() - GETBALL_BUFFER);
if (dis <= kick_area){
dis = pos.Dist(target);
if (buf) *buf = 0;
return 1;
}
double facing;
if (player.IsBodyDirValid()) {
facing = player.GetBodyDir();
}
else if (speed > 0.26){
facing = vel.Dir();
}
else {
facing = dir; //认为不用转身
}
double diffang = fabs(GetNormalizeAngleDeg(dir - facing));
const double oneturnang = player.GetMaxTurnAngle();
const double stamina_recovery_thr = ServerParam::instance().recoverDecThr() * ServerParam::instance().staminaMax();
double angbuf = FLOAT_EPS;
angbuf = ASin(kick_area / dis);
angbuf = Max(angbuf , 15.0);
if (inverse) {
diffang = 180.0 - diffang;
facing = GetNormalizeAngleDeg(180.0 + facing);
}
//I 调整阶段
if(diffang <= angbuf){ //不需要转身
target = (target - pos).Rotate(-facing);
dis = fabs(target.X());
double y = fabs(target.Y());
if(y < kick_area){
dis -= sqrt(kick_area * kick_area - y * y);
}
speed *= Cos(vel.Dir() - facing); //身体方向上的投影
}
else if(diffang <= oneturnang){
cycle += 1;
target -= predict_pos_1;
speed *= Cos(vel.Dir() - dir); //取得目标方向的投影
speed *= decay;//进行投影.垂直方向1个周期后衰减到10+厘米了,并且在1turn时可加入考虑修正掉
dis = target.Mod();
dis -= kick_area;
}
else{ //认为转身两下（不细致）
cycle += 2;
target -= predict_pos_2;
speed *= Cos(vel.Dir() - dir); //取得目标方向的投影
speed *= decay * decay;//进行投影.垂直方向1个周期后衰减到10+厘米了,并且在1turn时可加入考虑修正掉
dis = target.Mod();
dis -= kick_area;
}
if (dis <= 0){
if(buf != NULL){
*buf = -dis / ( speed / decay);
*buf = Min(*buf , 0.99);
}
return Max(cycle, 0);
}
//II 加速 & 消耗体力阶段
const double stamina_used_per_cycle = inverse? dash_max * 2.0: dash_max;
const int full_cyc = int((stamina - stamina_recovery_thr) / (stamina_used_per_cycle - stamina_inc_max)); //满体力阶段
int acc_cyc = 0;//加速阶段
const double speedmax_thr = speedmax * decay * 0.98;
const double accmax = accrate * dash_max;
while(acc_cyc < full_cyc && speed < speedmax_thr){
speed += accmax;
if(speed > speedmax){
speed = speedmax;
}
dis -= speed;
if(dis <= 0){//还没加速到最大就跑到了...
cycle += acc_cyc + 1;
if(buf != NULL){
*buf = -dis /( speed / decay );
*buf = Min(*buf , 0.99);
}
return Max(cycle, 0);
}
speed *= decay;
++ acc_cyc;
}
cycle += acc_cyc;
//III 满体匀速阶段
int aver_cyc = full_cyc - acc_cyc;
double aver_cyc_dis = aver_cyc * speedmax;
if(aver_cyc_dis >= dis){
if(buf != NULL){
double realcyc = cycle + dis / speedmax;
cycle = int( ceil(realcyc) );
*buf = cycle - realcyc;
return Max(cycle, 0);
}
else{
cycle = int(ceil( cycle + dis / speedmax));
return Max(cycle, 0);
}
}
else{
cycle += aver_cyc;
dis -= aver_cyc_dis;
}
//IV 没体(0消耗)减速阶段
double acc_tired = stamina_inc_max * accrate;
double speed_tired = acc_tired / (1 - decay);
double speed_tired_thr = speed_tired * decay;
speed *= decay;
while(dis > 0 && fabs(speed - speed_tired_thr) > 0.004){
speed += acc_tired;
dis -= speed;
speed *= decay;
++cycle;
}
if(dis <= 0){
if(buf != NULL){
*buf = -dis / ( speed / decay);
*buf = Min(*buf , 0.99);
}
return Max(cycle, 0);
}
//V 没体(0消耗)匀速阶段
if( buf != NULL){
double realcyc = cycle + dis / speed_tired;
cycle = int( ceil( realcyc ) );
*buf = cycle - realcyc;
return Max(cycle, 0);
}
else{
cycle = cycle + int(ceil ( dis / speed_tired));
return Max(cycle, 0);
}
}
/**
* 通过传入turn的参数，计算turn后球员的身体朝向和脖子朝向
* Calculate player body direction after a turn action.
* \param turn_angle.
* \param player_state state of the player who is turning.
* \param body_dir will be set to player's body direction after turn.
*/
void ActionEffector::ComputeInfoAfterTurn(const AngleDeg moment,
const PlayerState &player_state, AngleDeg &body_dir)
{
double turn_angle = GetTurnAngle(moment, player_state.GetPlayerType(), player_state.GetVel().Mod());
body_dir = GetNormalizeAngleDeg(player_state.GetBodyDir() + turn_angle);
}