public void Init(CCDIKBone child, CCDIKBone parent)
{
LastIKLocPosition = transform.localPosition;
IKParent = parent;
if (child != null)
{
SetChild(child);
}
IKChild = child;
}
/// <summary> Assigning bones for IK processor with CCD IK logics (unlimited bone count) </summary>
public FIK_CCDProcessor(Transform[] bonesChain)
{
IKBones = new CCDIKBone[bonesChain.Length];
Bones = new CCDIKBone[IKBones.Length];
for (int i = 0; i < bonesChain.Length; i++)
{
IKBones[i] = new CCDIKBone(bonesChain[i]);
Bones[i] = IKBones[i];
}
IKTargetPosition = EndBone.transform.position; IKTargetRotation = EndBone.transform.rotation;
}
public override void Init(Transform root)
{
if (Initialized)
{
return;
}
fullLength = 0f;
for (int i = 0; i < Bones.Length; i++)
{
CCDIKBone b = IKBones[i];
CCDIKBone child = null, parent = null;
if (i > 0)
{
parent = IKBones[i - 1];
}
else if (i < Bones.Length - 1)
{
child = IKBones[i + 1];
}
if (i < Bones.Length - 1)
{
IKBones[i].Init(child, parent);
fullLength += b.BoneLength;
b.ForwardOrientation = Quaternion.Inverse(b.transform.rotation) * (IKBones[i + 1].transform.position - b.transform.position);
}
else
{
IKBones[i].Init(child, parent);
b.ForwardOrientation = Quaternion.Inverse(b.transform.rotation) * (IKBones[IKBones.Length - 1].transform.position - IKBones[0].transform.position);
}
}
Initialized = true;
}
/// <summary> Updating processor with n-bones oriented inverse kinematics </summary>
public override void Update()
{
if (!Initialized)
{
return;
}
if (IKWeight <= 0f)
{
return;
}
CCDIKBone wb = IKBones[0];
// Restoring previous IK progress for continous solving
if (ContinousSolving)
{
while (wb != null)
{
wb.LastKeyLocalRotation = wb.transform.localRotation;
wb.transform.localPosition = wb.LastIKLocPosition;
wb.transform.localRotation = wb.LastIKLocRotation;
wb = wb.IKChild;
}
}
else
{
if (SyncWithAnimator > 0f)
{
// Memory for animator syncing
while (wb != null)
{
wb.LastKeyLocalRotation = wb.transform.localRotation;
wb = wb.IKChild;
}
}
}
if (ReactionQuality < 0)
{
ReactionQuality = 1;
}
Vector3 goalPivotOffset = Vector3.zero;
if (ReactionQuality > 1)
{
goalPivotOffset = GetGoalPivotOffset();
}
for (int itr = 0; itr < ReactionQuality; itr++)
{
// Restrictions for multiple interations
if (itr >= 1)
{
if (goalPivotOffset.sqrMagnitude == 0)
{
if (Smoothing > 0)
{
if (GetVelocityDifference() < Smoothing * Smoothing)
{
break;
}
}
}
}
LastLocalDirection = RefreshLocalDirection();
Vector3 ikGoal = IKTargetPosition + goalPivotOffset;
// Going in iterations in reversed way, from pre end child to root parent
wb = IKBones[IKBones.Length - 2];
if (!Use2D) // Full 3D space rotations calculations
{
while (wb != null)
{
float weight = wb.MotionWeight * IKWeight;
if (weight > 0f)
{
Quaternion targetRotation = Quaternion.FromToRotation(Bones[Bones.Length - 1].transform.position - wb.transform.position /*fromThisToEndChildBone*/, ikGoal - wb.transform.position /*fromThisToIKGoal*/) * wb.transform.rotation;
if (weight < 1f)
{
wb.transform.rotation = Quaternion.Lerp(wb.transform.rotation, targetRotation, weight);
}
else
{
wb.transform.rotation = targetRotation;
}
}
wb.AngleLimiting();
wb = wb.IKParent;
}
}
else
{
// Going in while() loop is 2x faster than for(i;i;i;) when there is more iterations
while (wb != null)
{
float weight = wb.MotionWeight * IKWeight;
if (weight > 0f)
{
Vector3 fromThisToEndChildBone = Bones[Bones.Length - 1].transform.position - wb.transform.position;
Vector3 fromThisToIKGoal = ikGoal - wb.transform.position;
wb.transform.rotation = Quaternion.AngleAxis(Mathf.DeltaAngle(Mathf.Atan2(fromThisToEndChildBone.x, fromThisToEndChildBone.y) * Mathf.Rad2Deg /* Angle to last bone */, Mathf.Atan2(fromThisToIKGoal.x, fromThisToIKGoal.y) * Mathf.Rad2Deg /* Angle to goal position */) * weight, Vector3.back) * wb.transform.rotation;
}
wb.AngleLimiting();
wb = wb.IKParent;
}
}
}
LastLocalDirection = RefreshLocalDirection();
// Support for stretching
if (MaxStretching > 0f)
{
ActiveLength = Mathf.Epsilon;
wb = IKBones[0];
while (wb.IKChild != null)
{
wb.FrameWorldLength = (wb.transform.position - wb.IKChild.transform.position).magnitude;
ActiveLength += wb.FrameWorldLength;
wb = wb.IKChild;
}
Vector3 toGoal = IKTargetPosition - StartBone.transform.position;
float stretch = toGoal.magnitude / ActiveLength;
if (stretch > 1f)
{
for (int i = 1; i < IKBones.Length; ++i)
{
IKBones[i].transform.position += toGoal.normalized * (IKBones[i - 1].BoneLength * MaxStretching) * StretchCurve.Evaluate(-(1f - stretch));
}
}
}
wb = IKBones[0];
while (wb != null)
{
// Storing final rotations for animator offset
wb.LastIKLocRotation = wb.transform.localRotation;
wb.LastIKLocPosition = wb.transform.localPosition;
// Offset based rotation sync with animator
Quaternion ikDiff = wb.LastIKLocRotation * Quaternion.Inverse(wb.InitialLocalRotation);
wb.transform.localRotation = Quaternion.Lerp(wb.LastIKLocRotation, ikDiff * wb.LastKeyLocalRotation, SyncWithAnimator);
if (IKWeight < 1f)
{
wb.transform.localRotation = Quaternion.Lerp(wb.LastKeyLocalRotation, wb.transform.localRotation, IKWeight);
}
wb = wb.IKChild;
}
}