private void LateUpdate()
{
FABRIKSolver fabrikSolver = new FABRIKSolver(root,
new Dictionary <Transform, Vector3>
{
{ handR, target1.position },
{ handL, target2.position }
});
if (!Input.GetKey(KeyCode.Space))
{
fabrikSolver.Solve();
}
}
public void FixLeg(byte iterations, float minErrorToStartSolving)
{
if (m_iKWeight <= Mathf.Epsilon || m_BonesCount <= 1)
{
return;
}
if ((m_footPosBeforeLookAt - m_FootBone.transform.position).magnitude < minErrorToStartSolving)
{
return;
}
//check if the bone numbers are changed at run-time or not to reinitialize and avoid getting null reference exceptions
if (m_BoneLengths.Length != m_BonesCount - 1)
{
Initialize();
}
Transform buffTr = m_FootBone;
Quaternion originalFootRotMS = m_FootBone.transform.rotation;
Transform[] transforms = new Transform[m_BonesCount];
Vector3[] positions = new Vector3[m_BonesCount];
Vector3[] fwdVecs = new Vector3[m_BonesCount - 1];
Vector3 firstBonePos = Vector3.zero;
for (int i = 0; i < m_BonesCount; i++)
{
positions[i] = buffTr.transform.position;
transforms[i] = buffTr.transform;
if (i < m_BonesCount - 1)
{
fwdVecs[i] = buffTr.transform.position - buffTr.parent.transform.position;
buffTr = buffTr.parent;
}
else
{
firstBonePos = buffTr.transform.position;
}
}
m_originalSwivelDir = FindProjectionVector(m_firstBonePosBeforeLookAt, m_middleBonePosBeforeLookAt, m_footPosBeforeLookAt);
FABRIKSolver.SolveFabrik(ref positions, ref m_BoneLengths, m_footPosBeforeLookAt, m_IKChainLength, iterations, minErrorToStartSolving);
//if knee angle is completely extended and angle between thigh and calf is small we skip updating swivel dir and use the last frame swivel dir. Ths helps to have the right knee hinge axis every frame.
float solvedKneeAngle = Vector3.Angle(m_FootBone.transform.position - firstBonePos, positions[m_BonesCount / 2] - firstBonePos);
if (solvedKneeAngle > Mathf.Epsilon)
{
m_afterSolvingSwivelDir = FindProjectionVector(firstBonePos, positions[m_BonesCount / 2], m_footPosBeforeLookAt);
}
FixSwivelAngle(ref positions);
Quaternion RotBuff = Quaternion.identity;
Vector3 currentFwdVec;
float iKBlendWeight = Mathf.Clamp01(m_iKWeight);
for (uint i = m_BonesCount - 1; i > 0; i--)
{
currentFwdVec = positions[i - 1] - positions[i];
if (i == 1)
{
fwdVecs[0] = m_FootBone.transform.position - m_FootBone.parent.transform.position;
}
RotBuff = Quaternion.Slerp(Quaternion.identity, PerfectLookAt.GetWorldLookAtRotation(currentFwdVec, fwdVecs[i - 1]), iKBlendWeight);
transforms[i].rotation = RotBuff * transforms[i].rotation;
}
m_FootBone.transform.rotation = Quaternion.Slerp(m_FootBone.transform.rotation, m_footRotBeforeLookAt, iKBlendWeight);
}
