/// <summary>
/// Initialize values
/// </summary>
protected virtual void Initialize()
{
Joints = new Transform[ChainLength + 1];
// initialize chain
Transform current = transform;
for (int i = ChainLength; i >= 0; i--)
{
if (current == null)
{
throw new UnityException("ChainLength greater than actual chain length");
}
JointConstraints constraints = current.GetComponent <JointConstraints>();
if (constraints != null)
{
Constraints.Add(i, constraints);
}
Joints[i] = current;
current = current.parent;
}
// initialize target if not given
if (Target == null)
{
Target = new GameObject(gameObject.name + " IK-Target").transform;
Target.position = transform.position;
Target.rotation = Quaternion.identity;
Target.localScale = Vector3.one;
}
}
/// <summary>
/// IK using Gradient descent per degree of freedom
/// </summary>
/// <param name="target">Target position</param>
/// <param name="rotations">Current joint rotations</param>
private void InverseKinematicsV2(Vector3 target, ref Quaternion[] rotations, Vector3 axis)
{
if (Error(target, rotations) < MaxError)
{
return;
}
// Gradient descent Starting at the last joint
for (int i = Joints.Length - 2; i >= 0; i--)
{
// Joint Rotation Constraints
JointConstraints constraints = null;
bool constrained = Constraints.TryGetValue(i, out constraints);
if ((constrained && constraints.RotationAxis.Contains(axis)) || !constrained)
{
float partialGradient = PartialGradient(target, rotations, i, axis);
Vector3 descent = LearningRate * partialGradient * axis;
Vector3 newAngles = rotations[i].eulerAngles - descent;
// Joint Axis Constraints
if (constrained)
{
Vector3 angles = Vector3.Scale(newAngles, axis);
float newAngle = angles.x + angles.y + angles.z;
newAngle = newAngle > 180 ? newAngle - 360 : newAngle;
newAngle = Mathf.Clamp
(
newAngle,
constraints.MinAngle(axis),
constraints.MaxAngle(axis)
);
// Sum up the final calculation
Vector3 unaffectedAxis = new Vector3(1, 1, 1) - axis;
newAngles = Vector3.Scale(newAngles, unaffectedAxis) + newAngle * axis;
}
rotations[i].eulerAngles = newAngles;
// Apply new rotation to the joint
Joints[i].localEulerAngles = newAngles;
// Early termination
if (Error(target, rotations) < MaxError)
{
return;
}
}
}
}
protected override void ResolveIK()
{
//get position
for (int i = 0; i < Joints.Length; i++)
{
_positions[i] = GetPositionRootSpace(Joints[i]);
}
Vector3 targetPosition = GetPositionRootSpace(Target);
Quaternion targetRotation = GetRotationRootSpace(Target);
//1st is possible to reach?
if ((targetPosition - GetPositionRootSpace(Joints[0])).sqrMagnitude >= _completeLength * _completeLength)
{
//just strech it
Vector3 direction = (targetPosition - _positions[0]).normalized;
//set everything after root
for (int i = 1; i < _positions.Length; i++)
{
_positions[i] = _positions[i - 1] + direction * _boneLength[i - 1];
}
}
else
{
for (int i = 0; i < _positions.Length - 1; i++)
{
_positions[i + 1] = Vector3.Lerp(_positions[i + 1], _positions[i] + _initOffsetsTargetRootTF[i], SnapBackStrength);
}
for (int iteration = 0; iteration < Iterations; iteration++)
{
//back
for (int i = _positions.Length - 1; i > 0; i--)
{
if (i == _positions.Length - 1)
{
_positions[i] = targetPosition; //set it to target
}
else
{
_positions[i] = _positions[i + 1] + (_positions[i] - _positions[i + 1]).normalized * _boneLength[i]; //set in line on distance
}
}
//forward
for (int i = 1; i < _positions.Length; i++)
{
_positions[i] = _positions[i - 1] + (_positions[i] - _positions[i - 1]).normalized * _boneLength[i - 1];
}
//close enough?
if ((_positions[_positions.Length - 1] - targetPosition).sqrMagnitude < MaxError * MaxError)
{
break;
}
}
}
//move towards pole
if (Pole != null)
{
Vector3 polePosition = GetPositionRootSpace(Pole);
for (int i = 1; i < _positions.Length - 1; i++)
{
Plane plane = new Plane(_positions[i + 1] - _positions[i - 1], _positions[i - 1]);
Vector3 projectedPole = plane.ClosestPointOnPlane(polePosition);
Vector3 projectedBone = plane.ClosestPointOnPlane(_positions[i]);
float angle = Vector3.SignedAngle(projectedBone - _positions[i - 1], projectedPole - _positions[i - 1], plane.normal);
_positions[i] = Quaternion.AngleAxis(angle, plane.normal) * (_positions[i] - _positions[i - 1]) + _positions[i - 1];
}
}
//set position & rotation
for (int i = 0; i < _positions.Length; i++)
{
JointConstraints constraints = null;
Constraints.TryGetValue(i, out constraints);
if (i == _positions.Length - 1)
{
SetRotationRootSpace(
Joints[i],
Quaternion.Inverse(targetRotation) * _initTargetRotationRootTF * Quaternion.Inverse(_initRotationsRootTF[i])
);
}
else
{
SetRotationRootSpace(
Joints[i],
Quaternion.FromToRotation(_initOffsetsTargetRootTF[i], _positions[i + 1] - _positions[i]) * Quaternion.Inverse(_initRotationsRootTF[i])
);
}
SetPositionRootSpace(Joints[i], _positions[i]);
}
}
/*
* Either have gradient:
* per dof (IKv1) -> calculate partialGradient for each DOF
* per joint (IKv2) -> call IK for each DOF
*/
/// <summary>
/// IK using Gradient descent per joint
/// </summary>
/// <param name="target">Target position</param>
/// <param name="rotations">Current joint rotations</param>
private void InverseKinematics(Vector3 target, ref Quaternion[] rotations)
{
if (Error(target, rotations) < MaxError)
{
return;
}
// Gradient descent Starting at the last joint
for (int i = ChainLength - 1; i >= 0; i--)
{
// DOF of the joint
JointConstraints constraints = null;
bool constrained = Constraints.TryGetValue(i, out constraints);
Vector3[] rotationAxis = constrained ? constraints.RotationAxis.ToArray() : THREE_ROTATION_DOF;
// Calculate the gradient descent
Vector3 descentVals = Vector3.zero;
foreach (Vector3 axis in rotationAxis)
{
float partialGradient = PartialGradient(target, rotations, i, axis);
descentVals += partialGradient * axis;
}
Vector3 newAngles = rotations[i].eulerAngles - (LearningRate * descentVals);
// Joint Axis Constraints
if (constrained)
{
foreach (Vector3 axis in rotationAxis)
{
Vector3 angleOfInterest = Vector3.Scale(newAngles, axis);
float newAngle = angleOfInterest.x + angleOfInterest.y + angleOfInterest.z;
// aQuaternion.eulerAngles always returns positive values
newAngle = newAngle > 180 ? newAngle - 360 : newAngle;
newAngle = Mathf.Clamp
(
newAngle,
constraints.MinAngle(axis),
constraints.MaxAngle(axis)
);
// Sum up the final calculation
Vector3 unaffectedAxis = new Vector3(1, 1, 1) - axis;
newAngles = Vector3.Scale(newAngles, unaffectedAxis) + newAngle * axis;
}
}
// Apply new rotation to the joint
Quaternion newLocalRot = Quaternion.Euler(newAngles);
if (i == 0)
{
Joints[0].localRotation = _rootParent != null?Quaternion.Inverse(_rootParent.rotation) * newLocalRot : newLocalRot;
}
else
{
Joints[i].rotation = Joints[i].parent.rotation * newLocalRot;
}
rotations[i].eulerAngles = newAngles;
// Early termination
if (Error(target, rotations) < MaxError)
{
return;
}
}
}
