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)));
}
// 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);
}
}
/// <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;
}
}