public static void DrawBones(ModelVisual3D model, BodyPosQuatData bpqd)
{
DrawIfExistsUsingTrackingState(model, bpqd, JointType.Head, JointType.Neck);
DrawIfExistsUsingTrackingState(model, bpqd, JointType.Neck, JointType.SpineShoulder);
DrawIfExistsUsingTrackingState(model, bpqd, JointType.SpineShoulder, JointType.SpineMid);
DrawIfExistsUsingTrackingState(model, bpqd, JointType.SpineMid, JointType.SpineBase);
DrawIfExistsUsingTrackingState(model, bpqd, JointType.SpineShoulder, JointType.ShoulderRight);
DrawIfExistsUsingTrackingState(model, bpqd, JointType.SpineShoulder, JointType.ShoulderLeft);
DrawIfExistsUsingTrackingState(model, bpqd, JointType.SpineBase, JointType.HipRight);
DrawIfExistsUsingTrackingState(model, bpqd, JointType.SpineBase, JointType.HipLeft);
// Right Arm
DrawIfExistsUsingTrackingState(model, bpqd, JointType.ShoulderRight, JointType.ElbowRight);
DrawIfExistsUsingTrackingState(model, bpqd, JointType.ElbowRight, JointType.WristRight);
//DrawIfExistsUsingTrackingState(model, bpqd, JointType.WristRight, JointType.HandRight);
//DrawIfExistsUsingTrackingState(model, bpqd, JointType.HandRight, JointType.HandTipRight);
//DrawIfExistsUsingTrackingState(model, bpqd, JointType.WristRight, JointType.ThumbRight);
// Left Arm
DrawIfExistsUsingTrackingState(model, bpqd, JointType.ShoulderLeft, JointType.ElbowLeft);
DrawIfExistsUsingTrackingState(model, bpqd, JointType.ElbowLeft, JointType.WristLeft);
//DrawIfExistsUsingTrackingState(model, bpqd, JointType.WristLeft, JointType.HandLeft);
//DrawIfExistsUsingTrackingState(model, bpqd, JointType.HandLeft, JointType.HandTipLeft);
//DrawIfExistsUsingTrackingState(model, bpqd, JointType.WristLeft, JointType.ThumbLeft);
// Right Leg
DrawIfExistsUsingTrackingState(model, bpqd, JointType.HipRight, JointType.KneeRight);
DrawIfExistsUsingTrackingState(model, bpqd, JointType.KneeRight, JointType.AnkleRight);
DrawIfExistsUsingTrackingState(model, bpqd, JointType.AnkleRight, JointType.FootRight);
// Left Leg
DrawIfExistsUsingTrackingState(model, bpqd, JointType.HipLeft, JointType.KneeLeft);
DrawIfExistsUsingTrackingState(model, bpqd, JointType.KneeLeft, JointType.AnkleLeft);
DrawIfExistsUsingTrackingState(model, bpqd, JointType.AnkleLeft, JointType.FootLeft);
}
public bool AddBodyPosQuatData(BodyPosQuatData bpqd)
{
if (this.KQBVDQeueu.Count < 1)
{
this.KQBVDQeueu.Enqueue(bpqd);
return(true);
}
else
{
return(false);
}
}
protected void SetupAndConnectKinect()
{
this.DPMC.AddKinectSensor();
(this.DPMC[KinectProducer.KinectAddress].IDataProducer as IDataProducer <KinectData>).NewTData += (senderNewTData, eNewTData) =>
{
//Quaternion q = this.GetLeftForearmQuat((KinectData)eNewIData);
bpqd = BodyPosQuatData.GetBodyPosQuatDataFromBody(eNewTData.Body);
RightArmQuat = ((KinectData)eNewTData).Body.JointOrientations[JointType.WristRight].Orientation.ToQuaternion();
((BluetoothController.WPFUserControls.SkeletonQuatBodyViewer) this.QuatBodyViewerHost.Child).AddBodyPosQuatData(bpqd);
BeginInvoke((Action) delegate
{
((BluetoothController.WPFUserControls.QuaternionArrow) this.QuatArrowHost.Child).Update(eNewTData.Body.JointOrientations[JointType.WristRight].Orientation.ToQuaternion());
});
};
}
protected void SetupAndConnectKinect()
{
this.DPMC.AddKinectSensor();
(this.DPMC[KinectProducer.KinectAddress].IDataProducer as IDataProducer <KinectData>).NewTData += (senderNewTData, eNewTData) =>
{
//Quaternion q = this.GetLeftForearmQuat((KinectData)eNewIData);
bpqd = BodyPosQuatData.GetBodyPosQuatDataFromBody(eNewTData.Body);
RightArmQuat = ((KinectData)eNewTData).Body.JointOrientations[JointType.WristRight].Orientation.ToQuaternion();
((BluetoothController.WPFUserControls.SkeletonQuatBodyViewer) this.QuatBodyViewerHost.Child).AddBodyPosQuatData(bpqd);
BeginInvoke((Action) delegate
{
((BluetoothController.WPFUserControls.QuaternionArrow) this.QuatArrowHost.Child).Update(eNewTData.Body.JointOrientations[JointType.WristRight].Orientation.ToQuaternion());
});
};
//Microsoft.Kinect.JointType[] jtoi = new Microsoft.Kinect.JointType[2];
//jtoi[0] = Microsoft.Kinect.JointType.HandRight;
//jtoi[1] = Microsoft.Kinect.JointType.HandLeft;
//jointreader = new KinectProducer(jtoi);
//this.DictionaryOfIDataReciever["Kinect"] = jointreader;
//this.DictionaryOfUISensorData["Kinect"] = new UISensorData(false) { DeviceName = "Kinect", MPS = -1, SensorType = SensorType.Kinect, DataPreview = "Waiting for Data Preivew" };
//((IRestartable)this.DictionaryOfIDataReciever["Kinect"]).Start();
//this.DictionaryOfUISensorData["KinectPos"] = new UISensorData(false) { DeviceName = "KinectPos", SensorType = SensorType.Kinect };
//this.AddChartAreaAndSetup(this.AccelChart, "KinectPos");
//this.SetupChartAreaForInertial(this.AccelChart, "KinectPos");
//this.DictionaryOfIDataReciever["Kinect"].NewIData += (senderNewIData, eNewIData) =>
//{
// //Quaternion q = this.GetLeftForearmQuat((KinectData)eNewIData);
// bpqd = BodyPosQuatData.GetBodyPosQuatDataFromBody(((KinectData)eNewIData).Body);
// //RightArmQuat = new Quaternion(((KinectData)eNewIData).Body.JointOrientations[JointType.WristRight].Orientation);
// ((BluetoothController.WPFUserControls.SkeletonQuatBodyViewer)this.QuatBodyViewerHost.Child).AddBodyPosQuatData(bpqd);
// DictionaryOfUISensorData["Kinect"].DataPreview = eNewIData.ToPreviewString();
// this.UpDateChartWithDataFromKinect(this.AccelChart, "KinectPos", (KinectData)eNewIData);
// BeginInvoke((Action)delegate
// {
// ((BluetoothController.WPFUserControls.QuaternionArrow)this.QuatArrowHost.Child).Update(/*new Quaternion(((KinectData)eNewIData).Body.JointOrientations[JointType.ShoulderRight].Orientation) * new Quaternion(((KinectData)eNewIData).Body.JointOrientations[JointType.ElbowRight].Orientation) */ new Quaternion(((KinectData)eNewIData).Body.JointOrientations[JointType.WristRight].Orientation));
// });
//};
//this.DictionaryOfIDataReciever["Kinect"].MeasuresPerSec += (senderMPS, eMPS) =>
//{
// this.DictionaryOfUISensorData["Kinect"].MPS = eMPS;
//};
}
public static void DrawIfExistsUsingTrackingState(ModelVisual3D model, BodyPosQuatData bpqd, JointType j1, JointType j2)
{
if (bpqd.JointConfidences[j1] == TrackingState.Tracked && bpqd.JointConfidences[j2] == TrackingState.Tracked)
{
CreateALine(model, bpqd.JointPoints[j1], bpqd.JointPoints[j2], Colors.Green);
}
else if (bpqd.JointConfidences[j1] == TrackingState.Tracked && bpqd.JointConfidences[j2] == TrackingState.Inferred ||
bpqd.JointConfidences[j1] == TrackingState.Inferred && bpqd.JointConfidences[j2] == TrackingState.Tracked)
{
CreateALine(model, bpqd.JointPoints[j1], bpqd.JointPoints[j2], Colors.Yellow);
}
else if (bpqd.JointConfidences[j1] == TrackingState.Inferred && bpqd.JointConfidences[j2] == TrackingState.Inferred)
{
CreateALine(model, bpqd.JointPoints[j1], bpqd.JointPoints[j2], Colors.Green);
}
}
public static void CreateKinectQuatBody(ModelVisual3D model, BodyPosQuatData bpqd)
{
Color[] TrackedColors = new Color[] { Color.FromRgb(152, 152, 152),
Color.FromRgb(30, 187, 238),
Color.FromRgb(68, 35, 80),
Colors.Green };
Color[] InferredColors = new Color[] { Color.FromRgb(152, 152, 152),
Color.FromRgb(30, 187, 238),
Color.FromRgb(68, 35, 80),
Colors.Red };
model.Children.Clear();
foreach (JointType j in bpqd.JointQuaternions.Keys)
{
KinectQuatViewer.CreateACoordinateArrow(model, bpqd.JointPoints[j], bpqd.JointQuaternions[j], bpqd.JointConfidences[j] == TrackingState.Tracked ? TrackedColors : InferredColors);
}
DrawBones(model, bpqd);
model.Transform = new RotateTransform3D(new QuaternionRotation3D(new Quaternion(0, 1, 0, 0)));
}