/// <summary>
/// Create a representation of a Joint.
/// </summary>
/// <param name="jointType">The joint type.</param>
/// <param name="position">The position from it's parent.</param>
/// <param name="rotation">The rotation.</param>
/// <param name="inferred">Was the joint inferred?</param>
public NUIJoint(NUIJointType jointType, Vector3 position, Quaternion rotation, bool inferred)
{
this.jointType = jointType;
this.position = position;
this.rotation = rotation;
this.inferred = inferred;
}
public NUIJoint(NUIJointType jointType, Vector3 position, Quaternion rotation, float inferredQuality)
{
this.jointType = jointType;
this.position = position;
this.rotation = rotation;
this.inferredQuality = inferredQuality;
}
public void AddJoint(NUIJointType jt, Vector3 position, Quaternion orientation, TrackingState trackingState)
{
jointTypes.Add(jt);
jointPositions.Add(position);
jointOrientations.Add(orientation);
jointTracking.Add(trackingState);
}
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 void UpdateSkeleton(SkeletonFrameData skeletonFrameData, InputSkeletonType inputSkeletonType)
{
if (lineRenderer == null)
{
Debug.Log("LineRenderer STILL not found!");
}
NUIJointType[] currentOrder = new NUIJointType[0];
if (inputSkeletonType != skeletonType)
{
switch (inputSkeletonType)
{
case InputSkeletonType.Kinect1_20Joint:
#if UNITY_5_5_OR_NEWER
lineRenderer.positionCount = 29;
#else
lineRenderer.SetVertexCount(29);
#endif
currentOrder = Skeleton20JointOrder;
break;
case InputSkeletonType.Kinect2_25Joint:
#if UNITY_5_5_OR_NEWER
lineRenderer.positionCount = 38;
#else
lineRenderer.SetVertexCount(38);
#endif
currentOrder = Skeleton25JointOrder;
break;
}
}
Vector3 pos;
int index = 0;
// don't care about below variables, but they're needed
Quaternion q;
TrackingState ts;
foreach (NUIJointType njt in currentOrder)
{
pos = Vector3.zero;
skeletonFrameData.GetJointData(njt, out pos, out q, out ts);
lineRenderer.SetPosition(index, pos);
index++;
}
}
/// <summary>
/// Set the Root node of the structure.
/// </summary>
/// <param name="rootJoint">The root joint of the skeleton.</param>
/// <returns>true if added successfully.</returns>
public bool SetRootJoint(NUIJointType rootJoint)
{
if (structure.Count == 0)
{
rootNode = rootJoint;
structure.Add(rootJoint, rootJoint);
extremity.Add(rootJoint, true);
multiChildren.Add(rootJoint, false);
return(true);
}
return(false);
}
public override void Update()
{
if (bodyReader != null)
{
getMainUser();
}
foreach (var viewer in viewers)
{
viewer.Update(bodyData, 0);
}
// Get the pose.
if (currentBody != null && currentBody.IsTracked)
{
// Encapsulate the important frame data
SkeletonFrameData frameData = new SkeletonFrameData();
frameData.TrackingId = currentBody.TrackingId;
frameData.IsTracked = currentBody.IsTracked;
// Build the skeleton
for (JointType jt = JointType.SpineBase; jt <= JointType.ThumbRight; jt++)
{
NUIJointType jointType = NUICaptureHelper.JointTypeToNUIJointTypeMapping(jt);
Vector3 position = new Vector3(currentBody.Joints[jt].Position.X, currentBody.Joints[jt].Position.Y, currentBody.Joints[jt].Position.Z);
position.z *= -1; // Should probably be done with meta data/mapper to avoid disturbing raw data.
Quaternion orientation = new Quaternion(currentBody.JointOrientations[jt].Orientation.X, currentBody.JointOrientations[jt].Orientation.Y, currentBody.JointOrientations[jt].Orientation.Z, currentBody.JointOrientations[jt].Orientation.W);
frameData.AddJoint(jointType, position, orientation, (CinemaSuite.CinemaMocap.System.Core.TrackingState)currentBody.Joints[jt].TrackingState);
}
// Hand info
frameData.LeftHandConfidence = (CinemaSuite.CinemaMocap.System.Core.TrackingConfidence)currentBody.HandLeftConfidence;
frameData.LeftHandState = (CinemaSuite.CinemaMocap.System.Core.HandState)currentBody.HandLeftState;
frameData.RightHandConfidence = (CinemaSuite.CinemaMocap.System.Core.TrackingConfidence)currentBody.HandRightConfidence;
frameData.RightHandState = (CinemaSuite.CinemaMocap.System.Core.HandState)currentBody.HandRightState;
// Frame info
frameData.ClippedEdges = (CinemaSuite.CinemaMocap.System.Core.FrameEdges)currentBody.ClippedEdges;
FrameDataEventArgs args = new FrameDataEventArgs(frameData);
OnFrameCaptured(args);
}
}
public override void Update()
{
if (ZigEditorInput.Instance.ReaderInited)
{
// Update Device
ZigEditorInput.Instance.Update();
// Get the tracked user
ZigTrackedUser user = null;
int userId = 0;
foreach (KeyValuePair <int, ZigTrackedUser> trackedUser in ZigEditorInput.Instance.TrackedUsers)
{
user = trackedUser.Value;
userId = trackedUser.Key;
}
// Update viewers
foreach (Kinect1EditorViewer viewer in viewers)
{
viewer.Update(ZigEditorInput.Instance, userId);
}
if (user != null && user.SkeletonTracked)
{
// Encapsulate the important frame data
SkeletonFrameData frameData = new SkeletonFrameData();
frameData.TrackingId = (ulong)user.Id;
frameData.IsTracked = user.SkeletonTracked;
foreach (ZigInputJoint inputJoint in user.Skeleton)
{
NUIJointType jointType = NUICaptureHelper.ZigToNUIJointMapping(inputJoint.Id);
// Convert position from mm to meters
Vector3 position = inputJoint.Position / 1000f;
Quaternion orientation = inputJoint.Rotation;
frameData.AddJoint(jointType, position, orientation, inputJoint.Inferred ? TrackingState.Inferred : TrackingState.Tracked);
}
FrameDataEventArgs args = new FrameDataEventArgs(frameData);
OnFrameCaptured(args);
}
}
}
public static NUISkeleton GetNUISkeleton(ZigInputJoint[] Skeleton)
{
NUISkeleton nuiSkeleton = new NUISkeleton();
foreach (ZigInputJoint inputJoint in Skeleton)
{
NUIJointType jointType = ZigToNUIJointMapping(inputJoint.Id);
// Convert position from mm to meters
Vector3 position = inputJoint.Position / 1000f;
NUIJoint joint = new NUIJoint(jointType, position, inputJoint.Rotation, inputJoint.Inferred);
if (!nuiSkeleton.Joints.ContainsKey(jointType))
{
nuiSkeleton.Joints.Add(jointType, joint);
}
}
return(nuiSkeleton);
}
public static NUISkeleton GetNUISkeleton(SkeletonFrameData skeleton)
{
var nuiSkeleton = new NUISkeleton();
for (NUIJointType jt = NUIJointType.SpineBase; jt <= NUIJointType.ThumbRight; jt++)
{
Vector3 position = new Vector3();
Quaternion orientation = new Quaternion();
var trackingState = CinemaSuite.CinemaMocap.System.Core.TrackingState.NotTracked;
skeleton.GetJointData(jt, out position, out orientation, out trackingState);
NUIJoint joint = new NUIJoint(jt, position, orientation, trackingState != CinemaSuite.CinemaMocap.System.Core.TrackingState.Tracked);
if (!nuiSkeleton.Joints.ContainsKey(jt))
{
nuiSkeleton.Joints.Add(jt, joint);
}
}
return(nuiSkeleton);
}
/// <summary>
/// Add a Bone to the skeleton structure. Structure must be built incrementally.
/// </summary>
/// <param name="child">The child joint.</param>
/// <param name="parent">The parent joint.</param>
/// <returns>true if added successfully.</returns>
public bool AddBone(NUIJointType child, NUIJointType parent)
{
if (structure.Count == 0)
{
SetRootJoint(parent);
}
if (!structure.ContainsKey(parent))
{
return(false);
}
// Add to the structure
structure.Add(child, parent);
// Update the extremity lookup table
extremity.Add(child, true);
bool parentHasChild = !extremity[parent];
extremity[parent] = false;
// Update the multiChildren lookup table
multiChildren.Add(child, false);
if (parentHasChild)
{
multiChildren[parent] = true;
childLookup[parent] = NUIJointType.Unspecified;
}
else
{
childLookup.Add(parent, child);
}
return(true);
}
public void GetJointData(NUIJointType jt, out Vector3 position, out Quaternion orientation, out TrackingState trackingState)
{
position = new Vector3();
orientation = new Quaternion();
trackingState = TrackingState.NotTracked;
int index = -1;
for (int i = 0; i < jointTypes.Count; i++)
{
if (jointTypes[i] == jt)
{
index = i;
break;
}
}
if (index >= 0)
{
position = jointPositions[index];
orientation = jointOrientations[index];
trackingState = jointTracking[index];
}
}
public static NUISkeleton GetNUISkeleton(Body body)
{
var nuiSkeleton = new NUISkeleton();
for (JointType jt = JointType.SpineBase; jt <= JointType.ThumbRight; jt++)
{
NUIJointType jointType = JointTypeToNUIJointTypeMapping(jt);
Vector3 position = new Vector3(body.Joints[jt].Position.X, body.Joints[jt].Position.Y, body.Joints[jt].Position.Z);
// Reverse the Z
position.z *= -1;
Quaternion orientation = new Quaternion(body.JointOrientations[jt].Orientation.X, body.JointOrientations[jt].Orientation.Y, body.JointOrientations[jt].Orientation.Z, body.JointOrientations[jt].Orientation.W);
NUIJoint joint = new NUIJoint(jointType, position, orientation, body.Joints[jt].TrackingState != Windows.Kinect.TrackingState.Tracked);
if (!nuiSkeleton.Joints.ContainsKey(jointType))
{
nuiSkeleton.Joints.Add(jointType, joint);
}
}
return(nuiSkeleton);
}
protected Vector3 FilterJoint(NUISkeleton skeleton, NUIJointType jt, float smoothingValue, float correctionValue,
float predictionValue, float jitterRadiusValue, float maxDeviationRadiusValue)
{
int jointIndex = (int)jt;
Vector3 filteredPosition;
Vector3 diffvec;
Vector3 trend;
float diffVal;
Vector3 rawPosition = skeleton.Joints[jt].Position;
Vector3 prevFilteredPosition = this.history[jointIndex].FilteredPosition;
Vector3 prevTrend = this.history[jointIndex].Trend;
Vector3 prevRawPosition = this.history[jointIndex].RawPosition;
bool jointIsValid = rawPosition.x != 0.0f || rawPosition.y != 0.0f || rawPosition.z != 0.0f;
if (!jointIsValid)
{
this.history[jointIndex].FrameCount = 0;
}
// Initial start values
if (this.history[jointIndex].FrameCount == 0)
{
filteredPosition = rawPosition;
trend = Vector3.zero;
}
else if (this.history[jointIndex].FrameCount == 1)
{
filteredPosition = (rawPosition + prevRawPosition) * 0.5f;
diffvec = (filteredPosition - prevFilteredPosition);
trend = (diffvec * correctionValue) + (prevTrend * (1.0f - correctionValue));
}
else
{
// First apply jitter filter
diffvec = (rawPosition - prevFilteredPosition);
diffVal = Math.Abs(diffvec.magnitude);
if (diffVal <= jitterRadiusValue)
{
filteredPosition = rawPosition * (diffVal / jitterRadiusValue) + (prevFilteredPosition * (1.0f - (diffVal / jitterRadiusValue)));
}
else
{
filteredPosition = rawPosition;
}
filteredPosition = (filteredPosition * (1.0f - smoothingValue)) + ((prevFilteredPosition + prevTrend) * smoothingValue);
diffvec = (filteredPosition - prevFilteredPosition);
trend = ((diffvec * correctionValue) + (prevTrend * (1.0f - correctionValue)));
}
Vector3 predictedPosition = (filteredPosition + (trend * predictionValue));
diffvec = (predictedPosition - rawPosition);
diffVal = Math.Abs(diffvec.magnitude);
if (diffVal > maxDeviationRadiusValue)
{
predictedPosition = ((predictedPosition * (maxDeviationRadiusValue / diffVal)) + (rawPosition * (1.0f - (maxDeviationRadiusValue / diffVal))));
}
this.history[jointIndex].RawPosition = rawPosition;
this.history[jointIndex].FilteredPosition = filteredPosition;
this.history[jointIndex].Trend = trend;
this.history[jointIndex].FrameCount++;
return(predictedPosition);
}
/// <summary>
/// Is a joint in this specific skeletal structure?
/// </summary>
/// <param name="joint">The joint to check.</param>
/// <returns>Return true if the joint is in the structure.</returns>
public bool IsJointInStructure(NUIJointType joint)
{
return(structure.ContainsKey(joint) || structure.ContainsValue(joint));
}
/// <summary>
/// Is this joint an extremity in the skeletal structure?
/// </summary>
/// <param name="joint">The joint to check for.</param>
/// <returns>true if the joint is an extremity in this structure.</returns>
public bool IsJointAnExtremity(NUIJointType joint)
{
return(extremity[joint]);
}
protected bool isJointMasked(NUIJointType jointType)
{
var result = false;
if (jointType == NUIJointType.AnkleLeft || jointType == NUIJointType.AnkleRight)
{
if ((Mask & SkeletonMask.Ankles) == 0)
{
result = true;
}
}
else if (jointType == NUIJointType.KneeLeft || jointType == NUIJointType.KneeRight)
{
if ((Mask & SkeletonMask.Knees) == 0)
{
result = true;
}
}
else if (jointType == NUIJointType.HipLeft || jointType == NUIJointType.HipRight)
{
if ((Mask & SkeletonMask.Hips) == 0)
{
result = true;
}
}
else if (jointType == NUIJointType.ShoulderRight || jointType == NUIJointType.ShoulderLeft)
{
if ((Mask & SkeletonMask.Shoulders) == 0)
{
result = true;
}
}
else if (jointType == NUIJointType.ElbowLeft || jointType == NUIJointType.ElbowRight)
{
if ((Mask & SkeletonMask.Elbows) == 0)
{
result = true;
}
}
else if (jointType == NUIJointType.WristLeft || jointType == NUIJointType.WristRight)
{
if ((Mask & SkeletonMask.Wrists) == 0)
{
result = true;
}
}
else if (jointType == NUIJointType.HandLeft || jointType == NUIJointType.HandRight)
{
if ((Mask & SkeletonMask.Hands) == 0)
{
result = true;
}
}
else if (jointType == NUIJointType.SpineMid || jointType == NUIJointType.SpineShoulder)
{
if ((Mask & SkeletonMask.Spine) == 0)
{
result = true;
}
}
else if (jointType == NUIJointType.Neck)
{
if ((Mask & SkeletonMask.Neck) == 0)
{
result = true;
}
}
return(result);
}
/// <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);
}
public static string NUIJointToColladaMapping(NUIJointType nuiJointId)
{
return(BaseSystem.Enum.GetName(typeof(NUIJointType), nuiJointId));
}
public NUIJointType GetChildJoint(NUIJointType parentType)
{
return(childLookup[parentType]);
}
public NUIJoint(NUIJointType jointType)
{
this.jointType = jointType;
}
/// <summary>
/// Is a joint a parent to more than one other joint?
/// </summary>
/// <param name="joint">The joint to check for.</param>
/// <returns>returns true if the joint is parent to more than one joint in this skeletal structure.</returns>
public bool IsJointParentToMany(NUIJointType joint)
{
return(multiChildren[joint]);
}
public NUIJointType GetParentJoint(NUIJointType childType)
{
return(structure[childType]);
}
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;
}
}
public static string NUIJointToColladaMapping(NUIJointType nuiJointId)
{
switch (nuiJointId)
{
case NUIJointType.SpineBase:
return("HIP");
case NUIJointType.SpineMid:
return("SPINE");
case NUIJointType.Neck:
return("SHOULDER_CENTER");
case NUIJointType.Head:
return("HEAD");
case NUIJointType.HipRight:
return("HIP_RIGHT");
case NUIJointType.KneeRight:
return("KNEE_RIGHT");
case NUIJointType.AnkleRight:
return("ANKLE_RIGHT");
case NUIJointType.FootRight:
return("FOOT_RIGHT");
case NUIJointType.HipLeft:
return("HIP_LEFT");
case NUIJointType.KneeLeft:
return("KNEE_LEFT");
case NUIJointType.AnkleLeft:
return("ANKLE_LEFT");
case NUIJointType.FootLeft:
return("FOOT_LEFT");
case NUIJointType.ShoulderRight:
return("SHOULDER_RIGHT");
case NUIJointType.ElbowRight:
return("ELBOW_RIGHT");
case NUIJointType.WristRight:
return("WRIST_RIGHT");
case NUIJointType.HandRight:
return("HAND_RIGHT");
case NUIJointType.ShoulderLeft:
return("SHOULDER_LEFT");
case NUIJointType.ElbowLeft:
return("ELBOW_LEFT");
case NUIJointType.WristLeft:
return("WRIST_LEFT");
case NUIJointType.HandLeft:
return("HAND_LEFT");
default:
return("HIP");
}
}
