static public int constructor(IntPtr l)
 {
     try {
                     #if DEBUG
         var    method     = System.Reflection.MethodBase.GetCurrentMethod();
         string methodName = GetMethodName(method);
                     #if UNITY_5_5_OR_NEWER
         UnityEngine.Profiling.Profiler.BeginSample(methodName);
                     #else
         Profiler.BeginSample(methodName);
                     #endif
                     #endif
         UnityEngine.Animations.AnimationStream o;
         o = new UnityEngine.Animations.AnimationStream();
         pushValue(l, true);
         pushValue(l, o);
         return(2);
     }
     catch (Exception e) {
         return(error(l, e));
     }
             #if DEBUG
     finally {
                     #if UNITY_5_5_OR_NEWER
         UnityEngine.Profiling.Profiler.EndSample();
                     #else
         Profiler.EndSample();
                     #endif
     }
             #endif
 }
Exemplo n.º 2
0
        // Detects linear singularity issue when the direction from first bone to IKPosition matches the direction from first bone to the last bone.
        private bool SingularityDetected(UnityEngine.Animations.AnimationStream s, Vector3 IKPosition)
        {
            Vector3 firstBonePos = bones[0].GetPosition(s);
            Vector3 toLastBone   = bones[bones.Length - 1].GetPosition(s) - firstBonePos;
            Vector3 toIKPosition = IKPosition - firstBonePos;

            float toLastBoneDistance   = toLastBone.sqrMagnitude;
            float toIKPositionDistance = toIKPosition.sqrMagnitude;

            if (toLastBoneDistance < toIKPositionDistance)
            {
                return(false);
            }
            if (toLastBoneDistance < chainSqrMag - (boneSqrMags[bones.Length - 2] * 0.1f))
            {
                return(false);
            }
            if (toLastBoneDistance == 0)
            {
                return(false);
            }
            if (toIKPositionDistance == 0)
            {
                return(false);
            }

            float dot = Vector3.Dot(toLastBone, toIKPosition);

            if (dot < 0.999f)
            {
                return(false);
            }

            return(true);
        }
Exemplo n.º 3
0
        private void Solve(UnityEngine.Animations.AnimationStream s, Vector3 targetPosition, bool XY, float weight, int it, bool useRotationLimits)
        {
            for (int i = bones.Length - 2; i > -1; i--)
            {
                //CCD tends to overemphasise the rotations of the bones closer to the target position. Reducing bone weight down the hierarchy will compensate for this effect.
                float w = weight * boneWeights[i].GetFloat(s);

                if (w > 0f)
                {
                    Vector3 bonePos    = bones[i].GetPosition(s);
                    Vector3 toLastBone = bones[bones.Length - 1].GetPosition(s) - bonePos;
                    Vector3 toTarget   = targetPosition - bonePos;

                    // Get the rotation to direct the last bone to the target
                    if (XY)
                    {
                        float angleToLastBone = Mathf.Atan2(toLastBone.x, toLastBone.y) * Mathf.Rad2Deg;
                        float angleToTarget   = Mathf.Atan2(toTarget.x, toTarget.y) * Mathf.Rad2Deg;

                        // Rotation to direct the last bone to the target
                        bones[i].SetRotation(s, Quaternion.AngleAxis(Mathf.DeltaAngle(angleToLastBone, angleToTarget) * w, Vector3.back) * bones[i].GetRotation(s));
                    }
                    else
                    {
                        Quaternion boneRot        = bones[i].GetRotation(s);
                        Quaternion targetRotation = Quaternion.FromToRotation(toLastBone, toTarget) * boneRot;

                        if (w >= 1)
                        {
                            bones[i].SetRotation(s, targetRotation);
                        }
                        else
                        {
                            bones[i].SetRotation(s, Quaternion.Lerp(boneRot, targetRotation, w));
                        }
                    }
                }

                // Rotation Constraints
                if (useRotationLimits)
                {
                    if (hingeFlags[i] == 1)
                    {
                        Quaternion localRotation = Quaternion.Inverse(limitDefaultLocalRotationArray[i]) * bones[i].GetLocalRotation(s);
                        Quaternion lastRotation  = hingeLastRotations[i];
                        float      lastAngle     = hingeLastAngles[i];
                        Quaternion r             = RotationLimitUtilities.LimitHinge(localRotation, hingeMinArray[i].GetFloat(s), hingeMaxArray[i].GetFloat(s), hingeUseLimitsArray[i].GetBool(s), limitAxisArray[i], ref lastRotation, ref lastAngle);
                        hingeLastRotations[i] = lastRotation;
                        hingeLastAngles[i]    = lastAngle;
                        bones[i].SetLocalRotation(s, limitDefaultLocalRotationArray[i] * r);
                    }
                    else if (angleFlags[i] == 1)
                    {
                        Quaternion localRotation = Quaternion.Inverse(limitDefaultLocalRotationArray[i]) * bones[i].GetLocalRotation(s);
                        Quaternion r             = RotationLimitUtilities.LimitAngle(localRotation, limitAxisArray[i], angleSecondaryAxisArray[i], angleLimitArray[i].GetFloat(s), angleTwistLimitArray[i].GetFloat(s));
                        bones[i].SetLocalRotation(s, limitDefaultLocalRotationArray[i] * r);
                    }
                }
            }
        }
