705 lines
27 KiB
C#
705 lines
27 KiB
C#
using System.Diagnostics;
|
||
using OpenCvSharp;
|
||
using Serilog;
|
||
|
||
namespace Poe2Trade.Navigation;
|
||
|
||
public class WorldMap : IDisposable
|
||
{
|
||
private readonly MinimapConfig _config;
|
||
private Mat _canvas;
|
||
private Mat _confidence; // CV_16SC1: per-pixel wall confidence counter
|
||
private int _canvasSize;
|
||
private MapPosition _position;
|
||
private int _frameCount;
|
||
private int _consecutiveMatchFails;
|
||
private Mat? _prevWallMask; // for frame deduplication
|
||
private readonly PathFinder _pathFinder = new();
|
||
|
||
// Checkpoint tracking (canvas coordinates)
|
||
private readonly List<(Point Pos, long LastSeenMs)> _checkpointsOff = [];
|
||
private readonly List<(Point Pos, long LastSeenMs)> _checkpointsOn = [];
|
||
private const int CheckpointDedupRadius = 20;
|
||
|
||
public MapPosition Position => _position;
|
||
public bool LastMatchSucceeded { get; private set; }
|
||
public int CanvasSize => _canvasSize;
|
||
internal List<Point>? LastBfsPath => _pathFinder.LastResult?.Path;
|
||
|
||
private const int GrowMargin = 500;
|
||
private const int GrowAmount = 2000; // 1000px added per side
|
||
|
||
public WorldMap(MinimapConfig config)
|
||
{
|
||
_config = config;
|
||
_canvasSize = config.CanvasSize;
|
||
_canvas = new Mat(_canvasSize, _canvasSize, MatType.CV_8UC1, Scalar.Black);
|
||
_confidence = new Mat(_canvasSize, _canvasSize, MatType.CV_16SC1, Scalar.Black);
|
||
_position = new MapPosition(_canvasSize / 2.0, _canvasSize / 2.0);
|
||
}
|
||
|
||
private void EnsureCapacity()
|
||
{
|
||
var x = (int)Math.Round(_position.X);
|
||
var y = (int)Math.Round(_position.Y);
|
||
if (x >= GrowMargin && y >= GrowMargin &&
|
||
x < _canvasSize - GrowMargin && y < _canvasSize - GrowMargin)
|
||
return;
|
||
|
||
var oldSize = _canvasSize;
|
||
var newSize = oldSize + GrowAmount;
|
||
var offset = GrowAmount / 2;
|
||
|
||
var newCanvas = new Mat(newSize, newSize, MatType.CV_8UC1, Scalar.Black);
|
||
var newConf = new Mat(newSize, newSize, MatType.CV_16SC1, Scalar.Black);
|
||
using (var dst = new Mat(newCanvas, new Rect(offset, offset, oldSize, oldSize)))
|
||
_canvas.CopyTo(dst);
|
||
using (var dst = new Mat(newConf, new Rect(offset, offset, oldSize, oldSize)))
|
||
_confidence.CopyTo(dst);
|
||
|
||
_canvas.Dispose();
|
||
_confidence.Dispose();
|
||
_canvas = newCanvas;
|
||
_confidence = newConf;
|
||
_canvasSize = newSize;
|
||
_position = new MapPosition(_position.X + offset, _position.Y + offset);
|
||
Log.Information("Canvas grown: {Old}→{New}, offset={Offset}", oldSize, newSize, offset);
|
||
}
|
||
|
||
/// <summary>
|
||
/// Match current wall mask against the accumulated map to find position,
|
||
/// then stitch walls and paint explored area.
|
||
/// </summary>
|
||
public MapPosition MatchAndStitch(Mat classifiedMat, Mat wallMask, MinimapMode mode = MinimapMode.Overlay,
|
||
List<Point>? checkpointsOff = null, List<Point>? checkpointsOn = null)
|
||
{
|
||
EnsureCapacity();
|
||
var sw = Stopwatch.StartNew();
|
||
_frameCount++;
|
||
|
||
var isCorner = mode == MinimapMode.Corner;
|
||
var warmupFrames = isCorner ? 2 : _config.WarmupFrames;
|
||
var needsBootstrap = _frameCount <= warmupFrames || _consecutiveMatchFails >= 30;
|
||
|
||
Log.Debug("MatchAndStitch: frame#{N} mode={Mode} frameSize={W}x{H} pos=({X:F1},{Y:F1}) bootstrap={Boot} prevWallMask={HasPrev}",
|
||
_frameCount, mode, wallMask.Width, wallMask.Height,
|
||
_position.X, _position.Y, needsBootstrap, _prevWallMask != null);
|
||
|
||
var wallCountBefore = Cv2.CountNonZero(wallMask);
|
||
|
||
if (ShouldSkipFrame(classifiedMat, wallMask, isCorner, needsBootstrap, sw))
|
||
return _position;
|
||
|
||
var wallCountAfter = Cv2.CountNonZero(wallMask);
|
||
var dedupMs = sw.Elapsed.TotalMilliseconds;
|
||
|
||
// Store current wall mask for next frame's dedup check
|
||
_prevWallMask?.Dispose();
|
||
_prevWallMask = wallMask.Clone();
|
||
|
||
// Warmup / re-bootstrap: stitch at current position to seed the canvas
|
||
if (needsBootstrap)
|
||
{
|
||
// Don't consume warmup slots on empty frames (game still loading minimap)
|
||
if (wallCountAfter < 50)
|
||
{
|
||
_frameCount--;
|
||
Log.Information("Warmup waiting for minimap ({Ms:F1}ms)", sw.Elapsed.TotalMilliseconds);
|
||
return _position;
|
||
}
|
||
|
||
StitchWithConfidence(classifiedMat, _position, boosted: true, mode: mode);
|
||
PaintExploredCircle(_position);
|
||
MergeCheckpoints(_position, classifiedMat.Width, checkpointsOff, checkpointsOn);
|
||
LastMatchSucceeded = true; // signal caller to update viewport
|
||
if (_consecutiveMatchFails >= 30)
|
||
{
|
||
Log.Information("Re-bootstrap: mode={Mode} pos=({X:F1},{Y:F1}) frameSize={FS} walls={W} stitch={Ms:F1}ms",
|
||
mode, _position.X, _position.Y, classifiedMat.Width, wallCountAfter, sw.Elapsed.TotalMilliseconds);
|
||
_consecutiveMatchFails = 0;
|
||
}
|
||
else
|
||
{
|
||
Log.Information("Warmup frame {N}/{Total}: walls={WallsBefore}→{WallsAfter}(filtered) frameSize={FS} stitch={Ms:F1}ms",
|
||
_frameCount, _config.WarmupFrames, wallCountBefore, wallCountAfter,
|
||
wallMask.Width, sw.Elapsed.TotalMilliseconds);
|
||
}
|
||
return _position;
|
||
}
|
||
|
||
// Match wallMask against canvas to find best position
|
||
var matchStart = sw.Elapsed.TotalMilliseconds;
|
||
var matched = MatchPosition(wallMask, _position);
|
||
var matchMs = sw.Elapsed.TotalMilliseconds - matchStart;
|
||
|
||
if (matched == null)
|
||
{
|
||
_consecutiveMatchFails++;
|
||
LastMatchSucceeded = false;
|
||
Log.Information("MatchAndStitch: dedup={Dedup:F1}ms match={Match:F1}ms (FAILED x{Fails}) total={Total:F1}ms",
|
||
dedupMs, matchMs, _consecutiveMatchFails, sw.Elapsed.TotalMilliseconds);
|
||
return _position; // don't stitch — wrong position would corrupt the canvas
|
||
}
|
||
|
||
_consecutiveMatchFails = 0;
|
||
LastMatchSucceeded = true;
|
||
var prevPos = _position;
|
||
_position = matched;
|
||
var stitchStart = sw.Elapsed.TotalMilliseconds;
|
||
StitchWithConfidence(classifiedMat, _position, boosted: false, mode: mode);
|
||
PaintExploredCircle(_position);
|
||
MergeCheckpoints(_position, classifiedMat.Width, checkpointsOff, checkpointsOn);
|
||
var stitchMs = sw.Elapsed.TotalMilliseconds - stitchStart;
|
||
|
||
var posDx = _position.X - prevPos.X;
|
||
var posDy = _position.Y - prevPos.Y;
|
||
Log.Debug("MatchAndStitch: mode={Mode} pos=({X:F1},{Y:F1}) moved=({Dx:F1},{Dy:F1}) dedup={Dedup:F1}ms match={Match:F1}ms stitch={Stitch:F1}ms total={Total:F1}ms",
|
||
mode, _position.X, _position.Y, posDx, posDy, dedupMs, matchMs, stitchMs, sw.Elapsed.TotalMilliseconds);
|
||
return _position;
|
||
}
|
||
|
||
/// <summary>
|
||
/// Noise filter + frame deduplication. Returns true if the frame should be skipped.
|
||
/// </summary>
|
||
private bool ShouldSkipFrame(Mat classifiedMat, Mat wallMask, bool isCorner, bool needsBootstrap, Stopwatch sw)
|
||
{
|
||
// Block-based noise filter: only needed for overlay (game effects bleed through)
|
||
// Skip during warmup — we need walls to seed the canvas, confidence handles noise
|
||
if (!isCorner && !needsBootstrap)
|
||
{
|
||
var cleanFraction = FilterNoisyBlocks(wallMask, classifiedMat);
|
||
if (cleanFraction < 0.25)
|
||
{
|
||
Log.Information("Noise filter: {Clean:P0} clean, skipping ({Ms:F1}ms)",
|
||
cleanFraction, sw.Elapsed.TotalMilliseconds);
|
||
return true;
|
||
}
|
||
}
|
||
|
||
// Frame deduplication: skip if minimap hasn't scrolled yet (but always allow warmup through)
|
||
if (!needsBootstrap && _prevWallMask != null && _frameCount > 1)
|
||
{
|
||
using var xor = new Mat();
|
||
Cv2.BitwiseXor(wallMask, _prevWallMask, xor);
|
||
var changedPixels = Cv2.CountNonZero(xor);
|
||
if (changedPixels < _config.FrameChangeThreshold)
|
||
{
|
||
Log.Debug("Frame dedup: {Changed} changed pixels, skipping ({Ms:F1}ms)",
|
||
changedPixels, sw.Elapsed.TotalMilliseconds);
|
||
return true;
|
||
}
|
||
}
|
||
|
||
return false;
|
||
}
|
||
|
||
private MapPosition? MatchPosition(Mat wallMask, MapPosition estimate)
|
||
{
|
||
var frameSize = wallMask.Width;
|
||
var searchPad = _config.MatchSearchRadius;
|
||
var searchSize = frameSize + 2 * searchPad;
|
||
|
||
var cx = (int)Math.Round(estimate.X);
|
||
var cy = (int)Math.Round(estimate.Y);
|
||
|
||
// Search region on canvas (centered on estimate)
|
||
var sx = cx - searchSize / 2;
|
||
var sy = cy - searchSize / 2;
|
||
|
||
// Clamp to canvas bounds
|
||
var sx0 = Math.Max(0, sx);
|
||
var sy0 = Math.Max(0, sy);
|
||
var sx1 = Math.Min(_canvasSize, sx + searchSize);
|
||
var sy1 = Math.Min(_canvasSize, sy + searchSize);
|
||
var sw = sx1 - sx0;
|
||
var sh = sy1 - sy0;
|
||
|
||
// Search region must be larger than template
|
||
if (sw <= frameSize || sh <= frameSize)
|
||
return null;
|
||
|
||
// Extract search ROI and convert to binary wall mask
|
||
using var searchRoi = new Mat(_canvas, new Rect(sx0, sy0, sw, sh));
|
||
using var canvasWalls = new Mat();
|
||
Cv2.Compare(searchRoi, new Scalar((byte)MapCell.Wall), canvasWalls, CmpType.EQ);
|
||
|
||
// Check if canvas has enough walls to match against
|
||
var canvasWallCount = Cv2.CountNonZero(canvasWalls);
|
||
var frameWallCount = Cv2.CountNonZero(wallMask);
|
||
if (frameWallCount < 50)
|
||
{
|
||
Log.Information("Match fail: too few frame walls ({FrameWalls}) frameSize={FS}",
|
||
frameWallCount, frameSize);
|
||
return null;
|
||
}
|
||
|
||
if (canvasWallCount < 50)
|
||
{
|
||
Log.Information("Match fail: too few canvas walls ({CanvasWalls}) frame walls={FrameWalls} frameSize={FS} searchROI={SW}x{SH}",
|
||
canvasWallCount, frameWallCount, frameSize, sw, sh);
|
||
return null;
|
||
}
|
||
|
||
// Template match: find where frame's walls best align with canvas walls
|
||
using var result = new Mat();
|
||
Cv2.MatchTemplate(canvasWalls, wallMask, result, TemplateMatchModes.CCoeffNormed);
|
||
Cv2.MinMaxLoc(result, out _, out var maxVal, out _, out var maxLoc);
|
||
|
||
if (maxVal < _config.MatchConfidence)
|
||
{
|
||
Log.Information("Match fail: low confidence {Conf:F3} (need {Min:F2}) frameSize={FS} searchROI={SW}x{SH} canvas={CanvasWalls} frame={FrameWalls}",
|
||
maxVal, _config.MatchConfidence, frameSize, sw, sh, canvasWallCount, frameWallCount);
|
||
return null;
|
||
}
|
||
|
||
// Convert match position to world coordinates
|
||
// maxLoc is the top-left of the template in the search ROI
|
||
var matchX = sx0 + maxLoc.X + frameSize / 2.0;
|
||
var matchY = sy0 + maxLoc.Y + frameSize / 2.0;
|
||
|
||
var deltaX = matchX - estimate.X;
|
||
var deltaY = matchY - estimate.Y;
|
||
Log.Information("Match OK: conf={Conf:F3} pos=({X:F1},{Y:F1}) delta=({Dx:F1},{Dy:F1}) frameSize={FS} searchROI={SW}x{SH} canvas={CanvasWalls} frame={FrameWalls}",
|
||
maxVal, matchX, matchY, deltaX, deltaY, frameSize, sw, sh, canvasWallCount, frameWallCount);
|
||
|
||
return new MapPosition(matchX, matchY);
|
||
}
|
||
|
||
private void StitchWithConfidence(Mat classifiedMat, MapPosition position, bool boosted,
|
||
MinimapMode mode = MinimapMode.Overlay)
|
||
{
|
||
var isCorner = mode == MinimapMode.Corner;
|
||
var frameSize = classifiedMat.Width;
|
||
var halfSize = frameSize / 2;
|
||
var canvasX = (int)Math.Round(position.X) - halfSize;
|
||
var canvasY = (int)Math.Round(position.Y) - halfSize;
|
||
|
||
// Clamp to canvas bounds
|
||
var srcX = Math.Max(0, -canvasX);
|
||
var srcY = Math.Max(0, -canvasY);
|
||
var dstX = Math.Max(0, canvasX);
|
||
var dstY = Math.Max(0, canvasY);
|
||
var w = Math.Min(frameSize - srcX, _canvasSize - dstX);
|
||
var h = Math.Min(frameSize - srcY, _canvasSize - dstY);
|
||
|
||
if (w <= 0 || h <= 0) return;
|
||
|
||
var srcRect = new Rect(srcX, srcY, w, h);
|
||
var dstRect = new Rect(dstX, dstY, w, h);
|
||
|
||
var srcRoi = new Mat(classifiedMat, srcRect);
|
||
var dstRoi = new Mat(_canvas, dstRect);
|
||
var confRoi = new Mat(_confidence, dstRect);
|
||
|
||
// Corner minimap is clean — trust walls immediately, lower threshold
|
||
var confInc = isCorner ? (short)_config.ConfidenceMax : (short)_config.ConfidenceInc;
|
||
var confDec = (short)_config.ConfidenceDec;
|
||
var confThreshold = isCorner ? (short)2 : (short)_config.ConfidenceThreshold;
|
||
var confMax = (short)_config.ConfidenceMax;
|
||
|
||
for (var row = 0; row < h; row++)
|
||
for (var col = 0; col < w; col++)
|
||
{
|
||
var srcVal = srcRoi.At<byte>(row, col);
|
||
var conf = confRoi.At<short>(row, col);
|
||
|
||
if (srcVal == (byte)MapCell.Wall)
|
||
{
|
||
conf = boosted
|
||
? confMax
|
||
: Math.Min((short)(conf + confInc), confMax);
|
||
}
|
||
else if (conf > 0)
|
||
{
|
||
conf = Math.Max((short)(conf - confDec), (short)0);
|
||
}
|
||
else
|
||
{
|
||
continue;
|
||
}
|
||
|
||
confRoi.Set(row, col, conf);
|
||
|
||
var current = dstRoi.At<byte>(row, col);
|
||
// Corner mode: no double-evidence needed (clean data)
|
||
var needed = !isCorner && current == (byte)MapCell.Explored
|
||
? (short)(confThreshold * 2)
|
||
: confThreshold;
|
||
|
||
if (conf >= needed)
|
||
dstRoi.Set(row, col, (byte)MapCell.Wall);
|
||
else if (current == (byte)MapCell.Wall && conf < confThreshold)
|
||
dstRoi.Set(row, col, (byte)MapCell.Explored);
|
||
}
|
||
|
||
// Mark fog on canvas (on top of Unknown or Explored — not Wall)
|
||
if (isCorner)
|
||
{
|
||
for (var row = 0; row < h; row++)
|
||
for (var col = 0; col < w; col++)
|
||
{
|
||
if (srcRoi.At<byte>(row, col) != (byte)MapCell.Fog) continue;
|
||
var dst = dstRoi.At<byte>(row, col);
|
||
if (dst == (byte)MapCell.Unknown || dst == (byte)MapCell.Explored)
|
||
dstRoi.Set(row, col, (byte)MapCell.Fog);
|
||
}
|
||
}
|
||
else
|
||
{
|
||
// Overlay: exclude small area near player center (spell effect noise)
|
||
const int fogInner = 30;
|
||
const int fogInner2 = fogInner * fogInner;
|
||
|
||
for (var row = 0; row < h; row++)
|
||
for (var col = 0; col < w; col++)
|
||
{
|
||
if (srcRoi.At<byte>(row, col) != (byte)MapCell.Fog) continue;
|
||
|
||
var fx = srcX + col - halfSize;
|
||
var fy = srcY + row - halfSize;
|
||
if (fx * fx + fy * fy < fogInner2) continue;
|
||
|
||
var dst = dstRoi.At<byte>(row, col);
|
||
if (dst == (byte)MapCell.Unknown || dst == (byte)MapCell.Explored)
|
||
dstRoi.Set(row, col, (byte)MapCell.Fog);
|
||
}
|
||
}
|
||
|
||
}
|
||
|
||
/// <summary>
|
||
/// Mark explored area: circle around player position.
|
||
/// Unknown cells are marked Explored within the full radius.
|
||
/// Fog cells are only cleared in the inner portion (fogClearRadius),
|
||
/// preserving fog at the edge so it stays visible on the viewport.
|
||
/// </summary>
|
||
private void PaintExploredCircle(MapPosition position)
|
||
{
|
||
var pcx = (int)Math.Round(position.X);
|
||
var pcy = (int)Math.Round(position.Y);
|
||
var r = _config.ExploredRadius;
|
||
var r2 = r * r;
|
||
var fogClear = r - 20;
|
||
var fogClear2 = fogClear * fogClear;
|
||
|
||
var y0 = Math.Max(0, pcy - r);
|
||
var y1 = Math.Min(_canvasSize - 1, pcy + r);
|
||
var x0 = Math.Max(0, pcx - r);
|
||
var x1 = Math.Min(_canvasSize - 1, pcx + r);
|
||
|
||
for (var y = y0; y <= y1; y++)
|
||
for (var x = x0; x <= x1; x++)
|
||
{
|
||
var dx = x - pcx;
|
||
var dy = y - pcy;
|
||
var d2 = dx * dx + dy * dy;
|
||
if (d2 > r2) continue;
|
||
var cell = _canvas.At<byte>(y, x);
|
||
if (cell == (byte)MapCell.Unknown)
|
||
_canvas.Set(y, x, (byte)MapCell.Explored);
|
||
else if (cell == (byte)MapCell.Fog && d2 <= fogClear2)
|
||
_canvas.Set(y, x, (byte)MapCell.Explored);
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// Zero out 50×50 blocks where wall density exceeds 25%.
|
||
/// Modifies wallMask and classifiedMat in-place.
|
||
/// Returns fraction of blocks that are clean (0.0–1.0).
|
||
/// </summary>
|
||
private static double FilterNoisyBlocks(Mat wallMask, Mat classifiedMat,
|
||
int blockSize = 50, double blockMaxDensity = 0.25)
|
||
{
|
||
var rows = wallMask.Rows;
|
||
var cols = wallMask.Cols;
|
||
var totalBlocks = 0;
|
||
var cleanBlocks = 0;
|
||
|
||
for (var by = 0; by < rows; by += blockSize)
|
||
for (var bx = 0; bx < cols; bx += blockSize)
|
||
{
|
||
var bw = Math.Min(blockSize, cols - bx);
|
||
var bh = Math.Min(blockSize, rows - by);
|
||
totalBlocks++;
|
||
|
||
var blockRect = new Rect(bx, by, bw, bh);
|
||
using var blockRoi = new Mat(wallMask, blockRect);
|
||
var wallCount = Cv2.CountNonZero(blockRoi);
|
||
|
||
if ((double)wallCount / (bw * bh) > blockMaxDensity)
|
||
{
|
||
// Zero out this noisy block in both mats
|
||
blockRoi.SetTo(Scalar.Black);
|
||
using var classBlock = new Mat(classifiedMat, blockRect);
|
||
classBlock.SetTo(Scalar.Black);
|
||
}
|
||
else
|
||
{
|
||
cleanBlocks++;
|
||
}
|
||
}
|
||
|
||
return totalBlocks > 0 ? (double)cleanBlocks / totalBlocks : 1.0;
|
||
}
|
||
|
||
public (double dirX, double dirY)? FindNearestUnexplored(MapPosition pos, int searchRadius = 400)
|
||
=> _pathFinder.FindNearestUnexplored(_canvas, _canvasSize, pos, searchRadius);
|
||
|
||
public (double dirX, double dirY)? FindPathToTarget(MapPosition pos, Point target, int searchRadius = 400)
|
||
=> _pathFinder.FindPathToTarget(_canvas, _canvasSize, pos, target, searchRadius);
|
||
|
||
public byte[] GetMapSnapshot()
|
||
{
|
||
Cv2.ImEncode(".png", _canvas, out var buf);
|
||
return buf;
|
||
}
|
||
|
||
public byte[] GetViewportSnapshot(MapPosition center, int viewSize = 400)
|
||
{
|
||
var cx = (int)Math.Round(center.X);
|
||
var cy = (int)Math.Round(center.Y);
|
||
var half = viewSize / 2;
|
||
|
||
var x0 = Math.Clamp(cx - half, 0, _canvasSize - viewSize);
|
||
var y0 = Math.Clamp(cy - half, 0, _canvasSize - viewSize);
|
||
var roi = new Mat(_canvas, new Rect(x0, y0, viewSize, viewSize));
|
||
|
||
using var colored = new Mat(viewSize, viewSize, MatType.CV_8UC3, new Scalar(23, 17, 13));
|
||
for (var r = 0; r < viewSize; r++)
|
||
for (var c = 0; c < viewSize; c++)
|
||
{
|
||
var v = roi.At<byte>(r, c);
|
||
if (v == (byte)MapCell.Explored)
|
||
colored.Set(r, c, new Vec3b(104, 64, 31));
|
||
else if (v == (byte)MapCell.Wall)
|
||
colored.Set(r, c, new Vec3b(26, 45, 61));
|
||
else if (v == (byte)MapCell.Fog)
|
||
colored.Set(r, c, new Vec3b(120, 70, 40));
|
||
}
|
||
|
||
// BFS overlay: frontier cells + direction line
|
||
var bfs = _pathFinder.LastResult;
|
||
if (bfs != null)
|
||
{
|
||
// Other frontier directions (dim cyan)
|
||
foreach (var pt in bfs.OtherFrontier)
|
||
{
|
||
var vx = pt.X - x0;
|
||
var vy = pt.Y - y0;
|
||
if (vx >= 0 && vx < viewSize && vy >= 0 && vy < viewSize)
|
||
colored.Set(vy, vx, new Vec3b(100, 80, 0));
|
||
}
|
||
|
||
// Best frontier direction (bright green)
|
||
foreach (var pt in bfs.BestFrontier)
|
||
{
|
||
var vx = pt.X - x0;
|
||
var vy = pt.Y - y0;
|
||
if (vx >= 0 && vx < viewSize && vy >= 0 && vy < viewSize)
|
||
colored.Set(vy, vx, new Vec3b(0, 220, 0));
|
||
}
|
||
|
||
// Direction line from player
|
||
var px2 = bfs.PlayerCx - x0;
|
||
var py2 = bfs.PlayerCy - y0;
|
||
var lineLen = 60;
|
||
var ex = (int)(px2 + bfs.DirX * lineLen);
|
||
var ey = (int)(py2 + bfs.DirY * lineLen);
|
||
Cv2.ArrowedLine(colored, new Point(px2, py2), new Point(ex, ey),
|
||
new Scalar(0, 220, 0), 2, tipLength: 0.3);
|
||
|
||
// BFS path polyline (bright green)
|
||
if (bfs.Path.Count >= 2)
|
||
{
|
||
var viewPath = new Point[bfs.Path.Count];
|
||
for (var i = 0; i < bfs.Path.Count; i++)
|
||
viewPath[i] = new Point(bfs.Path[i].X - x0, bfs.Path[i].Y - y0);
|
||
Cv2.Polylines(colored, [viewPath], isClosed: false,
|
||
color: new Scalar(0, 255, 0), thickness: 1);
|
||
}
|
||
}
|
||
|
||
// Checkpoint markers
|
||
foreach (var (cp, _) in _checkpointsOff)
|
||
{
|
||
var cpx = cp.X - x0;
|
||
var cpy = cp.Y - y0;
|
||
if (cpx >= 0 && cpx < viewSize && cpy >= 0 && cpy < viewSize)
|
||
Cv2.Circle(colored, new Point(cpx, cpy), 5, new Scalar(100, 100, 100), -1); // gray
|
||
}
|
||
foreach (var (cp, _) in _checkpointsOn)
|
||
{
|
||
var cpx = cp.X - x0;
|
||
var cpy = cp.Y - y0;
|
||
if (cpx >= 0 && cpx < viewSize && cpy >= 0 && cpy < viewSize)
|
||
Cv2.Circle(colored, new Point(cpx, cpy), 5, new Scalar(220, 200, 0), -1); // bright cyan (BGR)
|
||
}
|
||
|
||
// Player dot (orange, on top)
|
||
var px = cx - x0;
|
||
var py = cy - y0;
|
||
if (px >= 0 && px < viewSize && py >= 0 && py < viewSize)
|
||
Cv2.Circle(colored, new Point(px, py), 4, new Scalar(0, 140, 255), -1);
|
||
|
||
Cv2.ImEncode(".png", colored, out var buf);
|
||
return buf;
|
||
}
|
||
|
||
/// <summary>
|
||
/// Convert frame-relative checkpoint positions to canvas coords and merge with existing lists.
|
||
/// When a checkpoint-on appears near a checkpoint-off, remove the off entry.
|
||
/// </summary>
|
||
private void MergeCheckpoints(MapPosition position, int frameSize,
|
||
List<Point>? offPoints, List<Point>? onPoints)
|
||
{
|
||
var halfSize = frameSize / 2;
|
||
var canvasX = (int)Math.Round(position.X) - halfSize;
|
||
var canvasY = (int)Math.Round(position.Y) - halfSize;
|
||
var now = DateTimeOffset.UtcNow.ToUnixTimeMilliseconds();
|
||
var r2 = CheckpointDedupRadius * CheckpointDedupRadius;
|
||
|
||
if (offPoints != null)
|
||
{
|
||
foreach (var fp in offPoints)
|
||
{
|
||
var cp = new Point(canvasX + fp.X, canvasY + fp.Y);
|
||
if (!IsDuplicate(_checkpointsOff, cp, r2))
|
||
_checkpointsOff.Add((cp, now));
|
||
}
|
||
}
|
||
|
||
if (onPoints != null)
|
||
{
|
||
foreach (var fp in onPoints)
|
||
{
|
||
var cp = new Point(canvasX + fp.X, canvasY + fp.Y);
|
||
|
||
// Remove matching off-checkpoint (it got activated)
|
||
for (var i = _checkpointsOff.Count - 1; i >= 0; i--)
|
||
{
|
||
var dx = _checkpointsOff[i].Pos.X - cp.X;
|
||
var dy = _checkpointsOff[i].Pos.Y - cp.Y;
|
||
if (dx * dx + dy * dy <= r2)
|
||
{
|
||
_checkpointsOff.RemoveAt(i);
|
||
break;
|
||
}
|
||
}
|
||
|
||
if (!IsDuplicate(_checkpointsOn, cp, r2))
|
||
_checkpointsOn.Add((cp, now));
|
||
}
|
||
}
|
||
}
|
||
|
||
private static bool IsDuplicate(List<(Point Pos, long LastSeenMs)> list, Point pt, int r2)
|
||
{
|
||
foreach (var (pos, _) in list)
|
||
{
|
||
var dx = pos.X - pt.X;
|
||
var dy = pos.Y - pt.Y;
|
||
if (dx * dx + dy * dy <= r2)
|
||
return true;
|
||
}
|
||
return false;
|
||
}
|
||
|
||
/// <summary>
|
||
/// Returns the nearest unactivated checkpoint within maxDist canvas pixels, or null.
|
||
/// </summary>
|
||
public Point? GetNearestCheckpointOff(MapPosition pos, int maxDist = 200)
|
||
{
|
||
var px = (int)Math.Round(pos.X);
|
||
var py = (int)Math.Round(pos.Y);
|
||
var maxDist2 = maxDist * maxDist;
|
||
|
||
Point? best = null;
|
||
var bestDist2 = int.MaxValue;
|
||
foreach (var (cp, _) in _checkpointsOff)
|
||
{
|
||
var dx = cp.X - px;
|
||
var dy = cp.Y - py;
|
||
var d2 = dx * dx + dy * dy;
|
||
if (d2 <= maxDist2 && d2 < bestDist2)
|
||
{
|
||
bestDist2 = d2;
|
||
best = cp;
|
||
}
|
||
}
|
||
return best;
|
||
}
|
||
|
||
public void Reset()
|
||
{
|
||
_canvas.Dispose();
|
||
_confidence.Dispose();
|
||
_canvasSize = _config.CanvasSize;
|
||
_canvas = new Mat(_canvasSize, _canvasSize, MatType.CV_8UC1, Scalar.Black);
|
||
_confidence = new Mat(_canvasSize, _canvasSize, MatType.CV_16SC1, Scalar.Black);
|
||
_prevWallMask?.Dispose();
|
||
_prevWallMask = null;
|
||
_position = new MapPosition(_canvasSize / 2.0, _canvasSize / 2.0);
|
||
_frameCount = 0;
|
||
_consecutiveMatchFails = 0;
|
||
LastMatchSucceeded = false;
|
||
_checkpointsOff.Clear();
|
||
_checkpointsOn.Clear();
|
||
}
|
||
|
||
/// <summary>
|
||
/// Mode switch: halve confidence around current position so old mode's weak walls
|
||
/// decay quickly while strong walls provide matching reference for the new mode.
|
||
/// No re-bootstrap stitch — let the first match find the correct position.
|
||
/// </summary>
|
||
public void Rebootstrap()
|
||
{
|
||
Log.Information("Rebootstrap: frameCount={N} pos=({X:F1},{Y:F1}) matchFails={Fails} prevWallMask={Size}",
|
||
_frameCount, _position.X, _position.Y, _consecutiveMatchFails,
|
||
_prevWallMask != null ? $"{_prevWallMask.Width}x{_prevWallMask.Height}" : "null");
|
||
|
||
// Halve confidence around current position — weak walls get demoted,
|
||
// strong walls survive to help the new mode's first match find position
|
||
var halfClear = 250; // slightly larger than largest frame half (200)
|
||
var cx = (int)Math.Round(_position.X);
|
||
var cy = (int)Math.Round(_position.Y);
|
||
var x0 = Math.Max(0, cx - halfClear);
|
||
var y0 = Math.Max(0, cy - halfClear);
|
||
var w = Math.Min(_canvasSize, cx + halfClear) - x0;
|
||
var h = Math.Min(_canvasSize, cy + halfClear) - y0;
|
||
if (w > 0 && h > 0)
|
||
{
|
||
var rect = new Rect(x0, y0, w, h);
|
||
var confThreshold = (short)_config.ConfidenceThreshold;
|
||
|
||
using var confRoi = new Mat(_confidence, rect);
|
||
using var canvasRoi = new Mat(_canvas, rect);
|
||
|
||
var demoted = 0;
|
||
for (var row = 0; row < h; row++)
|
||
for (var col = 0; col < w; col++)
|
||
{
|
||
var conf = confRoi.At<short>(row, col);
|
||
if (conf <= 0) continue;
|
||
conf = (short)(conf / 2);
|
||
confRoi.Set(row, col, conf);
|
||
if (conf < confThreshold && canvasRoi.At<byte>(row, col) == (byte)MapCell.Wall)
|
||
{
|
||
canvasRoi.Set(row, col, (byte)MapCell.Explored);
|
||
demoted++;
|
||
}
|
||
}
|
||
Log.Information("Rebootstrap: halved confidence in {W}x{H} area, demoted {Demoted} weak walls", w, h, demoted);
|
||
}
|
||
|
||
_prevWallMask?.Dispose();
|
||
_prevWallMask = null;
|
||
_frameCount = 0; // force re-warmup with new mode's data
|
||
}
|
||
|
||
public void Dispose()
|
||
{
|
||
_canvas.Dispose();
|
||
_confidence.Dispose();
|
||
_prevWallMask?.Dispose();
|
||
}
|
||
}
|