bool Tackler::GetTackleInfoToDir(const Agent & agent, AngleDeg dir, AngleDeg *p_tackle_angle, Vector *p_ball_vel)
{
UpdateTackleData(agent);
Array<int, 3> dir_idx;
dir_idx[0] = dir2idx(dir); //当前区间
dir_idx[1] = dir2idx(dir - 1.0); //左边相邻区间
dir_idx[2] = dir2idx(dir + 1.0); //右边相邻区间
Vector optimal_ball_vel;
AngleDeg best_tackle_angle = 0.0;
double max_ball_speed = -1.0;
bool ret = false;
for (int j = 0; j < 3; ++j) {
for (uint i = 0; i < mDirMap[dir_idx[j]].size(); ++i) {
const int angle_idx1 = mDirMap[dir_idx[j]][i].first;
const int angle_idx2 = mDirMap[dir_idx[j]][i].second;
AngleDeg dir1 = mBallVelAfterTackle[angle_idx1].Dir();
AngleDeg dir2 = mBallVelAfterTackle[angle_idx2].Dir();
if (IsAngleDegInBetween(dir1, dir, dir2)) {
ret = true;
if (p_tackle_angle || p_ball_vel) { //这里要找出使出球速度最大的tackle_angle
dir2 = GetNormalizeAngleDeg(dir2, dir1);
dir = GetNormalizeAngleDeg(dir, dir1);
double rate = (dir2 - dir) / (dir2 - dir1);
Vector ball_vel = mBallVelAfterTackle[angle_idx1] * rate + mBallVelAfterTackle[angle_idx2] * (1.0 - rate);
double ball_speed = ball_vel.Mod();
if (ball_speed > max_ball_speed) {
optimal_ball_vel = ball_vel;
max_ball_speed = ball_speed;
best_tackle_angle = mTackleAngle[angle_idx1] * rate + mTackleAngle[angle_idx2] * (1.0 - rate);
}
continue;
}
return true;
}
}
}
if (ret) {
if (p_ball_vel) {
*p_ball_vel = optimal_ball_vel;
}
if (p_tackle_angle) {
*p_tackle_angle = best_tackle_angle;
}
}
return ret;
}
void VisualSystem::DoVisualExecute()
{
mBestVisualAction.mDir = GetNormalizeAngleDeg(mBestVisualAction.mDir);
AngleDeg finalnec = GetNormalizeAngleDeg(mBestVisualAction.mDir - mPreBodyDir); //最后要看的方向与当前（或者说执行过本周期动作后，如turn）身体正对方向的夹角，即最后的neckrelangle。
if (fabs(finalnec) > ServerParam::instance().maxNeckAngle()) {
if (mCanTurn) {
if (finalnec < 0.0) {
mpAgent->Turn(finalnec - ServerParam::instance().minNeckAngle());
mpAgent->TurnNeck(ServerParam::instance().minNeckMoment());
}
else {
mpAgent->Turn(finalnec - ServerParam::instance().maxNeckAngle());
mpAgent->TurnNeck(ServerParam::instance().maxNeckMoment());
}
}
else {
if (finalnec < 0.0) {
mpAgent->TurnNeck(ServerParam::instance().minNeckMoment());
}
else {
mpAgent->TurnNeck(ServerParam::instance().maxNeckMoment());
}
}
}
else {
finalnec -= mpSelfState->GetNeckDir();
mpAgent->TurnNeck(finalnec);
}
if (mViewWidth != mpSelfState->GetViewWidth()) {
mpAgent->ChangeView(mViewWidth);
}
}
bool VisualSystem::DealWithSetPlayMode()
{
static bool check_both_side = false;
static bool check_one_side = false;
PlayMode play_mode = mpWorldState->GetPlayMode();
PlayMode last_play_mode = mpWorldState->GetLastPlayMode();
if (play_mode == PM_Our_Penalty_Ready || play_mode == PM_Our_Penalty_Setup || play_mode == PM_Our_Penalty_Taken) return false;
if (play_mode != PM_Play_On && !(mForceToSeeObject.GetOfBall()) && !mpSelfState->IsGoalie()
&& play_mode != PM_Before_Kick_Off
&& last_play_mode != PM_Before_Kick_Off
&& last_play_mode != PM_Our_Kick_Off
&& last_play_mode != PM_Opp_Kick_Off
){
//特殊模式下要强制看看全场， 防止遗漏
if (mpWorldState->CurrentTime() < mpWorldState->GetPlayModeTime() + 3){ //前三个周期等视觉信息到来
check_both_side = false;
check_one_side = false;
}
else {
if (!mpAgent->IsNewSight()) return true;
if (!check_both_side) {
if (!check_one_side){
int diff = mpWorldState->CurrentTime() - mpWorldState->GetPlayModeTime();
int flag = diff % 6;
int base = mpSelfState->GetUnum() % 6; //不要大家都一起改变视角，这样可能一起错过对方发球的瞬间
if (flag == base) {
mPreBodyDir = mpAgent->GetSelfBodyDirWithQueuedActions();
mCanTurn = false;
mBestVisualAction.mDir = GetNormalizeAngleDeg(mPreBodyDir + 90.0);
ChangeViewWidth(VW_Wide); //new info will come in 3 cycle
mCanForceChangeViewWidth = false;
DoVisualExecute();
check_one_side = true;
return true;
}
}
else {
mBestVisualAction.mDir = GetNormalizeAngleDeg(mpSelfState->GetNeckGlobalDir() + 180.0);
ChangeViewWidth(VW_Wide); //new info will come in 3 cycle
mCanForceChangeViewWidth = false;
DoVisualExecute();
check_one_side = false;
check_both_side = true;
return true;
}
}
}
}
return false;
}
/** 以最快的方式跑到目标点
* Run to the destination point with the fastest method.
* \param agent the agent itself.
* \param act an atomic action caculated to go to pos for this cycle.
* \param pos the destination point.
* \param buffer point buffer, means the real destinantion is a cycle with the certer
* of pos and radius of buffer.
* \param power the intend power to use.
* \param can_inverse true means can run in the inverse direction of the agent's body.
* \param turn_first true means that the agent should turn first to adjust the direction.
*/
void Dasher::GoToPoint(Agent & agent, AtomicAction & act, Vector target, double buffer, double power, bool can_inverse, bool turn_first)
{
act.Clear();
const Vector & my_pre_pos = agent.Self().GetPredictedPos(1);
double dist = my_pre_pos.Dist(target);
if (dist < buffer){
act.mSucceed = true;
return;
}
AngleDeg dirdiff = GetNormalizeAngleDeg((target - my_pre_pos).Dir() - agent.GetSelf().GetBodyDir());
if (fabs(dirdiff) < 90.0){
can_inverse = false; //不需要倒着跑
}
bool inverse = false;
if (can_inverse){
int cycle = CycleNeedToPointWithCertainPosture(agent.Self(), target, true);
if (cycle < 9 && cycle < CycleNeedToPointWithCertainPosture(agent.Self(), target, false)){
inverse = true;
}
}
GoToPointWithCertainPosture(agent, act, target, buffer, power, inverse, turn_first);
}
/* picks a rotation (CW or CCW) to turnball based on the nearest opponent or
teamless player */
TurnDir RotToAvoidOpponent(float abs_dir)
{
TurnDir rot;
float dist = HUGE;
AngleDeg opp_ang;
Unum opp = Mem->ClosestOpponent();
AngleDeg ball_ang = Mem->BallAngleFromBody();
if (opp != Unum_Unknown) {
dist = Mem->OpponentDistance(opp);
opp_ang = Mem->OpponentAngleFromBody(opp);
}
if (Mem->NumTeamlessPlayers() > 0) {
Vector pos = Mem->ClosestTeamlessPlayerPosition();
float d = Mem->DistanceTo(pos);
if (d < dist) {
dist = d;
opp_ang = Mem->AngleToFromBody(pos);
}
}
if (dist < Mem->CP_turnball_opp_worry_dist) {
/* there is an opponent near enough to worry about */
DebugKick(printf("In RotToAvoidOpponent, avoiding opponent\n"));
AngleDeg ball_to_targ = GetNormalizeAngleDeg((abs_dir - Mem->MyBodyAng()) - ball_ang);
AngleDeg opp_to_targ = GetNormalizeAngleDeg((abs_dir - Mem->MyBodyAng()) - opp_ang);
if ( ball_to_targ * opp_to_targ < 0 ){ /* they're on opposite sides of the target */
LogAction2(120, "RotToAvoidOpponent: CLOSEST");
rot = TURN_CLOSEST;
}
else if ( ball_to_targ < opp_to_targ ){
LogAction2(120, "RotToAvoidOpponent: CW");
rot = TURN_CW;
}
else{
LogAction2(120, "RotToAvoidOpponent: CCW");
rot = TURN_CCW;
}
} else {
LogAction2(120, "RotToAvoidOpponent: no opponents close enough");
rot = TURN_CLOSEST;
}
return rot;
}
/**
* Get ball velocity after tackle.
* \param agent.
* \param angle tackle angle.
* \return ball velocity.
*/
Vector Tackler::GetBallVelAfterTackle(const Agent & agent, AngleDeg tackle_angle)
{
UpdateTackleData(agent);
tackle_angle = GetNormalizeAngleDeg(tackle_angle, 0.0);
int idx1 = ang2idx(tackle_angle); //下界
int idx2 = ang2idx(tackle_angle + 1.0); //上界
double rate = MinMax(0.0, idx2 - tackle_angle, 1.0);
return mBallVelAfterTackle[idx1] * rate + mBallVelAfterTackle[idx2] * (1.0 - rate);
}
bool BehaviorGoalieExecuter::Execute(const ActiveBehavior & act_bhv)
{
if (act_bhv.mDetailType == BDT_Goalie_Position) {
return Dasher::instance().GoToPoint(mAgent, act_bhv.mTarget);
}
else if (act_bhv.mDetailType == BDT_Goalie_Catch) {
return mAgent.Catch(GetNormalizeAngleDeg((mBallState.GetPos() - mSelfState.GetPos()).Dir() - mSelfState.GetBodyDir()));
}
else {
return false;
}
}
Dasher::Dasher()
{
Assert(std::fabs(ServerParam::instance().dashAngleStep() - 45.0) < FLOAT_EPS);
for (int i = 0; i < 8; ++i) {
DASH_DIR[i] = GetNormalizeAngleDeg(ServerParam::instance().dashAngleStep() * i);
DIR_RATE[i] = GetDashDirRate(DASH_DIR[i]);
}
for (int i = 0; i < 8; ++i) {
ANTI_DIR_IDX[i] = GetDashDirIdx(DASH_DIR[i] + 180.0);
}
}
/**
* 通过传入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);
}
}
/**
* 将身体转向特定方向
* Turn body to a certain direction.
* @param agent the agent itself.
* @param ang the angle to turn to.
* @return an atomic action to turn the body.
*/
AtomicAction Dasher::GetTurnBodyToAngleAction(const Agent & agent, AngleDeg ang)
{
AtomicAction action;
action.mType = CT_Turn;
double angle_turn = GetNormalizeAngleDeg(ang - agent.GetSelf().GetBodyDir());
if (fabs(angle_turn) < agent.GetSelf().GetMaxTurnAngle())
{
action.mSucceed = true;
action.mTurnAngle = angle_turn;
}
else
{
action.mSucceed = false;
action.mTurnAngle = Sign(angle_turn) * agent.GetSelf().GetMaxTurnAngle();
}
return action;
}
/** 以确定的姿势（指倒着跑和正跑），跑到目标点
* Run to the destination point with a certain posture, forward or backword.
* \param agent the agent itself.
* \param act an atomic action caculated to go to pos for this cycle.
* \param pos the destination point.
* \param buffer point buffer, means the real destinantion is a cycle with the center
* of pos and radius of buffer.
* \param power the intend power to use.
* \param inverse true means run backward.
* \param turn_first true means that the agent should turn first to adjust the direction.
*/
void Dasher::GoToPointWithCertainPosture(Agent & agent, AtomicAction & act, Vector target, double buffer, double power, const bool inverse, bool turn_first)
{
act.Clear();
const Vector & my_pre_pos = agent.Self().GetPredictedPos(1);
double dist = my_pre_pos.Dist(target);
AngleDeg dirdiff = inverse? GetNormalizeAngleDeg((my_pre_pos - target).Dir() - agent.GetSelf().GetBodyDir()): GetNormalizeAngleDeg((target - my_pre_pos).Dir() - agent.GetSelf().GetBodyDir()); //需要转身的角度
double bufang;
buffer = Max(0.05, buffer); //TODO: wait test
if (buffer > FLOAT_EPS){
bufang = ASin(buffer / dist / 2) * 2;
bufang = Max(10.0, bufang);
}
else {
bufang = 0.0;
}
if (fabs(dirdiff) > bufang){
TurnDashPlaning(agent, act, target, buffer, power, inverse, turn_first);
}
else {
power = inverse? -fabs(power): fabs(power);
power = agent.GetSelf().CorrectDashPowerForStamina(power);
if (fabs(power) > FLOAT_EPS){
act.mType = CT_Dash;
act.mDashPower = AdjustPowerForDash(agent.Self(), target, buffer, power);
act.mDashPower = agent.GetSelf().CorrectDashPowerForStamina(act.mDashPower);
if (fabs(act.mDashPower) > FLOAT_EPS){
act.mSucceed = true;
}
else {
act.mType = CT_None; //power为零时就不要执行了，没意义
TurnDashPlaning(agent, act, target, buffer, power, inverse, turn_first);//再次考虑转身，以免漏掉一线好的转身的机会
}
}
}
}
bool Tackler::MayDangerousIfTackle(const PlayerState & tackler, const WorldState & world_state)
{
if (tackler.GetTackleProb(false) < FLOAT_EPS && tackler.GetTackleProb(true) < FLOAT_EPS) { //不可铲
return false;
}
const Vector tackler_pos = tackler.GetPos();
const Vector ball_pos = world_state.GetBall().GetPos();
const Vector ball_2_tackler = ball_pos - tackler_pos;
const double ball_dist2 = ball_2_tackler.Mod2();
const AngleDeg ball_dir = ball_2_tackler.Dir();
for (unsigned i = 0; i < world_state.GetPlayerList().size(); ++i) {
const PlayerState & opp = *world_state.GetPlayerList()[i];
if (!opp.IsAlive()) continue; //dead
if (opp.GetUnum() * tackler.GetUnum() > 0) continue; //not opp
if (opp.IsIdling()) continue; //idling
if (!opp.IsKickable()) continue; //not kickable
//cond. 1. opp no dashing -- 无从知晓，这里认为对手一定会dash
//cond. 2. ball near -- 球离自己更近，不构成危险情况
Vector opp_2_tackler = opp.GetPos() - tackler_pos;
if (opp_2_tackler.Mod2() > ball_dist2) continue;
//cond. 3. behind or big y_diff -- 对手在自己与球连线方向上在自己身后或错开位置太大，不构成危险情况
opp_2_tackler = opp_2_tackler.Rotate(-ball_dir);
if (opp_2_tackler.X() < 0.0 || std::fabs( opp_2_tackler.Y() ) > opp.GetPlayerSize() + tackler.GetPlayerSize()) continue;
//cond. 4. over body_diff -- 对手身体角度跟自己与球连线方向的夹角大于90度，不构成危险情况
AngleDeg body_diff = std::fabs( GetNormalizeAngleDeg( opp.GetBodyDir() - ball_dir ) );
if (body_diff > 90.0) continue;
return true;
}
return false;
}
/**
* 计算用同一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);
}
}
/**
* 考虑转身或直接dash
* Planing to turn or to dash.
* @param agent the agent itself.
* @param act an atomic action to return the caculated decision.
* @param target the target position to go to.
* @param buffer
* @param power the intend power for dash.
* @param inverse true means running backwards.
* @param turn_first true means turn first manually.
*/
void Dasher::TurnDashPlaning(Agent & agent, AtomicAction & act, Vector target, double buffer, double power, bool inverse, bool turn_first)
{
act.Clear();
power = inverse? -fabs(power): fabs(power);
power = agent.GetSelf().CorrectDashPowerForStamina(power);
PlayerState & self = agent.Self();
const Vector & my_pre_pos = self.GetPredictedPos(1);
double target_dist = my_pre_pos.Dist(target);
AngleDeg target_ang = inverse? GetNormalizeAngleDeg((my_pre_pos - target).Dir() - agent.GetSelf().GetBodyDir()): GetNormalizeAngleDeg((target - my_pre_pos).Dir() - agent.GetSelf().GetBodyDir()); //需要转身的角度
//DT_none
//处理转身问题,可能1dash + 1turn更好
double bufang;
if(buffer > FLOAT_EPS){
bufang = ASin(buffer / target_dist / 2) * 2;
bufang = Max(10.0, bufang);
}
else{
bufang = 0.0;
}
const double & effort = self.GetEffort();
const double & accrate = self.GetDashPowerRate();
const double & inertia_moment = self.GetInertiaMoment();
const double & decay = self.GetPlayerDecay();
const double & speedmax = self.GetEffectiveSpeedMax();
const Vector & vel = self.GetVel();
const double & facing = self.GetBodyDir();
double speed = vel.Mod();
double diffang = fabs(target_ang);
double oneturnang = self.GetMaxTurnAngle();
if(buffer < FLOAT_EPS && target_dist > 2.6){
oneturnang += 2.6;
}
if(turn_first || diffang <= oneturnang || speed < 0.04){ //由于噪声的存在，在非身体方向也有速度，调也调不过来，太小时不如直接转,0.04是一般队员的误差极值
act.mType = CT_Turn;
act.mTurnAngle = target_ang;
act.mSucceed = true;
return;
}
else {
//计算1 dash + 1turn;
//下面都是算dash到什么速度最好
double mspd1 = (180.0/ ( diffang + 10 ) - 1.0 ) / inertia_moment; //10是buf，不然容易调整后正好还转不过来
double dash2spd = Min(mspd1 / decay, speedmax) / (1 + ServerParam::instance().playerRand());
if( fabs(GetNormalizeAngleDeg(vel.Dir() - facing)) > 90.0){
speed = -speed;
}
double spd_inf = Max(speed - accrate * ServerParam::instance().maxDashPower(), -dash2spd);
double spd_sup = Min(speed + accrate * ServerParam::instance().maxDashPower(), dash2spd);
Ray cur_r(self.GetPos(), facing);
Vector pt_inf = cur_r.GetPoint(spd_inf * (1.0 + decay));
Vector pt_sup = cur_r.GetPoint(spd_sup * (1.0 + decay));
Vector pt;//要取得点
bool bnegtive;//要跑的点是不是在身后
Line pdl = Line(cur_r).GetPerpendicular(target);//垂线
if(pdl.IsPointInSameSide(pt_inf, pt_sup)){
if(fabs(GetNormalizeAngleDeg(facing-target_ang)) < 90.0){
pt = pt_sup;
bnegtive = (spd_sup<0);
}
else{
pt = pt_inf;
bnegtive = (spd_inf<0);
}
}
else{
double dist;
bnegtive = !cur_r.Intersection(pdl, dist); //垂点
pt = cur_r.GetPoint(dist);
}
double dis = pt.Dist(target);//两个周期后离point的距离,用来compare with 2 turn
double twoturnang = oneturnang + 180.0 / (1.0 + inertia_moment * fabs(speed) * decay);
if(twoturnang > diffang){
const Vector & pt2 = self.GetPredictedPos(2);
double dis2 = pt2.Dist(target);
if(dis2 < dis || diffang > 102.6){ //一次转太大，误差也大不如直接2turn
act.mType = CT_Turn;
act.mTurnAngle = target_ang;
act.mSucceed = true;
return;
}
}
double spd_need;
if(bnegtive){
spd_need = -(pt - self.GetPos()).Mod() / (1.0 + decay);
}
else{
spd_need = (pt - self.GetPos()).Mod() / (1.0 + decay);
}
//会总是dash而dash后又转不过来...,看着就像不拿球
double turnang_after_dash = 180.0 / (1.0 + inertia_moment * fabs(spd_need) * decay) - 5.0;
if(GetAngleDegDiffer((target - pt).Dir(), facing) > turnang_after_dash ){
act.mType = CT_Turn;
act.mTurnAngle = target_ang;
act.mSucceed = true;
return;
}
double power = ( spd_need - speed ) / accrate / effort;
power = MinMax(-ServerParam::instance().maxDashPower(), power, ServerParam::instance().maxDashPower());
//I over
act.mType = CT_Dash;
act.mDashPower = AdjustPowerForDash(self, pt, FLOAT_EPS, power);
act.mDashPower = agent.GetSelf().CorrectDashPowerForStamina(act.mDashPower);
if(fabs(act.mDashPower) > FLOAT_EPS)
{
act.mSucceed = true;
}
else {
act.mType = CT_Turn; //power为零时就不要执行了，没意义
act.mTurnAngle = 0.0;
act.mSucceed = true;
}
}
}
/* returns 1 if a kick actually done */
int smart_kick_hard_abs(float abs_dir, KickMode mode, float targ_vel,
TurnDir rot)
{
TurnKickCommand HKCommand;
HKCommand.time = -1;
HKCommand.turn_neck = FALSE;
DebugKick(printf("\nsmart_kick_hard: Time: %d\n", Mem->CurrentTime.t));
LogAction5(50, "smart_kick_hard_abs: mode = %d, angle = %.1f, targ_vel %.4f ",
mode, abs_dir, targ_vel);
if (!Mem->BallPositionValid()) {
my_error("smart_kick_hard called with ball position not valid");
return 0;
}
if (!Mem->BallKickable()) {
my_error("smart_kick_hard called with ball not kickable!");
return 0;
}
/* if the velocity isn's valid, turn to face ball */
if ( !Mem->BallVelocityValid() ) {
float ball_ang_from_body = Mem->BallAngleFromBody(); /* for efficiency */
LogAction2(60, "smart_kick_hard_abs: turning to face ball");
DebugKick2(printf("smart_kcik_hard: turning to face ball\n"));
if (Mem->CanSeeBallWithNeck()) {
if (Mem->PlayMode == PM_Play_On ||
(Mem->PlayMode == PM_My_Goal_Kick && Mem->LastAction->type == CMD_kick)) {
LogAction2(70, "smart_kick_hard_abs: turning neck and stopping ball");
HKCommand.time = Mem->CurrentTime;
HKCommand.type = CMD_kick;
HKCommand.angle = ball_ang_from_body + 180;
HKCommand.power = Mem->CP_stop_ball_power;
HKCommand.turn_neck = TRUE;
HKCommand.turn_neck_angle = Mem->LimitTurnNeckAngle(Mem->BallAngleFromNeck());
} else {
LogAction2(70, "smart_kick_hard_abs: just turniong neck");
HKCommand.time = Mem->CurrentTime;
HKCommand.type = CMD_none;
HKCommand.angle = 0;
HKCommand.power = 0;
HKCommand.turn_neck = TRUE;
HKCommand.turn_neck_angle = Mem->LimitTurnNeckAngle(Mem->BallAngleFromNeck());
}
} else {
/* turn body to face ball, and turn neck to straight ahead */
LogAction2(70, "smart_kick_hard_abs: turning neck and body");
HKCommand.time = Mem->CurrentTime;
HKCommand.type = CMD_turn;
HKCommand.turn_neck = TRUE;
if (fabs(ball_ang_from_body) > Mem->MaxEffectiveTurn()) {
/* out body can't get to where we want to go */
HKCommand.angle = 180; /* get our maximum effective turn */
HKCommand.turn_neck_angle = ball_ang_from_body -
signf(ball_ang_from_body)*Mem->MaxEffectiveTurn();
} else {
HKCommand.angle = ball_ang_from_body;
HKCommand.turn_neck_angle = -Mem->MyNeckRelAng();
}
}
return DoTurnKickCommand(HKCommand);
}
LogAction2(210, "HardestKick: got past vel check");
rot = KickRotationDirectionAbs(abs_dir, rot);
LogAction2(210, "HardestKick: got past rot setting");
#ifdef NEVER
/* With the new kick code, the ball goes through the player,
so we don't usually need to do a turn-ball as part of a shot */
if (mode <= KM_Moderate &&
Mem->IsPointBehind(Mem->BallAbsolutePosition(), abs_dir)) {
/* see if we need to rotate one way */
DebugKick(cout << "smart_kick_hard: decign if rotating " << rot << endl);
/* now decide if ball is on the wrong side */
if (Mem->IsPointBehind(Mem->BallAbsolutePosition(), abs_dir+((int)rot)*90)) {
/* start rotating right way */
DebugKick(cout << "smart_kick_hard: special turnball to avoid opp" << endl);
TurnKickCommand com;
KickToRes res = turnball_kick( abs_dir - Mem->MyAng(),
//abs_dir + ((int)rot)*90 - Mem->MyAng(),
rot, FALSE, &com);
if (res == KT_DidNothing || res == KT_LostBall)
my_error("smart_kick_hard: special turnball; turnkick failed, res: %d", res);
else
return DoTurnKickCommand(com);
}
}
#endif
switch (mode) {
case KM_None:
my_error("KM_None is not a valid kick mode for smart_kick_hard!");
break;
// HKStep meanings: 0 turning player, 1 moving ball, 2 using moderate
case KM_HardestKick: {
if (Mem->CurrentTime - 1 != Mem->HKTime) {
DebugKick(printf("First in a chain of calls\n"));
LogAction2(210, "HardestKick: Setting up for first call");
/* this is the first in a chain of calls */
Mem->HKStep = 0;
/* decide which way to rotate ball */
Mem->HKrot = rot;
/*AngleDeg ang = Mem->BallAngle() + Mem->MyAng() - abs_dir;
NormalizeAngleDeg(&ang);
if (ang >= 0)
Mem->HKrot = TURN_CW;
else
Mem->HKrot = TURN_CCW;*/
} else {
LogAction4(210, "HardestKick: Setting new step %d (old %d)", Mem->HKStepNext, Mem->HKStep);
Mem->HKStep = Mem->HKStepNext;
}
if (Mem->HKStep == 0) {
/* see if we need to turn */
LogAction2(210, "HardestKick: processing for step 0");
AngleDeg target_dir = abs_dir - Mem->MyBodyAng() +
((int)Mem->HKrot)*Mem->CP_hardest_kick_player_ang;
NormalizeAngleDeg(&target_dir);
LogAction2(210, "HardestKick: (0) norm angle");
if (fabs(target_dir) > Mem->CP_max_hard_kick_angle_err) {
LogAction2(210, "HardestKick: (0) need to turn!");
LogAction3(210, "HardestKick: (0) BallVelValid: %f", Mem->BallVelocityValid());
LogAction4(210, "HardestKick: (0) speed %f, %f!",
Mem->BallSpeed(), Mem->CP_hard_kick_dist_buffer);
LogAction3(210, "HardestKick: (0) will be kick %d",
Mem->BallWillBeKickable(1,0,Mem->CP_hard_kick_dist_buffer));
if (!Mem->BallWillBeKickable(1, 0, Mem->CP_hard_kick_dist_buffer) &&
Mem->BallVelocityValid() &&
Mem->BallSpeed() > Mem->CP_hard_kick_dist_buffer) {
LogAction2(70, "smart_kick_hard_abs: would turn for power, but must stop ball");
if (Mem->BallVelocityValid()) {
LogAction2(70, "smart_kick_hard_abs: would turn for power, but stopping ball");
HKCommand = dokick(-180, 0, 1.0);
if (HKCommand.time == -1) {
my_error("Stopping ball for dokick failed!");
}
} else {
LogAction2(70, "smart_kick_hard_abs: would turn for power, but stopping unknown vel ball");
HKCommand.time = Mem->CurrentTime;
HKCommand.type = CMD_kick;
HKCommand.angle = Mem->BallAngleFromBody() + 180;
HKCommand.power = Mem->CP_stop_ball_power;
HKCommand.turn_neck = TRUE;
HKCommand.turn_neck_angle = Mem->LimitTurnNeckAngle(Mem->BallAngleFromNeck());
}
//Mem->HKTime = Mem->CurrentTime;
LogAction2(210, "HardestKick: (0) setting step next to 0");
Mem->HKStepNext = 0; //stay in turn, we'll drop through next times
} else {
LogAction4(70, "smart_kick_hard_abs: hardest turning for power %f %d",
target_dir, Mem->CP_max_hard_kick_angle_err);
DebugKick(printf("turning to get hard kick\n"));
//Mem->HKTime = Mem->CurrentTime;
Mem->HKStepNext = 0; //stay in turn, we'll drop through next times
HKCommand.type = CMD_turn;
HKCommand.angle = target_dir;
HKCommand.time = Mem->CurrentTime;
HKCommand.turn_neck = FALSE;
//break; //used to be: return DoTurnKickCommand(HKCommand);
}
}
}
LogAction2(210, "HardestKick: after 0 processing");
/* AngleDeg turn_target = abs_dir + 180 +
((int)Mem->HKrot)*Mem->CP_hardest_kick_angle_disp; */
AngleDeg turn_target = abs_dir +
((int)Mem->HKrot)*(Mem->CP_hardest_kick_ball_ang);
NormalizeAngleDeg(&turn_target);
AngleDeg targ_err = turn_target-Mem->MyBodyAng()-Mem->BallAngleFromBody();
NormalizeAngleDeg(&targ_err);
if ( (Mem->HKStep == 1 ||
(Mem->HKStep < 1 && HKCommand.time != Mem->CurrentTime)) &&
fabs(targ_err) > Mem->CP_KickTo_err) {
/* on step 1, we turn ball to back of us */
LogAction4(70, "smart_kick_hard_abs: hardest turning ball backwards %f %f",
turn_target, targ_err);
HKCommand = hard_kick_turnball(turn_target, TURN_CLOSEST, TRUE);//TRUE to stop ball
if (HKCommand.time != Mem->CurrentTime) {
LogAction2(70, "kick_hardest: turnball won't work, so stopping ball");
HKCommand = dokick(-180, 0, 1.0);
}
Mem->HKStepNext = 1;
}
LogAction2(210, "HardestKick: after 1 processing");
if (Mem->HKStep == 2 ||
(Mem->HKStep < 2 && HKCommand.time != Mem->CurrentTime)) {
/* on step 2, we call the moderate code */
/* or if step 0 had turnball problems, we'll go ahead and drop
to this code */
LogAction2(70, "smart_kick_hard_abs: calling kick_hard_moderate");
HKCommand = kick_hard_moderate(abs_dir, targ_vel, rot);
Mem->HKStepNext = 2;
}
LogAction2(210, "HardestKick: after 2 processing");
if (Mem->HKStep < 0 && Mem->HKStep > 2)
my_error("HKstep is not in a valid state: %d", Mem->HKStep);
Mem->HKTime = Mem->CurrentTime;
LogAction2(210, "HardestKick: after all processing");
break; //used to be: return DoTurnKickCommand(HKCommand);
}
case KM_Hard:
/* Actually, this is implemented as a "dash helping kick" at a higher level
in behave */
my_error("KM_Hard not implemented yet!");
//dump_core("blah");
break;
case KM_ModerateWTurn: {
/* This mode is *not* garunteed to be harder than moderate
It's really intended for dead ball situations */
AngleDeg target_dir = abs_dir - Mem->MyBodyAng() + rot*Mem->CP_hardest_kick_player_ang;
NormalizeAngleDeg(&target_dir);
LogAction3(70, "ModerateWTurn: norm angle %.2f", target_dir);
if (fabs(target_dir) > Mem->CP_max_hard_kick_angle_err) {
LogAction2(70, "ModerateWTurn: (0) need to turn!");
if (Mem->BallWillBeKickable()) {
HKCommand.type = CMD_turn;
HKCommand.angle = target_dir;
HKCommand.time = Mem->CurrentTime;
HKCommand.turn_neck = FALSE;
break;
} else {
LogAction2(70, "ModerateWTurn: need to turn, but ball won't be kickable, so call moderate");
}
}
HKCommand = kick_hard_moderate(abs_dir, targ_vel, rot);
} break;
case KM_Moderate:
HKCommand = kick_hard_moderate(abs_dir, targ_vel, rot);
break;
case KM_Quickly:
case KM_QuickestRelease: {
Mem->HKStep = Mem->HKStepNext = -1;
/* see if the hardest kick in the direction we want will be collision */
/* use our dokick function to correct for vel */
TurnKickCommand kickCom;
Vector vPredBall;
Bool CanKickToTraj;
if (!is_hard_kick_coll(abs_dir, &kickCom, &vPredBall, targ_vel, &CanKickToTraj)) {
/* there is no collsion! */
if (mode == KM_Quickly &&
Mem->BallWillBeKickable() &&
fabs(Mem->BallAngleFromBody()) > Mem->CP_max_hard_kick_angle_err) {
/* In KM_Quickly mode, we can turn to ball and get it next time */
LogAction2(70, "smart_kick_hard_abs: km_quickly: turning for power");
HKCommand.time = Mem->CurrentTime;
HKCommand.type = CMD_turn;
HKCommand.angle = Mem->AngleToFromBody(Mem->BallPredictedPosition());
} else
HKCommand = kickCom;
break; //used to be:return DoTurnKickCommand(HKCommand);
} else {
/* do turnball */
if (!CanKickToTraj) {
/* can't kick to trajectory, so just do the collision kick */
LogAction2(70, "smart_kick_hard_abs: km_quick: can't kick to traj, taking the collision");
HKCommand = kickCom;
} else {
LogAction2(70, "smart_kick_hard_abs: km_quick: turnball");
HKCommand = hard_kick_turnball(abs_dir, rot);
if (HKCommand.time != Mem->CurrentTime) {
LogAction2(70, "km_quick: I'm using the original kick commands, even though it's a collision");
HKCommand = kickCom;
}
}
break; //used to be:return DoTurnKickCommand(hard_kick_turnball(abs_dir, rot));
}
}
my_error("How did I get to end of KM_Quickly/KM_Quickest_Release");
break;
default:
my_error("Invalid/Unimplemented kick mode passed to smart_kick_hard");
break;
}
/* now we'll add a turn_neck into this to look at where the ball will be
If a turn_neck is already there, we won't do this */
LogAction2(210, "KickHard: checking for turn_neck");
if (!HKCommand.turn_neck) {
if (HKCommand.time != Mem->CurrentTime)
my_error("Adding a turn_neck to an invalid HKCommand");
LogAction2(70, "smart_kick_hard_abs: doing a turn_neck");
Vector pred_ball_pos;
if (HKCommand.type == CMD_kick)
pred_ball_pos = Mem->BallPredictedPosition(1, HKCommand.power, HKCommand.angle);
else
pred_ball_pos = Mem->BallPredictedPosition(1);
float pred_ball_dir = (pred_ball_pos - Mem->MyPredictedPosition()).dir();
pred_ball_dir -= Mem->MyNeckGlobalAng();
pred_ball_dir -= (HKCommand.type == CMD_turn ? HKCommand.angle : 0);
HKCommand.turn_neck = TRUE;
HKCommand.turn_neck_angle = Mem->LimitTurnNeckAngle(GetNormalizeAngleDeg(pred_ball_dir));
}
LogAction2(210, "KickHard: returning");
return DoTurnKickCommand(HKCommand);
}
/* we always reason about the right trajectory for the ball leave velocity
correction for dokick */
KickToRes turnball_kick(AngleDeg target_dir, TurnDir rotate,
Bool StopBall, TurnKickCommand* pCom,
float EndDist, float closeMarg, float kickFac)
{
float dir;
float dist;
Vector btraj;
pCom->time = -1;
pCom->turn_neck = FALSE;
DebugKick(printf("\nat turnball_kick: target_dir: %f\n", target_dir));
LogAction4(60, "Turnball_kick: targ_dir: %.1f dir: %d", target_dir, (int)rotate);
NormalizeAngleDeg(&target_dir);
//DebugKick(printf("HERE Time: %d\n", Mem->CurrentTime.t));
/* the pos valid is not really right - if we are turning the ball and didn't
actually see it last time, then there's a problem */
if ( !Mem->BallPositionValid() || !Mem->BallKickable() ) {
LogAction2(90, "turnball_kick: lost the ball");
return KT_LostBall;
}
/* if the velocity isn's valid, turn to face ball */
if ( !Mem->BallVelocityValid() ) {
float ball_ang_from_body = Mem->BallAngleFromBody(); /* for efficiency */
LogAction2(90, "turnball_kick: vel is not valid, looking at it");
DebugKick(printf("turning to face ball\n"));
if (Mem->CanSeeBallWithNeck()) {
pCom->time = Mem->CurrentTime;
pCom->type = CMD_kick;
pCom->angle = ball_ang_from_body + 180;
pCom->power = Mem->CP_stop_ball_power;
pCom->turn_neck = TRUE;
pCom->turn_neck_angle = Mem->LimitTurnNeckAngle(Mem->BallAngleFromNeck());
} else {
/* turn body to face ball, and turn neck to straight ahead */
pCom->time = Mem->CurrentTime;
pCom->type = CMD_turn;
pCom->turn_neck = TRUE;
if (fabs(ball_ang_from_body) > Mem->MaxEffectiveTurn()) {
/* out body can't get to where we want to go */
pCom->angle = 180; /* get our maximum effective turn */
pCom->turn_neck_angle = ball_ang_from_body -
signf(ball_ang_from_body)*Mem->MaxEffectiveTurn();
} else {
pCom->angle = ball_ang_from_body;
pCom->turn_neck_angle = -Mem->MyNeckRelAng();
}
}
return KT_TurnedToBall;
}
DebugKick(printf(" ball.dist: %f\t.dir: %f\n",
Mem->BallDistance(), Mem->BallAngle()));
DebugKick(printf(" HERE ball.vel.x: %f\t.y: %f\tmod: %f\n",
Mem->BallRelativeVelocity().x, Mem->BallRelativeVelocity().y,
Mem->BallSpeed()));
DebugKick(printf(" ball.rpos.x: %f\t.y: %f\n",
Mem->BallRelativePosition().x, Mem->BallRelativePosition().y));
DebugKick(printf(" target_dir: %f\n", target_dir));
if ( fabs(GetNormalizeAngleDeg(target_dir - Mem->BallAngleFromBody())) < Mem->CP_KickTo_err) {
/* Do something to indicate we are done */
if (!StopBall || Mem->BallSpeed() < Mem->CP_max_ignore_vel)
return KT_DidNothing;
LogAction2(90, "turnball_kick: we're there, stopping the ball");
DebugKick(printf(" Stop ball kick\n"));
dir = 0;
dist = 0;
PlayerMovementCorrection(&dir, &dist);
*pCom = dokick(dir, dist, 1.0);
pCom->turn_neck = FALSE;
return KT_Success;
}
if (rotate == TURN_AVOID) {
rotate = RotToAvoidOpponent(target_dir + Mem->MyBodyAng());
}
if (rotate == TURN_CLOSEST) {
rotate = RotClosest(target_dir + Mem->MyBodyAng());
}
if (is_straight_kick(target_dir, EndDist, closeMarg)) {
float pow;
btraj = Polar2Vector(EndDist, target_dir) - Mem->BallRelativeToBodyPosition();
dir = btraj.dir();
dist = btraj.mod();
/* now we're goign to do some distance twiddling to get the ball to
get to the right angle and stop */
pow = dist / Mem->BallKickRate();
pow = Min(pow, Mem->CP_max_turn_kick_pow);
dist = pow * Mem->BallKickRate();
LogAction4(90, "turnball_kick: striaght kick: dist %f at %f", dist, dir);
DebugKick(printf(" Straight kick# dir: %f dist: %f\n", dir, dist));
PlayerMovementCorrection(&dir, &dist);
*pCom = dokick(dir, dist, 1.0);
pCom->turn_neck = FALSE;
} else if (Mem->BallDistance() < closeMarg) {
/* ball is too close to do a tangent kick, so do a kick at 90 degrees */
dir = ((int)rotate)*(-90) + Mem->BallAngleFromBody();
dist = 2.0*sqrt(Sqr(Mem->CP_opt_ctrl_dist) - Sqr(Mem->BallDistance()));
LogAction2(90, "turnball_kick: 90 deg kick");
DebugKick(printf(" Close kick# dir: %f dist: %f\n", dir, dist));
PlayerMovementCorrection(&dir, &dist);
*pCom = dokick(dir, dist, kickFac);
pCom->turn_neck = FALSE;
} else {
/* do a turning kick */
/* we make a circle around the player of radius closeMarg
and calculate the trajectory that goes in the right direction and is
tangent to the circle */
dir = 180 + Mem->BallAngleFromBody() + ((int)rotate)*ASin(closeMarg / Mem->BallDistance());
DebugKick(printf(" ball dist: %f\tclosest_margin: %f\n",
Mem->BallDistance(), closeMarg));
dist = sqrt(Sqr(Mem->BallDistance()) - Sqr(closeMarg));
dist +=
sqrt(Sqr(Mem->CP_opt_ctrl_dist) - Sqr(closeMarg));
DebugKick(printf(" Turning ball# dir: %f dist: %f\n", dir, dist));
LogAction2(90, "turnball_kick: turning kick");
PlayerMovementCorrection(&dir, &dist);
*pCom = dokick(dir, dist, kickFac);
pCom->turn_neck = FALSE;
}
return KT_DidKick;
}
/* returns KickCommand.time = -1 for error */
TurnKickCommand dokick(AngleDeg ddir, float ddist, float distFactor,
Bool* pCanKickToTraj)
{
float v0;
float power;
float kick_dir = ddir;
TurnKickCommand com;
com.time = -1;
com.type = CMD_kick;
com.turn_neck = FALSE;
if (pCanKickToTraj)
*pCanKickToTraj = TRUE;
LogAction6(110, "dokick: %.2f (%.2f) at %.2f, rate %.5f",
ddist, distFactor, ddir, Mem->BallKickRate());
v0 = ddist * distFactor;
NormalizeAngleDeg(&ddir);
DebugKick(printf(" dokick: ddir: %f\tv0: %f\n", ddir, v0));
DebugKick(printf(" kickrate: %f\tmyang: %f\n", Mem->BallKickRate(), Mem->MyAng() ));
if (Mem->BallKickRate() == 0.0)
my_error("dokick: Huh? BallKickRate is 0!");
if (!Mem->BallVelocityValid()) {
DebugKick(printf("In dokick with velocity not valid. Assuming it's 0."));
LogAction2(130, "dokick: assuming vel 0");
}
if (Mem->BallVelocityValid() &&
Mem->BallAbsoluteVelocity() != 0) { /* correct for ball velocity */
Vector tmp = Polar2Vector(v0, ddir) - Mem->BallRelativeToBodyVelocity();
kick_dir = tmp.dir();
power = tmp.mod() / Mem->BallKickRate();
DebugKick(printf(" Correcting for ball velocity# vel.x: %f\tvel.y:%f\n",
Mem->BallRelativeVelocity().x, Mem->BallRelativeVelocity().y));
DebugKick(printf(" New stuff# power: %f\t ddir:%f\n", power, kick_dir));
LogAction4(130, "dokick: vel corr: %f at %f", power, kick_dir);
} else {
LogAction2(130, "dokick: vel is 0");
power = v0 / Mem->BallKickRate();
}
if (power > Mem->SP_max_power) {
DebugKick(printf("Trying to kick over SP_max_power! Correcting...\n"));
//LogAction2(130, "dokick: trying to kick too hard, correcting");
if (!Mem->BallVelocityValid() || Mem->BallAbsoluteVelocity() == 0) {
power = Mem->SP_max_power;
LogAction4(130, "dokick: max_pow corr: (stopped ball) %f at %f",
power, kick_dir);
} else if (ddist == 0) {
/* this is probably a stop_ball kick, but ddir is meaningless for
the RayCircleIntersection below.
Therefore, we just kick against the ball's velocity */
kick_dir = GetNormalizeAngleDeg(Mem->BallRelativeToBodyHeading() + 180);
power = Mem->SP_max_power;
LogAction4(130, "dokick: max_pow corr: (ddist = 0) %f at %f",
power, kick_dir);
} else {
/* this is a ray circle intersection problem
We want to kick in the right direction, but not as hard as desired */
Vector sol1, sol2;
int numSol;
Vector vNewTraj;
numSol =
RayCircleIntersect(Ray(Mem->BallRelativeToBodyPosition(), ddir),
Mem->SP_max_power * Mem->BallKickRate(),
Mem->Global2RelativeToMyBody(Mem->BallPredictedPosition()),
&sol1, &sol2);
/* we want the solution that's furthest along the ray - that's sol2
if there are two solution */
if (numSol == 0) {
/* we can't kick ball to desired trajectory, so let's just slow it
down by kicking directly against velocity
It might be better to return an error and have the caller decide
what to do, but this works pretty well */
DebugKick(printf("Can't kick ball to right trajectory!\n"));
power = Mem->SP_max_power;
kick_dir = Mem->BallRelativeToBodyHeading() + 180;
if (pCanKickToTraj)
*pCanKickToTraj = FALSE;
LogAction4(130, "dokick: max_pow corr: can't get to trajectory! %.2f at %.2f",
power, kick_dir);
} else {
if (numSol == 1) {
vNewTraj = sol1;
} else if (numSol == 2) {
/* we want the solution closer to the target point we wanted */
Vector targ = Mem->BallRelativeToBodyPosition() + Polar2Vector(v0, ddir);
LogAction8(130, "dokick: 2 solutions (%.2f, %.2f) (%.2f, %.2f) targ (%.2f, %.2f)",
sol1.x, sol1.y, sol2.x, sol2.y, targ.x, targ.y);
if (sol1.dist2(targ) < sol2.dist2(targ)) {
LogAction4(140, "Picked sol1: %.2f < %.2f",
sol1.dist2(targ), sol2.dist2(targ));
vNewTraj = sol1;
} else {
LogAction4(140, "Picked sol2: %.2f > %.2f",
sol1.dist2(targ), sol2.dist2(targ));
vNewTraj = sol2;
}
} else {
my_error("dokick: How many solutions to RayCircleIntersection? %d", numSol);
}
Vector vNewKick = vNewTraj -
Mem->Global2RelativeToMyBody(Mem->BallPredictedPosition());
power = vNewKick.mod() / Mem->BallKickRate();
kick_dir = vNewKick.dir();
DebugKick(printf(" Correcting for ball velocity AND max power# vel.x: %f\tvel.y:%f\n",
Mem->BallRelativeVelocity().x, Mem->BallRelativeVelocity().y));
DebugKick(printf("New stuff# power: %f\t dir:%f\n", power, kick_dir));
LogAction4(130, "dokick: max_pow corr: %f at %f", power, kick_dir);
}
}
}
power = Min(Round(power, -2), Mem->SP_max_power);
kick_dir = Round(kick_dir, -2);
NormalizeAngleDeg(&kick_dir);
DebugKick(printf("kicking with power: %f at dir: %f\n", power, kick_dir));
com.time = Mem->CurrentTime;
com.power = power;
com.angle = kick_dir;
return com;
}
/**
* 通过传入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);
}
