private void RefreshData(ushort[] depthData, int colorWidth, int colorHeight)
{
var frameDesc = _Sensor.DepthFrameSource.FrameDescription;
ColorSpacePoint[] colorSpace = new ColorSpacePoint[depthData.Length];
_Mapper.MapDepthFrameToColorSpace(depthData, colorSpace);
if (backgroundRemoval)
{
bodyIndexData = _MultiManager.GetBodyIndexData();
for (int y = 0; y < frameDesc.Height; y++)
{
for (int x = 0; x < frameDesc.Width; x++)
{
int bodyIndex = y * frameDesc.Width + x;
byte player = bodyIndexData [bodyIndex];
if (player == 0xFF)
{
depthData [bodyIndex] = 0;
}
}
}
}
///flip vertices
//for(int y = 0; y < frameDesc.Height; y++)
for (int y = 0; y < frameDesc.Height; y += _DownsampleSize)
{
for (int x = 0; x < frameDesc.Width; x += _DownsampleSize)
{
int indexX = x / _DownsampleSize;
int indexY = y / _DownsampleSize;
int smallIndex = (indexY * (frameDesc.Width / _DownsampleSize)) + indexX;
double avg = GetAvg(depthData, x, y, frameDesc.Width, frameDesc.Height);
avg = avg * _DepthScale;
_Vertices[smallIndex].z = (float)avg;
var colorSpacePoint = colorSpace[(y * frameDesc.Width) + x];
// Update UV mapping with CDRP
if (colorSpacePoint.X != -1.0f && colorSpacePoint.Y != 1.0f)
{
_UV[smallIndex] = new Vector2(colorSpacePoint.X / colorWidth, colorSpacePoint.Y / colorHeight);
}
//_UV[smallIndex] = new Vector2(colorSpacePoint.X / colorWidth, colorSpacePoint.Y / colorHeight);
}
}
_Mesh.vertices = _Vertices;
_Mesh.uv = _UV;
_Mesh.triangles = _Triangles;
_Mesh.RecalculateNormals();
}
/* Based on Kinect Coordinate Mapping Basics
* Modified by John Densoyers-Stewart
* 2018-03-28
*/
void Reader_MultiSourceFrameArrived(object sender, MultiSourceFrameArrivedEventArgs e)
{
particles.Clear();
if (MultiSourceManager == null)
{
return;
}
if (_MultiManager == null)
{
return;
}
int bodyCount = _Sensor.BodyFrameSource.BodyCount;
depthFrameData = _MultiManager.GetDepthData();
bodyIndexData = _MultiManager.GetBodyIndexData();
_Mapper.MapDepthFrameToCameraSpace(depthFrameData, cameraSpacePoints);
_Mapper.MapDepthFrameToColorSpace(depthFrameData, colorSpacePoints);
if (createParticles)
{
for (int x = 0; x < depthWidth; x += downsample)
{
for (int y = 0; y < depthHeight; y += downsample)
{
int i = x + (depthWidth * y);
CameraSpacePoint p = cameraSpacePoints[i];
if (!float.IsNegativeInfinity(p.X) && !float.IsNegativeInfinity(p.Y) && !float.IsNegativeInfinity(p.Z))
{
if (bodiesOnly)
{
if (bodyIndexData[i] < bodyCount)
{
//need to combine this with the other color stuff below to make it work?
/*
* ColorSpacePoint colorPoint = colorSpacePoints[i];
*
* byte r = 0;
* byte g = 0;
* byte b = 0;
* byte a = 0;
*
* int colorX = (int)System.Math.Floor(colorPoint.X + 0.5);
* int colorY = (int)System.Math.Floor(colorPoint.Y + 0.5);
*
* if ((colorX >= 0) && (colorX < colorWidth) && (colorY >= 0) && (colorY < colorHeight))
* {
* int colorIndex = ((colorY * colorWidth) + colorX) * bytesPerPixel;
* b = colorFrameData[colorIndex++];
* g = colorFrameData[colorIndex++];
* r = colorFrameData[colorIndex++];
* a = colorFrameData[colorIndex++];
* }*/
Vector3 particlePos = GetVector3FromCameraSpacePoint(p);
if (Vector3.SqrMagnitude(particlePos - transform.InverseTransformPoint(vrHeadset.position)) > headRadiusSquare)
{
//particleArray[i].position = particlePos;
//particleArray[i].startColor = color;
//particleArray[i].startSize = size;
//particleArray[i].startLifetime = 1.0f;
ParticleSystem.Particle particle = new ParticleSystem.Particle();
particle.position = particlePos;
particle.startColor = color; // new Color32(r,g,b,a);
particle.startSize = size;
particles.Add(particle);
}
}
}
else if (noBodies)
{
if (bodyIndexData[i] > bodyCount)
{
Vector3 particlePos = GetVector3FromCameraSpacePoint(p);
if (Vector3.SqrMagnitude(particlePos - transform.InverseTransformPoint(vrHeadset.position)) > headRadiusSquare)
{
//particleArray[i].position = particlePos;
//particleArray[i].startColor = color;
//particleArray[i].startSize = size;
ParticleSystem.Particle particle = new ParticleSystem.Particle();
particle.position = particlePos;
particle.startColor = color;
particle.startSize = size;
particles.Add(particle);
}
}
}
else
{
Vector3 particlePos = GetVector3FromCameraSpacePoint(p);
if (Vector3.SqrMagnitude(particlePos - transform.InverseTransformPoint(vrHeadset.position)) > headRadiusSquare)
{
//particleArray[i].position = particlePos;
//Debug.Log(particleArray[i].position);
//particleArray[i].startColor = color;
//particleArray[i].startSize = size;
ParticleSystem.Particle particle = new ParticleSystem.Particle();
particle.position = particlePos;
particle.startColor = color;
particle.startSize = size;
particles.Add(particle);
}
}
}
}
/*
* float colorMappedToDepthX = colorSpacePoints[colorIndex].X;
* float colorMappedToDepthY = colorSpacePoints[colorIndex].Y;
*
* CameraSpacePoint p = cameraSpacePoints[colorIndex];
*
* if (!float.IsNegativeInfinity(colorMappedToDepthX) && !float.IsNegativeInfinity(colorMappedToDepthY))
* {
* int depthX = (int)(colorMappedToDepthX + 0.5f);
* int depthY = (int)(colorMappedToDepthY + 0.5f);
*
* // If the point is not valid, there is no body index there.
* if ((depthX >= 0) && (depthX < depthWidth) && (depthY >= 0) && (depthY < depthHeight))
* {
* int depthIndex = (depthY * depthWidth) + depthX;
*
* //if (bodyIndexData[depthIndex] < bodyCount)
* //{
* if (!float.IsNegativeInfinity(p.X) && !float.IsNegativeInfinity(p.Y) && !float.IsNegativeInfinity(p.Z))
* {
* ParticleSystem.Particle particle = new ParticleSystem.Particle();
* particle.position = new Vector3(-p.X, p.Y, p.Z) * scale;
* particle.startColor = color;
* particle.startSize = size;
* particles.Add(particle);
* //}
* }
* }
* }*/
}
//Debug.Log(_particleSystem.particleCount);
//particleSystemTimer = Time.time;
//_particleSystem.SetParticles(particleArray, particleArray.Length);
if (particles.Count > 0)
{
//_particleSystem.SetParticles(particleArray, particleArray.Length);
_particleSystem.SetParticles(particles.ToArray(), particles.Count);
particleSystemTimer = Time.time;
}
}
/*if (createMesh)
* {
* KinectMesh.GetComponent<Renderer>().material.mainTexture = _MultiManager.GetColorTexture();
*
* RefreshData(_MultiManager.GetDepthData(), _MultiManager.ColorWidth, _MultiManager.ColorHeight);
* }*/
}
// Update is called once per frame
void Update()
{
//TMP
if (Input.GetKey(KeyCode.G))
{
generateOffsetRandoms();
}
if (lastDownSample != downSample)
{
updateDownSample();
lastDownSample = downSample;
return;
}
CameraSpacePoint[] realWorldMap = multiSourceManager.GetRealWorldData();
byte[] bodyIndexMap = multiSourceManager.GetBodyIndexData();
//Texture2D tex = multiSourceManager.GetColorTexture();
bodyTree = new PointOctree <PCLPoint>(10, bodyCenter, .01f); //take previous frame
bodyPoints.Clear();
bodyCenter = new Vector3();
for (int ix = 0; ix < pointsWidth; ix++)
{
for (int iy = 0; iy < pointsHeight; iy++)
{
int dsIndex = iy * pointsWidth + ix;
Vector2 rIndex = dsOffsetRandoms[dsIndex];
int index = Mathf.RoundToInt(rIndex.x * multiSourceManager.DepthHeight * multiSourceManager.DepthWidth) + Mathf.RoundToInt(rIndex.y * multiSourceManager.DepthWidth);
//Vector3 dv = new Vector3(rIndex.x * 10, rIndex.y * 10);
//Debug.DrawLine(dv, dv + Vector3.forward * .2f, Color.red);
if (index >= realWorldMap.Length)
{
continue;
}
CameraSpacePoint csp = realWorldMap[index];
Vector3 p = new Vector3(csp.X, csp.Y, csp.Z);
Vector3 tPoint = transform.TransformPoint(p);
int tIndex = iy * pointsWidth + ix;
bodyMask[tIndex] = bodyIndexMap[index] != 255;
bool isBody = bodyMask[tIndex];
bool isValid = true;
if (isBody)
{
if (bodyRandomProba < 1)
{
isBody = Random.value <= bodyRandomProba;
}
}
if (float.IsNaN(tPoint.x) || float.IsInfinity(tPoint.x))
{
isValid = false;
tPoint = Vector3.zero;
}
PCLPoint pp = new PCLPoint(tPoint, isBody, isValid, true, isBody ? Color.yellow : Color.white);
points[tIndex] = pp;
if (isBody && isValid)
{
bodyPoints.Add(pp);
bodyTree.Add(pp, tPoint);
bodyCenter += tPoint;
}
if (debug && isValid)
{
if (isBody || !debugBodyOnly)
{
Color c = isBody ? Color.yellow : Color.white;
Debug.DrawLine(tPoint, tPoint + Vector3.forward * .05f, c);
}
}
}
}
if (bodyPoints.Count > 0)
{
bodyCenter /= bodyPoints.Count;
}
}