Exemplo n.º 4
0
        //Clamping the IKPosition to legal range
        private Vector3 GetClampedIKPosition(UnityEngine.Animations.AnimationStream s, float clampWeight, int clampSmoothing, Vector3 IKPosition, Vector3 transformPosition, Vector3 transformAxis)
        {
            if (clampWeight <= 0f)
            {
                return(IKPosition);
            }

            if (clampWeight >= 1f)
            {
                return(transformPosition + transformAxis * (IKPosition - transformPosition).magnitude);
            }

            // Getting the dot product of IK direction and transformAxis
            float angle = Vector3.Angle(transformAxis, (IKPosition - transformPosition));
            float dot   = 1f - (angle / 180f);

            // Clamping the target
            float targetClampMlp = clampWeight > 0 ? Mathf.Clamp(1f - ((clampWeight - dot) / (1f - dot)), 0f, 1f) : 1f;

            // Calculating the clamp multiplier
            float clampMlp = clampWeight > 0 ? Mathf.Clamp(dot / clampWeight, 0f, 1f) : 1f;

            for (int i = 0; i < clampSmoothing; i++)
            {
                float sinF = clampMlp * Mathf.PI * 0.5f;
                clampMlp = Mathf.Sin(sinF);
            }

            // Slerping the IK direction (don't use Lerp here, it breaks it)
            return(transformPosition + Vector3.Slerp(transformAxis * 10f, IKPosition - transformPosition, clampMlp * targetClampMlp));
        }
Exemplo n.º 5
0
        private void Update(UnityEngine.Animations.AnimationStream s)
        {
            if (!_target.IsValid(s))
            {
                return;
            }

            float w = _IKPositionWeight.GetFloat(s);

            if (w <= 0)
            {
                return;
            }
            w = Mathf.Min(w, 1f);

            Read(s);

            bool  XY                = _XY.GetBool(s);
            float maxIterations     = _maxIterations.GetInt(s);
            float tolerance         = _tolerance.GetFloat(s);
            bool  useRotationLimits = _useRotationLimits.GetBool(s);

            Vector3 IKPosition = _target.GetPosition(s);

            if (XY)
            {
                IKPosition.z = bones[0].GetPosition(s).z;
            }

            Vector3 singularityOffset = maxIterations > 1 ? GetSingularityOffset(s, IKPosition, useRotationLimits) : Vector3.zero;

            // Iterating the solver
            int it = 1;

            for (int i = 0; i < maxIterations; i++)
            {
                // Optimizations
                Vector3 localDirection = GetLocalDirection(s);
                if (singularityOffset == Vector3.zero && i >= 1 && tolerance > 0 && GetPositionOffset(s, localDirection) < tolerance * tolerance)
                {
                    break;
                }
                lastLocalDirection = localDirection;

                Solve(s, IKPosition + (i == 0 ? singularityOffset : Vector3.zero), XY, w, it, useRotationLimits);

                it++;
                if (it >= bones.Length - 1)
                {
                    it -= bones.Length - 2;
                }
            }

            lastLocalDirection = GetLocalDirection(s);
        }
    public void ProcessAnimation(UnityEngine.Animations.AnimationStream stream)
    {
        float w = jobWeight.Get(stream);

        if (w > 0f)
        {
            constrained.SetPosition(
                stream,
                math.lerp(constrained.GetPosition(stream), -source.GetPosition(stream), w)
                );
        }
    }
    public void ProcessAnimation(UnityEngine.Animations.AnimationStream stream)
    {
        float w = jobWeight.Get(stream);

        if (w > 0f)
        {
            // TODO : Change code to consider inverted axis positions instead

            constrained.SetPosition(
                stream,
                math.lerp(constrained.GetPosition(stream), -source.GetPosition(stream), w)
                );
        }
    }
