private Vector3 GetVectorFromPoints(Point3 p, Point3 q)
{
return(Vector3.FromPoints(p, q, InferredZPositionClamp));
}
/// <summary>
/// Calculates the shoulder roll for the left arm.
/// </summary>
/// <param name="neckToSpine">The vector from the neck point to the spine point.</param>
/// <param name="shoulderR2L">The vector from right to left shoulder.</param>
/// <param name="shoulderToElbow">The vector from the left shoulder to the left elbow.</param>
/// <param name="elbowToWrist">The vector from the left elbow to the left wrist.</param>
/// <param name="inRadians">Whether to get the angle in radians instead of degrees. Defaults to false.</param>
/// <returns>The angle of the shoulder roll.</returns>
public static double GetShoulderRollLeft(Vector3 neckToSpine, Point3 shoulderL, Point3 shoulderR, Point3 elbow, Point3 wrist, bool inRadians = false)
{
Vector3 shoulderR2L = Vector3.FromPoints(shoulderR, shoulderL);
// 1. Get Body plane (neck-spine and shoulder-shoulder).
// 2. Get upper arm plane (shoulder-shoulder and shoulder-elbow).
// 3. Get Arm plane (shoulder-elbow and elbow-wrist).
// 4. Get angle of planes.
// 5. Adapt angle to be in the range of 90 deg (pointing up) to -90
// (pointing down).
Plane upperArmPlane = Plane.FromPoints(shoulderR, shoulderL, elbow);
Plane armPlane = Plane.FromPoints(shoulderL, elbow, wrist);
if (armPlane.AsVector().IsEmpty)
{
return(0);
}
double angle;
if (upperArmPlane.AsVector().IsEmpty)
{
// When the elbow is point perpendicular to the body to the
// side, the upper arm plane will be empty (as the shoulders
// and upper arm vectors will line up). Use the body plane's
// normal vector to create a new virtual shoulder to create an
// upper arm plane.
Plane bodyPlane = Plane.FromVectors(neckToSpine, shoulderR2L);
Vector3 bodyPlaneNormal = bodyPlane.AsVector();
Point3 shoulderOffset = new Point3(shoulderL.X + bodyPlaneNormal.X, shoulderL.Y + bodyPlaneNormal.Y, shoulderL.Z + bodyPlaneNormal.Z);
upperArmPlane = Plane.FromPoints(shoulderR, shoulderL, shoulderOffset);
}
angle = Plane.GetAngleBetweenPlanes(armPlane, upperArmPlane, inRadians);
double distFromPlane = new Point3(wrist.X - elbow.X, wrist.Y - elbow.Y, wrist.Z - elbow.Z).DistanceFromPlane(upperArmPlane);
if (distFromPlane > 0)
{
return(-angle);
}
return(angle);
}
/// <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
}));
}
public static InverseKinematics GetInverseKinematicsLeft(Point3 neck, Point3 spine, Point3 shoulderL, Point3 shoulderR, Point3 elbow, Point3 wrist, bool inRadians = false)
{
Vector3 neckToSpine = Vector3.FromPoints(neck, spine);
Vector3 shoulderR2L = Vector3.FromPoints(shoulderR, shoulderL);
Vector3 shoulderToElbow = Vector3.FromPoints(shoulderL, elbow);
Vector3 elbowToWrist = Vector3.FromPoints(elbow, wrist);
double shoulderYaw = GetShoulderYaw(neckToSpine, shoulderR2L, shoulderToElbow, inRadians: inRadians);
double shoulderPitch = -GetShoulderPitch(shoulderR2L, shoulderToElbow, inRadians: inRadians);
double shoulderRoll = GetShoulderRollLeft(neckToSpine, shoulderL, shoulderR, elbow, wrist, inRadians: inRadians);
double elbowPitch = -GetElbowAngle(shoulderToElbow, elbowToWrist, inRadians: inRadians);
return(new InverseKinematics(shoulderYaw, shoulderPitch, shoulderRoll, elbowPitch, shoulderToElbow.Magnitude, elbowToWrist.Magnitude, isInRadians: inRadians));
}
