From e71909e5253fceb59d6fea6eb5666f0366fbaf3d Mon Sep 17 00:00:00 2001
From: Nora <@Stereoarts>
Date: Fri, 18 Mar 2016 00:26:40 +0900
Subject: [PATCH] Putitfix for headEffector.pull > IKEpsilon (Stable fix)

---
 Scripts/FullBodyIK/BodyIK.cs | 18 ++++++++++++++++--
 1 file changed, 16 insertions(+), 2 deletions(-)

diff --git a/Scripts/FullBodyIK/BodyIK.cs b/Scripts/FullBodyIK/BodyIK.cs
index d857d1c..97773fa 100644
--- a/Scripts/FullBodyIK/BodyIK.cs
+++ b/Scripts/FullBodyIK/BodyIK.cs
@@ -2110,8 +2110,9 @@ public bool UpperSolve()
 						if( headPull > IKEpsilon || neckPull > IKEpsilon ) {
 							if( headMovingfixRate < 1.0f - IKEpsilon && headPull > IKEpsilon ) {
 								Vector3 tempNeckPos = _upperSolverPreArmsTemp.neckPos;
-								_KeepLength( ref tempNeckPos, ref _upperSolverTemp.targetHeadPos, _solverCaches.neckToHeadLength );
-								_upperSolverPreArmsTemp.neckPos = Vector3.Lerp( _upperSolverPreArmsTemp.neckPos, tempNeckPos, headPull );
+								if( _KeepMaxLength( ref tempNeckPos, ref _upperSolverTemp.targetHeadPos, _solverCaches.neckToHeadLength ) ) { // Not KeepLength
+									_upperSolverPreArmsTemp.neckPos = Vector3.Lerp( _upperSolverPreArmsTemp.neckPos, tempNeckPos, headPull );
+								}
                             }
 							for( int i = 0; i != 2; ++i ) {
 								if( bodyMovingfixRate < 1.0f - IKEpsilon && neckPull > IKEpsilon ) {
@@ -2642,6 +2643,19 @@ public void SolveShoulderToArmInternal( int i, ref Vector3 destArmPos )
 
 			//----------------------------------------------------------------------------------------------------------------------------------------
 
+			static bool _KeepMaxLength( ref Vector3 posTo, ref Vector3 posFrom, float keepLength )
+			{
+				Vector3 v = posTo - posFrom;
+				float len = SAFBIKVecLength( ref v );
+				if( len > IKEpsilon && len > keepLength ) {
+					v = v * (keepLength / len);
+					posTo = posFrom + v;
+					return true;
+				}
+
+				return false;
+			}
+
 			static bool _KeepMaxLength( ref Vector3 posTo, ref Vector3 posFrom, ref FastLength keepLength )
 			{
 				Vector3 v = posTo - posFrom;