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