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); }