private void UpdateHandler(object sender, FrameEventArgs e)
{
if (status != WindowStatus.ScanDataStage)
{
return;
}
if (_depthPixels == null)
{
return;
}
var depthPoints = new List <DrawablePoint3D>();
var depthMap = new double[_rectSize, _rectSize];
for (var x = 0; x < _rectSize; x++)
{
for (var y = 0; y < _rectSize; y++)
{
depthMap[x, y] = double.MaxValue;
}
}
for (var y = 0; y < _kinectDepthImageHeight; y++)
{
for (var x = 0; x < _kinectDepthImageWidth; x++)
{
// Bierzemy odpowiedni pixel poprzez offset
var offset = x + y * _kinectDepthImageWidth;
var rawDepth = _depthPixels[offset].Depth;
// Odrzucamy jezeli sie nie nadaje
if (Math.Abs(rawDepth) < 0)
{
continue;
}
// Normalizujemy na nasze wspolrzedne
var depthx = (int)((x - 320) * focal * rawDepth / 10 + 60);
var depthy = (int)((-y + 240) * focal * rawDepth / 10 + 60);
double depthz = rawDepth / 10 - 120;
var cp = new DrawablePoint3D()
{
X = depthx,
Y = depthy,
Z = depthz,
DrawFlag = true
};
depthPoints.Add(cp);
// Dodajemy do naszej mapy glebokosci w odpowiedni sposob
HandleDepthMap(depthMap, depthx, depthy, depthz);
}
}
_depthPoints = depthPoints;
_depthMap = depthMap;
UpdateCamera();
}
private static DrawablePoint3D DoShift(this DrawablePoint3D drawablePoint3D, Vector3 shift)
{
return(new DrawablePoint3D
{
X = drawablePoint3D.X + shift.X,
Y = drawablePoint3D.Y + shift.Y,
Z = drawablePoint3D.Z + shift.Z,
DrawFlag = drawablePoint3D.DrawFlag,
});
}
public static bool InRectNoDepth(this DrawablePoint3D p)
{
return(p.X >= 0 && p.X <= Constants.Constants.RectWidth &&
p.Y >= 0 && p.Y <= Constants.Constants.RectHeight);
}
public static void Draw(this DrawablePoint3D drawablePoint3D)
{
GL.Vertex3(drawablePoint3D.X, drawablePoint3D.Y, drawablePoint3D.Z);
}
public static void DrawWithShift(this DrawablePoint3D drawablePoint3D)
{
drawablePoint3D.DoShift(Shift).Draw();
}