Exemplo n.º 8
0
        private void Read(UnityEngine.Animations.AnimationStream s)
        {
            chainSqrMag = 0;

            for (int i = 0; i < bones.Length; i++)
            {
                // Calculate bone and chain sqr magnitudes
                if (i < bones.Length - 1)
                {
                    boneSqrMags[i] = (bones[i].GetPosition(s) - bones[i + 1].GetPosition(s)).sqrMagnitude;
                    chainSqrMag   += boneSqrMags[i];
                }
            }
        }
    public void ProcessAnimation(UnityEngine.Animations.AnimationStream stream)
    {
        float w = jobWeight.Get(stream);

        if (w > 0f)
        {
            float3 v = sourceMapping == TransformMapping.Location ? source.GetLocalPosition(stream) : sourceMapping == TransformMapping.Rotation ? source.GetLocalRotation(stream).eulerAngles : source.GetLocalScale(stream);
            if (sourceMapping == TransformMapping.Rotation)
            {
                if (math.abs(v.x - lastRotation.x) > 180)
                {
                    revolutions += math.sign(v.x - lastRotation.x);
                }
                if (math.abs(v.y - lastRotation.y) > 180)
                {
                    revolutions += math.sign(v.y - lastRotation.y);
                }
                if (math.abs(v.z - lastRotation.z) > 180)
                {
                    revolutions += math.sign(v.z - lastRotation.z);
                }
            }
            v -= revolutions * 360;
            var x = xMapping.sqrMagnitude > 0 ? math.remap(xMapping.x, xMapping.y, xMapping.z, xMapping.w, math.dot(v, toX)) : 0;
            var y = yMapping.sqrMagnitude > 0 ? math.remap(yMapping.x, yMapping.y, yMapping.z, yMapping.w, math.dot(v, toY)) : 0;
            var z = zMapping.sqrMagnitude > 0 ? math.remap(zMapping.x, zMapping.y, zMapping.z, zMapping.w, math.dot(v, toZ)) : 0;
            if (!extrapolate)
            {
                x = math.clamp(x, math.min(xMapping.z, xMapping.w), math.max(xMapping.z, xMapping.w));
                y = math.clamp(y, math.min(yMapping.z, yMapping.w), math.max(yMapping.z, yMapping.w));
                z = math.clamp(z, math.min(zMapping.z, zMapping.w), math.max(zMapping.z, zMapping.w));
            }
            switch (destinationMapping)
            {
            case TransformMapping.Location:
                destination.SetLocalPosition(stream, offsetPos + new float3(x, y, z));
                break;

            case TransformMapping.Rotation:
                destination.SetLocalRotation(stream, Quaternion.Euler(offsetRot + new float3(x, y, z)));
                break;

            case TransformMapping.Scale:
                destination.SetLocalScale(stream, new float3(x, y, z));
                break;
            }
            lastRotation = source.GetLocalRotation(stream).eulerAngles;
        }
    }
    public void ProcessAnimation(UnityEngine.Animations.AnimationStream stream)
    {
        float w = jobWeight.Get(stream);

        if (w > 0f)
        {
            var    tmp        = invert.Get(stream);
            float3 invertMask = new float3(
                math.select(1f, -1f, tmp.x),
                math.select(1f, -1f, tmp.y),
                math.select(1f, -1f, tmp.z)
                );

            constrained.SetPosition(
                stream,
                math.lerp(constrained.GetPosition(stream), source.GetPosition(stream) * invertMask, w)
                );
        }
    }
