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;
}
PointFrame TransformPoint(PointFrame point, double kinnectToFloorDistance, double floorSlopeAngle)
{
var resultPoint = new PointFrame(point);
var pointAngle = Math.Asin(resultPoint.Y / resultPoint.Z);
var newAngle = pointAngle - floorSlopeAngle;
resultPoint.Y = (int)(resultPoint.Z * Math.Sin(newAngle));
return resultPoint;
}
public PointFrame(PointFrame point)
{
X = point.X;
Y = point.Y;
Z = point.Z;
R = point.R;
G = point.G;
B = point.B;
}
public static PointFrame[,] NewCloud(int width, int height)
{
var result = new PointFrame[width, height];
for (int i = 0; i < width; ++i)
{
for (int j = 0; j < height; ++j)
{
result[i, j] = new PointFrame()
{
X = i,
Y = j,
Z = -1,
R = 0,
G = 0,
B = 0
};
}
}
return result;
}
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;
}
PointFrame[,] ConvertToPointCloud(ushort[] depths, int width, int height)
{
var points = PointFrame.NewCloud(width, height);
for (int i = 0; i < depths.Length; ++i)
{
var item = depths[i];
var x = (int)((i % width) * this.WallWidth / (float)width);
var y = (int)((height - i / width) * this.WallHeight / (float)height);
var z = item > 0 ? this.WallBreadth - (((float)item / (this.WallBreadth * 10)) * this.WallBreadth) : 0;
var b = item / 3;
var g = (item - b) / 3;
var r = (item - b - g) / 3;
points[x, y] = new PointFrame() { X = x, Y = y, Z = z, R = (byte)r, G = (byte)g, B = (byte)b };
}
return points;
}
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];
}
}
}
