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 }; }