Fix for bug #5250: inaccurate conversion between edit and pose mode bones.

Using acos(dot(u, v)) to find the angle between two vectors is quite
inaccurate, and there's a better way to do it, as explained here:
http://www.plunk.org/~hatch/rightway.php

Also changed the use of atan for computing roll to atan2 in some places,
the latter avoids accuracy and division by zero issues.

5 files changed, 46 insertions, 16 deletions

diff --git a/source/blender/blenkernel/intern/armature.c b/source/blender/blenkernel/intern/armature.c
index 0a1787cb58c..21f833fb32f 100644
--- a/source/blender/blenkernel/intern/armature.c
+++ b/source/blender/blenkernel/intern/armature.c
@@ -869,11 +869,11 @@ void vec_roll_to_mat3(float *vec, float roll, float mat[][3])
 	
 	/*	Find Axis & Amount for bone matrix*/
 	Crossf (axis,target,nor);
-	
+
 	if (Inpf(axis,axis) > 0.0000000000001) {
 		/* if nor is *not* a multiple of target ... */
 		Normalise (axis);
-		theta=(float) acos (Inpf (target,nor));
+		theta= NormalizedVecAngle2(target, nor);
 		
 		/*	Make Bone matrix*/
 		VecRotToMat3(axis, theta, bMatrix);
diff --git a/source/blender/blenlib/BLI_arithb.h b/source/blender/blenlib/BLI_arithb.h
index c2a81ebed65..c478262d500 100644
--- a/source/blender/blenlib/BLI_arithb.h
+++ b/source/blender/blenlib/BLI_arithb.h
@@ -537,6 +537,10 @@ saacos(
 	float fac
 );
 	float 
+saasin(
+	float fac
+);
+	float 
 sasqrt(
 	float fac
 );
@@ -661,6 +665,11 @@ VecAngle2(
 	float *v1, 
 	float *v2
 );
+	float
+NormalizedVecAngle2(
+	float *v1,
+	float *v2
+);
 
 	void 
 i_lookat(
diff --git a/source/blender/blenlib/intern/arithb.c b/source/blender/blenlib/intern/arithb.c
index 3fd65ca534c..51000b3b155 100644
--- a/source/blender/blenlib/intern/arithb.c
+++ b/source/blender/blenlib/intern/arithb.c
@@ -82,6 +82,13 @@ float saacos(float fac)
 	else return (float)acos(fac);
 }
 
+float saasin(float fac)
+{
+	if(fac<= -1.0f) return (float)-M_PI/2.0f;
+	else if(fac>=1.0f) return (float)M_PI/2.0f;
+	else return (float)asin(fac);
+}
+
 float sasqrt(float fac)
 {
 	if(fac<=0.0) return 0.0;
@@ -2382,7 +2389,7 @@ void VecRotToQuat( float *vec, float phi, float *quat)
 /* Return the angle in degrees between vecs 1-2 and 2-3 in degrees
    If v1 is a shoulder, v2 is the elbow and v3 is the hand,
    this would return the angle at the elbow */
-float VecAngle3( float *v1, float *v2, float *v3)
+float VecAngle3(float *v1, float *v2, float *v3)
 {
 	float vec1[3], vec2[3];
 
@@ -2390,20 +2397,36 @@ float VecAngle3( float *v1, float *v2, float *v3)
 	VecSubf(vec2, v2, v3);
 	Normalise(vec1);
 	Normalise(vec2);
-	return saacos(vec1[0]*vec2[0] + vec1[1]*vec2[1] + vec1[2]*vec2[2]) * 180.0/M_PI;
+
+	return NormalizedVecAngle2(vec1, vec2) * 180.0/M_PI;
 }
 
 /* Return the shortest angle in degrees between the 2 vectors */
-float VecAngle2( float *v1, float *v2)
+float VecAngle2(float *v1, float *v2)
 {
 	float vec1[3], vec2[3];
+
 	VecCopyf(vec1, v1);
 	VecCopyf(vec2, v2);
 	Normalise(vec1);
 	Normalise(vec2);
-	return saacos(vec1[0]*vec2[0] + vec1[1]*vec2[1] + vec1[2]*vec2[2]) * 180.0/M_PI;
+
+	return NormalizedVecAngle2(vec1, vec2)* 180.0/M_PI;
 }
 
+float NormalizedVecAngle2(float *v1, float *v2)
+{
+	/* this is the same as acos(Inpf(v1, v2)), but more accurate */
+	if (Inpf(v1, v2) < 0.0f) {
+		v2[0]= -v2[0];
+		v2[1]= -v2[1];
+		v2[2]= -v2[2];
+
+		return (float)M_PI - 2.0f*saasin(VecLenf(v2, v1)/2.0f);
+	}
+	else
+		return 2.0f*saasin(VecLenf(v2, v1)/2.0);
+}
 
 void euler_rot(float *beul, float ang, char axis)
 {
diff --git a/source/blender/python/api2_2x/Bone.c b/source/blender/python/api2_2x/Bone.c
index 0d6594e2aef..e00ccd00477 100644
--- a/source/blender/python/api2_2x/Bone.c
+++ b/source/blender/python/api2_2x/Bone.c
@@ -69,7 +69,7 @@ static double boneRoll_ToArmatureSpace(struct Bone *bone)
 	Mat3Inv(imat, postmat);					
 	Mat3MulMat3(difmat, imat, premat);	
 
-	roll = atan(difmat[2][0] / difmat[2][2]); 
+	roll = atan2(difmat[2][0], difmat[2][2]); 
 	if (difmat[0][0] < 0.0){
 		roll += M_PI;
 	}
diff --git a/source/blender/src/editarmature.c b/source/blender/src/editarmature.c
index b96242941f7..eeb1dca76a7 100644
--- a/source/blender/src/editarmature.c
+++ b/source/blender/src/editarmature.c
@@ -150,11 +150,11 @@ void make_boneList(ListBase* list, ListBase *bones, EditBone *parent)
 		vec_roll_to_mat3(delta, 0.0, postmat);
 		
 		Mat3CpyMat4(premat, curBone->arm_mat);
-		
+
 		Mat3Inv(imat, postmat);
 		Mat3MulMat3(difmat, imat, premat);
 		
-		eBone->roll = atan(difmat[2][0]/difmat[2][2]);
+		eBone->roll = atan2(difmat[2][0], difmat[2][2]);
 		if (difmat[0][0]<0.0) eBone->roll +=M_PI;
 		
 		/* rest of stuff copy */
@@ -214,10 +214,9 @@ static void fix_bonelist_roll (ListBase *bonelist, ListBase *editbonelist)
 			printmatrix4 ("premat", premat);
 			printmatrix4 ("postmat", postmat);
 			printmatrix4 ("difmat", difmat);
-			printf ("Roll = %f\n",  (-atan(difmat[2][0]/difmat[2][2]) * (180.0/M_PI)));
+			printf ("Roll = %f\n",  (-atan2(difmat[2][0], difmat[2][2]) * (180.0/M_PI)));
 #endif
-			curBone->roll = -atan(difmat[2][0]/difmat[2][2]);
-			
+			curBone->roll = -atan2(difmat[2][0], difmat[2][2]);
 			if (difmat[0][0]<0.0) curBone->roll +=M_PI;
 			
 			/* and set restposition again */
@@ -430,8 +429,8 @@ int join_armature(void)
 						Mat4Invert (imat, premat);
 						Mat4MulMat4 (difmat, postmat, imat);
 						
-						curbone->roll -=atan(difmat[2][0]/difmat[2][2]);
-						
+						curbone->roll -=atan2(difmat[2][0], difmat[2][2]);
+
 						if (difmat[0][0]<0)
 							curbone->roll +=M_PI;
 						
@@ -1130,8 +1129,7 @@ void auto_align_armature(void)
 				Mat3Inv(imat, targetmat);
 				Mat3MulMat3(diffmat, imat, curmat);
 				
-				ebone->roll = atan(diffmat[2][0]/diffmat[2][2]);
-				
+				ebone->roll = atan2(diffmat[2][0], diffmat[2][2]);
 			}
 		}
 	}