/// <summary>
/// Gets the angle of the shoulder joint pitch from the upper arm vector and left->right shoulder vector.
/// </summary>
/// <param name="joints">List of joint positions.</param>
private void TestShoulderPitch(ref IReadOnlyDictionary <JointType, Joint> joints)
{
// 1. Get vectors from elbow -> shoulderRight
Point3 pointForShoulderRight = Point3.FromCameraSpacePoint(joints[JointType.ShoulderRight].Position);
Point3 pointForElbow = Point3.FromCameraSpacePoint(joints[JointType.ElbowRight].Position);
Vector3 shoulderRightToElbow = GetVectorFromPoints(pointForElbow, pointForShoulderRight);
// 2. Get vectors from shoulderLeft -> shoulderRight
Point3 pointForShoulderLeft = Point3.FromCameraSpacePoint(joints[JointType.ShoulderLeft].Position);
Vector3 shoulderLeftToShoulderRight = GetVectorFromPoints(pointForShoulderLeft, pointForShoulderRight);
// 3. Get angle between vectors
double elbowAngle = Vector3.GetAngleBetweenVectors(shoulderLeftToShoulderRight, shoulderRightToElbow);
// 4. Send this angle to motors
SerialWrite(elbowAngle);
}
/// <summary>
/// Gets the angle of the shoulder joint pitch and yaw.
/// </summary>
/// <param name="joints">List of joint positions.</param>
private void TestShoulderToElbow(ref IReadOnlyDictionary <JointType, Joint> joints)
{
// 1. Get elbow and shoulder points
Point3 pointForShoulderRight = Point3.FromCameraSpacePoint(joints[JointType.ShoulderRight].Position);
Point3 pointForShoulderLeft = Point3.FromCameraSpacePoint(joints[JointType.ShoulderLeft].Position);
Point3 pointForElbowRight = Point3.FromCameraSpacePoint(joints[JointType.ElbowRight].Position);
Point3 pointForElbowLeft = Point3.FromCameraSpacePoint(joints[JointType.ElbowLeft].Position);
Point3 pointForWristRight = Point3.FromCameraSpacePoint(joints[JointType.WristRight].Position);
Point3 pointForWristLeft = Point3.FromCameraSpacePoint(joints[JointType.WristLeft].Position);
Point3 pointForNeck = Point3.FromCameraSpacePoint(joints[JointType.Neck].Position);
Point3 pointForSpine = Point3.FromCameraSpacePoint(joints[JointType.SpineShoulder].Position);
// 2. Calculate the inverse kinematics (angles of each joint).
InverseKinematics inverseKinematicsRight = InverseKinematics.GetInverseKinematicsRight(
pointForNeck, pointForSpine, pointForShoulderLeft,
pointForShoulderRight, pointForElbowRight, pointForWristRight);
InverseKinematics inverseKinematicsLeft = InverseKinematics.GetInverseKinematicsLeft(
pointForNeck, pointForSpine, pointForShoulderLeft,
pointForShoulderRight, pointForElbowLeft, pointForWristLeft);
// 3. Display the angles on the screen
using (DrawingContext dc = this.projectedGroup.Open())
{
// Using matrices and forward kinematics.
InverseKinematics ikRadiansRight = inverseKinematicsRight.ToRadians();
InverseKinematics ikRadiansLeft = inverseKinematicsLeft.ToRadians();
ForwardKinematics fkRight = ForwardKinematics.GetForwardKinematicsRight(pointForShoulderRight, ikRadiansRight);
ForwardKinematics fkLeft = ForwardKinematics.GetForwardKinematicsLeft(pointForShoulderLeft, ikRadiansLeft);
// Draw the skeleton (neck/spine and shoulders)
// Draw the projected vectors
Pen drawingPen = bodyColors[2]; // new Pen(new SolidColorBrush(Color.FromArgb(255, 68, 192, 68)), 3);
Pen skeletonPen = this.inferredBonePen;
// Use the angles to create projected vectors of joints. Use Forward Kinematics.
IReadOnlyDictionary <JointType, Joint> rightArmPoints = new Dictionary <JointType, Joint>
{
[JointType.ShoulderRight] = new Joint()
{
JointType = JointType.ShoulderRight,
Position = pointForShoulderRight.ToCameraSpacePoint(),
TrackingState = TrackingState.Tracked,
},
[JointType.ElbowRight] = new Joint()
{
JointType = JointType.ElbowRight,
Position = fkRight.Elbow.ToCameraSpacePoint(),
TrackingState = TrackingState.Tracked,
},
[JointType.WristRight] = new Joint()
{
JointType = JointType.WristRight,
Position = fkRight.Wrist.ToCameraSpacePoint(),
TrackingState = TrackingState.Tracked,
}
};
IReadOnlyDictionary <JointType, Joint> leftArmPoints = new Dictionary <JointType, Joint>
{
[JointType.ShoulderLeft] = new Joint()
{
JointType = JointType.ShoulderLeft,
Position = pointForShoulderLeft.ToCameraSpacePoint(),
TrackingState = TrackingState.Tracked,
},
[JointType.ElbowLeft] = new Joint()
{
JointType = JointType.ElbowLeft,
Position = fkLeft.Elbow.ToCameraSpacePoint(),
TrackingState = TrackingState.Tracked,
},
[JointType.WristLeft] = new Joint()
{
JointType = JointType.WristLeft,
Position = fkLeft.Wrist.ToCameraSpacePoint(),
TrackingState = TrackingState.Tracked,
}
};
// Draw a transparent background to set the render size
dc.DrawRectangle(Brushes.Black, null, new Rect(0.0, 0.0, this.displayWidth, this.displayHeight));
// convert the joint points to depth (display) space
Dictionary <JointType, Point> jointPoints = new Dictionary <JointType, Point>();
foreach (JointType jointType in rightArmPoints.Keys)
{
// Get the 2D representation of the depth point.
DepthSpacePoint depthSpacePoint = this.GetDepthSpacePoint(rightArmPoints[jointType].Position);
jointPoints[jointType] = new Point(depthSpacePoint.X, depthSpacePoint.Y);
}
foreach (JointType jointType in leftArmPoints.Keys)
{
// Get the 2D representation of the depth point.
DepthSpacePoint depthSpacePoint = this.GetDepthSpacePoint(leftArmPoints[jointType].Position);
jointPoints[jointType] = new Point(depthSpacePoint.X, depthSpacePoint.Y);
}
this.DrawBody(new Dictionary <JointType, Joint>
{
[JointType.ShoulderLeft] = leftArmPoints[JointType.ShoulderLeft],
[JointType.ShoulderRight] = rightArmPoints[JointType.ShoulderRight]
}, jointPoints, dc, inferredBonePen);
this.DrawBody(rightArmPoints, jointPoints, dc, bodyColors[4]);
this.DrawBody(leftArmPoints, jointPoints, dc, bodyColors[1]);
// prevent drawing outside of our render area
this.projectedGroup.ClipGeometry = new RectangleGeometry(new Rect(0.0, 0.0, this.displayWidth, this.displayHeight));
AnglesR.Text = String.Format(
"Shoulder {0}" + Environment.NewLine +
"Elbow {1}" + Environment.NewLine +
"Wrist {2}" + Environment.NewLine +
"U Arm L {3}" + Environment.NewLine +
"L Arm L {4}" + Environment.NewLine +
"Shoulder {5}, {6}, {7}" + Environment.NewLine +
"Elbow {8}",
SpacePointToString(rightArmPoints[JointType.ShoulderRight].Position), SpacePointToString(rightArmPoints[JointType.ElbowRight].Position),
SpacePointToString(rightArmPoints[JointType.WristRight].Position), inverseKinematicsRight.UpperArmLength, inverseKinematicsRight.LowerArmLength,
(int)inverseKinematicsRight.ShoulderYaw, (int)inverseKinematicsRight.ShoulderPitch, (int)inverseKinematicsRight.ShoulderRoll,
(int)inverseKinematicsRight.ElbowPitch);
AnglesL.Text = String.Format(
"Shoulder {0}" + Environment.NewLine +
"Elbow {1}" + Environment.NewLine +
"Wrist {2}" + Environment.NewLine +
"U Arm L {3}" + Environment.NewLine +
"L Arm L {4}" + Environment.NewLine +
"Shoulder {5}, {6}, {7}" + Environment.NewLine +
"Elbow {8}",
SpacePointToString(leftArmPoints[JointType.ShoulderLeft].Position), SpacePointToString(leftArmPoints[JointType.ElbowLeft].Position),
SpacePointToString(leftArmPoints[JointType.WristLeft].Position), inverseKinematicsLeft.UpperArmLength, inverseKinematicsLeft.LowerArmLength,
(int)inverseKinematicsLeft.ShoulderYaw, (int)inverseKinematicsLeft.ShoulderPitch, (int)inverseKinematicsLeft.ShoulderRoll,
(int)inverseKinematicsLeft.ElbowPitch);
}
// 4. Send the angles to the motors
SerialWrite(FilterAngles(new double[]
{
inverseKinematicsRight.ShoulderYaw,
inverseKinematicsRight.ShoulderPitch,
inverseKinematicsRight.ShoulderRoll,
inverseKinematicsRight.ElbowPitch,
inverseKinematicsLeft.ShoulderYaw,
inverseKinematicsLeft.ShoulderPitch,
inverseKinematicsLeft.ShoulderRoll,
inverseKinematicsLeft.ElbowPitch
}));
}