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