using System.Collections.Generic;
using UnityEngine;
public class IKMB : MonoBehaviour
{
public Transform goal;
public ArticulationBody effector;
public ArticulationBody baseBone;
public float sqrDistError = 0.01f;
[Range(0f, 1f)]
public float weight = 1f;
public int maxIterationCount = 10;
private List<ArticulationBody> m_Bones;
void OnEnable()
{
// Create a list of bones starting at the effector.
m_Bones = new List<ArticulationBody>();
ArticulationBody current = effector;
// Add bones up the hierarchy until the base bone is reached.
while (current != null && current != baseBone)
{
m_Bones.Add(current);
current = current.transform.parent != null ? current.transform.parent.GetComponent<ArticulationBody>() : null;
}
if (current != baseBone)
throw new UnityException("Base Bone is not an ancestor of Effector. IK will fail.");
// Ensure the base bone is included in the list
m_Bones.Add(baseBone);
}
void LateUpdate()
{
Solve();
}
void Solve()
{
Vector3 goalPosition = goal.position;
Vector3 effectorPosition = m_Bones[0].transform.position;
Vector3 targetPosition = Vector3.Lerp(effectorPosition, goalPosition, weight);
float sqrDistance;
int iterationCount = 0;
do
{
for (int i = 0; i < m_Bones.Count; i++)
{
RotateBone(m_Bones[0], m_Bones[i], targetPosition);
sqrDistance = (m_Bones[0].transform.position - targetPosition).sqrMagnitude;
if (sqrDistance <= sqrDistError)
return;
}
sqrDistance = (m_Bones[0].transform.position - targetPosition).sqrMagnitude;
iterationCount++;
}
while (sqrDistance > sqrDistError && iterationCount <= maxIterationCount);
}
public static void RotateBone(ArticulationBody effector, ArticulationBody bone, Vector3 goalPosition)
{
Vector3 effectorPosition = effector.transform.position;
Vector3 bonePosition = bone.transform.position;
Vector3 boneToEffector = effectorPosition - bonePosition;
Vector3 boneToGoal = goalPosition - bonePosition;
if (boneToEffector == Vector3.zero || boneToGoal == Vector3.zero)
return; // Prevent invalid rotation calculations
Quaternion fromToRotation = Quaternion.FromToRotation(boneToEffector, boneToGoal);
// Convert the rotation to local space
Quaternion localRotation = Quaternion.Inverse(bone.transform.rotation) * fromToRotation * bone.transform.rotation;
// Apply the local rotation to the articulation body's drive target
ArticulationDrive drive = bone.xDrive;
drive.target += localRotation.eulerAngles.x;
bone.xDrive = drive;
drive = bone.yDrive;
drive.target += localRotation.eulerAngles.y;
bone.yDrive = drive;
drive = bone.zDrive;
drive.target += localRotation.eulerAngles.z;
bone.zDrive = drive;
}
}