1122 test path smoothing with splines

apps/arena/lib/arena/bots/bot.ex

@@ -92,6 +92,49 @@ defmodule Arena.Bots.Bot do
   defp maybe_update_state_params(state, game_state) do
     state
     |> Map.put_new(:bot_player_id, get_in(game_state, [:client_to_player_map, state.bot_id]))
+    |> maybe_set_obstacles(game_state)
+  end
+
+  defp maybe_set_obstacles(%{bot_state_machine: %{obstacles: nil}} = state, %{obstacles: obstacles})
+       when not is_nil(obstacles) do
+    obstacles =
+      obstacles
+      |> Enum.map(fn {obstacle_id, obstacle} ->
+        obstacle =
+          obstacle
+          |> Map.take([
+            :id,
+            :shape,
+            :position,
+            :radius,
+            :vertices,
+            :speed,
+            :category,
+            :direction,
+            :is_moving,
+            :name
+          ])
+
+        obstacle =
+          obstacle
+          |> Map.put(:position, %{x: obstacle.position.x, y: obstacle.position.y})
+          |> Map.put(
+            :vertices,
+            Enum.map(obstacle.vertices.positions, fn position -> %{x: position.x, y: position.y} end)
+          )
+          |> Map.put(:direction, %{x: obstacle.direction.x, y: obstacle.direction.y})
+          |> Map.put(:shape, get_shape(obstacle.shape))
+          |> Map.put(:category, get_category(obstacle.category))
+
+        {obstacle_id, obstacle}
+      end)
+      |> Map.new()
+
+    %{state | bot_state_machine: %{state.bot_state_machine | obstacles: obstacles}}
+  end
+
+  defp maybe_set_obstacles(state, _game_state) do
+    state
   end
 
   defp generate_bot_name(bot_id), do: {:via, Registry, {BotRegistry, bot_id}}
@@ -166,4 +209,17 @@ defmodule Arena.Bots.Bot do
 
   defp min_decision_delay_ms(), do: 40
   defp max_decision_delay_ms(), do: 60
+
+  defp get_shape("polygon"), do: :polygon
+  defp get_shape("circle"), do: :circle
+  defp get_shape("line"), do: :line
+  defp get_shape("point"), do: :point
+  defp get_shape(_), do: nil
+  defp get_category("player"), do: :player
+  defp get_category("projectile"), do: :projectile
+  defp get_category("obstacle"), do: :obstacle
+  defp get_category("power_up"), do: :power_up
+  defp get_category("pool"), do: :pool
+  defp get_category("item"), do: :item
+  defp get_category("bush"), do: :bush
 end

apps/bot_manager/lib/astar_native.ex

@@ -8,5 +8,7 @@ defmodule AStarNative do
   # When your NIF is loaded, it will override this function.
   def a_star_shortest_path(_from, _to, _collision_grid), do: :erlang.nif_error(:nif_not_loaded)
 
+  def simplify_path(_path, _obstacles), do: :erlang.nif_error(:nif_not_loaded)
+
   def build_collision_grid(_obstacles), do: :erlang.nif_error(:nif_not_loaded)
 end

apps/bot_manager/lib/bot_state_machine.ex

@@ -327,16 +327,42 @@ defmodule BotManager.BotStateMachine do
     shortest_path = AStarNative.a_star_shortest_path(from, to, bot_state_machine.collision_grid)
 
     # If we don't have a path, retry finding new position in map
