Game AI

#include <cmath>
#include <cstdlib>
#include <utility>
#include <vector>

struct Vec3 {
    float x = 0, y = 0, z = 0;
    Vec3 operator+(const Vec3& o) const { return {x + o.x, y + o.y, z + o.z}; }
    Vec3 operator-(const Vec3& o) const { return {x - o.x, y - o.y, z - o.z}; }
    Vec3 operator*(float s) const { return {x * s, y * s, z * s}; }
    float length() const { return std::sqrt(x*x + y*y + z*z); }
    Vec3 normalised() const {
        float l = length();
        return l > 0 ? (*this) * (1.0f / l) : Vec3{};
    }
};

struct SteeringOutput {
    Vec3 linear{};
    float angular = 0;
};

class SteeringBehaviours {
    static inline float wanderAngle = 0.0f;
public:
    static SteeringOutput seek(Vec3 pos, Vec3 target, float maxSpeed, Vec3 velocity) {
        Vec3 desired = (target - pos).normalised() * maxSpeed;
        return {desired - velocity, 0};
    }

    static SteeringOutput flee(Vec3 pos, Vec3 target, float maxSpeed, Vec3 velocity) {
        Vec3 desired = (pos - target).normalised() * maxSpeed;
        return {desired - velocity, 0};
    }

    static SteeringOutput arrive(Vec3 pos, Vec3 target, float maxSpeed,
                                 float slowRadius, Vec3 velocity) {
        Vec3 toTarget = target - pos;
        float dist = toTarget.length();
        if (dist < 0.01f) return {};
        float speed = dist < slowRadius ? maxSpeed * (dist / slowRadius) : maxSpeed;
        Vec3 desired = toTarget.normalised() * speed;
        return {desired - velocity, 0};
    }

    static SteeringOutput wander(Vec3 pos, Vec3 forward, float radius, float distance,
                                 float jitter, float maxSpeed, Vec3 velocity) {
        Vec3 circleCentre = pos + forward * distance;
        Vec3 offset{std::cos(wanderAngle) * radius, 0, std::sin(wanderAngle) * radius};
        wanderAngle += (static_cast<float>(std::rand()) / RAND_MAX * 2 - 1) * jitter;
        return seek(pos + offset, circleCentre + offset, maxSpeed, velocity);
    }

    static SteeringOutput combine(const std::vector<std::pair<float, SteeringOutput>>& behaviours) {
        Vec3 linear{};
        float angular = 0, totalWeight = 0;
        for (const auto& [weight, b] : behaviours) {
            linear = linear + b.linear * weight;
            angular += b.angular * weight;
            totalWeight += weight;
        }
        if (totalWeight > 0) {
            linear = linear * (1.0f / totalWeight);
            angular /= totalWeight;
        }
        return {linear, angular};
    }
};

import java.util.*;
import java.util.concurrent.ThreadLocalRandom;

public final class Vec3 {
    public final float x, y, z;

    public Vec3() { this(0, 0, 0); }
    public Vec3(float x, float y, float z) { this.x = x; this.y = y; this.z = z; }
    public Vec3(float v) { this(v, v, v); }

    public Vec3 add(Vec3 o) { return new Vec3(x + o.x, y + o.y, z + o.z); }
    public Vec3 sub(Vec3 o) { return new Vec3(x - o.x, y - o.y, z - o.z); }
    public Vec3 mul(float s) { return new Vec3(x * s, y * s, z * s); }
    public Vec3 div(float s) { return new Vec3(x / s, y / s, z / s); }
    public float length() { return (float)Math.sqrt(x*x + y*y + z*z); }
    public Vec3 normalized() { float l = length(); return l > 0 ? div(l) : new Vec3(); }
}

public final class SteeringOutput {
    public final Vec3 linear;
    public final float angular;
    public SteeringOutput() { this(new Vec3(), 0); }
    public SteeringOutput(Vec3 linear, float angular) { this.linear = linear; this.angular = angular; }
}

public class SteeringBehaviors {
    private static final ThreadLocalRandom RNG = ThreadLocalRandom.current();
    private static final ThreadLocal<Float> WANDER_ANGLE = ThreadLocal.withInitial(() -> 0.0f);

    public static SteeringOutput seek(Vec3 pos, Vec3 target, float maxSpeed, Vec3 velocity) {
        Vec3 desired = target.sub(pos).normalized().mul(maxSpeed);
        return new SteeringOutput(desired.sub(velocity), 0);
    }

