Merge pull request #14 from Immortals-Robotics/dev/obstacle-cleanup

Obstacle code cleanup

source/common/math/geom/rect.h

@@ -12,6 +12,9 @@ struct Rect
           max(std::max(t_p1.x, t_p2.x), std::max(t_p1.y, t_p2.y))
     {}
 
+    Rect(const Vec2 t_p, const float t_w, const float t_h) : Rect(t_p, t_p + Vec2(t_w, t_h))
+    {}
+
     bool inside(Vec2 t_point) const;
 
     bool insideOffset(const Vec2 t_point, float offset) const;

source/soccer/CMakeLists.txt

@@ -10,7 +10,6 @@ set(HEADER_FILES
 	errt/errt.h
 	errt/tree.h
 
-	obstacle/obstacle_new.h
 	obstacle/obstacle.h
 
 	helpers/one_touch_detector.h
@@ -35,7 +34,6 @@ set(SOURCE_FILES
 	obstacle/obstacle_circle.cpp
 	obstacle/obstacle_map.cpp
 	obstacle/obstacle_rect.cpp
-	obstacle/obstacle.cpp
 
 	helpers/attack_fucking_angle.cpp
 	helpers/ball_is_goaling.cpp

source/soccer/dss/dss.cpp

@@ -148,7 +148,7 @@ Common::Vec2 Dss::ComputeSafeMotion(const int robot_num, const Common::Vec2 &mot
     Common::Vec2              a_cmd;
     const Common::RobotState &state = own_robots[robot_num].State;
 
-    if (state.seenState == Common::CompletelyOut || IsInObstacle(state.Position))
+    if (state.seenState == Common::CompletelyOut || obs_map.isInObstacle(state.Position))
     {
         a_cmd = target_a_cmd;
     }

source/soccer/dss/parabolic.cpp

@@ -53,7 +53,7 @@ bool Parabolic::HasStaticOverlap(const Parabolic &a)
     {
         const float        t = check_t[t_idx];
         const Common::Vec2 p = a.Evaluate(t);
-        if (IsInObstacle(p))
+        if (obs_map.isInObstacle(p))
         {
             return true;
         }

source/soccer/errt/errt.cpp

@@ -35,7 +35,7 @@ void Planner::init(Common::Vec2 init, Common::Vec2 final, float step)
     tree.reset();
     tree.AddNode(init_state, NULL);
 
-    started_in_obs = IsInObstacle(init);
+    started_in_obs = obs_map.isInObstacle(init);
 
     /*init_state = init;
     final_state = nearest_free ( nearest_free ( final ) );
@@ -44,7 +44,7 @@ void Planner::init(Common::Vec2 init, Common::Vec2 final, float step)
     tree.reset ( );
     tree.AddNode ( init_state , NULL );
 
-    if ( IsInObstacle ( init_state ) )
+    if ( obs_map.isInObstacle ( init_state ) )
     {
             init = nearest_free ( nearest_free ( init ) );
             tree.AddNode ( init , tree.GetNode ( 0 ) );
@@ -72,12 +72,12 @@ Common::Vec2 Planner::nearest_free(Common::Vec2 state)
     state.x = std::min(678.0f, std::max(52.0f, state.x));
     state.y = std::min(478.0f, std::max(52.0f, state.y));
 
-    if (!IsInObstacle(state))
+    if (!obs_map.isInObstacle(state))
         return state;
 
     for (int i = state.x; i < 678; i++)
     {
-        if (!IsInObstacle(Common::Vec2(i, state.y)))
+        if (!obs_map.isInObstacle(Common::Vec2(i, state.y)))
         {
             r = i - state.x;
             break;
@@ -86,7 +86,7 @@ Common::Vec2 Planner::nearest_free(Common::Vec2 state)
 
     for (int i = state.x; i > 52; i--)
     {
-        if (!IsInObstacle(Common::Vec2(i, state.y)))
+        if (!obs_map.isInObstacle(Common::Vec2(i, state.y)))
         {
             l = i - state.x;
             break;
@@ -95,7 +95,7 @@ Common::Vec2 Planner::nearest_free(Common::Vec2 state)
 
     for (int i = state.y; i < 478; i++)
     {
-        if (!IsInObstacle(Common::Vec2(state.x, i)))
+        if (!obs_map.isInObstacle(Common::Vec2(state.x, i)))
         {
             u = i - state.y;
             break;
@@ -104,7 +104,7 @@ Common::Vec2 Planner::nearest_free(Common::Vec2 state)
 
     for (int i = state.y; i > 52; i--)
     {
-        if (!IsInObstacle(Common::Vec2(state.x, i)))
+        if (!obs_map.isInObstacle(Common::Vec2(state.x, i)))
         {
             d = i - state.y;
             break;
@@ -127,7 +127,7 @@ Common::Vec2 Planner::nearest_free(Common::Vec2 state)
     // ( 529 , std::max ( 0 , state.y + sgn ( y ) * abs ( x ) ) ) );
     Common::Vec2 ans = Common::Vec2(state.x + Common::sign(x) * std::abs(y), state.y + Common::sign(y) * std::abs(x));
 
-    // if ( IsInObstacle ( ans ) )
+    // if ( obs_map.isInObstacle ( ans ) )
     //	return nearest_free ( ans );
 
     float coss, sinn;
@@ -136,7 +136,7 @@ Common::Vec2 Planner::nearest_free(Common::Vec2 state)
 
     Common::Vec2 current = ans;
 
-    while ((current.distanceTo(state) > 2) && (!IsInObstacle(current)))
+    while ((current.distanceTo(state) > 2) && (!obs_map.isInObstacle(current)))
     {
         ans = current;
 
@@ -150,7 +150,7 @@ Common::Vec2 Planner::nearest_free(Common::Vec2 state)
 Common::Vec2 Planner::nearest_free_prob(Common::Vec2 state)
 {
     const float acceptable_free_dis = 50;
-    if (!IsInObstacle(state))
+    if (!obs_map.isInObstacle(state))
         return state;
 
     Common::Vec2 ans    = state;
@@ -159,7 +159,7 @@ Common::Vec2 Planner::nearest_free_prob(Common::Vec2 state)
     for (int i = 0; i < 1000; i++)
     {
         Common::Vec2 newRndPoint = random_state();
-        if ((!IsInObstacle(newRndPoint)) && state.distanceTo(newRndPoint) < minDis)
+        if ((!obs_map.isInObstacle(newRndPoint)) && state.distanceTo(newRndPoint) < minDis)
         {
             ans    = newRndPoint;
             minDis = state.distanceTo(newRndPoint);
@@ -220,10 +220,10 @@ Node *Planner::extend(Node *s, Common::Vec2 &target)
     //         return NULL;
 
     // collision check
-    if (IsInObstacle(new_state))
+    if (obs_map.isInObstacle(new_state))
         return NULL;
 
-    if (collisionDetect(new_state, s->state))
+    if (obs_map.collisionDetect(new_state, s->state))
         return NULL;
 
     return tree.AddNode(new_state, s);
@@ -281,7 +281,7 @@ bool Planner::IsReached()
 Common::Vec2 Planner::Plan()
 {
     // return final_state;
-    if (!collisionDetect(init_state, final_state))
+    if (!obs_map.collisionDetect(init_state, final_state))
     {
         tree.AddNode(final_state, tree.NearestNeighbour(init_state));
     }
@@ -296,7 +296,7 @@ Common::Vec2 Planner::Plan()
             new_s = extend(tree.NearestNeighbour(r), r);
             if (new_s)
             {
-                /*if ( !collisionDetect ( new_s->state , final_state ) )
+                /*if ( !obs_map.collisionDetect ( new_s->state , final_state ) )
                 {
                     for (; ( !IsReached ( ) ) && ( new_s ) && (i < MAX_NODES ) ; i ++ )
                                 new_s = extend ( new_s , final_state );
@@ -306,7 +306,7 @@ Common::Vec2 Planner::Plan()
             // while ( !extend ( tree.NearestNeighbour ( r ) , r ) )
             //	r = choose_target ( );
         }
-        if ((IsReached()) && (!IsInObstacle(final_state)) &&
+        if ((IsReached()) && (!obs_map.isInObstacle(final_state)) &&
             (final_state.distanceTo(tree.NearestNeighbour(final_state)->state) > 1))
         {
             tree.AddNode(final_state, tree.NearestNeighbour(final_state));
@@ -315,7 +315,7 @@ Common::Vec2 Planner::Plan()
 
     SetWayPoints();
 
-    // if ( ( ! IsInObstacle ( init_state ) ) && ( GetWayPointNum() > 2 ) )
+    // if ( ( ! obs_map.isInObstacle ( init_state ) ) && ( GetWayPointNum() > 2 ) )
     optimize_tree();
 
     Common::Vec2 ans;
@@ -345,7 +345,7 @@ void Planner::optimize_tree()
 {
     for (int i = 0; i < GetWayPointNum(); i++)
     {
-        if (collisionDetect(waypoint[i], waypoint[GetWayPointNum() - 1]) == false)
+        if (obs_map.collisionDetect(waypoint[i], waypoint[GetWayPointNum() - 1]) == false)
         {
             Common::Vec2 tmp               = waypoint[i + 1];
             waypoint[i + 1]                = waypoint[GetWayPointNum() - 1];

source/soccer/errt/set_obstacles.cpp

@@ -26,15 +26,15 @@ void Ai::ERRTSetObstacles(int robot_num, bool bll, bool field)
 
     const float current_robot_radius = calculateRobotRadius(OwnRobot[robot_num].State);
 
-    clear_map();
+    obs_map.resetMap();
 
     // own
     for (int i = 0; i < Common::Setting::kMaxOnFieldTeamRobots; i++)
     {
         if ((OwnRobot[i].State.seenState != Common::CompletelyOut) && (i != robot_num) &&
             (OwnRobot[i].State.vision_id != OwnRobot[robot_num].State.vision_id))
         {
-            AddCircle(OwnRobot[i].State.Position.x, OwnRobot[i].State.Position.y, current_robot_radius + robotRadius);
+            obs_map.addCircle({OwnRobot[i].State.Position, current_robot_radius + robotRadius});
             // Common::debug().drawCircle(OwnRobot[i].State.Position,ownRobotRadius + (!dribble)*ownRobotRadius,Cyan);
         }
     }
@@ -46,32 +46,38 @@ void Ai::ERRTSetObstacles(int robot_num, bool bll, bool field)
         {
             const float radius = calculateRobotRadius(OppRobot[i]);
 
-            AddCircle(OppRobot[i].Position.x, OppRobot[i].Position.y, radius + current_robot_radius);
+            obs_map.addCircle({OppRobot[i].Position, radius + current_robot_radius});
             // Common::debug().drawCircle(OppRobot[i].Position,ownRobotRadius + (!dribble)*ownRobotRadius,Cyan);
         }
     }
 
     if (bll || !REF_playState->allowedNearBall())
     {
-        AddCircle(ball.Position.x, ball.Position.y, ballAreaRadius + current_robot_radius);
+        obs_map.addCircle({ball.Position, ballAreaRadius + current_robot_radius});
     }
 
     const float penalty_area_half_width = penalty_area_width / 2.0f;
 
     if (ourPenalty)
     {
-        AddRectangle(side * (field_width + penaltyAreaExtensionBehindGoal),
-                     -(penalty_area_half_width + current_robot_radius),
-                     -side * (penaltyAreaExtensionBehindGoal + current_robot_radius + penalty_area_r),
-                     penalty_area_width + 2 * current_robot_radius);
+        const Common::Vec2 start{side * (field_width + penaltyAreaExtensionBehindGoal),
+                                 -(penalty_area_half_width + current_robot_radius)};
+
+        const float w = -side * (penaltyAreaExtensionBehindGoal + current_robot_radius + penalty_area_r);
+        const float h = penalty_area_width + 2 * current_robot_radius;
+
+        obs_map.addRectangle({start, w, h});
     }
 
     if (oppPenalty)
     {
-        AddRectangle(-side * (field_width + penaltyAreaExtensionBehindGoal),
-                     -(penalty_area_half_width + current_robot_radius),
-                     side * (penaltyAreaExtensionBehindGoal + current_robot_radius + penalty_area_r),
-                     penalty_area_width + 2 * current_robot_radius);
+        const Common::Vec2 start{-side * (field_width + penaltyAreaExtensionBehindGoal),
+                                 -(penalty_area_half_width + current_robot_radius)};
+
+        const float w = side * (penaltyAreaExtensionBehindGoal + current_robot_radius + penalty_area_r);
+        const float h = penalty_area_width + 2 * current_robot_radius;
+
+        obs_map.addRectangle({start, w, h});
     }
 
     if (oppPenaltyBig)
@@ -80,12 +86,13 @@ void Ai::ERRTSetObstacles(int robot_num, bool bll, bool field)
         const float big_penalty_area_w      = penalty_area_width + bigPenaltyAddition;
         const float penalty_area_half_width = big_penalty_area_w / 2.0f;
 
-        AddRectangle(-side * (field_width + penaltyAreaExtensionBehindGoal),
-                     -(penalty_area_half_width + current_robot_radius),
-                     side * (penaltyAreaExtensionBehindGoal + current_robot_radius + big_penalty_area_r),
-                     big_penalty_area_w + 2 * current_robot_radius);
-        //		Common::debug().drawRect( Common::Vec2(-side*(field_width+185.0f) , -penalty_circle_center_y - 200) ,
-        // side*(385.0f+big_penalty_area_r) , penalty_area_width + 400,Purple );
+        const Common::Vec2 start{-side * (field_width + penaltyAreaExtensionBehindGoal),
+                                 -(penalty_area_half_width + current_robot_radius)};
+
+        const float w = side * (penaltyAreaExtensionBehindGoal + current_robot_radius + big_penalty_area_r);
+        const float h = big_penalty_area_w + 2 * current_robot_radius;
+
+        obs_map.addRectangle({start, w, h});
     }
 
     // avoid the line between the ball and the placement point
@@ -98,19 +105,24 @@ void Ai::ERRTSetObstacles(int robot_num, bool bll, bool field)
         {
             const float        t        = (float) i / (float) ball_obs_count;
             const Common::Vec2 ball_obs = ball.Position + ball_line * t;
-            AddCircle(ball_obs.x, ball_obs.y, ballAreaRadius + current_robot_radius);
+            obs_map.addCircle({ball_obs, ballAreaRadius + current_robot_radius});
         }
     }
 }
 
 void Ai::ERRTSetGkClearObstacles(int robot_num)
 {
-    clear_map();
+    obs_map.resetMap();
 
     // our penalty area
     static constexpr float area_extension_size     = 200.0f;
     const float            penalty_area_half_width = penalty_area_width / 2.0f;
-    AddRectangle(side * field_width, -(penalty_area_half_width + area_extension_size),
-                 -side * (area_extension_size + penalty_area_r), penalty_area_width + 2 * area_extension_size);
+
+    const Common::Vec2 start{side * field_width, -(penalty_area_half_width + area_extension_size)};
+
+    const float w = -side * (area_extension_size + penalty_area_r);
+    const float h = penalty_area_width + 2 * area_extension_size;
+
+    obs_map.addRectangle({start, w, h});
 }
 } // namespace Tyr::Soccer