/// <summary>
/// recurse search the skeleton structure to find the joint node
/// </summary>
/// <param name="root"></param>
/// <param name="joint"></param>
/// <returns></returns>
private JointNode getJointNode(JointNode root, JointType joint)
{
if (joint == root.JointIndex)
{
return(root);
}
else
{
if (root == null || NodeTypeEnum.END == root.Type)
{
return(null);
}
else
{
foreach (JointNode node in root.Children)
{
JointNode tempNode = getJointNode(node, joint);
if (tempNode == null)
{
continue;
}
else
{
return(tempNode);
}
}
return(null);
}
}
}
public JointNode(NodeTypeEnum type, JointType index, JointNode parent, TransAxis axis)
{
this.JointIndex = index;
this.Type = type;
if (NodeTypeEnum.END != Type)
Children = new ArrayList();
this.Parent = parent;
if(parent != null)
parent.AddChild(this);
this.Axis = axis;
}
public JointNode(NodeTypeEnum type, JointType index, JointNode parent, TransAxis axis)
{
this.JointIndex = index;
this.Type = type;
if (NodeTypeEnum.END != Type)
{
Children = new ArrayList();
}
this.Parent = parent;
if (parent != null)
{
parent.AddChild(this);
}
this.Axis = axis;
}
/// <summary>
/// one joint's rotation is actually stored in its parent joint, so for a given joint, we should get its child joint's rotation
/// </summary>
/// <param name="joint"></param>
/// <returns></returns>
private JointType getRotationJoint(JointNode node)
{
switch (node.JointIndex)
{
case JointType.ShoulderLeft:
return(JointType.ElbowLeft);
case JointType.ElbowLeft:
return(JointType.WristLeft);
case JointType.WristLeft:
return(JointType.HandLeft);
case JointType.ShoulderRight:
return(JointType.ElbowRight);
case JointType.ElbowRight:
return(JointType.WristRight);
case JointType.WristRight:
return(JointType.HandRight);
case JointType.HipLeft:
return(JointType.KneeLeft);
case JointType.KneeLeft:
return(JointType.AnkleLeft);
case JointType.AnkleLeft:
return(JointType.FootLeft);
case JointType.HipRight:
return(JointType.KneeRight);
case JointType.KneeRight:
return(JointType.AnkleRight);
case JointType.AnkleRight:
return(JointType.FootRight);
default:
return(node.JointIndex);
}
}
public void getJointPositions_Debug(JointNode node)
{
if (NodeTypeEnum.END == node.Type)
{
return;
}
if (node.Type == NodeTypeEnum.ROOT)
{
Positions_Debug[(int)node.JointIndex] = new Point3D(320, 240, 0);
}
else
{
Positions_Debug[(int)node.JointIndex] = new Point3D(node.OriginalOffset.X + Positions_Debug[(int)node.Parent.JointIndex].X,
node.OriginalOffset.Y + Positions_Debug[(int)node.Parent.JointIndex].Y, 0);
}
foreach (JointNode child in node.Children)
{
getJointPositions_Debug(child);
}
}
/// <summary>
/// recurse search the skeleton structure to find the joint node
/// </summary>
/// <param name="root"></param>
/// <param name="joint"></param>
/// <returns></returns>
private JointNode getJointNode(JointNode root, JointType joint)
{
if (joint == root.JointIndex)
{
return root;
}
else
{
if (root == null || NodeTypeEnum.END == root.Type)
{
return null;
}
else
{
foreach (JointNode node in root.Children)
{
JointNode tempNode = getJointNode(node, joint);
if (tempNode == null)
continue;
else
return tempNode;
}
return null;
}
}
}
public void getJointPositions_Debug(JointNode node)
{
if (NodeTypeEnum.END == node.Type)
return;
if (node.Type == NodeTypeEnum.ROOT)
{
Positions_Debug[(int)node.JointIndex] = new Point3D(320, 240, 0);
}
else
{
Positions_Debug[(int)node.JointIndex] = new Point3D(node.OriginalOffset.X + Positions_Debug[(int)node.Parent.JointIndex].X,
node.OriginalOffset.Y + Positions_Debug[(int)node.Parent.JointIndex].Y, 0);
}
foreach (JointNode child in node.Children)
{
getJointPositions_Debug(child);
}
}
/// <summary>
/// output a given joint in BVH format
/// </summary>
/// <param name="joint"></param>
/// <param name="sbOut"></param>
/// <param name="indents"></param>
private void buildBVHOutputSkeleton(JointNode root, string indents = "")
{
_bvhSkeletonBuffer.Append(indents);
_bvhSkeletonBuffer.Append(root.Type != NodeTypeEnum.END ?
root.Type.ToString() + CHAR_SPACE + root.JointIndex.ToString() : STR_END_SITE).Append(CHAR_LF);
_bvhSkeletonBuffer.Append(indents).Append(CHAR_OPEN_BRACE).Append(CHAR_LF);
_bvhSkeletonBuffer.Append(indents).Append(CHAR_SPACE).Append(CHAR_SPACE).Append(STR_OFFSET).Append(CHAR_SPACE)
.Append(root.OriginalOffset.X.ToString()).Append(CHAR_SPACE)
.Append(root.OriginalOffset.Y.ToString()).Append(CHAR_SPACE)
.Append(root.OriginalOffset.Z.ToString()).Append(CHAR_LF);
if (root.Type != NodeTypeEnum.END)
{
_bvhSkeletonBuffer.Append(indents).Append(CHAR_SPACE).Append(CHAR_SPACE).Append(STR_CHANNELS).Append(CHAR_SPACE)
.Append(root.Type == NodeTypeEnum.ROOT ? ROOT_CHANNEL_NUM : JOINT_CHANNEL_NUM);
if (root.Type == NodeTypeEnum.ROOT)
{
_bvhSkeletonBuffer.Append(CHAR_SPACE).Append(STR_XPOSITION)
.Append(CHAR_SPACE).Append(STR_YPOSITION)
.Append(CHAR_SPACE).Append(STR_ZPOSITION);
}
_bvhSkeletonBuffer.Append(CHAR_SPACE).Append(STR_ZROTATION)
.Append(CHAR_SPACE).Append(STR_XROTATION)
.Append(CHAR_SPACE).Append(STR_YROTATION).Append(CHAR_LF);
foreach (JointNode childJoint in root.Children)
{
buildBVHOutputSkeleton(childJoint, indents + CHAR_SPACE + CHAR_SPACE);
}
}
_bvhSkeletonBuffer.Append(indents).Append(CHAR_CLOSE_BRACE).Append(CHAR_LF);
}
private void AddBVHBone_Debug(JointNode node1, JointNode node2)
{
addLineToWindow(node1.Offset.X, node1.Offset.Y,
node2.Offset.X, node2.Offset.Y,
BTUSH_BONE_DEBUG, DOUBLE_THICKNESS_BONE_DEBUG);
}
public bool AddChild(JointNode node)
{
return(Children.Add(node) >= 0);
}
public bool AddChild(JointNode node)
{
return Children.Add(node) >= 0;
}
public JointNode(JointType index, JointNode parent, TransAxis axis)
: this(NodeTypeEnum.JOINT, index, parent, axis)
{
}
public static void UpdateSkeletonForDebug(JointNode node)
{
// rotation of a bone is actually the rotation of joint's parent joint
Quaternion parentRotation;
if (node.JointIndex == JointType.HipCenter)
parentRotation = ConvertToQuaternion(node.Rotation);
else
parentRotation = ConvertToQuaternion(node.Parent.Rotation);
RotatePoint(node.OriginalOffset, ConvertToQuaternion(node.Rotation), out node.Offset);
// update node rotation to absolute
node.Rotation = KinectDataConverter.QuaternionToAxisAngles(ConvertToQuaternion(node.Rotation) * parentRotation);
if (NodeTypeEnum.ROOT == node.Type)
{
node.Offset.X += _renderingSolution.OriginX;
node.Offset.Y += _renderingSolution.OriginY;
node.Offset.Z += 0;
}
else
{
node.Offset.X += node.Parent.Offset.X;
node.Offset.Y += node.Parent.Offset.Y;
node.Offset.Z += node.Parent.Offset.Z;
}
if (node.Type == NodeTypeEnum.END)
return;
foreach (JointNode child in node.Children)
{
UpdateSkeletonForDebug(child);
}
}
/// <summary>
/// initialize human skeleton structure
/// </summary>
private void initializeSkeletonStructure()
{
JointNodes = new JointNode[INT_JOINTCOUNT];
// root
hipRoot = new JointNode(NodeTypeEnum.ROOT, JointType.HipCenter, null, TransAxis.None);
JointNodes[(int)JointType.HipCenter] = hipRoot;
// spine, neck, head
JointNode spine = new JointNode(JointType.Spine, hipRoot, TransAxis.Y);
JointNodes[(int)JointType.Spine] = spine;
JointNode shouderCenter = new JointNode(JointType.ShoulderCenter, spine, TransAxis.Y);
JointNodes[(int)JointType.ShoulderCenter] = shouderCenter;
JointNode head = new JointNode(JointType.Head, shouderCenter, TransAxis.Y);
JointNodes[(int)JointType.Head] = head;
JointNode headEnd = new JointNode(NodeTypeEnum.END, JointType.Head, head, TransAxis.None);
// left arm
JointNode leftShoulder = new JointNode(JointType.ShoulderLeft, shouderCenter, TransAxis.X);
JointNodes[(int)JointType.ShoulderLeft] = leftShoulder;
JointNode leftElbow = new JointNode(JointType.ElbowLeft, leftShoulder, TransAxis.X);
JointNodes[(int)JointType.ElbowLeft] = leftElbow;
JointNode leftWrist = new JointNode(JointType.WristLeft, leftElbow, TransAxis.X);
JointNodes[(int)JointType.WristLeft] = leftWrist;
JointNode leftHand = new JointNode(NodeTypeEnum.END, JointType.HandLeft, leftWrist, TransAxis.X);
JointNodes[(int)JointType.HandLeft] = leftHand;
// right arm
JointNode rightShoulder = new JointNode(JointType.ShoulderRight, shouderCenter, TransAxis.nX);
JointNodes[(int)JointType.ShoulderRight] = rightShoulder;
JointNode rightElbow = new JointNode(JointType.ElbowRight, rightShoulder, TransAxis.nX);
JointNodes[(int)JointType.ElbowRight] = rightElbow;
JointNode rightWrist = new JointNode(JointType.WristRight, rightElbow, TransAxis.nX);
JointNodes[(int)JointType.WristRight] = rightWrist;
JointNode rightHand = new JointNode(NodeTypeEnum.END, JointType.HandRight, rightWrist, TransAxis.nX);
JointNodes[(int)JointType.HandRight] = rightHand;
// left lower part
JointNode leftUpLeg = new JointNode(JointType.HipLeft, hipRoot, TransAxis.X);
JointNodes[(int)JointType.HipLeft] = leftUpLeg;
JointNode leftKnee = new JointNode(JointType.KneeLeft, leftUpLeg, TransAxis.nY);
JointNodes[(int)JointType.KneeLeft] = leftKnee;
JointNode leftAnkle = new JointNode(JointType.AnkleLeft, leftKnee, TransAxis.nY);
JointNodes[(int)JointType.AnkleLeft] = leftAnkle;
JointNode leftFoot = new JointNode(NodeTypeEnum.END, JointType.FootLeft, leftAnkle, TransAxis.Z);
JointNodes[(int)JointType.FootLeft] = leftFoot;
// right lower part
JointNode rightUpLeg = new JointNode(JointType.HipRight, hipRoot, TransAxis.nX);
JointNodes[(int)JointType.HipRight] = rightUpLeg;
JointNode rightKnee = new JointNode(JointType.KneeRight, rightUpLeg, TransAxis.nY);
JointNodes[(int)JointType.KneeRight] = rightKnee;
JointNode rightAnkle = new JointNode(JointType.AnkleRight, rightKnee, TransAxis.nY);
JointNodes[(int)JointType.AnkleRight] = rightAnkle;
JointNode rightFoot = new JointNode(NodeTypeEnum.END, JointType.FootRight, rightAnkle, TransAxis.Z);
JointNodes[(int)JointType.FootRight] = rightFoot;
}
/// <summary>
/// one joint's rotation is actually stored in its parent joint, so for a given joint, we should get its child joint's rotation
/// </summary>
/// <param name="joint"></param>
/// <returns></returns>
private JointType getRotationJoint(JointNode node)
{
switch (node.JointIndex)
{
case JointType.ShoulderLeft:
return JointType.ElbowLeft;
case JointType.ElbowLeft:
return JointType.WristLeft;
case JointType.WristLeft:
return JointType.HandLeft;
case JointType.ShoulderRight:
return JointType.ElbowRight;
case JointType.ElbowRight:
return JointType.WristRight;
case JointType.WristRight:
return JointType.HandRight;
case JointType.HipLeft:
return JointType.KneeLeft;
case JointType.KneeLeft:
return JointType.AnkleLeft;
case JointType.AnkleLeft:
return JointType.FootLeft;
case JointType.HipRight:
return JointType.KneeRight;
case JointType.KneeRight:
return JointType.AnkleRight;
case JointType.AnkleRight:
return JointType.FootRight;
default:
return node.JointIndex;
}
}
/// <summary>
/// initialize human skeleton structure
/// </summary>
private void initializeSkeletonStructure()
{
JointNodes = new JointNode[INT_JOINTCOUNT];
// root
hipRoot = new JointNode(NodeTypeEnum.ROOT, JointType.HipCenter, null, TransAxis.None);
JointNodes[(int)JointType.HipCenter] = hipRoot;
// spine, neck, head
JointNode spine = new JointNode(JointType.Spine, hipRoot, TransAxis.Y);
JointNodes[(int)JointType.Spine] = spine;
JointNode shouderCenter = new JointNode(JointType.ShoulderCenter, spine, TransAxis.Y);
JointNodes[(int)JointType.ShoulderCenter] = shouderCenter;
JointNode head = new JointNode(JointType.Head, shouderCenter, TransAxis.Y);
JointNodes[(int)JointType.Head] = head;
JointNode headEnd = new JointNode(NodeTypeEnum.END, JointType.Head, head, TransAxis.None);
// left arm
JointNode leftShoulder = new JointNode(JointType.ShoulderLeft, shouderCenter, TransAxis.X);
JointNodes[(int)JointType.ShoulderLeft] = leftShoulder;
JointNode leftElbow = new JointNode(JointType.ElbowLeft, leftShoulder, TransAxis.X);
JointNodes[(int)JointType.ElbowLeft] = leftElbow;
JointNode leftWrist = new JointNode(JointType.WristLeft, leftElbow, TransAxis.X);
JointNodes[(int)JointType.WristLeft] = leftWrist;
JointNode leftHand = new JointNode(NodeTypeEnum.END, JointType.HandLeft, leftWrist, TransAxis.X);
JointNodes[(int)JointType.HandLeft] = leftHand;
// right arm
JointNode rightShoulder = new JointNode(JointType.ShoulderRight, shouderCenter, TransAxis.nX);
JointNodes[(int)JointType.ShoulderRight] = rightShoulder;
JointNode rightElbow = new JointNode(JointType.ElbowRight, rightShoulder, TransAxis.nX);
JointNodes[(int)JointType.ElbowRight] = rightElbow;
JointNode rightWrist = new JointNode(JointType.WristRight, rightElbow, TransAxis.nX);
JointNodes[(int)JointType.WristRight] = rightWrist;
JointNode rightHand = new JointNode(NodeTypeEnum.END, JointType.HandRight, rightWrist, TransAxis.nX);
JointNodes[(int)JointType.HandRight] = rightHand;
// left lower part
JointNode leftUpLeg = new JointNode(JointType.HipLeft, hipRoot, TransAxis.X);
JointNodes[(int)JointType.HipLeft] = leftUpLeg;
JointNode leftKnee = new JointNode(JointType.KneeLeft, leftUpLeg, TransAxis.nY);
JointNodes[(int)JointType.KneeLeft] = leftKnee;
JointNode leftAnkle = new JointNode(JointType.AnkleLeft, leftKnee, TransAxis.nY);
JointNodes[(int)JointType.AnkleLeft] = leftAnkle;
JointNode leftFoot = new JointNode(NodeTypeEnum.END, JointType.FootLeft, leftAnkle, TransAxis.Z);
JointNodes[(int)JointType.FootLeft] = leftFoot;
// right lower part
JointNode rightUpLeg = new JointNode(JointType.HipRight, hipRoot, TransAxis.nX);
JointNodes[(int)JointType.HipRight] = rightUpLeg;
JointNode rightKnee = new JointNode(JointType.KneeRight, rightUpLeg, TransAxis.nY);
JointNodes[(int)JointType.KneeRight] = rightKnee;
JointNode rightAnkle = new JointNode(JointType.AnkleRight, rightKnee, TransAxis.nY);
JointNodes[(int)JointType.AnkleRight] = rightAnkle;
JointNode rightFoot = new JointNode(NodeTypeEnum.END, JointType.FootRight, rightAnkle, TransAxis.Z);
JointNodes[(int)JointType.FootRight] = rightFoot;
}
public JointNode(JointType index, JointNode parent, TransAxis axis) :
this(NodeTypeEnum.JOINT, index, parent, axis)
{
}
