I am developing a Kinect Project with XNA Frameworks. I want to keep the avatar stable on the origin axes to see like on the floor in real world. I changed the bone transformation axes but I couldn't get any positive progress.
Here is the bone transformtion code block:
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);
// 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 Dude 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(0.0f, 0.0f, 0.0f, 0.0f); // 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(0.0f, 0.0f, 0.0f, 0.0f); // 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(0.0f, 0.0f, 0.0f, 0.0f); // 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(0.0f, 0.0f, -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(0.0f, 0.0f, 0.0f, 0.0f); // transform from Kinect to avatar coordinate system
tempMat = Matrix.CreateFromQuaternion(avatarRotation);
this.ReplaceBoneMatrix(bone.EndJoint, tempMat, false, ref boneTransforms);
}
}
}
Is there any way to keep the avatar on the origin?
The root of the body is defined in:
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);
// 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 Dude 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);
}
You should be able to ignore the rotation of the body ignoring hipOrientation. However if you want the avatar to turn back when the user turn back, you have to check the orientation of the body and set the avatar's orientation accordingly.
if (bone.StartJoint == JointType.HipCenter && bone.EndJoint == JointType.HipCenter)
{
bindRoot.Translation = Vector3.Zero;
Matrix invBindRoot = Matrix.Invert(bindRoot);
// Gets the orientation of the body
Matrix hipOrientation = KinectHelper.Matrix4ToXNAMatrix(bone.HierarchicalRotation.Matrix);
if (Vector3.DotProduct(hipOrientation.Forward, Vector3.Forward) > 0) // Checks if the body is forward or backward
hipOrientation = Matrix.Identity; // Fix the avatar orientation to a specific forward position
else
hipOrientation = Matrix.CreateRotationX(Math.PI); // Fix the avatar orientation to a specific backward position
Matrix pelvis = boneTransforms[1];
pelvis.Translation = Vector3.Zero;
Matrix invPelvis = Matrix.Invert(pelvis);
Matrix combined = (invBindRoot * hipOrientation) * invPelvis;
this.ReplaceBoneMatrix(JointType.HipCenter, combined, true, ref boneTransforms);
}
Maybe you should also try deleting the line:
pelvis.Translation = Vector3.Zero;