/*!
*/
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
}
/*!
*/
bool
Segment2D::onSegmentWeakly( const Vector2D & p ) const
{
Vector2D proj = projection( p );
return ( proj.isValid()
&& p.equalsWeakly( proj ) );
#if 0
Vector2D o = origin();
Vector2D t = terminal();
const double buf = ( allow_on_terminal
? EPSILON
: 0.0 );
if ( std::fabs( ( t - o ).outerProduct( p - o ) ) < EPSILON )
{
if ( std::fabs( o.x - t.x ) < EPSILON )
{
return ( ( o.y - buf < p.y && p.y < t.y + buf )
|| ( t.y - buf < p.y && p.y < o.y + buf ) );
}
else
{
return ( ( o.x - buf < p.x && p.x < t.x + buf )
|| ( t.x - buf < p.x && p.x < o.x + buf) );
}
}
return false;
#endif
}
/*!
*/
Vector2D
Segment2D::intersection( const Segment2D & other,
const bool allow_end_point ) const
{
Vector2D sol = this->line().intersection( other.line() );
if ( ! sol.isValid()
|| ! this->contains( sol )
|| ! other.contains( sol ) )
{
return Vector2D::INVALIDATED;
}
if ( ! allow_end_point
&& ! existIntersectionExceptEndpoint( other ) )
{
return Vector2D::INVALIDATED;
}
return sol;
#if 0
// Following algorithm seems faster ther above.
// In fact, the following algorithm slower...
Vector2D ab = terminal() - origin();
Vector2D dc = other.origin() - other.terminal();
Vector2D ad = other.terminal() - origin();
double det = dc.outerProduct( ab );
if ( std::fabs( det ) < 1.0e-9 )
{
// area size is 0.
// segments has same slope.
std::cerr << "Segment2D::intersection()"
<< " ***ERROR*** parallel segments"
<< std::endl;
return Vector2D::INVALIDATED;
}
double s = ( dc.x * ad.y - dc.y * ad.x ) / det;
double t = ( ab.x * ad.y - ab.y * ad.x ) / det;
if ( s < 0.0 || 1.0 < s || t < 0.0 || 1.0 < t )
{
return Vector2D::INVALIDATED;
}
return Vector2D( origin().x + ab.x * s, origin().y + ab.y * s );
#endif
}
/*!
*/
Vector2D
Segment2D::intersection( const Line2D & l ) const
{
Line2D my_line = this->line();
Vector2D sol = my_line.intersection( l );
if ( ! sol.isValid()
|| ! this->contains( sol ) )
{
return Vector2D::INVALIDATED;
}
return sol;
}
/*!
*/
bool
Bhv_PenaltyKick::doGoalieSlideChase( PlayerAgent* agent )
{
const WorldModel & wm = agent->world();
if ( std::fabs( 90.0 - wm.self().body().abs() ) > 2.0 )
{
Vector2D face_point( wm.self().pos().x, 100.0);
if ( wm.self().body().degree() < 0.0 )
{
face_point.y = -100.0;
}
Body_TurnToPoint( face_point ).execute( agent );
agent->setNeckAction( new Neck_TurnToBall() );
return true;
}
Ray2D ball_ray( wm.ball().pos(), wm.ball().vel().th() );
Line2D ball_line( ball_ray.origin(), ball_ray.dir() );
Line2D my_line( wm.self().pos(), wm.self().body() );
Vector2D intersection = my_line.intersection( ball_line );
if ( ! intersection.isValid()
|| ! ball_ray.inRightDir( intersection ) )
{
Body_Intercept( false ).execute( agent ); // goalie mode
agent->setNeckAction( new Neck_TurnToBall() );
return true;
}
if ( wm.self().pos().dist( intersection )
< ServerParam::i().catchAreaLength() * 0.7 )
{
Body_StopDash( false ).execute( agent ); // not save recovery
agent->setNeckAction( new Neck_TurnToBall() );
return true;
}
AngleDeg angle = ( intersection - wm.self().pos() ).th();
double dash_power = ServerParam::i().maxDashPower();
if ( ( angle - wm.self().body() ).abs() > 90.0 )
{
dash_power = ServerParam::i().minDashPower();
}
agent->doDash( dash_power );
agent->setNeckAction( new Neck_TurnToBall() );
return true;
}
/*!
*/
Circle2D
Circle2D::circumcircle( const Vector2D & p0,
const Vector2D & p1,
const Vector2D & p2 )
{
Vector2D center = Triangle2D::circumcenter( p0, p1, p2 );
if ( ! center.isValid() )
{
std::cerr << "Circle2D::circumcircle()"
<< " ***ERROR*** failed to get circumcenter from "
<< p0 << p1 << p2
<< std::endl;
return Circle2D();
}
return Circle2D( center, center.dist( p0 ) );
}
/*!
ball_pos & my_pos is set to self localization oriented.
if ( onfiled_side != our_side ), these coordinates must be reversed.
*/
Vector2D
Bhv_PenaltyKick::getGoalieMovePos( const Vector2D & ball_pos,
const Vector2D & my_pos )
{
const ServerParam & SP = ServerParam::i();
const double min_x = -SP.pitchHalfLength() + SP.catchAreaLength()*0.9;
if ( ball_pos.x < -49.0 )
{
if ( ball_pos.absY() < SP.goalHalfWidth() )
{
return Vector2D( min_x, ball_pos.y );
}
else
{
return Vector2D( min_x,
sign( ball_pos.y ) * SP.goalHalfWidth() );
}
}
Vector2D goal_l( -SP.pitchHalfLength(), -SP.goalHalfWidth() );
Vector2D goal_r( -SP.pitchHalfLength(), +SP.goalHalfWidth() );
AngleDeg ball2post_angle_l = ( goal_l - ball_pos ).th();
AngleDeg ball2post_angle_r = ( goal_r - ball_pos ).th();
// NOTE: post_angle_r < post_angle_l
AngleDeg line_dir = AngleDeg::bisect( ball2post_angle_r,
ball2post_angle_l );
Line2D line_mid( ball_pos, line_dir );
Line2D goal_line( goal_l, goal_r );
Vector2D intersection = goal_line.intersection( line_mid );
if ( intersection.isValid() )
{
Line2D line_l( ball_pos, goal_l );
Line2D line_r( ball_pos, goal_r );
AngleDeg alpha = AngleDeg::atan2_deg( SP.goalHalfWidth(),
SP.penaltyAreaLength() - 2.5 );
double dist_from_goal
= ( ( line_l.dist( intersection ) + line_r.dist( intersection ) ) * 0.5 )
/ alpha.sin();
dlog.addText( Logger::TEAM,
__FILE__": goalie move. intersection=(%.1f, %.1f) dist_from_goal=%.1f",
intersection.x, intersection.y, dist_from_goal );
if ( dist_from_goal <= SP.goalHalfWidth() )
{
dist_from_goal = SP.goalHalfWidth();
dlog.addText( Logger::TEAM,
__FILE__": goalie move. outer of goal. dist_from_goal=%.1f",
dist_from_goal );
}
if ( ( ball_pos - intersection ).r() + 1.5 < dist_from_goal )
{
dist_from_goal = ( ball_pos - intersection ).r() + 1.5;
dlog.addText( Logger::TEAM,
__FILE__": goalie move. near than ball. dist_from_goal=%.1f",
dist_from_goal );
}
AngleDeg position_error = line_dir - ( intersection - my_pos ).th();
const double danger_angle = 21.0;
dlog.addText( Logger::TEAM,
__FILE__": goalie move position_error_angle=%.1f",
position_error.degree() );
if ( position_error.abs() > danger_angle )
{
dist_from_goal *= ( ( 1.0 - ((position_error.abs() - danger_angle)
/ (180.0 - danger_angle)) )
* 0.5 );
dlog.addText( Logger::TEAM,
__FILE__": goalie move. error is big. dist_from_goal=%.1f",
dist_from_goal );
}
Vector2D result = intersection;
Vector2D add_vec = ball_pos - intersection;
add_vec.setLength( dist_from_goal );
dlog.addText( Logger::TEAM,
__FILE__": goalie move. intersection=(%.1f, %.1f) add_vec=(%.1f, %.1f)%.2f",
intersection.x, intersection.y,
add_vec.x, add_vec.y,
add_vec.r() );
result += add_vec;
if ( result.x < min_x )
{
result.x = min_x;
}
return result;
}
else
{
dlog.addText( Logger::TEAM,
__FILE__": goalie move. shot line has no intersection with goal line" );
if ( ball_pos.x > 0.0 )
{
return Vector2D(min_x , goal_l.y);
}
else if ( ball_pos.x < 0.0 )
{
return Vector2D(min_x , goal_r.y);
}
else
{
return Vector2D(min_x , 0.0);
}
}
}
/*!
execute action
*/
bool
Bhv_GoalieChaseBall::execute( PlayerAgent * agent )
{
dlog.addText( Logger::TEAM,
__FILE__": Bhv_GoalieChaseBall" );
//////////////////////////////////////////////////////////////////
// tackle
if ( Bhv_BasicTackle( 0.8, 90.0 ).execute( agent ) )
{
dlog.addText( Logger::TEAM,
__FILE__": tackle" );
return true;
}
const ServerParam & SP = ServerParam::i();
const WorldModel & wm = agent->world();
////////////////////////////////////////////////////////////////////////
// get active interception catch point
Vector2D my_int_pos = wm.ball().inertiaPoint( wm.interceptTable()->selfReachCycle() );
dlog.addText( Logger::TEAM,
__FILE__": execute. intercept point=(%.2f %.2f)",
my_int_pos.x, my_int_pos.y );
double pen_thr = wm.ball().distFromSelf() * 0.1 + 1.0;
if ( pen_thr < 1.0 ) pen_thr = 1.0;
//----------------------------------------------------------
const Line2D ball_line( wm.ball().pos(), wm.ball().vel().th() );
const Line2D defend_line( Vector2D( wm.self().pos().x, -10.0 ),
Vector2D( wm.self().pos().x, 10.0 ) );
if ( my_int_pos.x > - SP.pitchHalfLength() - 1.0
&& my_int_pos.x < SP.ourPenaltyAreaLineX() - pen_thr
&& my_int_pos.absY() < SP.penaltyAreaHalfWidth() - pen_thr )
{
bool save_recovery = false;
if ( ball_line.dist( wm.self().pos() ) < SP.catchableArea() )
{
save_recovery = true;
}
dlog.addText( Logger::TEAM,
__FILE__": execute normal intercept" );
agent->debugClient().addMessage( "Intercept(0)" );
Body_Intercept( save_recovery ).execute( agent );
agent->setNeckAction( new Neck_TurnToBall() );
return true;
}
int self_goalie_min = wm.interceptTable()->selfReachCycle();
int opp_min_cyc = wm.interceptTable()->opponentReachCycle();
Vector2D intersection = ball_line.intersection( defend_line );
if ( ! intersection.isValid()
|| ball_line.dist( wm.self().pos() ) < SP.catchableArea() * 0.8
|| intersection.absY() > SP.goalHalfWidth() + 3.0
)
{
if ( ! wm.existKickableOpponent() )
{
if ( self_goalie_min <= opp_min_cyc + 2
&& my_int_pos.x > -SP.pitchHalfLength() - 2.0
&& my_int_pos.x < SP.ourPenaltyAreaLineX() - pen_thr
&& my_int_pos.absY() < SP.penaltyAreaHalfWidth() - pen_thr
)
{
if ( Body_Intercept( false ).execute( agent ) )
{
dlog.addText( Logger::TEAM,
__FILE__": execute normal interception" );
agent->debugClient().addMessage( "Intercept(1)" );
agent->setNeckAction( new Neck_TurnToBall() );
return true;
}
}
dlog.addText( Logger::TEAM,
__FILE__": execute. ball vel has same slope to my body??"
" myvel-ang=%f body=%f. go to ball direct",
wm.ball().vel().th().degree(),
wm.self().body().degree() );
// ball vel angle is same to my body angle
agent->debugClient().addMessage( "GoToCatch(1)" );
doGoToCatchPoint( agent, wm.ball().pos() );
return true;
}
}
//----------------------------------------------------------
// body is already face to intersection
// only dash to intersection
// check catch point
if ( intersection.absX() > SP.pitchHalfLength() + SP.catchableArea()
|| intersection.x > SP.ourPenaltyAreaLineX() - SP.catchableArea()
|| intersection.absY() > SP.penaltyAreaHalfWidth() - SP.catchableArea()
)
{
dlog.addText( Logger::TEAM,
__FILE__": execute intersection(%.2f, %.2f) over range.",
intersection.x, intersection.y );
if ( Body_Intercept( false ).execute( agent ) )
{
agent->debugClient().addMessage( "Intercept(2)" );
agent->setNeckAction( new Neck_TurnToBall() );
return true;
}
else
{
return Bhv_GoalieBasicMove().execute( agent );
}
}
//----------------------------------------------------------
// check already there
const Vector2D my_inertia_final_pos
= wm.self().pos()
+ wm.self().vel()
/ (1.0 - wm.self().playerType().playerDecay());
double dist_thr = 0.2 + wm.ball().distFromSelf() * 0.1;
if ( dist_thr < 0.5 ) dist_thr = 0.5;
// if already intersection point stop dash
if ( my_inertia_final_pos.dist( intersection ) < dist_thr )
{
agent->debugClient().addMessage( "StopForChase" );
Body_StopDash( false ).execute( agent );
agent->setNeckAction( new Neck_TurnToBall() );
return true;
}
//----------------------------------------------------------
// forward or backward
dlog.addText( Logger::TEAM,
__FILE__": slide move. point=(%.1f, %.1f)",
intersection.x, intersection.y );
if ( wm.ball().pos().x > -35.0 )
{
if ( wm.ball().pos().y * intersection.y < 0.0 ) // opposite side
{
intersection.y = 0.0;
}
else
{
intersection.y *= 0.5;
}
}
agent->debugClient().addMessage( "GoToCatch(2)" );
doGoToCatchPoint( agent, intersection );
return true;
}
/*!
*/
double
TackleGenerator::evaluate( const WorldModel & wm,
const TackleResult & result )
{
const ServerParam & SP = ServerParam::i();
const Vector2D ball_end_point = inertia_final_point( wm.ball().pos(),
result.ball_vel_,
SP.ballDecay() );
const Segment2D ball_line( wm.ball().pos(), ball_end_point );
const double ball_speed = result.ball_speed_;
const AngleDeg ball_move_angle = result.ball_move_angle_;
#ifdef DEBUG_PRINT
dlog.addText( Logger::CLEAR,
"(evaluate) angle=%.1f speed=%.2f move_angle=%.1f end_point=(%.2f %.2f)",
result.tackle_angle_.degree(),
ball_speed,
ball_move_angle.degree(),
ball_end_point.x, ball_end_point.y );
#endif
//
// moving to their goal
//
if ( ball_end_point.x > SP.pitchHalfLength()
&& wm.ball().pos().dist2( SP.theirTeamGoalPos() ) < std::pow( 20.0, 2 ) )
{
const Line2D goal_line( Vector2D( SP.pitchHalfLength(), 10.0 ),
Vector2D( SP.pitchHalfLength(), -10.0 ) );
const Vector2D intersect = ball_line.intersection( goal_line );
if ( intersect.isValid()
&& intersect.absY() < SP.goalHalfWidth() )
{
double shoot_score = 1000000.0;
double speed_rate = 1.0 - std::exp( - std::pow( ball_speed, 2 )
/ ( 2.0 * std::pow( SP.ballSpeedMax()*0.5, 2 ) ) );
double y_rate = std::exp( - std::pow( intersect.absY(), 2 )
/ ( 2.0 * std::pow( SP.goalWidth(), 2 ) ) );
shoot_score *= speed_rate;
shoot_score *= y_rate;
#ifdef DEBUG_PRINT
dlog.addText( Logger::CLEAR,
"__ shoot %f (speed_rate=%f y_rate=%f)",
shoot_score, speed_rate, y_rate );
#endif
return shoot_score;
}
}
//
// moving to our goal
//
if ( ball_end_point.x < -SP.pitchHalfLength() )
{
const Line2D goal_line( Vector2D( -SP.pitchHalfLength(), 10.0 ),
Vector2D( -SP.pitchHalfLength(), -10.0 ) );
const Vector2D intersect = ball_line.intersection( goal_line );
if ( intersect.isValid()
&& intersect.absY() < SP.goalHalfWidth() + 1.0 )
{
double shoot_score = 0.0;
double y_penalty = ( -10000.0
* std::exp( - std::pow( intersect.absY() - SP.goalHalfWidth(), 2 )
/ ( 2.0 * std::pow( SP.goalHalfWidth(), 2 ) ) ) );
double speed_bonus = ( +10000.0
* std::exp( - std::pow( ball_speed, 2 )
/ ( 2.0 * std::pow( SP.ballSpeedMax()*0.5, 2 ) ) ) );
shoot_score = y_penalty + speed_bonus;
#ifdef DEBUG_PRINT
dlog.addText( Logger::CLEAR,
"__ in our goal %f (y_pealty=%f speed_bonus=%f)",
shoot_score, y_penalty, speed_bonus );
#endif
return shoot_score;
}
}
//
// normal evaluation
//
int opponent_reach_step = predictOpponentsReachStep( wm,
wm.ball().pos(),
result.ball_vel_,
ball_move_angle );
Vector2D final_point = inertia_n_step_point( wm.ball().pos(),
result.ball_vel_,
opponent_reach_step,
SP.ballDecay() );
{
Segment2D final_segment( wm.ball().pos(), final_point );
Rect2D pitch = Rect2D::from_center( 0.0, 0.0, SP.pitchLength(), SP.pitchWidth() );
Vector2D intersection;
int n = pitch.intersection( final_segment, &intersection, NULL );
if ( n > 0 )
{
final_point = intersection;
}
}
#if 1
AngleDeg our_goal_angle = ( SP.ourTeamGoalPos() - wm.ball().pos() ).th();
double our_goal_angle_diff = ( our_goal_angle - ball_move_angle ).abs();
double our_goal_angle_rate = 1.0 - std::exp( - std::pow( our_goal_angle_diff, 2 )
/ ( 2.0 * std::pow( 40.0, 2 ) ) );
double y_rate = ( final_point.absY() > SP.pitchHalfWidth() - 0.1
? 1.0
: std::exp( - std::pow( final_point.absY() - SP.pitchHalfWidth(), 2 )
/ ( 2.0 * std::pow( SP.pitchHalfWidth() * 0.7, 2 ) ) ) );
double opp_rate = 1.0 - std::exp( - std::pow( (double)opponent_reach_step, 2 )
/ ( 2.0 * std::pow( 30.0, 2 ) ) );
double score = 10000.0 * our_goal_angle_rate * y_rate * opp_rate;
#ifdef DEBUG_PRINT
dlog.addText( Logger::CLEAR,
"__ goal_angle_rate=%f",
our_goal_angle_rate, y_rate, score );
dlog.addText( Logger::CLEAR,
"__ y_rate=%f",
our_goal_angle_rate, y_rate, score );
dlog.addText( Logger::CLEAR,
"__ opp_rate=%f",
opp_rate );
dlog.addText( Logger::CLEAR,
">>> score=%f",
score );
#endif
return score;
#else
double x_val = ( final_point.x > SP.pitchHalfLength()
? 1.0
: std::exp( - std::pow( final_point.x - SP.pitchHalfLength(), 2 )
/ ( 2.0 * std::pow( SP.pitchLength() * 0.4, 2 ) ) ) );
double y_val = ( final_point.absY() > SP.pitchHalfWidth()
? 1.0
: std::exp( - std::pow( final_point.absY() - SP.pitchHalfWidth(), 2 )
/ ( 2.0 * std::pow( SP.pitchHalfWidth() * 0.7, 2 ) ) ) );
double opp_goal_dist = SP.theirTeamGoalPos().dist( final_point );
double opp_goal_dist_val = std::exp( - std::pow( opp_goal_dist, 2 )
/ ( 2.0 * std::pow( 20.0, 2 ) ) );
double our_goal_dist = SP.ourTeamGoalPos().dist( final_point );
double our_goal_dist_rate = 1.0 - std::exp( - std::pow( our_goal_dist, 2 )
/ ( 2.0 * std::pow( 20.0, 2 ) ) );
double opp_rate = 1.0 - std::exp( - std::pow( (double)opponent_reach_step, 2 )
/ ( 2.0 * std::pow( 30.0, 2 ) ) );
double score = 0.0;
score += 1000.0 * x_val;
#ifdef DEBUG_PRINT
dlog.addText( Logger::CLEAR,
"__ x_val %f (%f)", 10000.0 * x_val, score );
#endif
score += 1000.0 * y_val;
#ifdef DEBUG_PRINT
dlog.addText( Logger::CLEAR,
"__ y_val %f (%f)", 1000.0 * y_val, score );
#endif
score += 1000.0 * opp_goal_dist_val;
#ifdef DEBUG_PRINT
dlog.addText( Logger::CLEAR,
"__ opp_goal_dist_val %f (%f)", 1000.0 * opp_goal_dist_val, score );
#endif
score *= our_goal_dist_rate;
#ifdef DEBUG_PRINT
dlog.addText( Logger::CLEAR,
"__ our_goal_dist=%.2f rate=%f (%f)",
our_goal_dist, our_goal_dist_rate, score );
#endif
score *= opp_rate;
#ifdef DEBUG_PRINT
dlog.addText( Logger::CLEAR,
"__ opponent_reach_step=%d rate=%f (%f)",
opponent_reach_step, opp_rate, score );
#endif
return score;
#endif
}
