/// <summary>
/// we need to offset the root to achieve the results
/// </summary>
private void OffsetTargets()
{
Vector3 _newRootPos = Root().position;
float _toRight = Vector3.Distance(_newRootPos, rightLeg.GetIKPosition());
float _toLeft = Vector3.Distance(_newRootPos, leftLeg.GetIKPosition());
float _rootOffset = Mathf.Max(_toLeft, _toRight) - Mathf.Min(_toRight, _toLeft);
rootY = Mathf.Lerp(rootY, _rootOffset, rootLerp * Time.fixedDeltaTime);
_newRootPos.y -= rootY;
for (int i = 0; i < 2; i++)
{
Vector3 _targetOffset = i == 0 ? rightLeg.GetIKPosition() : leftLeg.GetIKPosition();
_targetOffset += Vector3.up * footHeight;
offsetedIK[i] = GenericMaths.Interpolate(offsetedIK[i], _targetOffset, footLerp * Time.fixedDeltaTime);
}
rightLeg.SetIKPosition(offsetedIK[0]);
leftLeg.SetIKPosition(offsetedIK[1]);
Root().position = _newRootPos;
//RayArtist.DrawHitPoints(rightLeg.GetEndEffector().position + Vector3.up * maxStep + Vector3.right * 0.3f, rightLeg.GetIKPosition() + Vector3.right * 0.3f, Color.red);
//RayArtist.DrawHitPoints(leftLeg.GetEndEffector().position + Vector3.up * maxStep + Vector3.left * 0.2f, leftLeg.GetIKPosition() + Vector3.left * 0.2f, Color.red);
}
/// <summary>
/// correct the orientation of the bones after the solve
/// </summary>
private void CorrectRotation()
{
for (int i = 0; i < chain.joints.Count - 1; i++)
{
//apply the new pos
chain.joints[i].transform.position = chain.joints[i].solvePos;
Vector3 _targetDir = chain.joints[i + 1].transform.position - chain.joints[i].transform.position;
Quaternion _sourceRot = chain.joints[i].transform.rotation;
Quaternion _targetRot = RootIK.RotateFromTo(RootIK.TransformVector(Vector3.up, _sourceRot), _targetDir);
_targetRot = Quaternion.Inverse(_targetRot);
chain.joints[i].transform.rotation = Quaternion.Lerp(_sourceRot, GenericMaths.ApplyQuaternion(_targetRot, _sourceRot), chain.weight);
}
//Bounce back to the 1st bone before the endeffector and correct its rotation
Vector3 _t = chain.GetIKPosition() - chain.joints[chain.joints.Count - 2].transform.position;
Vector3 _t2 = RootIK.TransformVector(Vector3.up, chain.joints[chain.joints.Count - 2].transform.rotation);
Quaternion _s = chain.joints[chain.joints.Count - 2].transform.rotation;
Quaternion _tr = RootIK.RotateFromTo(_t, _t2);
chain.joints[chain.joints.Count - 2].transform.rotation = Quaternion.Lerp(_s, GenericMaths.ApplyQuaternion(_tr, _s), chain.weight);
chain.joints[chain.joints.Count - 1].transform.rotation = chain.GetIKRotation(); //apply end effector rotation
}
/// <summary>
/// the _source vector will rotate to point at the _target vector
/// </summary>
/// <param name="_source"></param>
/// <param name="_target"></param>
/// <returns></returns>
public static Quaternion RotateFromTo(Vector3 _source, Vector3 _target)
{
_source.Normalize();
_target.Normalize();
Quaternion q = GenericMaths.QuaternionFromAngleAxis(Vector3.Cross(_source, _target).normalized, GenericMaths.VectorsAngle(_source, _target));
return(Quaternion.Inverse(q));
}
/// <summary>
/// Prepare the solver and solve the IK problem analytically with specific axis of rotation
/// (the chain must contain 2 joints, no more and no less)
/// </summary>
/// <param name="_hingeChain">the chain</param>
/// <param name="_direction">direction of the player</param>
/// <param name="_axis">axis of rotation for the 2nd joint (the Hinge joint)</param>
public void SolveAnalytically(RootIK.Chain _hingeChain, Vector3 _direction, Vector3 _axis)
{
if (_hingeChain.joints.Count > 3)
{
return;
}
if (_hingeChain.iterations <= 0)
{
return;
}
if (_hingeChain.joints.Count <= 0)
{
return;
}
//calculate bone length;
upperLength = Vector3.Distance(_hingeChain.joints[0].transform.position, _hingeChain.joints[1].transform.position);
lowerLength = Vector3.Distance(_hingeChain.joints[1].transform.position, _hingeChain.joints[2].transform.position);
systemLength = Vector3.Distance(_hingeChain.joints[0].transform.position, _hingeChain.GetIKPosition());
//lowerjoint 1DOF
float _angle = GenericMaths.Formula(upperLength, lowerLength, systemLength) + Mathf.PI * Mathf.Rad2Deg;
if (_axis == Vector3.zero)
{
_axis = Vector3.Cross(RootIK.TransformVector(Vector3.up, _hingeChain.joints[1].transform.rotation), _direction);
}
else
{
_axis = Vector3.Cross(RootIK.TransformVector(_axis, _hingeChain.joints[1].transform.rotation), _direction);
}
Quaternion _src = _hingeChain.joints[1].transform.rotation;
Quaternion _t = GenericMaths.QuaternionFromAngleAxis(_axis, _angle + Mathf.Acos(Mathf.Clamp(_src.w, -1f, 1f)) * 10f);
Quaternion _finalRot = Quaternion.Lerp(Quaternion.identity, _t, _hingeChain.weight);
_hingeChain.joints[1].transform.rotation = GenericMaths.ApplyQuaternion(_finalRot, _src);
//Upperjoint 3DOF
Vector3 _v1 = _hingeChain.GetIKPosition() - _hingeChain.joints[0].transform.position;
Vector3 _v2 = _hingeChain.joints[2].transform.position - _hingeChain.joints[0].transform.position;
Quaternion _src2 = _hingeChain.joints[0].transform.rotation;
Quaternion _t2 = RootIK.RotateFromTo(_v2, _v1);
Quaternion _finalRot2 = Quaternion.Lerp(Quaternion.identity, _t2, _hingeChain.weight);
_hingeChain.joints[0].transform.rotation = GenericMaths.ApplyQuaternion(Quaternion.Inverse(_finalRot2), _src2);
//foot 6DOF
_hingeChain.GetEndEffector().rotation = _hingeChain.GetIKRotation();
}
/// <summary>
/// solve the joints forward
/// </summary>
private void SolveOutward()
{
chain.joints[0].solvePos = chain.joints[0].transform.position;
for (int i = 1; i < chain.joints.Count; i++)
{
Vector3 _v1 = chain.joints[i - 1].solvePos;
Vector3 _v0 = chain.joints[i].solvePos - _v1;
_v0.Normalize();
_v0 *= Vector3.Distance(chain.joints[i].transform.position, chain.joints[i - 1].transform.position);
chain.joints[i].solvePos = GenericMaths.Interpolate(chain.joints[i].transform.position, _v0 + _v1, chain.weight);
}
}
/// <summary>
/// solve the joints backward
/// </summary>
private void SolveInward()
{
chain.joints[chain.joints.Count - 1].solvePos = GenericMaths.Interpolate(chain.GetEndEffector().position, chain.GetIKPosition(), chain.weight);
for (int i = chain.joints.Count - 2; i >= 0; i--)
{
Vector3 _v1 = chain.joints[i + 1].solvePos;
Vector3 _v0 = chain.joints[i].solvePos - _v1;
_v0.Normalize();
_v0 *= Vector3.Distance(chain.joints[i].transform.position, chain.joints[i + 1].transform.position);
chain.joints[i].solvePos = GenericMaths.Interpolate(chain.joints[i].transform.position, _v0 + _v1, chain.weight);
}
}
/// <summary>
/// Solve the KinematicBone
/// </summary>
/// <param name="_bone"></param>
public void SolveBone(RootIK.KinematicBone _bone)
{
if (_bone.weight <= 0)
{
return;
}
if (_bone.target == null)
{
Debug.LogError("No target was assigned");
return;
}
Vector3 _v0 = RootIK.TransformVector(_bone.axis == Vector3.zero ? Vector3.forward : _bone.axis, _bone.bone.rotation);
Vector3 _v1 = _bone.target.position - _bone.bone.position;
Quaternion _targetRot = Quaternion.Lerp(Quaternion.identity, RootIK.RotateFromTo(_v0, _v1), _bone.weight);
_targetRot = Quaternion.Inverse(_targetRot);
_bone.bone.rotation = GenericMaths.ApplyQuaternion(_targetRot, _bone.bone.rotation);
}
private void FindGround()
{
for (int i = 0; i < 2; i++)
{
Vector3 _endEffector = i == 0 ? rightLeg.GetEndEffector().position : leftLeg.GetEndEffector().position;
Ray _ray = new Ray(_endEffector + Vector3.up * maxStep, Vector3.down);
RaycastHit _hit = new RaycastHit();
if (Physics.Raycast(_ray, out _hit, rayLength, solverLayer))
{
Quaternion _ankleRot = RootIK.RotateFromTo(_hit.normal, RootIK.TransformVector(Vector3.up, Root().rotation));
rightLeg.SetIKRotation(i == 0 ? GenericMaths.ApplyQuaternion(_ankleRot, rightLeg.GetEndEffector().rotation) : rightLeg.GetIKRotation());
leftLeg.SetIKRotation(i == 1 ? GenericMaths.ApplyQuaternion(_ankleRot, leftLeg.GetEndEffector().rotation) : leftLeg.GetIKRotation());
IKTarget[i] = _hit.point;
}
}
rightLeg.SetIKPosition(IKTarget[0]);
leftLeg.SetIKPosition(IKTarget[1]);
}
/// <summary>
/// Solve the IK chain using CCD method
/// </summary>
/// <param name="_IKChain"></param>
public void SolveCCD(RootIK.Chain _IKChain)
{
if (_IKChain.weight <= 0f)
{
return;
}
if (_IKChain.joints.Count <= 0)
{
return;
}
_IKChain.SetIKPosition(_IKChain.target ? Vector3.zero : _IKChain.GetIKPosition());
for (int j = 0; j < _IKChain.iterations; j++)
{
for (int i = _IKChain.joints.Count - 1; i >= 0; i--)
{
_IKChain.weight = Mathf.Clamp(_IKChain.weight, 0f, 1f);
float _weight = _IKChain.weight * _IKChain.joints[i].weight;
Vector3 _v0 = _IKChain.GetIKPosition() - _IKChain.joints[i].transform.position;
Vector3 _v1 = _IKChain.joints[_IKChain.joints.Count - 1].transform.position - _IKChain.joints[i].transform.position;
Quaternion _sourceRotation = _IKChain.joints[i].transform.rotation;
Quaternion _targetRotation = Quaternion.Lerp(Quaternion.identity, RootIK.RotateFromTo(_v0, _v1), _weight);
_IKChain.joints[i].transform.rotation = Quaternion.Lerp(_sourceRotation, GenericMaths.ApplyQuaternion(_targetRotation, _sourceRotation), _weight);
}
}
_IKChain.joints[_IKChain.joints.Count - 1].transform.rotation = _IKChain.GetIKRotation();
}
