readonly double minimumDistanceToWall = 1500; // Milimetres
#endregion Fields
#region Methods
public PointFrame[,] Apply(PointFrame[,] currentCloud)
{
var resultCloud = PointFrame.NewCloud(currentCloud.GetLength(0), currentCloud.GetLength(1));
var kinnectToFloorDistance = Math.Sqrt((Math.Pow(kinnectHeight, 2) + Math.Pow(minimumDistanceToWall, 2)));
var floorSlopeAngle = Math.Asin(kinnectHeight / kinnectToFloorDistance);
for (int i = 0; i < currentCloud.GetLength(0); ++i)
{
for (int j = 0; j < currentCloud.GetLength(1); ++j)
{
if (currentCloud[i,j].Z <= 0)
{
continue;
}
var newPoint = TransformPoint(currentCloud[i, j], kinnectToFloorDistance, floorSlopeAngle);
if (newPoint.Y >= 0)
{
resultCloud[(int)newPoint.X, (int)newPoint.Y] = newPoint;
}
}
}
return resultCloud;
}
readonly int lightSourceHeight = 700; // Pixels
#endregion Fields
#region Methods
public PointFrame[,] Apply(PointFrame[,] currentCloud)
{
var resultCloud = PointFrame.NewCloud(currentCloud.GetLength(0), currentCloud.GetLength(1));
for (int i = 0; i < currentCloud.GetLength(0); ++i)
{
for (int j = 0; j < currentCloud.GetLength(1); ++j)
{
if (currentCloud[i,j].Z <= 0)
{
continue;
}
var newPoint = new PointFrame(currentCloud[i, j]);
var projectionSlope = (lightSourceHeight - newPoint.Y) / (lightSourceDepth - newPoint.Z);
var sourceIntersection = lightSourceHeight - (projectionSlope * lightSourceDepth);
newPoint.Y = (1f * projectionSlope) + sourceIntersection;
if (newPoint.Y > 0 && newPoint.Y < resultCloud.GetLength(1))
{
resultCloud[i, (int)newPoint.Y] = newPoint;
}
}
}
return resultCloud;
}
public PointFrame[,] Apply(PointFrame[,] currentCloud)
{
if (previousCloud == null)
{
previousCloud = PointFrame.NewCloud(currentCloud.GetLength(0), currentCloud.GetLength(1));
}
var resultCloud = PointFrame.NewCloud(currentCloud.GetLength(0), currentCloud.GetLength(1));
if (previousCloud.Length != currentCloud.Length)
{
Array.Copy(currentCloud, resultCloud, currentCloud.Length);
}
else
{
for (var i = 0; i < currentCloud.GetLength(0); ++i)
{
for (var j = 0; j < currentCloud.GetLength(1); ++j)
{
var currentPoint = currentCloud[i, j];
var previousPoint = previousCloud[i, j];
var resultPoint = new PointFrame(currentPoint);
if (!(currentPoint.Z < previousPoint.Z - minimumDelta || previousPoint.Z + minimumDelta < currentPoint.Z))
{
resultPoint.R = Math.Max(previousPoint.R - agingFactor, 1);
resultPoint.G = Math.Max(previousPoint.G - agingFactor, 1);
resultPoint.B = Math.Max(previousPoint.B - agingFactor, 1);
}
previousCloud[i, j] = resultPoint;
resultCloud[i, j] = resultPoint;
}
}
}
return resultCloud;
}
IEnumerable<PointFrame> AsEnumerable(PointFrame[,] cloud)
{
for (int i = cloud.GetLength(1) -1; i >= 0; --i)
{
for (int j = 0; j < cloud.GetLength(0); ++j)
{
yield return cloud[j, i];
}
}
}