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
}
// 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);
}
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);
}
}
}
}
//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));
}
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)
);
}
}
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)
);
}
}
// 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);
}
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);
}
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);
}
// 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);
}
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));
}
public void ProcessRootMotion(UnityEngine.Animations.AnimationStream stream)
{
}
//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);
}
//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);
}
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);
}