Exemplo n.º 11
0
        // Get target offset to break out of the linear singularity issue
        private Vector3 GetSingularityOffset(UnityEngine.Animations.AnimationStream s, Vector3 IKPosition, bool useRotationLimits)
        {
            if (!SingularityDetected(s, IKPosition))
            {
                return(Vector3.zero);
            }

            Vector3 IKDirection = (IKPosition - bones[0].GetPosition(s)).normalized;

            Vector3 secondaryDirection = new Vector3(IKDirection.y, IKDirection.z, IKDirection.x);

            // Avoiding getting locked by the Hinge Rotation Limit
            if (useRotationLimits && hingeFlags[bones.Length - 2] == 1)
            {
                secondaryDirection = bones[bones.Length - 2].GetRotation(s) * limitAxisArray[bones.Length - 2];
            }

            return(Vector3.Cross(IKDirection, secondaryDirection) * Mathf.Sqrt(boneSqrMags[bones.Length - 2]) * 0.5f);
        }
Exemplo n.º 12
0
 public float GetAngle(UnityEngine.Animations.AnimationStream s, Vector3 axis, Vector3 IKPosition)
 {
     return(Vector3.Angle(_transform.GetRotation(s) * axis, IKPosition - _transform.GetPosition(s)));
 }
 public void GetLocalTRS(AnimationStream stream, out Vector3 position, out Quaternion rotation, out Vector3 scale)
 {
     this.CheckIsValidAndResolve(ref stream);
     this.GetLocalTRSInternal(ref stream, out position, out rotation, out scale);
 }
Exemplo n.º 14
0
        private void Update(UnityEngine.Animations.AnimationStream s)
        {
            if (!_target.IsValid(s))
            {
                return;
            }
            if (!_poleTarget.IsValid(s))
            {
                return;
            }
            if (!_transform.IsValid(s))
            {
                return;
            }

            Vector3 axis     = new Vector3(_axisX.GetFloat(s), _axisY.GetFloat(s), _axisZ.GetFloat(s));
            Vector3 poleAxis = new Vector3(_poleAxisX.GetFloat(s), _poleAxisY.GetFloat(s), _poleAxisZ.GetFloat(s));

            float poleWeight = _poleWeight.GetFloat(s);

            poleWeight = Mathf.Clamp(poleWeight, 0f, 1f);

            if (axis == Vector3.zero)
            {
                return;
            }
            if (poleAxis == Vector3.zero && poleWeight > 0f)
            {
                return;
            }

            float IKPositionWeight = _IKPositionWeight.GetFloat(s);

            if (IKPositionWeight <= 0)
            {
                return;
            }
            IKPositionWeight = Mathf.Min(IKPositionWeight, 1f);

            bool  XY                = _XY.GetBool(s);
            float maxIterations     = _maxIterations.GetInt(s);
            float tolerance         = _tolerance.GetFloat(s);
            bool  useRotationLimits = _useRotationLimits.GetBool(s);

            Vector3 IKPosition = _target.GetPosition(s);

            if (XY)
            {
                IKPosition.z = bones[0].GetPosition(s).z;
            }

            Vector3 polePosition = _poleTarget.GetPosition(s);

            if (XY)
            {
                polePosition.z = IKPosition.z;
            }

            float clampWeight = _clampWeight.GetFloat(s);

            clampWeight = Mathf.Clamp(clampWeight, 0f, 1f);
            int clampSmoothing = _clampSmoothing.GetInt(s);

            clampSmoothing = Mathf.Clamp(clampSmoothing, 0, 2);

            Vector3    transformPosition = _transform.GetPosition(s);
            Quaternion transformRotation = _transform.GetRotation(s);
            Vector3    transformAxis     = transformRotation * axis;

            Vector3 clampedIKPosition = GetClampedIKPosition(s, clampWeight, clampSmoothing, IKPosition, transformPosition, transformAxis);

            Vector3 dir = clampedIKPosition - transformPosition;

            dir = Vector3.Slerp(transformAxis * dir.magnitude, dir, IKPositionWeight);
            clampedIKPosition = transformPosition + dir;

            // Iterating the solver
            for (int i = 0; i < maxIterations; i++)
            {
                // Optimizations
                if (i >= 0 && tolerance > 0 && GetAngle(s, axis, IKPosition) < tolerance)
                {
                    break;
                }
                lastLocalDirection = GetLocalDirection(s, _transform.GetRotation(s) * axis);

                for (int n = 0; n < bones.Length - 1; n++)
                {
                    RotateToTarget(s, clampedIKPosition, polePosition, n, step * (n + 1) * IKPositionWeight * boneWeights[n].GetFloat(s), poleWeight, XY, useRotationLimits, axis, poleAxis);
                }

                RotateToTarget(s, clampedIKPosition, polePosition, bones.Length - 1, IKPositionWeight * boneWeights[bones.Length - 1].GetFloat(s), poleWeight, XY, useRotationLimits, axis, poleAxis);
            }

            lastLocalDirection = GetLocalDirection(s, _transform.GetRotation(s) * axis);
        }
