/*!
*/
bool
Bhv_Hunt::execute( PlayerAgent * agent )
{
std::list<Vector2D> forces;
//Get force from prey
Vector2D preyForce = agent->world().ball().rpos(); //Vector to prey
int preyCo = 1; // for future usee
forces.push_front(preyForce * preyCo/preyForce.r2()); //1/r^2 relationship
// Get forces from team mates
// PlayerCont * team = agent->world().teammates();
// for ( PlayerCont::const_iterator tm = team.begin(); tm != team.end(); ++tm ){
// }
//Sum up all forces to create resultant vector
Vector2D * resultant = new Vector2D();
for ( std::list<Vector2D>::iterator it = forces.begin(); it != forces.end(); ++it ){
*resultant = *resultant + *it;
}
if ( Body_GoToPoint (*resultant, 5, resultant->r()).execute( agent ) ){
agent->setNeckAction( new Neck_TurnToBall());
return true;
}
return false;
}
/*!
*/
Vector2D
Segment2D::projection( const Vector2D & p ) const
{
Vector2D dir = terminal() - origin();
double len = dir.r();
if ( len < EPSILON )
{
return origin();
}
dir /= len; // normalize
double d = dir.innerProduct( p - origin() );
if ( -EPSILON < d && d < len + EPSILON )
{
dir *= d;
return Vector2D( origin() ) += dir;
}
return Vector2D::INVALIDATED;
#if 0
Line2D my_line = this->line();
Vector2D sol = my_line.projection( p );
if ( ! sol.isValid()
|| ! this->contains( sol ) )
{
return Vector2D::INVALIDATED;
}
return sol;
#endif
}
/*!
*/
int
TackleGenerator::predictOpponentsReachStep( const WorldModel & wm,
const Vector2D & first_ball_pos,
const Vector2D & first_ball_vel,
const AngleDeg & ball_move_angle )
{
int first_min_step = 50;
#if 1
const ServerParam & SP = ServerParam::i();
const Vector2D ball_end_point = inertia_final_point( first_ball_pos,
first_ball_vel,
SP.ballDecay() );
if ( ball_end_point.absX() > SP.pitchHalfLength()
|| ball_end_point.absY() > SP.pitchHalfWidth() )
{
Rect2D pitch = Rect2D::from_center( 0.0, 0.0, SP.pitchLength(), SP.pitchWidth() );
Ray2D ball_ray( first_ball_pos, ball_move_angle );
Vector2D sol1, sol2;
int n_sol = pitch.intersection( ball_ray, &sol1, &sol2 );
if ( n_sol == 1 )
{
first_min_step = SP.ballMoveStep( first_ball_vel.r(), first_ball_pos.dist( sol1 ) );
#ifdef DEBUG_PRINT
dlog.addText( Logger::CLEAR,
"(predictOpponent) ball will be out. step=%d reach_point=(%.2f %.2f)",
first_min_step,
sol1.x, sol1.y );
#endif
}
}
#endif
int min_step = first_min_step;
for ( AbstractPlayerCont::const_iterator
o = wm.theirPlayers().begin(),
end = wm.theirPlayers().end();
o != end;
++o )
{
int step = predictOpponentReachStep( *o,
first_ball_pos,
first_ball_vel,
ball_move_angle,
min_step );
if ( step < min_step )
{
min_step = step;
}
}
return ( min_step == first_min_step
? 1000
: min_step );
}
/*!
*/
bool
Bhv_SetPlayGoalKick::doKickToFarSide( PlayerAgent * agent )
{
const WorldModel & wm = agent->world();
Vector2D target_point( ServerParam::i().ourPenaltyAreaLineX() - 5.0,
ServerParam::i().penaltyAreaHalfWidth() );
if ( wm.ball().pos().y > 0.0 )
{
target_point.y *= -1.0;
}
double ball_move_dist = wm.ball().pos().dist( target_point );
double ball_first_speed = calc_first_term_geom_series_last( 0.7,
ball_move_dist,
ServerParam::i().ballDecay() );
ball_first_speed = std::min( ServerParam::i().ballSpeedMax(),
ball_first_speed );
ball_first_speed = std::min( wm.self().kickRate() * ServerParam::i().maxPower(),
ball_first_speed );
Vector2D accel = target_point - wm.ball().pos();
accel.setLength( ball_first_speed );
double kick_power = std::min( ServerParam::i().maxPower(),
accel.r() / wm.self().kickRate() );
AngleDeg kick_angle = accel.th();
dlog.addText( Logger::TEAM,
__FILE__" (doKickToFarSide) target=(%.2f %.2f) dist=%.3f ball_speed=%.3f",
target_point.x, target_point.y,
ball_move_dist,
ball_first_speed );
dlog.addText( Logger::TEAM,
__FILE__" (doKickToFarSide) kick_power=%f kick_angle=%.1f",
kick_power,
kick_angle.degree() );
agent->doKick( kick_power, kick_angle - wm.self().body() );
agent->setNeckAction( new Neck_ScanField() );
return true;
}
/*!
*/
void
ShootGenerator::createShoot( const WorldModel & wm,
const Vector2D & target_point )
{
const AngleDeg ball_move_angle = ( target_point - M_first_ball_pos ).th();
const PlayerObject * goalie = wm.getOpponentGoalie();
if ( goalie
&& 5 < goalie->posCount()
&& goalie->posCount() < 30
&& wm.dirCount( ball_move_angle ) > 3 )
{
#ifdef DEBUG_PRINT
dlog.addText( Logger::SHOOT,
"%d: __ xxx goalie_count=%d, low dir accuracy",
M_total_count,
goalie->posCount() );
#endif
return;
}
const ServerParam & SP = ServerParam::i();
const double ball_speed_max = ( wm.gameMode().type() == GameMode::PlayOn
|| wm.gameMode().isPenaltyKickMode()
? SP.ballSpeedMax()
: wm.self().kickRate() * SP.maxPower() );
const double ball_move_dist = M_first_ball_pos.dist( target_point );
const Vector2D max_one_step_vel
= ( wm.self().isKickable()
? KickTable::calc_max_velocity( ball_move_angle,
wm.self().kickRate(),
wm.ball().vel() )
: ( target_point - M_first_ball_pos ).setLengthVector( 0.1 ) );
const double max_one_step_speed = max_one_step_vel.r();
double first_ball_speed
= std::max( ( ball_move_dist + 5.0 ) * ( 1.0 - SP.ballDecay() ),
std::max( max_one_step_speed,
1.5 ) );
bool over_max = false;
#ifdef DEBUG_PRINT_FAILED_COURSE
bool success = false;
#endif
while ( ! over_max )
{
if ( first_ball_speed > ball_speed_max - 0.001 )
{
over_max = true;
first_ball_speed = ball_speed_max;
}
if ( createShoot( wm,
target_point,
first_ball_speed,
ball_move_angle,
ball_move_dist ) )
{
Course & course = M_courses.back();
if ( first_ball_speed <= max_one_step_speed + 0.001 )
{
course.kick_step_ = 1;
}
#ifdef DEBUG_PRINT_SUCCESS_COURSE
dlog.addText( Logger::SHOOT,
"%d: ok shoot target=(%.2f %.2f)"
" speed=%.3f angle=%.1f",
M_total_count,
target_point.x, target_point.y,
first_ball_speed,
ball_move_angle.degree() );
dlog.addRect( Logger::SHOOT,
target_point.x - 0.1, target_point.y - 0.1,
0.2, 0.2,
"#00ff00" );
char num[8];
snprintf( num, 8, "%d", M_total_count );
dlog.addMessage( Logger::SHOOT,
target_point, num, "#ffffff" );
#endif
#ifdef DEBUG_PRINT_FAILED_COURSE
success = true;
#endif
#ifdef SEARCH_UNTIL_MAX_SPEED_AT_SAME_POINT
if ( course.goalie_never_reach_
&& course.opponent_never_reach_ )
{
return;
}
++M_total_count;
#else
return;
#endif
}
first_ball_speed += 0.3;
}
#ifdef DEBUG_PRINT_FAILED_COURSE
if ( success )
{
return;
}
dlog.addText( Logger::SHOOT,
"%d: xxx shoot target=(%.2f %.2f)"
" speed=%.3f angle=%.1f",
M_total_count,
target_point.x, target_point.y,
first_ball_speed,
ball_move_angle.degree() );
dlog.addRect( Logger::SHOOT,
target_point.x - 0.1, target_point.y - 0.1,
0.2, 0.2,
"#ff0000" );
char num[8];
snprintf( num, 8, "%d", M_total_count );
dlog.addMessage( Logger::SHOOT,
target_point, num, "#ffffff" );
#endif
}
void
FullstateSensor::printWithWorld( const WorldModel & world ) const
{
Vector2D tmpv;
double tmpval;
dlog.addText( Logger::WORLD,
"FS ball (%+.3f %+.3f) (%+.3f %+.3f) %.3f",
ball().pos_.x, ball().pos_.y,
ball().vel_.x, ball().vel_.y,
ball().vel_.r() );
dlog.addText( Logger::WORLD,
"____internal (%+.3f %+.3f) (%+.3f %+.3f) %.3f gconf=%d rconf=%d",
world.ball().pos().x, world.ball().pos().y,
world.ball().vel().x, world.ball().vel().y,
world.ball().vel().r(),
world.ball().posCount(), world.ball().rposCount() );
tmpv = ball().pos_ - world.ball().pos();
dlog.addText( Logger::WORLD,
"__ball pos err (%+.3f %+.3f) %.3f",
tmpv.x, tmpv.y, tmpv.r() );
dlog.addText( Logger::WORLD,
"____internal (%+.3f %+.3f)",
world.ball().posError().x, world.ball().posError().y );
tmpv = ball().vel_ - world.ball().vel();
tmpval = tmpv.r();
dlog.addText( Logger::WORLD,
"__ball vel err (%+.3f %+.3f) %.3f %s",
tmpv.x, tmpv.y, tmpval, (tmpval > 1.0 ? "big error" : "" ) );
dlog.addText( Logger::WORLD,
"____internal (%+.3f %+.3f)",
world.ball().velError().x, world.ball().velError().y );
const FullstateSensor::PlayerCont& player_cont
= ( world.isOurLeft()
? leftTeam()
: rightTeam() );
for ( FullstateSensor::PlayerCont::const_iterator it = player_cont.begin();
it != player_cont.end();
++it )
{
if ( it->unum_ == world.self().unum() )
{
dlog.addText( Logger::WORLD,
"FS self (%+.3f %+.3f) (%+.3f %+.3f) b=%+.2f n=%+.2f f=%+.2f",
it->pos_.x, it->pos_.y,
it->vel_.x, it->vel_.y,
it->body_, it->neck_,
AngleDeg::normalize_angle( it->body_ + it->neck_ ) );
dlog.addText( Logger::WORLD,
"____internal (%+.3f %+.3f) (%+.3f %+.3f) b=%+.2f n=%+.2f f=%+.2f",
world.self().pos().x, world.self().pos().y,
world.self().vel().x, world.self().vel().y,
world.self().body().degree(),
world.self().neck().degree(),
world.self().face().degree() );
tmpv = it->pos_ - world.self().pos();
double d = tmpv.r();
dlog.addText( Logger::WORLD,
"__self pos err (%+.3f %+.3f) %.3f %s",
tmpv.x, tmpv.y, d, ( d > 0.3 ? " big error" : "" ) );
dlog.addText( Logger::WORLD,
"____internal (%+.3f %+.3f) %.3f",
world.self().posError().x,
world.self().posError().y,
world.self().posError().r() );
tmpv = it->vel_ - world.self().vel();
dlog.addText( Logger::WORLD,
"__self vel err (%+.3f %+.3f) %.3f",
tmpv.x, tmpv.y, tmpv.r() );
dlog.addText( Logger::WORLD,
"____internal (%+.3f %+.3f) %.3f",
world.self().velError().x,
world.self().velError().y,
world.self().velError().r() );
tmpv = ball().pos_ - it->pos_;
dlog.addText( Logger::WORLD,
"__ball rpos (%+.3f %+.3f) %.3f",
tmpv.x, tmpv.y, tmpv.r() );
dlog.addText( Logger::WORLD,
"____internal (%+.3f %+.3f) %.3f",
world.ball().rpos().x,
world.ball().rpos().y,
world.ball().rpos().r() );
tmpv -= world.ball().rpos();
dlog.addText( Logger::WORLD,
"__ball rpos err (%+.3f %+.3f) %.3f",
tmpv.x, tmpv.y, tmpv.r() );
dlog.addText( Logger::WORLD,
"____internal (%+.3f %+.3f) %.3f",
world.ball().rposError().x,
world.ball().rposError().y,
world.ball().rposError().r() );
break;
}
}
}
/*!
*/
void
Bhv_GoalieChaseBall::doGoToCatchPoint( PlayerAgent * agent,
const Vector2D & target_point )
{
const ServerParam & SP = ServerParam::i();
const WorldModel & wm = agent->world();
double dash_power = 0.0;
Vector2D rel = target_point - wm.self().pos();
rel.rotate( - wm.self().body() );
AngleDeg rel_angle = rel.th();
const double angle_buf
= std::fabs( AngleDeg::atan2_deg( SP.catchableArea() * 0.9, rel.r() ) );
dlog.addText( Logger::TEAM,
__FILE__": GoToCatchPoint. (%.1f, %.1f). angle_diff=%.1f. angle_buf=%.1f",
target_point.x, target_point.y,
rel_angle.degree(), angle_buf );
agent->debugClient().setTarget( target_point );
// forward dash
if ( rel_angle.abs() < angle_buf )
{
dash_power = std::min( wm.self().stamina() + wm.self().playerType().extraStamina(),
SP.maxDashPower() );
dlog.addText( Logger::TEAM,
__FILE__": forward dash" );
agent->debugClient().addMessage( "GoToCatch:Forward" );
agent->doDash( dash_power );
}
// back dash
else if ( rel_angle.abs() > 180.0 - angle_buf )
{
dash_power = SP.minDashPower();
double required_stamina = ( SP.minDashPower() < 0.0
? SP.minDashPower() * -2.0
: SP.minDashPower() );
if ( wm.self().stamina() + wm.self().playerType().extraStamina()
< required_stamina )
{
dash_power = wm.self().stamina() + wm.self().playerType().extraStamina();
if ( SP.minDashPower() < 0.0 )
{
dash_power *= -0.5;
if ( dash_power < SP.minDashPower() )
{
dash_power = SP.minDashPower();
}
}
}
dlog.addText( Logger::TEAM,
__FILE__": back dash. power=%.1f",
dash_power );
agent->debugClient().addMessage( "GoToCatch:Back" );
agent->doDash( dash_power );
}
// forward dash turn
else if ( rel_angle.abs() < 90.0 )
{
dlog.addText( Logger::TEAM,
__FILE__": turn %.1f for forward dash",
rel_angle.degree() );
agent->debugClient().addMessage( "GoToCatch:F-Turn" );
agent->doTurn( rel_angle );
}
else
{
rel_angle -= 180.0;
dlog.addText( Logger::TEAM,
__FILE__": turn %.1f for back dash",
rel_angle.degree() );
agent->debugClient().addMessage( "GoToCatch:B-Turn" );
agent->doTurn( rel_angle );
}
agent->setNeckAction( new Neck_TurnToBall() );
}
/*!
*/
void
TackleGenerator::calculate( const WorldModel & wm )
{
const ServerParam & SP = ServerParam::i();
const double min_angle = SP.minMoment();
const double max_angle = SP.maxMoment();
const double angle_step = std::fabs( max_angle - min_angle ) / ANGLE_DIVS;
#ifdef ASSUME_OPPONENT_KICK
const Vector2D goal_pos = SP.ourTeamGoalPos();
const bool shootable_area = ( wm.ball().pos().dist2( goal_pos ) < std::pow( 18.0, 2 ) );
const Vector2D shoot_accel = ( goal_pos - wm.ball().pos() ).setLengthVector( 2.0 );
#endif
const AngleDeg ball_rel_angle = wm.ball().angleFromSelf() - wm.self().body();
const double tackle_rate
= SP.tacklePowerRate()
* ( 1.0 - 0.5 * ball_rel_angle.abs() / 180.0 );
#ifdef DEBUG_PRINT
dlog.addText( Logger::CLEAR,
__FILE__": min_angle=%.1f max_angle=%.1f angle_step=%.1f",
min_angle, max_angle, angle_step );
dlog.addText( Logger::CLEAR,
__FILE__": ball_rel_angle=%.1f tackle_rate=%.1f",
ball_rel_angle.degree(), tackle_rate );
#endif
for ( int a = 0; a < ANGLE_DIVS; ++a )
{
const AngleDeg dir = min_angle + angle_step * a;
double eff_power= ( SP.maxBackTacklePower()
+ ( SP.maxTacklePower() - SP.maxBackTacklePower() )
* ( 1.0 - ( dir.abs() / 180.0 ) ) );
eff_power *= tackle_rate;
AngleDeg angle = wm.self().body() + dir;
Vector2D accel = Vector2D::from_polar( eff_power, angle );
#ifdef ASSUME_OPPONENT_KICK
if ( shootable_area
&& wm.existKickableOpponent() )
{
accel += shoot_accel;
double d = accel.r();
if ( d > SP.ballAccelMax() )
{
accel *= ( SP.ballAccelMax() / d );
}
}
#endif
Vector2D vel = wm.ball().vel() + accel;
double speed = vel.r();
if ( speed > SP.ballSpeedMax() )
{
vel *= ( SP.ballSpeedMax() / speed );
}
M_candidates.push_back( TackleResult( angle, vel ) );
#ifdef DEBUG_PRINT
const TackleResult & result = M_candidates.back();
dlog.addText( Logger::CLEAR,
"%d: angle=%.1f(dir=%.1f), result: vel(%.2f %.2f ) speed=%.2f move_angle=%.1f",
a,
result.tackle_angle_.degree(), dir.degree(),
result.ball_vel_.x, result.ball_vel_.y,
result.ball_speed_, result.ball_move_angle_.degree() );
#endif
}
M_best_result.clear();
const Container::iterator end = M_candidates.end();
for ( Container::iterator it = M_candidates.begin();
it != end;
++it )
{
it->score_ = evaluate( wm, *it );
#ifdef DEBUG_PRINT
Vector2D ball_end_point = inertia_final_point( wm.ball().pos(),
it->ball_vel_,
SP.ballDecay() );
dlog.addLine( Logger::CLEAR,
wm.ball().pos(),
ball_end_point,
"#0000ff" );
char buf[16];
snprintf( buf, 16, "%.3f", it->score_ );
dlog.addMessage( Logger::CLEAR,
ball_end_point, buf, "#ffffff" );
#endif
if ( it->score_ > M_best_result.score_ )
{
#ifdef DEBUG_PRINT
dlog.addText( Logger::CLEAR,
">>>> updated" );
#endif
M_best_result = *it;
}
}
#ifdef DEBUG_PRINT
dlog.addLine( Logger::CLEAR,
wm.ball().pos(),
inertia_final_point( wm.ball().pos(),
M_best_result.ball_vel_,
SP.ballDecay() ),
"#ff0000" );
dlog.addText( Logger::CLEAR,
"==== best_angle=%.1f score=%f speed=%.3f move_angle=%.1f",
M_best_result.tackle_angle_.degree(),
M_best_result.score_,
M_best_result.ball_speed_,
M_best_result.ball_move_angle_.degree() );
#endif
}
