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