Exemplo n.º 15
0
        // Rotating bone to get transform aim closer to target
        private void RotateToTarget(UnityEngine.Animations.AnimationStream s, Vector3 targetPosition, Vector3 polePosition, int i, float weight, float poleWeight, bool XY, bool useRotationLimits, Vector3 axis, Vector3 poleAxis)
        {
            // Swing
            if (XY)
            {
                if (weight >= 0f)
                {
                    Vector3 dir       = _transform.GetRotation(s) * axis;
                    Vector3 targetDir = targetPosition - _transform.GetPosition(s);

                    float angleDir    = Mathf.Atan2(dir.x, dir.y) * Mathf.Rad2Deg;
                    float angleTarget = Mathf.Atan2(targetDir.x, targetDir.y) * Mathf.Rad2Deg;

                    bones[i].SetRotation(s, Quaternion.AngleAxis(Mathf.DeltaAngle(angleDir, angleTarget), Vector3.back) * bones[i].GetRotation(s));
                }
            }
            else
            {
                if (weight >= 0f)
                {
                    Quaternion rotationOffset = Quaternion.FromToRotation(_transform.GetRotation(s) * axis, targetPosition - _transform.GetPosition(s));

                    if (weight >= 1f)
                    {
                        bones[i].SetRotation(s, rotationOffset * bones[i].GetRotation(s));
                    }
                    else
                    {
                        bones[i].SetRotation(s, Quaternion.Lerp(Quaternion.identity, rotationOffset, weight) * bones[i].GetRotation(s));
                    }
                }

                // Pole
                if (poleWeight > 0f)
                {
                    Vector3 poleDirection = polePosition - _transform.GetPosition(s);

                    // Ortho-normalize to transform axis to make this a twisting only operation
                    Vector3 poleDirOrtho = poleDirection;
                    Vector3 normal       = _transform.GetRotation(s) * axis;
                    Vector3.OrthoNormalize(ref normal, ref poleDirOrtho);

                    Quaternion toPole = Quaternion.FromToRotation(_transform.GetRotation(s) * poleAxis, poleDirOrtho);
                    bones[i].SetRotation(s, Quaternion.Lerp(Quaternion.identity, toPole, weight * poleWeight) * bones[i].GetRotation(s));
                }
            }

            // Rotation Constraints
            if (useRotationLimits)
            {
                if (hingeFlags[i] == 1)
                {
                    Quaternion localRotation = Quaternion.Inverse(limitDefaultLocalRotationArray[i]) * bones[i].GetLocalRotation(s);
                    Quaternion lastRotation  = hingeLastRotations[i];
                    float      lastAngle     = hingeLastAngles[i];
                    Quaternion r             = RotationLimitUtilities.LimitHinge(localRotation, hingeMinArray[i].GetFloat(s), hingeMaxArray[i].GetFloat(s), hingeUseLimitsArray[i].GetBool(s), limitAxisArray[i], ref lastRotation, ref lastAngle);
                    hingeLastRotations[i] = lastRotation;
                    hingeLastAngles[i]    = lastAngle;
                    bones[i].SetLocalRotation(s, limitDefaultLocalRotationArray[i] * r);
                }
                else if (angleFlags[i] == 1)
                {
                    Quaternion localRotation = Quaternion.Inverse(limitDefaultLocalRotationArray[i]) * bones[i].GetLocalRotation(s);
                    Quaternion r             = RotationLimitUtilities.LimitAngle(localRotation, limitAxisArray[i], angleSecondaryAxisArray[i], angleLimitArray[i].GetFloat(s), angleTwistLimitArray[i].GetFloat(s));
                    bones[i].SetLocalRotation(s, limitDefaultLocalRotationArray[i] * r);
                }
            }
        }
 private void SetLocalScaleInternal(ref AnimationStream stream, Vector3 scale)
 {
     TransformStreamHandle.SetLocalScaleInternal_Injected(ref this, ref stream, ref scale);
 }
