/// <summary>
/// Outputs wanted angles to format easily used by recordMotion()
/// </summary>
/// <returns>Array of angles at this time.</returns>
public static float[] copyAnglesToArray(Player player)
{
float[] angles = new float[10];
//just does upper body for now
angles[0] = degreeToRadian(player.head.yaw);
angles[1] = degreeToRadian(player.head.pitch);
angles[2] = degreeToRadian(player.lShoulder.pitch);
angles[3] = degreeToRadian(player.lShoulder.roll);
angles[4] = degreeToRadian(player.lElbow.yaw);
angles[5] = degreeToRadian(player.lElbow.roll);
angles[6] = degreeToRadian(player.rShoulder.pitch);
angles[7] = degreeToRadian(player.rShoulder.roll);
angles[8] = degreeToRadian(player.rElbow.yaw);
angles[9] = degreeToRadian(player.rElbow.roll);
return angles;
}
public void copyTo(Player copy)
{
copy.time = this.time;
this.head.copyTo(copy.head);
this.neck.copyTo(copy.neck);
this.spine.copyTo(copy.spine);
this.lShoulder.copyTo(copy.lShoulder);
this.rShoulder.copyTo(copy.rShoulder);
this.rElbow.copyTo(copy.rElbow);
this.lElbow.copyTo(copy.lElbow);
this.rHand.copyTo(copy.rHand);
this.lHand.copyTo(copy.lHand);
this.rHip.copyTo(copy.rHip);
this.lHip.copyTo(copy.lHip);
this.cHip.copyTo(copy.cHip);
this.lKnee.copyTo(copy.lKnee);
this.rKnee.copyTo(copy.rKnee);
this.pelvis.copyTo(copy.pelvis);
this.rAnkle.copyTo(copy.rAnkle);
this.lAnkle.copyTo(copy.lAnkle);
}
public Player copyPlayer()
{
Player newPlayer = new Player();
this.copyTo(newPlayer);
return newPlayer;
}
/// <summary>
/// Adds pose to list of poses.
/// </summary>
/// <param name="pose"></param>
public void addKeyframe(Player pose)
{
Keyframes++;
Poses.Add(pose);
}
/// <summary>
/// Reads from XML format outlined above.
/// </summary>
/// <param name="root"></param>
/// <returns></returns>
public static KinectMotion readFromXML(XElement root)
{
List<XElement> dataList = root.Descendants("Data").ToList<XElement>();
if(dataList == null)
return null;
XElement dataElement = dataList[0];
KinectMotion motion = new KinectMotion(int.Parse(dataElement.Attribute("Interval").Value),
float.Parse(dataElement.Attribute("Length").Value), int.Parse(dataElement.Attribute("Keyframes").Value));
IEnumerable<XElement> frames = dataElement.Descendants("Frame");
Player newFrame;
foreach (XElement frame in frames)
{
newFrame = new Player();
foreach (XElement joint in frame.Descendants("JointData")){
JointAngles jangles = KinectMotion.parseJoint(joint);
XElement nameNode = joint.Descendants("Name").ToList<XElement>()[0];
switch (nameNode.Value){
case "Head":
jangles.name = "Head";
newFrame.head = jangles;
break;
case "Neck":
jangles.name = "Neck";
newFrame.neck = jangles;
break;
case "Spine":
jangles.name = "Spine";
newFrame.spine = jangles;
break;
case "Left Shoulder":
jangles.name = "Left Shoulder";
newFrame.lShoulder = jangles;
break;
case "Right Shoulder":
jangles.name = "Right Shoulder";
newFrame.rShoulder = jangles;
break;
case "Left Elbow":
jangles.name = "Left Elbow";
newFrame.lElbow = jangles;
break;
case "Right Elbow":
jangles.name = "Right Elbow";
newFrame.rElbow = jangles;
break;
case "Right Hand":
jangles.name = "Right Hand";
newFrame.rHand = jangles;
break;
case "Left Hand":
jangles.name = "Left Hand";
newFrame.lHand = jangles;
break;
case "Right Hip":
jangles.name = "Right Hip";
newFrame.rHip = jangles;
break;
case "Left Hip":
jangles.name = "Left Hip";
newFrame.lHip = jangles;
break;
case "Center Hip":
jangles.name = "Center Hip";
newFrame.cHip = jangles;
break;
case "Left Knee":
jangles.name = "Left Knee";
newFrame.lKnee = jangles;
break;
case "Right Knee":
jangles.name = "Right Knee";
newFrame.rKnee = jangles;
break;
case "Pelvis":
jangles.name = "Pelvis";
newFrame.pelvis = jangles;
break;
case "Right Ankle":
jangles.name = "Right Ankle";
newFrame.rAnkle = jangles;
break;
case "Left Ankle":
jangles.name = "Left Ankle";
newFrame.lAnkle = jangles;
break;
}
}
//have to copy to keep list references?
motion.addKeyframe(newFrame.copyPlayer());
}
return motion;
}
/*
public void WriteXml(XmlWriter writer)
{
writer.WriteStartElement("Data");
writer.WriteAttributeString("Interval", TimeInterval.ToString());
writer.WriteAttributeString("Time", Seconds.ToString());
writer.WriteAttributeString("Keyframes", Keyframes.ToString());
writer.WriteStartElement("Frame");
foreach (Player player in this.poses){
writer.WriteElementString("Time", player.time.ToString());
foreach (JointAngles joint in player.jointList)
{
writer.WriteStartElement("JointData");
writer.WriteElementString("Name", joint.name);
writer.WriteStartElement("Position");
writer.WriteElementString("X", joint.x.ToString());
writer.WriteElementString("Y", joint.y.ToString());
writer.WriteElementString("Z", joint.z.ToString());
writer.WriteEndElement();
writer.WriteStartElement("Angle");
writer.WriteElementString("Roll", joint.roll.ToString());
writer.WriteElementString("Yaw", joint.yaw.ToString());
writer.WriteElementString("Pitch", joint.pitch.ToString());
writer.WriteEndElement();
writer.WriteEndElement();
}
}
writer.WriteEndElement();
writer.WriteEndElement();
}
public void ReadXml(XmlReader reader)
{
reader.MoveToContent();
Seconds = float.Parse(reader.GetAttribute("Seconds"));
Keyframes = int.Parse(reader.GetAttribute("Keyframes"));
TimeInterval = int.Parse(reader.GetAttribute("Interval"));
}*/
/// <summary>
/// Calculates all of the joint angles for the current Player stored in lastPlayer
/// and also stores them in lastPlayer to be used later.
/// </summary>
public static void calculateAngles(Player player)
{
List<float> neck_vec = getDirectionVector(player.spine, player.neck);
List<float> head_vec = getDirectionVector(player.neck, player.head);
float headPitch = getAngleBetweenVecs(neck_vec, head_vec);
if (neck_vec[2] < head_vec[2]) headPitch = headPitch * -1.0f;
player.head.fConfidence = 1.0f;
player.head.roll = 0.0f;
player.head.pitch = headPitch;
player.head.yaw = 0.0f;
List<float> rShoulder_vec = getDirectionVector(player.lShoulder,
player.rShoulder);
List<float> lShoulder_vec = getDirectionVector(player.rShoulder,
player.lShoulder);
List<float> rUpperArm_vec = getDirectionVector(player.rShoulder,
player.rElbow);
List<float> lUpperArm_vec = getDirectionVector(player.lShoulder,
player.lElbow);
List<float> center_vec = getDirectionVector(player.neck,
player.spine);
float rShoulderRoll = getAngleBetweenVecs(rShoulder_vec, rUpperArm_vec) - 90.0f;
float lShoulderRoll = Math.Abs(getAngleBetweenVecs(lShoulder_vec, lUpperArm_vec) - 90.0f);
float rShoulderPitch = (-1.0f * getAngleBetweenVecs(center_vec, rUpperArm_vec)) + 90.0f;
float lShoulderPitch = (-1.0f * getAngleBetweenVecs(center_vec, lUpperArm_vec)) + 90.0f;
player.rShoulder.fConfidence = 1.0f;
player.rShoulder.roll = rShoulderRoll;
player.rShoulder.pitch = rShoulderPitch;
player.rShoulder.yaw = 0.0f;
player.lShoulder.fConfidence = 1.0f;
player.lShoulder.roll = lShoulderRoll;
player.lShoulder.pitch = lShoulderPitch;
player.lShoulder.yaw = 0.0f;
List<float> rLowerArm_vec = getDirectionVector(player.rElbow,
player.rHand);
List<float> lLowerArm_vec = getDirectionVector(player.lElbow,
player.lHand);
List<float> rShCenter_vec = getDirectionVector(player.rShoulder,
player.neck);
List<float> lShCenter_vec = getDirectionVector(player.lShoulder,
player.neck);
float rElbowRoll = getAngleBetweenVecs(rUpperArm_vec, rLowerArm_vec);
float lElbowRoll = getAngleBetweenVecs(lUpperArm_vec, lLowerArm_vec) * -1.0f;
List<float> dVecRightShoulder = getVectorProduct(rShCenter_vec, rUpperArm_vec);
List<float> dVecLeftShoulder = getVectorProduct(lShCenter_vec, lUpperArm_vec);
float rElbowYaw = getAngleBetweenVecs(rLowerArm_vec, dVecRightShoulder) - 90.0f;
float lElbowYaw = getAngleBetweenVecs(lLowerArm_vec, dVecLeftShoulder) - 90.0f;
player.rElbow.fConfidence = 1.0f;
player.rElbow.roll = rElbowRoll;
player.rElbow.pitch = 0.0f;
player.rElbow.yaw = rElbowYaw;
player.lElbow.fConfidence = 1.0f;
player.lElbow.roll = lElbowRoll;
player.lElbow.pitch = 0.0f;
player.lElbow.yaw = lElbowYaw;
List<float> cHip_vec = getDirectionVector(player.cHip,
player.spine);
List<float> rUpperLeg_vec = getDirectionVector(player.rHip,
player.rKnee);
List<float> lUpperLeg_vec = getDirectionVector(player.lHip,
player.lKnee);
float HipYawPitch = (getAngleBetweenVecs(cHip_vec, center_vec) - 150.0f) * -1.0f;
float rHipPitch = getAngleBetweenVecs(center_vec, rUpperLeg_vec) * -1.0f;
float lHipPitch = getAngleBetweenVecs(center_vec, lUpperLeg_vec) * -1.0f;
if (center_vec[2] < rUpperLeg_vec[2]) rHipPitch = rHipPitch * -1.0f;
if (center_vec[2] < lUpperLeg_vec[2]) lHipPitch = lHipPitch * -1.0f;
float rHipRoll = getRoll(rUpperLeg_vec);
float lHipRoll = getRoll(lUpperLeg_vec);
player.rHip.fConfidence = 1.0f;
player.rHip.roll = rHipRoll;
player.rHip.pitch = rHipPitch;
player.rHip.yaw = HipYawPitch;
player.lHip.fConfidence = 1.0f;
player.lHip.roll = lHipRoll;
player.lHip.pitch = lHipPitch;
player.lHip.yaw = HipYawPitch;
List<float> rLowerLeg_vec = getDirectionVector(player.rKnee,
player.rAnkle);
List<float> lLowerLeg_vec = getDirectionVector(player.lKnee,
player.lAnkle);
float rKneePitch = getAngleBetweenVecs(rUpperLeg_vec, rLowerLeg_vec);
float lKneePitch = getAngleBetweenVecs(lUpperLeg_vec, lLowerLeg_vec);
player.rKnee.fConfidence = 1.0f;
player.rKnee.roll = 0.0f;
player.rKnee.pitch = rKneePitch;
player.rKnee.yaw = 0.0f;
player.lKnee.fConfidence = 1.0f;
player.lKnee.roll = 0.0f;
player.lKnee.pitch = lKneePitch;
player.lKnee.yaw = 0.0f;
List<float> Sh_vec = getDirectionVector(player.rShoulder,
player.lShoulder);
List<float> y_axis = new List<float>();
y_axis.Add(0.0f);
y_axis.Add(1.0f);
y_axis.Add(0.0f);
List<float> z_axis = new List<float>();
z_axis.Add(0.0f);
z_axis.Add(0.0f);
z_axis.Add(1.0f);
player.pelvis.fConfidence = 1.0f;
player.pelvis.x = 0.0f;
player.pelvis.y = getAngleBetweenVecs(center_vec, y_axis); // PITCH
player.pelvis.z = getAngleBetweenVecs(Sh_vec, z_axis); // YAW
player.rHand.fConfidence = 1.0f;
player.rHand.roll = 0.0f;
player.rHand.pitch = 0.0f;
player.rHand.yaw = 0.0f;
player.lHand.fConfidence = 1.0f;
player.lHand.roll = 0.0f;
player.lHand.pitch = 0.0f;
player.lHand.yaw = 0.0f;
player.rAnkle.fConfidence = 1.0f;
player.rAnkle.roll = 0.0f;
player.rAnkle.pitch = degreeToRadian(-20.0f);
player.rAnkle.yaw = 0.0f;
player.lAnkle.fConfidence = 1.0f;
player.lAnkle.roll = 0.0f;
player.lAnkle.pitch = degreeToRadian(-20.0f);
player.lAnkle.yaw = 0.0f;
}
