Fix for bug #17402: IK influence blending with pole targets

didn't give smooth transition. Now it blends the result of
IK solving in that case.

3 files changed, 40 insertions, 4 deletions

diff --git a/source/blender/blenkernel/intern/armature.c b/source/blender/blenkernel/intern/armature.c
index fb7d59c137a..720ed0513ed 100644
--- a/source/blender/blenkernel/intern/armature.c
+++ b/source/blender/blenkernel/intern/armature.c
@@ -1690,7 +1690,7 @@ static void initialize_posetree(struct Object *ob, bPoseChannel *pchan_tip)
 were executed & assigned. Now as last we do an IK pass */
 static void execute_posetree(Object *ob, PoseTree *tree)
 {
-	float R_parmat[3][3];
+	float R_parmat[3][3], identity[3][3];
 	float iR_parmat[3][3];
 	float R_bonemat[3][3];
 	float goalrot[3][3], goalpos[3];
@@ -1699,7 +1699,8 @@ static void execute_posetree(Object *ob, PoseTree *tree)
 	float irest_basis[3][3], full_basis[3][3];
 	float end_pose[4][4], world_pose[4][4];
 	float length, basis[3][3], rest_basis[3][3], start[3], *ikstretch=NULL;
-	int a, flag, hasstretch=0;
+	float resultinf=0.0f;
+	int a, flag, hasstretch=0, resultblend=0;
 	bPoseChannel *pchan;
 	IK_Segment *seg, *parent, **iktree, *iktarget;
 	IK_Solver *solver;
@@ -1844,6 +1845,12 @@ static void execute_posetree(Object *ob, PoseTree *tree)
 				Mat4MulMat4(goal, rootmat, goalinv);
 				VECCOPY(polepos, goal[3]);
 				poleconstrain= 1;
+
+				/* for pole targets, we blend the result of the ik solver
+				 * instead of the target position, otherwise we can't get
+				 * a smooth transition */
+				resultblend= 1;
+				resultinf= target->con->enforce;
 				
 				if(data->flag & CONSTRAINT_IK_GETANGLE) {
 					poleangledata= data;
@@ -1853,7 +1860,7 @@ static void execute_posetree(Object *ob, PoseTree *tree)
 		}
 
 		/* do we need blending? */
-		if (target->con->enforce!=1.0) {
+		if (!resultblend && target->con->enforce!=1.0) {
 			float q1[4], q2[4], q[4];
 			float fac= target->con->enforce;
 			float mfac= 1.0-fac;
@@ -1903,7 +1910,7 @@ static void execute_posetree(Object *ob, PoseTree *tree)
 	tree->basis_change= MEM_mallocN(sizeof(float[3][3])*tree->totchannel, "ik basis change");
 	if(hasstretch)
 		ikstretch= MEM_mallocN(sizeof(float)*tree->totchannel, "ik stretch");
-		
+	
 	for(a=0; a<tree->totchannel; a++) {
 		IK_GetBasisChange(iktree[a], tree->basis_change[a]);
 		
@@ -1931,6 +1938,12 @@ static void execute_posetree(Object *ob, PoseTree *tree)
 			VecMulf(tree->basis_change[a][1], stretch);
 			VecMulf(tree->basis_change[a][2], stretch);
 		}
+
+		if(resultblend && resultinf!=1.0f) {
+			Mat3One(identity);
+			Mat3BlendMat3(tree->basis_change[a], identity,
+				tree->basis_change[a], resultinf);
+		}
 		
 		IK_FreeSegment(iktree[a]);
 	}
diff --git a/source/blender/blenlib/BLI_arithb.h b/source/blender/blenlib/BLI_arithb.h
index 4448231b2b4..6e54fae58d0 100644
--- a/source/blender/blenlib/BLI_arithb.h
+++ b/source/blender/blenlib/BLI_arithb.h
@@ -164,6 +164,7 @@ void Mat3Inv(float m1[][3], float m2[][3]);
 void Mat3CpyMat4(float m1[][3],float m2[][4]);
 void Mat4CpyMat3(float m1[][4], float m2[][3]); 
 
+void Mat3BlendMat3(float out[][3], float dst[][3], float src[][3], float srcweight);
 void Mat4BlendMat4(float out[][4], float dst[][4], float src[][4], float srcweight);
 
 float Det2x2(float a,float b,float c, float d);
diff --git a/source/blender/blenlib/intern/arithb.c b/source/blender/blenlib/intern/arithb.c
index 50f8ba0fcde..f89f90f7045 100644
--- a/source/blender/blenlib/intern/arithb.c
+++ b/source/blender/blenlib/intern/arithb.c
@@ -759,6 +759,28 @@ void Mat4MulSerie(float answ[][4], float m1[][4],
 	}
 }
 
+void Mat3BlendMat3(float out[][3], float dst[][3], float src[][3], float srcweight)
+{
+	float squat[4], dquat[4], fquat[4];
+	float ssize[3], dsize[3], fsize[4];
+	float rmat[3][3], smat[3][3];
+	
+	Mat3ToQuat(dst, dquat);
+	Mat3ToSize(dst, dsize);
+
+	Mat3ToQuat(src, squat);
+	Mat3ToSize(src, ssize);
+	
+	/* do blending */
+	QuatInterpol(fquat, dquat, squat, srcweight);
+	VecLerpf(fsize, dsize, ssize, srcweight);
+
+	/* compose new matrix */
+	QuatToMat3(fquat, rmat);
+	SizeToMat3(fsize, smat);
+	Mat3MulMat3(out, rmat, smat);
+}
+
 void Mat4BlendMat4(float out[][4], float dst[][4], float src[][4], float srcweight)
 {
 	float squat[4], dquat[4], fquat[4];