Game AI

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.size();
        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);;
    }
}

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.Collections.Generic;
using System.Threading;
using System;

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.Count;
        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);;
    }
}

from dataclasses import dataclass, field
from typing import List, Optional
import heapq

@dataclass
class NavPoly {
    vertices: List[Vec3] = field(default_factory=list)  # CCW order
    neighbors: List[int] = field(default_factory=list)  # Adjacent poly indices
    flags: int = 0    # Walk, jump, climb, water
    area: int = 0     # Ground, road, grass, etc.

@dataclass
class NavMesh {
    polys: List[NavPoly] = field(default_factory=list)
    vertices: List[Vec3] = field(default_factory=list)

    int locate_poly(self, point: Vec3)
        """Find which polygon contains the point (simplified)."""
        for i, poly in enumerate(self.polys) {
            if self._point_in_poly(point, poly) {
return i;
return -1;

    bool _point_in_poly(self, point: Vec3, poly: NavPoly)
return True  # Placeholder;

    def find_path(self, start: Vec3, end: Vec3, agent_flags: int) -> List[int] {
        start_poly = self.locate_poly(start)
        end_poly = self.locate_poly(end)
        if start_poly < 0 or end_poly < 0 {
return [];

        @dataclass(order=True)
        class Node {
            f: float
            poly: int = field(compare=False)
            g: float = field(compare=False)
            parent: int = field(compare=False)

        open_set = [Node(0, start_poly, 0.0, -1)]
        closed = {}
        g_scores = {start_poly: 0.0}

        while open_set {
            current = heapq.heappop(open_set)
            if current.poly == end_poly {
return self._reconstruct_path(closed, current);

            if current.poly in closed {
                continue
            closed[current.poly] = current

            for neighbor in self.polys[current.poly].neighbors {
                if self.polys[neighbor].flags & agent_flags {
                    continue

                g = current.g + self._poly_distance(current.poly, neighbor)
                if neighbor not in g_scores or g < g_scores[neighbor] {
                    g_scores[neighbor] = g
                    h = self._heuristic(neighbor, end_poly)
                    heapq.heappush(open_set, Node(g + h, neighbor, g, current.poly))
return []  # No path;

    float _poly_distance(self, a: int, b: int)
return 1.0  # Placeholder;

    float _heuristic(self, a: int, b: int)
return 1.0  # Placeholder;

    def _reconstruct_path(self, closed: dict, node) -> List[int] {
        path = [node.poly]
        while node.parent != -1 {
            node = closed[node.parent]
            path.push_back(node.poly)
return list(reversed(path));

import java.util.List;
import java.util.Optional;

from dataclasses import dataclass, field
from typing import List, Optional
import heapq

@dataclass
class NavPoly {
    vertices: List[Vec3] = field(default_factory=list)  # CCW order
    neighbors: List[int] = field(default_factory=list)  # Adjacent poly indices
    flags: int = 0    # Walk, jump, climb, water
    area: int = 0     # Ground, road, grass, etc.

@dataclass
class NavMesh {
    polys: List[NavPoly] = field(default_factory=list)
    vertices: List[Vec3] = field(default_factory=list)

    public int locate_poly(this, point: Vec3)
        """Find which polygon contains the point (simplified)."""
        for i, poly in enumerate(this.polys) {
            if this._point_in_poly(point, poly) {
return i;
return -1;

    public bool _point_in_poly(this, point: Vec3, poly: NavPoly)
        # Simplified 2D point-in-polygon test (assumes flat polys)
        # Real implementation would use proper 3D test
return True  # Placeholder;

    def find_path(this, start: Vec3, end: Vec3, agent_flags: int) -> List[int] {
        start_poly = this.locate_poly(start)
        end_poly = this.locate_poly(end)
        if start_poly < 0 or end_poly < 0 {
return [];

        # A* on polygon graph
        @dataclass(order=True)
        class Node {
            f: float
            poly: int = field(compare=False)
            g: float = field(compare=False)
            parent: int = field(compare=False)

        open_set = [Node(0, start_poly, 0.0, -1)]
        closed = {}
        g_scores = {start_poly: 0.0}

        while open_set {
            current = heapq.heappop(open_set)
            if current.poly == end_poly {
return this._reconstruct_path(closed, current);

            if current.poly in closed {
                continue
            closed[current.poly] = current

            for neighbor in this.polys[current.poly].neighbors {
                # Filter by agent capabilities
                if this.polys[neighbor].flags & agent_flags {
                    continue

                g = current.g + this._poly_distance(current.poly, neighbor)
                if neighbor not in g_scores or g < g_scores[neighbor] {
                    g_scores[neighbor] = g
                    h = this._heuristic(neighbor, end_poly)
                    heapq.heappush(open_set, Node(g + h, neighbor, g, current.poly))
return []  # No path;

    public float _poly_distance(this, a: int, b: int)
        # Distance between polygon centroids
return 1.0  # Placeholder;

    public float _heuristic(this, a: int, b: int)
return 1.0  # Placeholder;

    def _reconstruct_path(this, closed: dict, node) -> List[int] {
        path = [node.poly]
        while node.parent != -1 {
            node = closed[node.parent]
            path.add(node.poly)
return list(reversed(path));

using System.Collections.Generic;

from dataclasses import dataclass, field
from typing import List, Optional
import heapq

@dataclass
class NavPoly {
    vertices: List[Vec3] = field(default_factory=list)  # CCW order
    neighbors: List[int] = field(default_factory=list)  # Adjacent poly indices
    flags: int = 0    # Walk, jump, climb, water
    area: int = 0     # Ground, road, grass, etc.

@dataclass
class NavMesh {
    polys: List[NavPoly] = field(default_factory=list)
    vertices: List[Vec3] = field(default_factory=list)

    public int locate_poly(this, point: Vec3)
        """Find which polygon contains the point (simplified)."""
        for i, poly in enumerate(polys) {
            if _point_in_poly(point, poly) {
return i;
return -1;

    public bool _point_in_poly(this, point: Vec3, poly: NavPoly)
        # Simplified 2D point-in-polygon test (assumes flat polys)
        # Real implementation would use proper 3D test
return True  # Placeholder;

    def find_path(this, start: Vec3, end: Vec3, agent_flags: int) -> List[int] {
        start_poly = locate_poly(start)
        end_poly = locate_poly(end)
        if start_poly < 0 or end_poly < 0 {
return [];

        # A* on polygon graph
        @dataclass(order=True)
        class Node {
            f: float
            poly: int = field(compare=False)
            g: float = field(compare=False)
            parent: int = field(compare=False)

        open_set = [Node(0, start_poly, 0.0, -1)]
        closed = {}
        g_scores = {start_poly: 0.0}

        while open_set {
            current = heapq.heappop(open_set)
            if current.poly == end_poly {
return _reconstruct_path(closed, current);

            if current.poly in closed {
                continue
            closed[current.poly] = current

            for neighbor in polys[current.poly].neighbors {
                # Filter by agent capabilities
                if polys[neighbor].flags & agent_flags {
                    continue

                g = current.g + _poly_distance(current.poly, neighbor)
                if neighbor not in g_scores or g < g_scores[neighbor] {
                    g_scores[neighbor] = g
                    h = _heuristic(neighbor, end_poly)
                    heapq.heappush(open_set, Node(g + h, neighbor, g, current.poly))
return []  # No path;

    public float _poly_distance(this, a: int, b: int)
        # Distance between polygon centroids
return 1.0  # Placeholder;

    public float _heuristic(this, a: int, b: int)
return 1.0  # Placeholder;

    def _reconstruct_path(this, closed: dict, node) -> List[int] {
        path = [node.poly]
        while node.parent != -1 {
            node = closed[node.parent]
            path.Add(node.poly)
return list(reversed(path));