void ProcessJointInformation(int joint, Joint j, BoneOrientation bo, double time, UdpWriter osc, StreamWriter fileWriter, int pointScale)
{
SendJointMessage(joint,
j.Position.X, j.Position.Y, j.Position.Z,
JointToConfidenceValue(j), time,
osc, fileWriter, pointScale);
}
/// <summary>
/// Returns a List of Skeletons which are currently looking at the Kinect.
/// </summary>
public List <Skeleton> GetLookingPeople()
{
List <Skeleton> lookingPeople = new List <Skeleton>();
foreach (List <SkeletonTimestamp> skeletonList in _skeletonsDict.Values)
{
// newest Skeleton
Skeleton skeleton = skeletonList[0].Skeleton;
if (skeleton.TrackingState != SkeletonTrackingState.Tracked)
{
continue;
}
Joint head = skeleton.Joints[JointType.Head];
if (head.TrackingState == JointTrackingState.NotTracked)
{
continue;
}
BoneOrientation headOrientation = skeleton.BoneOrientations[JointType.Head];
// Defines a four-dimensional vector (x,y,z,w), which is used to efficiently rotate an
// object about the (x, y, z) vector by the angle theta, where w = cos(theta/2).
var rot = headOrientation.AbsoluteRotation.Quaternion;
double angle = Math.Acos(rot.W) * 2 * 180 / Math.PI;
//Console.WriteLine("[{0} {1} {2} {3}", rot.X, rot.Y, rot.Z, angle);
// I don't care about the vector, for now.
if (Math.Abs(180 - angle) < 30)
{
lookingPeople.Add(skeleton);
}
}
return(lookingPeople);
}
public static ReplayBoneOrientation FromBoneOrientation(BoneOrientation boneOrientation)
{
return new ReplayBoneOrientation()
{
AbsoluteRotation = ReplayBoneRotation.FromBoneRotation(boneOrientation.AbsoluteRotation),
HierarchicalRotation = ReplayBoneRotation.FromBoneRotation(boneOrientation.HierarchicalRotation),
StartJoint = boneOrientation.StartJoint,
EndJoint = boneOrientation.EndJoint
};
}
private double GetOrientation(Skeleton user, JointType type)
{
BoneOrientation bol = user.BoneOrientations[type];
float hx = bol.HierarchicalRotation.Quaternion.X;
float hy = bol.HierarchicalRotation.Quaternion.Y;
double r = Math.Sqrt(hx * hx + hy * hy);
double rad = Math.Asin(hx / r);
return(RadianToDegree(rad));
}
public KinectJoint(Joint joint, BoneOrientation boneOrientation)
{
Type = joint.JointType;
TrackState = joint.TrackingState;
PositionSkeleton = joint.Position;
Orientation = boneOrientation;
SetUpMatrices();
Position2D = KinectMath.SkeletonPointToScreen(joint.Position);
Position3D = KinectMath.SkeletonPointToVector3(joint.Position);
HitBox = new Rect(new Point(Position2D.X - (BOUNDS / 2), Position2D.Y - (BOUNDS / 2)), new Size(BOUNDS, BOUNDS));
}
/// <summary>
/// Sets the orientation of this bone relative to it's starting position
/// </summary>
/// <param name="newOrientation"></param>
public void setOrientation(BoneOrientation newOrientation)
{
//R = X (front/back)
//G = Y (left/right)
//B = Z (up/down)
mOrientation = newOrientation;
mStartPoint = Vector3.Zero;
switch (newOrientation)
{
case BoneOrientation.Up:
mEndPoint.Z = mStartPoint.Z + mLength;
mDrawTransform = Skeleton.ORIENT_UP;
break;
case BoneOrientation.Down:
mDrawTransform = Skeleton.ORIENT_DOWN;
mEndPoint.Z = mStartPoint.Z - mLength;
break;
case BoneOrientation.Left:
mEndPoint.Y = mStartPoint.Y - mLength;
mDrawTransform = Skeleton.ORIENT_RIGHT;
break;
case BoneOrientation.Right:
mEndPoint.Y = mStartPoint.Y + mLength;
mDrawTransform = Skeleton.ORIENT_LEFT;
break;
case BoneOrientation.Front:
mEndPoint.X = mStartPoint.X - mLength;
mDrawTransform = Skeleton.ORIENT_FRONT;
break;
case BoneOrientation.Back:
mEndPoint.X = mStartPoint.X + mLength;
mDrawTransform = Skeleton.ORIENT_FRONT; //TODO FIX
break;
default:
break;
}
/** This is to update the vertex positions of the box geometry */
for (int i = 0; i < mBoxVerts.Count(); i++)
{
mBoxVerts[i] = Vector3.Transform(mInitialBoxVerts[i], mDrawTransform);
}
}
private void DrawBone(Skeleton skeleton, BoneOrientation bone, string timestamp)
{
Joint startJoint = skeleton.Joints[bone.StartJoint];
Joint endJoint = skeleton.Joints[bone.EndJoint];
Brush drawBrush = null;
int coordinateType = 0;
if (startJoint.TrackingState == JointTrackingState.NotTracked || endJoint.TrackingState == JointTrackingState.NotTracked)
{
return;
}
else if (startJoint.TrackingState == JointTrackingState.Tracked && endJoint.TrackingState == JointTrackingState.Tracked)
{
drawBrush = TrackedBoneBrush;
coordinateType = TrackedCoordinate;
}
else if (startJoint.TrackingState == JointTrackingState.Inferred && endJoint.TrackingState == JointTrackingState.Inferred)
{
drawBrush = InferredBoneBrush;
coordinateType = InferredCoordinate;
}
else
{
drawBrush = HalfInferredBoneBrush;
coordinateType = HalfInferredCoordinate;
}
ColorImagePoint startJointColorPoint = _kinect.CoordinateMapper.MapSkeletonPointToColorPoint(startJoint.Position, ColorStreamFormat);
ColorImagePoint endJointColorPoint = _kinect.CoordinateMapper.MapSkeletonPointToColorPoint(endJoint.Position, ColorStreamFormat);
var line = new Line
{
Stroke = drawBrush,
StrokeThickness = BoneThickness,
X1 = startJointColorPoint.X,
Y1 = startJointColorPoint.Y,
X2 = endJointColorPoint.X,
Y2 = endJointColorPoint.Y
};
canvas.Children.Add(line);
if (_isRecording)
{
SaveBoneData(bone, timestamp, coordinateType);
}
}
/// <summary>
/// Finds the angle between the provided joints.
/// </summary>
/// <param name="skeletonStamp"></param>
/// <param name="vertexJoint"></param>
/// <param name="adjacentJoints"></param>
/// <returns></returns>
private int FindAngle(SkeletonStamp skeletonStamp, out Joint vertexJoint, out Joint[] adjacentJoints)
{
// Test bone orientations
Skeleton skeleton = skeletonStamp.GetTrackedSkeleton();
BoneOrientation bo = skeleton.BoneOrientations[Vertex.GetJointType()];
// get the vertex and other joints off the skeleton stamp
vertexJoint = skeletonStamp.GetTrackedSkeleton().Joints[Vertex.GetJointType()];
adjacentJoints = new Joint[2];
adjacentJoints[0] = skeletonStamp.GetTrackedSkeleton().Joints[OtherJoints[0].GetJointType()];
adjacentJoints[1] = skeletonStamp.GetTrackedSkeleton().Joints[OtherJoints[1].GetJointType()];
int convertedDotAngle = JointAnalyzer.FindAngle(vertexJoint, adjacentJoints);
return(convertedDotAngle);
}
private void SaveBoneData(BoneOrientation bone, string timestamp, int coordinateType)
{
foreach (var bonePosition in _boneHistory)
{
if (bone.StartJoint == bonePosition.StartJoint && bone.EndJoint == bonePosition.EndJoint)
{
bonePosition.Timestamp.Add(timestamp);
bonePosition.AbsMatrix.Add(bone.AbsoluteRotation.Matrix);
bonePosition.AbsQuaternion.Add(bone.AbsoluteRotation.Quaternion);
bonePosition.HierMatrix.Add(bone.HierarchicalRotation.Matrix);
bonePosition.HierQuaternion.Add(bone.HierarchicalRotation.Quaternion);
bonePosition.CoordinateType.Add(coordinateType);
break;
}
}
}
/// <summary>
/// CopySkeleton copies the data from another skeleton.
/// </summary>
/// <param name="source">The source skeleton.</param>
/// <param name="destination">The destination skeleton.</param>
public static void CopySkeleton(Skeleton source, Skeleton destination)
{
if (null == source)
{
return;
}
if (null == destination)
{
destination = new Skeleton();
}
destination.TrackingState = source.TrackingState;
destination.TrackingId = source.TrackingId;
destination.Position = source.Position;
destination.ClippedEdges = source.ClippedEdges;
Array jointTypeValues = Enum.GetValues(typeof(JointType));
// This must copy before the joint orientations
foreach (JointType j in jointTypeValues)
{
Joint temp = destination.Joints[j];
temp.Position = source.Joints[j].Position;
temp.TrackingState = source.Joints[j].TrackingState;
destination.Joints[j] = temp;
}
if (null != source.BoneOrientations)
{
foreach (JointType j in jointTypeValues)
{
BoneOrientation temp = destination.BoneOrientations[j];
temp.HierarchicalRotation.Matrix = source.BoneOrientations[j].HierarchicalRotation.Matrix;
temp.HierarchicalRotation.Quaternion = source.BoneOrientations[j].HierarchicalRotation.Quaternion;
temp.AbsoluteRotation.Matrix = source.BoneOrientations[j].AbsoluteRotation.Matrix;
temp.AbsoluteRotation.Quaternion = source.BoneOrientations[j].AbsoluteRotation.Quaternion;
destination.BoneOrientations[j] = temp;
}
}
}
/// <summary>
/// Sets the orientation of this bone relative to it's starting position
/// </summary>
/// <param name="newOrientation"></param>
public void setOrientation( BoneOrientation newOrientation )
{
//R = X (front/back)
//G = Y (left/right)
//B = Z (up/down)
mOrientation = newOrientation;
mStartPoint = Vector3.Zero;
switch( newOrientation )
{
case BoneOrientation.Up:
mEndPoint.Z = mStartPoint.Z + mLength;
mDrawTransform = Skeleton.ORIENT_UP;
break;
case BoneOrientation.Down:
mDrawTransform = Skeleton.ORIENT_DOWN;
mEndPoint.Z = mStartPoint.Z - mLength;
break;
case BoneOrientation.Left:
mEndPoint.Y = mStartPoint.Y - mLength;
mDrawTransform = Skeleton.ORIENT_RIGHT;
break;
case BoneOrientation.Right:
mEndPoint.Y = mStartPoint.Y + mLength;
mDrawTransform = Skeleton.ORIENT_LEFT;
break;
case BoneOrientation.Front:
mEndPoint.X = mStartPoint.X - mLength;
mDrawTransform = Skeleton.ORIENT_FRONT;
break;
case BoneOrientation.Back:
mEndPoint.X = mStartPoint.X + mLength;
mDrawTransform = Skeleton.ORIENT_FRONT; //TODO FIX
break;
default:
break;
}
/** This is to update the vertex positions of the box geometry */
for( int i = 0; i < mBoxVerts.Count(); i++ )
{
mBoxVerts[ i ] = Vector3.Transform( mInitialBoxVerts[ i ], mDrawTransform );
}
}
/// <summary>
/// Etablit la transformation des os dans l'armature 3D de l'avatar
/// </summary>
/// <param name="bone">Nui Joint/bone orientation</param>
/// <param name="skeleton">The Kinect skeleton.</param>
/// <param name="bindRoot">The bind root matrix of the avatar mesh.</param>
/// <param name="boneTransforms">The avatar mesh rotation matrices.</param>
private void SetJointTransformation(BoneOrientation bone, Skeleton skeleton, Matrix bindRoot, ref Matrix[] boneTransforms)
{
// Toujours regarder la racine du squelette
if (bone.StartJoint == JointType.HipCenter && bone.EndJoint == JointType.HipCenter)
{
// Sauf si c'est le mode assis, le centre des hanches est spécial - c'est la racine du NuiSkeleton et ça décrit l'orientation du squelette dans le monde
// (camera) système de coordonnées. Toutes les autres orientations des os/armatures dans la hierarchie ont le centre des hanches comme un de leurs parents.
// Cependant, si on est en mode assis, c'est le centre des épaules qui sert de repère à l'orientation du squelette dans le système de coordonnées de la caméra.
bindRoot.Translation = Vector3.Zero;
Matrix invBindRoot = Matrix.Invert(bindRoot);
//Pour ne pas avoir une déformation étrage du modèle 3D, matrice de rotation autour de l'axe Z
Matrix zaxisflip = Matrix.CreateRotationZ(MathHelper.ToRadians(-90));
invBindRoot *= zaxisflip;
Matrix hipOrientation = KinectHelper.Matrix4ToXNAMatrix(bone.HierarchicalRotation.Matrix);
// Here we create a rotation matrix for the hips from the inverse of the bind pose
// for the pelvis rotation and the inverse of the bind pose for the root node (0) in the "Vêtements" model.
// This multiplication effectively removes the initial 90 degree rotations set in the first two model nodes.
Matrix pelvis = boneTransforms[1];
pelvis.Translation = Vector3.Zero; // Ensure pure rotation as we explicitly set world translation from the Kinect camera below.
Matrix invPelvis = Matrix.Invert(pelvis);
Matrix combined = (invBindRoot * hipOrientation) * invPelvis;
this.ReplaceBoneMatrix(JointType.HipCenter, combined, true, ref boneTransforms);
}
else if (bone.EndJoint == JointType.ShoulderCenter)
{
// This contains an absolute rotation if we are in seated mode, or the hip center is not tracked, as the HipCenter will be identity
if (this.chooser.SeatedMode || (this.Chooser.SeatedMode == false && skeleton.Joints[JointType.HipCenter].TrackingState == JointTrackingState.NotTracked))
{
bindRoot.Translation = Vector3.Zero;
Matrix invBindRoot = Matrix.Invert(bindRoot);
Matrix hipOrientation = KinectHelper.Matrix4ToXNAMatrix(bone.HierarchicalRotation.Matrix);
// We can use the same method as in HipCenter above to invert the root and pelvis bind pose,
// however, alternately we can also explicitly swap axes and adjust the rotations to get from
// the Kinect rotation to the model hip orientation, similar to what we do for the following joints/bones.
// Kinect = +X left, +Y up, +Z forward in body coordinate system
// Avatar = +Z left, +X up, +Y forward
Quaternion kinectRotation = KinectHelper.DecomposeMatRot(hipOrientation); // XYZ
Quaternion avatarRotation = new Quaternion(kinectRotation.Y, kinectRotation.Z, kinectRotation.X, kinectRotation.W); // transform from Kinect to avatar coordinate system
Matrix combined = Matrix.CreateFromQuaternion(avatarRotation);
// Add a small adjustment rotation to manually correct for the rotation in the parent bind
// pose node in the model mesh - this can be found by looking in the FBX or in 3DSMax/Maya.
Matrix adjustment = Matrix.CreateRotationY(MathHelper.ToRadians(-90));
combined *= adjustment;
Matrix adjustment2 = Matrix.CreateRotationZ(MathHelper.ToRadians(-90));
combined *= adjustment2;
// Although not strictly correct, we apply this to the hip center, as all other bones are children of this joint.
// Application at the spine or shoulder center instead would require manually updating of the bone orientations below for the whole body to move when the shoulders twist or tilt.
this.ReplaceBoneMatrix(JointType.HipCenter, combined, true, ref boneTransforms);
}
}
else if (bone.EndJoint == JointType.Spine)
{
Matrix tempMat = KinectHelper.Matrix4ToXNAMatrix(bone.HierarchicalRotation.Matrix);
// The Dude appears to lean back too far compared to a real person, so here we adjust this lean.
this.CorrectBackwardsLean(skeleton, ref tempMat);
// Also add a small constant adjustment rotation to correct for the hip center to spine bone being at a rear-tilted angle in the Kinect skeleton.
// The dude should now look more straight ahead when avateering
Matrix adjustment = Matrix.CreateRotationX(MathHelper.ToRadians(20)); // 20 degree rotation around the local Kinect x axis for the spine bone.
tempMat *= adjustment;
// Kinect = +X left, +Y up, +Z forward in body coordinate system
// Avatar = +Z left, +X up, +Y forward
Quaternion kinectRotation = KinectHelper.DecomposeMatRot(tempMat); // XYZ
Quaternion avatarRotation = new Quaternion(kinectRotation.Y, kinectRotation.Z, kinectRotation.X, kinectRotation.W); // transform from Kinect to avatar coordinate system
tempMat = Matrix.CreateFromQuaternion(avatarRotation);
// Set the corresponding matrix in the avatar using the translation table we specified.
// Note for the spine and shoulder center rotations, we could also try to spread the angle
// over all the Avatar skeleton spine joints, causing a more curved back, rather than apply
// it all to one joint, as we do here.
this.ReplaceBoneMatrix(bone.EndJoint, tempMat, false, ref boneTransforms);
}
else if (bone.EndJoint == JointType.Head)
{
Matrix tempMat = KinectHelper.Matrix4ToXNAMatrix(bone.HierarchicalRotation.Matrix);
// Add a small adjustment rotation to correct for the avatar skeleton head bones being defined pointing looking slightly down, not vertical.
// The dude should now look more straight ahead when avateering
Matrix adjustment = Matrix.CreateRotationX(MathHelper.ToRadians(-30)); // -30 degree rotation around the local Kinect x axis for the head bone.
tempMat *= adjustment;
// Kinect = +X left, +Y up, +Z forward in body coordinate system
// Avatar = +Z left, +X up, +Y forward
Quaternion kinectRotation = KinectHelper.DecomposeMatRot(tempMat); // XYZ
Quaternion avatarRotation = new Quaternion(kinectRotation.Y, kinectRotation.Z, kinectRotation.X, kinectRotation.W); // transform from Kinect to avatar coordinate system
tempMat = Matrix.CreateFromQuaternion(avatarRotation);
// Set the corresponding matrix in the avatar using the translation table we specified
this.ReplaceBoneMatrix(bone.EndJoint, tempMat, false, ref boneTransforms);
}
else if (bone.EndJoint == JointType.ElbowLeft || bone.EndJoint == JointType.WristLeft)
{
Matrix tempMat = KinectHelper.Matrix4ToXNAMatrix(bone.HierarchicalRotation.Matrix);
if (bone.EndJoint == JointType.ElbowLeft)
{
// Add a small adjustment rotation to correct for the avatar skeleton shoulder/upper arm bones.
// The dude should now be able to have arms correctly down at his sides when avateering
Matrix adjustment = Matrix.CreateRotationZ(MathHelper.ToRadians(-15)); // -15 degree rotation around the local Kinect z axis for the upper arm bone.
tempMat *= adjustment;
}
// Kinect = +Y along arm, +X down, +Z forward in body coordinate system
// Avatar = +X along arm, +Y down, +Z backwards
Quaternion kinectRotation = KinectHelper.DecomposeMatRot(tempMat); // XYZ
Quaternion avatarRotation = new Quaternion(kinectRotation.Y, -kinectRotation.Z, -kinectRotation.X, kinectRotation.W); // transform from Kinect to avatar coordinate system
tempMat = Matrix.CreateFromQuaternion(avatarRotation);
this.ReplaceBoneMatrix(bone.EndJoint, tempMat, false, ref boneTransforms);
}
else if (bone.EndJoint == JointType.HandLeft)
{
Matrix tempMat = KinectHelper.Matrix4ToXNAMatrix(bone.HierarchicalRotation.Matrix);
// Add a small adjustment rotation to correct for the avatar skeleton wist/hand bone.
// The dude should now have the palm of his hands toward his body when arms are straight down
Matrix adjustment = Matrix.CreateRotationY(MathHelper.ToRadians(-90)); // -90 degree rotation around the local Kinect y axis for the wrist-hand bone.
tempMat *= adjustment;
// Kinect = +Y along arm, +X down, +Z forward in body coordinate system
// Avatar = +X along arm, +Y down, +Z backwards
Quaternion kinectRotation = KinectHelper.DecomposeMatRot(tempMat); // XYZ
Quaternion avatarRotation = new Quaternion(kinectRotation.Y, kinectRotation.X, -kinectRotation.Z, kinectRotation.W);
tempMat = Matrix.CreateFromQuaternion(avatarRotation);
this.ReplaceBoneMatrix(bone.EndJoint, tempMat, false, ref boneTransforms);
}
else if (bone.EndJoint == JointType.ElbowRight || bone.EndJoint == JointType.WristRight)
{
Matrix tempMat = KinectHelper.Matrix4ToXNAMatrix(bone.HierarchicalRotation.Matrix);
if (bone.EndJoint == JointType.ElbowRight)
{
// Add a small adjustment rotation to correct for the avatar skeleton shoulder/upper arm bones.
// The dude should now be able to have arms correctly down at his sides when avateering
Matrix adjustment = Matrix.CreateRotationZ(MathHelper.ToRadians(15)); // 15 degree rotation around the local Kinect z axis for the upper arm bone.
tempMat *= adjustment;
}
// Kinect = +Y along arm, +X up, +Z forward in body coordinate system
// Avatar = +X along arm, +Y back, +Z down
Quaternion kinectRotation = KinectHelper.DecomposeMatRot(tempMat); // XYZ
Quaternion avatarRotation = new Quaternion(kinectRotation.Y, -kinectRotation.Z, -kinectRotation.X, kinectRotation.W); // transform from Kinect to avatar coordinate system
tempMat = Matrix.CreateFromQuaternion(avatarRotation);
this.ReplaceBoneMatrix(bone.EndJoint, tempMat, false, ref boneTransforms);
}
else if (bone.EndJoint == JointType.HandRight)
{
Matrix tempMat = KinectHelper.Matrix4ToXNAMatrix(bone.HierarchicalRotation.Matrix);
// Add a small adjustment rotation to correct for the avatar skeleton wist/hand bone.
// The dude should now have the palm of his hands toward his body when arms are straight down
Matrix adjustment = Matrix.CreateRotationY(MathHelper.ToRadians(90)); // -90 degree rotation around the local Kinect y axis for the wrist-hand bone.
tempMat *= adjustment;
// Kinect = +Y along arm, +X up, +Z forward in body coordinate system
// Avatar = +X along arm, +Y down, +Z forwards
Quaternion kinectRotation = KinectHelper.DecomposeMatRot(tempMat); // XYZ
Quaternion avatarRotation = new Quaternion(kinectRotation.Y, -kinectRotation.X, kinectRotation.Z, kinectRotation.W); // transform from Kinect to avatar coordinate system
tempMat = Matrix.CreateFromQuaternion(avatarRotation);
this.ReplaceBoneMatrix(bone.EndJoint, tempMat, false, ref boneTransforms);
}
else if (bone.EndJoint == JointType.KneeLeft)
{
// Combine the two joint rotations from the hip and knee
Matrix hipLeft = KinectHelper.Matrix4ToXNAMatrix(skeleton.BoneOrientations[JointType.HipLeft].HierarchicalRotation.Matrix);
Matrix kneeLeft = KinectHelper.Matrix4ToXNAMatrix(bone.HierarchicalRotation.Matrix);
Matrix combined = kneeLeft * hipLeft;
this.SetLegMatrix(bone.EndJoint, combined, ref boneTransforms);
}
else if (bone.EndJoint == JointType.AnkleLeft || bone.EndJoint == JointType.AnkleRight)
{
Matrix tempMat = KinectHelper.Matrix4ToXNAMatrix(bone.HierarchicalRotation.Matrix);
this.SetLegMatrix(bone.EndJoint, tempMat, ref boneTransforms);
}
else if (bone.EndJoint == JointType.KneeRight)
{
// Combine the two joint rotations from the hip and knee
Matrix hipRight = KinectHelper.Matrix4ToXNAMatrix(skeleton.BoneOrientations[JointType.HipRight].HierarchicalRotation.Matrix);
Matrix kneeRight = KinectHelper.Matrix4ToXNAMatrix(bone.HierarchicalRotation.Matrix);
Matrix combined = kneeRight * hipRight;
this.SetLegMatrix(bone.EndJoint, combined, ref boneTransforms);
}
else if (bone.EndJoint == JointType.FootLeft || bone.EndJoint == JointType.FootRight)
{
// Only set this if we actually have a good track on this and the parent
if (skeleton.Joints[bone.EndJoint].TrackingState == JointTrackingState.Tracked && skeleton.Joints[skeleton.BoneOrientations[bone.EndJoint].StartJoint].TrackingState == JointTrackingState.Tracked)
{
Matrix tempMat = KinectHelper.Matrix4ToXNAMatrix(bone.HierarchicalRotation.Matrix);
// Add a small adjustment rotation to correct for the avatar skeleton foot bones being defined pointing down at 45 degrees, not horizontal
Matrix adjustment = Matrix.CreateRotationX(MathHelper.ToRadians(-45));
tempMat *= adjustment;
// Kinect = +Y along foot (fwd), +Z up, +X right in body coordinate system
// Avatar = +X along foot (fwd), +Y up, +Z right
Quaternion kinectRotation = KinectHelper.DecomposeMatRot(tempMat); // XYZ
Quaternion avatarRotation = new Quaternion(kinectRotation.Y, kinectRotation.Z, kinectRotation.X, kinectRotation.W); // transform from Kinect to avatar coordinate system
tempMat = Matrix.CreateFromQuaternion(avatarRotation);
this.ReplaceBoneMatrix(bone.EndJoint, tempMat, false, ref boneTransforms);
}
}
}
private void DrawJointRight(SpriteBatch spriteBatch, Joint joint, BoneOrientation bo, Vector2 resolution, Texture2D img, int offsetX, int offsetY, int angle)
{
p = (double)bo.AbsoluteRotation.Quaternion.Y;
double theta = Math.Acos(p) - 180;
spriteBatch.Draw(img, FixJoint(joint, resolution, offsetX, offsetY), null, Color.White, ((float)theta * angle), Vector2.Zero, 1.0f, SpriteEffects.None, 1.0f);
}
internal void SetRotations(BoneOrientation boneOrientation)
{
RelativeRotationNow = boneOrientation.HierarchicalRotation;
AbsoluteRotationNow = boneOrientation.AbsoluteRotation;
}
// NEW METHOD
private void SetJointTransformation(BoneOrientation bone, Skeleton skeleton, Matrix bindRoot, ref Matrix[] boneTransforms)
{
if (bone.StartJoint == JointType.HipCenter && bone.EndJoint == JointType.HipCenter)
{
bindRoot.Translation = Vector3.Zero;
Matrix invBindRoot = Matrix.Invert(bindRoot);
Matrix hipOrientation = KinectHelper.Matrix4ToXNAMatrix(bone.HierarchicalRotation.Matrix);
Matrix pelvis = boneTransforms[0];
pelvis.Translation = Vector3.Zero;
Matrix invPelvis = Matrix.Invert(pelvis);
Matrix combined;
if (avatarFront)
{
combined = (invBindRoot * hipOrientation) * invPelvis;
}
else
{
combined = invBindRoot;
}
this.ReplaceBoneMatrix(JointType.HipCenter, combined, true, ref boneTransforms);
}
else if (bone.EndJoint == JointType.ShoulderCenter)
{
if (skeleton.Joints[JointType.HipCenter].TrackingState == JointTrackingState.NotTracked)
{
Matrix hipOrientation = KinectHelper.Matrix4ToXNAMatrix(bone.HierarchicalRotation.Matrix);
Quaternion kinectRotation = KinectHelper.DecomposeMatRot(hipOrientation);
Quaternion avatarRotation = new Quaternion(kinectRotation.Y, kinectRotation.Z, kinectRotation.X, kinectRotation.W);
Matrix combined = Matrix.CreateFromQuaternion(avatarRotation);
this.ReplaceBoneMatrix(JointType.HipCenter, combined, true, ref boneTransforms);
}
}
else if (bone.EndJoint == JointType.Spine)
{
Matrix tempMat = KinectHelper.Matrix4ToXNAMatrix(bone.HierarchicalRotation.Matrix);
Matrix adjustment = Matrix.CreateRotationX(MathHelper.ToRadians(30));
tempMat *= adjustment;
Quaternion kinectRotation = KinectHelper.DecomposeMatRot(tempMat); // XYZ
Quaternion avatarRotation = new Quaternion(kinectRotation.Y, kinectRotation.Z, kinectRotation.X, kinectRotation.W); // transform from Kinect to avatar coordinate system
tempMat = Matrix.CreateFromQuaternion(avatarRotation);
this.ReplaceBoneMatrix(bone.EndJoint, tempMat, false, ref boneTransforms);
}
else if (bone.EndJoint == JointType.Head)
{
Matrix tempMat = KinectHelper.Matrix4ToXNAMatrix(bone.HierarchicalRotation.Matrix);
Quaternion kinectRotation = KinectHelper.DecomposeMatRot(tempMat);
Quaternion avatarRotation = new Quaternion(kinectRotation.Y, kinectRotation.Z, kinectRotation.X, kinectRotation.W);
tempMat = Matrix.CreateFromQuaternion(avatarRotation);
this.ReplaceBoneMatrix(bone.EndJoint, tempMat, false, ref boneTransforms);
}
else if (bone.EndJoint == JointType.ElbowLeft || bone.EndJoint == JointType.WristLeft)
{
Matrix tempMat = KinectHelper.Matrix4ToXNAMatrix(bone.HierarchicalRotation.Matrix);
if (bone.EndJoint == JointType.ElbowLeft)
{
Matrix adjustment = Matrix.CreateRotationY(MathHelper.ToRadians(60));
tempMat *= adjustment;
Matrix adjustment2 = Matrix.CreateRotationZ(MathHelper.ToRadians(15)); //15
tempMat *= adjustment2;
}
Quaternion kinectRotation = KinectHelper.DecomposeMatRot(tempMat); // XYZ
Quaternion avatarRotation = new Quaternion(kinectRotation.Y, kinectRotation.Z, kinectRotation.X, kinectRotation.W);
tempMat = Matrix.CreateFromQuaternion(avatarRotation);
this.ReplaceBoneMatrix(bone.EndJoint, tempMat, false, ref boneTransforms);
}
else if (bone.EndJoint == JointType.HandLeft)
{
Matrix tempMat = KinectHelper.Matrix4ToXNAMatrix(bone.HierarchicalRotation.Matrix);
Quaternion kinectRotation = KinectHelper.DecomposeMatRot(tempMat); // XYZ
Quaternion avatarRotation = new Quaternion(kinectRotation.Y, kinectRotation.X, kinectRotation.Z, kinectRotation.W);
tempMat = Matrix.CreateFromQuaternion(avatarRotation);
this.ReplaceBoneMatrix(bone.EndJoint, tempMat, false, ref boneTransforms);
}
else if (bone.EndJoint == JointType.ElbowRight || bone.EndJoint == JointType.WristRight)
{
Matrix tempMat = KinectHelper.Matrix4ToXNAMatrix(bone.HierarchicalRotation.Matrix);
if (bone.EndJoint == JointType.ElbowRight)
{
Matrix adjustment = Matrix.CreateRotationY(MathHelper.ToRadians(-60));
tempMat *= adjustment;
Matrix adjustment2 = Matrix.CreateRotationZ(MathHelper.ToRadians(-15)); //-15
tempMat *= adjustment2;
}
Quaternion kinectRotation = KinectHelper.DecomposeMatRot(tempMat); // XYZ
Quaternion avatarRotation = new Quaternion(kinectRotation.Y, kinectRotation.Z, kinectRotation.X, kinectRotation.W);
tempMat = Matrix.CreateFromQuaternion(avatarRotation);
this.ReplaceBoneMatrix(bone.EndJoint, tempMat, false, ref boneTransforms);
}
else if (bone.EndJoint == JointType.HandRight)
{
Matrix tempMat = KinectHelper.Matrix4ToXNAMatrix(bone.HierarchicalRotation.Matrix);
Quaternion kinectRotation = KinectHelper.DecomposeMatRot(tempMat); // XYZ
Quaternion avatarRotation = new Quaternion(kinectRotation.Y, kinectRotation.X, kinectRotation.Z, kinectRotation.W);
tempMat = Matrix.CreateFromQuaternion(avatarRotation);
this.ReplaceBoneMatrix(bone.EndJoint, tempMat, false, ref boneTransforms);
}
else if (bone.EndJoint == JointType.KneeLeft)
{
Matrix kneeLeft = KinectHelper.Matrix4ToXNAMatrix(bone.HierarchicalRotation.Matrix);
Matrix combined = kneeLeft;
Matrix adjustment = Matrix.CreateRotationZ(MathHelper.ToRadians(20));
combined *= adjustment;
adjustment = Matrix.CreateRotationY(MathHelper.ToRadians(10));
combined *= adjustment;
this.SetLegMatrix(bone.EndJoint, combined, ref boneTransforms);
}
else if (bone.EndJoint == JointType.AnkleLeft || bone.EndJoint == JointType.AnkleRight)
{
Matrix tempMat = KinectHelper.Matrix4ToXNAMatrix(bone.HierarchicalRotation.Matrix);
if (bone.EndJoint == JointType.AnkleLeft)
{
Matrix adjustment = Matrix.CreateRotationZ(MathHelper.ToRadians(-90));
tempMat *= adjustment;
adjustment = Matrix.CreateRotationY(MathHelper.ToRadians(10));
tempMat *= adjustment;
}
else
{
Matrix adjustment = Matrix.CreateRotationZ(MathHelper.ToRadians(90));
tempMat *= adjustment;
adjustment = Matrix.CreateRotationY(MathHelper.ToRadians(-10));
tempMat *= adjustment;
}
Quaternion kinectRotation = KinectHelper.DecomposeMatRot(tempMat); // XYZ
Quaternion avatarRotation = new Quaternion(kinectRotation.Y, kinectRotation.Z, kinectRotation.X, kinectRotation.W);
tempMat = Matrix.CreateFromQuaternion(avatarRotation);
this.ReplaceBoneMatrix(bone.EndJoint, tempMat, false, ref boneTransforms);
}
else if (bone.EndJoint == JointType.KneeRight)
{
Matrix kneeRight = KinectHelper.Matrix4ToXNAMatrix(bone.HierarchicalRotation.Matrix);
Matrix combined = kneeRight;
Matrix adjustment = Matrix.CreateRotationZ(MathHelper.ToRadians(-20));
combined *= adjustment;
adjustment = Matrix.CreateRotationY(MathHelper.ToRadians(-10));
combined *= adjustment;
this.SetLegMatrix(bone.EndJoint, combined, ref boneTransforms);
}
}
// NEW METHOD
private void SetJointTransformation(BoneOrientation bone, Skeleton skeleton, Matrix bindRoot, ref Matrix[] boneTransforms)
{
if (bone.StartJoint == JointType.HipCenter && bone.EndJoint == JointType.HipCenter)
{
bindRoot.Translation = Vector3.Zero;
Matrix invBindRoot = Matrix.Invert(bindRoot);
Matrix hipOrientation = KinectHelper.Matrix4ToXNAMatrix(bone.HierarchicalRotation.Matrix);
Matrix pelvis = boneTransforms[0];
pelvis.Translation = Vector3.Zero;
Matrix invPelvis = Matrix.Invert(pelvis);
Matrix combined;
if (avatarFront) combined = (invBindRoot * hipOrientation) * invPelvis;
else combined = invBindRoot;
this.ReplaceBoneMatrix(JointType.HipCenter, combined, true, ref boneTransforms);
}
else if (bone.EndJoint == JointType.ShoulderCenter)
{
if (skeleton.Joints[JointType.HipCenter].TrackingState == JointTrackingState.NotTracked)
{
Matrix hipOrientation = KinectHelper.Matrix4ToXNAMatrix(bone.HierarchicalRotation.Matrix);
Quaternion kinectRotation = KinectHelper.DecomposeMatRot(hipOrientation);
Quaternion avatarRotation = new Quaternion(kinectRotation.Y, kinectRotation.Z, kinectRotation.X, kinectRotation.W);
Matrix combined = Matrix.CreateFromQuaternion(avatarRotation);
this.ReplaceBoneMatrix(JointType.HipCenter, combined, true, ref boneTransforms);
}
}
else if (bone.EndJoint == JointType.Spine)
{
Matrix tempMat = KinectHelper.Matrix4ToXNAMatrix(bone.HierarchicalRotation.Matrix);
Matrix adjustment = Matrix.CreateRotationX(MathHelper.ToRadians(30));
tempMat *= adjustment;
Quaternion kinectRotation = KinectHelper.DecomposeMatRot(tempMat); // XYZ
Quaternion avatarRotation = new Quaternion(kinectRotation.Y, kinectRotation.Z, kinectRotation.X, kinectRotation.W); // transform from Kinect to avatar coordinate system
tempMat = Matrix.CreateFromQuaternion(avatarRotation);
this.ReplaceBoneMatrix(bone.EndJoint, tempMat, false, ref boneTransforms);
}
else if (bone.EndJoint == JointType.Head)
{
Matrix tempMat = KinectHelper.Matrix4ToXNAMatrix(bone.HierarchicalRotation.Matrix);
Quaternion kinectRotation = KinectHelper.DecomposeMatRot(tempMat);
Quaternion avatarRotation = new Quaternion(kinectRotation.Y, kinectRotation.Z, kinectRotation.X, kinectRotation.W);
tempMat = Matrix.CreateFromQuaternion(avatarRotation);
this.ReplaceBoneMatrix(bone.EndJoint, tempMat, false, ref boneTransforms);
}
else if (bone.EndJoint == JointType.ElbowLeft || bone.EndJoint == JointType.WristLeft)
{
Matrix tempMat = KinectHelper.Matrix4ToXNAMatrix(bone.HierarchicalRotation.Matrix);
if (bone.EndJoint == JointType.ElbowLeft)
{
Matrix adjustment = Matrix.CreateRotationY(MathHelper.ToRadians(60));
tempMat *= adjustment;
Matrix adjustment2 = Matrix.CreateRotationZ(MathHelper.ToRadians(15)); //15
tempMat *= adjustment2;
}
Quaternion kinectRotation = KinectHelper.DecomposeMatRot(tempMat); // XYZ
Quaternion avatarRotation = new Quaternion(kinectRotation.Y, kinectRotation.Z, kinectRotation.X, kinectRotation.W);
tempMat = Matrix.CreateFromQuaternion(avatarRotation);
this.ReplaceBoneMatrix(bone.EndJoint, tempMat, false, ref boneTransforms);
}
else if (bone.EndJoint == JointType.HandLeft)
{
Matrix tempMat = KinectHelper.Matrix4ToXNAMatrix(bone.HierarchicalRotation.Matrix);
Quaternion kinectRotation = KinectHelper.DecomposeMatRot(tempMat); // XYZ
Quaternion avatarRotation = new Quaternion(kinectRotation.Y, kinectRotation.X, kinectRotation.Z, kinectRotation.W);
tempMat = Matrix.CreateFromQuaternion(avatarRotation);
this.ReplaceBoneMatrix(bone.EndJoint, tempMat, false, ref boneTransforms);
}
else if (bone.EndJoint == JointType.ElbowRight || bone.EndJoint == JointType.WristRight)
{
Matrix tempMat = KinectHelper.Matrix4ToXNAMatrix(bone.HierarchicalRotation.Matrix);
if (bone.EndJoint == JointType.ElbowRight)
{
Matrix adjustment = Matrix.CreateRotationY(MathHelper.ToRadians(-60));
tempMat *= adjustment;
Matrix adjustment2 = Matrix.CreateRotationZ(MathHelper.ToRadians(-15)); //-15
tempMat *= adjustment2;
}
Quaternion kinectRotation = KinectHelper.DecomposeMatRot(tempMat); // XYZ
Quaternion avatarRotation = new Quaternion(kinectRotation.Y, kinectRotation.Z, kinectRotation.X, kinectRotation.W);
tempMat = Matrix.CreateFromQuaternion(avatarRotation);
this.ReplaceBoneMatrix(bone.EndJoint, tempMat, false, ref boneTransforms);
}
else if (bone.EndJoint == JointType.HandRight)
{
Matrix tempMat = KinectHelper.Matrix4ToXNAMatrix(bone.HierarchicalRotation.Matrix);
Quaternion kinectRotation = KinectHelper.DecomposeMatRot(tempMat); // XYZ
Quaternion avatarRotation = new Quaternion(kinectRotation.Y, kinectRotation.X, kinectRotation.Z, kinectRotation.W);
tempMat = Matrix.CreateFromQuaternion(avatarRotation);
this.ReplaceBoneMatrix(bone.EndJoint, tempMat, false, ref boneTransforms);
}
else if (bone.EndJoint == JointType.KneeLeft)
{
Matrix kneeLeft = KinectHelper.Matrix4ToXNAMatrix(bone.HierarchicalRotation.Matrix);
Matrix combined = kneeLeft;
Matrix adjustment = Matrix.CreateRotationZ(MathHelper.ToRadians(20));
combined *= adjustment;
adjustment = Matrix.CreateRotationY(MathHelper.ToRadians(10));
combined *= adjustment;
this.SetLegMatrix(bone.EndJoint, combined, ref boneTransforms);
}
else if (bone.EndJoint == JointType.AnkleLeft || bone.EndJoint == JointType.AnkleRight)
{
Matrix tempMat = KinectHelper.Matrix4ToXNAMatrix(bone.HierarchicalRotation.Matrix);
if (bone.EndJoint == JointType.AnkleLeft)
{
Matrix adjustment = Matrix.CreateRotationZ(MathHelper.ToRadians(-90));
tempMat *= adjustment;
adjustment = Matrix.CreateRotationY(MathHelper.ToRadians(10));
tempMat *= adjustment;
}
else
{
Matrix adjustment = Matrix.CreateRotationZ(MathHelper.ToRadians(90));
tempMat *= adjustment;
adjustment = Matrix.CreateRotationY(MathHelper.ToRadians(-10));
tempMat *= adjustment;
}
Quaternion kinectRotation = KinectHelper.DecomposeMatRot(tempMat); // XYZ
Quaternion avatarRotation = new Quaternion(kinectRotation.Y, kinectRotation.Z, kinectRotation.X , kinectRotation.W);
tempMat = Matrix.CreateFromQuaternion(avatarRotation);
this.ReplaceBoneMatrix(bone.EndJoint, tempMat, false, ref boneTransforms);
}
else if (bone.EndJoint == JointType.KneeRight)
{
Matrix kneeRight = KinectHelper.Matrix4ToXNAMatrix(bone.HierarchicalRotation.Matrix);
Matrix combined = kneeRight;
Matrix adjustment = Matrix.CreateRotationZ(MathHelper.ToRadians(-20));
combined *= adjustment;
adjustment = Matrix.CreateRotationY(MathHelper.ToRadians(-10));
combined *= adjustment;
this.SetLegMatrix(bone.EndJoint, combined, ref boneTransforms);
}
}
private void SetJointTransformation(BoneOrientation bone, Skeleton skeleton, Matrix bindRoot, ref Matrix[] boneTransforms)
{
// Always look at the skeleton root
if (bone.StartJoint == JointType.HipCenter && bone.EndJoint == JointType.HipCenter)
{
// Unless in seated mode, the hip center is special - it is the root of the NuiSkeleton and describes the skeleton orientation in the world
// (camera) coordinate system. All other bones/joint orientations in the hierarchy have hip center as one of their parents.
// However, if in seated mode, the shoulder center then holds the skeleton orientation in the world (camera) coordinate system.
bindRoot.Translation = Vector3.Zero;
Matrix invBindRoot = Matrix.Invert(bindRoot);
Matrix hipOrientation = KinectHelper.Matrix4ToXNAMatrix(bone.HierarchicalRotation.Matrix);
Vector3 hipRotation = RotationHelper.GetInstance().MatrixToEulerAngleVector3(hipOrientation);
// ensure pure rotation, as we set world translation from the Kinect camera below
int hipCenterIndex = 0;
Matrix hipCenter = boneTransforms[hipCenterIndex];
hipCenter.Translation = Vector3.Zero;
Matrix invPelvis = Matrix.Invert(hipCenter);
//Matrix combined = (invBindRoot * hipOrientation) * invPelvis;
//this.ReplaceBoneMatrix(JointType.HipCenter, Matrix.Identity, true, ref boneTransforms);
}
else if (bone.EndJoint == JointType.ShoulderCenter)
{
// This contains an absolute rotation if we are in seated mode, or the hip center is not tracked, as the HipCenter will be identity
if (this.Chooser.SeatedMode || (this.Chooser.SeatedMode == false && skeleton.Joints[JointType.HipCenter].TrackingState == JointTrackingState.NotTracked))
{
bindRoot.Translation = Vector3.Zero;
Matrix invBindRoot = Matrix.Invert(bindRoot);
Matrix hipOrientation = KinectHelper.Matrix4ToXNAMatrix(bone.HierarchicalRotation.Matrix);
// ensure pure rotation, as we set world translation from the Kinect camera
Matrix hipCenter = boneTransforms[0];
hipCenter.Translation = Vector3.Zero;
Matrix invPelvis = Matrix.Invert(hipCenter);
Matrix combined = (invBindRoot * hipOrientation) * invPelvis;
this.ReplaceBoneMatrix(JointType.HipCenter, combined, true, ref boneTransforms);
}
}
else if (bone.EndJoint == JointType.Spine)
{
Matrix tempMat = KinectHelper.Matrix4ToXNAMatrix(bone.HierarchicalRotation.Matrix);
// The Dude appears to lean back too far compared to a real person, so here we adjust this lean.
CorrectBackwardsLean(skeleton, ref tempMat);
Vector3 rotationVector = RotationHelper.GetInstance().MatrixToEulerAngleVector3(tempMat);
// Kinect = +X left, +Y up, +Z forward in body coordinate system
// Avatar = +Z left, +X up, +Y forward
Quaternion kinectRotation = KinectHelper.DecomposeMatRot(tempMat); // XYZ
Quaternion avatarRotation = new Quaternion(kinectRotation.X, kinectRotation.Y, kinectRotation.Z, kinectRotation.W); // transform from Kinect to avatar coordinate system
tempMat = Matrix.CreateFromQuaternion(avatarRotation);
// Set the corresponding matrix in the avatar using the translation table we specified.
// Note for the spine and shoulder center rotations, we could also try to spread the angle
// over all the Avatar skeleton spine joints, causing a more curved back, rather than apply
// it all to one joint, as we do here.
this.ReplaceBoneMatrix(bone.EndJoint, tempMat, false, ref boneTransforms);
}
else if (bone.EndJoint == JointType.Head)
{
Matrix tempMat = KinectHelper.Matrix4ToXNAMatrix(bone.HierarchicalRotation.Matrix);
// Kinect = +X left, +Y up, +Z forward in body coordinate system
// Avatar = +Z left, +X up, +Y forward
Quaternion kinectRotation = KinectHelper.DecomposeMatRot(tempMat); // XYZ
Quaternion avatarRotation = new Quaternion(kinectRotation.X, -kinectRotation.Y, kinectRotation.Z, kinectRotation.W); // transform from Kinect to avatar coordinate system
tempMat = Matrix.CreateFromQuaternion(avatarRotation);
// Set the corresponding matrix in the avatar using the translation table we specified
this.ReplaceBoneMatrix(bone.EndJoint, tempMat, false, ref boneTransforms);
}
// Mirror View for the People Model's Left Arm, the real people's Right Arm
else if (bone.EndJoint == JointType.ElbowLeft || bone.EndJoint == JointType.WristLeft)
{
Matrix tempMat = KinectHelper.Matrix4ToXNAMatrix(bone.HierarchicalRotation.Matrix);
// The Dude appears to lean back too far compared to a real person, so here we adjust this lean.
CorrectBackwardsLean(skeleton, ref tempMat);
Vector3 rotationVector = RotationHelper.GetInstance().MatrixToEulerAngleVector3(tempMat);
//DebugText += String.Format("Joint{0}:(x={1},y={2},z={3})\n", bone.EndJoint, rotationVector.X, rotationVector.Y, rotationVector.Z);
// Kinect = +Y along arm, +X down, -Z backward in body coordinate system
// Avatar = +X along arm, -Z down, +Y backwards
// People = +X along arm, -Y down, +Z backwards
Quaternion kinectRotation = KinectHelper.DecomposeMatRot(tempMat); // XYZ
Quaternion avatarRotation = new Quaternion(kinectRotation.Y, -kinectRotation.X, kinectRotation.Z, kinectRotation.W); // transform from Kinect to avatar coordinate system
tempMat = Matrix.CreateFromQuaternion(avatarRotation);
if (bone.EndJoint == JointType.ElbowLeft)
{
tempMat *= Matrix.CreateFromYawPitchRoll(0.0f, 0.0f, -0.7f);
}
// Set the corresponding matrix in the avatar using the translation table we specified.
// Note for the spine and shoulder center rotations, we could also try to spread the angle
// over all the Avatar skeleton spine joints, causing a more curved back, rather than apply
// it all to one joint, as we do here.
this.ReplaceBoneMatrix(bone.EndJoint, tempMat, false, ref boneTransforms);
}
else if (bone.EndJoint == JointType.HandLeft)
{
Matrix tempMat = KinectHelper.Matrix4ToXNAMatrix(bone.HierarchicalRotation.Matrix);
// Kinect = +Y along arm, +X down, +Z forward in body coordinate system
// Avatar = +X along arm, +Y down, +Z backwards
Quaternion kinectRotation = KinectHelper.DecomposeMatRot(tempMat); // XYZ
Quaternion avatarRotation = new Quaternion(-kinectRotation.Y, kinectRotation.Z, kinectRotation.X, kinectRotation.W);
tempMat = Matrix.CreateFromQuaternion(avatarRotation);
this.ReplaceBoneMatrix(bone.EndJoint, tempMat, false, ref boneTransforms);
}
else if (bone.EndJoint == JointType.ElbowRight || bone.EndJoint == JointType.WristRight)
{
Matrix tempMat = KinectHelper.Matrix4ToXNAMatrix(bone.HierarchicalRotation.Matrix);
// The Dude appears to lean back too far compared to a real person, so here we adjust this lean.
CorrectBackwardsLean(skeleton, ref tempMat);
// Kinect = +X left, +Y up, +Z forward in body coordinate system
// Avatar = +Z left, +X up, +Y forward
// People = +X left, +Y up, +z forward
Quaternion kinectRotation = KinectHelper.DecomposeMatRot(tempMat); // XYZ
Quaternion avatarRotation = new Quaternion(-kinectRotation.Y, kinectRotation.X, kinectRotation.Z, kinectRotation.W); // transform from Kinect to avatar coordinate system
tempMat = Matrix.CreateFromQuaternion(avatarRotation);
// Set the corresponding matrix in the avatar using the translation table we specified.
// Note for the spine and shoulder center rotations, we could also try to spread the angle
// over all the Avatar skeleton spine joints, causing a more curved back, rather than apply
// it all to one joint, as we do here.
this.ReplaceBoneMatrix(bone.EndJoint, tempMat, false, ref boneTransforms);
}
else if (bone.EndJoint == JointType.KneeLeft)
{
// Combine the two joint rotations from the hip and knee
Matrix hipLeft = KinectHelper.Matrix4ToXNAMatrix(skeleton.BoneOrientations[JointType.HipLeft].HierarchicalRotation.Matrix);
Vector3 rotationVector = RotationHelper.GetInstance().MatrixToEulerAngleVector3(hipLeft);
Matrix kneeLeft = KinectHelper.Matrix4ToXNAMatrix(bone.HierarchicalRotation.Matrix);
Matrix legRotation = kneeLeft * hipLeft * Matrix.CreateRotationZ(rotationVector.Z);
Quaternion kinectRotation = KinectHelper.DecomposeMatRot(legRotation); // XYZ
Quaternion avatarRotation = new Quaternion(-kinectRotation.X, kinectRotation.Y, kinectRotation.Z, kinectRotation.W); // transform from Kinect to avatar coordinate system
rotationVector = RotationHelper.GetInstance().QuaternionToEulerAngleVector3(kinectRotation);
legRotation = Matrix.CreateFromQuaternion(avatarRotation);
this.ReplaceBoneMatrix(bone.EndJoint, legRotation, false, ref boneTransforms);
}
else if (bone.EndJoint == JointType.AnkleLeft || bone.EndJoint == JointType.AnkleRight)
{
Matrix tempMat = KinectHelper.Matrix4ToXNAMatrix(bone.HierarchicalRotation.Matrix);
Matrix legRotation = tempMat;
Quaternion kinectRotation = KinectHelper.DecomposeMatRot(legRotation); // XYZ
Quaternion avatarRotation = new Quaternion(-kinectRotation.X, kinectRotation.Y, kinectRotation.Z, kinectRotation.W); // transform from Kinect to avatar coordinate system
Vector3 rotationVector = RotationHelper.GetInstance().QuaternionToEulerAngleVector3(kinectRotation);
legRotation = Matrix.CreateFromQuaternion(avatarRotation);
this.ReplaceBoneMatrix(bone.EndJoint, legRotation, false, ref boneTransforms);
}
else if (bone.EndJoint == JointType.KneeRight)
{
// Combine the two joint rotations from the hip and knee
Matrix hipRight = KinectHelper.Matrix4ToXNAMatrix(skeleton.BoneOrientations[JointType.HipRight].HierarchicalRotation.Matrix);
Matrix kneeRight = KinectHelper.Matrix4ToXNAMatrix(bone.HierarchicalRotation.Matrix);
Vector3 rotationVector = RotationHelper.GetInstance().MatrixToEulerAngleVector3(hipRight);
Matrix legRotation = kneeRight * hipRight * Matrix.CreateRotationZ(rotationVector.Z);
Quaternion kinectRotation = KinectHelper.DecomposeMatRot(legRotation); // XYZ
Quaternion avatarRotation = new Quaternion(-kinectRotation.X, kinectRotation.Y, kinectRotation.Z, kinectRotation.W); // transform from Kinect to avatar coordinate system
rotationVector = RotationHelper.GetInstance().QuaternionToEulerAngleVector3(kinectRotation);
legRotation = Matrix.CreateFromQuaternion(avatarRotation);
this.ReplaceBoneMatrix(bone.EndJoint, legRotation, false, ref boneTransforms);
}
else if (bone.EndJoint == JointType.FootLeft || bone.EndJoint == JointType.FootRight)
{
// Only set this if we actually have a good track on this and the parent
if (skeleton.Joints[bone.EndJoint].TrackingState == JointTrackingState.Tracked && skeleton.Joints[skeleton.BoneOrientations[bone.EndJoint].StartJoint].TrackingState == JointTrackingState.Tracked)
{
Matrix tempMat = KinectHelper.Matrix4ToXNAMatrix(bone.HierarchicalRotation.Matrix);
// Kinect = +Y along foot (fwd), +Z up, +X right in body coordinate system
// Avatar = +X along foot (fwd), +Y up, +Z right
Quaternion kinectRotation = KinectHelper.DecomposeMatRot(tempMat); // XYZ
Quaternion avatarRotation = new Quaternion(kinectRotation.Y, kinectRotation.Z, kinectRotation.X, kinectRotation.W); // transform from Kinect to avatar coordinate system
tempMat = Matrix.CreateFromQuaternion(avatarRotation);
this.ReplaceBoneMatrix(bone.EndJoint, tempMat, false, ref boneTransforms);
}
}
}
/*******************************************************************************************
* Public to string functions.
******************************************************************************************/
public string get_bone_orientation( BoneOrientation bo )
{
//return bo.HierarchicalRotation.Quaternion.W+","+
// bo.HierarchicalRotation.Quaternion.X+","+
// bo.HierarchicalRotation.Quaternion.Y+","+
// bo.HierarchicalRotation.Quaternion.Z;
return bo.AbsoluteRotation.Quaternion.W+","+
bo.AbsoluteRotation.Quaternion.X+","+
bo.AbsoluteRotation.Quaternion.Y+","+
bo.AbsoluteRotation.Quaternion.Z;
}
// fetches the bone orientation given a skeleton id and string for the joint name
public BoneOrientation get_bone( int skid, string joint_name )
{
if (skid != -1) {
foreach (Skeleton skeleton in this.skeletons) {
if ( skeleton.TrackingState == SkeletonTrackingState.Tracked &&
skeleton.TrackingId == skid ) {
switch (joint_name) {
case "AnkleLeft":
return skeleton.BoneOrientations[JointType.AnkleLeft];
case "AnkleRight":
return skeleton.BoneOrientations[JointType.AnkleRight];
case "ElbowLeft":
return skeleton.BoneOrientations[JointType.ElbowLeft];
case "ElbowRight":
return skeleton.BoneOrientations[JointType.ElbowRight];
case "FootLeft":
return skeleton.BoneOrientations[JointType.FootLeft];
case "FootRight":
return skeleton.BoneOrientations[JointType.FootRight];
case "HandLeft":
return skeleton.BoneOrientations[JointType.HandLeft];
case "HandRight":
return skeleton.BoneOrientations[JointType.HandRight];
case "Head":
return skeleton.BoneOrientations[JointType.Head];
case "HipCenter":
return skeleton.BoneOrientations[JointType.HipCenter];
case "HipLeft":
return skeleton.BoneOrientations[JointType.HipLeft];
case "HipRight":
return skeleton.BoneOrientations[JointType.HipRight];
case "KneeLeft":
return skeleton.BoneOrientations[JointType.KneeLeft];
case "KneeRight":
return skeleton.BoneOrientations[JointType.KneeRight];
case "ShoulderCenter":
return skeleton.BoneOrientations[JointType.ShoulderCenter];
case "ShoulderLeft":
return skeleton.BoneOrientations[JointType.ShoulderLeft];
case "ShoulderRight":
return skeleton.BoneOrientations[JointType.ShoulderRight];
case "Spine":
return skeleton.BoneOrientations[JointType.Spine];
case "WristLeft":
return skeleton.BoneOrientations[JointType.WristLeft];
case "WristRight":
return skeleton.BoneOrientations[JointType.WristRight];
}
}
}
}
BoneOrientation bad_bone = new BoneOrientation(JointType.HipCenter);
return bad_bone;
}
/// <summary>
/// Set the bone transform in the avatar mesh.
/// </summary>
/// <param name="bone">Nui Joint/bone orientation</param>
/// <param name="skeleton">The Kinect skeleton.</param>
/// <param name="bindRoot">The bind root matrix of the avatar mesh.</param>
/// <param name="boneTransforms">The avatar mesh rotation matrices.</param>
private void SetJointTransformation(BoneOrientation bone, Skeleton skeleton, Matrix bindRoot, ref Matrix[] boneTransforms)
{
// Always look at the skeleton root
if (bone.StartJoint == JointType.HipCenter && bone.EndJoint == JointType.HipCenter)
{
// Unless in seated mode, the hip center is special - it is the root of the NuiSkeleton and describes the skeleton orientation in the world
// (camera) coordinate system. All other bones/joint orientations in the hierarchy have hip center as one of their parents.
// However, if in seated mode, the shoulder center then holds the skeleton orientation in the world (camera) coordinate system.
bindRoot.Translation = Vector3.Zero;
Matrix invBindRoot = Matrix.Invert(bindRoot);
Matrix hipOrientation = KinectHelper.Matrix4ToXNAMatrix(bone.HierarchicalRotation.Matrix);
// ensure pure rotation, as we set world translation from the Kinect camera below
Matrix hipCenter = boneTransforms[1];
hipCenter.Translation = Vector3.Zero;
Matrix invPelvis = Matrix.Invert(hipCenter);
Matrix combined = (invBindRoot * hipOrientation) * invPelvis;
this.ReplaceBoneMatrix(JointType.HipCenter, combined, true, ref boneTransforms);
}
else if (bone.EndJoint == JointType.ShoulderCenter)
{
// This contains an absolute rotation if we are in seated mode, or the hip center is not tracked, as the HipCenter will be identity
if (this.chooser.SeatedMode || (this.Chooser.SeatedMode == false && skeleton.Joints[JointType.HipCenter].TrackingState == JointTrackingState.NotTracked))
{
bindRoot.Translation = Vector3.Zero;
Matrix invBindRoot = Matrix.Invert(bindRoot);
Matrix hipOrientation = KinectHelper.Matrix4ToXNAMatrix(bone.HierarchicalRotation.Matrix);
// ensure pure rotation, as we set world translation from the Kinect camera
Matrix hipCenter = boneTransforms[1];
hipCenter.Translation = Vector3.Zero;
Matrix invPelvis = Matrix.Invert(hipCenter);
Matrix combined = (invBindRoot * hipOrientation) * invPelvis;
this.ReplaceBoneMatrix(JointType.HipCenter, combined, true, ref boneTransforms);
}
}
else if (bone.EndJoint == JointType.Spine)
{
Matrix tempMat = KinectHelper.Matrix4ToXNAMatrix(bone.HierarchicalRotation.Matrix);
// The Dude appears to lean back too far compared to a real person, so here we adjust this lean.
CorrectBackwardsLean(skeleton, ref tempMat);
// Also add a small constant adjustment rotation to correct for the hip center to spine bone being at a rear-tilted angle in the Kinect skeleton.
// The dude should now look more straight ahead when avateering
Matrix adjustment = Matrix.CreateRotationX(MathHelper.ToRadians(20)); // 20 degree rotation around the local Kinect x axis for the spine bone.
tempMat *= adjustment;
// Kinect = +X left, +Y up, +Z forward in body coordinate system
// Avatar = +Z left, +X up, +Y forward
Quaternion kinectRotation = KinectHelper.DecomposeMatRot(tempMat); // XYZ
Quaternion avatarRotation = new Quaternion(kinectRotation.Y, kinectRotation.Z, kinectRotation.X, kinectRotation.W); // transform from Kinect to avatar coordinate system
tempMat = Matrix.CreateFromQuaternion(avatarRotation);
// Set the corresponding matrix in the avatar using the translation table we specified.
// Note for the spine and shoulder center rotations, we could also try to spread the angle
// over all the Avatar skeleton spine joints, causing a more curved back, rather than apply
// it all to one joint, as we do here.
this.ReplaceBoneMatrix(bone.EndJoint, tempMat, false, ref boneTransforms);
}
else if (bone.EndJoint == JointType.Head)
{
Matrix tempMat = KinectHelper.Matrix4ToXNAMatrix(bone.HierarchicalRotation.Matrix);
// Add a small adjustment rotation to correct for the avatar skeleton head bones being defined pointing looking slightly down, not vertical.
// The dude should now look more straight ahead when avateering
Matrix adjustment = Matrix.CreateRotationX(MathHelper.ToRadians(-30)); // -30 degree rotation around the local Kinect x axis for the head bone.
tempMat *= adjustment;
// Kinect = +X left, +Y up, +Z forward in body coordinate system
// Avatar = +Z left, +X up, +Y forward
Quaternion kinectRotation = KinectHelper.DecomposeMatRot(tempMat); // XYZ
Quaternion avatarRotation = new Quaternion(kinectRotation.Y, kinectRotation.Z, kinectRotation.X, kinectRotation.W); // transform from Kinect to avatar coordinate system
tempMat = Matrix.CreateFromQuaternion(avatarRotation);
// Set the corresponding matrix in the avatar using the translation table we specified
this.ReplaceBoneMatrix(bone.EndJoint, tempMat, false, ref boneTransforms);
}
else if (bone.EndJoint == JointType.ElbowLeft || bone.EndJoint == JointType.WristLeft)
{
Matrix tempMat = KinectHelper.Matrix4ToXNAMatrix(bone.HierarchicalRotation.Matrix);
if (bone.EndJoint == JointType.ElbowLeft)
{
// Add a small adjustment rotation to correct for the avatar skeleton shoulder/upper arm bones.
// The dude should now be able to have arms correctly down at his sides when avateering
Matrix adjustment = Matrix.CreateRotationZ(MathHelper.ToRadians(-15)); // -15 degree rotation around the local Kinect z axis for the upper arm bone.
tempMat *= adjustment;
}
// Kinect = +Y along arm, +X down, +Z forward in body coordinate system
// Avatar = +X along arm, +Y down, +Z backwards
Quaternion kinectRotation = KinectHelper.DecomposeMatRot(tempMat); // XYZ
Quaternion avatarRotation = new Quaternion(kinectRotation.Y, -kinectRotation.Z, -kinectRotation.X, kinectRotation.W); // transform from Kinect to avatar coordinate system
tempMat = Matrix.CreateFromQuaternion(avatarRotation);
this.ReplaceBoneMatrix(bone.EndJoint, tempMat, false, ref boneTransforms);
}
else if (bone.EndJoint == JointType.HandLeft)
{
Matrix tempMat = KinectHelper.Matrix4ToXNAMatrix(bone.HierarchicalRotation.Matrix);
// Add a small adjustment rotation to correct for the avatar skeleton wist/hand bone.
// The dude should now have the palm of his hands toward his body when arms are straight down
Matrix adjustment = Matrix.CreateRotationY(MathHelper.ToRadians(-90)); // -90 degree rotation around the local Kinect y axis for the wrist-hand bone.
tempMat *= adjustment;
// Kinect = +Y along arm, +X down, +Z forward in body coordinate system
// Avatar = +X along arm, +Y down, +Z backwards
Quaternion kinectRotation = KinectHelper.DecomposeMatRot(tempMat); // XYZ
Quaternion avatarRotation = new Quaternion(kinectRotation.Y, kinectRotation.X, -kinectRotation.Z, kinectRotation.W);
tempMat = Matrix.CreateFromQuaternion(avatarRotation);
this.ReplaceBoneMatrix(bone.EndJoint, tempMat, false, ref boneTransforms);
}
else if (bone.EndJoint == JointType.ElbowRight || bone.EndJoint == JointType.WristRight)
{
Matrix tempMat = KinectHelper.Matrix4ToXNAMatrix(bone.HierarchicalRotation.Matrix);
if (bone.EndJoint == JointType.ElbowRight)
{
// Add a small adjustment rotation to correct for the avatar skeleton shoulder/upper arm bones.
// The dude should now be able to have arms correctly down at his sides when avateering
Matrix adjustment = Matrix.CreateRotationZ(MathHelper.ToRadians(15)); // 15 degree rotation around the local Kinect z axis for the upper arm bone.
tempMat *= adjustment;
}
// Kinect = +Y along arm, +X up, +Z forward in body coordinate system
// Avatar = +X along arm, +Y back, +Z down
Quaternion kinectRotation = KinectHelper.DecomposeMatRot(tempMat); // XYZ
Quaternion avatarRotation = new Quaternion(kinectRotation.Y, -kinectRotation.Z, -kinectRotation.X, kinectRotation.W); // transform from Kinect to avatar coordinate system
tempMat = Matrix.CreateFromQuaternion(avatarRotation);
this.ReplaceBoneMatrix(bone.EndJoint, tempMat, false, ref boneTransforms);
}
else if (bone.EndJoint == JointType.HandRight)
{
Matrix tempMat = KinectHelper.Matrix4ToXNAMatrix(bone.HierarchicalRotation.Matrix);
// Add a small adjustment rotation to correct for the avatar skeleton wist/hand bone.
// The dude should now have the palm of his hands toward his body when arms are straight down
Matrix adjustment = Matrix.CreateRotationY(MathHelper.ToRadians(90)); // -90 degree rotation around the local Kinect y axis for the wrist-hand bone.
tempMat *= adjustment;
// Kinect = +Y along arm, +X up, +Z forward in body coordinate system
// Avatar = +X along arm, +Y down, +Z forwards
Quaternion kinectRotation = KinectHelper.DecomposeMatRot(tempMat); // XYZ
Quaternion avatarRotation = new Quaternion(kinectRotation.Y, -kinectRotation.X, kinectRotation.Z, kinectRotation.W); // transform from Kinect to avatar coordinate system
tempMat = Matrix.CreateFromQuaternion(avatarRotation);
this.ReplaceBoneMatrix(bone.EndJoint, tempMat, false, ref boneTransforms);
}
else if (bone.EndJoint == JointType.KneeLeft)
{
// Combine the two joint rotations from the hip and knee
/*Matrix hipLeft = KinectHelper.Matrix4ToXNAMatrix(skeleton.BoneOrientations[JointType.HipLeft].HierarchicalRotation.Matrix);
Matrix kneeLeft = KinectHelper.Matrix4ToXNAMatrix(bone.HierarchicalRotation.Matrix);
Matrix combined = kneeLeft * hipLeft;
this.SetLegMatrix(bone.EndJoint, combined, ref boneTransforms);*/
}
else if (bone.EndJoint == JointType.AnkleLeft || bone.EndJoint == JointType.AnkleRight)
{
Matrix tempMat = KinectHelper.Matrix4ToXNAMatrix(bone.HierarchicalRotation.Matrix);
this.SetLegMatrix(bone.EndJoint, tempMat, ref boneTransforms);
}
else if (bone.EndJoint == JointType.KneeRight)
{
// Combine the two joint rotations from the hip and knee
Matrix hipRight = KinectHelper.Matrix4ToXNAMatrix(skeleton.BoneOrientations[JointType.HipRight].HierarchicalRotation.Matrix);
Matrix kneeRight = KinectHelper.Matrix4ToXNAMatrix(bone.HierarchicalRotation.Matrix);
Matrix combined = kneeRight * hipRight;
this.SetLegMatrix(bone.EndJoint, combined, ref boneTransforms);
}
else if (bone.EndJoint == JointType.FootLeft || bone.EndJoint == JointType.FootRight)
{
// Only set this if we actually have a good track on this and the parent
if (skeleton.Joints[bone.EndJoint].TrackingState == JointTrackingState.Tracked && skeleton.Joints[skeleton.BoneOrientations[bone.EndJoint].StartJoint].TrackingState == JointTrackingState.Tracked)
{
Matrix tempMat = KinectHelper.Matrix4ToXNAMatrix(bone.HierarchicalRotation.Matrix);
// Add a small adjustment rotation to correct for the avatar skeleton foot bones being defined pointing down at 45 degrees, not horizontal
Matrix adjustment = Matrix.CreateRotationX(MathHelper.ToRadians(-45));
tempMat *= adjustment;
// Kinect = +Y along foot (fwd), +Z up, +X right in body coordinate system
// Avatar = +X along foot (fwd), +Y up, +Z right
Quaternion kinectRotation = KinectHelper.DecomposeMatRot(tempMat); // XYZ
Quaternion avatarRotation = new Quaternion(kinectRotation.Y, kinectRotation.Z, kinectRotation.X, kinectRotation.W); // transform from Kinect to avatar coordinate system
tempMat = Matrix.CreateFromQuaternion(avatarRotation);
this.ReplaceBoneMatrix(bone.EndJoint, tempMat, false, ref boneTransforms);
}
}
}
public override void OnInspectorGUI()
{
this.serializedObject.Update();
this.dqs = (DualQuaternionSkinner)this.target;
EditorGUILayout.BeginHorizontal();
EditorGUILayout.LabelField("Mode: ", GUILayout.Width(80));
EditorGUILayout.LabelField(Application.isPlaying ? "Play" : "Editor", GUILayout.Width(80));
EditorGUILayout.EndHorizontal();
EditorGUILayout.BeginHorizontal();
EditorGUILayout.LabelField("DQ skinning: ", GUILayout.Width(80));
EditorGUILayout.LabelField(this.dqs.started ? "ON" : "OFF", GUILayout.Width(80));
EditorGUILayout.EndHorizontal();
EditorGUILayout.Space();
this.dqs.SetViewFrustrumCulling(EditorGUILayout.Toggle("View frustrum culling: ", this.dqs.viewFrustrumCulling));
EditorGUILayout.Space();
BoneOrientation currentOrientation = BoneOrientation.X;
foreach (BoneOrientation orientation in this.boneOrientationVectors.Keys)
{
if (this.dqs.boneOrientationVector == this.boneOrientationVectors[orientation])
{
currentOrientation = orientation;
break;
}
}
var newOrientation = (BoneOrientation)EditorGUILayout.EnumPopup("Bone orientation: ", currentOrientation);
if (this.dqs.boneOrientationVector != this.boneOrientationVectors[newOrientation])
{
this.dqs.boneOrientationVector = this.boneOrientationVectors[newOrientation];
this.dqs.UpdatePerVertexCompensationCoef();
}
EditorGUILayout.PropertyField(this.bulgeCompensation);
EditorGUILayout.Space();
this.showBlendShapes = EditorGUILayout.Foldout(this.showBlendShapes, "Blend shapes");
if (this.showBlendShapes)
{
if (this.dqs.started == false)
{
EditorGUI.BeginChangeCheck();
Undo.RecordObject(this.dqs.gameObject.GetComponent <SkinnedMeshRenderer>(), "changed blendshape weights by DualQuaternionSkinner component");
}
for (int i = 0; i < this.dqs.mesh.blendShapeCount; i++)
{
EditorGUILayout.BeginHorizontal();
EditorGUILayout.LabelField(" " + this.dqs.mesh.GetBlendShapeName(i), GUILayout.Width(EditorGUIUtility.labelWidth - 10));
float weight = EditorGUILayout.Slider(this.dqs.GetBlendShapeWeight(i), 0, 100);
EditorGUILayout.EndHorizontal();
this.dqs.SetBlendShapeWeight(i, weight);
}
}
EditorGUILayout.Space();
EditorGUILayout.PropertyField(this.shaderComputeBoneDQ);
EditorGUILayout.PropertyField(this.shaderDQBlend);
EditorGUILayout.PropertyField(this.shaderApplyMorph);
EditorGUILayout.Space();
EditorGUILayout.LabelField("Problems: ");
if (this.CheckProblems() == false)
{
EditorGUILayout.LabelField("not detected (this is good)");
}
this.serializedObject.ApplyModifiedProperties();
}
