Ejemplo n.º 1
0
        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);
                        }
                    }
                }
            }
        }
Ejemplo n.º 2
0
        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);
                                }
                            }
                        }
                    }
                }
            }
        }