private void animateLower(float[] angles)
    {
        float[] waistAngles = new float[] { angles[1], angles[2], angles[3] };
        chainLower = jointRotations.rotateWaist(waistAngles, new float[] { 0, 0, 0 }, hmd);

        waist.localRotation = chainLower;

        float[] ltAngles = new float[] { angles[5], angles[6], angles[7] };
        chainLower = jointRotations.rotateLeftLeg(ltAngles, waist.localRotation,
                                                  hmd);

        leftThigh.localRotation = chainLower;

        float[] lsAngles = new float[] { angles[9], angles[10], angles[11] };
        chainLower = jointRotations.rotateLeftShin(lsAngles, waist.localRotation,
                                                   leftThigh.localRotation, hmd);

        leftShin.localRotation = chainLower;

        float[] rtAngles = new float[] { angles[13], angles[14], angles[15] };
        chainLower = jointRotations.rotateRightLeg(rtAngles, waist.localRotation,
                                                   hmd);

        rightThigh.localRotation = chainLower;

        float[] rsAngles = new float[] { angles[17], angles[18], angles[19] };
        chainLower = jointRotations.rotateRightShin(rsAngles, waist.localRotation,
                                                    rightThigh.localRotation, hmd);

        rightShin.localRotation = chainLower;
    }
Ejemplo n.º 2
0
        private void OnLowerBodyAnglesChanged(HumanoidAngles <Vector3> absoluteAngles)
        {
            var headsetRotation    = Quaternion.identity;
            var initialWaistYaw    = WaistBaseOrientation.z;
            var initialWaistAngles = Quaternion.AngleAxis(initialWaistYaw, Vector3.up);

            _yawAdjustedWaistAngles[0] = absoluteAngles.Waist.x;
            _yawAdjustedWaistAngles[1] = absoluteAngles.Waist.y;
            _yawAdjustedWaistAngles[2] = absoluteAngles.Waist.z;

            _yawAdjustedLeftUpperLegAngles[0] = absoluteAngles.LeftUpperLeg.x;
            _yawAdjustedLeftUpperLegAngles[1] = absoluteAngles.LeftUpperLeg.y;
            _yawAdjustedLeftUpperLegAngles[2] = absoluteAngles.LeftUpperLeg.z;

            _yawAdjustedLeftLowerLegAngles[0] = absoluteAngles.LeftLowerLeg.x;
            _yawAdjustedLeftLowerLegAngles[1] = absoluteAngles.LeftLowerLeg.y;
            _yawAdjustedLeftLowerLegAngles[2] = absoluteAngles.LeftLowerLeg.z;

            _yawAdjustedRightUpperLegAngles[0] = absoluteAngles.RightUpperLeg.x;
            _yawAdjustedRightUpperLegAngles[1] = absoluteAngles.RightUpperLeg.y;
            _yawAdjustedRightUpperLegAngles[2] = absoluteAngles.RightUpperLeg.z;

            _yawAdjustedRightLowerLegAngles[0] = absoluteAngles.RightLowerLeg.x;
            _yawAdjustedRightLowerLegAngles[1] = absoluteAngles.RightLowerLeg.y;
            _yawAdjustedRightLowerLegAngles[2] = absoluteAngles.RightLowerLeg.z;

            // Transform absolute lower body angles into relative ones
            var localAngleWaist = _jointRotations.rotateWaist(
                _yawAdjustedWaistAngles,
                initialWaistAngles,
                headsetRotation
                );
            var localAngleLeftUpperLeg = _jointRotations.rotateLeftLeg(
                _yawAdjustedLeftUpperLegAngles,
                localAngleWaist,
                initialWaistAngles
                );
            var localAngleLeftLowerLeg = _jointRotations.rotateLeftShin(
                _yawAdjustedLeftLowerLegAngles,
                localAngleWaist,
                localAngleLeftUpperLeg,
                initialWaistAngles
                );
            var localAngleRightUpperLeg = _jointRotations.rotateRightLeg(
                _yawAdjustedRightUpperLegAngles,
                localAngleWaist,
                initialWaistAngles
                );
            var localAngleRightLowerLeg = _jointRotations.rotateRightShin(
                _yawAdjustedRightLowerLegAngles,
                localAngleWaist,
                localAngleRightUpperLeg,
                initialWaistAngles
                );

            LocalAngles.SetLowerBodyAngles(localAngleWaist, localAngleLeftUpperLeg, localAngleLeftLowerLeg,
                                           localAngleRightUpperLeg, localAngleRightLowerLeg);
        }
    public void operate(float[] angles)
    {
        if (angles != null)
        {
            if (initState == InitState.PREINIT && angles != null)
            {
                if (!useCore)
                {
                    Buffer.BlockCopy((float[])angles.Clone(), 1 * sizeof(float), initWaist, 0, 3 * sizeof(float));
                }
                else
                {
                    initWaist = upperReference.getCoreInit();
                }

                setInitRot();
            }
            else if (initState == InitState.INIT)
            {
                if (!useCore)
                {
                    //core node 1
                    float[] waistAngles = new float[] { angles[1], angles[2], angles[3] - initWaist[2] };
                    chain = jointRotations.rotateWaist(waistAngles, initWaist, refCoord.localRotation);
                }
                else
                {
                    chain = core.localRotation;
                }

                waistPose.Enqueue(chain);

                //Left Thigh
                float[] ltAngles = new float[] { angles[5], angles[6], angles[7] - initWaist[2] };
                chain = jointRotations.rotateLeftLeg(ltAngles, waist.localRotation,
                                                     refCoord.localRotation);

                leftThighPose.Enqueue(chain);

                //Left shin
                float[] lsAngles = new float[] { angles[9], angles[10], angles[11] - initWaist[2] };
                chain = jointRotations.rotateLeftShin(lsAngles, waist.localRotation,
                                                      leftThigh.localRotation, refCoord.localRotation);

                leftShinPose.Enqueue(chain);

                //Right Thigh
                float[] rtAngles = new float[] { angles[13], angles[14], angles[15] - initWaist[2] };
                chain = jointRotations.rotateRightLeg(rtAngles, waist.localRotation,
                                                      refCoord.localRotation);

                rightThighPose.Enqueue(chain);

                //Right shin
                float[] rsAngles = new float[] { angles[17], angles[18], angles[19] - initWaist[2] };
                chain = jointRotations.rotateRightShin(rsAngles, waist.localRotation,
                                                       rightThigh.localRotation, refCoord.localRotation);

                rightShinPose.Enqueue(chain);
            }
        }
    }