private void DrawLocalCoordinates(IReadOnlyDictionary<JointType, JointOrientation> jointOrientations
                                            , IReadOnlyDictionary<JointType, Joint> joints
                                            , IDictionary<JointType, Point> jointPoints
                                            , DrawingContext drawingContext)
        {
            Pen xCoordPen = new Pen(Brushes.Red, 2);
            Pen yCoordPen = new Pen(Brushes.Green, 2);
            Pen zCoordPen = new Pen(Brushes.Blue, 2);
            //Draw the local coordinates
            foreach (JointType jointType in jointOrientations.Keys)
            {
                Vector4 vec = jointOrientations[jointType].Orientation;
                Quaternion qOrientation = new Quaternion(vec.W, vec.X, vec.Y, vec.Z);
                CameraSpacePoint csX = CreateEndPoint(joints[jointType].Position, qOrientation.Rotate(0.1f, 0.0f, 0.0f));
                CameraSpacePoint csY = CreateEndPoint(joints[jointType].Position, qOrientation.Rotate(0.0f, 0.1f, 0.0f));
                CameraSpacePoint csZ = CreateEndPoint(joints[jointType].Position, qOrientation.Rotate(0.0f, 0.0f, 0.1f));

                DepthSpacePoint dsX = this.coordinateMapper.MapCameraPointToDepthSpace(csX);
                DepthSpacePoint dsY = this.coordinateMapper.MapCameraPointToDepthSpace(csY);
                DepthSpacePoint dsZ = this.coordinateMapper.MapCameraPointToDepthSpace(csZ);

                drawingContext.DrawLine(xCoordPen, jointPoints[jointType], new Point(dsX.X, dsX.Y));
                drawingContext.DrawLine(yCoordPen, jointPoints[jointType], new Point(dsY.X, dsY.Y));
                drawingContext.DrawLine(zCoordPen, jointPoints[jointType], new Point(dsZ.X, dsZ.Y));

                if (Instance.DrawOrientationAnglesX == true
                    || Instance.DrawOrientationAnglesY == true
                    || Instance.DrawOrientationAnglesZ == true)
                {

                    JointType parentJoint = KinectHelpers.GetParentJoint(jointType);
                    double AngleBetweenParentChildY = 0;
                    double AngleBetweenParentChildX = 0;
                    double AngleBetweenParentChildZ = 0;

                    //For each vector in the DepthSpacePoint, compute the angle between
                    //  parent and child (only if the joint has a parent)
                    if (parentJoint != jointType)
                    {

                        Vector4 vecParent = jointOrientations[parentJoint].Orientation;
                        Quaternion qOrientationParent = new Quaternion(vecParent.W, vecParent.X, vecParent.Y, vecParent.Z);
                        //(only compute if requested)
                        if (DrawOrientationAnglesX == true)
                        {
                            CameraSpacePoint csXParent = CreateEndPoint(joints[parentJoint].Position, qOrientationParent.Rotate(0.1f, 0.0f, 0.0f));
                            DepthSpacePoint dsXParent = this.coordinateMapper.MapCameraPointToDepthSpace(csXParent);
                            AngleBetweenParentChildX = MathHelpers.AngleBetweenPoints(new Point(dsX.X, dsX.Y), new Point(dsXParent.X, dsXParent.Y));
                        }
                        if (DrawOrientationAnglesY == true)
                        {
                            CameraSpacePoint csYParent = CreateEndPoint(joints[parentJoint].Position, qOrientationParent.Rotate(0.0f, 0.1f, 0.0f));
                            DepthSpacePoint dsYParent = this.coordinateMapper.MapCameraPointToDepthSpace(csYParent);
                            AngleBetweenParentChildY = MathHelpers.AngleBetweenPoints(new Point(dsY.X, dsY.Y), new Point(dsYParent.X, dsYParent.Y));
                        }
                        if (DrawOrientationAnglesZ == true)
                        {
                            CameraSpacePoint csZParent = CreateEndPoint(joints[parentJoint].Position, qOrientationParent.Rotate(0.0f, 0.0f, 0.1f));
                            DepthSpacePoint dsZParent = this.coordinateMapper.MapCameraPointToDepthSpace(csZParent);
                            AngleBetweenParentChildZ = MathHelpers.AngleBetweenPoints(new Point(dsZ.X, dsY.Y), new Point(dsZParent.X, dsZParent.Y));
                        }
                    }

                    if (DrawOrientationAnglesY == true)
                    {
                        //Fun With Formatted Text
                        ///http://msdn.microsoft.com/en-us/library/system.windows.media.formattedtext(v=vs.110).aspx
                        FormattedText fteY = new FormattedText(String.Format("{0,0:F0}°", AngleBetweenParentChildY),
                                                                    CultureInfo.GetCultureInfo("en-us"),
                                                                    FlowDirection.LeftToRight,
                                                                    new Typeface("Verdana"),
                                                                    8, yCoordPen.Brush);

                        drawingContext.DrawText(fteY, GetTextTopLeft(fteY, new Point(dsY.X, dsY.Y), jointPoints[jointType]));

                    }
                    if (DrawOrientationAnglesZ == true)
                    {
                        FormattedText fteZ = new FormattedText(String.Format("{0,0:F0}°", AngleBetweenParentChildZ),
                                                                    CultureInfo.GetCultureInfo("en-us"),
                                                                    FlowDirection.LeftToRight,
                                                                    new Typeface("Verdana"),
                                                                    8, zCoordPen.Brush);

                        drawingContext.DrawText(fteZ, GetTextTopLeft(fteZ, new Point(dsZ.X, dsZ.Y), jointPoints[jointType]));
                    }
                    if (DrawOrientationAnglesX == true)
                    {
                        FormattedText fteX = new FormattedText(String.Format("{0,0:F0}°", AngleBetweenParentChildX),
                                                                    CultureInfo.GetCultureInfo("en-us"),
                                                                    FlowDirection.LeftToRight,
                                                                    new Typeface("Verdana"),
                                                                    8, xCoordPen.Brush);

                        drawingContext.DrawText(fteX, GetTextTopLeft(fteX, new Point(dsX.X, dsX.Y), jointPoints[jointType]));
                    }
                }
            }
        }
 public float[] Rotate(float x1, float y1, float z1)
 {
     Quaternion q = new Quaternion(0.0f, x1, y1, z1);
     Quaternion r = this * q * this.Conj;
     return new float[3] { r.X, r.Y, r.Z };
 }