public void Lerp() { ray = null; if (!lerp) { n.position = end.position; chain.target = n; follow = true; lerp = true; } else { var timeMultiplier = 3.6f; var minSpeed = 0.12f; if ((target.position - n.position).sqrMagnitude < Mathf.Pow(minSpeed, 2)) { n.position = target.position; return; } else { var lerp = Vector3.Lerp(n.position, target.position, Time.deltaTime * timeMultiplier); var x = lerp - n.position; if (x.sqrMagnitude < Mathf.Pow(minSpeed, 2)) { n.position += x.normalized * minSpeed; } else { n.position = lerp; } } } }
public void DoReset() { chain = new IKBoneChain(); if (joints.Count == 0) { return; } ChangeChain(); joints[0].transform.localPosition = Vector3.zero; joints[0].parent.transform.localPosition = Vector3.zero; chain.end = end; chain.target = target; ScaleChain(); UpdateValues(); lerp = false; follow = false; index = 0; ray = null; }
public void SolveAll() { ray = null; UpdateValues(); chain.Solve(); }
public void SolveStep() { UpdateValues(); ray = chain.Solve(index % chain.joints.Length); index++; }
/// <summary> /// 计算骨骼链 /// </summary> /// <param name="关节"></param> /// <param name="目标位置"></param> /// <param name="末端效应器"></param> /// <param name="每次迭代最大旋转角度"></param> /// <param name="迭代次数"></param> public static WRay IKSolve(Joint joint, Transform target, Transform end, float limitAngle) { // 关节相对于目标位置和末端效应器的位置 Vector3 absJoint2End = end.position - joint.transform.position; Vector3 absJoint2Target = target.position - joint.transform.position; // 转为本地坐标系 Quaternion invRotation = joint.transform.rotation.Conjugate(); Vector3 localJoint2End = invRotation * absJoint2End; Vector3 localJoint2Target = invRotation * absJoint2Target; float deltaAngle = Mathf.Rad2Deg * Mathf.Acos(Vector3.Dot(localJoint2End.normalized, localJoint2Target.normalized)); if (CloseZero(deltaAngle)) { return(null); } Vector3 rotateAxis = Vector3.Cross(localJoint2Target, localJoint2End).normalized; Quaternion deltaRotation = Quaternion.AngleAxis(deltaAngle, rotateAxis); deltaRotation = deltaRotation.Conjugate(); var ray = new WRay(joint.transform.position, rotateAxis, Color.green); var delta = deltaRotation.eulerAngles; var cur = joint.transform.localRotation.eulerAngles; var deltaV = Vector3.zero; if ((joint.constraints & RigidbodyConstraints.FreezeRotationX) == 0) { deltaV += new Vector3(limit(delta.x, limitAngle), 0f, 0f); } if ((joint.constraints & RigidbodyConstraints.FreezeRotationY) == 0) { deltaV += new Vector3(0f, limit(delta.y, limitAngle), 0f); } if ((joint.constraints & RigidbodyConstraints.FreezeRotationZ) == 0) { deltaV += new Vector3(0f, 0f, limit(delta.z, limitAngle)); } deltaRotation.eulerAngles = deltaV; joint.transform.localRotation *= deltaRotation; var euler = joint.transform.localRotation.eulerAngles; var quat = new Quaternion(); quat.eulerAngles = Vector3.zero; if ((joint.constraints & RigidbodyConstraints.FreezeRotationX) == 0) { quat.eulerAngles += new Vector3(Mathf.Clamp(euler.x, joint.minAngle, joint.maxAngle), 0f, 0f); } if ((joint.constraints & RigidbodyConstraints.FreezeRotationY) == 0) { quat.eulerAngles += new Vector3(0f, Mathf.Clamp(euler.y, joint.minAngle, joint.maxAngle), 0f); } if ((joint.constraints & RigidbodyConstraints.FreezeRotationZ) == 0) { quat.eulerAngles += new Vector3(0f, 0f, Mathf.Clamp(euler.z, joint.minAngle, joint.maxAngle)); } joint.transform.localRotation = quat; return(ray); }