private void setInitRot()
{
initCorePose = jointRotations.rotateCore(new float[] { 0, 0, 0 },
new float[] { 0, 0, 0 }, refCoord.localRotation);
initHeadPose = head.rotation;
initState = InitState.INIT;
StartCoroutine(setPoses());
}
private void setInitRot()
{
initCorePose = jointRotations.rotateCore(new float[] { 0, 0, 0 },
new float[] { 0, 0, 0 }, refCoord.localRotation);
//set core rotation to get heading right
//core.localRotation = initCorePose;
initHeadPose = head.rotation;
}
private void animateUpper(float[] angles)
{
if (!initUpper)
{
initCore = new float[] { angles[1], angles[2], angles[3] };
initCorePose = jointRotations.rotateCore(initCore,
new float[] { 0, 0, 0 }, hmd);
}
//core node 1
float[] coreAngles = new float[] { angles[1], angles[2], angles[3] };
chainUpper = jointRotations.rotateCore(coreAngles, initCore, hmd);
core.localRotation = chainUpper;
float[] luAngles = new float[] { angles[5], angles[6], angles[7] };
chainUpper = jointRotations.rotateLeftArm(luAngles, core.localRotation,
hmd);
leftUpper.localRotation = chainUpper;
float[] lfAngles = new float[] { angles[9], angles[10], angles[11] };
chainUpper = jointRotations.rotateLeftForearm(lfAngles, core.localRotation,
leftUpper.localRotation, hmd);
leftFore.localRotation = chainUpper;
float[] ruAngles = new float[] { angles[13], angles[14], angles[15] };
chainUpper = jointRotations.rotateRightArm(ruAngles, core.localRotation,
hmd);
rightUpper.localRotation = chainUpper;
float[] rfAngles = new float[] { angles[17], angles[18], angles[19] };
chainUpper = jointRotations.rotateRightForearm(rfAngles, core.localRotation,
rightUpper.localRotation, hmd);
rightFore.localRotation = chainUpper;
}
private void OnUpperBodyAnglesChanged(HumanoidAngles <Vector3> absoluteAngles)
{
var headsetRotation = Quaternion.identity;
// Pack absolute angles into arrays for calculations
var baseChestYaw = ChestBaseOrientation.z;
_baseChestAngles[0] = ChestBaseOrientation.x;
_baseChestAngles[1] = ChestBaseOrientation.y;
_baseChestAngles[2] = ChestBaseOrientation.z;
_yawAdjustedChestAngles[0] = absoluteAngles.Chest.x;
_yawAdjustedChestAngles[1] = absoluteAngles.Chest.y;
_yawAdjustedChestAngles[2] = absoluteAngles.Chest.z - baseChestYaw;
_yawAdjustedLeftUpperArmAngles[0] = absoluteAngles.LeftUpperArm.x;
_yawAdjustedLeftUpperArmAngles[1] = absoluteAngles.LeftUpperArm.y;
_yawAdjustedLeftUpperArmAngles[2] = absoluteAngles.LeftUpperArm.z - baseChestYaw;
_yawAdjustedLeftLowerArmAngles[0] = absoluteAngles.LeftLowerArm.x;
_yawAdjustedLeftLowerArmAngles[1] = absoluteAngles.LeftLowerArm.y;
_yawAdjustedLeftLowerArmAngles[2] = absoluteAngles.LeftLowerArm.z - baseChestYaw;
_yawAdjustedRightUpperArmAngles[0] = absoluteAngles.RightUpperArm.x;
_yawAdjustedRightUpperArmAngles[1] = absoluteAngles.RightUpperArm.y;
_yawAdjustedRightUpperArmAngles[2] = absoluteAngles.RightUpperArm.z - baseChestYaw;
_yawAdjustedRightLowerArmAngles[0] = absoluteAngles.RightLowerArm.x;
_yawAdjustedRightLowerArmAngles[1] = absoluteAngles.RightLowerArm.y;
_yawAdjustedRightLowerArmAngles[2] = absoluteAngles.RightLowerArm.z - baseChestYaw;
// Transform absolute upper body angles into local ones
var localAngleChest = _jointRotations.rotateCore(
_yawAdjustedChestAngles,
_baseChestAngles,
headsetRotation
);
var localAngleLeftUpperArm = _jointRotations.rotateLeftArm(
_yawAdjustedLeftUpperArmAngles,
localAngleChest,
headsetRotation
);
var localAngleLeftLowerArm = _jointRotations.rotateLeftForearm(
_yawAdjustedLeftLowerArmAngles,
localAngleChest,
localAngleLeftUpperArm,
headsetRotation
);
var localAngleRightUpperArm = _jointRotations.rotateRightArm(
_yawAdjustedRightUpperArmAngles,
localAngleChest,
headsetRotation
);
var localAngleRightLowerArm = _jointRotations.rotateRightForearm(
_yawAdjustedRightLowerArmAngles,
localAngleChest,
localAngleRightUpperArm,
headsetRotation
);
LocalAngles.SetUpperBodyAngles(localAngleChest, localAngleLeftUpperArm, localAngleLeftLowerArm,
localAngleRightUpperArm, localAngleRightLowerArm);
}