private void NativeWindow_Render(object sender, NativeWindowEventArgs e)
{
using (var lastFrame = visualizerData.TakeFrameWithOwnership())
{
if (lastFrame == null)
{
return;
}
NativeWindow nativeWindow = (NativeWindow)sender;
Gl.Viewport(0, 0, (int)nativeWindow.Width, (int)nativeWindow.Height);
Gl.Clear(ClearBufferMask.ColorBufferBit);
// Update model/view/projective matrices in shader
var proj = Matrix4x4.CreatePerspectiveFieldOfView(ToRadians(65.0f), (float)nativeWindow.Width / nativeWindow.Height, 0.1f, 150.0f);
var view = Matrix4x4.CreateLookAt(Vector3.Zero, Vector3.UnitZ, -Vector3.UnitY);
SphereRenderer.View = view;
SphereRenderer.Projection = proj;
CylinderRenderer.View = view;
CylinderRenderer.Projection = proj;
PointCloudRenderer.View = view;
PointCloudRenderer.Projection = proj;
PointCloud.ComputePointCloud(lastFrame.Capture.Depth, ref pointCloud);
PointCloudRenderer.Render(pointCloud, new Vector4(1, 1, 1, 1));
for (uint i = 0; i < lastFrame.NumberOfBodies; ++i)
{
var skeleton = lastFrame.GetBodySkeleton(i);
var bodyId = lastFrame.GetBodyId(i);
var bodyColor = BodyColors.GetColorAsVector(bodyId);
for (int jointId = 0; jointId < (int)JointId.Count; ++jointId)
{
var joint = skeleton.GetJoint(jointId);
// Render the joint as a sphere.
const float radius = 0.024f;
SphereRenderer.Render(joint.Position / 1000, radius, bodyColor);
if (JointConnections.JointParent.TryGetValue((JointId)jointId, out JointId parentId))
{
// Render a bone connecting this joint and its parent as a cylinder.
CylinderRenderer.Render(joint.Position / 1000, skeleton.GetJoint((int)parentId).Position / 1000, bodyColor);
}
}
}
}
}
private void NativeWindow_Render(object sender, NativeWindowEventArgs e)
{
float head_height;
float feet_y_min;
float delta;
using (var lastFrame = visualizerData.TakeFrameWithOwnership())
{
if (lastFrame == null)
{
return;
}
NativeWindow nativeWindow = (NativeWindow)sender;
Gl.Viewport(0, 0, (int)nativeWindow.Width, (int)nativeWindow.Height);
Gl.Clear(ClearBufferMask.ColorBufferBit);
// Update model/view/projective matrices in shader
var proj = Matrix4x4.CreatePerspectiveFieldOfView(ToRadians(65.0f), (float)nativeWindow.Width / nativeWindow.Height, 0.1f, 150.0f);
var view = Matrix4x4.CreateLookAt(Vector3.Zero, Vector3.UnitZ, -Vector3.UnitY);
SphereRenderer.View = view;
SphereRenderer.Projection = proj;
CylinderRenderer.View = view;
CylinderRenderer.Projection = proj;
//PointCloudRenderer.View = view;
//PointCloudRenderer.Projection = proj;
//PointCloud.ComputePointCloud(lastFrame.Capture.Depth, ref pointCloud);
//PointCloudRenderer.Render(pointCloud, new Vector4(1, 1, 1, 1));
if (lastFrame.NumberOfBodies > 0 && !TrackingAnybody)
{
Console.WriteLine("+++++++++++++++++ TRACKING SOMEBODY ++++++++++++++++");
TrackingAnybody = true;
}
if (lastFrame.NumberOfBodies == 0 && TrackingAnybody)
{
Console.WriteLine("----------------- NOBODY TRACKED -----------------");
TrackingAnybody = false;
}
for (uint i = 0; i < lastFrame.NumberOfBodies; ++i)
{
var skeleton = lastFrame.GetBodySkeleton(i);
var bodyId = lastFrame.GetBodyId(i);
var bodyColor = BodyColors.GetColorAsVector(bodyId);
switch (Status)
{
case STATUS_INIT:
case STATUS_RUNNING:
case STATUS_CALIBRATION_PROCESSING:
break;
case STATUS_CALIBRATION_ACQUIRING:
LeftHand_Kinect_PA.AddPoint(
skeleton.GetJoint(JointId.HandLeft).Position.X,
skeleton.GetJoint(JointId.HandLeft).Position.Y,
skeleton.GetJoint(JointId.HandLeft).Position.Z);
RightHand_Kinect_PA.AddPoint(
skeleton.GetJoint(JointId.HandRight).Position.X,
skeleton.GetJoint(JointId.HandRight).Position.Y,
skeleton.GetJoint(JointId.HandRight).Position.Z);
//LeftHand_Hololens_PA.AddPoint(LH_Hololens_pos.X, LH_Hololens_pos.Y, LH_Hololens_pos.Z);
//RightHand_Hololens_PA.AddPoint(RH_Hololens_pos.X, RH_Hololens_pos.Y, RH_Hololens_pos.Z);
break;
}
if (true)
{
if (true)
{
data_to_send[0] = skeleton.GetJoint(JointId.FootLeft).Position.X;
data_to_send[1] = GetKinectHeight(skeleton.GetJoint(JointId.FootLeft).Position.Y);
data_to_send[2] = skeleton.GetJoint(JointId.FootLeft).Position.Z;
data_to_send[3] = skeleton.GetJoint(JointId.FootRight).Position.X;
data_to_send[4] = GetKinectHeight(skeleton.GetJoint(JointId.FootRight).Position.Y);
data_to_send[5] = skeleton.GetJoint(JointId.FootRight).Position.Z;
}
else
{
data_to_send[0] = skeleton.GetJoint(JointId.HandLeft).Position.X;
data_to_send[1] = skeleton.GetJoint(JointId.HandLeft).Position.Y;
data_to_send[2] = skeleton.GetJoint(JointId.HandLeft).Position.Z;
data_to_send[3] = skeleton.GetJoint(JointId.HandRight).Position.X;
data_to_send[4] = skeleton.GetJoint(JointId.HandRight).Position.Y;
data_to_send[5] = skeleton.GetJoint(JointId.HandRight).Position.Z;
}
}
else
{
//DEBUG
data_to_send[0] = 1.0f;
data_to_send[1] = 2.0f;
data_to_send[2] = 3.0f;
data_to_send[3] = 4.0f;
data_to_send[4] = 5.0f;
data_to_send[5] = 6.0f;
}
head_height = GetKinectHeight(skeleton.GetJoint(JointId.Head).Position.Y);
feet_y_min = min(data_to_send[1], data_to_send[4]);
delta = head_height - feet_y_min;
data_to_send[1] = (data_to_send[1] - feet_y_min) / delta;
data_to_send[4] = (data_to_send[4] - feet_y_min) / delta;
//data_to_send[1] /= head_height;
if (data_to_send[1] > 1.0f)
{
data_to_send[1] = 1.0f;
}
else
{
if (data_to_send[1] < 0)
{
data_to_send[1] = 0;
}
}
//data_to_send[4] /= head_height;
if (data_to_send[4] > 1.0f)
{
data_to_send[4] = 1.0f;
}
else
{
if (data_to_send[4] < 0)
{
data_to_send[4] = 0;
}
}
//unbias data
if (false)
{
if (true)
{
//unbias data subtracting the measures from the head's ones
feet_y_min = min(data_to_send[1], data_to_send[4]);
delta = head_height - feet_y_min;
data_to_send[0] = skeleton.GetJoint(JointId.Head).Position.X - data_to_send[0];
data_to_send[1] = data_to_send[1] - feet_y_min;
data_to_send[2] = skeleton.GetJoint(JointId.Head).Position.Z - data_to_send[2];
data_to_send[3] = skeleton.GetJoint(JointId.Head).Position.X - data_to_send[3];
data_to_send[4] = head_height - data_to_send[4];
data_to_send[5] = skeleton.GetJoint(JointId.Head).Position.Z - data_to_send[5];
if (data_to_send[1] < 0)
{
data_to_send[1] = -data_to_send[1];
}
if (data_to_send[4] < 0)
{
data_to_send[4] = -data_to_send[4];
}
if (normalize_height && head_height > 0)
{
data_to_send[1] /= head_height;
if (data_to_send[1] > 1.0f)
{
data_to_send[1] = 1.0f;
}
else
if (data_to_send[1] < 0)
{
data_to_send[1] = 0;
}
data_to_send[4] /= head_height;
if (data_to_send[4] > 1.0f)
{
data_to_send[4] = 1.0f;
}
else
if (data_to_send[4] < 0)
{
data_to_send[4] = 0;
}
}
}
else
{
//unbias data subtracting the the head's measures from the actual ones
data_to_send[0] -= skeleton.GetJoint(JointId.Head).Position.X;
data_to_send[1] -= skeleton.GetJoint(JointId.Head).Position.Y;
data_to_send[2] -= skeleton.GetJoint(JointId.Head).Position.Z;
data_to_send[3] -= skeleton.GetJoint(JointId.Head).Position.X;
data_to_send[4] -= skeleton.GetJoint(JointId.Head).Position.Y;
data_to_send[5] -= skeleton.GetJoint(JointId.Head).Position.Z;
}
}
if (debug_print_sending_data_UDP)
{
Console.WriteLine("SENDING-> LH: " + data_to_send[0].ToString("0.00") + " " + data_to_send[1].ToString("0.00") + " " + data_to_send[2].ToString("0.00") +
" --- RH: " + data_to_send[3].ToString("0.00") + " " + data_to_send[4].ToString("0.00") + " " + data_to_send[5].ToString("0.00") + " Head height = " + head_height.ToString("0.00"));
}
if (true)
{
SendPositions(data_to_send);
}
for (int jointId = 0; jointId < (int)JointId.Count; ++jointId)
{
if (jointId == (int)JointId.SpineNavel)
{
float f = skeleton.GetJoint(jointId).Position.X;
//SendFloat(f);
}
if (jointId == (int)JointId.HandRight || jointId == (int)JointId.HandLeft ||
jointId == (int)JointId.FootLeft || jointId == (int)JointId.FootRight ||
jointId == (int)JointId.SpineNavel)
{
var joint = skeleton.GetJoint(jointId);
// Render the joint as a sphere.
const float radius = 0.012f;
SphereRenderer.Render(joint.Position / 1000, radius, bodyColor);
if (false)
{
if (JointConnections.JointParent.TryGetValue((JointId)jointId, out JointId parentId))
{
// Render a bone connecting this joint and its parent as a cylinder.
CylinderRenderer.Render(joint.Position / 1000, skeleton.GetJoint((int)parentId).Position / 1000, bodyColor);
}
}
}
}
}
}
}