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>
/// Transforms the local joints from the given coordinate system to
/// world space.
/// </summary>
/// <param name="src">The source buffer containing the joints in local space.</param>
/// <param name="dst">The destination buffer in which the transformed joints will be written.</param>
/// <param name="origin">The origin of the coordinate system.</param>
public static void TransformJointData(KinectJoint[] src, KinectJoint[] dst, Transform origin)
{
for (int i = 0; i < src.Length; ++i)
{
var position = origin.TransformPoint(src[i].position);
var rotation = origin.rotation * src[i].rotation;
dst[i] = new KinectJoint(position, rotation, src[i].trackingState);
}
}
internal void OnUpdateTrackingData(KinectManager manager, Body body, FaceFrameResult face, long frame)
{
this.updatedAtFrame = frame;
this.m_BodyFrame.RefreshFrameData(body, face, manager.floorClipPlane);
KinectJoint.TransformJointData(this.m_BodyFrame.joints, this.m_Joints, manager.transform);
RecalculatePositionAndBounds();
this.onTrackingDataUpdated?.Invoke();
}
/// <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);
}