2.5 - Rotation Orders for Bones [Durian Rigging Request]

This commit is the start of an implementation of (euler) rotation orders for Bones (later to be extended to Objects too). 

Technical details and references can be found at:
http://wiki.blender.org/index.php/User:Aligorith/EulerRotationOrder

In short, I've added a new set of Euler conversion functions (EulO... and ...EulO), coexisting with the old functions for now, which can handle different rotation orders.

Changes have only been made to the basic evaluation code. However, the following places will still need modifications:
* Transform code - needs to be made to use functions which take rotation order into account instead of using XYZ only
* Rotation constraints - same story
* Other rotation editing tools for armatures also need a check up, since there might have been some missing code when I ported eulers earlier 

2 files changed, 237 insertions, 27 deletions

diff --git a/source/blender/blenlib/BLI_arithb.h b/source/blender/blenlib/BLI_arithb.h
index 0a0749d3382..4156e3c52ec 100644
--- a/source/blender/blenlib/BLI_arithb.h
+++ b/source/blender/blenlib/BLI_arithb.h
@@ -174,7 +174,36 @@ void CalcNormShort(short *v1, short *v2, short *v3, float *n);
 float power_of_2(float val);
 
 /**
- * @section Euler conversion routines
+ * @section Euler conversion routines (With Custom Order)
+ */
+
+/* Defines for rotation orders 
+ * WARNING: must match the ePchan_RotMode in DNA_action_types.h
+ *		   order matters - types are saved to file!
+ */
+typedef enum eEulerRotationOrders {
+	EULER_ORDER_DEFAULT = 1,	/* Blender 'default' (classic) is basically XYZ */
+	EULER_ORDER_XYZ = 1,		/* Blender 'default' (classic) - must be as 1 to sync with PoseChannel rotmode */
+	EULER_ORDER_XZY,
+	EULER_ORDER_YXZ,
+	EULER_ORDER_YZX,
+	EULER_ORDER_ZXY,
+	EULER_ORDER_ZYX,
+	/* NOTE: there are about 6 more entries when including duplicated entries too */
+} eEulerRotationOrders;
+
+void EulOToQuat(float eul[3], short order, float quat[4]);
+
+void EulOToMat3(float eul[3], short order, float Mat[3][3]);
+void EulOToMat4(float eul[3], short order, float Mat[4][4]);
+ 
+void Mat3ToEulO(float Mat[3][3], float eul[3], short order);
+void Mat4ToEulO(float Mat[4][4], float eul[3], short order);
+
+void QuatToEulO(float quat[4], float eul[3], short order);
+ 
+/**
+ * @section Euler conversion routines (Blender XYZ)
  */
 
 void EulToMat3(float *eul, float mat[][3]);
@@ -185,11 +214,14 @@ void Mat4ToEul(float tmat[][4],float *eul);
 
 void EulToQuat(float *eul, float *quat);
 
-void compatible_eul(float *eul, float *oldrot);
-
 void Mat3ToCompatibleEul(float mat[][3], float *eul, float *oldrot);
 
 
+
+void compatible_eul(float *eul, float *oldrot);
+void euler_rot(float *beul, float ang, char axis);
+
+
 /**
  * @section Quaternion arithmetic routines
  */
@@ -216,6 +248,8 @@ void printquat(char *str, float q[4]);
 void QuatInterpol(float *result, float *quat1, float *quat2, float t);
 void QuatAdd(float *result, float *quat1, float *quat2, float t);
 
