1 files changed, 46 insertions, 18 deletions

diff --git a/source/blender/blenlib/intern/math_rotation.c b/source/blender/blenlib/intern/math_rotation.c
index 4af0a3a4248..f72269f6d7b 100644
--- a/source/blender/blenlib/intern/math_rotation.c
+++ b/source/blender/blenlib/intern/math_rotation.c
@@ -36,7 +36,7 @@ void unit_qt(float *q)
 	q[1]= q[2]= q[3]= 0.0f;
 }
 
-void copy_qt_qt(float *q1, float *q2)
+void copy_qt_qt(float *q1, const float *q2)
 {
 	q1[0]= q2[0];
 	q1[1]= q2[1];
@@ -49,7 +49,7 @@ int is_zero_qt(float *q)
 	return (q[0] == 0 && q[1] == 0 && q[2] == 0 && q[3] == 0);
 }
 
-void mul_qt_qtqt(float *q, float *q1, float *q2)
+void mul_qt_qtqt(float *q, const float *q1, const float *q2)
 {
 	float t0,t1,t2;
 
@@ -63,7 +63,7 @@ void mul_qt_qtqt(float *q, float *q1, float *q2)
 }
 
 /* Assumes a unit quaternion */
-void mul_qt_v3(float *q, float *v)
+void mul_qt_v3(const float *q, float *v)
 {
 	float t0, t1, t2;
 
@@ -104,8 +104,14 @@ void invert_qt(float *q)
 	mul_qt_fl(q, 1.0f/f);
 }
 
+void invert_qt_qt(float *q1, const float *q2)
+{
+	copy_qt_qt(q1, q2);
+	invert_qt(q1);
+}
+
 /* simple mult */
-void mul_qt_fl(float *q, float f)
+void mul_qt_fl(float *q, const float f)
 {
 	q[0] *= f;
 	q[1] *= f;
@@ -121,7 +127,7 @@ void sub_qt_qtqt(float *q, float *q1, float *q2)
 }
 
 /* angular mult factor */
-void mul_fac_qt_fl(float *q, float fac)
+void mul_fac_qt_fl(float *q, const float fac)
 {
 	float angle= fac*saacos(q[0]);	/* quat[0]= cos(0.5*angle), but now the 0.5 and 2.0 rule out */
 	
@@ -129,10 +135,7 @@ void mul_fac_qt_fl(float *q, float fac)
 	float si= (float)sin(angle);
 	q[0]= co;
 	normalize_v3(q+1);
-	q[1]*= si;
-	q[2]*= si;
-	q[3]*= si;
-	
+	mul_v3_fl(q+1, si);
 }
 
 void quat_to_mat3(float m[][3], float *q)
@@ -336,6 +339,23 @@ void rotation_between_vecs_to_quat(float *q, const float v1[3], const float v2[3
 	axis_angle_to_quat(q, axis, angle);
 }
 
+void rotation_between_quats_to_quat(float *q, const float q1[4], const float q2[4])
+{
+	float tquat[4];
+	double dot = 0.0f;
+	int x;
+
+	copy_qt_qt(tquat, q1);
+	conjugate_qt(tquat);
+	dot = 1.0f / dot_qtqt(tquat, tquat);
+
+	for(x = 0; x < 4; x++)
+		tquat[x] *= dot;
+
+	mul_qt_qtqt(q, tquat, q2);
+}
+
+
 void vec_to_quat(float *q,float *vec, short axis, short upflag)
 {
 	float q2[4], nor[3], *fp, mat[3][3], angle, si, co, x2, y2, z2, len1;
@@ -572,9 +592,8 @@ void axis_angle_to_quat(float q[4], float axis[3], float angle)
 {
 	float nor[3];
 	float si;
-	
-	copy_v3_v3(nor, axis);
-	normalize_v3(nor);
+
+	normalize_v3_v3(nor, axis);
 	
 	angle /= 2;
 	si = (float)sin(angle);
@@ -631,8 +650,7 @@ void axis_angle_to_mat3(float mat[3][3],float axis[3], float angle)
 	float nor[3], nsi[3], co, si, ico;
 	
 	/* normalise the axis first (to remove unwanted scaling) */
-	copy_v3_v3(nor, axis);
-	normalize_v3(nor);
+	normalize_v3_v3(nor, axis);
 	
 	/* now convert this to a 3x3 matrix */
 	co= (float)cos(angle);		
@@ -919,8 +937,8 @@ void rotate_eul(float *beul, char axis, float ang)
 	float eul[3], mat1[3][3], mat2[3][3], totmat[3][3];
 	
 	eul[0]= eul[1]= eul[2]= 0.0f;
-	if(axis=='x') eul[0]= ang;
-	else if(axis=='y') eul[1]= ang;
+	if(axis=='X') eul[0]= ang;
+	else if(axis=='Y') eul[1]= ang;
 	else eul[2]= ang;
 	
 	eul_to_mat3(mat1,eul);
@@ -1238,9 +1256,9 @@ void rotate_eulO(float beul[3], short order, char axis, float ang)
 	float eul[3], mat1[3][3], mat2[3][3], totmat[3][3];
 	
 	eul[0]= eul[1]= eul[2]= 0.0f;
-	if (axis=='x') 
+	if (axis=='X') 
 		eul[0]= ang;
-	else if (axis=='y') 
+	else if (axis=='Y')
 		eul[1]= ang;
 	else 
 		eul[2]= ang;
@@ -1503,3 +1521,13 @@ void copy_dq_dq(DualQuat *dq1, DualQuat *dq2)
 	memcpy(dq1, dq2, sizeof(DualQuat));
 }
 
+/* lense/angle conversion (radians) */
+float lens_to_angle(float lens)
+{
+	return 2.0f * atan(16.0f/lens);
+}
+
+float angle_to_lens(float angle)
+{
+	return 16.0f / tan(angle * 0.5f);
+}