    public static SteeringOutput flee(Vec3 pos, Vec3 target, float maxSpeed, Vec3 velocity) {
        Vec3 desired = pos.sub(target).normalized().mul(maxSpeed);
        return new SteeringOutput(desired.sub(velocity), 0);
    }

    public static SteeringOutput arrive(Vec3 pos, Vec3 target, float maxSpeed, float slowRadius, Vec3 velocity) {
        Vec3 toTarget = target.sub(pos);
        float dist = toTarget.length();
        if (dist < 0.01f) return new SteeringOutput();
        float speed = (dist < slowRadius) ? maxSpeed * (dist / slowRadius) : maxSpeed;
        Vec3 desired = toTarget.normalized().mul(speed);
        return new SteeringOutput(desired.sub(velocity), 0);
    }

    public static SteeringOutput wander(Vec3 pos, Vec3 forward, float radius, float distance, float jitter, float maxSpeed, Vec3 velocity) {
        float angle = WANDER_ANGLE.get();
        Vec3 circleCenter = pos.add(forward.mul(distance));
        Vec3 offset = new Vec3(
            (float)Math.cos(angle) * radius,
            0,
            (float)Math.sin(angle) * radius
        );
        WANDER_ANGLE.set(angle + (float)(ThreadLocalRandom.current().nextDouble() * 2 - 1) * jitter);
        return seek(pos.add(offset), circleCenter.add(offset), maxSpeed, velocity);
    }

    public static SteeringOutput combine(List<Map.Entry<Float, SteeringOutput>> behaviors) {
        Vec3 linear = new Vec3();
        float angular = 0, totalWeight = 0;
        for (var entry : behaviors) {
            float weight = entry.getKey();
            SteeringOutput b = entry.getValue();
            linear = linear.add(b.linear.mul(weight));
            angular += b.angular * weight;
            totalWeight += weight;
        }
        if (totalWeight > 0) {
            linear = linear.mul(1.0f / totalWeight);
            angular /= totalWeight;
        }
        return new SteeringOutput(linear, angular);
    }
}

using System;
using System.Collections.Generic;

public readonly struct Vec3
{
    public readonly float X, Y, Z;
    public Vec3(float x, float y, float z) { X = x; Y = y; Z = z; }

    public static Vec3 operator +(Vec3 a, Vec3 b) => new(a.X + b.X, a.Y + b.Y, a.Z + b.Z);
    public static Vec3 operator -(Vec3 a, Vec3 b) => new(a.X - b.X, a.Y - b.Y, a.Z - b.Z);
    public static Vec3 operator *(Vec3 a, float s) => new(a.X * s, a.Y * s, a.Z * s);
    public float Length() => MathF.Sqrt(X * X + Y * Y + Z * Z);
    public Vec3 Normalised() { var l = Length(); return l > 0 ? this * (1f / l) : default; }
}

public readonly struct SteeringOutput
{
    public readonly Vec3 Linear;
    public readonly float Angular;
    public SteeringOutput(Vec3 linear, float angular) { Linear = linear; Angular = angular; }
}

public static class SteeringBehaviours
{
    private static readonly Random Rng = new();
    private static float _wanderAngle;

    public static SteeringOutput Seek(Vec3 pos, Vec3 target, float maxSpeed, Vec3 velocity)
    {
        var desired = (target - pos).Normalised() * maxSpeed;
        return new SteeringOutput(desired - velocity, 0);
    }

    public static SteeringOutput Flee(Vec3 pos, Vec3 target, float maxSpeed, Vec3 velocity)
    {
        var desired = (pos - target).Normalised() * maxSpeed;
        return new SteeringOutput(desired - velocity, 0);
    }

    public static SteeringOutput Arrive(Vec3 pos, Vec3 target, float maxSpeed,
                                        float slowRadius, Vec3 velocity)
    {
        var toTarget = target - pos;
        var dist = toTarget.Length();
        if (dist < 0.01f) return default;
        var speed = dist < slowRadius ? maxSpeed * (dist / slowRadius) : maxSpeed;
        var desired = toTarget.Normalised() * speed;
        return new SteeringOutput(desired - velocity, 0);
    }

