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