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