// get one joint's offset in kinect skeleton frame
private Point3D getNodeOffset(JointNode node, Skeleton skeleton)
{
if (!node.IsKinectJoint)
{
return(new Point3D());
}
Point3D offset;
if (node.Type == NodeTypeEnum.ROOT)
{
offset = new Point3D(skeleton.Position.X, skeleton.Position.Y, skeleton.Position.Z);
}
else
{
JointNode parent = node.Parent;
while (null != parent && !parent.IsKinectJoint)
{
parent = parent.Parent;
}
offset = new Point3D(skeleton.Joints[node.JointIndex].Position.X - skeleton.Joints[parent.JointIndex].Position.X,
skeleton.Joints[node.JointIndex].Position.Y - skeleton.Joints[parent.JointIndex].Position.Y,
skeleton.Joints[node.JointIndex].Position.Z - skeleton.Joints[parent.JointIndex].Position.Z);
}
BVHFile.BVHScalePoint(ref offset);
return(offset);
}
/// <summary>
/// length of hip center - spine - shoulder center - head
/// </summary>
/// <param name="skeleton"></param>
/// <param name="bone"></param>
/// <param name="startJoint"></param>
/// <param name="endJoint"></param>
private void calCenterBoneLength(Skeleton skeleton, JointNode node)
{
double length = BVHFile.BVHScaledValue(calDescartesLength(skeleton.Joints[node.Parent.JointIndex].Position,
skeleton.Joints[node.JointIndex].Position));
if (_boneLengths.ContainsKey(node.JointIndex))
{
_boneLengths[node.JointIndex] = (length + _boneLengths[node.JointIndex]) / 2;
}
else
{
_boneLengths.Add(node.JointIndex, length);
}
}
/// <summary>
/// length of shoulder center - shoulder - elbow - wrist - hand, hip center - hip - knee - ankle - foot
/// </summary>
/// <param name="skeleton"></param>
/// <param name="bone"></param>
/// <param name="startJoint"></param>
/// <param name="endJoint"></param>
private void calSideBoneLength(Skeleton skeleton, JointNode node)
{
double leftLength = BVHFile.BVHScaledValue(calDescartesLength(skeleton.Joints[node.Parent.JointIndex].Position,
skeleton.Joints[node.JointIndex].Position));
double rightLength = BVHFile.BVHScaledValue(calDescartesLength(skeleton.Joints[GetSymmetricJoint(node.Parent.JointIndex)].Position,
skeleton.Joints[GetSymmetricJoint(node.JointIndex)].Position));
double length = (leftLength + rightLength) / 2;
if (_boneLengths.ContainsKey(node.JointIndex))
{
_boneLengths[node.JointIndex] = (length + _boneLengths[node.JointIndex]) / 2;
}
else
{
_boneLengths.Add(node.JointIndex, length);
}
}
public MainWindow()
{
InitializeComponent();
_kinectSkeleton = new KinectSkeleton();
bvhWriter = new BVHFile(_kinectSkeleton);
}
public MainWindow()
{
InitializeComponent();
_kinectSkeleton = new KinectSkeleton();
bvhWriter = new BVHFile(_kinectSkeleton);
}
/// <summary>
/// update all nodes' offsets and rotations with the latest skeleton frame
/// </summary>
/// <param name="skeleton"></param>
public void UpdateJoints(Skeleton skeleton)
{
foreach (JointNode node in JointNodes.Values)
{
if (NodeTypeEnum.END != node.Type)
{
JointType rotationJoint = getRotationJoint(node);
Vector3D rotation;
Quaternion rotationQuat;
// root joint
if (node.Type == NodeTypeEnum.ROOT)
{
// offset
Point3D skeletonOffset = new Point3D(skeleton.Position.X, skeleton.Position.Y, skeleton.Position.Z);
BVHFile.BVHScalePoint(ref skeletonOffset);
node.Offset = new Point3D(skeletonOffset.X - _initialOffsets[node.Name].X,
skeletonOffset.Y - _initialOffsets[node.Name].Y + calRealHeightOfHipJoint(),
-(skeletonOffset.Z - _initialOffsets[node.Name].Z));
// rotation
rotationQuat = MathHelper.Vector4ToQuaternion(skeleton.BoneOrientations[rotationJoint].AbsoluteRotation.Quaternion);
rotation = MathHelper.QuaternionToAxisAngles(rotationQuat);
rotation.X = 0;
rotation.Y *= -1;
rotation.Z = 0;
}
else // all other joints
{ // bones need no rotation
if (!(node.Name == "Spine" || node.Name == "Head" ||
node.Name == "CollarLeft" || node.Name == "CollarRight" || node.Name == "AnkleLeft" || node.Name == "AnkleRight"))
{
if (node.Name == "HipCenter2" ||
node.Name == "ShoulderLeft" || node.Name == "ShoulderRight" ||
node.Name == "HipLeft" || node.Name == "HipRight")
{
Vector3D offset = new Vector3D();
Vector3D axis;
JointType parentJoint = node.JointIndex;
switch (node.Name)
{
case "HipLeft":
axis = new Vector3D(0, -1, 0);
break;
case "HipRight":
axis = new Vector3D(0, -1, 0);
break;
case "ShoulderLeft":
axis = new Vector3D(-1, 0, 0);
break;
case "ShoulderRight":
axis = new Vector3D(1, 0, 0);
break;
case "HipCenter2":
axis = new Vector3D(0, 1, 0);
rotationJoint = JointType.ShoulderCenter;
parentJoint = JointType.Spine;
break;
default:
axis = new Vector3D();
break;
}
offset.X = skeleton.Joints[rotationJoint].Position.X - skeleton.Joints[parentJoint].Position.X;
offset.Y = skeleton.Joints[rotationJoint].Position.Y - skeleton.Joints[parentJoint].Position.Y;
offset.Z = skeleton.Joints[rotationJoint].Position.Z - skeleton.Joints[parentJoint].Position.Z;
offset.Normalize();
if (node.Name == "ShoulderLeft" || node.Name == "ShoulderRight")
{
Vector3D rotationOffset = MathHelper.QuaternionToAxisAngles(
MathHelper.Vector4ToQuaternion(skeleton.BoneOrientations[JointType.ShoulderCenter].AbsoluteRotation.Quaternion));
Matrix3D rotationMatrix = MathHelper.GetRotationMatrix(-rotationOffset.X * Math.PI / 180 - 180, 0, 0);
rotation = Vector3D.Multiply(offset, rotationMatrix);
rotationQuat = MathHelper.GetQuaternion(axis, rotation);
rotation = MathHelper.QuaternionToAxisAngles(rotationQuat);
rotation.Z *= -1;
}
else if (node.Name == "HipLeft" || node.Name == "HipRight")
{
Vector3D rotationOffset = MathHelper.QuaternionToAxisAngles(
MathHelper.Vector4ToQuaternion(skeleton.BoneOrientations[JointType.HipCenter].AbsoluteRotation.Quaternion));
rotation = Vector3D.Multiply(offset, MathHelper.GetRotationMatrixY(-rotationOffset.Y * Math.PI / 180));
rotationQuat = MathHelper.GetQuaternion(axis, rotation);
rotation = MathHelper.QuaternionToAxisAngles(rotationQuat);
rotation.X *= -1;
rotation.Y *= -1;
rotation.Z *= -1;
}
else
{
Vector3D rotationOffset = MathHelper.QuaternionToAxisAngles(
MathHelper.Vector4ToQuaternion(skeleton.BoneOrientations[JointType.HipCenter].AbsoluteRotation.Quaternion));
rotation = Vector3D.Multiply(offset, MathHelper.GetRotationMatrixY(-rotationOffset.Y * Math.PI / 180));
rotationQuat = MathHelper.GetQuaternion(axis, rotation);
rotation = MathHelper.QuaternionToAxisAngles(rotationQuat);
rotation.X *= -1;
rotation.Y = 0;
rotation.Z *= -1;
}
}
else
{
rotationQuat = MathHelper.Vector4ToQuaternion(skeleton.BoneOrientations[rotationJoint].HierarchicalRotation.Quaternion);
rotation = MathHelper.QuaternionToAxisAngles(rotationQuat);
if (node.BaseAxis == Axis.nY)
{
rotation.X *= -1;
rotation.Y *= -1;
}
if (node.BaseAxis == Axis.nX)
{
Vector3D tempVector = new Vector3D(rotation.X, rotation.Y, rotation.Z);
rotation.X = -tempVector.Z;
rotation.Y = -tempVector.Y;
rotation.Z = -tempVector.X;
}
if (node.BaseAxis == Axis.X)
{
Vector3D tempVector = new Vector3D(rotation.X, rotation.Y, rotation.Z);
rotation.X = tempVector.Z;
rotation.Y = tempVector.Y;
rotation.Z = tempVector.X;
}
}
rotation.X = illegalDoubleFilter(rotation.X);
rotation.Y = illegalDoubleFilter(rotation.Y);
rotation.Z = illegalDoubleFilter(rotation.Z);
}
else
{
rotation = new Vector3D();
}
}
if (_clibrationAngles.ContainsKey(node.Name))
{
node.Rotation = rotation + _clibrationAngles[node.Name];
}
else
{
node.Rotation = rotation;
}
}
}
}