Exemplo n.º 17
0
 public void ProcessAnimation(UnityEngine.Animations.AnimationStream stream)
 {
     Update(stream);
 }
 private void SetLocalPositionInternal(ref AnimationStream stream, Vector3 position)
 {
     TransformStreamHandle.SetLocalPositionInternal_Injected(ref this, ref stream, ref position);
 }
 private void SetLocalRotationInternal(ref AnimationStream stream, Quaternion rotation)
 {
     TransformStreamHandle.SetLocalRotationInternal_Injected(ref this, ref stream, ref rotation);
 }
 public void SetGlobalTR(AnimationStream stream, Vector3 position, Quaternion rotation, bool useMask)
 {
     this.CheckIsValidAndResolve(ref stream);
     this.SetGlobalTRInternal(ref stream, position, rotation, useMask);
 }
 private void ResolveInternal(ref AnimationStream stream)
 {
     TransformStreamHandle.ResolveInternal_Injected(ref this, ref stream);
 }
 public Vector3 GetLocalScale(AnimationStream stream)
 {
     this.CheckIsValidAndResolve(ref stream);
     return(this.GetLocalScaleInternal(ref stream));
 }
Exemplo n.º 23
0
 public void ProcessRootMotion(UnityEngine.Animations.AnimationStream stream)
 {
 }
Exemplo n.º 24
0
 //Gets the direction from last bone to first bone in first bone's local space.
 private Vector3 GetLocalDirection(UnityEngine.Animations.AnimationStream s, Vector3 transformAxis)
 {
     return(Quaternion.Inverse(bones[0].GetRotation(s)) * transformAxis);
 }
 public bool GetScaleReadMask(AnimationStream stream)
 {
     this.CheckIsValidAndResolve(ref stream);
     return(this.GetScaleReadMaskInternal(ref stream));
 }
 public void SetLocalScale(AnimationStream stream, Vector3 scale)
 {
     this.CheckIsValidAndResolve(ref stream);
     this.SetLocalScaleInternal(ref stream, scale);
 }
Exemplo n.º 27
0
 //Gets the offset from last position of the last bone to its current position.
 private float GetPositionOffset(UnityEngine.Animations.AnimationStream s, Vector3 localDirection)
 {
     return(Vector3.SqrMagnitude(localDirection - lastLocalDirection));
 }
 public void SetLocalTRS(AnimationStream stream, Vector3 position, Quaternion rotation, Vector3 scale, bool useMask)
 {
     this.CheckIsValidAndResolve(ref stream);
     this.SetLocalTRSInternal(ref stream, position, rotation, scale, useMask);
 }
