public override void Solve(
IKJoint[] transforms,
Transform endEffector,
Vector3 target)
{
throw new System.NotImplementedException();
}
public void Solve(Transform[] transforms, Vector3 target)
{
IKJoint[] joints = new IKJoint[transforms.Length];
for (int i = 0; i < transforms.Length; i++)
joints[i] = new IKJoint(transforms[i]);
this.Solve(joints, target);
}
public override void Solve(IKJoint[] joints, Vector3 target)
{
Transform hip = joints[0].transform;
Transform knee = joints[1].transform;
Transform ankle = joints[2].transform;
// Calculate the direction in which the knee should be pointing
Vector3 vKneeDir = Vector3.Cross(
ankle.position - hip.position,
Vector3.Cross(
ankle.position-hip.position,
ankle.position-knee.position
)
);
// Get lengths of leg bones
float fThighLength = (knee.position-hip.position).magnitude;
float fShinLength = (ankle.position-knee.position).magnitude;
// Calculate the desired new joint positions
Vector3 pHip = hip.position;
Vector3 pAnkle = target;
Vector3 pKnee = findKnee(pHip,pAnkle,fThighLength,fShinLength,vKneeDir);
// Rotate the bone transformations to align correctly
Quaternion hipRot = Quaternion.FromToRotation(knee.position-hip.position, pKnee-pHip) * hip.rotation;
if (System.Single.IsNaN(hipRot.x)) {
Debug.LogWarning("hipRot="+hipRot+" pHip="+pHip+" pAnkle="+pAnkle+" fThighLength="+fThighLength+" fShinLength="+fShinLength+" vKneeDir="+vKneeDir);
}
else {
hip.rotation = hipRot;
knee.rotation = Quaternion.FromToRotation(ankle.position-knee.position, pAnkle-pKnee) * knee.rotation;
}
}
public override void Solve(
IKJoint[] joints,
Transform endEffector,
Vector3 tarPos)
{
float damp = this.dampingMax * Time.deltaTime;
int link = joints.Length - 1;
Vector3 endPos = endEffector.position;
// Cap out the number of iterations
for (int tries = 0; tries < this.maxIterations; tries++)
{
// Are we there yet?
if ((endPos - tarPos).sqrMagnitude <= epsilon)
break;
if (link < 0)
link = joints.Length - 1;
endPos = endEffector.position;
Vector3 rootPos = joints[link].position;
Vector3 currentDirection = (endPos - rootPos).normalized;
Vector3 targetDirection = (tarPos - rootPos).normalized;
float dot = Vector3.Dot(currentDirection, targetDirection);
if (dot < (1.0f - epsilon))
{
float turnRad = Mathf.Acos(dot);
if (damping == true && turnRad > damp)
turnRad = damp;
float turnDeg = turnRad * Mathf.Rad2Deg;
// Use the cross product to determine which way to rotate
Vector3 cross =
Vector3.Cross(currentDirection, targetDirection);
joints[link].rotation =
Quaternion.AngleAxis(turnDeg, cross)
* joints[link].rotation;
joints[link].Constrain();
joints[link].Relax(Time.deltaTime);
}
//// Move back in the array
link--;
}
}
public override void Solve(IKJoint[] joints, float angle, Vector3 target)
{
throw new System.NotImplementedException();
}
public abstract void Solve(
IKJoint[] transforms,
Transform endEffector,
Vector3 target);
public int maxIterations = 100; // (If applicable)
#endregion Fields
#region Methods
public abstract void Solve(IKJoint[] joints, Vector3 target);
public override void Solve(
IKJoint[] joints,
Transform endEffector,
Vector3 target)
{
// Get the axis of rotation for each joint
Vector3[] rotateAxes = new Vector3[joints.Length-2];
float[] rotateAngles = new float[joints.Length-2];
Quaternion[] rotations = new Quaternion[joints.Length-2];
for (int i=0; i<joints.Length-2; i++)
{
rotateAxes[i] = Vector3.Cross(
joints[i+1].position - joints[i].position,
joints[i+2].position - joints[i+1].position);
rotateAxes[i] =
Quaternion.Inverse(joints[i].rotation) * rotateAxes[i];
rotateAxes[i] = rotateAxes[i].normalized;
rotateAngles[i] = Vector3.Angle(
joints[i+1].position - joints[i].position,
joints[i+1].position - joints[i+2].position);
rotations[i] = joints[i+1].localRotation;
}
// Get the length of each bone
float[] boneLengths = new float[joints.Length-1];
float legLength = 0;
for (int i=0; i<joints.Length-1; i++)
{
boneLengths[i] =
(joints[i+1].position-joints[i].position).magnitude;
legLength += boneLengths[i];
}
epsilon = legLength*0.001f;
float currentDistance =
(endEffector.position-joints[0].position).magnitude;
float targetDistance = (target-joints[0].position).magnitude;
// Search for right joint bendings to get target distance between
// the hip and the 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;
float testDistance = Mathf.Abs(currentDistance - targetDistance);
while (testDistance > epsilon && tries < maxIterations)
{
tries++;
float bendingNew;
if (minIsFound == true)
bendingNew = (bendingLow + bendingHigh) / 2;
else
bendingNew = bendingHigh;
for (int i=0; i < joints.Length - 2; i++)
{
float newAngle;
if (bendMore == false)
newAngle = Mathf.Lerp(180, rotateAngles[i], bendingNew);
else
newAngle =
rotateAngles[i] * (1 - bendingNew)
+ (rotateAngles[i] - 30) * bendingNew;
float angleDiff = (rotateAngles[i]-newAngle);
Quaternion addedRotation =
Quaternion.AngleAxis(angleDiff, rotateAxes[i]);
Quaternion newRotation = addedRotation * rotations[i];
joints[i + 1].localRotation = newRotation;
}
Vector3 difference = endEffector.position - joints[0].position;
if (targetDistance > difference.magnitude)
minIsFound = true;
if (minIsFound == true)
{
if (targetDistance > currentDistance)
bendingHigh = bendingNew;
else
bendingLow = bendingNew;
if (bendingHigh < 0.01f)
break;
}
else
{
bendingLow = bendingHigh;
bendingHigh++;
}
testDistance = Mathf.Abs(currentDistance - targetDistance);
}
// Rotate hip bone such that foot is at desired position
float angle =
Vector3.Angle(
endEffector.position - joints[0].position,
target-joints[0].position);
Vector3 axis =
Vector3.Cross(
endEffector.position - joints[0].position,
target-joints[0].position);
joints[0].rotation =
Quaternion.AngleAxis(angle, axis) * joints[0].rotation;
}
public override void Solve(IKJoint[] joints, Vector3 target)
{
this.Solve(joints, joints[joints.Length - 1].transform, target);
}
