private void initializeJoints(Skeleton skeleton)
{
foreach (JointNode node in JointNodes)
{
if (_cliberateNum == 1)
{
_initialRotations = new Quaternion[INT_JOINTCOUNT];
_initialOffsets = new Point3D[INT_JOINTCOUNT];
if (node.JointIndex == JointType.HipCenter)
{
_initialOffsets[(int)node.JointIndex].X = skeleton.Joints[node.JointIndex].Position.X;
_initialOffsets[(int)node.JointIndex].Y = skeleton.Joints[node.JointIndex].Position.Y;
_initialOffsets[(int)node.JointIndex].Z = skeleton.Joints[node.JointIndex].Position.Z;
_initialRotations[(int)node.JointIndex] = KinectDataConverter.Vector4ToQuaternion(skeleton.BoneOrientations[node.JointIndex].AbsoluteRotation.Quaternion);
}
else
{
_initialOffsets[(int)node.JointIndex].X = skeleton.Joints[node.JointIndex].Position.X - skeleton.Joints[node.Parent.JointIndex].Position.X;
_initialOffsets[(int)node.JointIndex].Y = skeleton.Joints[node.JointIndex].Position.Y - skeleton.Joints[node.Parent.JointIndex].Position.Y;
_initialOffsets[(int)node.JointIndex].Z = skeleton.Joints[node.JointIndex].Position.Z - skeleton.Joints[node.Parent.JointIndex].Position.Z;
_initialRotations[(int)node.JointIndex] = KinectDataConverter.Vector4ToQuaternion(skeleton.BoneOrientations[getRotationJoint(node)].HierarchicalRotation.Quaternion);
}
}
else
{
if (node.JointIndex == JointType.HipCenter)
{
_initialOffsets[(int)node.JointIndex].X = (_initialOffsets[(int)node.JointIndex].X + skeleton.Joints[node.JointIndex].Position.X) / 2;
_initialOffsets[(int)node.JointIndex].Y = (_initialOffsets[(int)node.JointIndex].Y + skeleton.Joints[node.JointIndex].Position.Y) / 2;
_initialOffsets[(int)node.JointIndex].Z = (_initialOffsets[(int)node.JointIndex].Z + skeleton.Joints[node.JointIndex].Position.Z) / 2;
_initialRotations[(int)node.JointIndex] = KinectDataConverter.Vector4ToQuaternion(skeleton.BoneOrientations[node.JointIndex].AbsoluteRotation.Quaternion);
}
else
{
_initialOffsets[(int)node.JointIndex].X = (_initialOffsets[(int)node.JointIndex].X + skeleton.Joints[node.JointIndex].Position.X - skeleton.Joints[node.Parent.JointIndex].Position.X) / 2;
_initialOffsets[(int)node.JointIndex].Y = (_initialOffsets[(int)node.JointIndex].Y + skeleton.Joints[node.JointIndex].Position.Y - skeleton.Joints[node.Parent.JointIndex].Position.Y) / 2;
_initialOffsets[(int)node.JointIndex].Z = (_initialOffsets[(int)node.JointIndex].Z + skeleton.Joints[node.JointIndex].Position.Z - skeleton.Joints[node.Parent.JointIndex].Position.Z) / 2;
_initialRotations[(int)node.JointIndex] = KinectDataConverter.Vector4ToQuaternion(skeleton.BoneOrientations[getRotationJoint(node)].HierarchicalRotation.Quaternion);
}
}
}
}
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>
/// update all nodes' offsets and rotations with the latest skeleton frame
/// </summary>
/// <param name="skeleton"></param>
public void UpdateJoints(Skeleton skeleton)
{
//StringBuilder sb = new StringBuilder();
//foreach (JointType joint in Enum.GetValues(typeof(JointType)))
//{
// sb.Append(joint.ToString() + ", Start: " + skeleton.BoneOrientations[joint].StartJoint.ToString()
// + ", End : " + skeleton.BoneOrientations[joint].EndJoint.ToString()
// + " Rotation: " + getrotationString(skeleton.BoneOrientations[joint].HierarchicalRotation) + "\n");
//}
//MessageBox.Show(sb.ToString());
/******* using position to calculate orientation ********/
//Vector3D vectorX, vectorY, vectorZ;
//if (node.JointIndex == JointType.HipCenter)
//{
// vectorY = KinectDataConverter.SkeletonPosToVector3d(skeleton.Joints[JointType.Spine].Position)
// - KinectDataConverter.SkeletonPosToVector3d(skeleton.Joints[JointType.HipCenter].Position);
// vectorX = KinectDataConverter.SkeletonPosToVector3d(skeleton.Joints[JointType.HipLeft].Position)
// - KinectDataConverter.SkeletonPosToVector3d(skeleton.Joints[JointType.HipRight].Position);
// Matrix3D rotationMatrix = new Matrix3D();
// KinectDataConverter.MakeMatrixFromYX(vectorX, vectorY, ref rotationMatrix);
// Quaternion rotationQuaternion = KinectDataConverter.LookRotation(
// KinectDataConverter.GetZColumn(rotationMatrix), KinectDataConverter.GetYColumn(rotationMatrix));
//}
/******* using position to calculate orientation ********/
foreach (JointNode node in JointNodes)
{
JointType rotationJoint = getRotationJoint(node);
Quaternion rotation = new Quaternion();
if (node.Type == NodeTypeEnum.ROOT)
{
node.Offset.X = BVHWriter.BVHScaledValue(skeleton.Joints[node.JointIndex].Position.X - _initialOffsets[(int)node.JointIndex].X);
node.Offset.Y = BVHWriter.BVHScaledValue(skeleton.Joints[node.JointIndex].Position.Y - _initialOffsets[(int)node.JointIndex].Y + calRealHeightOfHipJoint());
node.Offset.Z = BVHWriter.BVHScaledValue(skeleton.Joints[node.JointIndex].Position.Z - _initialOffsets[(int)node.JointIndex].Z);
rotation = KinectDataConverter.Vector4ToQuaternion(skeleton.BoneOrientations[node.JointIndex].AbsoluteRotation.Quaternion);
Quaternion initialQuat = _initialRotations[(int)node.JointIndex];
initialQuat.Invert();
rotation = initialQuat * rotation;
node.Rotation = KinectDataConverter.QuaternionToAxisAngles(rotation);
}
else
{
rotation = KinectDataConverter.Vector4ToQuaternion(skeleton.BoneOrientations[rotationJoint].HierarchicalRotation.Quaternion);
Quaternion initialQuat = _initialRotations[(int)node.JointIndex];
initialQuat.Invert();
rotation = initialQuat * rotation;
node.Rotation = KinectDataConverter.QuaternionToAxisAngles(rotation);
//if (node.Axis == TransAxis.X || node.Axis == TransAxis.nX)
//{
// Vector3D tempRotation = node.Rotation;
// node.Rotation.X = tempRotation.Z;
// node.Rotation.Z = tempRotation.X;
//}
}
}
}
