/// <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> /// 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(); }
/// <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 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> /// A helping method to update 2 IK chains based on the dist to their IK target /// </summary> /// <param name="_IKChain"></param> /// <param name="_IKChain2"></param> public void SolveCCD(RootIK.Chain _IKChain, RootIK.Chain _IKChain2) { //in CCD algorithm, the last chain to be updated will have the biggest influence on the overall results if (_IKChain.joints.Count <= 0 || _IKChain2.joints.Count <= 0) { return; } float _distToTarget = Vector3.Distance(_IKChain.GetIKPosition(), _IKChain.joints[0].transform.position); float _distToTarget2 = Vector3.Distance(_IKChain2.GetIKPosition(), _IKChain2.joints[0].transform.position); if (_distToTarget > _distToTarget2) { SolveCCD(_IKChain); SolveCCD(_IKChain2); } else { SolveCCD(_IKChain2); SolveCCD(_IKChain); } }