public override void solve() { Vector3 toTarget = target.transform.position - upper.getStartPointWorld(); float maxLength = upper.length + lower.length; float minLength = upper.length - lower.length; float targetLength = toTarget.magnitude; upper.rotateLookAtLocal(target.transform.position); lower.rotateReset(); end.rotateReset(); if (maxLength < targetLength) { //Quaternion rotTowardsEnd = Quaternion.LookRotation(toTarget,upper.transform.forward); //rotTowardsEnd *= Quaternion.AngleAxis(90.0f, Vector3.right); //upper.transform.rotation = rotTowardsEnd; //lower.transform.rotation = rotTowardsEnd; Debug.DrawLine(upper.getStartPointWorld(), target.transform.position, Color.red); return; } else if (targetLength < minLength) { Debug.DrawLine(upper.getStartPointWorld(), target.transform.position, Color.red); return; } else { //Debug.Log("upper: " + upper.length); //Debug.Log("lower: " + lower.length); //Debug.Log("distance: " + targetLength); float shoulderAngle = The5.Math.Trigonometry.angleOppositeC(upper.length, targetLength, lower.length); float ellbowAngle = The5.Math.Trigonometry.angleOppositeC(upper.length, lower.length, targetLength); float shoulderTwist = target.transform.rotation.eulerAngles.y; float wristTwisth = 0.0f; //Debug.Log("ellbow: " + ellbowAngle); //Debug.Log("shoulder: " + shoulderAngle); if (true) { upper.transform.localEulerAngles += new Vector3(shoulderAngle, 0.0f, 0.0f); float flip = (upper.transform.position.y - target.transform.position.y) > 0.0f ? 1.0f : -1.0f; //FIXME lower.transform.localEulerAngles += flip * new Vector3(180.0f - ellbowAngle, 0.0f, 0.0f); } //upper.transform.localRotation = Quaternion.EulerRotation(); Debug.DrawLine(upper.getStartPointWorld(), target.transform.position, Color.green); return; } }
public IKSolverTwoBone(string name, IKBone chainRoot, IKBone targetRoot, float length, float width, float thickness, float proportion) : base(name, chainRoot, targetRoot) { neutralDir = Vector3.right; hingeAxis = Vector3.up; upper = chainRoot.addBone(name + "_upper", length * proportion, width * proportion, thickness * proportion); float proportionInv = 1.0f - proportion; lower = upper.addBone(name + "_lower", length * proportionInv, width * proportionInv, thickness * proportionInv); float maxWT = Mathf.Max(width, thickness); end = lower.addBone(name + "_end", maxWT * proportionInv, maxWT * proportionInv, maxWT * proportionInv); target = IKTarget.createTarget(name + "_target", targetRoot); target.transform.position = upper.getStartPointWorld() + neutralDir * (upper.length + lower.length); }