-    if Enum.empty?(shortest_path) do
-      Map.put(bot_state_machine, :path_towards_position, nil)
-      |> Map.put(:position_to_move_to, nil)
-    else
-      Map.put(bot_state_machine, :position_to_move_to, position_to_move_to)
-      |> Map.put(
-        :path_towards_position,
-        shortest_path
-      )
-      |> Map.put(:last_time_position_changed, :os.system_time(:millisecond))
+    cond do
+      Enum.empty?(shortest_path) ->
+        Map.put(bot_state_machine, :path_towards_position, nil)
+        |> Map.put(:position_to_move_to, nil)
+
+      length(shortest_path) == 1 ->
+        Map.put(bot_state_machine, :position_to_move_to, position_to_move_to)
+        |> Map.put(
+          :path_towards_position,
+          [to]
+        )
+        |> Map.put(:last_time_position_changed, :os.system_time(:millisecond))
+
+      true ->
+        # Replacing first and last points with the actual start and end points
+        shortest_path =
+          ([from] ++ Enum.slice(shortest_path, 1, Enum.count(shortest_path) - 2) ++ [to])
+          |> AStarNative.simplify_path(bot_state_machine.obstacles)
+
+        shortest_path =
+          if System.get_env("TEST_PATHFINDING_SPLINES") == "true" do
+            shortest_path
+            |> SplinePath.smooth_path()
+          else
+            shortest_path
+          end
+
+        # The first point should only be necessary to simplify the path
+        shortest_path = tl(shortest_path)
+
+        Map.put(bot_state_machine, :position_to_move_to, position_to_move_to)
+        |> Map.put(
+          :path_towards_position,
+          shortest_path
+        )
+        |> Map.put(:last_time_position_changed, :os.system_time(:millisecond))
     end
   end
 end

apps/bot_manager/lib/bot_state_machine_checker.ex

@@ -6,7 +6,7 @@ defmodule BotManager.BotStateMachineChecker do
   alias BotManager.Math.Vector
 
   @time_stuck_in_position 400
-  @distance_threshold 100
+  @distance_threshold 150
 
   # Bots will track a player for at most @tracking_timeout_ms milliseconds
   # after which it will transition to another state
@@ -45,7 +45,8 @@ defmodule BotManager.BotStateMachineChecker do
           collision_grid: binary() | nil,
           last_time_state_changed: integer(),
           last_time_tracking_exited: integer(),
-          last_time_attacking_exited: integer()
+          last_time_attacking_exited: integer(),
+          obstacles: list() | nil
         }
 
   defstruct [
@@ -92,7 +93,9 @@ defmodule BotManager.BotStateMachineChecker do
     # The last time the bot exited the tracking state
     :last_time_tracking_exited,
     # The last time the bot exited the tracking state
-    :last_time_attacking_exited
+    :last_time_attacking_exited,
+    # The obstacles on the level
+    :obstacles
   ]
 
   @spec new() :: BotManager.BotStateMachineChecker.t()
@@ -119,7 +122,8 @@ defmodule BotManager.BotStateMachineChecker do
       collision_grid: nil,
       last_time_state_changed: 0,
       last_time_tracking_exited: 0,
-      last_time_attacking_exited: 0
+      last_time_attacking_exited: 0,
+      obstacles: nil
     }
   end
 

apps/bot_manager/lib/math/vector.ex

