private void DrawDebugImg(KinectManager manager, int leftIdx, int rightIdx)
{
byte[] colorData = new byte[3 * manager.DepthData.Length];
float depthLimt = 1000 * manager.JointData[Kinect.JointType.SpineBase].Position.Z;
for (int i = 0; i < manager.DepthData.Length; i++)
{
if (manager.DepthData[i] < depthLimt)
{
colorData[3 * i] = (byte)(255 * (manager.DepthData[i] / depthLimt));
}
}
System.Action <int> colorRed = (idx) =>
{
colorData[3 * idx + 1] = 255;
};
colorRed(leftIdx);
colorRed(rightIdx);
//debugTexture.LoadRawTextureData(colorData);
Kinect.DepthSpacePoint leftHand = manager.Mapper.MapCameraPointToDepthSpace(manager.JointData[Kinect.JointType.HandLeft].Position);
Kinect.DepthSpacePoint rightHand = manager.Mapper.MapCameraPointToDepthSpace(manager.JointData[Kinect.JointType.HandRight].Position);
DrawLine(debugTexture, leftIdx % manager.DepthFrameDesc.Width, (int)leftIdx / manager.DepthFrameDesc.Width,
(int)leftHand.X, (int)leftHand.Y, Color.green);
DrawLine(debugTexture, rightIdx % manager.DepthFrameDesc.Width, (int)rightIdx / manager.DepthFrameDesc.Width,
(int)rightHand.X, (int)rightHand.Y, Color.green);
//debugTexture.Apply();
//debugPlane.GetComponent<Renderer>().material.mainTexture = debugTexture;
}
private float getDistanceWithDepthSpacePoint(Kinect.DepthSpacePoint p, Kinect.DepthSpacePoint q, ushort[] depthData)
{
Kinect.CameraSpacePoint P = _Mapper.MapDepthPointToCameraSpace(p, depthData[Pos2Idx((int)p.X, (int)p.Y)]);
Kinect.CameraSpacePoint Q = _Mapper.MapDepthPointToCameraSpace(q, depthData[Pos2Idx((int)q.X, (int)q.Y)]);
return(Mathf.Sqrt((P.X - Q.X) * (P.X - Q.X) + (P.Y - Q.Y) * (P.Y - Q.Y) + (P.Z - Q.Z) * (P.Z - Q.Z)));
}
/// <summary>
/// Maps the depth point to color coords.
/// </summary>
/// <returns>The depth point to color coords.</returns>
/// <param name="depthPos">Depth position.</param>
/// <param name="depthVal">Depth value.</param>
public Vector2 MapDepthPointToColorCoords(Vector2 depthPos, ushort depthVal)
{
Vector2 vPoint = Vector2.zero;
Kinect.CoordinateMapper coordMapper = m_MultiSourceManager.GetCoordinateMapper();
if (coordMapper != null && depthPos != Vector2.zero)
{
Kinect.DepthSpacePoint depthPoint = new Kinect.DepthSpacePoint();
depthPoint.X = depthPos.x;
depthPoint.Y = depthPos.y;
Kinect.DepthSpacePoint[] depthPoints = new Kinect.DepthSpacePoint[1];
depthPoints [0] = depthPoint;
ushort[] depthVals = new ushort[1];
depthVals [0] = depthVal;
Kinect.ColorSpacePoint[] colPoints = new Kinect.ColorSpacePoint[1];
coordMapper.MapDepthPointsToColorSpace(depthPoints, depthVals, colPoints);
Kinect.ColorSpacePoint colPoint = colPoints [0];
vPoint.x = colPoint.X;
vPoint.y = colPoint.Y;
}
return vPoint;
}
int getStickEnd(ushort[] depthData, Kinect.CameraSpacePoint handCameraPoint, Kinect.DepthSpacePoint handDepthPoint)
{
//depthData = (ushort[])depthData.Clone();
//cutDepthData(depthData, (int)handDepthPoint.Y + 100, (int)(handCameraPoint.Z * 1000));
return(DFS(depthData, (int)handDepthPoint.X, (int)handDepthPoint.Y));
}
void drawLineBetweenJoints(ref Color[] mapPixels, Windows.Kinect.Body body, Windows.Kinect.JointType jointA, Windows.Kinect.JointType jointB)
{
depthSpacePoint_1 = _Sensor.CoordinateMapper.MapCameraPointToDepthSpace(body.Joints[jointA].Position);
depthSpacePoint_2 = _Sensor.CoordinateMapper.MapCameraPointToDepthSpace(body.Joints[jointB].Position);
DrawLine.DrawSimpleLine(ref mapPixels,
(int)depthSpacePoint_1.X / _DownsampleSize, (int)depthSpacePoint_1.Y / _DownsampleSize,
(int)depthSpacePoint_2.X / _DownsampleSize, (int)depthSpacePoint_2.Y / _DownsampleSize,
imageWidth / _DownsampleSize, imageHeight / _DownsampleSize,
Color.white);
}
// Token: 0x060029C7 RID: 10695 RVA: 0x000D4E9C File Offset: 0x000D329C
public DepthSpacePoint MapCameraPointToDepthSpace(CameraSpacePoint cameraPoint)
{
if (this._pNative == IntPtr.Zero)
{
throw new ObjectDisposedException("CoordinateMapper");
}
IntPtr intPtr = CoordinateMapper.Windows_Kinect_CoordinateMapper_MapCameraPointToDepthSpace(this._pNative, cameraPoint);
ExceptionHelper.CheckLastError();
DepthSpacePoint result = (DepthSpacePoint)Marshal.PtrToStructure(intPtr, typeof(DepthSpacePoint));
Marshal.FreeHGlobal(intPtr);
return(result);
}
// Token: 0x060029CD RID: 10701 RVA: 0x000D4FB4 File Offset: 0x000D33B4
public ColorSpacePoint MapDepthPointToColorSpace(DepthSpacePoint depthPoint, ushort depth)
{
if (this._pNative == IntPtr.Zero)
{
throw new ObjectDisposedException("CoordinateMapper");
}
IntPtr intPtr = CoordinateMapper.Windows_Kinect_CoordinateMapper_MapDepthPointToColorSpace(this._pNative, depthPoint, depth);
ExceptionHelper.CheckLastError();
ColorSpacePoint result = (ColorSpacePoint)Marshal.PtrToStructure(intPtr, typeof(ColorSpacePoint));
Marshal.FreeHGlobal(intPtr);
return(result);
}
public ColorSpacePoint MapDepthPointToColorSpace(DepthSpacePoint depthPoint,
ushort depth)
{
if (_pNative == RootSystem.IntPtr.Zero)
{
throw new RootSystem.ObjectDisposedException("CoordinateMapper");
}
var objectPointer = Windows_Kinect_CoordinateMapper_MapDepthPointToColorSpace(_pNative, depthPoint, depth);
ExceptionHelper.CheckLastError();
var obj = (ColorSpacePoint)Marshal.PtrToStructure(
objectPointer, typeof(ColorSpacePoint));
Marshal.FreeHGlobal(objectPointer);
return(obj);
}
bool[] getStickIndex(ushort[] depthData)
{
bool[] stickIndex = new bool[depthData.Length];
for (int i = 0; i < depthData.Length; i++)
{
stickIndex[i] = false;
}
Kinect.CameraSpacePoint leftHandCameraPoint = joints[Kinect.JointType.HandLeft].Position;
Kinect.DepthSpacePoint leftHandDepthPoint = mapper.MapCameraPointToDepthSpace(leftHandCameraPoint);
Kinect.CameraSpacePoint rightHandCameraPoint = joints[Kinect.JointType.HandRight].Position;
Kinect.DepthSpacePoint rightHandDepthPoint = mapper.MapCameraPointToDepthSpace(rightHandCameraPoint);
depthData = (ushort[])depthData.Clone();
cutDepthData(depthData, (int)Mathf.Min(leftHandDepthPoint.Y, rightHandDepthPoint.Y) + 300, (int)(Mathf.Max(leftHandCameraPoint.Z, rightHandCameraPoint.Z) * 1000));
// left hand
int leftStickEndInd = getStickEnd(depthData, leftHandCameraPoint, leftHandDepthPoint);
bool useCache = leftStickEndInd == 0;
if (useCache)
{
return(null);
}
int rightStickEndInd = getStickEnd(depthData, rightHandCameraPoint, rightHandDepthPoint);
useCache |= rightStickEndInd == 0;
if (useCache)
{
return(null);
}
depthDataCache = (ushort[])depthData.Clone();
addStickIndex(stickIndex, depthData, leftHandDepthPoint, leftStickEndInd);
// right hand
addStickIndex(stickIndex, depthData, rightHandDepthPoint, rightStickEndInd);
stickIndexCache = (bool[])stickIndex.Clone();
return(stickIndex);
}
void ZigTrackedUserCenter(KinectOneLabelMap labelmap)
{
if (m_engageuser == null || m_engageuser.engagedTrackedUser == null)
{
trackedUserIndex = 255;
return;
}
ZigTrackedUser _engagedUser = m_engageuser.engagedTrackedUser;
Kinect.CameraSpacePoint positionPoint = new Kinect.CameraSpacePoint();
positionPoint.X = _engagedUser.Position.x;
positionPoint.Y = _engagedUser.Position.y;
positionPoint.Z = _engagedUser.Position.z;
Kinect.DepthSpacePoint depthSpacePoint = _mapper.MapCameraPointToDepthSpace(positionPoint);
short[] labelData = labelmap.data;
int index = (int)(depthSpacePoint.Y * depthInfo.xres + depthSpacePoint.X);
trackedUserIndex = (index >= 0 && index < labelData.Length) ? labelData[index] : (short)255;
}
void addStickIndex(bool[] stickIndex, ushort[] depthData, Kinect.DepthSpacePoint hand, int stickEndInd)
{
int handX = (int)hand.X;
int handY = (int)hand.Y;
int stickEndX = stickEndInd % depthFrameInfo.Width;
int stickEndY = stickEndInd / depthFrameInfo.Width;
float slope = (float)(handY - stickEndY) / (float)(handX - stickEndX);
int direction = stickEndX > handX ? 1 : -1;
for (int x = handX; x != stickEndX; x += direction)
{
int y = (int)(handY + (x - handX) * slope);
if (pos2ind(x, y) < depthData.Length)
{
stickIndex[pos2ind(x, y)] = true;
}
}
}
public bool Equals(DepthSpacePoint obj)
{
return X.Equals(obj.X) && Y.Equals(obj.Y);
}
// 3次元座標の取得
void calcColorPointToWorld(ushort[] depthData, int colorWidth, int colorHeight, int[] srcPointx, int[] srcPointy, int[] proPointx, int[] proPointy, int srcLen,
ref List<double> dstPoint2dx, ref List<double> dstPoint2dy, ref List<double> dstPoint3dx, ref List<double> dstPoint3dy, ref List<double> dstPoint3dz, ref int dstLen)
{
var frameDesc = KinectSensor.GetDefault().DepthFrameSource.FrameDescription;
// depthをcolorに合わせる
DepthSpacePoint[] depthSpace = new DepthSpacePoint[colorWidth * colorHeight];
mapper.MapColorFrameToDepthSpace(depthData, depthSpace);
// depthをcameraに合わせる
CameraSpacePoint[] cameraSpace = new CameraSpacePoint[depthData.Length];
mapper.MapDepthFrameToCameraSpace(depthData, cameraSpace);
dstPoint2dx.Clear();
dstPoint2dy.Clear();
dstPoint3dx.Clear();
dstPoint3dy.Clear();
dstPoint3dz.Clear();
dstLen = 0;
for (int i = 0; i < srcLen; ++i)
{
int src_index = srcPointy[i] * multiSourceManagerScript.ColorWidth + srcPointx[i];
int dst_x = (int)(depthSpace[src_index].X);
int dst_y = (int)(depthSpace[src_index].Y);
int dst_index = dst_y * frameDesc.Width + dst_x;
if( ((0<=dst_x) && (dst_x < frameDesc.Width)) && ((0<=dst_y) && (dst_y<frameDesc.Height)) )
{
if( (0.5f<=cameraSpace[dst_index].Z) && (cameraSpace[dst_index].Z<=8.0f) )
{
dstPoint2dx.Add((double)(proPointx[i]));
dstPoint2dy.Add((double)(proPointy[i]));
dstPoint3dx.Add(-cameraSpace[dst_index].X);
dstPoint3dy.Add(cameraSpace[dst_index].Y);
dstPoint3dz.Add(cameraSpace[dst_index].Z);
dstLen++;
}
}
}
}
public void FindTip(KinectManager manager,
out Kinect.CameraSpacePoint leftTipCameraPoint, out Kinect.CameraSpacePoint rightTipCameraPoint,
out bool leftStatus, out bool rightStatus)
{
Kinect.CameraSpacePoint leftHandCameraPoint = manager.JointData[Kinect.JointType.HandLeft].Position;
Kinect.DepthSpacePoint leftHandDepthPoint = manager.Mapper.MapCameraPointToDepthSpace(leftHandCameraPoint);
Kinect.CameraSpacePoint rightHandCameraPoint = manager.JointData[Kinect.JointType.HandRight].Position;
Kinect.DepthSpacePoint rightHandDepthPoint = manager.Mapper.MapCameraPointToDepthSpace(rightHandCameraPoint);
byte[] colorData = new byte[3 * manager.DepthData.Length];
ushort[] tempDepthData = new ushort[manager.DepthData.Length];
tempDepthData = (ushort[])manager.DepthData.Clone();
for (int i = 0; i < tempDepthData.Length; i++)
{
if (tempDepthData[i] > (tempDepthData[Pos2Idx((int)leftHandDepthPoint.X, (int)leftHandDepthPoint.Y)] + 30))
{
tempDepthData[i] = 0;
}
}
int leftStickEndIdx = GetStickEnd(tempDepthData, leftHandCameraPoint, leftHandDepthPoint, ref colorData);
if (leftStickEndIdx == 0)
{
leftTipCameraPoint = _LeftTipCache;
if (_LeftCacheElapsedFrame >= 10)
{
leftStatus = false;
}
else
{
leftStatus = true;
_LeftCacheElapsedFrame++;
}
}
else
{
Kinect.DepthSpacePoint leftTipDepthPoint = new Kinect.DepthSpacePoint();
leftTipDepthPoint.X = leftStickEndIdx % _Width;
leftTipDepthPoint.Y = leftStickEndIdx / _Width;
leftTipCameraPoint = manager.Mapper.MapDepthPointToCameraSpace(leftTipDepthPoint, manager.DepthData[leftStickEndIdx]);
_LeftTipCache = leftTipCameraPoint;
_LeftCacheElapsedFrame = 0;
leftStatus = true;
}
tempDepthData = (ushort[])manager.DepthData.Clone();
for (int i = 0; i < tempDepthData.Length; i++)
{
if (tempDepthData[i] > (tempDepthData[Pos2Idx((int)rightHandDepthPoint.X, (int)rightHandDepthPoint.Y)] + 30))
{
tempDepthData[i] = 0;
}
}
//int rightStickEndIdx = GetStickEnd(manager.DepthData, rightHandCameraPoint, rightHandDepthPoint);
int rightStickEndIdx = GetStickEnd(tempDepthData, rightHandCameraPoint, rightHandDepthPoint, ref colorData);
if (rightStickEndIdx == 0)
{
rightTipCameraPoint = _RightTipCache;
if (_RightCacheElapsedFrame >= 10)
{
rightStatus = false;
}
else
{
rightStatus = true;
_RightCacheElapsedFrame++;
}
}
else
{
Kinect.DepthSpacePoint rightTipDepthPoint = new Kinect.DepthSpacePoint();
rightTipDepthPoint.X = rightStickEndIdx % _Width;
rightTipDepthPoint.Y = rightStickEndIdx / _Width;
rightTipCameraPoint = manager.Mapper.MapDepthPointToCameraSpace(rightTipDepthPoint, manager.DepthData[rightStickEndIdx]);
_RightTipCache = rightTipCameraPoint;
_RightCacheElapsedFrame = 0;
rightStatus = true;
}
ushort[] depthData = manager.DepthData;
//DrawDebugImg(manager, leftStickEndIdx, rightStickEndIdx);
}
// 1点の3次元座標の取得
void calcColorPointToWorld(ushort[] depthData, int colorWidth, int colorHeight, List<int> srcPoint, ref List<double> dstPoint)
{
var frameDesc = KinectSensor.GetDefault().DepthFrameSource.FrameDescription;
// depthをcolorに合わせる
DepthSpacePoint[] depthSpace = new DepthSpacePoint[colorWidth * colorHeight];
mapper.MapColorFrameToDepthSpace(depthData, depthSpace);
// depthをcameraに合わせる
CameraSpacePoint[] cameraSpace = new CameraSpacePoint[depthData.Length];
mapper.MapDepthFrameToCameraSpace(depthData, cameraSpace);
dstPoint.Clear();
int src_index = srcPoint[1] * multiSourceManagerScript.ColorWidth + srcPoint[0];
int dst_x = (int)(depthSpace[src_index].X);
int dst_y = (int)(depthSpace[src_index].Y);
int dst_index = dst_y * frameDesc.Width + dst_x;
if (((0 <= dst_x) && (dst_x < frameDesc.Width)) && ((0 <= dst_y) && (dst_y < frameDesc.Height)))
{
if ((0.5f <= cameraSpace[dst_index].Z) && (cameraSpace[dst_index].Z <= 8.0f))
{
dstPoint.Add(-cameraSpace[dst_index].X);
dstPoint.Add(cameraSpace[dst_index].Y);
dstPoint.Add(cameraSpace[dst_index].Z);
}
}
}
private void DFS_helper(ref DFS_Datas datas, int start)
{
datas.visited.Add(start);
ushort curDep = datas.depthData[start];
if (curDep == 0)
{
return;
}
int x = start % _Width;
int y = start / _Width;
Kinect.DepthSpacePoint curDSP = new Kinect.DepthSpacePoint();
curDSP.X = x;
curDSP.Y = y;
Kinect.DepthSpacePoint handDSP = new Kinect.DepthSpacePoint();
handDSP.X = datas.handIndX;
handDSP.Y = datas.handIndY;
float len = getDistanceWithDepthSpacePoint(curDSP, handDSP, datas.depthData);
if (len > 0.15)
{
float slope = (float)(datas.handIndY - y) / (x - datas.handIndX);
float angle = Mathf.Atan(slope);
float distance = 30;
int dx = (int)(distance * Mathf.Cos(angle));
int dy = (int)(distance * Mathf.Sin(angle));
int outThreshold = 400;
ushort[] depthData = datas.depthData;
System.Func <int, int, bool> isBadNeighbor = (X, Y) =>
{
return(validateDepthPosition(X, Y) && Mathf.Abs(depthData[Pos2Idx(X, Y)] - curDep) <= outThreshold);
};
if (isBadNeighbor(x + dx, y + dy) || isBadNeighbor(x - dx, y - dy))
{
return;
}
}
if (len > datas.max)
{
datas.max = len;
datas.maxX = x;
datas.maxY = y;
}
int walk = 1;
//int inThreshold = len < 800 ? 1500 : 300;
//System.Func<int, int, bool> depthCondition = (X, Y) => { return Mathf.Abs(depthData[Pos2Idx(X, Y)] - curDep) < inThreshold; };
System.Func <int, int, bool> depthCondition = (X, Y) => { return(true); };
int newX = x + walk, newY = y;
if (validateDepthPosition(newX, newY) && depthCondition(newX, newY) && !datas.visited.Contains(Pos2Idx(newX, newY)))
{
DFS_helper(ref datas, Pos2Idx(newX, newY));
}
newX = x - walk;
if (validateDepthPosition(newX, newY) && depthCondition(newX, newY) && !datas.visited.Contains(Pos2Idx(newX, newY)))
{
DFS_helper(ref datas, Pos2Idx(newX, newY));
}
newX = x;
newY = y + walk;
if (validateDepthPosition(newX, newY) && depthCondition(newX, newY) && !datas.visited.Contains(Pos2Idx(newX, newY)))
{
DFS_helper(ref datas, Pos2Idx(newX, newY));
}
newY = y - walk;
if (validateDepthPosition(newX, newY) && depthCondition(newX, newY) && !datas.visited.Contains(Pos2Idx(newX, newY)))
{
DFS_helper(ref datas, Pos2Idx(newX, newY));
}
}
private int GetStickEnd(ushort[] depthData, Kinect.CameraSpacePoint handCameraPoint, Kinect.DepthSpacePoint handDepthPoint, ref byte[] colorData)
{
return(DFS(depthData, (int)handDepthPoint.X, (int)handDepthPoint.Y, ref colorData));
}
private static extern RootSystem.IntPtr Windows_Kinect_CoordinateMapper_MapDepthPointToCameraSpace(
RootSystem.IntPtr pNative, DepthSpacePoint depthPoint, ushort depth);
/// <summary>
/// Maps the space point to depth coords.
/// </summary>
/// <returns>The space point to depth coords.</returns>
/// <param name="spacePos">Space position.</param>
private Vector2 MapSpacePointToDepthCoords(Vector3 spacePos)
{
Vector2 vPoint = Vector2.zero;
Kinect.CoordinateMapper coordMapper = m_MultiSourceManager.GetCoordinateMapper();
if (coordMapper != null)
{
Kinect.CameraSpacePoint camPoint = new Kinect.CameraSpacePoint();
camPoint.X = spacePos.x;
camPoint.Y = spacePos.y;
camPoint.Z = spacePos.z;
Kinect.CameraSpacePoint[] camPoints = new Kinect.CameraSpacePoint[1];
camPoints [0] = camPoint;
Kinect.DepthSpacePoint[] depthPoints = new Kinect.DepthSpacePoint[1];
coordMapper.MapCameraPointsToDepthSpace(camPoints, depthPoints);
Kinect.DepthSpacePoint depthPoint = depthPoints [0];
if (depthPoint.X >= 0 && depthPoint.X < cDepthImageWidth &&
depthPoint.Y >= 0 && depthPoint.Y < cDepthImageHeight)
{
vPoint.x = depthPoint.X;
vPoint.y = depthPoint.Y;
}
}
return vPoint;
}
public Vector3 MapDepthPointToSpaceCoords(KinectInterop.SensorData sensorData, Vector2 depthPos, ushort depthVal)
{
Vector3 vPoint = Vector3.zero;
if(coordMapper != null && depthPos != Vector2.zero)
{
DepthSpacePoint depthPoint = new DepthSpacePoint();
depthPoint.X = depthPos.x;
depthPoint.Y = depthPos.y;
DepthSpacePoint[] depthPoints = new DepthSpacePoint[1];
depthPoints[0] = depthPoint;
ushort[] depthVals = new ushort[1];
depthVals[0] = depthVal;
CameraSpacePoint[] camPoints = new CameraSpacePoint[1];
coordMapper.MapDepthPointsToCameraSpace(depthPoints, depthVals, camPoints);
CameraSpacePoint camPoint = camPoints[0];
vPoint.x = camPoint.X;
vPoint.y = camPoint.Y;
vPoint.z = camPoint.Z;
}
return vPoint;
}
public bool Equals(DepthSpacePoint obj)
{
return(X.Equals(obj.X) && Y.Equals(obj.Y));
}
// Token: 0x06002C82 RID: 11394 RVA: 0x000DDB3C File Offset: 0x000DBF3C
public bool Equals(DepthSpacePoint obj)
{
return(this.X.Equals(obj.X) && this.Y.Equals(obj.Y));
}
public Vector2 MapDepthPointToColorCoords(KinectInterop.SensorData sensorData, Vector2 depthPos, ushort depthVal)
{
Vector2 vPoint = Vector2.zero;
if(coordMapper != null && depthPos != Vector2.zero)
{
DepthSpacePoint depthPoint = new DepthSpacePoint();
depthPoint.X = depthPos.x;
depthPoint.Y = depthPos.y;
DepthSpacePoint[] depthPoints = new DepthSpacePoint[1];
depthPoints[0] = depthPoint;
ushort[] depthVals = new ushort[1];
depthVals[0] = depthVal;
ColorSpacePoint[] colPoints = new ColorSpacePoint[1];
coordMapper.MapDepthPointsToColorSpace(depthPoints, depthVals, colPoints);
ColorSpacePoint colPoint = colPoints[0];
vPoint.x = colPoint.X;
vPoint.y = colPoint.Y;
}
return vPoint;
}
private static extern IntPtr Windows_Kinect_CoordinateMapper_MapDepthPointToColorSpace(IntPtr pNative, DepthSpacePoint depthPoint, ushort depth);
public Vector2 MapSpacePointToDepthCoords(KinectInterop.SensorData sensorData, Vector3 spacePos)
{
Vector2 vPoint = Vector2.zero;
if(coordMapper != null)
{
CameraSpacePoint camPoint = new CameraSpacePoint();
camPoint.X = spacePos.x;
camPoint.Y = spacePos.y;
camPoint.Z = spacePos.z;
CameraSpacePoint[] camPoints = new CameraSpacePoint[1];
camPoints[0] = camPoint;
DepthSpacePoint[] depthPoints = new DepthSpacePoint[1];
coordMapper.MapCameraPointsToDepthSpace(camPoints, depthPoints);
DepthSpacePoint depthPoint = depthPoints[0];
if(depthPoint.X >= 0 && depthPoint.X < sensorData.depthImageWidth &&
depthPoint.Y >= 0 && depthPoint.Y < sensorData.depthImageHeight)
{
vPoint.x = depthPoint.X;
vPoint.y = depthPoint.Y;
}
}
return vPoint;
}
// // Polls for new color histogram frame data
// public static bool PollUserHistogramFrame(ref UserHistogramBuffer userHistImage, bool bUseColorData)
// {
// bool bNewFrame = false;
//
// IntPtr imagePtr = IntPtr.Zero;
// Int64 liFrameTime = 0;
//
// int hr = GetUserHistogramFrameData(ref imagePtr, ref liFrameTime, bUseColorData);
//
// if(hr == 0)
// {
// userHistImage = (UserHistogramBuffer)Marshal.PtrToStructure(imagePtr, typeof(UserHistogramBuffer));
// bNewFrame = true;
// }
//
// return bNewFrame;
// }
// returns depth frame coordinates for the given 3d Kinect-space point
public static Vector2 GetKinectPointDepthCoords(SensorData sensorData, Vector3 kinectPos)
{
Vector2 vPoint = Vector2.zero;
// int dx = 0, dy = 0;
// int hr = GetKinectPointDepthCoords(kinectPos.x, kinectPos.y, kinectPos.z, out dx, out dy);
//
// if(hr == 0)
// {
// return new Vector2(dx, dy);
// }
if(sensorData.coordMapper != null)
{
CameraSpacePoint camPoint = new CameraSpacePoint();
camPoint.X = kinectPos.x;
camPoint.Y = kinectPos.y;
camPoint.Z = kinectPos.z;
CameraSpacePoint[] camPoints = new CameraSpacePoint[1];
camPoints[0] = camPoint;
DepthSpacePoint[] depthPoints = new DepthSpacePoint[1];
sensorData.coordMapper.MapCameraPointsToDepthSpace(camPoints, depthPoints);
DepthSpacePoint depthPoint = depthPoints[0];
if(depthPoint.X >= 0 && depthPoint.X < sensorData.depthImageWidth &&
depthPoint.Y >= 0 && depthPoint.Y < sensorData.depthImageHeight)
{
vPoint.x = depthPoint.X;
vPoint.y = depthPoint.Y;
}
}
return vPoint;
}
