protected override unsafe void AddToModel(Frame frame, int sign) { var height = frame.Height; var width = frame.Width; if (Model == null) { Model = new Frame(width, height, frame.PixelFormat, false); histograms = FrameFilterExtensions.CreateJaggedArray <int[][][][]>(height, width, 3, 256); medianBinOffset = FrameFilterExtensions.CreateJaggedArray <double[][][]>(height, width, 3); //Start at -0.5 -> to make the first frame the median for (int i = 0; i < medianBinOffset.Length; i++) { for (int j = 0; j < medianBinOffset[i].Length; j++) { for (int c = 0; c < medianBinOffset[i][j].Length; c++) { medianBinOffset[i][j][c] = -0.5; } } } } var modelFirstPx = Model.FirstPixelPointer; var modelStride = Model.Stride; var newFrameFirstPx = frame.FirstPixelPointer; var newFrameStride = frame.Stride; var n = Size; var multiplier = sign >= 0 ? 1 : -1; frameCount += multiplier; Parallel.For(0, height, new ParallelOptions { //MaxDegreeOfParallelism = 1 }, y => { var modelRowStart = modelFirstPx + y * modelStride; var newFrameRowStart = newFrameFirstPx + y * newFrameStride; var histY = histograms[y]; var medianBinOffsetY = medianBinOffset[y]; for (var x = width - 1; x >= 0; x--) { var x3 = x * 3; var modelPxAddr = modelRowStart + x3; var newFramePxAddr = newFrameRowStart + x3; var newFramePxIntVal = *(int *)newFramePxAddr; var modelPxIntVal = *(int *)modelPxAddr; var histYX = histY[x]; var medianBinOffsetYX = medianBinOffsetY[x]; //For every color channel, update their medians, by //updating the locations of the halfway points for (var c = 2; c >= 0; c--) { var cshift = 8 * c; var histYXC = histYX[c]; //Get the new color value //var newColorValue = newFramePxAddr[c]; var newColorValue = (newFramePxIntVal >> cshift) & 0xff; //Update the histogram histYXC[newColorValue] += multiplier; //Update the halfway-point //var currBin = modelPxAddr[c]; //The model contains the medians var currBin = (modelPxIntVal >> cshift) & 0xff; var halfwayMovedBy = (newColorValue < currBin ? -0.5 : 0.5) * multiplier; var newOffset = medianBinOffsetYX[c] + halfwayMovedBy; //No move if added to existing median bin if (currBin != newColorValue) { //NOTE: Treating offset value of BinCount-0.5 as between Bin and Bin+1; //If added to after or current, and no longer in the current bin, go to the next one if (halfwayMovedBy > 0 && newOffset > histYXC[currBin] - 0.5) { newOffset = 0; //Find the next bin that has elements do { currBin++; }while (histYXC[currBin] == 0); *(modelPxAddr + c) = (byte)currBin; } //If added to befor, and no longer in the current bin, go to the previous one else if (newOffset < 0) { var binCount = 0; //Find an earlier bin with elements do { currBin--; binCount = histYXC[currBin]; }while (binCount == 0); newOffset = binCount - 0.5; *(modelPxAddr + c) = (byte)currBin; } } medianBinOffsetYX[c] = newOffset; } } }); }
public DPoint CrawlToTip(Frame target, System.Drawing.Point start, System.Drawing.Point origin, int threshold = 50, int maxDistance = 30, int maxHopDistance = 2, bool crawlingAway = true) { var loopCount = 0; var startColor = target.GetColor(start); if (startColor.GetBrightness() == 0) { return(start); } var bestDistance = start.Distance(origin); var bestPoint = start; var visited = FrameFilterExtensions.CreateJaggedArray <bool[][]>(maxDistance * 2 + 1, maxDistance * 2 + 1); var visitedOffsetX = start.X - maxDistance; var visitedOffsetY = start.Y - maxDistance; var bestLocationLockpad = new object(); var queue = new Queue <System.Drawing.Point>(); queue.Enqueue(start); while (queue.Count > 0 && loopCount < 500) { loopCount++; var point = queue.Dequeue(); var candidates = getCandidateLocations(point, maxHopDistance); Parallel.ForEach(candidates, p => { //Check bounds if (p.X < 0 || p.X >= target.Width || p.Y < 0 || p.Y >= target.Height) { return; } //Check if within search radius var distFromStart = p.Distance(start); if (distFromStart > maxDistance) { return; } //Check if already checked var visitedX = p.X - visitedOffsetX; var visitedY = p.Y - visitedOffsetY; var exists = visited[visitedX][visitedY]; if (exists) { return; } //Mark as checked, regardless of outcome lock (visited) { visited[visitedX][visitedY] = true; } //Check if within color range var distFromStartColor = target.GetColor(p).Distance(startColor); if (distFromStartColor > threshold) { return; } //See if distance is further var dist = p.Distance(origin); //Don't bother if distance is much worse than the best (maximizing distance) if (crawlingAway && dist < bestDistance * 0.975) { return; } //Or opposite if crawling closer (minimizing distance) else if (!crawlingAway && dist > bestDistance * 1.025) { return; } //Save best distance (away = maximizing, towards = minimizing) if ((crawlingAway && dist > bestDistance) || (!crawlingAway && dist < bestDistance)) { lock (bestLocationLockpad) { bestDistance = dist; bestPoint = p; } } lock (queue) { queue.Enqueue(p); } }); } return(bestPoint); }