+void QuatToMat3(float *q, float m[][3]);
+void QuatToMat4(float *q, float m[][4]);
 
 /**
  * @section matrix multiplication and copying routines
@@ -349,8 +383,6 @@ float NormalizedVecAngle2(float *v1, float *v2);
 
 float VecAngle3_2D(float *v1, float *v2, float *v3);
 float NormalizedVecAngle2_2D(float *v1, float *v2);
-
-void euler_rot(float *beul, float ang, char axis);
 	
 void NormalShortToFloat(float *out, short *in);
 void NormalFloatToShort(short *out, float *in);
@@ -422,9 +454,6 @@ void VecStar(float mat[][3],float *vec);
 
 short EenheidsMat(float mat[][3]);
 
-void QuatToMat3(float *q, float m[][3]);
-void QuatToMat4(float *q, float m[][4]);
-
 void Mat3ToQuat_is_ok(float wmat[][3], float *q);
 
 void i_ortho(float left, float right, float bottom, float top, float nearClip, float farClip, float matrix[][4]);
@@ -435,8 +464,6 @@ void i_rotate(float angle, char axis, float mat[][4]);
 
 
 
-
-
 void MinMax3(float *min, float *max, float *vec);
 void SizeToMat3(float *size, float mat[][3]);
 void SizeToMat4(float *size, float mat[][4]);
diff --git a/source/blender/blenlib/intern/arithb.c b/source/blender/blenlib/intern/arithb.c
index 32bacc9472c..e0f17864d73 100644
--- a/source/blender/blenlib/intern/arithb.c
+++ b/source/blender/blenlib/intern/arithb.c
@@ -1408,22 +1408,6 @@ void RotationBetweenVectorsToQuat(float *q, float v1[3], float v2[3])
 	AxisAngleToQuat(q, axis, angle);
 }
 
-void AxisAngleToQuat(float *q, float *axis, float angle)
-{
-	float nor[3];
-	float si;
-	
-	VecCopyf(nor, axis);
-	Normalize(nor);
-	
-	angle /= 2;
-	si = (float)sin(angle);
-	q[0] = (float)cos(angle);
-	q[1] = nor[0] * si;
-	q[2] = nor[1] * si;
-	q[3] = nor[2] * si;	
-}
-
 void vectoquat(float *vec, short axis, short upflag, float *q)
 {
 	float q2[4], nor[3], *fp, mat[3][3], angle, si, co, x2, y2, z2, len1;
@@ -2807,6 +2791,156 @@ void MeanValueWeights(float v[][3], int n, float *co, float *w)
 
 /* ************ EULER *************** */
 
