// compute angle in measure unit
private void ComputeJointAngle(Skeleton ske, MeasurementUnit unit, ref JointStatus status)
{
if (ske == null)
{
return;
}
if (unit.ifSingleJoint)
{
// compute bone-bone angle centered in single joint
if (jointNeighbors.ContainsKey(unit.singleJoint) &&
jointNeighbors[unit.singleJoint].Count == 2)
{
SkeletonPoint cur_joint_pos = ske.Joints[unit.singleJoint].Position;
JointType neighbor_jointtype1 = jointNeighbors[unit.singleJoint][0];
JointType neighbor_jointtype2 = jointNeighbors[unit.singleJoint][1];
SkeletonPoint neighbor_joint_pos1 = ske.Joints[neighbor_jointtype1].Position;
SkeletonPoint neighbor_joint_pos2 = ske.Joints[neighbor_jointtype2].Position;
// compute bone-bone angle
Point3D vec1 = new Point3D(neighbor_joint_pos1.X - cur_joint_pos.X,
neighbor_joint_pos1.Y - cur_joint_pos.Y,
neighbor_joint_pos1.Z - cur_joint_pos.Z);
Point3D vec2 = new Point3D(neighbor_joint_pos2.X - cur_joint_pos.X,
neighbor_joint_pos2.Y - cur_joint_pos.Y,
neighbor_joint_pos2.Z - cur_joint_pos.Z);
status.angle = Tools.ComputeAngle(vec1, vec2);
}
}
else
{
// compute bone angle
SkeletonPoint cur_joint_pos = ske.Joints[unit.boneJoint1].Position;
SkeletonPoint neighbor_joint_pos1 = ske.Joints[unit.boneJoint2].Position;
if (!status.planeAngles.ContainsKey(unit.boneJoint2))
{
Dictionary <PlaneName, double> plane_temp = new Dictionary <PlaneName, double>();
plane_temp.Add(PlaneName.XYPlane, 0);
plane_temp.Add(PlaneName.YZPlane, 0);
plane_temp.Add(PlaneName.XZPlane, 0);
status.planeAngles.Add(unit.boneJoint2, plane_temp);
}
Point3D vec1 = new Point3D(neighbor_joint_pos1.X - cur_joint_pos.X,
neighbor_joint_pos1.Y - cur_joint_pos.Y,
neighbor_joint_pos1.Z - cur_joint_pos.Z);
Point3D xyplane1 = new Point3D(vec1.X, vec1.Y, 0); // projection of vec1 to xy plane
status.planeAngles[unit.boneJoint2][PlaneName.XYPlane] = Tools.ComputeAngle(vec1, xyplane1);
Point3D yzplane1 = new Point3D(0, vec1.Y, vec1.Z);
status.planeAngles[unit.boneJoint2][PlaneName.YZPlane] = Tools.ComputeAngle(vec1, yzplane1);
Point3D xzplane1 = new Point3D(vec1.X, 0, vec1.Z);
status.planeAngles[unit.boneJoint2][PlaneName.XZPlane] = Tools.ComputeAngle(vec1, xzplane1);
}
}
// compute angle in measure unit
private void ComputeJointAngle(Skeleton ske, MeasurementUnit unit, ref JointStatus status)
{
if (ske == null)
return;
if (unit.ifSingleJoint)
{
// compute bone-bone angle centered in single joint
if (jointNeighbors.ContainsKey(unit.singleJoint) &&
jointNeighbors[unit.singleJoint].Count == 2)
{
SkeletonPoint cur_joint_pos = ske.Joints[unit.singleJoint].Position;
JointType neighbor_jointtype1 = jointNeighbors[unit.singleJoint][0];
JointType neighbor_jointtype2 = jointNeighbors[unit.singleJoint][1];
SkeletonPoint neighbor_joint_pos1 = ske.Joints[neighbor_jointtype1].Position;
SkeletonPoint neighbor_joint_pos2 = ske.Joints[neighbor_jointtype2].Position;
// compute bone-bone angle
Point3D vec1 = new Point3D(neighbor_joint_pos1.X - cur_joint_pos.X,
neighbor_joint_pos1.Y - cur_joint_pos.Y,
neighbor_joint_pos1.Z - cur_joint_pos.Z);
Point3D vec2 = new Point3D(neighbor_joint_pos2.X - cur_joint_pos.X,
neighbor_joint_pos2.Y - cur_joint_pos.Y,
neighbor_joint_pos2.Z - cur_joint_pos.Z);
status.angle = Tools.ComputeAngle(vec1, vec2);
}
}
else
{
// compute bone angle
SkeletonPoint cur_joint_pos = ske.Joints[unit.boneJoint1].Position;
SkeletonPoint neighbor_joint_pos1 = ske.Joints[unit.boneJoint2].Position;
if (!status.planeAngles.ContainsKey(unit.boneJoint2))
{
Dictionary<PlaneName, double> plane_temp = new Dictionary<PlaneName, double>();
plane_temp.Add(PlaneName.XYPlane, 0);
plane_temp.Add(PlaneName.YZPlane, 0);
plane_temp.Add(PlaneName.XZPlane, 0);
status.planeAngles.Add(unit.boneJoint2, plane_temp);
}
Point3D vec1 = new Point3D(neighbor_joint_pos1.X - cur_joint_pos.X,
neighbor_joint_pos1.Y - cur_joint_pos.Y,
neighbor_joint_pos1.Z - cur_joint_pos.Z);
Point3D xyplane1 = new Point3D(vec1.X, vec1.Y, 0); // projection of vec1 to xy plane
status.planeAngles[unit.boneJoint2][PlaneName.XYPlane] = Tools.ComputeAngle(vec1, xyplane1);
Point3D yzplane1 = new Point3D(0, vec1.Y, vec1.Z);
status.planeAngles[unit.boneJoint2][PlaneName.YZPlane] = Tools.ComputeAngle(vec1, yzplane1);
Point3D xzplane1 = new Point3D(vec1.X, 0, vec1.Z);
status.planeAngles[unit.boneJoint2][PlaneName.XZPlane] = Tools.ComputeAngle(vec1, xzplane1);
}
}
// compute all angles associated with input joint (must have two neighbors)
private void ComputeJointAllAngles(Skeleton ske, JointType type, ref JointStatus status)
{
if (ske == null)
return;
if (jointNeighbors.ContainsKey(type) && jointNeighbors[type].Count == 2)
{
SkeletonPoint cur_joint_pos = ske.Joints[type].Position;
JointType neighbor_jointtype1 = jointNeighbors[type][0];
JointType neighbor_jointtype2 = jointNeighbors[type][1];
SkeletonPoint neighbor_joint_pos1 = ske.Joints[neighbor_jointtype1].Position;
SkeletonPoint neighbor_joint_pos2 = ske.Joints[neighbor_jointtype2].Position;
// compute bone-bone angle
Point3D vec1 = new Point3D(neighbor_joint_pos1.X - cur_joint_pos.X,
neighbor_joint_pos1.Y - cur_joint_pos.Y,
neighbor_joint_pos1.Z - cur_joint_pos.Z);
Point3D vec2 = new Point3D(neighbor_joint_pos2.X - cur_joint_pos.X,
neighbor_joint_pos2.Y - cur_joint_pos.Y,
neighbor_joint_pos2.Z - cur_joint_pos.Z);
status.angle = Tools.ComputeAngle(vec1, vec2);
// add items
// axis
Dictionary<AxisName, double> axis_temp = new Dictionary<AxisName, double>();
axis_temp.Add(AxisName.XAxis, 0);
axis_temp.Add(AxisName.YAsix, 0);
axis_temp.Add(AxisName.ZAsix, 0);
if (!status.axisAngles.ContainsKey(neighbor_jointtype1))
status.axisAngles.Add(neighbor_jointtype1, axis_temp);
if (!status.axisAngles.ContainsKey(neighbor_jointtype2))
status.axisAngles.Add(neighbor_jointtype2, axis_temp);
// plane
Dictionary<PlaneName, double> plane_temp = new Dictionary<PlaneName, double>();
plane_temp.Add(PlaneName.XYPlane, 0);
plane_temp.Add(PlaneName.YZPlane, 0);
plane_temp.Add(PlaneName.XZPlane, 0);
if (!status.planeAngles.ContainsKey(neighbor_jointtype1))
status.planeAngles.Add(neighbor_jointtype1, plane_temp);
if (!status.planeAngles.ContainsKey(neighbor_jointtype2))
status.planeAngles.Add(neighbor_jointtype2, plane_temp);
// compute axis angle
Point3D xaxis = new Point3D(1, 0, 0);
status.axisAngles[neighbor_jointtype1][AxisName.XAxis] = Tools.ComputeAngle(vec1, xaxis);
status.axisAngles[neighbor_jointtype2][AxisName.XAxis] = Tools.ComputeAngle(vec2, xaxis);
Point3D yaxis = new Point3D(0, 1, 0);
status.axisAngles[neighbor_jointtype1][AxisName.YAsix] = Tools.ComputeAngle(vec1, yaxis);
status.axisAngles[neighbor_jointtype2][AxisName.YAsix] = Tools.ComputeAngle(vec2, yaxis);
Point3D zaxis = new Point3D(0, 0, 1);
status.axisAngles[neighbor_jointtype1][AxisName.ZAsix] = Tools.ComputeAngle(vec1, zaxis);
status.axisAngles[neighbor_jointtype2][AxisName.ZAsix] = Tools.ComputeAngle(vec2, zaxis);
// compute plane angle
Point3D xyplane1 = new Point3D(vec1.X, vec1.Y, 0); // projection of vec1 to xy plane
status.planeAngles[neighbor_jointtype1][PlaneName.XYPlane] = Tools.ComputeAngle(vec1, xyplane1);
Point3D xyplane2 = new Point3D(vec2.X, vec2.Y, 0);
status.planeAngles[neighbor_jointtype2][PlaneName.XYPlane] = Tools.ComputeAngle(vec2, xyplane2);
Point3D yzplane1 = new Point3D(0, vec1.Y, vec1.Z);
status.planeAngles[neighbor_jointtype1][PlaneName.YZPlane] = Tools.ComputeAngle(vec1, yzplane1);
Point3D yzplane2 = new Point3D(0, vec2.Y, vec2.Z);
status.planeAngles[neighbor_jointtype2][PlaneName.YZPlane] = Tools.ComputeAngle(vec2, yzplane2);
Point3D xzplane1 = new Point3D(vec1.X, 0, vec1.Z);
status.planeAngles[neighbor_jointtype1][PlaneName.XZPlane] = Tools.ComputeAngle(vec1, xzplane1);
Point3D xzplane2 = new Point3D(vec2.X, 0, vec2.Z);
status.planeAngles[neighbor_jointtype2][PlaneName.XZPlane] = Tools.ComputeAngle(vec2, xzplane2);
}
else
status.angle = -1;
}
/// <summary>
/// given current skeleton, update status for each joint
/// </summary>
/// <param name="ske"></param>
public void UpdateJointStatus(Skeleton ske, List<MeasurementUnit> toMeasureUnits)
{
if (ske == null)
{
// gradually remove buffer to track latest skeleton only
if (jointStatusSeq.Count > 0)
jointStatusSeq.RemoveAt(0);
return;
}
if (ske.TrackingState != SkeletonTrackingState.Tracked)
{
Debug.WriteLine("Input skeleton not tracked.");
return;
}
// extract information for specified joint
Dictionary<JointType, JointStatus> cur_joint_status = new Dictionary<JointType, JointStatus>();
foreach (MeasurementUnit unit in toMeasureUnits)
{
JointStatus stat = null;
if (unit.ifSingleJoint)
{
if (cur_joint_status.ContainsKey(unit.singleJoint))
stat = cur_joint_status[unit.singleJoint];
else
stat = new JointStatus();
// position
stat.position.X = ske.Joints[unit.singleJoint].Position.X;
stat.position.Y = ske.Joints[unit.singleJoint].Position.Y;
stat.position.Z = ske.Joints[unit.singleJoint].Position.Z;
// angle
ComputeJointAngle(ske, unit, ref stat);
// speed
if (jointStatusSeq.Count > 0 &&
jointStatusSeq[jointStatusSeq.Count - 1].ContainsKey(unit.singleJoint))
{
stat.speed.X = stat.position.X -
jointStatusSeq[jointStatusSeq.Count - 1][unit.singleJoint].position.X;
stat.speed.Y = stat.position.Y -
jointStatusSeq[jointStatusSeq.Count - 1][unit.singleJoint].position.Y;
stat.speed.Z = stat.position.Z -
jointStatusSeq[jointStatusSeq.Count - 1][unit.singleJoint].position.Z;
stat.abs_speed = 30 * Math.Sqrt(Math.Pow(stat.speed.X, 2) +
Math.Pow(stat.speed.Y, 2) + Math.Pow(stat.speed.Z, 2));
}
// add to dict
cur_joint_status.Add(unit.singleJoint, stat);
}
else
{
// centered at bonejoint1
if (cur_joint_status.ContainsKey(unit.boneJoint1))
stat = cur_joint_status[unit.boneJoint1];
else
stat = new JointStatus();
// position
stat.position.X = ske.Joints[unit.boneJoint1].Position.X;
stat.position.Y = ske.Joints[unit.boneJoint1].Position.Y;
stat.position.Z = ske.Joints[unit.boneJoint1].Position.Z;
// angle
ComputeJointAngle(ske, unit, ref stat);
// add to dict
cur_joint_status.Add(unit.boneJoint1, stat);
}
}
//foreach (Joint joint in ske.Joints)
//{
// JointStatus stat = new JointStatus();
// // position
// stat.position.X = joint.Position.X;
// stat.position.Y = joint.Position.Y;
// stat.position.Z = joint.Position.Z;
// // angle
// ComputeJointAllAngles(ske, joint.JointType, ref stat);
// // compute speed using last frame data
// if(jointStatusSeq.Count > 0)
// {
// stat.speed.X = stat.position.X -
// jointStatusSeq[jointStatusSeq.Count - 1][joint.JointType].position.X;
// stat.speed.Y = stat.position.Y -
// jointStatusSeq[jointStatusSeq.Count - 1][joint.JointType].position.Y;
// stat.speed.Z = stat.position.Z -
// jointStatusSeq[jointStatusSeq.Count - 1][joint.JointType].position.Z;
// stat.abs_speed = 30 * Math.Sqrt(Math.Pow(stat.speed.X, 2) +
// Math.Pow(stat.speed.Y, 2) + Math.Pow(stat.speed.Z, 2));
// }
// // add to dict
// cur_joint_status.Add(joint.JointType, stat);
//}
// add to sequence
if (jointStatusSeq.Count == MAX_TRACK_LEN)
jointStatusSeq.RemoveAt(0);
jointStatusSeq.Add(cur_joint_status);
}
/// <summary>
/// given current skeleton, update status for each joint
/// </summary>
/// <param name="ske"></param>
public void UpdateJointStatus(Skeleton ske, List <MeasurementUnit> toMeasureUnits)
{
if (ske == null)
{
// gradually remove buffer to track latest skeleton only
if (jointStatusSeq.Count > 0)
{
jointStatusSeq.RemoveAt(0);
}
return;
}
if (ske.TrackingState != SkeletonTrackingState.Tracked)
{
Debug.WriteLine("Input skeleton not tracked.");
return;
}
// extract information for specified joint
Dictionary <JointType, JointStatus> cur_joint_status = new Dictionary <JointType, JointStatus>();
foreach (MeasurementUnit unit in toMeasureUnits)
{
JointStatus stat = null;
if (unit.ifSingleJoint)
{
if (cur_joint_status.ContainsKey(unit.singleJoint))
{
stat = cur_joint_status[unit.singleJoint];
}
else
{
stat = new JointStatus();
}
// position
stat.position.X = ske.Joints[unit.singleJoint].Position.X;
stat.position.Y = ske.Joints[unit.singleJoint].Position.Y;
stat.position.Z = ske.Joints[unit.singleJoint].Position.Z;
// angle
ComputeJointAngle(ske, unit, ref stat);
// speed
if (jointStatusSeq.Count > 0 &&
jointStatusSeq[jointStatusSeq.Count - 1].ContainsKey(unit.singleJoint))
{
stat.speed.X = stat.position.X -
jointStatusSeq[jointStatusSeq.Count - 1][unit.singleJoint].position.X;
stat.speed.Y = stat.position.Y -
jointStatusSeq[jointStatusSeq.Count - 1][unit.singleJoint].position.Y;
stat.speed.Z = stat.position.Z -
jointStatusSeq[jointStatusSeq.Count - 1][unit.singleJoint].position.Z;
stat.abs_speed = 30 * Math.Sqrt(Math.Pow(stat.speed.X, 2) +
Math.Pow(stat.speed.Y, 2) + Math.Pow(stat.speed.Z, 2));
}
// add to dict
cur_joint_status.Add(unit.singleJoint, stat);
}
else
{
// centered at bonejoint1
if (cur_joint_status.ContainsKey(unit.boneJoint1))
{
stat = cur_joint_status[unit.boneJoint1];
}
else
{
stat = new JointStatus();
}
// position
stat.position.X = ske.Joints[unit.boneJoint1].Position.X;
stat.position.Y = ske.Joints[unit.boneJoint1].Position.Y;
stat.position.Z = ske.Joints[unit.boneJoint1].Position.Z;
// angle
ComputeJointAngle(ske, unit, ref stat);
// add to dict
cur_joint_status.Add(unit.boneJoint1, stat);
}
}
//foreach (Joint joint in ske.Joints)
//{
// JointStatus stat = new JointStatus();
// // position
// stat.position.X = joint.Position.X;
// stat.position.Y = joint.Position.Y;
// stat.position.Z = joint.Position.Z;
// // angle
// ComputeJointAllAngles(ske, joint.JointType, ref stat);
// // compute speed using last frame data
// if(jointStatusSeq.Count > 0)
// {
// stat.speed.X = stat.position.X -
// jointStatusSeq[jointStatusSeq.Count - 1][joint.JointType].position.X;
// stat.speed.Y = stat.position.Y -
// jointStatusSeq[jointStatusSeq.Count - 1][joint.JointType].position.Y;
// stat.speed.Z = stat.position.Z -
// jointStatusSeq[jointStatusSeq.Count - 1][joint.JointType].position.Z;
// stat.abs_speed = 30 * Math.Sqrt(Math.Pow(stat.speed.X, 2) +
// Math.Pow(stat.speed.Y, 2) + Math.Pow(stat.speed.Z, 2));
// }
// // add to dict
// cur_joint_status.Add(joint.JointType, stat);
//}
// add to sequence
if (jointStatusSeq.Count == MAX_TRACK_LEN)
{
jointStatusSeq.RemoveAt(0);
}
jointStatusSeq.Add(cur_joint_status);
}
// compute all angles associated with input joint (must have two neighbors)
private void ComputeJointAllAngles(Skeleton ske, JointType type, ref JointStatus status)
{
if (ske == null)
{
return;
}
if (jointNeighbors.ContainsKey(type) && jointNeighbors[type].Count == 2)
{
SkeletonPoint cur_joint_pos = ske.Joints[type].Position;
JointType neighbor_jointtype1 = jointNeighbors[type][0];
JointType neighbor_jointtype2 = jointNeighbors[type][1];
SkeletonPoint neighbor_joint_pos1 = ske.Joints[neighbor_jointtype1].Position;
SkeletonPoint neighbor_joint_pos2 = ske.Joints[neighbor_jointtype2].Position;
// compute bone-bone angle
Point3D vec1 = new Point3D(neighbor_joint_pos1.X - cur_joint_pos.X,
neighbor_joint_pos1.Y - cur_joint_pos.Y,
neighbor_joint_pos1.Z - cur_joint_pos.Z);
Point3D vec2 = new Point3D(neighbor_joint_pos2.X - cur_joint_pos.X,
neighbor_joint_pos2.Y - cur_joint_pos.Y,
neighbor_joint_pos2.Z - cur_joint_pos.Z);
status.angle = Tools.ComputeAngle(vec1, vec2);
// add items
// axis
Dictionary <AxisName, double> axis_temp = new Dictionary <AxisName, double>();
axis_temp.Add(AxisName.XAxis, 0);
axis_temp.Add(AxisName.YAsix, 0);
axis_temp.Add(AxisName.ZAsix, 0);
if (!status.axisAngles.ContainsKey(neighbor_jointtype1))
{
status.axisAngles.Add(neighbor_jointtype1, axis_temp);
}
if (!status.axisAngles.ContainsKey(neighbor_jointtype2))
{
status.axisAngles.Add(neighbor_jointtype2, axis_temp);
}
// plane
Dictionary <PlaneName, double> plane_temp = new Dictionary <PlaneName, double>();
plane_temp.Add(PlaneName.XYPlane, 0);
plane_temp.Add(PlaneName.YZPlane, 0);
plane_temp.Add(PlaneName.XZPlane, 0);
if (!status.planeAngles.ContainsKey(neighbor_jointtype1))
{
status.planeAngles.Add(neighbor_jointtype1, plane_temp);
}
if (!status.planeAngles.ContainsKey(neighbor_jointtype2))
{
status.planeAngles.Add(neighbor_jointtype2, plane_temp);
}
// compute axis angle
Point3D xaxis = new Point3D(1, 0, 0);
status.axisAngles[neighbor_jointtype1][AxisName.XAxis] = Tools.ComputeAngle(vec1, xaxis);
status.axisAngles[neighbor_jointtype2][AxisName.XAxis] = Tools.ComputeAngle(vec2, xaxis);
Point3D yaxis = new Point3D(0, 1, 0);
status.axisAngles[neighbor_jointtype1][AxisName.YAsix] = Tools.ComputeAngle(vec1, yaxis);
status.axisAngles[neighbor_jointtype2][AxisName.YAsix] = Tools.ComputeAngle(vec2, yaxis);
Point3D zaxis = new Point3D(0, 0, 1);
status.axisAngles[neighbor_jointtype1][AxisName.ZAsix] = Tools.ComputeAngle(vec1, zaxis);
status.axisAngles[neighbor_jointtype2][AxisName.ZAsix] = Tools.ComputeAngle(vec2, zaxis);
// compute plane angle
Point3D xyplane1 = new Point3D(vec1.X, vec1.Y, 0); // projection of vec1 to xy plane
status.planeAngles[neighbor_jointtype1][PlaneName.XYPlane] = Tools.ComputeAngle(vec1, xyplane1);
Point3D xyplane2 = new Point3D(vec2.X, vec2.Y, 0);
status.planeAngles[neighbor_jointtype2][PlaneName.XYPlane] = Tools.ComputeAngle(vec2, xyplane2);
Point3D yzplane1 = new Point3D(0, vec1.Y, vec1.Z);
status.planeAngles[neighbor_jointtype1][PlaneName.YZPlane] = Tools.ComputeAngle(vec1, yzplane1);
Point3D yzplane2 = new Point3D(0, vec2.Y, vec2.Z);
status.planeAngles[neighbor_jointtype2][PlaneName.YZPlane] = Tools.ComputeAngle(vec2, yzplane2);
Point3D xzplane1 = new Point3D(vec1.X, 0, vec1.Z);
status.planeAngles[neighbor_jointtype1][PlaneName.XZPlane] = Tools.ComputeAngle(vec1, xzplane1);
Point3D xzplane2 = new Point3D(vec2.X, 0, vec2.Z);
status.planeAngles[neighbor_jointtype2][PlaneName.XZPlane] = Tools.ComputeAngle(vec2, xzplane2);
}
else
{
status.angle = -1;
}
}