public override NUISkeleton MapSkeleton(NUISkeleton inputSkeleton)
{
NUISkeleton outputSkeleton = new NUISkeleton(OutputStructure.Structure);
NUIJoint parentJoint = OutputStructure.Joints[OutputStructure.Structure.RootNode];
outputSkeleton.Joints.Add(OutputStructure.Structure.RootNode, parentJoint);
// Needs to iterate in order.
foreach (var jointType in OutputStructure.Structure.OrderedJointTypes)
{
if (isJointMasked(jointType))
{
NUIJoint maskedJoint = OutputStructure.Joints[jointType];
outputSkeleton.Joints.Add(jointType, maskedJoint);
continue;
}
if (OutputStructure.Structure.IsJointInStructure(jointType))
{
if (!OutputStructure.Structure.IsJointAnExtremity(jointType))
{
// Build the skeleton for this current joint.
RotateJoint(jointType, inputSkeleton, outputSkeleton);
}
else
{
// If extremity, just copy from OutputStructure.
NUIJoint extremity = OutputStructure.Joints[jointType];
outputSkeleton.Joints.Add(jointType, extremity);
}
}
}
return(outputSkeleton);
}
private void swapJoints(NUISkeleton skeleton, NUIJointType left, NUIJointType right)
{
NUIJoint leftJoint = new NUIJoint(right, skeleton.Joints[left].Position, skeleton.Joints[left].Rotation, skeleton.Joints[left].Inferred);
NUIJoint rightJoint = new NUIJoint(left, skeleton.Joints[right].Position, skeleton.Joints[right].Rotation, skeleton.Joints[right].Inferred);
skeleton.Joints[right] = leftJoint;
skeleton.Joints[left] = rightJoint;
}
public override Vector3 GetHipPosition(NUISkeleton skeleton)
{
if (!skeleton.Joints.Keys.Contains <NUIJointType>(NUIJointType.SpineBase))
{
Debug.Log("skeleton has problem: " + skeleton.Joints.Count);
}
NUIJoint joint = skeleton.Joints[NUIJointType.SpineBase];
Vector3 positionInMeters = joint.Position;
return(positionInMeters);
}
public override NUISkeleton MapSkeleton(NUISkeleton inputSkeleton)
{
NUISkeleton outputSkeleton = new NUISkeleton(OutputStructure.Structure);
NUIJoint parentJoint = OutputStructure.Joints[OutputStructure.Structure.RootNode];
outputSkeleton.Joints.Add(OutputStructure.Structure.RootNode, parentJoint);
foreach (NUIJointType jointType in Enum.GetValues(typeof(NUIJointType)))
{
if (isJointMasked(jointType))
{
if (OutputStructure.Joints.ContainsKey(jointType))
{
NUIJoint maskedJoint = OutputStructure.Joints[jointType];
outputSkeleton.Joints.Add(jointType, maskedJoint);
continue;
}
}
if (OutputStructure.Structure.IsJointInStructure(jointType))
{
if (!OutputStructure.Structure.IsJointAnExtremity(jointType))
{
//if (jointType == NUIJointType.SpineBase)
{
// Build the skeleton for this current joint.
RotateJoint(jointType, inputSkeleton, outputSkeleton);
}
}
else
{
// If extremity, just copy from OutputStructure.
NUIJoint extremity = OutputStructure.Joints[jointType];
outputSkeleton.Joints.Add(jointType, extremity);
}
}
}
return(outputSkeleton);
}
private void RotateJoint(NUIJointType jointType, NUISkeleton skeleton, NUISkeleton rig)
{
// Get Info from the OutputStructure
NUIJointType parentJointType = rig.Structure.GetParentJoint(jointType);
NUIJointType childJointType = rig.Structure.GetChildJoint(jointType);
Quaternion jointLocalRotation = OutputStructure.Joints[jointType].Rotation;
Vector3 direction = OutputStructure.Joints[jointType].directionToChild;
Matrix4x4 matrix = rig.Joints[parentJointType].WorldTransformationMatrix;
// Get the target direction based on the captured skeleton.
Vector3 target = new Vector3();
if (rig.Structure.IsJointParentToMany(jointType))
{
if (jointType == NUIJointType.SpineBase)
{
Vector3 worldDirection = matrix.inverse.MultiplyVector(direction);
bool hipDirectionInverted = (worldDirection.y > 0);
target = ((skeleton.Joints[NUIJointType.HipLeft].Position + skeleton.Joints[NUIJointType.HipRight].Position) / 2F) - skeleton.Joints[NUIJointType.SpineBase].Position;
if (hipDirectionInverted)
{
target.y *= -1;
}
}
}
else
{
target = skeleton.Joints[childJointType].Position - skeleton.Joints[jointType].Position;
}
// Get the parent's matrix data
NUIJoint outputJoint = OutputStructure.Joints[jointType];
matrix *= outputJoint.TransformationMatrix;
// Transform the target from capture space to skeleton space.
target = matrix.inverse.MultiplyVector(target);
// Obtain the rotation from the original joint direction to the target direction.
Quaternion quat = Quaternion.FromToRotation(direction, target);
//if (jointType == NUIJointType.SpineMid)
//{
// direction = OutputStructure.ChestRight;
// target = skeleton.Joints[NUIJointType.ShoulderRight].Position - skeleton.Joints[NUIJointType.ShoulderLeft].Position;
// target = matrix.inverse.MultiplyVector(target);
// target -= Vector3.Project(target, OutputStructure.Joints[jointType].directionToChild);
// quat *= Quaternion.FromToRotation(direction, target);
//}
if (jointType == NUIJointType.SpineBase)
{
direction = OutputStructure.SpineBaseRight;
target = skeleton.Joints[NUIJointType.HipRight].Position - skeleton.Joints[NUIJointType.HipLeft].Position;
target = matrix.inverse.MultiplyVector(target);
target -= Vector3.Project(target, OutputStructure.Joints[jointType].directionToChild);
quat *= Quaternion.FromToRotation(direction, target);
}
// Knee correction TODO: Make this optional
if (jointType == NUIJointType.KneeLeft || jointType == NUIJointType.KneeRight ||
jointType == NUIJointType.HipLeft || jointType == NUIJointType.HipRight)
{
quat *= Quaternion.Euler(0, 0, -5f);
}
if (jointType == NUIJointType.AnkleLeft || jointType == NUIJointType.AnkleRight)
{
quat *= Quaternion.Euler(0, 0, -30f);
}
jointLocalRotation *= quat;
if (jointType == NUIJointType.SpineBase)
{
//jointLocalRotation = Quaternion.identity;
}
// Update the rig
NUIJoint newJoint = new NUIJoint(jointType);
newJoint.Position = new Vector3(-outputJoint.Position.x, outputJoint.Position.y, outputJoint.Position.z);
newJoint.Rotation = jointLocalRotation;
newJoint.TransformationMatrix = Matrix4x4.TRS(newJoint.Position, newJoint.Rotation, Vector3.one);
newJoint.WorldTransformationMatrix = rig.Joints[parentJointType].WorldTransformationMatrix * newJoint.TransformationMatrix;
if (!rig.Joints.ContainsKey(jointType))
{
rig.Joints.Add(jointType, newJoint);
}
else
{
rig.Joints[jointType] = newJoint;
}
}
/// <summary>
/// Get the structure for what is expected in this output method.
/// </summary>
/// <returns></returns>
public override NUISkeleton GetTargetStructure()
{
// Define the skeletal structure
SkeletonStructure structure = new SkeletonStructure();
structure.SetRootJoint(NUIJointType.SpineBase);
structure.AddBone(ColladaToNUIJointMapping("SpineMid"), ColladaToNUIJointMapping("SpineBase"));
structure.AddBone(ColladaToNUIJointMapping("SpineShoulder"), ColladaToNUIJointMapping("SpineMid"));
structure.AddBone(ColladaToNUIJointMapping("Neck"), ColladaToNUIJointMapping("SpineShoulder"));
structure.AddBone(ColladaToNUIJointMapping("Head"), ColladaToNUIJointMapping("Neck"));
structure.AddBone(ColladaToNUIJointMapping("ShoulderLeft"), ColladaToNUIJointMapping("SpineShoulder"));
structure.AddBone(ColladaToNUIJointMapping("ElbowLeft"), ColladaToNUIJointMapping("ShoulderLeft"));
structure.AddBone(ColladaToNUIJointMapping("WristLeft"), ColladaToNUIJointMapping("ElbowLeft"));
structure.AddBone(ColladaToNUIJointMapping("HandLeft"), ColladaToNUIJointMapping("WristLeft"));
structure.AddBone(ColladaToNUIJointMapping("HandTipLeft"), ColladaToNUIJointMapping("HandLeft"));
structure.AddBone(ColladaToNUIJointMapping("ThumbLeft"), ColladaToNUIJointMapping("HandLeft"));
structure.AddBone(ColladaToNUIJointMapping("ShoulderRight"), ColladaToNUIJointMapping("SpineShoulder"));
structure.AddBone(ColladaToNUIJointMapping("ElbowRight"), ColladaToNUIJointMapping("ShoulderRight"));
structure.AddBone(ColladaToNUIJointMapping("WristRight"), ColladaToNUIJointMapping("ElbowRight"));
structure.AddBone(ColladaToNUIJointMapping("HandRight"), ColladaToNUIJointMapping("WristRight"));
structure.AddBone(ColladaToNUIJointMapping("HandTipRight"), ColladaToNUIJointMapping("HandRight"));
structure.AddBone(ColladaToNUIJointMapping("ThumbRight"), ColladaToNUIJointMapping("HandRight"));
structure.AddBone(ColladaToNUIJointMapping("HipLeft"), ColladaToNUIJointMapping("SpineBase"));
structure.AddBone(ColladaToNUIJointMapping("KneeLeft"), ColladaToNUIJointMapping("HipLeft"));
structure.AddBone(ColladaToNUIJointMapping("AnkleLeft"), ColladaToNUIJointMapping("KneeLeft"));
structure.AddBone(ColladaToNUIJointMapping("FootLeft"), ColladaToNUIJointMapping("AnkleLeft"));
structure.AddBone(ColladaToNUIJointMapping("HipRight"), ColladaToNUIJointMapping("SpineBase"));
structure.AddBone(ColladaToNUIJointMapping("KneeRight"), ColladaToNUIJointMapping("HipRight"));
structure.AddBone(ColladaToNUIJointMapping("AnkleRight"), ColladaToNUIJointMapping("KneeRight"));
structure.AddBone(ColladaToNUIJointMapping("FootRight"), ColladaToNUIJointMapping("AnkleRight"));
// Define the skeleton in Unity terms.
NUISkeleton skeleton = new NUISkeleton(structure);
// Get the hip right and the chest right.
Vector3 hipRightTranslation = rigData.GetJoint("HipRight").LHSWorldTransformationMatrix.GetColumn(3);
Vector3 hipLeftTranslation = rigData.GetJoint("HipLeft").LHSWorldTransformationMatrix.GetColumn(3);
Vector3 shoulderRightTranslation = rigData.GetJoint("ShoulderRight").LHSWorldTransformationMatrix.GetColumn(3);
Vector3 shoulderLeftTranslation = rigData.GetJoint("ShoulderLeft").LHSWorldTransformationMatrix.GetColumn(3);
Vector3 hipRight = hipRightTranslation - hipLeftTranslation;
Vector3 chestRight = shoulderRightTranslation - shoulderLeftTranslation;
skeleton.SpineBaseRight = rigData.GetJoint("SpineBase").LHSWorldTransformationMatrix.inverse.MultiplyVector(hipRight);
skeleton.ChestRight = rigData.GetJoint("SpineMid").LHSWorldTransformationMatrix.inverse.MultiplyVector(chestRight);
foreach (KeyValuePair <string, ColladaJointData> jointData in rigData.JointData)
{
NUIJointType jointType = ColladaToNUIJointMapping(jointData.Key);
NUIJoint joint = new NUIJoint(jointType);
// Convert the Collada Joint rotation from RHS to LHS (Unity)
ColladaJointData currentJoint = jointData.Value;
joint.Position = currentJoint.Translation;
joint.Rotation = QuaternionHelper.RHStoLHS(currentJoint.RotationVector);
joint.TransformationMatrix = currentJoint.LHSTransformationMatrix;
joint.WorldTransformationMatrix = currentJoint.LHSWorldTransformationMatrix;
Vector3 directionToChild = Vector3.zero;
if (!structure.IsJointAnExtremity(jointType)) // directionToChild is not needed for extremeties.
{
NUIJointType childType = structure.GetChildJoint(jointType);
if (childType != NUIJointType.Unspecified)
{
Vector3 child = rigData.GetJoint(NUIJointToColladaMapping(childType)).LHSWorldTransformationMatrix.GetColumn(3);
Vector3 parent = currentJoint.LHSWorldTransformationMatrix.GetColumn(3);
directionToChild = child - parent;
}
}
if (jointType == NUIJointType.SpineBase) // The Hip is a special case.
{
Vector3 rightHipWorldPosition = rigData.GetJoint("HipRight").LHSWorldTransformationMatrix.GetColumn(3);
Vector3 leftHipWorldPosition = rigData.GetJoint("HipLeft").LHSWorldTransformationMatrix.GetColumn(3);
Vector3 hipWorldPosition = rigData.GetJoint("SpineBase").LHSWorldTransformationMatrix.GetColumn(3);
directionToChild = ((rightHipWorldPosition + leftHipWorldPosition) / 2F) - hipWorldPosition;
//float angle = Vector3.Angle(Vector3.up, directionToChild);
//directionToChild = Quaternion.AngleAxis(40 - angle, hipRight) * directionToChild;
}
joint.directionToChild = currentJoint.LHSWorldTransformationMatrix.inverse.MultiplyVector(directionToChild);
skeleton.Joints.Add(jointType, joint);
}
skeleton.ChestRight -= Vector3.Project(skeleton.ChestRight, skeleton.Joints[NUIJointType.SpineMid].directionToChild);
return(skeleton);
}
