private static void CopyRotation(Microsoft.Azure.Kinect.BodyTracking.JointId A_id, Microsoft.Azure.Kinect.BodyTracking.JointId B_id, GameObject RootPosition)
{
GameObject Abone = GetRocketboxBone(A_id, RootPosition);
GameObject Bbone = GetRocketboxBone(B_id, RootPosition);
if (Abone != null && Bbone != null)
{
Bbone.transform.rotation = Abone.transform.rotation;
}
}
public static GameObject GetRocketboxBone(Microsoft.Azure.Kinect.BodyTracking.JointId joinId, GameObject RootPosition)
{
switch (joinId)
{
case Microsoft.Azure.Kinect.BodyTracking.JointId.Pelvis: return(RootPosition);
case Microsoft.Azure.Kinect.BodyTracking.JointId.SpineNavel: return(RootPosition.transform.Find("Bip01 Spine").gameObject);
case Microsoft.Azure.Kinect.BodyTracking.JointId.SpineChest: return(RootPosition.transform.Find("Bip01 Spine").Find("Bip01 Spine1").gameObject);
case Microsoft.Azure.Kinect.BodyTracking.JointId.Neck: return(RootPosition.transform.Find("Bip01 Spine").Find("Bip01 Spine1").Find("Bip01 Spine2").Find("Bip01 Neck").gameObject);
case Microsoft.Azure.Kinect.BodyTracking.JointId.Head: return(RootPosition.transform.Find("Bip01 Spine").Find("Bip01 Spine1").Find("Bip01 Spine2").Find("Bip01 Neck").Find("Bip01 Head").gameObject);
case Microsoft.Azure.Kinect.BodyTracking.JointId.HipLeft: return(RootPosition.transform.Find("Bip01 L Thigh").gameObject);
case Microsoft.Azure.Kinect.BodyTracking.JointId.KneeLeft: return(RootPosition.transform.Find("Bip01 L Thigh").Find("Bip01 L Calf").gameObject);
case Microsoft.Azure.Kinect.BodyTracking.JointId.AnkleLeft: return(RootPosition.transform.Find("Bip01 L Thigh").Find("Bip01 L Calf").gameObject);
case Microsoft.Azure.Kinect.BodyTracking.JointId.FootLeft: return(RootPosition.transform.Find("Bip01 L Thigh").Find("Bip01 L Calf").Find("Bip01 L Foot").gameObject);
case Microsoft.Azure.Kinect.BodyTracking.JointId.HipRight: return(RootPosition.transform.Find("Bip01 R Thigh").gameObject);
case Microsoft.Azure.Kinect.BodyTracking.JointId.KneeRight: return(RootPosition.transform.Find("Bip01 R Thigh").Find("Bip01 R Calf").gameObject);
case Microsoft.Azure.Kinect.BodyTracking.JointId.AnkleRight: return(RootPosition.transform.Find("Bip01 R Thigh").Find("Bip01 R Calf").gameObject);
case Microsoft.Azure.Kinect.BodyTracking.JointId.FootRight: return(RootPosition.transform.Find("Bip01 R Thigh").Find("Bip01 R Calf").Find("Bip01 R Foot").gameObject);
case Microsoft.Azure.Kinect.BodyTracking.JointId.ClavicleLeft: return(RootPosition.transform.Find("Bip01 Spine").Find("Bip01 Spine1").Find("Bip01 Spine2").Find("Bip01 L Clavicle").gameObject);
case Microsoft.Azure.Kinect.BodyTracking.JointId.ShoulderLeft: return(RootPosition.transform.Find("Bip01 Spine").Find("Bip01 Spine1").Find("Bip01 Spine2").Find("Bip01 L Clavicle").Find("Bip01 L UpperArm").gameObject);
case Microsoft.Azure.Kinect.BodyTracking.JointId.ElbowLeft: return(RootPosition.transform.Find("Bip01 Spine").Find("Bip01 Spine1").Find("Bip01 Spine2").Find("Bip01 L Clavicle").Find("Bip01 L UpperArm").Find("Bip01 L Forearm").gameObject);
case Microsoft.Azure.Kinect.BodyTracking.JointId.WristLeft: return(RootPosition.transform.Find("Bip01 Spine").Find("Bip01 Spine1").Find("Bip01 Spine2").Find("Bip01 L Clavicle").Find("Bip01 L UpperArm").Find("Bip01 L Forearm").Find("Bip01 L Hand").gameObject);
case Microsoft.Azure.Kinect.BodyTracking.JointId.ClavicleRight: return(RootPosition.transform.Find("Bip01 Spine").Find("Bip01 Spine1").Find("Bip01 Spine2").Find("Bip01 R Clavicle").gameObject);
case Microsoft.Azure.Kinect.BodyTracking.JointId.ShoulderRight: return(RootPosition.transform.Find("Bip01 Spine").Find("Bip01 Spine1").Find("Bip01 Spine2").Find("Bip01 R Clavicle").Find("Bip01 R UpperArm").gameObject);
case Microsoft.Azure.Kinect.BodyTracking.JointId.ElbowRight: return(RootPosition.transform.Find("Bip01 Spine").Find("Bip01 Spine1").Find("Bip01 Spine2").Find("Bip01 R Clavicle").Find("Bip01 R UpperArm").Find("Bip01 R Forearm").gameObject);
case Microsoft.Azure.Kinect.BodyTracking.JointId.WristRight: return(RootPosition.transform.Find("Bip01 Spine").Find("Bip01 Spine1").Find("Bip01 Spine2").Find("Bip01 R Clavicle").Find("Bip01 R UpperArm").Find("Bip01 R Forearm").Find("Bip01 R Hand").gameObject);
default: return(null);
}
}
public Quaternion GetRelativeJointRotation(Microsoft.Azure.Kinect.BodyTracking.JointId joint)
{
Microsoft.Azure.Kinect.BodyTracking.JointId parent = parentJointMap[joint];
Quaternion parentJointRotationBodySpace = Quaternion.identity;
if (parent == Microsoft.Azure.Kinect.BodyTracking.JointId.Count)
{
parentJointRotationBodySpace = Y_180_FLIP;
}
else
{
parentJointRotationBodySpace = absoluteJointRotations[(int)parent];
}
Quaternion jointRotationBodySpace = absoluteJointRotations[(int)joint];
Quaternion relativeRotation = Quaternion.Inverse(parentJointRotationBodySpace) * jointRotationBodySpace;
return(relativeRotation);
}
public void renderSkeleton(Body skeleton, int skeletonNumber)
{
int total_joins = (int)Microsoft.Azure.Kinect.BodyTracking.JointId.Count;
if (isKinect2)
{
total_joins = (int)Windows.Kinect.JointType.Count;
}
for (int joint = 0; joint < total_joins; joint++)
{
if (isKinect2 && (joint == 22 || joint == 24))
{
// The thumb joints were breaking this function for the kinect 2. Ignore them for now.
continue;
}
Microsoft.Azure.Kinect.BodyTracking.JointId jointNum = (Microsoft.Azure.Kinect.BodyTracking.JointId)joint;
if (isKinect2)
{
jointNum = Body.FromKinect2ToAzure((Windows.Kinect.JointType)joint);
}
Vector3 jointPos;
if (isKinect2)// Note: Kinect2 X & Y axis are actually opposite from K4A. Make -x and +y for compatibility
{
jointPos = new Vector3(-skeleton.JointPositions3D[joint].X, skeleton.JointPositions3D[joint].Y, skeleton.JointPositions3D[joint].Z);
}
else
{
jointPos = new Vector3(skeleton.JointPositions3D[(int)jointNum].X, -skeleton.JointPositions3D[(int)jointNum].Y, skeleton.JointPositions3D[(int)jointNum].Z);
}
Vector3 offsetPosition = transform.rotation * jointPos;
Vector3 positionInTrackerRootSpace = transform.position + offsetPosition;
Quaternion jointRot = new Quaternion();
if (isKinect2)
{
jointRot = Y_180_FLIP * new Quaternion(skeleton.JointRotations[joint].X, skeleton.JointRotations[joint].Y,
skeleton.JointRotations[joint].Z, skeleton.JointRotations[joint].W) * Quaternion.Inverse(basisJointMapK2[(Windows.Kinect.JointType)joint]);
}
else
{
jointRot = Y_180_FLIP * new Quaternion(skeleton.JointRotations[(int)jointNum].X, skeleton.JointRotations[(int)jointNum].Y,
skeleton.JointRotations[(int)jointNum].Z, skeleton.JointRotations[(int)jointNum].W) * Quaternion.Inverse(basisJointMap[jointNum]);
}
if (jointNum != Microsoft.Azure.Kinect.BodyTracking.JointId.Count)
{
absoluteJointRotations[(int)jointNum] = jointRot;
// these are absolute body space because each joint has the body root for a parent in the scene graph
transform.GetChild(skeletonNumber).GetChild((int)jointNum).localPosition = jointPos;
transform.GetChild(skeletonNumber).GetChild((int)jointNum).localRotation = jointRot;
const int boneChildNum = 0;
if (parentJointMap[(Microsoft.Azure.Kinect.BodyTracking.JointId)jointNum] != Microsoft.Azure.Kinect.BodyTracking.JointId.Head && parentJointMap[(Microsoft.Azure.Kinect.BodyTracking.JointId)jointNum] != Microsoft.Azure.Kinect.BodyTracking.JointId.Count)
{
Vector3 parentTrackerSpacePosition;
if (isKinect2)
{
parentTrackerSpacePosition = new Vector3(-skeleton.JointPositions3D[(int)parentJointMapK2[(Windows.Kinect.JointType)joint]].X,
skeleton.JointPositions3D[(int)parentJointMapK2[(Windows.Kinect.JointType)joint]].Y, skeleton.JointPositions3D[(int)parentJointMapK2[(Windows.Kinect.JointType)joint]].Z);
}
else
{
parentTrackerSpacePosition = new Vector3(skeleton.JointPositions3D[(int)parentJointMap[jointNum]].X,
-skeleton.JointPositions3D[(int)parentJointMap[jointNum]].Y, skeleton.JointPositions3D[(int)parentJointMap[jointNum]].Z);
}
Vector3 boneDirectionTrackerSpace = jointPos - parentTrackerSpacePosition;
Vector3 boneDirectionWorldSpace = transform.rotation * boneDirectionTrackerSpace;
Vector3 boneDirectionLocalSpace = Quaternion.Inverse(transform.GetChild(skeletonNumber).GetChild((int)jointNum).rotation) * Vector3.Normalize(boneDirectionWorldSpace);
transform.GetChild(skeletonNumber).GetChild((int)jointNum).GetChild(boneChildNum).localScale = new Vector3(1, 20.0f * 0.5f * boneDirectionWorldSpace.magnitude, 1);
transform.GetChild(skeletonNumber).GetChild((int)jointNum).GetChild(boneChildNum).localRotation = Quaternion.FromToRotation(Vector3.up, boneDirectionLocalSpace);
transform.GetChild(skeletonNumber).GetChild((int)jointNum).GetChild(boneChildNum).position = transform.GetChild(skeletonNumber).GetChild((int)jointNum).position - 0.5f * boneDirectionWorldSpace;
}
else
{
transform.GetChild(skeletonNumber).GetChild((int)jointNum).GetChild(boneChildNum).gameObject.SetActive(false);
}
}
}
}
