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; }