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