// private Dictionary<Kinect.JointType, Kinect.JointType> _BoneMap = new Dictionary<Kinect.JointType, Kinect.JointType>()
// {
// { Kinect.JointType.FootLeft, Kinect.JointType.AnkleLeft },
// { Kinect.JointType.AnkleLeft, Kinect.JointType.KneeLeft },
// { Kinect.JointType.KneeLeft, Kinect.JointType.HipLeft },
// { Kinect.JointType.HipLeft, Kinect.JointType.SpineBase },
//
// { Kinect.JointType.FootRight, Kinect.JointType.AnkleRight },
// { Kinect.JointType.AnkleRight, Kinect.JointType.KneeRight },
// { Kinect.JointType.KneeRight, Kinect.JointType.HipRight },
// { Kinect.JointType.HipRight, Kinect.JointType.SpineBase },
//
// { Kinect.JointType.HandTipLeft, Kinect.JointType.HandLeft },
// { Kinect.JointType.ThumbLeft, Kinect.JointType.HandLeft },
// { Kinect.JointType.HandLeft, Kinect.JointType.WristLeft },
// { Kinect.JointType.WristLeft, Kinect.JointType.ElbowLeft },
// { Kinect.JointType.ElbowLeft, Kinect.JointType.ShoulderLeft },
// { Kinect.JointType.ShoulderLeft, Kinect.JointType.SpineShoulder },
//
// { Kinect.JointType.HandTipRight, Kinect.JointType.HandRight },
// { Kinect.JointType.ThumbRight, Kinect.JointType.HandRight },
// { Kinect.JointType.HandRight, Kinect.JointType.WristRight },
// { Kinect.JointType.WristRight, Kinect.JointType.ElbowRight },
// { Kinect.JointType.ElbowRight, Kinect.JointType.ShoulderRight },
// { Kinect.JointType.ShoulderRight, Kinect.JointType.SpineShoulder },
//
// { Kinect.JointType.SpineBase, Kinect.JointType.SpineMid },
// { Kinect.JointType.SpineMid, Kinect.JointType.SpineShoulder },
// { Kinect.JointType.SpineShoulder, Kinect.JointType.Neck },
// { Kinect.JointType.Neck, Kinect.JointType.Head },
// };
public Kinect.Body[] getData(out bool newFrame)
{
newFrame = _SourceManager.isNewFrame;
if (_SourceManager)
{
return(_SourceManager.GetBodyData());
}
else
{
return(null);
}
}
void Update()
{
_BodyData = _SourceManager.GetBodyData();
if (_BodyData != null)
{
// Update tracking ID array
for (int y = 0; y < trackingIDs.Length; y++)
{
trackingIDs[y].isTracking = false;
trackingIDs[y].index = -1;
}
// Check tracking status and assing old indexes
var arrayIndex = 0;
foreach (var body in _BodyData)
{
if (body.IsTracked)
{
for (int y = 0; y < trackingIDs.Length; y++)
{
if (trackingIDs[y].trackingId == body.TrackingId) // Body found in tracking IDs array
{
trackingIDs[y].isTracking = true; // Reset as tracked
trackingIDs[y].kinect2ArrayIndex = arrayIndex; // Set current kinect2 array index
if (trackingIDtoIndex.ContainsKey(body.TrackingId)) // If key added to trackingIDtoIndex array earlier...
{
trackingIDs[y].index = trackingIDtoIndex[body.TrackingId]; // Set old index
}
}
}
}
arrayIndex++;
}
// Add new bodies
arrayIndex = 0;
foreach (var body in _BodyData)
{
if (body.IsTracked)
{
if (!trackingIDtoIndex.ContainsKey(body.TrackingId)) // A new body
{
for (int y = 0; y < trackingIDs.Length; y++)
{
if (!trackingIDs[y].isTracking) // Find an array slot that does not have a tracked body
{
trackingIDs[y].index = y; // Set index to trackingIDs array index
trackingIDs[y].trackingId = body.TrackingId;
trackingIDtoIndex[body.TrackingId] = y; // Add tracking id to trackingIDtoIndex array
trackingIDs[y].kinect2ArrayIndex = arrayIndex;
trackingIDs[y].isTracking = true;
break;
}
}
}
}
arrayIndex++;
}
}
_DepthData = _SourceManager.GetDepthData();
_BodyData = _SourceManager.GetBodyData();
_BodyIndexData = _SourceManager.GetBodyIndexData();
Color[] mapPixels = new Color[(imageWidth / _DownsampleSize) * (imageHeight / _DownsampleSize)];
if (_DepthData != null && _BodyIndexData != null && _Sensor != null)
{
for (int y = 0; y < imageHeight; y += _DownsampleSize)
{
for (int x = 0; x < imageWidth; x += _DownsampleSize)
{
double depth = GetAvg(_DepthData, x, y, imageWidth, imageHeight);
depth = 1 - (depth / 4500);
if (_BodyIndexData[y * imageWidth + x] != 0xff)
{
int kinect2ArrayIndex = -2;
if (_BodyIndexData[y * imageWidth + x] == 0x0)
{
kinect2ArrayIndex = 0;
}
if (_BodyIndexData[y * imageWidth + x] == 0x1)
{
kinect2ArrayIndex = 1;
}
if (_BodyIndexData[y * imageWidth + x] == 0x2)
{
kinect2ArrayIndex = 2;
}
if (_BodyIndexData[y * imageWidth + x] == 0x3)
{
kinect2ArrayIndex = 3;
}
if (_BodyIndexData[y * imageWidth + x] == 0x4)
{
kinect2ArrayIndex = 4;
}
if (_BodyIndexData[y * imageWidth + x] == 0x5)
{
kinect2ArrayIndex = 5;
}
bool found = false;
for (int a = 0; a < trackingIDs.Length; a++)
{
if (trackingIDs[a].kinect2ArrayIndex == kinect2ArrayIndex && trackingIDs[a].isTracking)
{
if (trackingIDs[a].index == 0)
{
texture.SetPixel(x / _DownsampleSize, y / _DownsampleSize, new Color(0.0f, (float)depth, 0.0f, 1)); // Green
}
else
{
texture.SetPixel(x / _DownsampleSize, y / _DownsampleSize, Color.red);
}
found = true;
}
}
if (!found)
{
texture.SetPixel(x / _DownsampleSize, y / _DownsampleSize, Color.yellow);
}
}
else
{
texture.SetPixel(x / _DownsampleSize, y / _DownsampleSize, new Color((float)depth, 0.0f, 0.0f, 1)); // Red
}
}
}
}
if (_BodyData != null)
{
foreach (var body in _BodyData)
{
if (body.IsTracked)
{
drawLineBetweenJoints(ref mapPixels, body, Windows.Kinect.JointType.HandRight, Windows.Kinect.JointType.ElbowRight);
drawLineBetweenJoints(ref mapPixels, body, Windows.Kinect.JointType.HandLeft, Windows.Kinect.JointType.ElbowLeft);
drawLineBetweenJoints(ref mapPixels, body, Windows.Kinect.JointType.ShoulderRight, Windows.Kinect.JointType.SpineShoulder);
drawLineBetweenJoints(ref mapPixels, body, Windows.Kinect.JointType.ShoulderLeft, Windows.Kinect.JointType.SpineShoulder);
drawLineBetweenJoints(ref mapPixels, body, Windows.Kinect.JointType.ElbowRight, Windows.Kinect.JointType.ShoulderRight);
drawLineBetweenJoints(ref mapPixels, body, Windows.Kinect.JointType.ElbowLeft, Windows.Kinect.JointType.ShoulderLeft);
drawLineBetweenJoints(ref mapPixels, body, Windows.Kinect.JointType.SpineShoulder, Windows.Kinect.JointType.Head);
drawLineBetweenJoints(ref mapPixels, body, Windows.Kinect.JointType.SpineShoulder, Windows.Kinect.JointType.SpineBase);
drawLineBetweenJoints(ref mapPixels, body, Windows.Kinect.JointType.SpineBase, Windows.Kinect.JointType.HipRight);
drawLineBetweenJoints(ref mapPixels, body, Windows.Kinect.JointType.SpineBase, Windows.Kinect.JointType.HipLeft);
drawLineBetweenJoints(ref mapPixels, body, Windows.Kinect.JointType.HipRight, Windows.Kinect.JointType.KneeRight);
drawLineBetweenJoints(ref mapPixels, body, Windows.Kinect.JointType.HipLeft, Windows.Kinect.JointType.KneeLeft);
drawLineBetweenJoints(ref mapPixels, body, Windows.Kinect.JointType.KneeRight, Windows.Kinect.JointType.AnkleRight);
drawLineBetweenJoints(ref mapPixels, body, Windows.Kinect.JointType.KneeLeft, Windows.Kinect.JointType.AnkleLeft);
}
}
for (int i = 0; i < mapPixels.Length; i++)
{
if (mapPixels[i] == Color.white)
{
texture.SetPixel((int)Mathf.Round(i % (imageWidth / _DownsampleSize)), (int)Mathf.Round(i / (imageWidth / _DownsampleSize)), Color.white);
}
}
}
texture.Apply();
}
private Vector3 getSample(RUISDevice device)
{
Vector3 sample = new Vector3(0, 0, 0);
Vector3 tempSample;
updateBodyData();
if (device == RUISDevice.Kinect_2)
{
Kinect.Body[] data = kinect2SourceManager.GetBodyData();
bool trackedBodyFound = false;
int foundBodies = 0;
foreach (var body in data)
{
foundBodies++;
if (body.IsTracked)
{
if (trackingIDtoIndex[body.TrackingId] == 0)
{
trackedBodyFound = true;
if (body.Joints[Kinect.JointType.HandRight].TrackingState == Kinect.TrackingState.Tracked)
{
tempSample = new Vector3(body.Joints[Kinect.JointType.HandRight].Position.X,
body.Joints[Kinect.JointType.HandRight].Position.Y,
body.Joints[Kinect.JointType.HandRight].Position.Z);
tempSample = coordinateSystem.ConvertRawKinect2Location(tempSample);
if (Vector3.Distance(tempSample, lastKinect2Sample) > 0.1)
{
sample = tempSample;
lastKinect2Sample = sample;
device1Error = false;
if (!device2Error)
{
this.guiTextUpperLocal = "";
}
}
else
{
device1Error = true;
this.guiTextUpperLocal = "Not enough hand movement.";
}
}
}
}
}
if (!trackedBodyFound && foundBodies > 1)
{
device1Error = true;
this.guiTextUpperLocal = "Step out of the Kinect's\nview and come back.";
}
}
if (device == RUISDevice.Oculus_DK2)
{
Ovr.Posef headpose = RUISOVRManager.ovrHmd.GetTrackingState().HeadPose.ThePose;
float px = headpose.Position.x;
float py = headpose.Position.y;
float pz = -headpose.Position.z; // This needs to be negated TODO: might change with future OVR version
tempSample = new Vector3(px, py, pz);
tempSample = coordinateSystem.ConvertRawOculusDK2Location(tempSample);
if ((Vector3.Distance(tempSample, lastOculusDK2Sample) > 0.1) &&
(RUISOVRManager.ovrHmd.GetTrackingState().StatusFlags & (uint)StatusBits.PositionTracked) != 0) // Code from OVRManager.cs
{
sample = tempSample;
lastOculusDK2Sample = sample;
device2Error = false;
if (!device1Error)
{
this.guiTextUpperLocal = "";
}
}
else
{
device2Error = true;
this.guiTextUpperLocal = "Not enough hand movement.";
}
}
return(sample);
}
private Vector3 getSample(RUISDevice device) {
Vector3 sample = new Vector3(0,0,0);
Vector3 tempSample;
updateBodyData();
if(device == RUISDevice.Kinect_2)
{
Kinect.Body[] data = kinect2SourceManager.GetBodyData();
bool trackedBodyFound = false;
int foundBodies = 0;
foreach(var body in data)
{
foundBodies++;
if(body.IsTracked)
{
if(trackingIDtoIndex[body.TrackingId] == 0)
{
trackedBodyFound = true;
if(body.Joints[Kinect.JointType.HandRight].TrackingState == Kinect.TrackingState.Tracked)
{
tempSample = new Vector3(body.Joints[Kinect.JointType.HandRight].Position.X,
body.Joints[Kinect.JointType.HandRight].Position.Y,
body.Joints[Kinect.JointType.HandRight].Position.Z);
tempSample = coordinateSystem.ConvertRawKinect2Location(tempSample);
if(Vector3.Distance(tempSample, lastKinect2Sample) > 0.1)
{
sample = tempSample;
lastKinect2Sample = sample;
device1Error = false;
if(!device2Error) this.guiTextUpperLocal = "";
}
else
{
device1Error = true;
this.guiTextUpperLocal = "Not enough hand movement.";
}
}
}
}
}
if(!trackedBodyFound && foundBodies > 1)
{
device1Error = true;
this.guiTextUpperLocal = "Step out of the Kinect's\nview and come back.";
}
}
if(device == RUISDevice.PS_Move)
{
if(psMoveWrapper.sphereVisible[calibratingPSMoveControllerId] &&
psMoveWrapper.handleVelocity[calibratingPSMoveControllerId].magnitude <= 10.0f)
{
tempSample = coordinateSystem.ConvertRawPSMoveLocation(psMoveWrapper.handlePosition[calibratingPSMoveControllerId]);
if(Vector3.Distance(tempSample, lastPSMoveSample) > 0.1)
{
sample = tempSample;
lastPSMoveSample = sample;
device2Error = false;
if(!device1Error) this.guiTextUpperLocal = "";
}
else {
device2Error = true;
this.guiTextUpperLocal = "Not enough hand movement.";
}
}
}
return sample;
}
private Vector3 getSample(RUISDevice device)
{
Vector3 sample = new Vector3(0, 0, 0);
Vector3 tempSample;
updateBodyData();
if (device == RUISDevice.Kinect_2)
{
Kinect.Body[] data = kinect2SourceManager.GetBodyData();
bool trackedBodyFound = false;
int foundBodies = 0;
foreach (var body in data)
{
foundBodies++;
if (body.IsTracked)
{
if (trackingIDtoIndex[body.TrackingId] == 0)
{
trackedBodyFound = true;
if (body.Joints[Kinect.JointType.HandRight].TrackingState == Kinect.TrackingState.Tracked)
{
tempSample = new Vector3(body.Joints[Kinect.JointType.HandRight].Position.X,
body.Joints[Kinect.JointType.HandRight].Position.Y,
body.Joints[Kinect.JointType.HandRight].Position.Z);
tempSample = coordinateSystem.ConvertRawKinect2Location(tempSample);
if (Vector3.Distance(tempSample, lastKinect2Sample) > 0.2)
{
sample = tempSample;
lastKinect2Sample = sample;
device1Error = false;
if (!device2Error)
{
this.guiTextUpperLocal = "";
}
}
else
{
device1Error = true;
this.guiTextUpperLocal = "Not enough hand movement.";
}
}
}
}
}
if (!trackedBodyFound && foundBodies > 1)
{
device1Error = true;
this.guiTextUpperLocal = "Step out of the Kinect's\nview and come back.";
}
}
if (device == RUISDevice.Kinect_1)
{
OpenNI.SkeletonJointPosition jointPosition;
bool success = kinectSelection.GetPlayer(0).GetSkeletonJointPosition(OpenNI.SkeletonJoint.RightHand, out jointPosition);
if (success && jointPosition.Confidence >= 0.5)
{
tempSample = coordinateSystem.ConvertRawKinectLocation(jointPosition.Position);
if (Vector3.Distance(tempSample, lastKinectSample) > 0.2)
{
sample = tempSample;
lastKinectSample = sample;
device2Error = false;
if (!device1Error)
{
this.guiTextUpperLocal = "";
}
}
else
{
device2Error = true;
// this.guiTextUpperLocal = "Not enough hand movement.";
}
}
else
{
device2Error = true;
// this.guiTextUpperLocal = "Not enough hand movement.";
}
}
return(sample);
}