@@ -3,6 +3,20 @@ defmodule BotManager.Math.Vector do
   Module to handle math operations with vectors
   """
 
+  def add(vector, value) when is_integer(value) or is_float(value) do
+    %{
+      x: vector.x + value,
+      y: vector.y + value
+    }
+  end
+
+  def add(first_vector, second_vector) do
+    %{
+      x: first_vector.x + second_vector.x,
+      y: first_vector.y + second_vector.y
+    }
+  end
+
   def sub(vector, value) when is_integer(value) or is_float(value) do
     %{
       x: vector.x - value,

apps/bot_manager/lib/spline_path.ex

@@ -0,0 +1,87 @@
+defmodule SplinePath do
+  @moduledoc """
+  This module defines methods to generate a spline path out of a waypoint path
+
+  Based on https://qroph.github.io/2018/07/30/smooth-paths-using-catmull-rom-splines.html
+  """
+  alias BotManager.Math.Vector
+
+  @segment_point_amount 5
+  @tension 0.2
+  @alpha 0.5
+
+  def smooth_path(waypoints) when length(waypoints) < 3 do
+    waypoints
+  end
+
+  def smooth_path(waypoints) do
+    first_point = Enum.at(waypoints, 0)
+    second_point = Enum.at(waypoints, 1)
+    last_point = Enum.at(waypoints, -1)
+    second_to_last_point = Enum.at(waypoints, -2)
+
+    first_control_point = Vector.add(first_point, Vector.sub(first_point, second_point) |> Vector.normalize())
+    last_control_point = Vector.add(last_point, Vector.sub(last_point, second_to_last_point) |> Vector.normalize())
+    control_points = [first_control_point] ++ waypoints ++ [last_control_point]
+
+    generate_spline_from_control_points(control_points) ++ [last_point]
+  end
+
+  defp generate_spline_from_control_points(control_points) do
+    Enum.chunk_every(control_points, 4, 1, :discard)
+    |> Enum.map(fn cps -> build_points_for_spline(cps) end)
+    |> List.flatten()
+  end
+
+  defp build_points_for_spline([p0, p1, p2, p3]) do
+    t01 = :math.pow(Vector.distance(p0, p1), @alpha)
+    t12 = :math.pow(Vector.distance(p1, p2), @alpha)
+    t23 = :math.pow(Vector.distance(p2, p3), @alpha)
+
+    m1 =
+      Vector.sub(
+        Vector.mult(Vector.sub(p1, p0), 1 / t01),
+        Vector.mult(Vector.sub(p1, p0), 1 / (t01 + t12))
+      )
+      |> Vector.mult(t12)
+      |> Vector.add(p2)
+      |> Vector.sub(p1)
+      |> Vector.mult(1.0 - @tension)
+
+    m2 =
+      Vector.sub(
+        Vector.mult(Vector.sub(p3, p2), 1 / t23),
+        Vector.mult(Vector.sub(p3, p1), 1 / (t12 + t23))
+      )
+      |> Vector.mult(t12)
+      |> Vector.add(p2)
+      |> Vector.sub(p1)
+      |> Vector.mult(1.0 - @tension)
+
+    a =
+      Vector.sub(p1, p2)
+      |> Vector.mult(2.0)
+      |> Vector.add(m1)
+      |> Vector.add(m2)
+
+    b =
+      Vector.sub(p1, p2)
+      |> Vector.mult(-3.0)
+      |> Vector.sub(m1)
+      |> Vector.sub(m1)
+      |> Vector.sub(m2)
+
+    c = m1
+    d = p1
+
+    # last point will be the next part start so do not add it
+    Enum.map(0..(@segment_point_amount - 1), fn segment_num ->
+      t = segment_num / @segment_point_amount
+
+      d
+      |> Vector.add(Vector.mult(c, t))
+      |> Vector.add(Vector.mult(b, t * t))
+      |> Vector.add(Vector.mult(a, t * t * t))
+    end)
+  end
+end

apps/bot_manager/native/astarnative/src/lib.rs

@@ -80,6 +80,32 @@ fn build_collision_grid<'a>(env: Env<'a>, obstacles: HashMap<u64, Entity>) -> Re
     return Ok(grid.release(env));
 }
 
+#[rustler::nif()]
+fn simplify_path(path: Vec<Position>, obstacles: HashMap<u64, Entity>) -> Vec<Position> {
+    if path.len() < 3 {
+        return path;
+    }
+
+    let obstacles = obstacles.into_values().collect::<Vec<_>>();
+    let mut final_path = vec![path[0]];
+
+    let mut checkpoint_index = 1;
+    while checkpoint_index < path.len() - 1 {
+        let mut line = Entity::new_line(0, vec![final_path[final_path.len() - 1], path[checkpoint_index + 1]]);
+
+        if !line.collides_with(&obstacles).is_empty() {
+            final_path.push(path[checkpoint_index]);
+        }
+
+        checkpoint_index += 1;
+    } 
+
+    final_path.push(path[path.len() - 1]);
+
+    return final_path;
+}
+
+
 #[derive(Clone, Copy, Eq, PartialEq)]
 struct NodeEntry {
     node: (i64, i64),
@@ -175,4 +201,4 @@ fn grid_to_world(grid_pos: &(i64, i64)) -> Position {
     }
 }
 
-rustler::init!("Elixir.AStarNative", [a_star_shortest_path, build_collision_grid]);
+rustler::init!("Elixir.AStarNative", [a_star_shortest_path, build_collision_grid, simplify_path]);