+/* Euler Rotation Order Code:
+ * was adapted from  
+  		ANSI C code from the article
+		"Euler Angle Conversion"
+		by Ken Shoemake, shoemake@graphics.cis.upenn.edu
+		in "Graphics Gems IV", Academic Press, 1994
+ * for use in Blender
+ */
+
+/* Type for rotation order info - see wiki for derivation details */
+typedef struct RotOrderInfo {
+	short i;		/* first axis index */
+	short j;		/* second axis index */
+	short k;		/* third axis index */
+	short parity;	/* parity of axis permuation (even=0, odd=1) - 'n' in original code */
+} RotOrderInfo;
+
+/* Array of info for Rotation Order calculations 
+ * WARNING: must be kept in same order as eEulerRotationOrders
+ */
+static RotOrderInfo rotOrders[]= {
+	/* i, j, k, n */
+	{0, 1, 2, 0}, // XYZ
+	{0, 2, 1, 1}, // XZY
+	{1, 0, 2, 1}, // YXZ
+	{1, 2, 0, 0}, // YZX
+	{2, 0, 1, 0}, // ZXY
+	{2, 1, 0, 1}  // ZYZ
+};
+
+/* Get relevant pointer to rotation order set from the array 
+ * NOTE: since we start at 1 for the values, but arrays index from 0, 
+ *		 there is -1 factor involved in this process...
+ */
+#define GET_ROTATIONORDER_INFO(order) (&rotOrders[(order)-1])
+
+/* Construct quaternion from Euler angles (in radians). */
+void EulOToQuat(float e[3], short order, float q[4])
+{
+	RotOrderInfo *R= GET_ROTATIONORDER_INFO(order); 
+	short i=R->i,  j=R->j, 	k=R->k;
+	double ti, tj, th, ci, cj, ch, si, sj, sh, cc, cs, sc, ss;
+	double a[3];
+	
+	if (R->parity) e[1] = -e[1];
+	
+	ti = e[0]/2; tj = e[1]/2; th = e[2]/2;
+	
+	ci = cos(ti);  cj = cos(tj);  ch = cos(th);
+	si = sin(ti);  sj = sin(tj);  sh = sin(th);
+	
+	cc = ci*ch; cs = ci*sh; 
+	sc = si*ch; ss = si*sh;
+	
+	a[i] = cj*sc - sj*cs;
+	a[j] = cj*ss + sj*cc;
+	a[k] = cj*cs - sj*sc;
+	
+	q[0] = cj*cc + sj*ss;
+	q[1] = a[0];
+	q[2] = a[1];
+	q[3] = a[2];
+	
+	if (R->parity) q[j] = -q[j];
+}
+
+/* Construct 3x3 matrix from Euler angles (in radians). */
+void EulOToMat3(float e[3], short order, float M[3][3])
+{
+	RotOrderInfo *R= GET_ROTATIONORDER_INFO(order); 
+	short i=R->i,  j=R->j, 	k=R->k;
+	double ti, tj, th, ci, cj, ch, si, sj, sh, cc, cs, sc, ss;
+	
+	if (R->parity) {
+		e[0] = -e[0]; 
+		e[1] = -e[1]; 
+		e[2] = -e[2];
+	}
+	
+	ti = e[0];	  tj = e[1];	th = e[2];
+	
+	ci = cos(ti); cj = cos(tj); ch = cos(th);
+	si = sin(ti); sj = sin(tj); sh = sin(th);
+	
+	cc = ci*ch; cs = ci*sh; 
+	sc = si*ch; ss = si*sh;
+	
+	M[i][i] = cj*ch; M[j][i] = sj*sc-cs; M[k][i] = sj*cc+ss;
+	M[i][j] = cj*sh; M[j][j] = sj*ss+cc; M[k][j] = sj*cs-sc;
+	M[i][k] = -sj;	 M[j][k] = cj*si;	 M[k][k] = cj*ci;
+}
+
+/* Construct 4x4 matrix from Euler angles (in radians). */
+void EulOToMat4(float e[3], short order, float M[4][4])
+{
+	float m[3][3];
+	
+	/* for now, we'll just do this the slow way (i.e. copying matrices) */
+	Mat3Ortho(m);
+	EulOToMat3(e, order, m);
+	Mat4CpyMat3(M, m);
+}
+
+/* Convert 3x3 matrix to Euler angles (in radians). */
+void Mat3ToEulO(float M[3][3], float e[3], short order)
+{
+	RotOrderInfo *R= GET_ROTATIONORDER_INFO(order); 
+	short i=R->i,  j=R->j, 	k=R->k;
+	double cy = sqrt(M[i][i]*M[i][i] + M[j][i]*M[j][i]);
+	
+	if (cy > 16*FLT_EPSILON) {
+		e[0] = atan2(M[j][k], M[k][k]);
+		e[1] = atan2(-M[i][k], cy);
+		e[2] = atan2(M[i][j], M[i][i]);
+	} 
+	else {
+		e[0] = atan2(-M[k][j], M[j][j]);
+		e[1] = atan2(-M[i][k], cy);
+		e[2] = 0;
+	}
+	
+	if (R->parity) {
+		e[0] = -e[0]; 
+		e[1] = -e[1]; 
+		e[2] = -e[2];
+	}
+}
+
+/* Convert 4x4 matrix to Euler angles (in radians). */
+void Mat4ToEulO(float M[4][4], float e[3], short order)
+{
+	float m[3][3];
+	
+	/* for now, we'll just do this the slow way (i.e. copying matrices) */
+	Mat3CpyMat4(m, M);
+	Mat3ToEulO(m, e, order);
+}
+
+/* Convert quaternion to Euler angles (in radians). */
+void QuatToEulO(float q[4], float e[3], short order)
+{
+	float M[3][3];
+	
+	QuatToMat3(q, M);
+	Mat3ToEulO(M, e, order);
+}
+
+/* ************ EULER (old XYZ) *************** */
+
+/* XYZ order */
 void EulToMat3( float *eul, float mat[][3])
 {
 	double ci, cj, ch, si, sj, sh, cc, cs, sc, ss;
@@ -2834,6 +2968,7 @@ void EulToMat3( float *eul, float mat[][3])
 
 }
 
+/* XYZ order */
 void EulToMat4( float *eul,float mat[][4])
 {
 	double ci, cj, ch, si, sj, sh, cc, cs, sc, ss;
@@ -2865,6 +3000,7 @@ void EulToMat4( float *eul,float mat[][4])
 }
 
 /* returns two euler calculation methods, so we can pick the best */