Exemplo n.º 29
0
    public void ProcessAnimation(UnityEngine.Animations.AnimationStream stream)
    {
        if (!settings.enabled || AnimGraph_Stand.useFootIk.IntValue < 1)
        {
            return;
        }

        ikWeight = Mathf.Clamp01(ikWeight + (1 - settings.enterStateEaseIn));

        if (stream.isHumanStream)
        {
            var human = stream.AsHuman();

            var leftToePos  = leftToe.GetPosition(stream);
            var rightToePos = rightToe.GetPosition(stream);

            var leftIkOffset  = ikOffset.x * ikWeight;
            var rightIkOffset = ikOffset.y * ikWeight;

            leftToePos  += new Vector3(0f, leftIkOffset, 0f);
            rightToePos += new Vector3(0f, rightIkOffset, 0f);


            var leftAnklePos    = human.GetGoalPosition(AvatarIKGoal.LeftFoot);
            var rightAnklePos   = human.GetGoalPosition(AvatarIKGoal.RightFoot);
            var leftAnkleRot    = human.GetGoalRotation(AvatarIKGoal.LeftFoot);
            var rightAnkleRot   = human.GetGoalRotation(AvatarIKGoal.RightFoot);
            var leftAnkleIkPos  = new Vector3(leftAnklePos.x, leftAnklePos.y + leftIkOffset, leftAnklePos.z);
            var rightAnkleIkPos = new Vector3(rightAnklePos.x, rightAnklePos.y + rightIkOffset, rightAnklePos.z);

            var hipHeightOffset = (leftIkOffset + rightIkOffset) * 0.5f;
            var forwardBackBias = (leftIkOffset - rightIkOffset) * settings.weightShiftHorizontal;

            // TODO: (sunek) Rework weight shift to move towards actual lower foot?
            hipHeightOffset += Mathf.Abs(leftIkOffset - rightIkOffset) * settings.weightShiftVertical;
            var standAngle = Quaternion.AngleAxis(settings.weightShiftAngle, Vector3.up) * Vector3.forward;
            human.bodyLocalPosition += new Vector3(standAngle.x * forwardBackBias, hipHeightOffset, standAngle.z * forwardBackBias);

            // Figure out the normal rotation
            var leftNormalRot  = Quaternion.LookRotation(Vector3.forward, normalLeftFoot);
            var rightNormalRot = Quaternion.LookRotation(Vector3.forward, normalRightFoot);

            // Clamp normal rotation
            var leftAngle  = Quaternion.Angle(Quaternion.identity, leftNormalRot);
            var rightAngle = Quaternion.Angle(Quaternion.identity, rightNormalRot);

            if (leftAngle > settings.maxFootRotationOffset && settings.maxFootRotationOffset > 0f)
            {
                var fraction = settings.maxFootRotationOffset / leftAngle;
                leftNormalRot = Quaternion.Lerp(Quaternion.identity, leftNormalRot, fraction);
            }

            if (rightAngle > settings.maxFootRotationOffset && settings.maxFootRotationOffset > 0f)
            {
                var fraction = settings.maxFootRotationOffset / rightAngle;
                rightNormalRot = Quaternion.Lerp(Quaternion.identity, rightNormalRot, fraction);
            }

            // Apply rotation to ankle
            var leftToesMatrix  = Matrix4x4.TRS(leftToePos, Quaternion.identity, Vector3.one);
            var rightToesMatrix = Matrix4x4.TRS(rightToePos, Quaternion.identity, Vector3.one);

            var leftToesNormalDeltaMatrix  = Matrix4x4.TRS(leftToePos, leftNormalRot, Vector3.one) * leftToesMatrix.inverse;
            var rightToesNormalDeltaMatrix = Matrix4x4.TRS(rightToePos, rightNormalRot, Vector3.one) * rightToesMatrix.inverse;

            var leftAnkleMatrix  = Matrix4x4.TRS(leftAnkleIkPos, leftAnkleRot, Vector3.one) * leftToesMatrix.inverse;
            var rightAnkleMatrix = Matrix4x4.TRS(rightAnkleIkPos, rightAnkleRot, Vector3.one) * rightToesMatrix.inverse;

            leftAnkleMatrix  = leftToesNormalDeltaMatrix * leftAnkleMatrix * leftToesMatrix;
            rightAnkleMatrix = rightToesNormalDeltaMatrix * rightAnkleMatrix * rightToesMatrix;

            leftAnkleIkPos  = leftAnkleMatrix.GetColumn(3);
            rightAnkleIkPos = rightAnkleMatrix.GetColumn(3);

            leftAnkleRot  = Quaternion.Lerp(leftAnkleRot, leftAnkleMatrix.rotation, ikWeight);
            rightAnkleRot = Quaternion.Lerp(rightAnkleRot, rightAnkleMatrix.rotation, ikWeight);

            // Update ik position
            // TODO: (sunek) Consider combating leg overstretch
            var leftPosition  = Vector3.Lerp(leftAnklePos, leftAnkleIkPos, ikWeight);
            var rightPosition = Vector3.Lerp(rightAnklePos, rightAnkleIkPos, ikWeight);

            human.SetGoalPosition(AvatarIKGoal.LeftFoot, leftPosition);
            human.SetGoalPosition(AvatarIKGoal.RightFoot, rightPosition);
            human.SetGoalRotation(AvatarIKGoal.LeftFoot, leftAnkleRot);
            human.SetGoalRotation(AvatarIKGoal.RightFoot, rightAnkleRot);

            human.SetGoalWeightPosition(AvatarIKGoal.LeftFoot, 1f);
            human.SetGoalWeightPosition(AvatarIKGoal.RightFoot, 1f);
            human.SetGoalWeightRotation(AvatarIKGoal.LeftFoot, 1f);
            human.SetGoalWeightRotation(AvatarIKGoal.RightFoot, 1f);
        }
    }
 public void GetGlobalTR(AnimationStream stream, out Vector3 position, out Quaternion rotation)
 {
     this.CheckIsValidAndResolve(ref stream);
     this.GetGlobalTRInternal(ref stream, out position, out rotation);
 }