replace VECCOPY and QUATCOPY with inline funcs.
[blender.git] / source / blender / blenlib / intern / math_rotation.c
index ea78518..0ca8b72 100644 (file)
@@ -1,6 +1,4 @@
-/**
- * $Id$
- *
+/*
  * ***** BEGIN GPL LICENSE BLOCK *****
  *
  * This program is free software; you can redistribute it and/or
@@ -15,7 +13,7 @@
  *
  * You should have received a copy of the GNU General Public License
  * along with this program; if not, write to the Free Software Foundation,
- * Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
+ * Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
  *
  * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV.
  * All rights reserved.
  * ***** END GPL LICENSE BLOCK *****
  * */
 
-#include <float.h>
-#include <stdio.h>
-#include <stdlib.h>
-#include <string.h>
+/** \file blender/blenlib/intern/math_rotation.c
+ *  \ingroup bli
+ */
+
 
+
+#include <assert.h>
 #include "BLI_math.h"
 
 /******************************** Quaternions ********************************/
 
-void unit_qt(float *q)
+/* used to test is a quat is not normalized */
+#define QUAT_EPSILON 0.0001
+
+/* convenience, avoids setting Y axis everywhere */
+void unit_axis_angle(float axis[3], float *angle)
+{
+       axis[0]= 0.0f;
+       axis[1]= 1.0f;
+       axis[2]= 0.0f;
+       *angle= 0.0f;
+}
+
+
+void unit_qt(float q[4])
 {
        q[0]= 1.0f;
        q[1]= q[2]= q[3]= 0.0f;
 }
 
-void copy_qt_qt(float *q1, float *q2)
+void copy_qt_qt(float q1[4], const float q2[4])
 {
        q1[0]= q2[0];
        q1[1]= q2[1];
@@ -53,7 +66,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;
 
@@ -67,7 +80,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;
 
@@ -92,7 +105,7 @@ void conjugate_qt(float *q)
        q[3] = -q[3];
 }
 
-float dot_qtqt(float *q1, float *q2)
+float dot_qtqt(const float q1[4], const float q2[4])
 {
        return q1[0]*q2[0] + q1[1]*q2[1] + q1[2]*q2[2] + q1[3]*q2[3];
 }
@@ -108,8 +121,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;
@@ -117,15 +136,20 @@ void mul_qt_fl(float *q, float f)
        q[3] *= f;
 }
 
-void sub_qt_qtqt(float *q, float *q1, float *q2)
+void sub_qt_qtqt(float q[4], const float q1[4], const float q2[4])
 {
-       q2[0]= -q2[0];
-       mul_qt_qtqt(q, q1, q2);
-       q2[0]= -q2[0];
+       float nq2[4];
+
+       nq2[0]= -q2[0];
+       nq2[1]=  q2[1];
+       nq2[2]=  q2[2];
+       nq2[3]=  q2[3];
+
+       mul_qt_qtqt(q, q1, nq2);
 }
 
 /* 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 */
        
@@ -133,20 +157,18 @@ 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)
+/* skip error check, currently only needed by mat3_to_quat_is_ok */
+static void quat_to_mat3_no_error(float m[][3], const float q[4])
 {
        double q0, q1, q2, q3, qda,qdb,qdc,qaa,qab,qac,qbb,qbc,qcc;
 
-       q0= M_SQRT2 * q[0];
-       q1= M_SQRT2 * q[1];
-       q2= M_SQRT2 * q[2];
-       q3= M_SQRT2 * q[3];
+       q0= M_SQRT2 * (double)q[0];
+       q1= M_SQRT2 * (double)q[1];
+       q2= M_SQRT2 * (double)q[2];
+       q3= M_SQRT2 * (double)q[3];
 
        qda= q0*q1;
        qdb= q0*q2;
@@ -171,14 +193,33 @@ void quat_to_mat3(float m[][3], float *q)
        m[2][2]= (float)(1.0-qaa-qbb);
 }
 
