void updateSkeleton() { float width = 0.025f; for (int j = 0; j < spheres.Length; j++) { if (ZEDSupportFunctions.IsVector3NaN(joint[sphereList[j]])) { spheres[j].transform.position = Vector3.zero; spheres[j].SetActive(false); } else { spheres[j].transform.position = joint[sphereList[j]]; spheres[j].SetActive(true); } } for (int i = 0; i < bones.Length; i++) { Vector3 start = spheres[Array.IndexOf(sphereList, bonesList[2 * i])].transform.position; Vector3 end = spheres[Array.IndexOf(sphereList, bonesList[2 * i + 1])].transform.position; if (start == Vector3.zero || end == Vector3.zero) { bones[i].SetActive(false); continue; } bones[i].SetActive(true); Vector3 offset = end - start; Vector3 scale = new Vector3(width, offset.magnitude / 2.0f, width); Vector3 position = start + (offset / 2.0f); bones[i].transform.position = position; bones[i].transform.up = offset; bones[i].transform.localScale = scale; } }
/// <summary> /// For Debug only. Set the joint position as sphere. /// </summary> /// <param name="jt">Jt.</param> public void setJointSpherePoint(Vector3[] jt) { if (sphere.Count != 18) { for (int i = 0; i < jointCount; i++) { sphere.Add(GameObject.CreatePrimitive(PrimitiveType.Sphere)); } } for (int i = 0; i < jointCount; i++) { if (ZEDSupportFunctions.IsVector3NaN(joint[i])) { continue; } joint[i] = new Vector3(jt[i].x, jt[i].y, jt[i].z); sphere[i].transform.localScale = new Vector3(0.05f, 0.05f, 0.05f); sphere[i].transform.position = joint[i]; } }
/// <summary> /// Function that handles the humanoid position, rotation and bones movement /// </summary> /// <param name="position_center">Position center.</param> private void setHumanPoseControl(Vector3 position_center) { Vector3 waist; Quaternion waistrot = oldwaistrot; Quaternion inv_waistrot = Quaternion.Inverse(waistrot); if (!ZEDSupportFunctions.IsVector3NaN(joint[JointType_HipRight]) && !ZEDSupportFunctions.IsVector3NaN(joint[JointType_HipLeft])) { waist = joint[JointType_HipRight] - joint[JointType_HipLeft]; waist = new Vector3(waist.x, 0, waist.z); waistrot = Quaternion.FromToRotation(Vector3.right, waist); inv_waistrot = Quaternion.Inverse(waistrot); } Vector3 shoulder; Quaternion shoulderrot = oldshoulderrot; Quaternion inv_shoulderrot = Quaternion.Inverse(waistrot); if (!ZEDSupportFunctions.IsVector3NaN(joint[JointType_ShoulderRight]) && !ZEDSupportFunctions.IsVector3NaN(joint[JointType_ShoulderLeft])) { shoulder = joint[JointType_ShoulderRight] - joint[JointType_ShoulderLeft]; shoulder = new Vector3(shoulder.x, 0, shoulder.z); shoulderrot = Quaternion.FromToRotation(Vector3.right, shoulder); inv_shoulderrot = Quaternion.Inverse(shoulderrot); } if (Quaternion.Angle(waistrot, shoulderrot) > 45 || Quaternion.Angle(waistrot, shoulderrot) < -45) { shoulderrot = oldshoulderrot; } for (int i = 0; i < targetBone.Length; i++) { int s = jointSegment[2 * i], e = jointSegment[2 * i + 1]; if (!ZEDSupportFunctions.IsVector3NaN(joint[e]) && !ZEDSupportFunctions.IsVector3NaN(joint[s])) { trackingSegment[targetBone[i]] = (joint[e] - joint[s]).normalized; } } foreach (HumanBodyBones bone in targetBone) { rigBoneTarget[bone] = waistrot * Quaternion.identity; } Vector3 eyesVector = (joint[JointType_EyesLeft] + joint[JointType_HearLeft]) / 2 - (joint[JointType_EyesRight] + joint[JointType_HearRight]) / 2; Vector3 headVector = joint[JointType_Head] - joint[JointType_Neck]; Vector3 headOrientationVector = Vector3.Cross(headVector, eyesVector); if (headOrientationVector != Vector3.zero && headVector != Vector3.zero && !ZEDSupportFunctions.IsVector3NaN(headOrientationVector) && !ZEDSupportFunctions.IsVector3NaN(headVector)) { rigBoneTarget[HumanBodyBones.Neck] = Quaternion.LookRotation(headOrientationVector, headVector); } else { rigBoneTarget[HumanBodyBones.Neck] = Quaternion.FromToRotation(shoulderrot * Vector3.up, trackingSegment[HumanBodyBones.Spine]) * shoulderrot; } rigBoneTarget[HumanBodyBones.Spine] = Quaternion.FromToRotation(waistrot * Vector3.up, trackingSegment[HumanBodyBones.Spine]) * waistrot; rigBoneTarget[HumanBodyBones.LeftUpperArm] = Quaternion.FromToRotation(shoulderrot * Vector3.left, trackingSegment[HumanBodyBones.LeftUpperArm]) * shoulderrot; rigBoneTarget[HumanBodyBones.LeftLowerArm] = Quaternion.FromToRotation(shoulderrot * Vector3.left, trackingSegment[HumanBodyBones.LeftLowerArm]) * shoulderrot; rigBoneTarget[HumanBodyBones.RightUpperArm] = Quaternion.FromToRotation(shoulderrot * Vector3.right, trackingSegment[HumanBodyBones.RightUpperArm]) * shoulderrot; rigBoneTarget[HumanBodyBones.RightLowerArm] = Quaternion.FromToRotation(shoulderrot * Vector3.right, trackingSegment[HumanBodyBones.RightLowerArm]) * shoulderrot; rigBoneTarget[HumanBodyBones.LeftUpperLeg] = Quaternion.FromToRotation(waistrot * Vector3.down, trackingSegment[HumanBodyBones.LeftUpperLeg]) * waistrot; rigBoneTarget[HumanBodyBones.LeftLowerLeg] = Quaternion.FromToRotation(waistrot * Vector3.down, trackingSegment[HumanBodyBones.LeftLowerLeg]) * waistrot; rigBoneTarget[HumanBodyBones.RightUpperLeg] = Quaternion.FromToRotation(waistrot * Vector3.down, trackingSegment[HumanBodyBones.RightUpperLeg]) * waistrot; rigBoneTarget[HumanBodyBones.RightLowerLeg] = Quaternion.FromToRotation(waistrot * Vector3.down, trackingSegment[HumanBodyBones.RightLowerLeg]) * waistrot; rigBone[HumanBodyBones.UpperChest].offset(inv_waistrot * shoulderrot); targetBodyOrientation = waistrot; targetBodyPosition = new Vector3(position_center.x, position_center.y, position_center.z); oldshoulderrot = shoulderrot; oldwaistrot = waistrot; }