//interface method
public void operate(float[] angles)
{
//parse angles
//apply to upper
if (initState == InitState.PREINIT && angles != null)
{
//do initialization
Buffer.BlockCopy((float[])angles.Clone(), 1 * sizeof(float), initCore, 0, 3 * sizeof(float));
float initSum = initCore[0] + initCore[1] + initCore[2];
if (!Mathf.Approximately(initSum, 0))
{
setInitRot();
}
}
else if (initState == InitState.INIT && angles != null)
{
//core node 1
float[] coreAngles = new float[] { angles[1], angles[2], angles[3] - initCore[2] };
chain = jointRotations.rotateCore(coreAngles, initCore, refCoord.localRotation);
corePose.Enqueue(chain);
//Left Upper user node 2
//90 deg transform puts sensor in correct orientation
float[] luAngles = new float[] { angles[5], angles[6], angles[7] - initCore[2] };
chain = jointRotations.rotateLeftArm(luAngles, core.localRotation, refCoord.localRotation);
leftUpperPose.Enqueue(chain);
//Left Fore node 4
float[] lfAngles = new float[] { angles[9], angles[10], angles[11] - initCore[2] };
chain = jointRotations.rotateLeftForearm(lfAngles, core.localRotation,
leftUpper.localRotation, refCoord.localRotation);
leftForePose.Enqueue(chain);
//Right Upper node 3
float[] ruAngles = new float[] { angles[13], angles[14], angles[15] - initCore[2] };
chain = jointRotations.rotateRightArm(ruAngles, core.localRotation, refCoord.localRotation);
rightUpperPose.Enqueue(chain);
//Right Fore (Animation) Right Fore (User) node 5
float[] rfAngles = new float[] { angles[17], angles[18], angles[19] - initCore[2] };
chain = jointRotations.rotateRightForearm(rfAngles, core.localRotation,
rightUpper.localRotation, refCoord.localRotation);
rightForePose.Enqueue(chain);
}
}
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);
}