-void quat_to_mat4(float m[][4], float *q)
+
+void quat_to_mat3(float m[][3], const float q[4])
+{
+#ifdef DEBUG
+       float f;
+       if(!((f=dot_qtqt(q, q))==0.0f || (fabsf(f-1.0f) < (float)QUAT_EPSILON))) {
+               fprintf(stderr, "Warning! quat_to_mat3() called with non-normalized: size %.8f *** report a bug ***\n", f);
+       }
+#endif
+
+       quat_to_mat3_no_error(m, q);
+}
+
+void quat_to_mat4(float m[][4], const float q[4])
 {
        double q0, q1, q2, q3, qda,qdb,qdc,qaa,qab,qac,qbb,qbc,qcc;
 
-       q0= M_SQRT2 * q[0];
-       q1= M_SQRT2 * q[1];
-       q2= M_SQRT2 * q[2];
-       q3= M_SQRT2 * q[3];
+#ifdef DEBUG
+       if(!((q0=dot_qtqt(q, q))==0.0f || (fabs(q0-1.0) < QUAT_EPSILON))) {
+               fprintf(stderr, "Warning! quat_to_mat4() called with non-normalized: size %.8f *** report a bug ***\n", (float)q0);
+       }
+#endif
+
+       q0= M_SQRT2 * (double)q[0];
+       q1= M_SQRT2 * (double)q[1];
+       q2= M_SQRT2 * (double)q[2];
+       q3= M_SQRT2 * (double)q[3];
 
        qda= q0*q1;
        qdb= q0*q2;
@@ -218,9 +259,9 @@ void mat3_to_quat(float *q, float wmat[][3])
        copy_m3_m3(mat, wmat);
        normalize_m3(mat);                      /* this is needed AND a NormalQuat in the end */
        
-       tr= 0.25*(1.0+mat[0][0]+mat[1][1]+mat[2][2]);
+       tr= 0.25* (double)(1.0f+mat[0][0]+mat[1][1]+mat[2][2]);
        
-       if(tr>FLT_EPSILON) {
+       if(tr>(double)FLT_EPSILON) {
                s= sqrt(tr);
                q[0]= (float)s;
                s= 1.0/(4.0*s);
@@ -230,7 +271,7 @@ void mat3_to_quat(float *q, float wmat[][3])
        }
        else {
                if(mat[0][0] > mat[1][1] && mat[0][0] > mat[2][2]) {
-                       s= 2.0*sqrtf(1.0 + mat[0][0] - mat[1][1] - mat[2][2]);
+                       s= 2.0*sqrtf(1.0f + mat[0][0] - mat[1][1] - mat[2][2]);
                        q[1]= (float)(0.25*s);
 
                        s= 1.0/s;
@@ -239,7 +280,7 @@ void mat3_to_quat(float *q, float wmat[][3])
                        q[3]= (float)((mat[2][0] + mat[0][2])*s);
                }
                else if(mat[1][1] > mat[2][2]) {
-                       s= 2.0*sqrtf(1.0 + mat[1][1] - mat[0][0] - mat[2][2]);
+                       s= 2.0*sqrtf(1.0f + mat[1][1] - mat[0][0] - mat[2][2]);
                        q[2]= (float)(0.25*s);
 
                        s= 1.0/s;
@@ -269,21 +310,74 @@ void mat4_to_quat(float *q, float m[][4])
        mat3_to_quat(q,mat);
 }
 
-void normalize_qt(float *q)
+void mat3_to_quat_is_ok(float q[4], float wmat[3][3])
+{
+       float mat[3][3], matr[3][3], matn[3][3], q1[4], q2[4], angle, si, co, nor[3];
+
+       /* work on a copy */
+       copy_m3_m3(mat, wmat);
+       normalize_m3(mat);
+       
+       /* rotate z-axis of matrix to z-axis */
+
+       nor[0] = mat[2][1];             /* cross product with (0,0,1) */
+       nor[1] =  -mat[2][0];
+       nor[2] = 0.0;
+       normalize_v3(nor);
+       
+       co= mat[2][2];
+       angle= 0.5f*saacos(co);
+       
+       co= (float)cos(angle);
+       si= (float)sin(angle);
+       q1[0]= co;
+       q1[1]= -nor[0]*si;              /* negative here, but why? */
+       q1[2]= -nor[1]*si;
+       q1[3]= -nor[2]*si;
+
+       /* rotate back x-axis from mat, using inverse q1 */
+       quat_to_mat3_no_error( matr,q1);
+       invert_m3_m3(matn, matr);
+       mul_m3_v3(matn, mat[0]);
+       
+       /* and align x-axes */
+       angle= (float)(0.5*atan2(mat[0][1], mat[0][0]));
+       
+       co= (float)cos(angle);
+       si= (float)sin(angle);
+       q2[0]= co;
+       q2[1]= 0.0f;
+       q2[2]= 0.0f;
+       q2[3]= si;
+       
+       mul_qt_qtqt(q, q1, q2);
+}
+
+
+float normalize_qt(float *q)
 {
        float len;
        
        len= (float)sqrt(dot_qtqt(q, q));
-       if(len!=0.0) {
+       if(len!=0.0f) {
                mul_qt_fl(q, 1.0f/len);
        }
        else {
                q[1]= 1.0f;
                q[0]= q[2]= q[3]= 0.0f;                 
        }
+
+       return len;
+}
+
+float normalize_qt_qt(float r[4], const float q[4])
+{
+       copy_qt_qt(r, q);
+       return normalize_qt(r);
 }
 
-void rotation_between_vecs_to_quat(float *q, float v1[3], float v2[3])
+/* note: expects vectors to be normalized */
+void rotation_between_vecs_to_quat(float *q, const float v1[3], const float v2[3])
 {
        float axis[3];
        float angle;
@@ -295,10 +389,30 @@ void rotation_between_vecs_to_quat(float *q, float v1[3], float v2[3])
        axis_angle_to_quat(q, axis, angle);
 }
 
-void vec_to_quat(float *q,float *vec, short axis, short upflag)
+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[4], const float vec[3], short axis, const short upflag)
 {
        float q2[4], nor[3], *fp, mat[3][3], angle, si, co, x2, y2, z2, len1;
        
+       assert(axis >= 0 && axis <= 5);
+       assert(upflag >= 0 && upflag <= 2);
+       
        /* first rotate to axis */
        if(axis>2) {    
                x2= vec[0] ; y2= vec[1] ; z2= vec[2];
@@ -312,7 +426,7 @@ void vec_to_quat(float *q,float *vec, short axis, short upflag)
        q[1]=q[2]=q[3]= 0.0;
 
        len1= (float)sqrt(x2*x2+y2*y2+z2*z2);
-       if(len1 == 0.0) return;
+       if(len1 == 0.0f) return;
 
        /* nasty! I need a good routine for this...
         * problem is a rotation of an Y axis to the negative Y-axis for example.
@@ -376,8 +490,8 @@ void vec_to_quat(float *q,float *vec, short axis, short upflag)
                        else angle= (float)(-0.5*atan2(-fp[0], -fp[1]));
                }
                                
-               co= (float)cos(angle);
-               si= (float)(sin(angle)/len1);
+               co= cosf(angle);
+               si= sinf(angle)/len1;
                q2[0]= co;
                q2[1]= x2*si;
                q2[2]= y2*si;
@@ -430,7 +544,7 @@ void QuatInterpolW(float *result, float *quat1, float *quat2, float t)
 }
 #endif
 
-void interp_qt_qtqt(float *result, float *quat1, float *quat2, float t)
+void interp_qt_qtqt(float result[4], const float quat1[4], const float quat2[4], const float t)
 {
        float quat[4], omega, cosom, sinom, sc1, sc2;
 
@@ -467,7 +581,7 @@ void interp_qt_qtqt(float *result, float *quat1, float *quat2, float t)
        result[3] = sc1 * quat[3] + sc2 * quat2[3];
 }
 
-void add_qt_qtqt(float *result, float *quat1, float *quat2, float t)
+void add_qt_qtqt(float result[4], const float quat1[4], const float quat2[4], const float t)
 {
        result[0]= quat1[0] + t*quat2[0];
        result[1]= quat1[1] + t*quat2[1];
@@ -475,7 +589,7 @@ void add_qt_qtqt(float *result, float *quat1, float *quat2, float t)
        result[3]= quat1[3] + t*quat2[3];
 }
 
-void tri_to_quat(float *quat, float *v1,  float *v2,  float *v3)
+void tri_to_quat(float quat[4], const float v1[3], const float v2[3], const float v3[3])
 {
        /* imaginary x-axis, y-axis triangle is being rotated */
        float vec[3], q1[4], q2[4], n[3], si, co, angle, mat[3][3], imat[3][3];
@@ -515,11 +629,11 @@ void tri_to_quat(float *quat, float *v1,  float *v2,  float *v3)
        q2[1]= 0.0f;
        q2[2]= 0.0f;
        q2[3]= si;
-       
+
        mul_qt_qtqt(quat, q1, q2);
 }
 
-void print_qt(char *str,  float q[4])
+void print_qt(const char *str,  const float q[4])
 {
        printf("%s: %.3f %.3f %.3f %.3f\n", str, q[0], q[1], q[2], q[3]);
 }
@@ -527,14 +641,16 @@ void print_qt(char *str,  float q[4])
 /******************************** Axis Angle *********************************/
 
 /* Axis angle to Quaternions */
-void axis_angle_to_quat(float q[4], float axis[3], float angle)
+void axis_angle_to_quat(float q[4], const float axis[3], float angle)
 {
        float nor[3];
        float si;
-       
-       copy_v3_v3(nor, axis);
-       normalize_v3(nor);
-       
+
+       if(normalize_v3_v3(nor, axis) == 0.0f) {
+               unit_qt(q);
+               return;
+       }
+
        angle /= 2;
        si = (float)sin(angle);
        q[0] = (float)cos(angle);
@@ -544,10 +660,16 @@ void axis_angle_to_quat(float q[4], float axis[3], float angle)
 }
 
 /* Quaternions to Axis Angle */
-void quat_to_axis_angle(float axis[3], float *angle,float q[4])
+void quat_to_axis_angle(float axis[3], float *angle, const float q[4])
 {
        float ha, si;
-       
+
+#ifdef DEBUG
+       if(!((ha=dot_qtqt(q, q))==0.0f || (fabsf(ha-1.0f) < (float)QUAT_EPSILON))) {
+               fprintf(stderr, "Warning! quat_to_axis_angle() called with non-normalized: size %.8f *** report a bug ***\n", ha);
+       }
+#endif
+
        /* calculate angle/2, and sin(angle/2) */
        ha= (float)acos(q[0]);
        si= (float)sin(ha);
@@ -565,7 +687,7 @@ void quat_to_axis_angle(float axis[3], float *angle,float q[4])
 }
 
 /* Axis Angle to Euler Rotation */
-void axis_angle_to_eulO(float eul[3], short order,float axis[3], float angle)
+void axis_angle_to_eulO(float eul[3], const short order, const float axis[3], const float angle)
 {
        float q[4];
        
@@ -575,7 +697,7 @@ void axis_angle_to_eulO(float eul[3], short order,float axis[3], float angle)
 }
 
 /* Euler Rotation to Axis Angle */
-void eulO_to_axis_angle(float axis[3], float *angle,float eul[3], short order)
+void eulO_to_axis_angle(float axis[3], float *angle, const float eul[3], const short order)
 {
        float q[4];
        
@@ -585,13 +707,15 @@ void eulO_to_axis_angle(float axis[3], float *angle,float eul[3], short order)
 }
 
 /* axis angle to 3x3 matrix - safer version (normalisation of axis performed) */
-void axis_angle_to_mat3(float mat[3][3],float axis[3], float angle)
+void axis_angle_to_mat3(float mat[3][3], const float axis[3], const 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);
+       if(normalize_v3_v3(nor, axis) == 0.0f) {
+               unit_m3(mat);
+               return;
+       }
        
        /* now convert this to a 3x3 matrix */
        co= (float)cos(angle);          
@@ -614,7 +738,7 @@ void axis_angle_to_mat3(float mat[3][3],float axis[3], float angle)
 }
 
 /* axis angle to 4x4 matrix - safer version (normalisation of axis performed) */
-void axis_angle_to_mat4(float mat[4][4],float axis[3], float angle)
+void axis_angle_to_mat4(float mat[4][4], const float axis[3], const float angle)
 {
        float tmat[3][3];
        
@@ -645,33 +769,57 @@ void mat4_to_axis_angle(float axis[3], float *angle,float mat[4][4])
        quat_to_axis_angle(axis, angle,q);
 }
 
-/****************************** Vector/Rotation ******************************/
-/* TODO: the following calls should probably be depreceated sometime         */
 
-/* 3x3 matrix to axis angle */
-void mat3_to_vec_rot(float axis[3], float *angle,float mat[3][3])
-{
-       float q[4];
-       
-       /* use quaternions as intermediate representation */
-       // TODO: it would be nicer to go straight there...
-       mat3_to_quat(q,mat);
-       quat_to_axis_angle(axis, angle,q);
-}
 
-/* 4x4 matrix to axis angle */
-void mat4_to_vec_rot(float axis[3], float *angle,float mat[4][4])
+void single_axis_angle_to_mat3(float mat[3][3], const char axis, const float angle)
 {
-       float q[4];
-       
-       /* use quaternions as intermediate representation */
-       // TODO: it would be nicer to go straight there...
-       mat4_to_quat(q,mat);
-       quat_to_axis_angle(axis, angle,q);
+       const float angle_cos= cosf(angle);
+       const float angle_sin= sinf(angle);
+
+       switch(axis) {
+       case 'X': /* rotation around X */
+               mat[0][0] =  1.0f;
+               mat[0][1] =  0.0f;
+               mat[0][2] =  0.0f;
+               mat[1][0] =  0.0f;
+               mat[1][1] =  angle_cos;
+               mat[1][2] =  angle_sin;
+               mat[2][0] =  0.0f;
+               mat[2][1] = -angle_sin;
+               mat[2][2] =  angle_cos;
+               break;
+       case 'Y': /* rotation around Y */
+               mat[0][0] =  angle_cos;
+               mat[0][1] =  0.0f;
+               mat[0][2] = -angle_sin;
+               mat[1][0] =  0.0f;
+               mat[1][1] =  1.0f;
+               mat[1][2] =  0.0f;
+               mat[2][0] =  angle_sin;
+               mat[2][1] =  0.0f;
+               mat[2][2] =  angle_cos;
+               break;
+       case 'Z': /* rotation around Z */
+               mat[0][0] =  angle_cos;
+               mat[0][1] =  angle_sin;
+               mat[0][2] =  0.0f;
+               mat[1][0] = -angle_sin;
+               mat[1][1] =  angle_cos;
+               mat[1][2] =  0.0f;
+               mat[2][0] =  0.0f;
+               mat[2][1] =  0.0f;
+               mat[2][2] =  1.0f;
+               break;
+       default:
+               assert("invalid axis");
+       }
 }
 
+/****************************** Vector/Rotation ******************************/
+/* TODO: the following calls should probably be depreceated sometime         */
+
 /* axis angle to 3x3 matrix */
-void vec_rot_to_mat3(float mat[][3],float *vec, float phi)
+void vec_rot_to_mat3(float mat[][3], const float vec[3], const float phi)
 {
        /* rotation of phi radials around vec */
        float vx, vx2, vy, vy2, vz, vz2, co, si;
@@ -697,7 +845,7 @@ void vec_rot_to_mat3(float mat[][3],float *vec, float phi)
 }
 
 /* axis angle to 4x4 matrix */
-void vec_rot_to_mat4(float mat[][4],float *vec, float phi)
+void vec_rot_to_mat4(float mat[][4], const float vec[3], const float phi)
 {
        float tmat[3][3];
        
@@ -707,7 +855,7 @@ void vec_rot_to_mat4(float mat[][4],float *vec, float phi)
 }
 
 /* axis angle to quaternion */
-void vec_rot_to_quat(float *quat,float *vec, float phi)
+void vec_rot_to_quat(float *quat, const float vec[3], const float phi)
 {
        /* rotation of phi radials around vec */
        float si;
@@ -720,8 +868,8 @@ void vec_rot_to_quat(float *quat,float *vec, float phi)
                unit_qt(quat);
        }
        else {
-               quat[0]= (float)cos(phi/2.0);
-               si= (float)sin(phi/2.0);
+               quat[0]= (float)cos((double)phi/2.0);
+               si= (float)sin((double)phi/2.0);
                quat[1] *= si;
                quat[2] *= si;
                quat[3] *= si;
@@ -731,7 +879,7 @@ void vec_rot_to_quat(float *quat,float *vec, float phi)
 /******************************** XYZ Eulers *********************************/
 
 /* XYZ order */
-void eul_to_mat3(float mat[][3], float *eul)
+void eul_to_mat3(float mat[][3], const float eul[3])
 {
        double ci, cj, ch, si, sj, sh, cc, cs, sc, ss;
        
@@ -759,7 +907,7 @@ void eul_to_mat3(float mat[][3], float *eul)
 }
 
 /* XYZ order */
-void eul_to_mat4(float mat[][4], float *eul)
+void eul_to_mat4(float mat[][4], const float eul[3])
 {
        double ci, cj, ch, si, sj, sh, cc, cs, sc, ss;
        
@@ -791,7 +939,7 @@ void eul_to_mat4(float mat[][4], float *eul)
 
 /* 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)
+static void mat3_to_eul2(float tmat[][3], float eul1[3], float eul2[3])
 {
        float cy, quat[4], mat[3][3];
        
@@ -802,7 +950,7 @@ static void mat3_to_eul2(float tmat[][3], float *eul1, float *eul2)
        
        cy = (float)sqrt(mat[0][0]*mat[0][0] + mat[0][1]*mat[0][1]);
        
-       if (cy > 16.0*FLT_EPSILON) {
+       if (cy > 16.0f*FLT_EPSILON) {
                
                eul1[0] = (float)atan2(mat[1][2], mat[2][2]);
                eul1[1] = (float)atan2(-mat[0][2], cy);
@@ -848,23 +996,23 @@ void mat4_to_eul(float *eul,float tmat[][4])
 }
 
 /* XYZ order */
-void quat_to_eul(float *eul,float *quat)
+void quat_to_eul(float *eul, const float quat[4])
 {
        float mat[3][3];
-       
+
        quat_to_mat3(mat,quat);
        mat3_to_eul(eul,mat);
 }
 
 /* XYZ order */
-void eul_to_quat(float *quat,float *eul)
+void eul_to_quat(float *quat, const float eul[3])
 {
-    float ti, tj, th, ci, cj, ch, si, sj, sh, cc, cs, sc, ss;
+       float ti, tj, th, ci, cj, ch, si, sj, sh, cc, cs, sc, ss;
  
-    ti = eul[0]*0.5f; tj = eul[1]*0.5f; th = eul[2]*0.5f;
-    ci = (float)cos(ti);  cj = (float)cos(tj);  ch = (float)cos(th);
-    si = (float)sin(ti);  sj = (float)sin(tj);  sh = (float)sin(th);
-    cc = ci*ch; cs = ci*sh; sc = si*ch; ss = si*sh;
+       ti = eul[0]*0.5f; tj = eul[1]*0.5f; th = eul[2]*0.5f;
+       ci = (float)cos(ti);  cj = (float)cos(tj);  ch = (float)cos(th);
+       si = (float)sin(ti);  sj = (float)sin(tj);  sh = (float)sin(th);
+       cc = ci*ch; cs = ci*sh; sc = si*ch; ss = si*sh;
        
        quat[0] = cj*cc + sj*ss;
        quat[1] = cj*sc - sj*cs;
@@ -873,13 +1021,15 @@ void eul_to_quat(float *quat,float *eul)
 }
 
 /* XYZ order */
-void rotate_eul(float *beul, char axis, float ang)
+void rotate_eul(float *beul, const char axis, const float ang)
 {
        float eul[3], mat1[3][3], mat2[3][3], totmat[3][3];
-       
+
+       assert(axis >= 'X' && axis <= 'Z');
+
        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);
@@ -891,10 +1041,9 @@ void rotate_eul(float *beul, char axis, float ang)
        
 }
 
-#if 0
 /* exported to transform.c */
 /* order independent! */
-void compatible_eul(float *eul, float *oldrot)
+void compatible_eul(float eul[3], const float oldrot[3])
 {
        float dx, dy, dz;
        
@@ -918,13 +1067,13 @@ void compatible_eul(float *eul, float *oldrot)
        
        /* is 1 of the axis rotations larger than 180 degrees and the other small? NO ELSE IF!! */      
        if(fabs(dx) > 3.2 && fabs(dy)<1.6 && fabs(dz)<1.6) {
-               if(dx > 0.0) eul[0] -= 2.0f*(float)M_PI; else eul[0]+= 2.0f*(float)M_PI;
+               if(dx > 0.0f) eul[0] -= 2.0f*(float)M_PI; else eul[0]+= 2.0f*(float)M_PI;
        }
        if(fabs(dy) > 3.2 && fabs(dz)<1.6 && fabs(dx)<1.6) {
-               if(dy > 0.0) eul[1] -= 2.0f*(float)M_PI; else eul[1]+= 2.0f*(float)M_PI;
+               if(dy > 0.0f) eul[1] -= 2.0f*(float)M_PI; else eul[1]+= 2.0f*(float)M_PI;
        }
        if(fabs(dz) > 3.2 && fabs(dx)<1.6 && fabs(dy)<1.6) {
-               if(dz > 0.0) eul[2] -= 2.0f*(float)M_PI; else eul[2]+= 2.0f*(float)M_PI;
+               if(dz > 0.0f) eul[2] -= 2.0f*(float)M_PI; else eul[2]+= 2.0f*(float)M_PI;
        }
        
        /* the method below was there from ancient days... but why! probably because the code sucks :)
@@ -955,11 +1104,10 @@ void compatible_eul(float *eul, float *oldrot)
        }
 #endif 
 }
-#endif
 
 /* uses 2 methods to retrieve eulers, and picks the closest */
 /* XYZ order */
-void mat3_to_compatible_eul(float *eul, float *oldrot,float mat[][3])
+void mat3_to_compatible_eul(float eul[3], const float oldrot[3], float mat[][3])
 {
        float eul1[3], eul2[3];
        float d1, d2;
@@ -986,7 +1134,7 @@ void mat3_to_compatible_eul(float *eul, float *oldrot,float mat[][3])
 
 /* Euler Rotation Order Code:
  * was adapted from  
-               ANSI C code from the article
+                 ANSI C code from the article
                "Euler Angle Conversion"
                by Ken Shoemake, shoemake@graphics.cis.upenn.edu
                in "Graphics Gems IV", Academic Press, 1994
@@ -1009,27 +1157,27 @@ static RotOrderInfo rotOrders[]= {
        {{1, 0, 2}, 1}, // YXZ
        {{1, 2, 0}, 0}, // YZX
        {{2, 0, 1}, 0}, // ZXY
-       {{2, 1, 0}, 1}  // ZYZ
+       {{2, 1, 0}, 1}  // ZYX
 };
 
 /* 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) (((order)>=1) ? &rotOrders[(order)-1] : &rotOrders[0])
+#define GET_ROTATIONORDER_INFO(order) (assert(order>=0 && order<=6), (order < 1) ? &rotOrders[0] : &rotOrders[(order)-1])
 
 /* Construct quaternion from Euler angles (in radians). */
-void eulO_to_quat(float q[4],float e[3], short order)
+void eulO_to_quat(float q[4], const float e[3], const short order)
 {
        RotOrderInfo *R= GET_ROTATIONORDER_INFO(order); 
        short i=R->axis[0],  j=R->axis[1],      k=R->axis[2];
        double ti, tj, th, ci, cj, ch, si, sj, sh, cc, cs, sc, ss;
        double a[3];
        
-       ti = e[i]/2; tj = e[j]/2; th = e[k]/2;
-       
-       if (R->parity) e[j] = -e[j];
-       
+       ti = e[i] * 0.5f;
+       tj = e[j] * (R->parity ? -0.5f : 0.5f);
+       th = e[k] * 0.5f;
+
        ci = cos(ti);  cj = cos(tj);  ch = cos(th);
        si = sin(ti);  sj = sin(tj);  sh = sin(th);
        
@@ -1045,11 +1193,11 @@ void eulO_to_quat(float q[4],float e[3], short order)
        q[2] = a[1];
        q[3] = a[2];
        
-       if (R->parity) q[j] = -q[j];
+       if (R->parity) q[j+1] = -q[j+1];
 }
 
 /* Convert quaternion to Euler angles (in radians). */
-void quat_to_eulO(float e[3], short order,float q[4])
+void quat_to_eulO(float e[3], short const order, const float q[4])
 {
        float M[3][3];
        
@@ -1058,7 +1206,7 @@ void quat_to_eulO(float e[3], short order,float q[4])
 }
 
 /* Construct 3x3 matrix from Euler angles (in radians). */
-void eulO_to_mat3(float M[3][3],float e[3], short order)
+void eulO_to_mat3(float M[3][3], const float e[3], const short order)
 {
        RotOrderInfo *R= GET_ROTATIONORDER_INFO(order); 
        short i=R->axis[0],  j=R->axis[1],      k=R->axis[2];
@@ -1082,53 +1230,6 @@ void eulO_to_mat3(float M[3][3],float e[3], short order)
        M[i][k] = -sj;   M[j][k] = cj*si;        M[k][k] = cj*ci;
 }
 
-/* Construct 4x4 matrix from Euler angles (in radians). */
-void eulO_to_mat4(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) */
-       normalize_m3(m);
-       eulO_to_mat3(m,e, order);
-       copy_m4_m3(M, m);
-}
-
-/* Convert 3x3 matrix to Euler angles (in radians). */
-void mat3_to_eulO(float e[3], short order,float M[3][3])
-{
-       RotOrderInfo *R= GET_ROTATIONORDER_INFO(order); 
-       short i=R->axis[0],  j=R->axis[1],      k=R->axis[2];
-       double cy = sqrt(M[i][i]*M[i][i] + M[i][j]*M[i][j]);
-       
-       if (cy > 16*FLT_EPSILON) {
-               e[i] = atan2(M[j][k], M[k][k]);
-               e[j] = atan2(-M[i][k], cy);
-               e[k] = atan2(M[i][j], M[i][i]);
-       } 
-       else {
-               e[i] = atan2(-M[k][j], M[j][j]);
-               e[j] = atan2(-M[i][k], cy);
-               e[k] = 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 mat4_to_eulO(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) */
-       copy_m3_m4(m, M);
-       normalize_m3(m);
-       mat3_to_eulO(e, order,m);
-}
-
 /* returns two euler calculation methods, so we can pick the best */
 static void mat3_to_eulo2(float M[3][3], float *e1, float *e2, short order)
 {
@@ -1143,7 +1244,7 @@ static void mat3_to_eulo2(float M[3][3], float *e1, float *e2, short order)
        
        cy= sqrt(m[i][i]*m[i][i] + m[i][j]*m[i][j]);
        
-       if (cy > 16*FLT_EPSILON) {
+       if (cy > 16.0*(double)FLT_EPSILON) {
                e1[i] = atan2(m[j][k], m[k][k]);
                e1[j] = atan2(-m[i][k], cy);
                e1[k] = atan2(m[i][j], m[i][i]);
@@ -1171,6 +1272,45 @@ static void mat3_to_eulo2(float M[3][3], float *e1, float *e2, short order)
        }
 }
 
+/* Construct 4x4 matrix from Euler angles (in radians). */
+void eulO_to_mat4(float M[4][4], const float e[3], const short order)
+{
+       float m[3][3];
+       
+       /* for now, we'll just do this the slow way (i.e. copying matrices) */
+       normalize_m3(m);
+       eulO_to_mat3(m,e, order);
+       copy_m4_m3(M, m);
+}
+
+
+/* Convert 3x3 matrix to Euler angles (in radians). */
+void mat3_to_eulO(float eul[3], const short order,float M[3][3])
+{
+       float eul1[3], eul2[3];
+       
+       mat3_to_eulo2(M, eul1, eul2, order);
+               
+       /* return best, which is just the one with lowest values it in */
+       if(fabs(eul1[0])+fabs(eul1[1])+fabs(eul1[2]) > fabs(eul2[0])+fabs(eul2[1])+fabs(eul2[2])) {
+               copy_v3_v3(eul, eul2);
+       }
+       else {
+               copy_v3_v3(eul, eul1);
+       }
+}
+
+/* Convert 4x4 matrix to Euler angles (in radians). */
+void mat4_to_eulO(float e[3], const short order,float M[4][4])
+{
+       float m[3][3];
+       
+       /* for now, we'll just do this the slow way (i.e. copying matrices) */
+       copy_m3_m4(m, M);
+       normalize_m3(m);
+       mat3_to_eulO(e, order,m);
+}
+
 /* uses 2 methods to retrieve eulers, and picks the closest */
 void mat3_to_compatible_eulO(float eul[3], float oldrot[3], short order,float mat[3][3])
 {
@@ -1192,16 +1332,27 @@ void mat3_to_compatible_eulO(float eul[3], float oldrot[3], short order,float ma
                copy_v3_v3(eul, eul1);
 }
 
+void mat4_to_compatible_eulO(float eul[3], float oldrot[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) */
+       copy_m3_m4(m, M);
+       normalize_m3(m);
+       mat3_to_compatible_eulO(eul, oldrot, order, m);
+}
 /* rotate the given euler by the given angle on the specified axis */
 // NOTE: is this safe to do with different axis orders?
 void rotate_eulO(float beul[3], short order, char axis, float ang)
 {
        float eul[3], mat1[3][3], mat2[3][3], totmat[3][3];
-       
+
+       assert(axis >= 'X' && axis <= 'Z');
+
        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;
@@ -1215,7 +1366,7 @@ void rotate_eulO(float beul[3], short order, char axis, float ang)
 }
 
 /* the matrix is written to as 3 axis vectors */
-void eulO_to_gimbal_axis(float gmat[][3], float *eul, short order)
+void eulO_to_gimbal_axis(float gmat[][3], const float eul[3], const short order)
 {
        RotOrderInfo *R= GET_ROTATIONORDER_INFO(order);
 
@@ -1260,11 +1411,11 @@ void eulO_to_gimbal_axis(float gmat[][3], float *eul, short order)
    freely, subject to the following restrictions:
 
    1. The origin of this software must not be misrepresented; you must not
-      claim that you wrote the original software. If you use this software
-      in a product, an acknowledgment in the product documentation would be
-      appreciated but is not required.
+         claim that you wrote the original software. If you use this software
+         in a product, an acknowledgment in the product documentation would be
+         appreciated but is not required.
    2. Altered source versions must be plainly marked as such, and must not be
-      misrepresented as being the original software.
+         misrepresented as being the original software.
    3. This notice may not be removed or altered from any source distribution.
 
    Author: Ladislav Kavan, kavanl@cs.tcd.ie
@@ -1288,10 +1439,16 @@ void mat4_to_dquat(DualQuat *dq,float basemat[][4], float mat[][4])
        copy_v3_v3(dscale, scale);
        dscale[0] -= 1.0f; dscale[1] -= 1.0f; dscale[2] -= 1.0f;
 
-       if((determinant_m4(mat) < 0.0f) || len_v3(dscale) > 1e-4) {
+       if((determinant_m4(mat) < 0.0f) || len_v3(dscale) > 1e-4f) {
                /* extract R and S  */
-               mat4_to_quat(basequat,baseRS);
-               quat_to_mat4(baseR,basequat);
+               float tmp[4][4];
+
+                /* extra orthogonalize, to avoid flipping with stretched bones */
+               copy_m4_m4(tmp, baseRS);
+               orthogonalize_m4(tmp, 1);
+               mat4_to_quat(basequat, tmp);
+
+               quat_to_mat4(baseR, basequat);
                copy_v3_v3(baseR[3], baseRS[3]);
 
                invert_m4_m4(baseinv, basemat);
@@ -1301,7 +1458,7 @@ void mat4_to_dquat(DualQuat *dq,float basemat[][4], float mat[][4])
                mul_m4_m4m4(S, baseRS, baseRinv);
 
                /* set scaling part */
-               mul_serie_m4(dq->scale, basemat, S, baseinv, 0, 0, 0, 0, 0);
+               mul_serie_m4(dq->scale, basemat, S, baseinv, NULL, NULL, NULL, NULL, NULL);
                dq->scale_weight= 1.0f;
        }
        else {
@@ -1322,7 +1479,7 @@ void mat4_to_dquat(DualQuat *dq,float basemat[][4], float mat[][4])
        dq->trans[3]=  0.5f*(t[0]*q[2] - t[1]*q[1] + t[2]*q[0]);
 }
 
-void dquat_to_mat4(float mat[][4],DualQuat *dq)
+void dquat_to_mat4(float mat[][4], DualQuat *dq)
 {
        float len, *t, q0[4];
        
@@ -1458,3 +1615,101 @@ void copy_dq_dq(DualQuat *dq1, DualQuat *dq2)
        memcpy(dq1, dq2, sizeof(DualQuat));
 }
 
+/* axis matches eTrackToAxis_Modes */
+void quat_apply_track(float quat[4], short axis, short upflag)
+{      
+       /* rotations are hard coded to match vec_to_quat */
+       const float quat_track[][4]= {{0.70710676908493, 0.0, -0.70710676908493, 0.0},  /* pos-y90 */ 
+                                     {0.5, 0.5, 0.5, 0.5},  /* Quaternion((1,0,0), radians(90)) * Quaternion((0,1,0), radians(90)) */ 
+                                     {0.70710676908493, 0.0, 0.0, 0.70710676908493},  /* pos-z90 */ 
+                                     {0.70710676908493, 0.0, 0.70710676908493, 0.0}, /* neg-y90 */ 
+                                     {0.5, -0.5, -0.5, 0.5}, /* Quaternion((1,0,0), radians(-90)) * Quaternion((0,1,0), radians(-90)) */ 
+                                     {-3.0908619663705394e-08, 0.70710676908493, 0.70710676908493, 3.0908619663705394e-08}}; /* no rotation */
+
+       assert(axis >= 0 && axis <= 5);
+       assert(upflag >= 0 && upflag <= 2);
+       
+       mul_qt_qtqt(quat, quat, quat_track[axis]);
+
+       if(axis>2)
+               axis= axis-3;
+
+       /* there are 2 possible up-axis for each axis used, the 'quat_track' applies so the first
+        * up axis is used X->Y, Y->X, Z->X, if this first up axis isn used then rotate 90d
+        * the strange bit shift below just find the low axis {X:Y, Y:X, Z:X} */
+       if(upflag != (2-axis)>>1) {
+               float q[4]= {0.70710676908493, 0.0, 0.0, 0.0}; /* assign 90d rotation axis */
+               q[axis+1] = ((axis==1)) ? 0.70710676908493 : -0.70710676908493; /* flip non Y axis */
+               mul_qt_qtqt(quat, quat, q);
+       }
+}
+
+
+void vec_apply_track(float vec[3], short axis)
+{
+       float tvec[3];
+
+       assert(axis >= 0 && axis <= 5);
+       
+       copy_v3_v3(tvec, vec);
+
+       switch(axis) {
+       case 0: /* pos-x */
+               /* vec[0]=  0.0; */
+               vec[1]=  tvec[2];
+               vec[2]=  -tvec[1];
+               break;
+       case 1: /* pos-y */
+               /* vec[0]= tvec[0]; */
+               /* vec[1]=  0.0; */
+               /* vec[2]= tvec[2]; */ 
+               break;
+       case 2: /* pos-z */
+               /* vec[0]= tvec[0]; */
+               /* vec[1]= tvec[1]; */
+               // vec[2]=  0.0; */
+               break;
+       case 3: /* neg-x */
+               /* vec[0]=  0.0; */
+               vec[1]=  tvec[2];
+               vec[2]= -tvec[1];
+               break;
+       case 4: /* neg-y */
+               vec[0]= -tvec[2];
+               /* vec[1]=  0.0; */
+               vec[2]= tvec[0];
+               break;
+       case 5: /* neg-z */
+               vec[0]= -tvec[0];
+               vec[1]= -tvec[1];
+               /* vec[2]=  0.0; */
+               break;
+       }
+}
+
+/* lens/angle conversion (radians) */
+float lens_to_angle(float lens)
+{
+       return 2.0f * atanf(16.0f/lens);
+}
+
+float angle_to_lens(float angle)
+{
+       return 16.0f / tanf(angle * 0.5f);
+}
+
+/* 'mod_inline(-3,4)= 1', 'fmod(-3,4)= -3' */
+static float mod_inline(float a, float b)
+{
+       return a - (b * floorf(a / b));
+}
+
+float angle_wrap_rad(float angle)
+{
+       return mod_inline(angle + (float)M_PI, (float)M_PI*2.0f) - (float)M_PI;
+}
+
+float angle_wrap_deg(float angle)
+{
+       return mod_inline(angle + 180.0f, 360.0f) - 180.0f;
+}