void tausche(Vector2D* a, Vector2D*b)
{
Vector2D help = *a;
b->kopiereIn(a);
help.kopiereIn(b);
}
/*!
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;
}
float Vector2D::normalizedDot(Vector2D v){
Vector2D normedV1 = normalize();
Vector2D normedV2 = v.normalize();
return ( normedV1.x*normedV2.x + normedV1.y*normedV2.y );
}
void VideoManager::translate(const Vector2D &vector) const
{
glTranslatef(vector.getX(), vector.getY(), 0.0f);
}
/*!
*/
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() );
}
Circle(const Point2D < T > &Point, const Vector2D < T > &Vector) {
this->Centre = Point;
this->R = Vector.Norm();
}
Vector3D::Vector3D(const Vector2D &vector):xp(vector.x()), yp(vector.y()), zp(0.0){}
/*!
*/
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
}
float Vector2D::distance(const Vector2D& rhs) const
{
Vector2D val = (*this);
val -= rhs;
return val.length();
}
void tausche(Vector2D &a, Vector2D &b){
Vector2D temp;
temp.kopiereIn(a);
a.kopiereIn(b);
b.kopiereIn(temp);
}
/*!
*/
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
}
void Chaser::handleMovement(Vector2D velocity)
{
Vector2D newPos = m_position;
newPos.x(m_position.x() + velocity.x());
if (newPos.x() == m_playerPos->x())
{
m_velocity.x(0);
}
else
{
// check if the chaser is trying to go off the map
if (newPos.x() + getCollider().x < 0)
{
m_position.x(-getCollider().x);
}
else if (newPos.x() + getCollider().x + getCollider().w > TheGame::Instance().getMapWidth())
{
m_position.x(TheGame::Instance().getMapWidth() - getCollider().w - getCollider().x);
}
else
{
if (checkCollideTile(newPos) || checkCollideObject(newPos))
{
// collision, stop x movement
m_velocity.x(0);
if (checkCollideTile(newPos))
{
// Collision with the map, move to contact
if (m_position.x() < newPos.x()) // Collision with tile to the right
{
m_position.x(m_position.x() + (
abs(m_xSpeed) - (int(newPos.x() + getCollider().x + getCollider().w) % (*m_pCollisionLayers->begin())->getTileSize())));
}
else // Collision with tile to the left
{
m_position.x(m_position.x() -
(int(m_position.x() + getCollider().x) % (*m_pCollisionLayers->begin())->getTileSize()));
}
}
}
else
{
// no collision, add to the actual x position
m_position.x(newPos.x());
}
}
}
newPos.x(m_position.x());
newPos.y(m_position.y() + velocity.y());
// check if the chaser is going below map limits
if (newPos.y() + getCollider().y + getCollider().h > TheGame::Instance().getMapHeight())
{
m_position.y(TheGame::Instance().getMapHeight() - getCollider().h - getCollider().y);
m_velocity.y(0);
}
else
{
if (checkCollideTile(newPos) || checkCollideObject(newPos))
{
// Collision, stop y movement
m_velocity.y(0);
if (checkCollideTile(newPos))
{
// Collision with map, move to contact. Chaser doesn't jump so it's a lower tile
m_position.y(m_position.y() +
(m_ySpeed - (int(newPos.y() + getCollider().y + getCollider().h) % (*m_pCollisionLayers->begin())->getTileSize())));
}
}
else
{
// no collision, add to the actual y position
m_position.y(newPos.y());
}
}
}
double bilinearInterpolate(double xy, double x1y,double xy1, double x1y1, Vector2D p){
return (1-p.x())*(1-p.y())*xy + p.x()*(1-p.y())*x1y +
(1-p.x())*p.y()*xy1 + p.x()*p.y()*x1y1;
}
RobotCommand TacticTransferObject::getCommand()
{
AllInMargin=true;
RobotCommand rc;
if(!wm->ourRobot[id].isValid) return rc;
rc.useNav=true;
rc.maxSpeed = 1;
rc.fin_pos.loc=wm->endPoint;//Vector2D(300,0);
int object;
addData();
mergeData();
sortData();
// if(wm->balls.size() > 0)
// {
// qDebug()<< " BALLL . X = " << wm->balls.at(0)->pos.loc.x << " BALLL . Y = " << wm->balls.at(0)->pos.loc.y;
// qDebug() << " MAX x : " << region[1].maxX() << " MIN X : " << region[1].minX() ;
// qDebug() << " MAX y : " << region[1].maxY() << " MIN y : " << region[1].minY() ;
// if(region[0].IsInside(wm->balls.at(0)->pos.loc)) qDebug() << " THE BALLLLL ISSS INNNNN SIDE !!!!!!!!!!!!!!!!!!!!!!1";
// }
index = -1;
for(int i=0;i<mergedList.size();i++)
{
// qDebug() << i << " AT : (" << mergedList.at(i).pos.x << "," << mergedList.at(i).pos.y << ")";
temp=0;
if(!region[mergedList.at(i).goalRegion].IsInside(mergedList.at(i).pos) && !IsInmargins(mergedList.at(i).pos,300))
{
//qDebug() <<" OBJECT : " << mergedList.at(i).pos.x << " ------ Y = " << mergedList.at(i).pos.y;// TOOOOOOOOOOOOOOOOOOOSHE !!!!!!!" << index ;
// AllInMargin=false;
index=i;
goalRegion=mergedList.at(i).goalRegion;
temp=1;
break;
}
}
for(int i=0; i<mergedList.size(); i++)
{
if(!IsInmargins(mergedList.at(i).pos,300))
{
AllInMargin=false;
}
}
if(AllInMargin)
{
for(int i=0;i<mergedList.size();i++)
{
if(!region[mergedList.at(i).goalRegion].IsInside(mergedList.at(i).pos))
{
index=i;
goalRegion=mergedList.at(i).goalRegion;
break;
}
}
}
// if(index ==-1)
// {
// for(int i=0;i<wm->Chasbideh.size(); i++)
// {
// if(!region[0].IsInside(wm->Chasbideh.at(i).position) && !region[1].IsInside(wm->Chasbideh.at(i).position))
// {
// //qDebug() <<" OBJECT : " << mergedList.at(i).pos.x << " ------ Y = " << mergedList.at(i).pos.y;// TOOOOOOOOOOOOOOOOOOOSHE !!!!!!!" << index ;
// index=i;
// goalRegion=0;//mergedList.at(i).goalRegion;
// temp=1;
// break;
// }
// }
// }
// qDebug() << mergedList.size() << " MERGED SIZE " ;
if(index != -1)
{
Vector2D point2 = mergedList.at(index).pos;
Vector2D diff2 = region[goalRegion].center() - point2;
bool reach=false;
if(temp!=0)
{
switch(state)
{
case 0:{ //Go Behind the Object
Vector2D space2=diff2;
space2.setLength(300);
rc.maxSpeed=1.4;
rc.useNav = true;
rc.fin_pos.loc=point2 - space2;
rc.fin_pos.dir=diff2.dir().radian();
object=findnearestObject(mergedShapeList,wm->ourRobot[id].pos.loc);
if(object!=-1) ObsC=Circle2D(mergedShapeList.at(object).position,(mergedShapeList.at(object).roundedRadios+ROBOT_RADIUS+150));
rc.fin_pos.loc=AvoidtoEnterCircle(ObsC,wm->ourRobot[id].pos.loc,rc.fin_pos.loc);
reach=wm->kn->ReachedToPos(wm->ourRobot[id].pos.loc,rc.fin_pos.loc,150);
if(reach) state = 1;
}
break;
case 1:{//Ready to Push
rc.useNav = false;
rc.maxSpeed=1.2;
rc.fin_pos.loc.x=point2.x - (100 + ROBOT_RADIUS)*(diff2.x)/(diff2.length()); // 100 >> Rounded Radius
rc.fin_pos.loc.y=point2.y - (100 + ROBOT_RADIUS)*(diff2.y)/(diff2.length());
rc.fin_pos.dir=diff2.dir().radian();
if(((wm->ourRobot[id].pos.loc-point2).length())>400) state=0;
reach=wm->kn->ReachedToPos(wm->ourRobot[id].pos.loc,rc.fin_pos.loc,100);
if(reach)
state = 2;
}
break;
case 2:{//Push
//Vector2D diff2 = region2.center() - wm->ourRobot[id].pos.loc ;
rc.useNav = false;
rc.maxSpeed=1;
//if(diff2.length() > 1500) diff2.setLength(1500);
// if(((wm->ourRobot[id].pos.loc-point2).length())>400) state=0;
diff2.setLength(300);
if(((wm->ourRobot[id].pos.loc-point2).length())>600) state=0;
if(((wm->ourRobot[id].pos.loc-rc.fin_pos.loc).length())<50) state=0;
Vector2D o2r = ( point2 - wm->ourRobot[id].pos.loc );
if(fabs(wm->ourRobot[id].pos.dir - o2r.dir().radian()) > AngleDeg::deg2rad(40))
{
qDebug() << " !!!! Out OF Direction !!!! " ;
state=1;//4;
}
rc.fin_pos.loc=point2 + diff2;//5;
rc.fin_pos.dir=diff2.dir().radian();
reach=wm->kn->ReachedToPos(wm->ourRobot[id].pos.loc,rc.fin_pos.loc,10);
if(reach)
state = 3;
}
break;
case 3:{//Release
if(region[goalRegion].IsInside(point2))
{
//qDebug() << " INNNNNNNNNNNN SIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIDE !!!";
//if(index==agentsR1.size()) rc.fin_pos.loc=Vector2D(0,0);
if(temp==0)
{
rc.fin_pos.loc=Vector2D(0,0);
break;
}
//agentsR1.takeFirst();
//index++;
}
//if(reach)
state = 0;
}
break;
case 4:{// back behind the object
}
break;
}
}
Vector2D dlta;
double mrgn=300;
if(IsInmargins(point2,mrgn))
{
// qDebug() << " IS IN MARGIN !!!!!!!!!";
int side = ((point2.x-mean_x)/abs(point2.x-mean_x))*((point2.y-mean_y)/abs(point2.y-mean_y));
if(point2.x > MAX_X-mrgn || point2.x < MIN_X+mrgn) {
side *= ((point2.y-mean_y)/abs(point2.y-mean_y));
dlta=Vector2D(side*10,side*(ROBOT_RADIUS+/*mergedShapeList.at(index).roundedRadios+*/50));}
else if(point2.y > MAX_Y-mrgn || point2.y < MIN_Y+mrgn) {
side *=((point2.x-mean_x)/abs(point2.x-mean_x));
dlta=Vector2D(side*(ROBOT_RADIUS+/*mergedShapeList.at(index).roundedRadios+*/50),side*10);}
switch(statemargin)
{
case 0:{
rc.fin_pos.loc=point2+dlta;
// object=findnearestObject(mergedShapeList,wm->ourRobot[id].pos.loc);
// if(object!=-1) ObsC=Circle2D(mergedShapeList.at(object).position,(mergedShapeList.at(object).roundedRadios+ROBOT_RADIUS));
// rc.fin_pos.loc=AvoidtoEnterCircle(ObsC,wm->ourRobot[id].pos.loc,rc.fin_pos.loc);
// int rad = mergedShapeList.at(index).roundedRadios+ROBOT_RADIUS;
// Circle2D c(point2,rad);
// rc.fin_pos.loc=AvoidtoEnterCircle(c,wm->ourRobot[id].pos.loc,rc.fin_pos.loc);
// qDebug()<< "In Margins Pos : ball = ( " << point2.x << ","<< point2.y << ")";
// qDebug()<< "In Margins Pos : delta = ( " << dlta.x << ","<< dlta.y << ")";
// qDebug()<< "In Margins Pos : fin_pos = ( " << rc.fin_pos.loc.x << ","<<rc.fin_pos.loc.y << ")";
// qDebug()<< "In Margins Pos : Robot = ( " << wm->ourRobot[id].pos.loc.x << ","<<wm->ourRobot[id].pos.loc.y << ")";
rc.fin_pos.dir=dlta.dir().radian()-side*M_PI/2;
reach=wm->kn->ReachedToPos(wm->ourRobot[id].pos,rc.fin_pos,20,7);
// wm->ourRobot[id].pos.loc,rc.fin_pos.loc,200);
// qDebug() << "dist To final Pos : " << (wm->ourRobot[id].pos.loc-rc.fin_pos.loc).length();
// qDebug() << " Avoided : " << Avoided << " reach" << reach;
if(reach) statemargin = 1;
}
break;
case 1:{
rc.fin_pos.dir = dlta.dir().radian() - side*0.9*M_PI ;
rc.fin_pos.loc=point2-dlta;
// qDebug() << "Fin_POS . dir = " << AngleDeg::rad2deg(rc.fin_pos.dir) << " ROBOT . dir = " << AngleDeg::rad2deg(wm->ourRobot[id].pos.dir);
if(((wm->ourRobot[id].pos.loc-point2).length())>300) statemargin=0;
double delta_ang=wm->ourRobot[id].pos.dir-rc.fin_pos.dir;
if (delta_ang > M_PI) delta_ang -= (M_PI * 2);
if (delta_ang < -M_PI) delta_ang += (M_PI * 2);
if(fabs(delta_ang) < AngleDeg::deg2rad(10)) statemargin=0;
rc.maxSpeed=1.7;
// bool chighz=wm->kn->ReachedToPos(wm->ourRobot[id].pos,rc.fin_pos,20,10);
// if(chighz) statemargin=0;
// if((wm->ourRobot[id].pos.loc.dir()-rc.fin_pos.dir)<AngleDeg::deg2rad(10)) statemargin=0;
// if(((wm->ourRobot[id].pos.loc-rc.fin_pos.loc).length())<250) state=0;
}
break;
}
}
// qDebug() << rc.fin_pos.loc.x << " ------- Y = " << rc.fin_pos.loc.y << " STATE = " << state;
// qDebug() << "STATE = " << state;
}
//rc.maxSpeed = 1.2;//rc.maxSpeed;
// rc.fin_pos.loc.x=rcpast.x + 0.1*(rc.fin_pos.loc.x-rcpast.x);
// rc.fin_pos.loc.y=rcpast.y + 0.1*(rc.fin_pos.loc.y-rcpast.y);
// rcpast=rc.fin_pos.loc;
// qDebug() << " INDEX = " << index ;
rc.maxSpeed/=1.4;
if(IsInmargins(wm->ourRobot[id].pos.loc,500)) rc.maxSpeed /= 1.5 ;
rc.fin_pos.loc=KeepInField(rc);
// qDebug() << " This Object Is For Region " << goalRegion ;
rc.useNav=false;
rc.isBallObs = false;
rc.isKickObs = true;
return rc;
}
float Vector2D::SqDistance(const Vector2D & v) const
{
Vector2D mag = v - *this;
return mag.GetLengthSq();
}
/*!
*/
void
CrossGenerator::createCross( const WorldModel & wm,
const AbstractPlayerObject * receiver )
{
static const int MIN_RECEIVE_STEP = 2;
static const int MAX_RECEIVE_STEP = 12; // Magic Number
static const double MIN_RECEIVE_BALL_SPEED
= ServerParam::i().defaultPlayerSpeedMax();
// = std::max( ServerParam::i().defaultPlayerSpeedMax(),
// ServerParam::i().ballSpeedMax()
// * std::pow( ServerParam::i().ballDecay(), MAX_RECEIVE_STEP ) );
// static const MAX_RECEIVE_BALL_SPEED
// = ServerParam::i().ballSpeedMax()
// * std::pow( ServerParam::i().ballDecay(), MIN_RECEIVE_STEP );
static const double ANGLE_STEP = 3.0;
static const double DIST_STEP = 0.9;
const ServerParam & SP = ServerParam::i();
const double min_first_ball_speed = SP.ballSpeedMax() * 0.67; // Magic Number
const double max_first_ball_speed = ( wm.gameMode().type() == GameMode::PlayOn
? SP.ballSpeedMax()
: wm.self().isKickable()
? wm.self().kickRate() * SP.maxPower()
: SP.kickPowerRate() * SP.maxPower() );
const PlayerType * ptype = receiver->playerTypePtr();
const Vector2D receiver_pos = receiver->inertiaFinalPoint();
const double receiver_dist = M_first_point.dist( receiver_pos );
const AngleDeg receiver_angle_from_ball = ( receiver_pos - M_first_point ).th();
#ifdef USE_ONLY_MAX_ANGLE_WIDTH
double max_angle_diff = -1.0;
CooperativeAction::Ptr best_action;
#endif
//
// angle loop
//
for ( int a = -2; a < 3; ++a )
{
const AngleDeg cross_angle = receiver_angle_from_ball + ( ANGLE_STEP * a );
//
// distance loop
//
for ( int d = 0; d < 5; ++d )
{
const double sub_dist = DIST_STEP * d;
const double ball_move_dist = receiver_dist - sub_dist;
const Vector2D receive_point
= M_first_point
+ Vector2D::from_polar( ball_move_dist, cross_angle );
#ifdef DEBUG_PRINT
dlog.addText( Logger::CROSS,
"==== receiver=%d receivePos=(%.2f %.2f) loop=%d angle=%.1f",
receiver->unum(),
receive_point.x, receive_point.y,
a, cross_angle.degree() );
#endif
if ( receive_point.x > SP.pitchHalfLength() - 0.5
|| receive_point.absY() > SP.pitchHalfWidth() - 3.0 )
{
#ifdef DEBUG_PRINT
dlog.addText( Logger::CROSS,
"%d: xxx unum=%d (%.2f %.2f) outOfBounds",
M_total_count, receiver->unum(),
receive_point.x, receive_point.y );
debug_paint_failed( M_total_count, receive_point );
#endif
continue;
}
const int receiver_step = ptype->cyclesToReachDistance( sub_dist ) + 1;
//
// step loop
//
for ( int step = std::max( MIN_RECEIVE_STEP, receiver_step );
step <= MAX_RECEIVE_STEP;
++step )
{
++M_total_count;
double first_ball_speed = calc_first_term_geom_series( ball_move_dist,
SP.ballDecay(),
step );
if ( first_ball_speed < min_first_ball_speed )
{
#ifdef DEBUG_PRINT_FAILED_COURSE
dlog.addText( Logger::CROSS,
"%d: xxx unum=%d (%.1f %.1f) step=%d firstSpeed=%.3f < min=%.3f",
M_total_count,
receiver->unum(),
receive_point.x, receive_point.y,
step,
first_ball_speed, min_first_ball_speed );
//debug_paint_failed( M_total_count, receive_point );
#endif
break;
}
if ( max_first_ball_speed < first_ball_speed )
{
#ifdef DEBUG_PRINT_FAILED_COURSE
dlog.addText( Logger::CROSS,
"%d: xxx unum=%d (%.1f %.1f) step=%d firstSpeed=%.3f > max=%.3f",
M_total_count,
receiver->unum(),
receive_point.x, receive_point.y,
step,
first_ball_speed, max_first_ball_speed );
//debug_paint_failed( M_total_count, receive_point );
#endif
continue;
}
double receive_ball_speed = first_ball_speed * std::pow( SP.ballDecay(), step );
if ( receive_ball_speed < MIN_RECEIVE_BALL_SPEED )
{
#ifdef DEBUG_PRINT_FAILED_COURSE
dlog.addText( Logger::CROSS,
"%d: xxx unum=%d (%.1f %.1f) step=%d recvSpeed=%.3f < min=%.3f",
M_total_count,
receiver->unum(),
receive_point.x, receive_point.y,
step,
receive_ball_speed, min_first_ball_speed );
//debug_paint_failed( M_total_count, receive_point );
#endif
break;
}
int kick_count = FieldAnalyzer::predict_kick_count( wm,
M_passer,
first_ball_speed,
cross_angle );
if ( ! checkOpponent( M_first_point,
receiver,
receive_point,
first_ball_speed,
cross_angle,
step + kick_count - 1 ) ) // 1 step penalty for observation delay
{
break;
}
#ifdef USE_ONLY_MAX_ANGLE_WIDTH
double min_angle_diff = getMinimumAngleWidth( ball_move_dist, cross_angle );
if ( min_angle_diff > max_angle_diff )
{
CooperativeAction::Ptr ptr( new Pass( M_passer->unum(),
receiver->unum(),
receive_point,
first_ball_speed,
step + kick_count,
kick_count,
false,
"cross" ) );
ptr->setIndex( M_total_count );
max_angle_diff = min_angle_diff;
best_action = ptr;
}
#else
CooperativeAction::Ptr ptr( new Pass( M_passer->unum(),
receiver->unum(),
receive_point,
first_ball_speed,
step + kick_count,
kick_count,
false,
"cross" ) );
ptr->setIndex( M_total_count );
M_courses.push_back( ptr );
#endif
// M_courses.push_back( ptr );
#ifdef DEBUG_PRINT_SUCCESS_COURSE
dlog.addText( Logger::CROSS,
"%d: ok Cross step=%d pos=(%.1f %.1f) speed=%.3f->%.3f nKick=%d",
M_total_count,
step, kick_count,
receive_point.x, receive_point.y,
first_ball_speed, receive_ball_speed,
kick_count );
char num[8];
snprintf( num, 8, "%d", M_total_count );
dlog.addMessage( Logger::CROSS,
receive_point, num );
dlog.addRect( Logger::CROSS,
receive_point.x - 0.1, receive_point.y - 0.1,
0.2, 0.2,
"#00ff00" );
#endif
break;
}
}
}
#ifdef USE_ONLY_MAX_ANGLE_WIDTH
if ( best_action )
{
M_courses.push_back( best_action );
}
#endif
}
void Tamer::onCollision(Game::Object* other, const pair<int, int >& firstColliderId)
{
Vector2D toOther = other->getPosC().position - getPosC().position;
getPos().addForce( -toOther.normalised() * 5);
}
/*!
*/
bool
CrossGenerator::checkOpponent( const Vector2D & first_ball_pos,
const rcsc::AbstractPlayerObject * receiver,
const Vector2D & receive_pos,
const double & first_ball_speed,
const AngleDeg & ball_move_angle,
const int max_cycle )
{
static const double CONTROL_AREA_BUF = 0.15; // buffer for kick table
const ServerParam & SP = ServerParam::i();
const double receiver_dist = receiver->pos().dist( first_ball_pos );
const Vector2D first_ball_vel
= Vector2D( receive_pos - first_ball_pos ).setLength( first_ball_speed );
const AbstractPlayerCont::const_iterator end = M_opponents.end();
for ( AbstractPlayerCont::const_iterator o = M_opponents.begin();
o != end;
++o )
{
const PlayerType * ptype = (*o)->playerTypePtr();
const double control_area = ( (*o)->goalie()
? SP.catchableArea()
: ptype->kickableArea() );
const Vector2D opponent_pos = (*o)->inertiaFinalPoint();
const int min_cycle = FieldAnalyzer::estimate_min_reach_cycle( opponent_pos,
ptype->realSpeedMax(),
first_ball_pos,
ball_move_angle );
if ( opponent_pos.dist( first_ball_pos ) > receiver_dist + 1.0 )
{
#ifdef DEBUG_PRINT
dlog.addText( Logger::CROSS,
"__ opponent[%d](%.2f %.2f) skip. distance over",
(*o)->unum(),
opponent_pos.x, opponent_pos.y );
#endif
continue;
}
for ( int cycle = std::max( 1, min_cycle );
cycle <= max_cycle;
++cycle )
{
Vector2D ball_pos = inertia_n_step_point( first_ball_pos,
first_ball_vel,
cycle,
SP.ballDecay() );
double target_dist = opponent_pos.dist( ball_pos );
if ( target_dist - control_area - CONTROL_AREA_BUF < 0.001 )
{
#ifdef DEBUG_PRINT_FAILED_COURSE
dlog.addText( Logger::CROSS,
"%d: xxx recvPos(%.2f %.2f) step=%d/%d"
" opponent(%d)(%.2f %.2f) kickable"
" ballPos(%.2f %.2f)",
M_total_count,
receive_pos.x, receive_pos.y,
cycle, max_cycle,
(*o)->unum(), opponent_pos.x, opponent_pos.y ,
ball_pos.x, ball_pos.y );
debug_paint_failed( M_total_count, receive_pos );
#endif
return false;
}
double dash_dist = target_dist;
if ( cycle > 1 )
{
//dash_dist -= control_area*0.8;
dash_dist -= control_area*0.6;
//dash_dist -= control_area*0.5;
}
if ( dash_dist > ptype->realSpeedMax() * cycle )
{
continue;
}
//
// dash
//
int n_dash = ptype->cyclesToReachDistance( dash_dist * 1.05 ); // add penalty
if ( n_dash > cycle )
{
continue;
}
//
// turn
//
int n_turn = ( (*o)->bodyCount() >= 3
? 2
: FieldAnalyzer::predict_player_turn_cycle( ptype,
(*o)->body(),
(*o)->vel().r(),
target_dist,
( ball_pos - opponent_pos ).th(),
control_area,
true ) );
int n_step = n_turn + n_dash + 1; // 1 step penalty for observation delay
if ( (*o)->isTackling() )
{
n_step += 5; // Magic Number
}
if ( n_step <= cycle )
{
#ifdef DEBUG_PRINT_FAILED_COURSE
dlog.addText( Logger::CROSS,
"%d: xxx recvPos(%.1f %.1f) step=%d/%d"
" opponent(%d)(%.1f %.1f) can reach"
" ballPos(%.2f %.2f)",
M_total_count,
receive_pos.x, receive_pos.y,
cycle, max_cycle,
(*o)->unum(), opponent_pos.x, opponent_pos.y,
ball_pos.x, ball_pos.y );
debug_paint_failed( M_total_count, receive_pos );
#endif
return false;
}
}
}
return true;
}
//----------------------- isPassSafeFromOpponent -------------------------
//
// test if a pass from 'from' to 'to' can be intercepted by an opposing
// player
//------------------------------------------------------------------------
bool AbstSoccerTeam::isPassSafeFromOpponent(Vector2D from,
Vector2D target,
const PlayerBase* const receiver,
const PlayerBase* const opp,
double PassingForce)const
{
//move the opponent into local space.
Vector2D ToTarget = target - from;
Vector2D ToTargetNormalized = Vec2DNormalize(ToTarget);
Vector2D LocalPosOpp = PointToLocalSpace(opp->Pos(),
ToTargetNormalized,
ToTargetNormalized.Perp(),
from);
//if opponent is behind the kicker then pass is considered okay(this is
//based on the assumption that the ball is going to be kicked with a
//velocity greater than the opponent's max velocity)
if ( LocalPosOpp.x < 0 )
{
return true;
}
//if the opponent is further away than the target we need to consider if
//the opponent can reach the position before the receiver.
if (Vec2DDistanceSq(from, target) < Vec2DDistanceSq(opp->Pos(), from))
{
if (receiver)
{
if ( Vec2DDistanceSq(target, opp->Pos()) >
Vec2DDistanceSq(target, receiver->Pos()) )
{
return true;
}
else
{
return false;
}
}
else
{
return true;
}
}
//calculate how long it takes the ball to cover the distance to the
//position orthogonal to the opponents position
double TimeForBall =
Pitch()->Ball()->TimeToCoverDistance(Vector2D(0,0),
Vector2D(LocalPosOpp.x, 0),
PassingForce);
//now calculate how far the opponent can run in this time
double reach = opp->MaxSpeed() * TimeForBall +
Pitch()->Ball()->BRadius()+
opp->BRadius();
//if the distance to the opponent's y position is less than his running
//range plus the radius of the ball and the opponents radius then the
//ball can be intercepted
if ( fabs(LocalPosOpp.y) < reach )
{
return false;
}
return true;
}
int main(int argc, char* argv[])
{
CS325Graphics window(argc, argv);
float delta = 0.1;
Point2D p1(CS325Graphics::X_MIN, CS325Graphics::Y_MAX / 4.5);
Point2D p2(CS325Graphics::X_MAX, CS325Graphics::Y_MAX / 4.5);
Point2D p3(CS325Graphics::X_MIN, CS325Graphics::Y_MIN);
Point2D p4(CS325Graphics::X_MAX, CS325Graphics::Y_MAX);
//Points 41, 42, 45, 46 control the sandbox. DON"T MESS WITH THEM!
Point3D p30(0.5, 0.5,-3.5);
Point3D p31(0.5, -0.5,-3.5);
Point3D p32(-0.5,-0.5,-3.5);
Point3D p33(-0.5, 0.5,-3.5);
Point3D p34(0.5, 0.5,-1.5);
Point3D p35(0.5, -0.5,-1.5);
Point3D p36(-0.5,-0.5,-1.5);
Point3D p37(-0.5, 0.5,-1.5);
Point3D p40( -70.8, 28.8, -50.8);
Point3D p41( 50.8,-2.8, 50.8);
Point3D p42(-50.8,-2.8, 50.8);
Point3D p43(-58.8, 25.8, 50.8);
Point3D p44( 50.8, 50.8, -50.8);
Point3D p45( 50.8,-2.8, -50.8);
Point3D p46(-50.8,-2.8, -50.8);
Point3D p47(-84.8,-2.8, -50.8);
Point3D p49(-8.5,22.0, 50.8);
Point3D p48(70,20,50.8);
Point3D p50(3.5, 0.5,-3.5);
Point3D p51(3.5, -0.5,-3.5);
Point3D p52(2.5,-0.5,-3.5);
Point3D p53(2.5, 0.5,-3.5);
Point3D p54(3.5, 0.5,-1.5);
Point3D p55(3.5, -0.5,-1.5);
Point3D p56(2.5,-0.5,-1.5);
Point3D p57(2.5, 0.5,-1.5);
Point3D p60(3.5, 0.5, 13.5);
Point3D p61(3.5, -0.5, 13.5);
Point3D p62(2.5,-0.5, 13.5);
Point3D p63(2.5, 0.5, 13.5);
Point3D p64(3.5, 0.5, 16.5);
Point3D p65(3.5, -0.5, 16.5);
Point3D p66(2.5,-0.5, 16.5);
Point3D p67(2.5, 0.5, 16.5);
Point2D viewPos;
Vector2D viewDir;
Vector3D deltaV;
viewDir.setAngle(0);
// move view position
for(int i = 0; i < MOVE_TEST; i){
/*window.DrawLineOnScreen(p1, p2);*/
//window.DrawLineOnScreen(p4, p3);
window.DrawLineInSpace(p30, p31);
window.DrawLineInSpace(p31, p32);
window.DrawLineInSpace(p32, p33);
window.DrawLineInSpace(p33, p30);
window.DrawLineInSpace(p34, p35);
window.DrawLineInSpace(p35, p36);
window.DrawLineInSpace(p36, p37);
window.DrawLineInSpace(p37, p34);
window.DrawLineInSpace(p30, p34);
window.DrawLineInSpace(p31, p35);
window.DrawLineInSpace(p32, p36);
window.DrawLineInSpace(p33, p37);
window.DrawLineInSpace(p50, p51);
window.DrawLineInSpace(p51, p52);
window.DrawLineInSpace(p52, p53);
window.DrawLineInSpace(p53, p50);
window.DrawLineInSpace(p54, p55);
window.DrawLineInSpace(p55, p56);
window.DrawLineInSpace(p56, p57);
window.DrawLineInSpace(p57, p54);
window.DrawLineInSpace(p50, p54);
window.DrawLineInSpace(p51, p55);
window.DrawLineInSpace(p52, p56);
window.DrawLineInSpace(p53, p57);
window.DrawLineInSpace(p60, p61);
window.DrawLineInSpace(p61, p62);
window.DrawLineInSpace(p62, p63);
window.DrawLineInSpace(p63, p60);
window.DrawLineInSpace(p64, p65);
window.DrawLineInSpace(p65, p66);
window.DrawLineInSpace(p66, p67);
window.DrawLineInSpace(p67, p64);
window.DrawLineInSpace(p60, p64);
window.DrawLineInSpace(p61, p65);
window.DrawLineInSpace(p62, p66);
window.DrawLineInSpace(p63, p67);
//window.DrawLineInSpace(p40, p41);
window.DrawLineInSpace(p41, p42);
window.DrawLineInSpace(p42, p43);
//window.DrawLineInSpace(p43, p40);
//window.DrawLineInSpace(p44, p45);
window.DrawLineInSpace(p45, p46);
//window.DrawLineInSpace(p46, p47);
//window.DrawLineInSpace(p47, p44);
window.DrawLineInSpace(p40, p45);
window.DrawLineInSpace(p41, p45);
window.DrawLineInSpace(p42, p46);
window.DrawLineInSpace(p43, p47);
window.DrawLineInSpace(p40, p47);
window.DrawLineInSpace(p41, p49);
window.DrawLineInSpace(p42, p49);
window.DrawLineInSpace(p41, p48);
window.DrawLineInSpace(p45, p48);
if(GetAsyncKeyState(VK_DOWN)) // the DOWN arrow was pressed, let's do something
{
delta = -.1;
viewPos.setY(viewPos.getY() + cos(-viewDir.getAngle())*delta);
viewPos.setX(viewPos.getX() + sin(-viewDir.getAngle())*delta);
window.SetViewPosition(viewPos);
cout << "view pos: " << viewPos.toString() << endl;
window.DisplayNow();
Sleep(50);
}
if(GetAsyncKeyState(VK_UP)) // the UP arrow was pressed, let's do something
{
delta = .1;
viewPos.setY(viewPos.getY() + cos(-viewDir.getAngle())*delta);
viewPos.setX(viewPos.getX() + sin(-viewDir.getAngle())*delta);
window.SetViewPosition(viewPos);
cout << "view pos: " << viewPos.toString() << endl;
window.DisplayNow();
Sleep(50);
}
if(GetAsyncKeyState(VK_RIGHT)) // the RIGHT arrow was pressed, let's do something
{
delta = .1;
viewDir.setAngle(viewDir.getAngle()+delta);
window.SetViewDirection(viewDir);
cout << "view dir: " << viewDir.getAngle() << endl;
window.DisplayNow();
Sleep(50);
}
if(GetAsyncKeyState(VK_LEFT)) // the LEFT arrow was pressed, let's do something
{
delta = -.1;
viewDir.setAngle(viewDir.getAngle()+delta);
window.SetViewDirection(viewDir);
cout << "view dir: " << viewDir.getAngle() << endl;
window.DisplayNow();
Sleep(50);
}
if(GetAsyncKeyState(VK_ESCAPE))
{
return 1;
}
}
}
Vector3D::Vector3D(const Vector2D &vector, double zpos):xp(vector.x()), yp(vector.y()), zp(zpos){}
Vector2D operator *(float f, Vector2D v) {
return Vector2D(v.X() * f, v.Y() * f);
}
void VideoManager::scale(const Vector2D &scale) const
{
glScalef(scale.getX(), scale.getY(), 1.0f);
}
Vector2D operator /(float f, Vector2D v) {
return Vector2D(f / v.X(), f / v.Y());
}
/*!
*/
bool
Bhv_GoalieChaseBall::is_ball_chase_situation( const PlayerAgent * agent )
{
const WorldModel & wm = agent->world();
if ( wm.gameMode().type() != GameMode::PlayOn )
{
return false;
}
const ServerParam & SP = ServerParam::i();
int self_min = wm.interceptTable()->selfReachCycle();
int opp_min = wm.interceptTable()->opponentReachCycle();
////////////////////////////////////////////////////////////////////////
// ball is in very dangerous area
const Vector2D ball_next_pos = wm.ball().pos() + wm.ball().vel();
if ( ball_next_pos.x < -SP.pitchHalfLength() + 8.0
&& ball_next_pos.absY() < SP.goalHalfWidth() + 3.0 )
{
// exist kickable teammate
// avoid back pass
if ( wm.existKickableTeammate() )
{
dlog.addText( Logger::TEAM,
__FILE__": danger area. exist kickable teammate?" );
return false;
}
else if ( wm.ball().distFromSelf() < 3.0
&& self_min <= 3 )
{
dlog.addText( Logger::TEAM,
__FILE__": danger area. ball is very near." );
return true;
}
else if ( self_min > opp_min + 3
&& opp_min < 7 )
{
dlog.addText( Logger::TEAM,
__FILE__": danger area. opponent may get tha ball faster than me" );
return false;
}
else
{
dlog.addText( Logger::TEAM,
__FILE__": danger area. chase ball" );
return true;
}
}
////////////////////////////////////////////////////////////////////////
// check shoot moving
if ( is_ball_shoot_moving( agent )
&& self_min < opp_min )
{
dlog.addText( Logger::TEAM,
__FILE__": shoot moving. chase ball" );
return true;
}
////////////////////////////////////////////////////////////////////////
// get active interception catch point
const Vector2D my_int_pos = wm.ball().inertiaPoint( wm.interceptTable()->selfReachCycle() );
double pen_thr = wm.ball().distFromSelf() * 0.1 + 1.0;
if ( pen_thr < 1.0 ) pen_thr = 1.0;
if ( my_int_pos.absY() > SP.penaltyAreaHalfWidth() - pen_thr
|| my_int_pos.x > SP.ourPenaltyAreaLineX() - pen_thr )
{
dlog.addText( Logger::TEAM,
__FILE__": intercept point is out of penalty" );
return false;
}
////////////////////////////////////////////////////////////////////////
// Now, I can chase the ball
// check the ball possessor
if ( wm.existKickableTeammate()
&& ! wm.existKickableOpponent() )
{
dlog.addText( Logger::TEAM,
__FILE__": exist kickable player" );
return false;
}
if ( opp_min <= self_min - 2 )
{
dlog.addText( Logger::TEAM,
__FILE__": opponent reach the ball faster than me" );
return false;
}
const double my_dist_to_catch = wm.self().pos().dist( my_int_pos );
double opp_min_dist = 10000.0;
wm.getOpponentNearestTo( my_int_pos, 30, &opp_min_dist );
if ( opp_min_dist < my_dist_to_catch - 2.0 )
{
dlog.addText( Logger::TEAM,
__FILE__": opponent is nearer than me. my_dist=%.2f opp_dist=%.2f",
my_dist_to_catch, opp_min_dist );
return false;
}
dlog.addText( Logger::TEAM,
__FILE__": exist interception point. try chase." );
return true;
}
Vector2D operator +(float f, Vector2D v) {
return Vector2D(v.X() + f, v.Y() + f);
}
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;
}
}
}
Vector2D operator -(float f, Vector2D v) {
return Vector2D(f - v.X(), f - v.Y());
}
void CHLSL_Mesh::CreateCylinder( float size, int subdiv )
{
subdiv /= 2;
delete [] m_Vertices;
delete [] m_Triangles;
int num_verts_u = subdiv + 1;
int num_verts_v = subdiv + 1;
int num_verts_side = num_verts_u * num_verts_v;
int num_verts_top = 1 + num_verts_u - 1;
m_iNumVertices = num_verts_side + num_verts_top * 2;
int num_tris_u = (num_verts_u-1);
int num_tris_v = (num_verts_v-1);
int num_tris_side = num_tris_u * num_tris_v * 2;
int num_tris_top = num_tris_u;
m_iNumTriangles = num_tris_side + num_tris_top * 2;
m_Vertices = new CHLSL_Vertex[ m_iNumVertices ];
m_Triangles = new CHLSL_Triangle[ m_iNumTriangles ];
QAngle rotate( 0, 0, 0 );
float yaw_step = 360.0f / (num_verts_u-1);
float up_step = size * 2.0f / (num_verts_v-1);
float size_radius = size * 0.5f;
Vector fwd, right, up, pos;
/// verts for side
for ( int vert_u = 0; vert_u < num_verts_u; vert_u++ )
{
AngleVectors( rotate, &fwd, &right, &up );
pos = up * size + fwd * size_radius;
for ( int vert_v = 0; vert_v < num_verts_v; vert_v++ )
{
int vindex = vert_u * num_verts_v +
vert_v;
Assert( vindex < m_iNumVertices );
CHLSL_Vertex &vert = m_Vertices[ vindex ];
Q_memcpy( vert.normal, fwd.Base(), sizeof( float ) * 3 );
Q_memcpy( vert.pos, pos.Base(), sizeof( float ) * 3 );
vert.uv[0][0] = vert_u / (float)( num_verts_u - 1 );
vert.uv[0][1] = vert_v / (float)( num_verts_v - 1 );
pos -= up * up_step;
}
rotate.y += yaw_step;
}
/// verts for top/bottom
for ( int v_side = 0; v_side < 2; v_side++ )
{
rotate.y = 0;
float sign = (v_side==1) ? -1.0f : 1.0f;
up.Init( 0, 0, sign );
Vector height = up * size;
for ( int v_u = 0; v_u < num_verts_top; v_u++ )
{
int vindex = num_verts_side +
v_side * num_verts_top +
v_u;
Assert( vindex < m_iNumVertices );
CHLSL_Vertex &vert = m_Vertices[ vindex ];
Vector2D uv;
Vector pos = height;
if ( v_u > 0 )
{
AngleVectors( rotate, &fwd, NULL, NULL );
pos += fwd * size_radius;
uv.x = -sign * fwd.x * 0.5f + 0.5f;
uv.y = fwd.y * 0.5f + 0.5f;
rotate.y += yaw_step * sign;
}
else
uv.Init( 0.5f, 0.5f );
Q_memcpy( vert.pos, pos.Base(), sizeof( float ) * 3 );
Q_memcpy( vert.normal, up.Base(), sizeof( float ) * 3 );
Q_memcpy( vert.uv, uv.Base(), sizeof( float ) * 2 );
}
}
/// tris for side
for ( int tri_u = 0; tri_u < num_tris_u; tri_u++ )
{
for ( int tri_v = 0; tri_v < num_tris_v; tri_v++ )
{
int tri_0 = tri_u * num_tris_v +
tri_v;
int tri_1 = tri_0 + num_tris_side / 2;
Assert( tri_0 < m_iNumTriangles );
Assert( tri_1 < m_iNumTriangles );
CHLSL_Triangle &t0 = m_Triangles[ tri_0 ];
CHLSL_Triangle &t1 = m_Triangles[ tri_1 ];
int v00 = tri_u * num_verts_v +
tri_v;
int v01 = v00 + 1;
int v10 = (tri_u+1) * num_verts_v +
tri_v;
int v11 = v10 + 1;
Assert( v00 < m_iNumVertices );
Assert( v01 < m_iNumVertices );
Assert( v10 < m_iNumVertices );
Assert( v11 < m_iNumVertices );
t0.vertices[ 0 ] = &m_Vertices[ v00 ];
t0.vertices[ 1 ] = &m_Vertices[ v10 ];
t0.vertices[ 2 ] = &m_Vertices[ v01 ];
t1.vertices[ 0 ] = &m_Vertices[ v10 ];
t1.vertices[ 1 ] = &m_Vertices[ v11 ];
t1.vertices[ 2 ] = &m_Vertices[ v01 ];
}
}
/// tris for top
for ( int dir = 0; dir < 2; dir++ )
{
int v_mid = num_verts_side +
dir * num_verts_top;
Assert( v_mid < m_iNumVertices );
for ( int tri_n = 0; tri_n < num_tris_top; tri_n++ )
{
int tIndex = num_tris_side +
dir * num_tris_top +
tri_n;
Assert( tIndex < m_iNumTriangles );
CHLSL_Triangle &t = m_Triangles[ tIndex ];
int v00 = v_mid + tri_n + 1;
int v10 = (tri_n < (num_tris_top-1)) ? v00 + 1 : v_mid + 1;
Assert( v00 < m_iNumVertices );
Assert( v10 < m_iNumVertices );
t.vertices[ 0 ] = &m_Vertices[ v00 ];
t.vertices[ 1 ] = &m_Vertices[ v_mid ];
t.vertices[ 2 ] = &m_Vertices[ v10 ];
}
}
GenerateTangentSpace();
}
bool NavigationDefault::execute(int rid, Position FinalPos, Position &TargetPos)
{
Vector2D currentLoc = robotLoc(rid,true);
Vector2D nearestOBS;
bool foundNearest = false;
//for oppROBOTS
for(int i=0; i < PLAYERS_MAX_NUM ;i++)
{
if(!_wm->oppRobot[i].isValid)
continue;
Vector2D rbTemp = _wm->oppRobot[i].pos.loc;
if(intersectOBS(rbTemp,currentLoc,FinalPos.loc))
{
if(!foundNearest)
{
nearestOBS = rbTemp;
foundNearest = true;
}
else if (currentLoc.dist2(rbTemp) <= currentLoc.dist2(nearestOBS) )
{
nearestOBS = rbTemp;
}
}
}
//for ourROBOT
for(int i=0; i < PLAYERS_MAX_NUM ; i++)
{
if(!_wm->oppRobot[i].isValid)
continue;
if(i==rid)
continue;
Vector2D rbTemp = _wm->ourRobot[i].pos.loc;
if(intersectOBS(rbTemp,currentLoc,FinalPos.loc))
{
if(!foundNearest)
{
nearestOBS = rbTemp;
foundNearest = true;
}
else if (currentLoc.dist2(rbTemp) <= currentLoc.dist2(nearestOBS) )
{
nearestOBS = rbTemp;
}
}
}
//check ball
if(_wm->ball.isValid)
{
Vector2D balltmp = _wm->ball.pos.loc;
if(intersectOBS(balltmp,currentLoc,FinalPos.loc)){
if(!foundNearest)
{
nearestOBS = balltmp;
foundNearest = true;
}
else if (currentLoc.dist2(balltmp) <= currentLoc.dist2(nearestOBS) )
{
nearestOBS = balltmp;
}
}
}
if(!foundNearest)
TargetPos = FinalPos;
else {
Line2D interline(currentLoc,nearestOBS);
Circle2D obs(nearestOBS,300);
Vector2D res1,res2;
obs.intersection(interline.perpendicular(nearestOBS),&res1,&res2);
if(FinalPos.loc.dist2(res1) <= FinalPos.loc.dist2(res2))
{
FinalPos.loc = res1;
TargetPos = FinalPos;
}else
{
FinalPos.loc = res2;
TargetPos = FinalPos;
}
}
return true;
}
