public void Solve(IKConstrainedJoint[] bones, Transform target) {
/* This code works by cleverly looking at the rotations in the current pose and determining
* an axis of rotation for each joint from that.
* It then assumes that more bending shortens the overall length of the chain. and adjusts a single
* bending coefficient to achieve the desired overall chain length to the target, then adjusts
* the first bone, 'hip', as a ball/socket pivot to get the correct direction of the chain to the target
* point. This works well for legs and not bad for arms.
*
* Right now, we feed it all 7 bones from the collarbone to the palm, most of which are not useful
* in solving the problem. We need to tell it which bones to adjust, and how much min/max, in what order,
* but not ignore all the intervening transforms in checking the placement.
*
* We can improve by re-writing the algorithm to manipulate only the elbow, add in some spine rotation for
* extending reach (a couple of spine bones and rotations to apply for greater reach could give us turn and
* bend, applied as needed in the order elbow, turn, bend. We need to provide maximun bends for each joint.
* transferring twist to the lower arm. we could think about an up vector for elbow.
* A cone constraint on the final alignment rotation at the shoulder.
*/
Transform endEffector = bones[bones.Length-1].go.transform;
//public Transform targetEffector = null; //the actual target end effector that we move around
Vector3 targetPos = target.position + target.TransformDirection(offset);
// Get the axis of rotation for each joint
// rather than this, we should supply known min/max rotations for each adjustable joint.
Vector3[] rotateAxes = new Vector3[bones.Length-2];
float[] rotateAngles = new float[bones.Length-2];
Quaternion[] rotations = new Quaternion[bones.Length-2];
float spineBend = 0;
for (int i=0; i<bones.Length-2; i++) {
rotateAxes[i] = Vector3.Cross(
bones[i+1].go.transform.position-bones[i].go.transform.position,
bones[i+2].go.transform.position-bones[i+1].go.transform.position
);
rotateAxes[i] = Quaternion.Inverse(bones[i].go.transform.rotation) * rotateAxes[i];
rotateAxes[i] = rotateAxes[i].normalized;
rotateAngles[i] = Vector3.Angle(
bones[i+1].go.transform.position-bones[i].go.transform.position,
bones[i+1].go.transform.position-bones[i+2].go.transform.position
);
rotations[i] = bones[i+1].go.transform.localRotation;
}
// Get the length of each bone - this is only used to calculate a reasonable accuracy tolerance
float[] boneLengths = new float[bones.Length-1];
float legLength = 0;
for (int i=0; i<bones.Length-1; i++) {
boneLengths[i] = (bones[i+1].go.transform.position-bones[i].go.transform.position).magnitude;
legLength += boneLengths[i];
}
positionAccuracy = legLength*0.001f;
float currentDistance = (endEffector.position-bones[iShoulder].go.transform.position).magnitude;
float targetDistance = (targetPos - bones[iShoulder].go.transform.position).magnitude;
// Search for right joint bendings to get target distance between hip and foot
float bendingLow, bendingHigh;
bool minIsFound = false;
bool bendMore = false;
if (targetDistance > currentDistance) {
minIsFound = true;
bendingHigh = 1;
bendingLow = 0;
}
else {
bendMore = true;
bendingHigh = 1;
bendingLow = 0;
}
int tries = 0;
while ( Mathf.Abs(currentDistance-targetDistance) > positionAccuracy && tries < maxIterations ) {
tries++;
float bendingNew;
if (!minIsFound) bendingNew = bendingHigh;
else bendingNew = (bendingLow+bendingHigh)/2;
// for (int i=0; i<bones.Length-2; i++) {
// bend the elbow for length
int elbow = iElbow-1;{
float newAngle;
if (!bendMore) newAngle = Mathf.Lerp(180, rotateAngles[elbow], bendingNew);
else newAngle = rotateAngles[elbow]*(1-bendingNew) + (rotateAngles[elbow]-30)*bendingNew;
float angleDiff = (rotateAngles[elbow]-newAngle);
Quaternion addedRotation = Quaternion.AngleAxis(angleDiff,rotateAxes[elbow]);
Quaternion newRotation = addedRotation * rotations[elbow];
bones[elbow+1].go.transform.localRotation = newRotation;
}
currentDistance = (endEffector.position-bones[iShoulder].go.transform.position).magnitude;
if (targetDistance > currentDistance) minIsFound = true;
if (minIsFound) {
if (targetDistance > currentDistance) bendingHigh = bendingNew;
else bendingLow = bendingNew;
// if we are below a certain amount of bend, and still can't reach our target,
// reach by rotating the spine and bending forward, distributed along 3 spine bones,
// the rotation to add is created during setup, for left or right side.
// and remember to re-calculate the target distance after doing so.
if (!bendMore && targetDistance > currentDistance) spineBend += (targetDistance - currentDistance)*25.0f;
if (spineBend > 20) spineBend = 20;
{
for (int s=iSpine+1; s<iSpine+4; s++){
// float newAngle;
// newAngle = spineBend; //rotateAngles[s-1]*(1-bendingNew) + (rotateAngles[s-1]-30)*bendingNew;
// float angleDiff = (rotateAngles[s-1]-newAngle);
Quaternion addedRotation = Quaternion.AngleAxis(spineBend,spineAxis);
Quaternion newRotation = addedRotation * rotations[s-1];
bones[s].go.transform.localRotation = newRotation;
}
targetDistance = (targetPos - bones[iShoulder].go.transform.position).magnitude;
}
if (bendingHigh < 0.01f) break;
}
else {
spineBend = 0; //?
bendingLow = bendingHigh;
bendingHigh++;
}
}
//Debug.Log("tries: "+tries);
//Debug.Log("Result at "+bones[1].go.transform.localRotation.eulerAngles+" spineBend="+spineBend);
// Rotate iShoulder bone such that foot is at desired position
bones[iShoulder].go.transform.rotation = (
Quaternion.AngleAxis(
Vector3.Angle(
(endEffector.position-bones[iShoulder].go.transform.position),
(targetPos - bones[iShoulder].go.transform.position)
),
Vector3.Cross(
(endEffector.position-bones[iShoulder].go.transform.position),
(targetPos - bones[iShoulder].go.transform.position)
)
) * bones[iShoulder].go.transform.rotation
);
// we need to try and blend in the target orientation as well...
// separate out the twist and apply that at the wrist bone
// bones[iWrist].go.transform.rotation = target.rotation * orientation;
bones[iHand].go.transform.rotation = target.rotation * orientation;
}
public void BlendChain(IKConstrainedJoint[] chain, float blendWeight){
// test just jamming the values...
// for (int i = 0; i<chain.Length; i++){
// chain[i].master.position = chain[i].go.transform.position;
// chain[i].master.rotation = chain[i].go.transform.rotation;
// }
// return;
for (int i = 0; i<chain.Length; i++){
// chain[i].master.position = Vector3.Lerp(chain[i].master.position,chain[i].go.transform.position,blendWeight);
chain[i].master.localRotation = Quaternion.Slerp(chain[i].master.localRotation,chain[i].go.transform.localRotation,blendWeight);
}
}
public void InitChain(IKConstrainedJoint[] chain){
for (int i = 0; i<chain.Length; i++){
chain[i].go = new GameObject(chain[i].master.name+"_IK_Ghost");
if (i==0)
chain[i].go.transform.parent = chain[i].master.parent;
else
chain[i].go.transform.parent = chain[i-1].go.transform;
chain[i].go.transform.position = chain[i].master.position;
chain[i].go.transform.rotation = chain[i].master.rotation;
if (chain[i].master.name.Contains("Spine1"))
iSpine = i;
if (chain[i].master.name.Contains("Upperarm1"))
iShoulder = i;
if (chain[i].master.name.Contains("Forearm1"))
iElbow = i;
// if (chain[i].master.name.Contains("Forearm3"))
// iWrist = i;
if (chain[i].master.name.Contains("Palm"))
iHand = i;
}
MirrorChain(chain);
}
public void MirrorChain(IKConstrainedJoint[] chain){
for (int i = 0; i<chain.Length; i++){
chain[i].go.transform.position = chain[i].master.position;
chain[i].go.transform.rotation = chain[i].master.rotation;
}
}
// Get the chain of transforms from one transform to a descendent one
public IKConstrainedJoint[] GetChain(Transform upper, Transform lower) {
Transform t = lower;
int chainLength = 1;
while (t != upper) {
t = t.parent;
chainLength++;
}
IKConstrainedJoint[] chain = new IKConstrainedJoint[chainLength];
t = lower;
for (int j=0; j<chainLength; j++) {
chain[chainLength-1-j] = new IKConstrainedJoint();
chain[chainLength-1-j].master = t;
t = t.parent;
}
InitChain(chain);
return chain;
}
