public Vector3 GetJointPosColorOverlay(Int64 userId, int joint, Camera camera, Rect imageRect)
{
if (dictUserIdToIndex.ContainsKey(userId) && camera != null)
{
int index = dictUserIdToIndex[userId];
if (index >= 0 && index < sensorData.bodyCount &&
bodyFrame.bodyData[index].bIsTracked != 0)
{
if (joint >= 0 && joint < sensorData.jointCount)
{
KinectInterop.JointData jointData = bodyFrame.bodyData[index].joint[joint];
Vector3 posJointRaw = jointData.kinectPos;
if (posJointRaw != Vector3.zero)
{
// 3d position to depth
Vector2 posDepth = MapSpacePointToDepthCoords(posJointRaw);
ushort depthValue = GetDepthForPixel((int)posDepth.x, (int)posDepth.y);
if (posDepth != Vector2.zero && depthValue > 0 && sensorData != null)
{
// depth pos to color pos
Vector2 posColor = MapDepthPointToColorCoords(posDepth, depthValue);
if (!float.IsInfinity(posColor.x) && !float.IsInfinity(posColor.y))
{
// float xNorm = (float)posColor.x / sensorData.colorImageWidth;
// float yNorm = 1.0f - (float)posColor.y / sensorData.colorImageHeight;
float xScaled = (float)posColor.x * imageRect.width / sensorData.colorImageWidth;
float yScaled = (float)posColor.y * imageRect.height / sensorData.colorImageHeight;
float xScreen = imageRect.x + xScaled;
float yScreen = camera.pixelHeight - (imageRect.y + yScaled);
Plane cameraPlane = new Plane(camera.transform.forward, camera.transform.position);
float zDistance = cameraPlane.GetDistanceToPoint(posJointRaw);
//float zDistance = (jointData.kinectPos - camera.transform.position).magnitude;
//Vector3 vPosJoint = camera.ViewportToWorldPoint(new Vector3(xNorm, yNorm, zDistance));
Vector3 vPosJoint = camera.ScreenToWorldPoint(new Vector3(xScreen, yScreen, zDistance));
return vPosJoint;
}
}
}
}
}
}
return Vector3.zero;
}