    public static SteeringOutput Wander(Vec3 pos, Vec3 forward, float radius, float distance,
                                        float jitter, float maxSpeed, Vec3 velocity)
    {
        var circleCentre = pos + forward * distance;
        var offset = new Vec3(MathF.Cos(_wanderAngle) * radius, 0,
                              MathF.Sin(_wanderAngle) * radius);
        _wanderAngle += ((float)Rng.NextDouble() * 2 - 1) * jitter;
        return Seek(pos + offset, circleCentre + offset, maxSpeed, velocity);
    }

    public static SteeringOutput Combine(IEnumerable<(float Weight, SteeringOutput Output)> behaviours)
    {
        var linear = default(Vec3);
        float angular = 0, totalWeight = 0;
        foreach (var (weight, b) in behaviours)
        {
            linear = linear + b.Linear * weight;
            angular += b.Angular * weight;
            totalWeight += weight;
        }
        if (totalWeight > 0)
        {
            linear = linear * (1f / totalWeight);
            angular /= totalWeight;
        }
        return new SteeringOutput(linear, angular);
    }
}

Vec3 = Struct.new(:x, :y, :z) do
  def self.zero
    new(0.0, 0.0, 0.0)
  end

  def +(other)
    Vec3.new(x + other.x, y + other.y, z + other.z)
  end

  def -(other)
    Vec3.new(x - other.x, y - other.y, z - other.z)
  end

  def *(scalar)
    Vec3.new(x * scalar, y * scalar, z * scalar)
  end

  def length
    Math.sqrt(x * x + y * y + z * z)
  end

  def normalised
    l = length
    l > 0 ? self * (1.0 / l) : Vec3.zero
  end
end

SteeringOutput = Struct.new(:linear, :angular) do
  def self.none
    new(Vec3.zero, 0.0)
  end
end

module SteeringBehaviours
  module_function

  def seek(pos, target, max_speed, velocity)
    desired = (target - pos).normalised * max_speed
    SteeringOutput.new(desired - velocity, 0.0)
  end

  def flee(pos, target, max_speed, velocity)
    desired = (pos - target).normalised * max_speed
    SteeringOutput.new(desired - velocity, 0.0)
  end

  def arrive(pos, target, max_speed, slow_radius, velocity)
    to_target = target - pos
    dist = to_target.length
    return SteeringOutput.none if dist < 0.01

    speed = dist < slow_radius ? max_speed * (dist / slow_radius) : max_speed
    desired = to_target.normalised * speed
    SteeringOutput.new(desired - velocity, 0.0)
  end

  def wander(pos, forward, radius, distance, jitter, max_speed, velocity)
    @wander_angle ||= 0.0
    circle_centre = pos + forward * distance
    offset = Vec3.new(Math.cos(@wander_angle) * radius, 0.0,
                      Math.sin(@wander_angle) * radius)
    @wander_angle += (rand * 2 - 1) * jitter
    seek(pos + offset, circle_centre + offset, max_speed, velocity)
  end

  # behaviours: array of [weight, SteeringOutput] pairs
  def combine(behaviours)
    linear = Vec3.zero
    angular = 0.0
    total_weight = 0.0
    behaviours.each do |weight, b|
      linear += b.linear * weight
      angular += b.angular * weight
      total_weight += weight
    end
    if total_weight > 0
      linear *= 1.0 / total_weight
      angular /= total_weight
    end
    SteeringOutput.new(linear, angular)
  end
end

#include <queue>
#include <unordered_map>
#include <vector>

struct NavPoly {
    std::vector<Vec3> vertices;   // CCW order
    std::vector<int> neighbours;  // Adjacent poly indices
    int flags = 0;                // Walk, jump, climb, water
    int area = 0;                 // Ground, road, grass, etc.
};

class NavMesh {
public:
    std::vector<NavPoly> polys;
    std::vector<Vec3> vertices;

    // Find which polygon contains the point (simplified).
    int locatePoly(const Vec3& point) const {
        for (int i = 0; i < static_cast<int>(polys.size()); ++i)
            if (pointInPoly(point, polys[i])) return i;
        return -1;
    }

