// ConstrainSelfIntersection collides joints with the skeleton to keep the skeleton's hands and wrists from puncturing its body
// A cylinder is created to represent the torso. Intersecting joints have their positions changed to push them outside the torso.
public void Constrain(ref KinectWrapper.NuiSkeletonData skeleton)
{
// if (null == skeleton)
// {
// return;
// }
int shoulderCenterIndex = (int)KinectWrapper.NuiSkeletonPositionIndex.ShoulderCenter;
int hipCenterIndex = (int)KinectWrapper.NuiSkeletonPositionIndex.HipCenter;
if (skeleton.eSkeletonPositionTrackingState[shoulderCenterIndex] != KinectWrapper.NuiSkeletonPositionTrackingState.NotTracked &&
skeleton.eSkeletonPositionTrackingState[hipCenterIndex] != KinectWrapper.NuiSkeletonPositionTrackingState.NotTracked)
{
Vector3 shoulderDiffLeft = KinectHelper.VectorBetween(ref skeleton, shoulderCenterIndex, (int)KinectWrapper.NuiSkeletonPositionIndex.ShoulderLeft);
Vector3 shoulderDiffRight = KinectHelper.VectorBetween(ref skeleton, shoulderCenterIndex, (int)KinectWrapper.NuiSkeletonPositionIndex.ShoulderRight);
float shoulderLengthLeft = shoulderDiffLeft.magnitude;
float shoulderLengthRight = shoulderDiffRight.magnitude;
// The distance between shoulders is averaged for the radius
float cylinderRadius = (shoulderLengthLeft + shoulderLengthRight) * 0.5f;
// Calculate the shoulder center and the hip center. Extend them up and down respectively.
Vector3 shoulderCenter = (Vector3)skeleton.SkeletonPositions[shoulderCenterIndex];
Vector3 hipCenter = (Vector3)skeleton.SkeletonPositions[hipCenterIndex];
Vector3 hipShoulder = hipCenter - shoulderCenter;
hipShoulder.Normalize();
shoulderCenter = shoulderCenter - (hipShoulder * (ShoulderExtend * cylinderRadius));
hipCenter = hipCenter + (hipShoulder * (HipExtend * cylinderRadius));
// Optionally increase radius to account for bulky avatars
cylinderRadius *= RadiusMultiplier;
// joints to collide
int[] collisionIndices =
{
(int)KinectWrapper.NuiSkeletonPositionIndex.WristLeft,
(int)KinectWrapper.NuiSkeletonPositionIndex.HandLeft,
(int)KinectWrapper.NuiSkeletonPositionIndex.WristRight,
(int)KinectWrapper.NuiSkeletonPositionIndex.HandRight
};
foreach (int j in collisionIndices)
{
Vector3 collisionJoint = (Vector3)skeleton.SkeletonPositions[j];
Vector4 distanceNormal = KinectHelper.DistanceToLineSegment(shoulderCenter, hipCenter, collisionJoint);
Vector3 normal = new Vector3(distanceNormal.x, distanceNormal.y, distanceNormal.z);
// if distance is within the cylinder then push the joint out and away from the cylinder
if (distanceNormal.w < cylinderRadius)
{
collisionJoint += normal * ((cylinderRadius - distanceNormal.w) * CollisionTolerance);
skeleton.SkeletonPositions[j] = (Vector4)collisionJoint;
}
}
}
}
/// <summary>
/// Create Helpers to manage the streams from the Kinect Sensor
/// </summary>
public void CreateHelpers()
{
LogHelper.logInput("Create Helpers", LogHelper.logType.INFO, "Manager");
////
// Kinect
////
kinectMgr = QlikMove.Kinect.KinectHelper.Instance;
////
// Gesture Helper
////
GestureMgr = GestureHelper.Instance;
//add the callback method when an event is triggered
GestureMgr.EventRecognised += new EventHandler <QlikMove.StandardHelper.EventArguments.QlikMoveEventArgs>(this.Event_EventRecognised);
////
// AudioStream
////
VoiceMgr = VoiceHelper.Instance;
////
// Launch the ActionRecogniser with its method
////
ActionMgr = ActionHelper.Instance;
}
public static void RefreshJointData(KinectJoint[] buffer, UnityEngine.Vector4 floorClipPlane,
Dictionary <JointType, Windows.Kinect.Joint> joints, Dictionary <JointType, JointOrientation> jointOrientations)
{
var correction = KinectHelper.CalculateFloorRotationCorrection(floorClipPlane);
var index = 0;
// Trick: Because SpineShoulder is not the successor of SpineMid in the enum,
// the loop does the first iteration for SpineShoulder and restarts at index = 0 = SpineBase.
for (int i = (int)JointType.SpineShoulder; i < buffer.Length; i = index++)
{
var jointType = (JointType)i;
var joint = joints[jointType];
var jointOrientation = jointOrientations[jointType];
var position = correction * KinectHelper.CameraSpacePointToRealSpace(joint.Position, floorClipPlane);
var rotation = correction * KinectHelper.OrientationToRealSpace(jointOrientation.Orientation);
if (rotation.IsZero())
{
var parent = KinectHelper.parentJointTypes[i];
rotation = KinectHelper.InferrRotationFromParentPosition(position, buffer[(int)parent].position);
}
buffer[i] = new KinectJoint(position, rotation, joint.TrackingState);
}
// This is a fix for the head rotation.
// Normally the rotation should be inferred from the parent spine
// like other joints but for some reason this does not work correctly.
var head = buffer[(int)JointType.Head];
var neck = buffer[(int)JointType.Neck];
var fixedRotation = Quaternion.LookRotation(neck.rotation * Vector3.forward, head.position - neck.position);
buffer[(int)JointType.Head] = new KinectJoint(head.position, fixedRotation, head.trackingState);
}
/// <summary>
/// CorrectSensorTilt applies camera tilt correction to the skeleton data.
/// </summary>
/// <param name="skeleton">The skeleton to correct</param>
/// <param name="floorPlane">The floor plane (consisting of up normal and sensor height) detected by skeleton tracking (if any).</param>
/// <param name="sensorElevationAngle">The tilt of the sensor as detected by Kinect.</param>
public static void CorrectSensorTilt(Skeleton skeleton, Tuple <float, float, float, float> floorPlane, int sensorElevationAngle)
{
if (null == skeleton)
{
return;
}
// To correct the tilt of the skeleton due to a tilted camera, we have three possible up vectors:
// one from any floor plane detected in Skeleton Tracking, one from the gravity normal produced by the 3D accelerometer,
// and one from the tilt value sensed by the camera motor.
// The raw accelerometer value is not currently available in the Kinect for Windows SDK, so instead we use the
// the sensorElevationAngle, as the floor plane from skeletal tracking is typically only detected when the
// camera is pointing down and sees the floor.
// Note: SensorElevationAngle value varies around +/- 60 degrees.
Vector3 floorNormal = Vector3.UnitY; // default value (has no tilt effect)
// Assume camera base is level, and use the tilt of the Kinect motor.
// Rotate an up vector by the negated elevation angle around the X axis
floorNormal = Vector3.Transform(
floorNormal,
Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), ConvertToRadians(sensorElevationAngle)));
if (floorPlane != null)
{
Vector4 floorPlaneVec = new Vector4(floorPlane.Item1, floorPlane.Item2, floorPlane.Item3, floorPlane.Item4);
if (floorPlaneVec.Length() > float.Epsilon && (sensorElevationAngle == 0 || Math.Abs(sensorElevationAngle) > 50))
{
// Use the floor plane for everything.
floorNormal = new Vector3(floorPlaneVec.X, floorPlaneVec.Y, floorPlaneVec.Z);
}
}
Array jointTypeValues = Enum.GetValues(typeof(JointType));
// Running average of floor normal
averagedFloorNormal = (averagedFloorNormal * 0.9f) + (floorNormal * 0.1f);
Quaternion rotationToRoomSpace = KinectHelper.GetShortestRotationBetweenVectors(Vector3.UnitY, averagedFloorNormal);
Vector3 hipCenter = KinectHelper.SkeletonPointToVector3(skeleton.Joints[JointType.HipCenter].Position);
// De-tilt.
foreach (JointType j in jointTypeValues)
{
Joint joint = skeleton.Joints[j];
SkeletonPoint pt = joint.Position;
Vector3 pos = KinectHelper.SkeletonPointToVector3(pt);
// Move it back to the origin to rotate
pos -= hipCenter;
Vector3 rotatedVec = Vector3.Transform(pos, rotationToRoomSpace);
rotatedVec += hipCenter;
joint.Position = KinectHelper.Vector3ToSkeletonPoint(rotatedVec);
skeleton.Joints[j] = joint;
}
}
/// <summary>
/// ConstrainSelfIntersection collides joints with the skeleton to keep the skeleton's hands and wrists from puncturing its body
/// A cylinder is created to represent the torso. Intersecting joints have their positions changed to push them outside the torso.
/// </summary>
/// <param name="skeleton">The skeleton.</param>
public static void Constrain(Skeleton skeleton)
{
if (null == skeleton)
{
return;
}
const float ShoulderExtend = 0.5f;
const float HipExtend = 6.0f;
const float CollisionTolerance = 1.01f;
const float RadiusMultiplier = 1.3f; // increase for bulky avatars
if (skeleton.Joints[JointType.ShoulderCenter].TrackingState != JointTrackingState.NotTracked &&
skeleton.Joints[JointType.HipCenter].TrackingState != JointTrackingState.NotTracked)
{
Vector3 shoulderDiffLeft = KinectHelper.VectorBetween(skeleton, JointType.ShoulderCenter, JointType.ShoulderLeft);
Vector3 shoulderDiffRight = KinectHelper.VectorBetween(skeleton, JointType.ShoulderCenter, JointType.ShoulderRight);
float shoulderLengthLeft = shoulderDiffLeft.Length();
float shoulderLengthRight = shoulderDiffRight.Length();
// The distance between shoulders is averaged for the radius
float cylinderRadius = (shoulderLengthLeft + shoulderLengthRight) * 0.5f;
// Calculate the shoulder center and the hip center. Extend them up and down respectively.
Vector3 shoulderCenter = KinectHelper.SkeletonPointToVector3(skeleton.Joints[JointType.ShoulderCenter].Position);
Vector3 hipCenter = KinectHelper.SkeletonPointToVector3(skeleton.Joints[JointType.HipCenter].Position);
Vector3 hipShoulder = hipCenter - shoulderCenter;
//hipShoulder.Normalize();
shoulderCenter = shoulderCenter - (hipShoulder * (ShoulderExtend * cylinderRadius));
hipCenter = hipCenter + (hipShoulder * (HipExtend * cylinderRadius));
// Optionally increase radius to account for bulky avatars
cylinderRadius *= RadiusMultiplier;
// joints to collide
JointType[] collisionIndices = { JointType.WristLeft, JointType.HandLeft, JointType.WristRight, JointType.HandRight };
foreach (JointType j in collisionIndices)
{
Vector3 collisionJoint = KinectHelper.SkeletonPointToVector3(skeleton.Joints[j].Position);
System.Numerics.Vector4 distanceNormal = KinectHelper.DistanceToLineSegment(shoulderCenter, hipCenter, collisionJoint);
Vector3 normal = new Vector3(distanceNormal.X, distanceNormal.Y, distanceNormal.Z);
// if distance is within the cylinder then push the joint out and away from the cylinder
if (distanceNormal.W < cylinderRadius)
{
collisionJoint += normal * ((cylinderRadius - distanceNormal.W) * CollisionTolerance);
Joint joint = skeleton.Joints[j];
joint.Position = KinectHelper.Vector3ToSkeletonPoint(collisionJoint);
skeleton.Joints[j] = joint;
}
}
}
}
private void SetLegMatrix(JointType joint, Matrix legRotation, ref Matrix[] boneTransforms)
{
Quaternion kinectRotation = KinectHelper.DecomposeMatRot(legRotation); // XYZ
Quaternion avatarRotation = new Quaternion(kinectRotation.Y, kinectRotation.Z, kinectRotation.X, kinectRotation.W);
legRotation = Matrix.CreateFromQuaternion(avatarRotation);
this.ReplaceBoneMatrix(joint, legRotation, false, ref boneTransforms);
}
/*
* Event
*/
private void HelperReady()
{
helper = KinectHelper.Instance;
skeleton = helper.GetFixedSkeleton();
GreenScreen.RenderImageData(helper.DepthImagePixels, helper.ColorPixels);
Accessories.SetSkeletons(helper.Skeletons);
KinectHelper.Instance.SetTransform(GreenScreen);
KinectHelper.Instance.SetTransform(Accessories);
}
/// <summary>
/// Refreshes preallocated buffers for frame and joint data.
/// The goal is to avoid per frame allocations in the <see cref="Windows.Kinect.Body.Joints"/>
/// and <see cref="Windows.Kinect.Body.JointOrientations"/> properties.
/// </summary>
public void RefreshFrameData(Body body, FaceFrameResult face, UnityEngine.Vector4 floorClipPlane)
{
this.body = body;
this.face = face;
this.trackingId = this.body.GetTrackingIdFast();
this.lean = this.body.GetLeanDirection();
this.faceRotation = this.face == null ? Quaternion.identity : KinectHelper.FaceRotationToRealSpace(face.FaceRotationQuaternion);
body.RefreshJointsFast(this.rawJoints);
body.RefreshJointOrientationsFast(this.rawJointOrientations);
KinectJoint.RefreshJointData(this.joints, floorClipPlane, this.rawJoints, this.rawJointOrientations);
}
public Scanner(KinectHelper khelper, BoundingBox scanVolume, Vector3 centerPoint, int steps, int stepdelay)
{
m_centerPoint = centerPoint;
m_stepDelay = stepdelay;
ScannerContext = this;
m_kinectHelper = khelper;
//m_serialCommunicator = schelper;
m_scanVolume = scanVolume;
m_kinectThread = new Thread(Scan);
m_kinectThread.IsBackground = true;
m_kinectThread.Start(steps);
}
private void SetAccessoriesNew(KinectHelper helper)
{
Skeleton activeSkeleton = helper.GetFixedSkeleton();
if (helper.GetTrackedSkeletons() > 1)
{
if (Accessories.CheckAccessoriesNew())
{
activeSkeleton = helper.SetNewFixedSkeleton();
}
}
Accessories.SetActiveSkeleton(activeSkeleton);
}
private void FixJointHierarchy(KinectSkeletonJoint joint)
{
// I'm not 100% sure if this is right,
// but at the moment it works fine.
for (int i = 0; i < this.m_JointCount; ++i)
{
if (this.m_Joints[i].transform.parent == joint.transform.parent &&
KinectHelper.InJointTypeHierachy(joint.jointType, this.m_Joints[i].jointType))
{
SetJointParentWithoutRebuild(this.m_Joints[i], joint.transform);
}
}
}
public void CopySkeleton(Skeleton sourceSkeleton)
{
if (null == sourceSkeleton)
{
return;
}
if (null == this.skeleton)
{
this.skeleton = new Skeleton();
}
KinectHelper.CopySkeleton(sourceSkeleton, this.skeleton);
}
public OscMessage BuildJointMessage(Skeleton body, Joint joint)
{
string jointName = joint.JointType.ToString();
if (jointName.IndexOf("Left") >= 0)
{
jointName = jointName.Replace("Left", "Right");
}
else if (jointName.IndexOf("Right") >= 0)
{
jointName = jointName.Replace("Right", "Left");
}
var jointRotation = body.BoneOrientations[joint.JointType].AbsoluteRotation.Matrix;
var position = body.Joints[joint.JointType].Position;
var rotation = body.BoneOrientations[joint.JointType].AbsoluteRotation.Matrix;
var qrotation = body.BoneOrientations[joint.JointType].AbsoluteRotation.Quaternion;
Quaternion qOrientation = new Quaternion(qrotation.X, qrotation.Y, qrotation.Z, qrotation.W);
Matrix4x4 mjointRot = new Matrix4x4(jointRotation.M11, jointRotation.M12, jointRotation.M13, jointRotation.M14, jointRotation.M21, jointRotation.M22, jointRotation.M23, jointRotation.M24, jointRotation.M31, jointRotation.M32, jointRotation.M33, jointRotation.M34, jointRotation.M41, jointRotation.M42, jointRotation.M43, jointRotation.M44);
//System.Numerics.Vector4 v = new System.Numerics.Vector4(qrotation.X, qrotation.Y, qrotation.Z, qrotation.W);
//Plane pln = new Plane(v);
//Matrix4x4 reflectionParent = System.Numerics.Matrix4x4.CreateReflection(pln);
//qSend = Quaternion.CreateFromRotationMatrix(reflectionParent);
//qSend.W = qrotation.W;
var flipMat = new Matrix4x4(1, 0, 0, 0,
0, -1, 0, 0,
0, 0, 1, 0,
0, 0, 0, 1
);
mjointRot = flipMat * mjointRot * flipMat;
var address = String.Format("/{0}", joint.JointType);
if (KinectHelper.BoneOrientationIsValid(qOrientation))
{
Quaternion qSend;
qSend = Quaternion.CreateFromRotationMatrix(mjointRot);
return(new OscMessage(address, (body.Position.X + position.X), (body.Position.Y + position.Y), (body.Position.Z + position.Z), qSend.W, qSend.X, qSend.Y, qSend.Z));
}
return(new OscMessage(address, (body.Position.X + position.X), (body.Position.Y + position.Y), (body.Position.Z + position.Z)));
}
/// <summary>
/// Helper method to calculate a rotated line for the constraint cones.
/// </summary>
private void AddRotatedLine(Vector3 rotatedLine, Skeleton skeleton, int i, Color color)
{
BoneOrientationConstraint jc = this.jointConstraints[i];
Vector3 coneTipVertex = KinectHelper.Position(skeleton, KinectHelper.ParentBoneJoint(jc.Joint));
// Scale dir vector for display
rotatedLine *= LineScale;
Vector3 coneBaseVertex = coneTipVertex + rotatedLine;
coneTipVertex *= this.SkeletonTranslationScaleFactor; // This will scale and optionally mirror the skeleton
coneBaseVertex *= this.SkeletonTranslationScaleFactor;
this.lineVertices.Add(new VertexPositionColor(coneTipVertex, color));
this.lineIndices.Add((short)(this.lineVertices.Count - 1));
this.lineVertices.Add(new VertexPositionColor(coneBaseVertex, color));
this.lineIndices.Add((short)(this.lineVertices.Count - 1));
}
/// <summary>
/// Helper method to add a line between two joints to the list of lines for drawing.
/// </summary>
/// <param name="skeleton">The skeleton.</param>
/// <param name="vertex1">The joint type of the first vertex.</param>
/// <param name="vertex2">The joint type of the second vertex.</param>
private void AddLine(Skeleton skeleton, JointType vertex1, JointType vertex2)
{
// Do not draw bone if not tracked
if (!KinectHelper.IsTrackedOrInferred(skeleton, vertex1) ||
!KinectHelper.IsTrackedOrInferred(skeleton, vertex2))
{
return;
}
Color color = Color.Green;
// Change color of bone to black if at least one joint is inferred
if (!KinectHelper.IsTracked(skeleton, vertex1) ||
!KinectHelper.IsTracked(skeleton, vertex2))
{
color = Color.Black;
}
// Change color of bone to red if constrained
for (int i = 0; i < this.jointConstraints.Count; i++)
{
if (this.jointConstraints[i].Joint == vertex2 && this.jointConstraints[i].Constraint > 1)
{
color = Color.Red;
}
}
Vector3 jointPosition1 = KinectHelper.Position(skeleton, vertex1);
Vector3 jointPosition2 = KinectHelper.Position(skeleton, vertex2);
jointPosition1 *= this.SkeletonTranslationScaleFactor; // This will scale and optionally mirror the skeleton
jointPosition2 *= this.SkeletonTranslationScaleFactor;
this.lineVertices.Add(new VertexPositionColor(jointPosition1, color));
this.lineIndices.Add((short)(this.lineVertices.Count - 1));
this.lineVertices.Add(new VertexPositionColor(jointPosition2, color));
this.lineIndices.Add((short)(this.lineVertices.Count - 1));
}
public static void DrawGizmos(KinectManager manager, GizmoType type)
{
if (manager.sensor == null || !manager.sensor.IsAvailable)
{
return;
}
Transform transform = manager.transform;
var position = transform.position;
var rotation = transform.rotation;
var correction = KinectHelper.CalculateFloorRotationCorrection(manager.floorClipPlane);
transform.position = transform.position + KinectHelper.SensorFloorPlaneYOffset(manager.floorClipPlane);
transform.rotation = Quaternion.Inverse(correction) * transform.rotation;
Gizmos.color = KinectEditorUtils.green;
Gizmos.matrix = transform.localToWorldMatrix;
Gizmos.DrawFrustum(Vector3.zero, KinectHelper.fieldOfView.y, KinectHelper.clippingPlane.x,
KinectHelper.clippingPlane.y, KinectHelper.aspectRatio);
transform.rotation = rotation;
transform.position = position;
}
/// <summary>
/// This method draws the skeleton frame data.
/// </summary>
/// <param name="gameTime">The elapsed game time.</param>
/// <param name="skeleton">The skeleton to correct.</param>
/// <param name="seatedMode">Set true if in seated mode.</param>
/// <param name="world">The world matrix.</param>
/// <param name="view">The view matrix.</param>
/// <param name="projection">The projection matrix.</param>
public void Draw(GameTime gameTime, Skeleton skeleton, bool seatedMode, Matrix world, Matrix view, Matrix projection)
{
// If we don't have data, lets leave
if (null == skeleton || null == this.Game || null == this.effect)
{
return;
}
GraphicsDevice device = this.Game.GraphicsDevice;
// Disable the depth buffer so we can render the Kinect skeleton inside the model
device.DepthStencilState = DepthStencilState.None;
// Update the Kinect skeleton in the display
this.CreateKinectSkeleton(skeleton, seatedMode);
if (this.drawKinectSkeletonConstraintCones)
{
this.AddConstraintDirectionCones(skeleton, seatedMode);
}
this.effect.World = world;
this.effect.View = view;
this.effect.Projection = projection;
this.effect.VertexColorEnabled = true;
foreach (EffectPass pass in this.effect.CurrentTechnique.Passes)
{
pass.Apply();
// Draw Kinect Skeleton vertices as Line List
device.DrawUserIndexedPrimitives <VertexPositionColor>(
PrimitiveType.LineList,
this.lineVertices.ToArray(),
0,
this.lineVertices.Count,
this.lineIndices.ToArray(),
0,
this.lineIndices.Count / 2);
}
if (null != this.localJointCoordinateSystemCrosses)
{
Array jointTypeValues = Enum.GetValues(typeof(JointType));
foreach (JointType j in jointTypeValues)
{
if (KinectHelper.IsTrackedOrInferred(skeleton, j))
{
Matrix localWorld = KinectHelper.Matrix4ToXNAMatrix(skeleton.BoneOrientations[j].AbsoluteRotation.Matrix);
Vector3 jointPosVec = KinectHelper.Position(skeleton, j);
jointPosVec *= this.SkeletonTranslationScaleFactor; // This will scale and optionally mirror the skeleton
localWorld.Translation = jointPosVec; // set the translation into the rotation matrix
this.localJointCoordinateSystemCrosses.Draw(gameTime, localWorld * world, view, projection);
}
}
}
// Re-enable the depth buffer
device.DepthStencilState = DepthStencilState.Default;
}
// Update the filter for one joint.
protected void FilterJoint(ref KinectWrapper.NuiSkeletonData skeleton, int jointIndex, ref KinectWrapper.NuiTransformSmoothParameters smoothingParameters, ref Matrix4x4[] jointOrientations)
{
// if (null == skeleton)
// {
// return;
// }
// int jointIndex = (int)jt;
Quaternion filteredOrientation;
Quaternion trend;
Vector3 fwdVector = (Vector3)jointOrientations[jointIndex].GetColumn(2);
if (fwdVector == Vector3.zero)
{
return;
}
Quaternion rawOrientation = Quaternion.LookRotation(fwdVector, jointOrientations[jointIndex].GetColumn(1));
Quaternion prevFilteredOrientation = this.history[jointIndex].FilteredBoneOrientation;
Quaternion prevTrend = this.history[jointIndex].Trend;
Vector3 rawPosition = (Vector3)skeleton.SkeletonPositions[jointIndex];
bool orientationIsValid = KinectHelper.JointPositionIsValid(rawPosition) && KinectHelper.IsTrackedOrInferred(skeleton, jointIndex) && KinectHelper.BoneOrientationIsValid(rawOrientation);
if (!orientationIsValid)
{
if (this.history[jointIndex].FrameCount > 0)
{
rawOrientation = history[jointIndex].FilteredBoneOrientation;
history[jointIndex].FrameCount = 0;
}
}
// Initial start values or reset values
if (this.history[jointIndex].FrameCount == 0)
{
// Use raw position and zero trend for first value
filteredOrientation = rawOrientation;
trend = Quaternion.identity;
}
else if (this.history[jointIndex].FrameCount == 1)
{
// Use average of two positions and calculate proper trend for end value
Quaternion prevRawOrientation = this.history[jointIndex].RawBoneOrientation;
filteredOrientation = KinectHelper.EnhancedQuaternionSlerp(prevRawOrientation, rawOrientation, 0.5f);
Quaternion diffStarted = KinectHelper.RotationBetweenQuaternions(filteredOrientation, prevFilteredOrientation);
trend = KinectHelper.EnhancedQuaternionSlerp(prevTrend, diffStarted, smoothingParameters.fCorrection);
}
else
{
// First apply a jitter filter
Quaternion diffJitter = KinectHelper.RotationBetweenQuaternions(rawOrientation, prevFilteredOrientation);
float diffValJitter = (float)Math.Abs(KinectHelper.QuaternionAngle(diffJitter));
if (diffValJitter <= smoothingParameters.fJitterRadius)
{
filteredOrientation = KinectHelper.EnhancedQuaternionSlerp(prevFilteredOrientation, rawOrientation, diffValJitter / smoothingParameters.fJitterRadius);
}
else
{
filteredOrientation = rawOrientation;
}
// Now the double exponential smoothing filter
filteredOrientation = KinectHelper.EnhancedQuaternionSlerp(filteredOrientation, prevFilteredOrientation * prevTrend, smoothingParameters.fSmoothing);
diffJitter = KinectHelper.RotationBetweenQuaternions(filteredOrientation, prevFilteredOrientation);
trend = KinectHelper.EnhancedQuaternionSlerp(prevTrend, diffJitter, smoothingParameters.fCorrection);
}
// Use the trend and predict into the future to reduce latency
Quaternion predictedOrientation = filteredOrientation * KinectHelper.EnhancedQuaternionSlerp(Quaternion.identity, trend, smoothingParameters.fPrediction);
// Check that we are not too far away from raw data
Quaternion diff = KinectHelper.RotationBetweenQuaternions(predictedOrientation, filteredOrientation);
float diffVal = (float)Math.Abs(KinectHelper.QuaternionAngle(diff));
if (diffVal > smoothingParameters.fMaxDeviationRadius)
{
predictedOrientation = KinectHelper.EnhancedQuaternionSlerp(filteredOrientation, predictedOrientation, smoothingParameters.fMaxDeviationRadius / diffVal);
}
// predictedOrientation.Normalize();
// filteredOrientation.Normalize();
// trend.Normalize();
// Save the data from this frame
history[jointIndex].RawBoneOrientation = rawOrientation;
history[jointIndex].FilteredBoneOrientation = filteredOrientation;
history[jointIndex].Trend = trend;
history[jointIndex].FrameCount++;
// Set the filtered and predicted data back into the bone orientation
if (KinectHelper.BoneOrientationIsValid(predictedOrientation))
{
jointOrientations[jointIndex].SetTRS(Vector3.zero, predictedOrientation, Vector3.one);
}
}
// Implements the per-frame filter logic for the arms up patch.
public bool FilterSkeleton(ref KinectWrapper.NuiSkeletonData skeleton, float deltaNuiTime)
{
// if (null == skeleton)
// {
// return false;
// }
// exit early if we lose tracking on the entire skeleton
if (skeleton.eTrackingState != KinectWrapper.NuiSkeletonTrackingState.SkeletonTracked)
{
filterJoints.Reset();
}
KinectHelper.CopySkeleton(ref skeleton, ref filteredSkeleton);
filterJoints.UpdateFilter(ref filteredSkeleton);
// Update lerp state with the current delta NUI time.
this.lerpLeftKnee.Tick(deltaNuiTime);
this.lerpLeftAnkle.Tick(deltaNuiTime);
this.lerpLeftFoot.Tick(deltaNuiTime);
this.lerpRightKnee.Tick(deltaNuiTime);
this.lerpRightAnkle.Tick(deltaNuiTime);
this.lerpRightFoot.Tick(deltaNuiTime);
// Exit early if we do not have a valid body basis - too much of the skeleton is invalid.
if ((!KinectHelper.IsTracked(skeleton, (int)KinectWrapper.NuiSkeletonPositionIndex.HipCenter)) ||
(!KinectHelper.IsTrackedOrInferred(skeleton, (int)KinectWrapper.NuiSkeletonPositionIndex.HipLeft)) ||
(!KinectHelper.IsTrackedOrInferred(skeleton, (int)KinectWrapper.NuiSkeletonPositionIndex.HipRight)))
{
return(false);
}
// Determine if the skeleton is clipped by the bottom of the FOV.
bool clippedBottom = (skeleton.dwQualityFlags & (int)KinectWrapper.FrameEdges.Bottom) != 0;
// Select a mask for the left leg depending on which joints are not tracked.
// These masks define how much of the filtered joint positions should be blended
// with the raw positions. Based on the tracking state of the leg joints, we apply
// more filtered data as more joints lose tracking.
Vector3 leftLegMask = this.allTracked;
if (!KinectHelper.IsTracked(skeleton, (int)KinectWrapper.NuiSkeletonPositionIndex.KneeLeft))
{
leftLegMask = this.kneeInferred;
}
else if (!KinectHelper.IsTracked(skeleton, (int)KinectWrapper.NuiSkeletonPositionIndex.AnkleLeft))
{
leftLegMask = this.ankleInferred;
}
else if (!KinectHelper.IsTracked(skeleton, (int)KinectWrapper.NuiSkeletonPositionIndex.FootLeft))
{
leftLegMask = this.footInferred;
}
// Select a mask for the right leg depending on which joints are not tracked.
Vector3 rightLegMask = this.allTracked;
if (!KinectHelper.IsTracked(skeleton, (int)KinectWrapper.NuiSkeletonPositionIndex.KneeRight))
{
rightLegMask = this.kneeInferred;
}
else if (!KinectHelper.IsTracked(skeleton, (int)KinectWrapper.NuiSkeletonPositionIndex.AnkleRight))
{
rightLegMask = this.ankleInferred;
}
else if (!KinectHelper.IsTracked(skeleton, (int)KinectWrapper.NuiSkeletonPositionIndex.FootRight))
{
rightLegMask = this.footInferred;
}
// If the skeleton is not clipped by the bottom of the FOV, cut the filtered data
// blend in half.
float clipMask = clippedBottom ? 1.0f : 0.5f;
// Apply the mask values to the joints of each leg, by placing the mask values into the lerp targets.
this.lerpLeftKnee.SetEnabled(leftLegMask.x * clipMask);
this.lerpLeftAnkle.SetEnabled(leftLegMask.y * clipMask);
this.lerpLeftFoot.SetEnabled(leftLegMask.z * clipMask);
this.lerpRightKnee.SetEnabled(rightLegMask.x * clipMask);
this.lerpRightAnkle.SetEnabled(rightLegMask.y * clipMask);
this.lerpRightFoot.SetEnabled(rightLegMask.z * clipMask);
// The bSkeletonUpdated flag tracks whether we have modified the output skeleton or not.
bool skeletonUpdated = false;
// Apply lerp to the left knee, which will blend the raw joint position with the filtered joint position based on the current lerp value.
if (this.lerpLeftKnee.IsLerpEnabled())
{
int jointIndex = (int)KinectWrapper.NuiSkeletonPositionIndex.KneeLeft;
KinectHelper.LerpAndApply(ref skeleton, jointIndex, (Vector3)filteredSkeleton.SkeletonPositions[jointIndex], lerpLeftKnee.SmoothValue, KinectWrapper.NuiSkeletonPositionTrackingState.Tracked);
skeletonUpdated = true;
}
// Apply lerp to the left ankle.
if (this.lerpLeftAnkle.IsLerpEnabled())
{
int jointIndex = (int)KinectWrapper.NuiSkeletonPositionIndex.AnkleLeft;
KinectHelper.LerpAndApply(ref skeleton, jointIndex, (Vector3)filteredSkeleton.SkeletonPositions[jointIndex], lerpLeftAnkle.SmoothValue, KinectWrapper.NuiSkeletonPositionTrackingState.Tracked);
skeletonUpdated = true;
}
// Apply lerp to the left foot.
if (this.lerpLeftFoot.IsLerpEnabled())
{
int jointIndex = (int)KinectWrapper.NuiSkeletonPositionIndex.FootLeft;
KinectHelper.LerpAndApply(ref skeleton, jointIndex, (Vector3)filteredSkeleton.SkeletonPositions[jointIndex], lerpLeftFoot.SmoothValue, KinectWrapper.NuiSkeletonPositionTrackingState.Inferred);
skeletonUpdated = true;
}
// Apply lerp to the right knee.
if (this.lerpRightKnee.IsLerpEnabled())
{
int jointIndex = (int)KinectWrapper.NuiSkeletonPositionIndex.KneeRight;
KinectHelper.LerpAndApply(ref skeleton, jointIndex, (Vector3)filteredSkeleton.SkeletonPositions[jointIndex], lerpRightKnee.SmoothValue, KinectWrapper.NuiSkeletonPositionTrackingState.Tracked);
skeletonUpdated = true;
}
// Apply lerp to the right ankle.
if (this.lerpRightAnkle.IsLerpEnabled())
{
int jointIndex = (int)KinectWrapper.NuiSkeletonPositionIndex.AnkleRight;
KinectHelper.LerpAndApply(ref skeleton, jointIndex, (Vector3)filteredSkeleton.SkeletonPositions[jointIndex], lerpRightAnkle.SmoothValue, KinectWrapper.NuiSkeletonPositionTrackingState.Tracked);
skeletonUpdated = true;
}
// Apply lerp to the right foot.
if (this.lerpRightFoot.IsLerpEnabled())
{
int jointIndex = (int)KinectWrapper.NuiSkeletonPositionIndex.FootRight;
KinectHelper.LerpAndApply(ref skeleton, jointIndex, (Vector3)filteredSkeleton.SkeletonPositions[jointIndex], lerpRightFoot.SmoothValue, KinectWrapper.NuiSkeletonPositionTrackingState.Inferred);
skeletonUpdated = true;
}
return(skeletonUpdated);
}
// Update the filter for one joint.
protected void FilterJoint(ref KinectWrapper.NuiSkeletonData skeleton, int jointIndex, ref KinectWrapper.NuiTransformSmoothParameters smoothingParameters)
{
// if (null == skeleton)
// {
// return;
// }
//int jointIndex = (int)jt;
Vector3 filteredPosition;
Vector3 diffvec;
Vector3 trend;
Vector3 predictedPosition; // modify
float diffVal;
Vector3 rawPosition = (Vector3)skeleton.SkeletonPositions[jointIndex];
Vector3 prevFilteredPosition = this.history[jointIndex].FilteredPosition;
Vector3 prevTrend = this.history[jointIndex].Trend;
Vector3 prevRawPosition = this.history[jointIndex].RawPosition;
Vector3 prevPredictedPosition = this.history[jointIndex].PredictedPosition; // modify
bool jointIsValid = KinectHelper.JointPositionIsValid(rawPosition);
// If joint is invalid, reset the filter
if (!jointIsValid)
{
history[jointIndex].FrameCount = 0;
}
// modify
if (filterType == 1)
{
// Initial start values
// Single Exponential Smoothing Filter
if (this.history[jointIndex].FrameCount == 0)
{
filteredPosition = rawPosition;
}
else if (this.history[jointIndex].FrameCount == 1 || this.history[jointIndex].FrameCount == 2)
{
filteredPosition = (rawPosition + prevRawPosition) * 0.5f;
}
else
{
// First apply jitter filter
diffvec = rawPosition - prevFilteredPosition;
diffVal = Math.Abs(diffvec.magnitude);
if (diffVal <= smoothingParameters.fJitterRadius)
{
filteredPosition = (rawPosition * (diffVal / smoothingParameters.fJitterRadius)) + (prevFilteredPosition * (1.0f - (diffVal / smoothingParameters.fJitterRadius)));
}
else
{
filteredPosition = rawPosition;
}
// Single exponential smoothing filter
filteredPosition = (filteredPosition * (1.0f - smoothingParameters.fSmoothing)) + (prevFilteredPosition * smoothingParameters.fSmoothing);
}
predictedPosition = filteredPosition;
// Check if we are not too far away from raw data
diffvec = predictedPosition - rawPosition;
diffVal = Mathf.Abs(diffvec.magnitude);
if (diffVal > smoothingParameters.fMaxDeviationRadius)
{
predictedPosition = (predictedPosition * (smoothingParameters.fMaxDeviationRadius / diffVal)) + (rawPosition * (1.0f - (smoothingParameters.fMaxDeviationRadius / diffVal)));
}
trend = Vector3.zero;
}
else
{
// Initial start values
// Double Exponential Smoothing Filter
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 * smoothingParameters.fCorrection) + (prevTrend * (1.0f - smoothingParameters.fCorrection));
}
else
{
// First apply jitter filter
diffvec = rawPosition - prevFilteredPosition;
diffVal = Math.Abs(diffvec.magnitude);
if (diffVal <= smoothingParameters.fJitterRadius)
{
filteredPosition = (rawPosition * (diffVal / smoothingParameters.fJitterRadius)) + (prevFilteredPosition * (1.0f - (diffVal / smoothingParameters.fJitterRadius)));
}
else
{
filteredPosition = rawPosition;
}
// Double exponential smoothing filter
filteredPosition = (filteredPosition * (1.0f - smoothingParameters.fSmoothing)) + ((prevFilteredPosition + prevTrend) * smoothingParameters.fSmoothing);
diffvec = filteredPosition - prevFilteredPosition;
trend = (diffvec * smoothingParameters.fCorrection) + (prevTrend * (1.0f - smoothingParameters.fCorrection));
}
// Predict into the future to reduce latency
// Vector3 predictedPosition = filteredPosition + (trend * smoothingParameters.fPrediction);
predictedPosition = filteredPosition + (trend * smoothingParameters.fPrediction); //modify
// Check if we are not too far away from raw data
diffvec = predictedPosition - rawPosition;
diffVal = Mathf.Abs(diffvec.magnitude);
if (diffVal > smoothingParameters.fMaxDeviationRadius)
{
predictedPosition = (predictedPosition * (smoothingParameters.fMaxDeviationRadius / diffVal)) + (rawPosition * (1.0f - (smoothingParameters.fMaxDeviationRadius / diffVal)));
}
}
// Save the data from this frame
history[jointIndex].RawPosition = rawPosition;
history[jointIndex].FilteredPosition = filteredPosition;
history[jointIndex].Trend = trend;
history[jointIndex].FrameCount++;
// Set the filtered data back into the joint
// Joint j = skeleton.Joints[jt];
// j.Position = KinectHelper.Vector3ToSkeletonPoint(predictedPosition);
// skeleton.Joints[jt] = j;
skeleton.SkeletonPositions[jointIndex] = (Vector4)predictedPosition;
}
private void SetAccessoriesNew(KinectHelper helper)
{
Skeleton activeSkeleton = helper.GetFixedSkeleton();
if (helper.GetTrackedSkeletons() > 1)
{
if (Accessories.CheckAccessoriesNew())
{
activeSkeleton = helper.SetNewFixedSkeleton();
}
}
Accessories.SetActiveSkeleton(activeSkeleton);
}
/// <summary>
/// CorrectSkeletonOffsetFromFloor moves the skeleton to the floor.
/// If no floor found in Skeletal Tracking, we can try and use the foot position
/// but this can be very noisy, which causes the skeleton to bounce up and down.
/// Note: Using the foot positions will reduce the visual effect of jumping when
/// an avateer jumps, as we perform a running average.
/// </summary>
/// <param name="skeleton">The skeleton to correct.</param>
/// <param name="floorPlane">The floor plane (consisting of up normal and sensor height) detected by skeleton tracking (if any).</param>
/// <param name="avatarHipCenterHeight">The height of the avatar Hip Center joint.</param>
public void CorrectSkeletonOffsetFromFloor(Skeleton skeleton, Tuple <float, float, float, float> floorPlane, float avatarHipCenterHeight)
{
if (skeleton == null || skeleton.TrackingState != SkeletonTrackingState.Tracked)
{
return;
}
Vector4 floorPlaneVec = Vector4.Zero;
bool haveFloor = false;
if (null != floorPlane)
{
floorPlaneVec = new Vector4(floorPlane.Item1, floorPlane.Item2, floorPlane.Item3, floorPlane.Item4);
haveFloor = floorPlaneVec.Length() > float.Epsilon;
}
// If there's no floor found, try to use the lower foot position, if visible.
Vector3 hipCenterPosition = KinectHelper.SkeletonPointToVector3(skeleton.Joints[JointType.HipCenter].Position);
bool haveLeftFoot = KinectHelper.IsTrackedOrInferred(skeleton, JointType.FootLeft);
bool haveLeftAnkle = KinectHelper.IsTracked(skeleton, JointType.AnkleLeft);
bool haveRightFoot = KinectHelper.IsTrackedOrInferred(skeleton, JointType.FootRight);
bool haveRightAnkle = KinectHelper.IsTracked(skeleton, JointType.AnkleRight);
if (haveLeftFoot || haveLeftAnkle || haveRightFoot || haveRightAnkle)
{
// As this runs after de-tilt of the skeleton, so the floor-camera offset will
// be the foot to camera 0 height in meters as the foot is at the floor plane.
// Jumping is enabled to some extent due to the running average, but will appear reduced in height.
Vector3 leftFootPosition = KinectHelper.SkeletonPointToVector3(skeleton.Joints[JointType.FootLeft].Position);
Vector3 rightFootPosition = KinectHelper.SkeletonPointToVector3(skeleton.Joints[JointType.FootRight].Position);
Vector3 leftAnklePosition = KinectHelper.SkeletonPointToVector3(skeleton.Joints[JointType.AnkleLeft].Position);
Vector3 rightAnklePosition = KinectHelper.SkeletonPointToVector3(skeleton.Joints[JointType.AnkleRight].Position);
// Average the foot and ankle if we have it
float leftFootAverage = (haveLeftFoot && haveLeftAnkle) ? (leftFootPosition.Y + leftAnklePosition.Y) * 0.5f : haveLeftFoot ? leftFootPosition.Y : leftAnklePosition.Y;
float rightFootAverage = (haveRightFoot && haveRightAnkle) ? (rightFootPosition.Y + rightAnklePosition.Y) * 0.5f : haveRightFoot ? rightFootPosition.Y : rightAnklePosition.Y;
// We assume the lowest foot is placed on the floor
float lowestFootPosition = 0;
if ((haveLeftFoot || haveLeftAnkle) && (haveRightFoot || haveRightAnkle))
{
// Negate, as we are looking for the camera height above the floor plane
lowestFootPosition = Math.Min(leftFootAverage, rightFootAverage);
}
else if (haveLeftFoot || haveLeftAnkle)
{
lowestFootPosition = leftFootAverage;
}
else
{
lowestFootPosition = rightFootAverage;
}
// Running average of floor position
this.averageFloorOffset = (this.averageFloorOffset * 0.9f) + (lowestFootPosition * 0.1f);
}
else if (haveFloor)
{
// Get the detected height of the camera off the floor in meters.
if (0.0f == this.averageFloorOffset)
{
// If it's the initial frame of detection, just set the floor plane directly.
this.averageFloorOffset = -floorPlaneVec.W;
}
else
{
// Running average of floor position
this.averageFloorOffset = (this.averageFloorOffset * 0.9f) + (-floorPlaneVec.W * 0.1f);
}
}
else
{
// Just set the avatar offset directly
this.averageFloorOffset = hipCenterPosition.Y - avatarHipCenterHeight;
}
Array jointTypeValues = Enum.GetValues(typeof(JointType));
// Move to the floor.
foreach (JointType j in jointTypeValues)
{
Joint joint = skeleton.Joints[j];
SkeletonPoint pt = joint.Position;
pt.Y = pt.Y - this.averageFloorOffset;
joint.Position = pt;
skeleton.Joints[j] = joint;
}
}
/// <summary>
/// Update the filter for one joint.
/// </summary>
/// <param name="skeleton">The Skeleton to filter.</param>
/// <param name="jt">The Skeleton Joint index to filter.</param>
/// <param name="smoothingParameters">The Smoothing parameters to apply.</param>
protected void FilterJoint(Skeleton skeleton, JointType jt, TransformSmoothParameters smoothingParameters)
{
if (null == skeleton)
{
return;
}
int jointIndex = (int)jt;
Quaternion filteredOrientation;
Quaternion trend;
Quaternion rawOrientation = Quaternion.CreateFromRotationMatrix(KinectHelper.Matrix4ToXNAMatrix(skeleton.BoneOrientations[jt].HierarchicalRotation.Matrix));
Quaternion prevFilteredOrientation = this.history[jointIndex].FilteredBoneOrientation;
Quaternion prevTrend = this.history[jointIndex].Trend;
Vector3 rawPosition = KinectHelper.SkeletonPointToVector3(skeleton.Joints[jt].Position);
bool orientationIsValid = KinectHelper.JointPositionIsValid(rawPosition) && KinectHelper.IsTrackedOrInferred(skeleton, jt) && KinectHelper.BoneOrientationIsValid(rawOrientation);
if (!orientationIsValid)
{
if (this.history[jointIndex].FrameCount > 0)
{
rawOrientation = this.history[jointIndex].FilteredBoneOrientation;
this.history[jointIndex].FrameCount = 0;
}
}
// Initial start values or reset values
if (this.history[jointIndex].FrameCount == 0)
{
// Use raw position and zero trend for first value
filteredOrientation = rawOrientation;
trend = Quaternion.Identity;
}
else if (this.history[jointIndex].FrameCount == 1)
{
// Use average of two positions and calculate proper trend for end value
Quaternion prevRawOrientation = this.history[jointIndex].RawBoneOrientation;
filteredOrientation = KinectHelper.EnhancedQuaternionSlerp(prevRawOrientation, rawOrientation, 0.5f);
Quaternion diffStarted = KinectHelper.RotationBetweenQuaternions(filteredOrientation, prevFilteredOrientation);
trend = KinectHelper.EnhancedQuaternionSlerp(prevTrend, diffStarted, smoothingParameters.Correction);
}
else
{
// First apply a jitter filter
Quaternion diffJitter = KinectHelper.RotationBetweenQuaternions(rawOrientation, prevFilteredOrientation);
float diffValJitter = (float)Math.Abs(KinectHelper.QuaternionAngle(diffJitter));
if (diffValJitter <= smoothingParameters.JitterRadius)
{
filteredOrientation = KinectHelper.EnhancedQuaternionSlerp(prevFilteredOrientation, rawOrientation, diffValJitter / smoothingParameters.JitterRadius);
}
else
{
filteredOrientation = rawOrientation;
}
// Now the double exponential smoothing filter
filteredOrientation = KinectHelper.EnhancedQuaternionSlerp(filteredOrientation, Quaternion.Multiply(prevFilteredOrientation, prevTrend), smoothingParameters.Smoothing);
diffJitter = KinectHelper.RotationBetweenQuaternions(filteredOrientation, prevFilteredOrientation);
trend = KinectHelper.EnhancedQuaternionSlerp(prevTrend, diffJitter, smoothingParameters.Correction);
}
// Use the trend and predict into the future to reduce latency
Quaternion predictedOrientation = Quaternion.Multiply(filteredOrientation, KinectHelper.EnhancedQuaternionSlerp(Quaternion.Identity, trend, smoothingParameters.Prediction));
// Check that we are not too far away from raw data
Quaternion diff = KinectHelper.RotationBetweenQuaternions(predictedOrientation, filteredOrientation);
float diffVal = (float)Math.Abs(KinectHelper.QuaternionAngle(diff));
if (diffVal > smoothingParameters.MaxDeviationRadius)
{
predictedOrientation = KinectHelper.EnhancedQuaternionSlerp(filteredOrientation, predictedOrientation, smoothingParameters.MaxDeviationRadius / diffVal);
}
predictedOrientation.Normalize();
filteredOrientation.Normalize();
trend.Normalize();
// Save the data from this frame
this.history[jointIndex].RawBoneOrientation = rawOrientation;
this.history[jointIndex].FilteredBoneOrientation = filteredOrientation;
this.history[jointIndex].Trend = trend;
this.history[jointIndex].FrameCount++;
// Set the filtered and predicted data back into the bone orientation
skeleton.BoneOrientations[jt].HierarchicalRotation.Quaternion = KinectHelper.XNAQuaternionToVector4(predictedOrientation); // local rotation
skeleton.BoneOrientations[jt].HierarchicalRotation.Matrix = KinectHelper.XNAMatrixToMatrix4(Matrix.CreateFromQuaternion(predictedOrientation));
// HipCenter has no parent and is the root of our skeleton - leave the HipCenter absolute set as it is
if (jt != JointType.HipCenter)
{
Quaternion parentRot = KinectHelper.Vector4ToXNAQuaternion(skeleton.BoneOrientations[KinectHelper.ParentBoneJoint(jt)].AbsoluteRotation.Quaternion);
// create a new world rotation
Quaternion worldRot = Quaternion.Multiply(parentRot, predictedOrientation);
skeleton.BoneOrientations[jt].AbsoluteRotation.Quaternion = KinectHelper.XNAQuaternionToVector4(worldRot);
skeleton.BoneOrientations[jt].AbsoluteRotation.Matrix = KinectHelper.XNAMatrixToMatrix4(Matrix.CreateFromQuaternion(worldRot));
}
else
{
// In the Hip Center root joint, absolute and relative are the same
skeleton.BoneOrientations[jt].AbsoluteRotation.Quaternion = skeleton.BoneOrientations[jt].HierarchicalRotation.Quaternion;
skeleton.BoneOrientations[jt].AbsoluteRotation.Matrix = skeleton.BoneOrientations[jt].HierarchicalRotation.Matrix;
}
}
/// <summary>
/// Name: FilterSkeleton - Implements the per-frame filter logic for the arms up patch.
/// </summary>
/// <param name="skeleton">The skeleton to filter.</param>
/// <param name="deltaNuiTime">Time since the last filter update.</param>
/// <returns>Returns true if filter runs, false if it did not run.</returns>
public bool FilterSkeleton(Skeleton skeleton, float deltaNuiTime)
{
if (null == skeleton)
{
return(false);
}
// exit early if we lose tracking on the entire skeleton
if (skeleton.TrackingState != SkeletonTrackingState.Tracked)
{
this.filterDoubleExp.Reset();
}
KinectHelper.CopySkeleton(skeleton, this.filteredSkeleton);
this.filterDoubleExp.UpdateFilter(this.filteredSkeleton);
// Update lerp state with the current delta NUI time.
this.lerpLeftKnee.Tick(deltaNuiTime);
this.lerpLeftAnkle.Tick(deltaNuiTime);
this.lerpLeftFoot.Tick(deltaNuiTime);
this.lerpRightKnee.Tick(deltaNuiTime);
this.lerpRightAnkle.Tick(deltaNuiTime);
this.lerpRightFoot.Tick(deltaNuiTime);
// Exit early if we do not have a valid body basis - too much of the skeleton is invalid.
if ((!KinectHelper.IsTracked(skeleton, JointType.HipCenter)) || (!KinectHelper.IsTrackedOrInferred(skeleton, JointType.HipLeft)) || (!KinectHelper.IsTrackedOrInferred(skeleton, JointType.HipRight)))
{
return(false);
}
// Determine if the skeleton is clipped by the bottom of the FOV.
bool clippedBottom = skeleton.ClippedEdges == FrameEdges.Bottom;
// Select a mask for the left leg depending on which joints are not tracked.
// These masks define how much of the filtered joint positions should be blended
// with the raw positions. Based on the tracking state of the leg joints, we apply
// more filtered data as more joints lose tracking.
Vector3 leftLegMask = this.allTracked;
if (!KinectHelper.IsTracked(skeleton, JointType.KneeLeft))
{
leftLegMask = this.kneeInferred;
}
else if (!KinectHelper.IsTracked(skeleton, JointType.AnkleLeft))
{
leftLegMask = this.ankleInferred;
}
else if (!KinectHelper.IsTracked(skeleton, JointType.FootLeft))
{
leftLegMask = this.footInferred;
}
// Select a mask for the right leg depending on which joints are not tracked.
Vector3 rightLegMask = this.allTracked;
if (!KinectHelper.IsTracked(skeleton, JointType.KneeRight))
{
rightLegMask = this.kneeInferred;
}
else if (!KinectHelper.IsTracked(skeleton, JointType.AnkleRight))
{
rightLegMask = this.ankleInferred;
}
else if (!KinectHelper.IsTracked(skeleton, JointType.FootRight))
{
rightLegMask = this.footInferred;
}
// If the skeleton is not clipped by the bottom of the FOV, cut the filtered data
// blend in half.
float clipMask = clippedBottom ? 1.0f : 0.5f;
// Apply the mask values to the joints of each leg, by placing the mask values into the lerp targets.
this.lerpLeftKnee.SetEnabled(leftLegMask.X * clipMask);
this.lerpLeftAnkle.SetEnabled(leftLegMask.Y * clipMask);
this.lerpLeftFoot.SetEnabled(leftLegMask.Z * clipMask);
this.lerpRightKnee.SetEnabled(rightLegMask.X * clipMask);
this.lerpRightAnkle.SetEnabled(rightLegMask.Y * clipMask);
this.lerpRightFoot.SetEnabled(rightLegMask.Z * clipMask);
// The bSkeletonUpdated flag tracks whether we have modified the output skeleton or not.
bool skeletonUpdated = false;
// Apply lerp to the left knee, which will blend the raw joint position with the filtered joint position based on the current lerp value.
if (this.lerpLeftKnee.IsLerpEnabled())
{
KinectHelper.LerpAndApply(skeleton, JointType.KneeLeft, KinectHelper.SkeletonPointToVector3(this.filteredSkeleton.Joints[JointType.KneeLeft].Position), this.lerpLeftKnee.SmoothValue, JointTrackingState.Tracked);
skeletonUpdated = true;
}
// Apply lerp to the left ankle.
if (this.lerpLeftAnkle.IsLerpEnabled())
{
KinectHelper.LerpAndApply(skeleton, JointType.AnkleLeft, KinectHelper.SkeletonPointToVector3(this.filteredSkeleton.Joints[JointType.AnkleLeft].Position), this.lerpLeftAnkle.SmoothValue, JointTrackingState.Tracked);
skeletonUpdated = true;
}
// Apply lerp to the left foot.
if (this.lerpLeftFoot.IsLerpEnabled())
{
KinectHelper.LerpAndApply(skeleton, JointType.FootLeft, KinectHelper.SkeletonPointToVector3(this.filteredSkeleton.Joints[JointType.FootLeft].Position), this.lerpLeftFoot.SmoothValue, JointTrackingState.Inferred);
skeletonUpdated = true;
}
// Apply lerp to the right knee.
if (this.lerpRightKnee.IsLerpEnabled())
{
KinectHelper.LerpAndApply(skeleton, JointType.KneeRight, KinectHelper.SkeletonPointToVector3(this.filteredSkeleton.Joints[JointType.KneeRight].Position), this.lerpRightKnee.SmoothValue, JointTrackingState.Tracked);
skeletonUpdated = true;
}
// Apply lerp to the right ankle.
if (this.lerpRightAnkle.IsLerpEnabled())
{
KinectHelper.LerpAndApply(skeleton, JointType.AnkleRight, KinectHelper.SkeletonPointToVector3(this.filteredSkeleton.Joints[JointType.AnkleRight].Position), this.lerpRightAnkle.SmoothValue, JointTrackingState.Tracked);
skeletonUpdated = true;
}
// Apply lerp to the right foot.
if (this.lerpRightFoot.IsLerpEnabled())
{
KinectHelper.LerpAndApply(skeleton, JointType.FootRight, KinectHelper.SkeletonPointToVector3(this.filteredSkeleton.Joints[JointType.FootRight].Position), this.lerpRightFoot.SmoothValue, JointTrackingState.Inferred);
skeletonUpdated = true;
}
return(skeletonUpdated);
}
// 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);
}
}
/// <summary>
/// Update the filter for one joint.
/// </summary>
/// <param name="skeleton">The Skeleton to filter.</param>
/// <param name="jt">The Skeleton Joint index to filter.</param>
/// <param name="smoothingParameters">The Smoothing parameters to apply.</param>
protected void FilterJoint(Skeleton skeleton, JointType jt, TransformSmoothParameters smoothingParameters)
{
if (null == skeleton)
{
return;
}
int jointIndex = (int)jt;
Vector3 filteredPosition;
Vector3 diffvec;
Vector3 trend;
float diffVal;
Vector3 rawPosition = KinectHelper.SkeletonPointToVector3(skeleton.Joints[jt].Position);
Vector3 prevFilteredPosition = this.history[jointIndex].FilteredPosition;
Vector3 prevTrend = this.history[jointIndex].Trend;
Vector3 prevRawPosition = this.history[jointIndex].RawPosition;
bool jointIsValid = KinectHelper.JointPositionIsValid(rawPosition);
// If joint is invalid, reset the filter
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 = Vector3.Multiply(Vector3.Add(rawPosition, prevRawPosition), 0.5f);
diffvec = Vector3.Subtract(filteredPosition, prevFilteredPosition);
trend = Vector3.Add(Vector3.Multiply(diffvec, smoothingParameters.Correction), Vector3.Multiply(prevTrend, 1.0f - smoothingParameters.Correction));
}
else
{
// First apply jitter filter
diffvec = Vector3.Subtract(rawPosition, prevFilteredPosition);
diffVal = Math.Abs(diffvec.Length());
if (diffVal <= smoothingParameters.JitterRadius)
{
filteredPosition = Vector3.Add(Vector3.Multiply(rawPosition, diffVal / smoothingParameters.JitterRadius), Vector3.Multiply(prevFilteredPosition, 1.0f - (diffVal / smoothingParameters.JitterRadius)));
}
else
{
filteredPosition = rawPosition;
}
// Now the double exponential smoothing filter
filteredPosition = Vector3.Add(Vector3.Multiply(filteredPosition, 1.0f - smoothingParameters.Smoothing), Vector3.Multiply(Vector3.Add(prevFilteredPosition, prevTrend), smoothingParameters.Smoothing));
diffvec = Vector3.Subtract(filteredPosition, prevFilteredPosition);
trend = Vector3.Add(Vector3.Multiply(diffvec, smoothingParameters.Correction), Vector3.Multiply(prevTrend, 1.0f - smoothingParameters.Correction));
}
// Predict into the future to reduce latency
Vector3 predictedPosition = Vector3.Add(filteredPosition, Vector3.Multiply(trend, smoothingParameters.Prediction));
// Check that we are not too far away from raw data
diffvec = Vector3.Subtract(predictedPosition, rawPosition);
diffVal = Math.Abs(diffvec.Length());
if (diffVal > smoothingParameters.MaxDeviationRadius)
{
predictedPosition = Vector3.Add(Vector3.Multiply(predictedPosition, smoothingParameters.MaxDeviationRadius / diffVal), Vector3.Multiply(rawPosition, 1.0f - (smoothingParameters.MaxDeviationRadius / diffVal)));
}
// Save the data from this frame
this.history[jointIndex].RawPosition = rawPosition;
this.history[jointIndex].FilteredPosition = filteredPosition;
this.history[jointIndex].Trend = trend;
this.history[jointIndex].FrameCount++;
// Set the filtered data back into the joint
Joint j = skeleton.Joints[jt];
j.Position = KinectHelper.Vector3ToSkeletonPoint(predictedPosition);
skeleton.Joints[jt] = j;
}
/// <summary>
/// Helper method to add the constraint cone for drawing.
/// </summary>
/// <param name="skeleton">The skeleton.</param>
/// <param name="seatedMode">Set true if in seated mode.</param>
private void AddConstraintDirectionCones(Skeleton skeleton, bool seatedMode)
{
// Calculate constraint values. 0.0-1.0 means the bone is within the constraint cone. Greater than 1.0 means
// it lies outside the constraint cone.
for (int i = 0; i < this.jointConstraints.Count; i++)
{
BoneOrientationConstraint jc = this.jointConstraints[i];
JointType joint = this.jointConstraints[i].Joint;
// Do not draw bone if not tracked
if (skeleton.Joints[joint].TrackingState == JointTrackingState.NotTracked)
{
continue;
}
// Do not draw the following bones if in seated mode
if (
seatedMode &&
(JointType.HipCenter == joint ||
JointType.Spine == joint ||
JointType.ShoulderCenter == joint ||
JointType.HipLeft == joint ||
JointType.KneeLeft == joint ||
JointType.AnkleLeft == joint ||
JointType.FootLeft == joint ||
JointType.HipRight == joint ||
JointType.KneeRight == joint ||
JointType.AnkleRight == joint ||
JointType.FootRight == joint))
{
continue;
}
// Get the constraint direction in world space from the parent orientation
Matrix matConstraintLocalToWorld = KinectHelper.Matrix4ToXNAMatrix(skeleton.BoneOrientations[skeleton.BoneOrientations[jc.Joint].StartJoint].AbsoluteRotation.Matrix);
Vector3 constraintDirWs = jc.Dir;
constraintDirWs.Normalize();
Vector3 rotatedConstraintDirWs = Vector3.Transform(constraintDirWs, matConstraintLocalToWorld);
rotatedConstraintDirWs.Normalize();
// Draw the constraint direction line itself
rotatedConstraintDirWs *= 0.5f;
this.AddRotatedLine(rotatedConstraintDirWs, skeleton, i, Color.DeepPink);
// Get the bone direction in world space
Vector3 boneDirWs = KinectHelper.VectorBetween(
skeleton,
skeleton.BoneOrientations[jc.Joint].StartJoint,
skeleton.BoneOrientations[jc.Joint].EndJoint);
boneDirWs.Normalize();
Quaternion constraintRotation = KinectHelper.GetShortestRotationBetweenVectors(constraintDirWs, Vector3.Up);
for (int c = 0; c < Tesselation; c++)
{
Vector3 circlePoint = GetCircleVector(c, Tesselation);
// Calculate distance based on known radius (opposite side) of 1 and angle
circlePoint.Y = 1.0f / (float)Math.Tan(MathHelper.ToRadians(jc.Angle));
circlePoint.Normalize();
Vector3 tranCirclePoint = Vector3.Transform(circlePoint, constraintRotation);
// now transform this by the parent rotation
Vector3 rotatedConstraintConePointWs = Vector3.Transform(tranCirclePoint, matConstraintLocalToWorld);
rotatedConstraintConePointWs *= LineScale; // scale
this.AddRotatedLine(rotatedConstraintConePointWs, skeleton, i, Color.HotPink);
}
}
}
/// <summary>
/// ApplyBoneOrientationConstraints and constrain rotations.
/// </summary>
/// <param name="skeleton">The skeleton to correct.</param>
/// <param name="mirrorView">Set this true if the skeleton joints are mirrored.</param>
public void Constrain(Skeleton skeleton, bool mirrorView)
{
if (null == skeleton || skeleton.TrackingState != SkeletonTrackingState.Tracked)
{
return;
}
if (this.jointConstraints.Count == 0)
{
this.AddDefaultConstraints();
}
if (mirrorView != this.constraintsMirrored)
{
this.MirrorConstraints();
}
// Constraints are defined as a vector with respect to the parent bone vector, and a constraint angle,
// which is the maximum angle with respect to the constraint axis that the bone can move through.
// Calculate constraint values. 0.0-1.0 means the bone is within the constraint cone. Greater than 1.0 means
// it lies outside the constraint cone.
for (int i = 0; i < this.jointConstraints.Count; i++)
{
BoneOrientationConstraint jc = this.jointConstraints[i];
if (skeleton.Joints[jc.Joint].TrackingState == JointTrackingState.NotTracked || jc.Joint == JointType.HipCenter)
{
// End joint is not tracked or Hip Center has no parent to perform this calculation with.
continue;
}
// If the joint has a parent, constrain the bone direction to be within the constraint cone
JointType parentIdx = skeleton.BoneOrientations[jc.Joint].StartJoint;
// Local bone orientation relative to parent
Matrix boneOrientationRelativeToParent = KinectHelper.Matrix4ToXNAMatrix(skeleton.BoneOrientations[jc.Joint].HierarchicalRotation.Matrix);
Quaternion boneOrientationRelativeToParentQuat = KinectHelper.Vector4ToXNAQuaternion(skeleton.BoneOrientations[jc.Joint].HierarchicalRotation.Quaternion);
// Local bone direction is +Y vector in parent coordinate system
Vector3 boneRelDirVecLs = new Vector3(boneOrientationRelativeToParent.M21, boneOrientationRelativeToParent.M22, boneOrientationRelativeToParent.M23);
boneRelDirVecLs.Normalize();
// Constraint is relative to the parent orientation, which is +Y/identity relative rotation
Vector3 constraintDirLs = jc.Dir;
constraintDirLs.Normalize();
// Test this against the vector of the bone to find angle
float boneDotConstraint = Vector3.Dot(boneRelDirVecLs, constraintDirLs);
// Calculate the constraint value. 0.0 is in the center of the constraint cone, 1.0 and above are outside the cone.
jc.Constraint = (float)Math.Acos(boneDotConstraint) / MathHelper.ToRadians(jc.Angle);
this.jointConstraints[i] = jc;
// Slerp between identity and the inverse of the current constraint rotation by the amount over the constraint amount
if (jc.Constraint > 1)
{
Quaternion inverseRotation = Quaternion.Inverse(boneOrientationRelativeToParentQuat);
Quaternion slerpedInverseRotation = Quaternion.Slerp(Quaternion.Identity, inverseRotation, jc.Constraint - 1);
Quaternion constrainedRotation = boneOrientationRelativeToParentQuat * slerpedInverseRotation;
// Put it back into the bone orientations
skeleton.BoneOrientations[jc.Joint].HierarchicalRotation.Quaternion = KinectHelper.XNAQuaternionToVector4(constrainedRotation);
skeleton.BoneOrientations[jc.Joint].HierarchicalRotation.Matrix = KinectHelper.XNAMatrixToMatrix4(Matrix.CreateFromQuaternion(constrainedRotation));
}
}
// Recalculate the absolute rotations from the hierarchical relative rotations
Array jointTypeValues = Enum.GetValues(typeof(JointType));
foreach (JointType j in jointTypeValues)
{
if (j != JointType.HipCenter)
{
JointType parentIdx = skeleton.BoneOrientations[j].StartJoint;
// Calculate the absolute/world equivalents of the hierarchical rotation
Quaternion parentRotation = KinectHelper.Vector4ToXNAQuaternion(skeleton.BoneOrientations[parentIdx].AbsoluteRotation.Quaternion);
Quaternion relativeRotation = KinectHelper.Vector4ToXNAQuaternion(skeleton.BoneOrientations[j].HierarchicalRotation.Quaternion);
// Create a new world rotation
Quaternion worldRotation = Quaternion.Multiply(parentRotation, relativeRotation);
skeleton.BoneOrientations[j].AbsoluteRotation.Quaternion = KinectHelper.XNAQuaternionToVector4(worldRotation);
skeleton.BoneOrientations[j].AbsoluteRotation.Matrix = KinectHelper.XNAMatrixToMatrix4(Matrix.CreateFromQuaternion(worldRotation));
}
}
}
