/// <summary>
/// override base Init function
/// </summary>
new internal void Init()
{
base.Init();
if (this.jointSmoother == null)
{
this.jointSmoother = new DoubleExponentialFilter();
this.smoothingParams = jointSmoother.SmoothingParameters;
this.filteredJoint = new DoubleExponentialFilter.Joint();
}
}
private void UpdateJoints(Kinect.Body body)
{
if (body == null)
{
return;
}
DoubleExponentialFilter.TRANSFORM_SMOOTH_PARAMETERS smoothingParams = jointSmoother.SmoothingParameters;
// get the floorClipPlace from the body information
Vector4 floorClipPlane = Helpers.FloorClipPlane;
// get rotation of camera
Quaternion cameraRotation = Helpers.CalculateFloorRotationCorrection(floorClipPlane);
// generate a vertical offset from floor plane
Vector3 floorOffset = cameraRotation * Vector3.up * floorClipPlane.w;
foreach (Kinect.JointType jt in Enum.GetValues(typeof(Kinect.JointType)))
{
// If inferred, we smooth a bit more by using a bigger jitter radius
Windows.Kinect.Joint kinectJoint = body.Joints[jt];
if (kinectJoint.TrackingState == Kinect.TrackingState.Inferred)
{
smoothingParams.fJitterRadius *= 2.0f;
smoothingParams.fMaxDeviationRadius *= 2.0f;
}
Joint parent = this.Joints[jt].Parent;
// set initial joint value from Kinect
Vector3 rawPosition = cameraRotation * ConvertJointPositionToUnityVector3(body, jt) + floorOffset;
Quaternion rawRotation = cameraRotation * ConvertJointQuaternionToUnityQuaterion(body, jt);
if (Helpers.QuaternionZero.Equals(rawRotation) && parent != null)
{
Vector3 direction = rawPosition - parent.RawPosition;
Vector3 perpendicular = Vector3.Cross(direction, Vector3.up);
Vector3 normal = Vector3.Cross(perpendicular, direction);
// calculate a rotation, Y forward for Kinect
if (normal.magnitude != 0)
{
rawRotation = Quaternion.LookRotation(normal, direction);
}
else
{
rawRotation = Quaternion.identity;
}
}
DoubleExponentialFilter.Joint fj
= new DoubleExponentialFilter.Joint(rawPosition, rawRotation);
fj = jointSmoother.UpdateJoint(jt, fj, smoothingParams);
// set the raw joint value for the node
this.Joints[jt].SetRawtData(fj.Position, fj.Rotation);
}
//Vector3 offsetPosition = this.Joints[Kinect.JointType.SpineBase].RawPosition - floorOffset;
Vector3 offsetPosition = Vector3.zero;
Quaternion offsetRotation = Quaternion.identity;
// calculate the relative joint and rotation
this.Joints[Kinect.JointType.SpineBase].CalculateOffset(offsetPosition, offsetRotation);
}
private void UpdateJoint(Joint joint, Kinect.Body body, Vector3 cameraPosition, Quaternion cameraRotation)
{
if (joint == null || body == null)
{
return;
}
Kinect.JointType?jt = Helpers.ParseEnum <Kinect.JointType>(joint.Name);
// get Kinects raw value and filter it
if (jt != null)
{
// If inferred, we smooth a bit more by using a bigger jitter radius
Windows.Kinect.Joint kinectJoint = body.Joints[jt.Value];
if (kinectJoint.TrackingState == Kinect.TrackingState.Inferred)
{
this.smoothingParams.fJitterRadius *= 2.0f;
this.smoothingParams.fMaxDeviationRadius *= 2.0f;
}
// get the initial joint value from Kinect, correct for camera
Vector3 rawPosition = ConvertJointPositionToUnityVector3(body, jt.Value, this.MirrorJoints);
Quaternion rawRotation = ConvertJointQuaternionToUnityQuaterion(body, jt.Value, this.MirrorJoints);
// visualize the unadjusted joint information
// turn on Gizmos if you want to see these in Game mode
// Helpers.DrawDebugQuaternion(rawPosition, rawRotation, Helpers.ColorRange.BW, .05f);
// adjust for camera
rawPosition = cameraRotation * rawPosition;
rawRotation = cameraRotation * rawRotation;
// if this is a leaf bone, with no orientation
if (Helpers.QuaternionZero.Equals(rawRotation) && joint.Parent != null)
{
Vector3 direction = rawPosition - joint.Parent.RawPosition;
Vector3 perpendicular = Vector3.Cross(direction, Vector3.up);
Vector3 normal = Vector3.Cross(perpendicular, direction);
// calculate a rotation, Y forward for Kinect
if (normal.magnitude != 0)
{
rawRotation = Quaternion.LookRotation(normal, direction);
}
else
{
rawRotation = Quaternion.identity;
}
}
// set the filtered joint property with the updates values
this.filteredJoint.Position = rawPosition;
this.filteredJoint.Rotation = rawRotation;
// filter the raw joint
this.filteredJoint = jointSmoother.UpdateJoint(jt.Value, this.filteredJoint, smoothingParams);
// set the raw joint value for the node
joint.SetRawData(this.filteredJoint.Position, this.filteredJoint.Rotation);
}
// calculate offsets from world position/rotation
joint.CalculateOffset(cameraPosition, cameraRotation);
// continue through hierarchy
if (joint.Children != null)
{
foreach (var child in joint.Children)
{
UpdateJoint(child, body, cameraPosition, cameraRotation);
}
}
}