    std::vector<int> findPath(const Vec3& start, const Vec3& end, int agentFlags) const {
        int startPoly = locatePoly(start);
        int endPoly = locatePoly(end);
        if (startPoly < 0 || endPoly < 0) return {};

        struct Node { float f; int poly; float g; int parent; };
        auto cmp = [](const Node& a, const Node& b) { return a.f > b.f; };
        std::priority_queue<Node, std::vector<Node>, decltype(cmp)> open(cmp);
        std::unordered_map<int, Node> closed;
        std::unordered_map<int, float> gScores;

        open.push({0, startPoly, 0.0f, -1});
        gScores[startPoly] = 0.0f;

        while (!open.empty()) {
            Node current = open.top();
            open.pop();
            if (current.poly == endPoly) return reconstructPath(closed, current);
            if (closed.count(current.poly)) continue;
            closed[current.poly] = current;

            for (int neighbour : polys[current.poly].neighbours) {
                // Filter by agent capabilities
                if (polys[neighbour].flags & agentFlags) continue;

                float g = current.g + polyDistance(current.poly, neighbour);
                auto it = gScores.find(neighbour);
                if (it == gScores.end() || g < it->second) {
                    gScores[neighbour] = g;
                    float h = heuristic(neighbour, endPoly);
                    open.push({g + h, neighbour, g, current.poly});
                }
            }
        }
        return {};  // No path
    }

private:
    bool pointInPoly(const Vec3&, const NavPoly&) const { return true; }  // Placeholder
    float polyDistance(int, int) const { return 1.0f; }                   // Placeholder
    float heuristic(int, int) const { return 1.0f; }                      // Placeholder

    std::vector<int> reconstructPath(const std::unordered_map<int, Node>& closed, Node node) const {
        std::vector<int> path{node.poly};
        while (node.parent != -1) {
            node = closed.at(node.parent);
            path.push_back(node.poly);
        }
        return {path.rbegin(), path.rend()};
    }
};

import java.util.*;

public class NavPoly {
    public List<Vec3> vertices = new ArrayList<>();   // CCW order
    public List<Integer> neighbours = new ArrayList<>(); // Adjacent poly indices
    public int flags = 0;   // Walk, jump, climb, water
    public int area = 0;    // Ground, road, grass, etc.
}

public class NavMesh {
    public List<NavPoly> polys = new ArrayList<>();
    public List<Vec3> vertices = new ArrayList<>();

    private record Node(float f, int poly, float g, int parent) {}

    /** Find which polygon contains the point (simplified). */
    public int locatePoly(Vec3 point) {
        for (int i = 0; i < polys.size(); i++)
            if (pointInPoly(point, polys.get(i))) return i;
        return -1;
    }

    public List<Integer> findPath(Vec3 start, Vec3 end, int agentFlags) {
        int startPoly = locatePoly(start);
        int endPoly = locatePoly(end);
        if (startPoly < 0 || endPoly < 0) return List.of();

        PriorityQueue<Node> open = new PriorityQueue<>(Comparator.comparingDouble(Node::f));
        Map<Integer, Node> closed = new HashMap<>();
        Map<Integer, Float> gScores = new HashMap<>();

        open.add(new Node(0, startPoly, 0f, -1));
        gScores.put(startPoly, 0f);

        while (!open.isEmpty()) {
            Node current = open.poll();
            if (current.poly() == endPoly) return reconstructPath(closed, current);
            if (closed.containsKey(current.poly())) continue;
            closed.put(current.poly(), current);

            for (int neighbour : polys.get(current.poly()).neighbours) {
                // Filter by agent capabilities
                if ((polys.get(neighbour).flags & agentFlags) != 0) continue;

                float g = current.g() + polyDistance(current.poly(), neighbour);
                Float best = gScores.get(neighbour);
                if (best == null || g < best) {
                    gScores.put(neighbour, g);
                    float h = heuristic(neighbour, endPoly);
                    open.add(new Node(g + h, neighbour, g, current.poly()));
                }
            }
        }
        return List.of();  // No path
    }

    private boolean pointInPoly(Vec3 point, NavPoly poly) { return true; }  // Placeholder
    private float polyDistance(int a, int b) { return 1f; }                 // Placeholder
    private float heuristic(int a, int b) { return 1f; }                    // Placeholder

    private List<Integer> reconstructPath(Map<Integer, Node> closed, Node node) {
        List<Integer> path = new ArrayList<>(List.of(node.poly()));
        while (node.parent() != -1) {
            node = closed.get(node.parent());
            path.add(node.poly());
        }
        Collections.reverse(path);
        return path;
    }
}

using System.Collections.Generic;

public class NavPoly
{
    public List<Vec3> Vertices { get; } = new();     // CCW order
    public List<int> Neighbours { get; } = new();    // Adjacent poly indices
    public int Flags { get; set; }                   // Walk, jump, climb, water
    public int Area { get; set; }                    // Ground, road, grass, etc.
}

