static void DrawLimb(IKLimb limb)
{
var current = limb.transform;
for (int y = 0; current != null && current.parent != null; y++) //limb.chainLength &&
{
var scale = Vector3.Distance(current.position, current.parent.position) * 0.1f;
Handles.matrix = Matrix4x4.TRS(current.position,
Quaternion.FromToRotation(Vector3.up, current.parent.position - current.position),
new Vector3(scale, Vector3.Distance(current.parent.position, current.position), scale));
Handles.color = Color.green;
Handles.DrawWireCube(Vector3.up * 0.5f, Vector3.one);
Gizmos.color = Color.magenta;
Gizmos.DrawWireSphere(current.position, 1f);
current = current.parent;
}
// Gizmos.DrawWireSphere(limb.pole.position, 1f);
}
public void Init(IKLimb ik)
{
//initial array
ik.bones = new Transform[ik.chainLength + 1];
ik.positions = new Vector3[ik.chainLength + 1];
ik.bonesLength = new float[ik.chainLength];
ik.startDirectionSucc = new Vector3[ik.chainLength + 1];
ik.startRotationBone = new Quaternion[ik.chainLength + 1];
//init target
if (ik.target == null)
{
ik.target = new GameObject(ik.gameObject.name + " Target").transform;
SetPositionRootSpace(ik.target, GetPositionRootSpace(ik.transform, ik.root), ik.root);
}
ik.startRotationTarget = GetRotationRootSpace(ik.target, ik.root);
//init data
var current = ik.transform;
ik.completeLength = 0;
for (var i = ik.bones.Length - 1; i >= 0; i--)
{
ik.bones[i] = current;
ik.startRotationBone[i] = GetRotationRootSpace(current, ik.root);
if (i == ik.bones.Length - 1)
{
//leaf
ik.startDirectionSucc[i] = GetPositionRootSpace(ik.target, ik.root) - GetPositionRootSpace(current, ik.root);
}
else
{
//mid bone
ik.startDirectionSucc[i] = GetPositionRootSpace(ik.bones[i + 1], ik.root) - GetPositionRootSpace(current, ik.root);
ik.bonesLength[i] = ik.startDirectionSucc[i].magnitude;
ik.completeLength += ik.bonesLength[i];
}
current = current.parent;
}
}
public void ResolveIK(IKLimb limb)
{
//if (Target == null)
// return;
//if (BonesLength.Length != ChainLength)
// Init();
//Fabric
// root
// (bone0) (bonelen 0) (bone1) (bonelen 1) (bone2)...
// x--------------------x--------------------x---...
//get position
for (int i = 0; i < limb.bones.Length; i++)
{
limb.positions[i] = GetPositionRootSpace(limb.bones[i], limb.root);
}
var targetPosition = GetPositionRootSpace(limb.target, limb.root);
var targetRotation = GetRotationRootSpace(limb.target, limb.root);
//1st is possible to reach?
if ((targetPosition - GetPositionRootSpace(limb.bones[0], limb.root)).sqrMagnitude >= limb.completeLength * limb.completeLength)
{
//just strech it
var direction = (targetPosition - limb.positions[0]).normalized;
//set everything after root
for (int i = 1; i < limb.positions.Length; i++)
{
limb.positions[i] = limb.positions[i - 1] + direction * limb.bonesLength[i - 1];
}
}
else
{
for (int i = 0; i < limb.positions.Length - 1; i++)
{
limb.positions[i + 1] = Vector3.Lerp(limb.positions[i + 1], limb.positions[i] + limb.startDirectionSucc[i], snapBackStrength);
}
for (int iteration = 0; iteration < iterations; iteration++)
{
//https://www.youtube.com/watch?v=UNoX65PRehA
//back
for (int i = limb.positions.Length - 1; i > 0; i--)
{
if (i == limb.positions.Length - 1)
{
limb.positions[i] = targetPosition; //set it to target
}
else
{
limb.positions[i] = limb.positions[i + 1] + (limb.positions[i] - limb.positions[i + 1]).normalized * limb.bonesLength[i]; //set in line on distance
}
}
//forward
for (int i = 1; i < limb.positions.Length; i++)
{
limb.positions[i] = limb.positions[i - 1] + (limb.positions[i] - limb.positions[i - 1]).normalized * limb.bonesLength[i - 1];
}
//close enough?
if ((limb.positions[limb.positions.Length - 1] - targetPosition).sqrMagnitude < delta * delta)
{
break;
}
}
}
//move towards pole
//if (limb.pole != null)
//{
var polePosition = GetPositionRootSpace(limb.pole, limb.root);
for (int i = 1; i < limb.positions.Length - 1; i++)
{
var plane = new Plane(limb.positions[i + 1] - limb.positions[i - 1], limb.positions[i - 1]);
var projectedPole = plane.ClosestPointOnPlane(polePosition);
var projectedBone = plane.ClosestPointOnPlane(limb.positions[i]);
var angle = Vector3.SignedAngle(projectedBone - limb.positions[i - 1], projectedPole - limb.positions[i - 1], plane.normal);
limb.positions[i] = Quaternion.AngleAxis(angle, plane.normal) * (limb.positions[i] - limb.positions[i - 1]) + limb.positions[i - 1];
}
// }
//set position & rotation
for (int i = 0; i < limb.positions.Length; i++)
{
if (i == limb.positions.Length - 1)
{
SetRotationRootSpace(limb.bones[i],
Quaternion.Inverse(targetRotation) * limb.startRotationTarget * Quaternion.Inverse(limb.startRotationBone[i]), limb.root);
}
else
{
SetRotationRootSpace(limb.bones[i],
Quaternion.FromToRotation(limb.startDirectionSucc[i], limb.positions[i + 1] - limb.positions[i])
* Quaternion.Inverse(limb.startRotationBone[i]), limb.root);
}
SetPositionRootSpace(limb.bones[i], limb.positions[i], limb.root);
}
}
static void DrawGizmoForLimbs(IKLimb limb, GizmoType gizmoType)
{
DrawLimb(limb);
}