+/* XYZ order */
 static void mat3_to_eul2(float tmat[][3], float *eul1, float *eul2)
 {
 	float cy, quat[4], mat[3][3];
@@ -2895,6 +3031,7 @@ static void mat3_to_eul2(float tmat[][3], float *eul1, float *eul2)
 	}
 }
 
+/* XYZ order */
 void Mat3ToEul(float tmat[][3], float *eul)
 {
 	float eul1[3], eul2[3];
@@ -2910,6 +3047,7 @@ void Mat3ToEul(float tmat[][3], float *eul)
 	}
 }
 
+/* XYZ order */
 void Mat4ToEul(float tmat[][4], float *eul)
 {
 	float tempMat[3][3];
@@ -2919,6 +3057,7 @@ void Mat4ToEul(float tmat[][4], float *eul)
 	Mat3ToEul(tempMat, eul);
 }
 
+/* XYZ order */
 void QuatToEul(float *quat, float *eul)
 {
 	float mat[3][3];
@@ -2927,7 +3066,7 @@ void QuatToEul(float *quat, float *eul)
 	Mat3ToEul(mat, eul);
 }
 
-
+/* XYZ order */
 void EulToQuat(float *eul, float *quat)
 {
     float ti, tj, th, ci, cj, ch, si, sj, sh, cc, cs, sc, ss;
@@ -2943,6 +3082,7 @@ void EulToQuat(float *eul, float *quat)
 	quat[3] = cj*cs - sj*sc;
 }
 
+/* XYZ order */
 void euler_rot(float *beul, float ang, char axis)
 {
 	float eul[3], mat1[3][3], mat2[3][3], totmat[3][3];
@@ -2962,6 +3102,7 @@ void euler_rot(float *beul, float ang, char axis)
 }
 
 /* exported to transform.c */
+/* XYZ order */
 void compatible_eul(float *eul, float *oldrot)
 {
 	float dx, dy, dz;
@@ -3025,6 +3166,7 @@ void compatible_eul(float *eul, float *oldrot)
 }
 
 /* uses 2 methods to retrieve eulers, and picks the closest */
+/* XYZ order */
 void Mat3ToCompatibleEul(float mat[][3], float *eul, float *oldrot)
 {
 	float eul1[3], eul2[3];
@@ -3048,6 +3190,46 @@ void Mat3ToCompatibleEul(float mat[][3], float *eul, float *oldrot)
 	
 }
 
+/* ************ AXIS ANGLE *************** */
+
+/* Axis angle to Quaternions */
+void AxisAngleToQuat(float *q, float *axis, float angle)
+{
+	float nor[3];
+	float si;
+	
+	VecCopyf(nor, axis);
+	Normalize(nor);
+	
+	angle /= 2;
+	si = (float)sin(angle);
+	q[0] = (float)cos(angle);
+	q[1] = nor[0] * si;
+	q[2] = nor[1] * si;
+	q[3] = nor[2] * si;	
+}
+
+/* Quaternions to Axis Angle */
+void QuatToAxisAngle(float q[4], float axis[3], float *angle)
+{
+	float ha, si;
+	
+	/* calculate angle/2, and sin(angle/2) */
+	ha= (float)acos(q[0]);
+	si= (float)sin(ha);
+	
+	/* from half-angle to angle */
+	*angle= ha * 2;
+	
+	/* prevent division by zero for axis conversion */
+	if (fabs(si) < 0.0005)
+		si= 1.0f;
+	
+	axis[0]= q[1] / si;
+	axis[1]= q[2] / si;
+	axis[2]= q[3] / si;
+}
+
 /* axis angle to 3x3 matrix */
 void VecRotToMat3(float *vec, float phi, float mat[][3])
 {
@@ -4704,6 +4886,7 @@ float PdistVL3Dfl(float *v1, float *v2, float *v3)
 
 /* make a 4x4 matrix out of 3 transform components */
 /* matrices are made in the order: scale * rot * loc */
+// TODO: need to have a version that allows for rotation order...
 void LocEulSizeToMat4(float mat[][4], float loc[3], float eul[3], float size[3])
 {
 	float rmat[3][3], smat[3][3], tmat[3][3];