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