public class NavMesh
{
    public List<NavPoly> Polys { get; } = new();
    public List<Vec3> Vertices { get; } = new();

    private readonly record struct Node(float F, int Poly, float G, int Parent);

    // Find which polygon contains the point (simplified).
    public int LocatePoly(Vec3 point)
    {
        for (var i = 0; i < Polys.Count; i++)
            if (PointInPoly(point, Polys[i])) return i;
        return -1;
    }

    public List<int> FindPath(Vec3 start, Vec3 end, int agentFlags)
    {
        var startPoly = LocatePoly(start);
        var endPoly = LocatePoly(end);
        if (startPoly < 0 || endPoly < 0) return new List<int>();

        var open = new PriorityQueue<Node, float>();
        var closed = new Dictionary<int, Node>();
        var gScores = new Dictionary<int, float> { [startPoly] = 0f };

        open.Enqueue(new Node(0, startPoly, 0f, -1), 0);

        while (open.Count > 0)
        {
            var current = open.Dequeue();
            if (current.Poly == endPoly) return ReconstructPath(closed, current);
            if (closed.ContainsKey(current.Poly)) continue;
            closed[current.Poly] = current;

            foreach (var neighbour in Polys[current.Poly].Neighbours)
            {
                // Filter by agent capabilities
                if ((Polys[neighbour].Flags & agentFlags) != 0) continue;

                var g = current.G + PolyDistance(current.Poly, neighbour);
                if (!gScores.TryGetValue(neighbour, out var best) || g < best)
                {
                    gScores[neighbour] = g;
                    var h = Heuristic(neighbour, endPoly);
                    open.Enqueue(new Node(g + h, neighbour, g, current.Poly), g + h);
                }
            }
        }
        return new List<int>();  // No path
    }

    private bool PointInPoly(Vec3 point, NavPoly poly) => true;  // Placeholder
    private float PolyDistance(int a, int b) => 1f;              // Placeholder
    private float Heuristic(int a, int b) => 1f;                 // Placeholder

    private List<int> ReconstructPath(Dictionary<int, Node> closed, Node node)
    {
        var path = new List<int> { node.Poly };
        while (node.Parent != -1)
        {
            node = closed[node.Parent];
            path.Add(node.Poly);
        }
        path.Reverse();
        return path;
    }
}

NavPoly = Struct.new(:vertices, :neighbours, :flags, :area) do
  def initialize(vertices: [], neighbours: [], flags: 0, area: 0)
    super(vertices, neighbours, flags, area)
  end
end

Node = Struct.new(:f, :poly, :g, :parent)

class NavMesh
  attr_reader :polys, :vertices

  def initialize
    @polys = []
    @vertices = []
  end

  # Find which polygon contains the point (simplified).
  def locate_poly(point)
    polys.each_with_index do |poly, i|
      return i if point_in_poly?(point, poly)
    end
    -1
  end

  def find_path(start, finish, agent_flags)
    start_poly = locate_poly(start)
    end_poly = locate_poly(finish)
    return [] if start_poly.negative? || end_poly.negative?

    # A* on the polygon graph (open list kept sorted by f; a heap
    # such as the pqueue gem would be the production choice)
    open_set = [Node.new(0.0, start_poly, 0.0, -1)]
    closed = {}
    g_scores = { start_poly => 0.0 }

    until open_set.empty?
      current = open_set.min_by(&:f)
      open_set.delete(current)

      return reconstruct_path(closed, current) if current.poly == end_poly
      next if closed.key?(current.poly)

      closed[current.poly] = current

      polys[current.poly].neighbours.each do |neighbour|
        # Filter by agent capabilities
        next unless (polys[neighbour].flags & agent_flags).zero?

        g = current.g + poly_distance(current.poly, neighbour)
        if !g_scores.key?(neighbour) || g < g_scores[neighbour]
          g_scores[neighbour] = g
          h = heuristic(neighbour, end_poly)
          open_set << Node.new(g + h, neighbour, g, current.poly)
        end
      end
    end

    []  # No path
  end

  private

  def point_in_poly?(_point, _poly) = true  # Placeholder
  def poly_distance(_a, _b) = 1.0           # Placeholder
  def heuristic(_a, _b) = 1.0               # Placeholder

  def reconstruct_path(closed, node)
    path = [node.poly]
    while node.parent != -1
      node = closed[node.parent]
      path << node.poly
    end
    path.reverse
  end
end