Math Lib: add transpose_m3_m3, m3_m4, m4_m4
[blender.git] / source / blender / blenlib / intern / math_matrix.c
index 3fb3d2b58ffb9f1db600b3f3f90b2cde47e540cd..293e90c87130cf12967e26cb470e7552eb008284 100644 (file)
 #include <assert.h>
 #include "BLI_math.h"
 
 #include <assert.h>
 #include "BLI_math.h"
 
+#include "BLI_strict_flags.h"
+
 /********************************* Init **************************************/
 
 /********************************* Init **************************************/
 
+void zero_m2(float m[2][2])
+{
+       memset(m, 0, sizeof(float[2][2]));
+}
+
 void zero_m3(float m[3][3])
 {
 void zero_m3(float m[3][3])
 {
-       memset(m, 0, 3 * 3 * sizeof(float));
+       memset(m, 0, sizeof(float[3][3]));
 }
 
 void zero_m4(float m[4][4])
 {
 }
 
 void zero_m4(float m[4][4])
 {
-       memset(m, 0, 4 * 4 * sizeof(float));
+       memset(m, 0, sizeof(float[4][4]));
 }
 
 }
 
-void unit_m3(float m[][3])
+void unit_m2(float m[2][2])
 {
 {
-       m[0][0] = m[1][1] = m[2][2] = 1.0;
-       m[0][1] = m[0][2] = 0.0;
-       m[1][0] = m[1][2] = 0.0;
-       m[2][0] = m[2][1] = 0.0;
+       m[0][0] = m[1][1] = 1.0f;
+       m[0][1] = 0.0f;
+       m[1][0] = 0.0f;
 }
 
 }
 
-void unit_m4(float m[][4])
+void unit_m3(float m[3][3])
 {
 {
-       m[0][0] = m[1][1] = m[2][2] = m[3][3] = 1.0;
-       m[0][1] = m[0][2] = m[0][3] = 0.0;
-       m[1][0] = m[1][2] = m[1][3] = 0.0;
-       m[2][0] = m[2][1] = m[2][3] = 0.0;
-       m[3][0] = m[3][1] = m[3][2] = 0.0;
+       m[0][0] = m[1][1] = m[2][2] = 1.0f;
+       m[0][1] = m[0][2] = 0.0f;
+       m[1][0] = m[1][2] = 0.0f;
+       m[2][0] = m[2][1] = 0.0f;
+}
+
+void unit_m4(float m[4][4])
+{
+       m[0][0] = m[1][1] = m[2][2] = m[3][3] = 1.0f;
+       m[0][1] = m[0][2] = m[0][3] = 0.0f;
+       m[1][0] = m[1][2] = m[1][3] = 0.0f;
+       m[2][0] = m[2][1] = m[2][3] = 0.0f;
+       m[3][0] = m[3][1] = m[3][2] = 0.0f;
+}
+
+void copy_m2_m2(float m1[2][2], float m2[2][2])
+{
+       memcpy(m1, m2, sizeof(float[2][2]));
 }
 
 }
 
-void copy_m3_m3(float m1[][3], float m2[][3])
+void copy_m3_m3(float m1[3][3], float m2[3][3])
 {
        /* destination comes first: */
 {
        /* destination comes first: */
-       memcpy(&m1[0], &m2[0], 9 * sizeof(float));
+       memcpy(m1, m2, sizeof(float[3][3]));
 }
 
 }
 
-void copy_m4_m4(float m1[][4], float m2[][4])
+void copy_m4_m4(float m1[4][4], float m2[4][4])
 {
 {
-       memcpy(m1, m2, 4 * 4 * sizeof(float));
+       memcpy(m1, m2, sizeof(float[4][4]));
 }
 
 }
 
-void copy_m3_m4(float m1[][3], float m2[][4])
+void copy_m3_m4(float m1[3][3], float m2[4][4])
 {
        m1[0][0] = m2[0][0];
        m1[0][1] = m2[0][1];
 {
        m1[0][0] = m2[0][0];
        m1[0][1] = m2[0][1];
@@ -86,7 +105,7 @@ void copy_m3_m4(float m1[][3], float m2[][4])
        m1[2][2] = m2[2][2];
 }
 
        m1[2][2] = m2[2][2];
 }
 
-void copy_m4_m3(float m1[][4], float m2[][3]) /* no clear */
+void copy_m4_m3(float m1[4][4], float m2[3][3]) /* no clear */
 {
        m1[0][0] = m2[0][0];
        m1[0][1] = m2[0][1];
 {
        m1[0][0] = m2[0][0];
        m1[0][1] = m2[0][1];
@@ -101,18 +120,34 @@ void copy_m4_m3(float m1[][4], float m2[][3]) /* no clear */
        m1[2][2] = m2[2][2];
 
        /*      Reevan's Bugfix */
        m1[2][2] = m2[2][2];
 
        /*      Reevan's Bugfix */
-       m1[0][3] = 0.0F;
-       m1[1][3] = 0.0F;
-       m1[2][3] = 0.0F;
+       m1[0][3] = 0.0f;
+       m1[1][3] = 0.0f;
+       m1[2][3] = 0.0f;
 
 
-       m1[3][0] = 0.0F;
-       m1[3][1] = 0.0F;
-       m1[3][2] = 0.0F;
-       m1[3][3] = 1.0F;
+       m1[3][0] = 0.0f;
+       m1[3][1] = 0.0f;
+       m1[3][2] = 0.0f;
+       m1[3][3] = 1.0f;
 
 }
 
 
 }
 
-void swap_m3m3(float m1[][3], float m2[][3])
+void copy_m3_m3d(float R[3][3], double A[3][3])
+{
+       /* Keep it stupid simple for better data flow in CPU. */
+       R[0][0] = (float)A[0][0];
+       R[0][1] = (float)A[0][1];
+       R[0][2] = (float)A[0][2];
+
+       R[1][0] = (float)A[1][0];
+       R[1][1] = (float)A[1][1];
+       R[1][2] = (float)A[1][2];
+
+       R[2][0] = (float)A[2][0];
+       R[2][1] = (float)A[2][1];
+       R[2][2] = (float)A[2][2];
+}
+
+void swap_m3m3(float m1[3][3], float m2[3][3])
 {
        float t;
        int i, j;
 {
        float t;
        int i, j;
@@ -126,7 +161,7 @@ void swap_m3m3(float m1[][3], float m2[][3])
        }
 }
 
        }
 }
 
-void swap_m4m4(float m1[][4], float m2[][4])
+void swap_m4m4(float m1[4][4], float m2[4][4])
 {
        float t;
        int i, j;
 {
        float t;
        int i, j;
@@ -142,7 +177,7 @@ void swap_m4m4(float m1[][4], float m2[][4])
 
 /******************************** Arithmetic *********************************/
 
 
 /******************************** Arithmetic *********************************/
 
-void mult_m4_m4m4(float m1[][4], float m3_[][4], float m2_[][4])
+void mul_m4_m4m4(float m1[4][4], float m3_[4][4], float m2_[4][4])
 {
        float m2[4][4], m3[4][4];
 
 {
        float m2[4][4], m3[4][4];
 
@@ -173,7 +208,7 @@ void mult_m4_m4m4(float m1[][4], float m3_[][4], float m2_[][4])
 
 }
 
 
 }
 
-void mul_m3_m3m3(float m1[][3], float m3_[][3], float m2_[][3])
+void mul_m3_m3m3(float m1[3][3], float m3_[3][3], float m2_[3][3])
 {
        float m2[3][3], m3[3][3];
 
 {
        float m2[3][3], m3[3][3];
 
@@ -195,7 +230,7 @@ void mul_m3_m3m3(float m1[][3], float m3_[][3], float m2_[][3])
        m1[2][2] = m2[2][0] * m3[0][2] + m2[2][1] * m3[1][2] + m2[2][2] * m3[2][2];
 }
 
        m1[2][2] = m2[2][0] * m3[0][2] + m2[2][1] * m3[1][2] + m2[2][2] * m3[2][2];
 }
 
-void mul_m4_m4m3(float (*m1)[4], float (*m3_)[4], float (*m2_)[3])
+void mul_m4_m4m3(float m1[4][4], float m3_[4][4], float m2_[3][3])
 {
        float m2[3][3], m3[4][4];
 
 {
        float m2[3][3], m3[4][4];
 
@@ -215,8 +250,14 @@ void mul_m4_m4m3(float (*m1)[4], float (*m3_)[4], float (*m2_)[3])
 }
 
 /* m1 = m2 * m3, ignore the elements on the 4th row/column of m3 */
 }
 
 /* m1 = m2 * m3, ignore the elements on the 4th row/column of m3 */
-void mult_m3_m3m4(float m1[][3], float m3[][4], float m2[][3])
+void mul_m3_m3m4(float m1[3][3], float m3_[4][4], float m2_[3][3])
 {
 {
+       float m2[3][3], m3[4][4];
+
+       /* copy so it works when m1 is the same pointer as m2 or m3 */
+       copy_m3_m3(m2, m2_);
+       copy_m4_m4(m3, m3_);
+
        /* m1[i][j] = m2[i][k] * m3[k][j] */
        m1[0][0] = m2[0][0] * m3[0][0] + m2[0][1] * m3[1][0] + m2[0][2] * m3[2][0];
        m1[0][1] = m2[0][0] * m3[0][1] + m2[0][1] * m3[1][1] + m2[0][2] * m3[2][1];
        /* m1[i][j] = m2[i][k] * m3[k][j] */
        m1[0][0] = m2[0][0] * m3[0][0] + m2[0][1] * m3[1][0] + m2[0][2] * m3[2][0];
        m1[0][1] = m2[0][0] * m3[0][1] + m2[0][1] * m3[1][1] + m2[0][2] * m3[2][1];
@@ -231,8 +272,14 @@ void mult_m3_m3m4(float m1[][3], float m3[][4], float m2[][3])
        m1[2][2] = m2[2][0] * m3[0][2] + m2[2][1] * m3[1][2] + m2[2][2] * m3[2][2];
 }
 
        m1[2][2] = m2[2][0] * m3[0][2] + m2[2][1] * m3[1][2] + m2[2][2] * m3[2][2];
 }
 
-void mul_m4_m3m4(float (*m1)[4], float (*m3)[3], float (*m2)[4])
+void mul_m4_m3m4(float m1[4][4], float m3_[3][3], float m2_[4][4])
 {
 {
+       float m2[4][4], m3[3][3];
+
+       /* copy so it works when m1 is the same pointer as m2 or m3 */
+       copy_m4_m4(m2, m2_);
+       copy_m3_m3(m3, m3_);
+
        m1[0][0] = m2[0][0] * m3[0][0] + m2[0][1] * m3[1][0] + m2[0][2] * m3[2][0];
        m1[0][1] = m2[0][0] * m3[0][1] + m2[0][1] * m3[1][1] + m2[0][2] * m3[2][1];
        m1[0][2] = m2[0][0] * m3[0][2] + m2[0][1] * m3[1][2] + m2[0][2] * m3[2][2];
        m1[0][0] = m2[0][0] * m3[0][0] + m2[0][1] * m3[1][0] + m2[0][2] * m3[2][0];
        m1[0][1] = m2[0][0] * m3[0][1] + m2[0][1] * m3[1][1] + m2[0][2] * m3[2][1];
        m1[0][2] = m2[0][0] * m3[0][2] + m2[0][1] * m3[1][2] + m2[0][2] * m3[2][2];
@@ -244,109 +291,222 @@ void mul_m4_m3m4(float (*m1)[4], float (*m3)[3], float (*m2)[4])
        m1[2][2] = m2[2][0] * m3[0][2] + m2[2][1] * m3[1][2] + m2[2][2] * m3[2][2];
 }
 
        m1[2][2] = m2[2][0] * m3[0][2] + m2[2][1] * m3[1][2] + m2[2][2] * m3[2][2];
 }
 
-void mul_serie_m3(float answ[][3],
-                  float m1[][3], float m2[][3], float m3[][3],
-                  float m4[][3], float m5[][3], float m6[][3],
-                  float m7[][3], float m8[][3])
-{
-       float temp[3][3];
-
-       if (m1 == NULL || m2 == NULL) return;
-
-       mul_m3_m3m3(answ, m2, m1);
-       if (m3) {
-               mul_m3_m3m3(temp, m3, answ);
-               if (m4) {
-                       mul_m3_m3m3(answ, m4, temp);
-                       if (m5) {
-                               mul_m3_m3m3(temp, m5, answ);
-                               if (m6) {
-                                       mul_m3_m3m3(answ, m6, temp);
-                                       if (m7) {
-                                               mul_m3_m3m3(temp, m7, answ);
-                                               if (m8) {
-                                                       mul_m3_m3m3(answ, m8, temp);
-                                               }
-                                               else copy_m3_m3(answ, temp);
-                                       }
-                               }
-                               else copy_m3_m3(answ, temp);
-                       }
-               }
-               else copy_m3_m3(answ, temp);
-       }
+
+/** \name Macro helpers for: mul_m3_series
+ * \{ */
+void _va_mul_m3_series_3(
+        float r[3][3],
+        float m1[3][3], float m2[3][3])
+{
+       mul_m3_m3m3(r, m1, m2);
+}
+void _va_mul_m3_series_4(
+        float r[3][3],
+        float m1[3][3], float m2[3][3], float m3[3][3])
+{
+       mul_m3_m3m3(r, m1, m2);
+       mul_m3_m3m3(r, r, m3);
+}
+void _va_mul_m3_series_5(
+        float r[3][3],
+        float m1[3][3], float m2[3][3], float m3[3][3], float m4[3][3])
+{
+       mul_m3_m3m3(r, m1, m2);
+       mul_m3_m3m3(r, r, m3);
+       mul_m3_m3m3(r, r, m4);
+}
+void _va_mul_m3_series_6(
+        float r[3][3],
+        float m1[3][3], float m2[3][3], float m3[3][3], float m4[3][3],
+        float m5[3][3])
+{
+       mul_m3_m3m3(r, m1, m2);
+       mul_m3_m3m3(r, r, m3);
+       mul_m3_m3m3(r, r, m4);
+       mul_m3_m3m3(r, r, m5);
+}
+void _va_mul_m3_series_7(
+        float r[3][3],
+        float m1[3][3], float m2[3][3], float m3[3][3], float m4[3][3],
+        float m5[3][3], float m6[3][3])
+{
+       mul_m3_m3m3(r, m1, m2);
+       mul_m3_m3m3(r, r, m3);
+       mul_m3_m3m3(r, r, m4);
+       mul_m3_m3m3(r, r, m5);
+       mul_m3_m3m3(r, r, m6);
+}
+void _va_mul_m3_series_8(
+        float r[3][3],
+        float m1[3][3], float m2[3][3], float m3[3][3], float m4[3][3],
+        float m5[3][3], float m6[3][3], float m7[3][3])
+{
+       mul_m3_m3m3(r, m1, m2);
+       mul_m3_m3m3(r, r, m3);
+       mul_m3_m3m3(r, r, m4);
+       mul_m3_m3m3(r, r, m5);
+       mul_m3_m3m3(r, r, m6);
+       mul_m3_m3m3(r, r, m7);
+}
+void _va_mul_m3_series_9(
+        float r[3][3],
+        float m1[3][3], float m2[3][3], float m3[3][3], float m4[3][3],
+        float m5[3][3], float m6[3][3], float m7[3][3], float m8[3][3])
+{
+       mul_m3_m3m3(r, m1, m2);
+       mul_m3_m3m3(r, r, m3);
+       mul_m3_m3m3(r, r, m4);
+       mul_m3_m3m3(r, r, m5);
+       mul_m3_m3m3(r, r, m6);
+       mul_m3_m3m3(r, r, m7);
+       mul_m3_m3m3(r, r, m8);
 }
 }
+/** \} */
 
 
-void mul_serie_m4(float answ[][4], float m1[][4],
-                  float m2[][4], float m3[][4], float m4[][4],
-                  float m5[][4], float m6[][4], float m7[][4],
-                  float m8[][4])
-{
-       float temp[4][4];
-
-       if (m1 == NULL || m2 == NULL) return;
-
-       mult_m4_m4m4(answ, m1, m2);
-       if (m3) {
-               mult_m4_m4m4(temp, answ, m3);
-               if (m4) {
-                       mult_m4_m4m4(answ, temp, m4);
-                       if (m5) {
-                               mult_m4_m4m4(temp, answ, m5);
-                               if (m6) {
-                                       mult_m4_m4m4(answ, temp, m6);
-                                       if (m7) {
-                                               mult_m4_m4m4(temp, answ, m7);
-                                               if (m8) {
-                                                       mult_m4_m4m4(answ, temp, m8);
-                                               }
-                                               else copy_m4_m4(answ, temp);
-                                       }
-                               }
-                               else copy_m4_m4(answ, temp);
-                       }
-               }
-               else copy_m4_m4(answ, temp);
-       }
+/** \name Macro helpers for: mul_m4_series
+ * \{ */
+void _va_mul_m4_series_3(
+        float r[4][4],
+        float m1[4][4], float m2[4][4])
+{
+       mul_m4_m4m4(r, m1, m2);
+}
+void _va_mul_m4_series_4(
+        float r[4][4],
+        float m1[4][4], float m2[4][4], float m3[4][4])
+{
+       mul_m4_m4m4(r, m1, m2);
+       mul_m4_m4m4(r, r, m3);
+}
+void _va_mul_m4_series_5(
+        float r[4][4],
+        float m1[4][4], float m2[4][4], float m3[4][4], float m4[4][4])
+{
+       mul_m4_m4m4(r, m1, m2);
+       mul_m4_m4m4(r, r, m3);
+       mul_m4_m4m4(r, r, m4);
+}
+void _va_mul_m4_series_6(
+        float r[4][4],
+        float m1[4][4], float m2[4][4], float m3[4][4], float m4[4][4],
+        float m5[4][4])
+{
+       mul_m4_m4m4(r, m1, m2);
+       mul_m4_m4m4(r, r, m3);
+       mul_m4_m4m4(r, r, m4);
+       mul_m4_m4m4(r, r, m5);
+}
+void _va_mul_m4_series_7(
+        float r[4][4],
+        float m1[4][4], float m2[4][4], float m3[4][4], float m4[4][4],
+        float m5[4][4], float m6[4][4])
+{
+       mul_m4_m4m4(r, m1, m2);
+       mul_m4_m4m4(r, r, m3);
+       mul_m4_m4m4(r, r, m4);
+       mul_m4_m4m4(r, r, m5);
+       mul_m4_m4m4(r, r, m6);
+}
+void _va_mul_m4_series_8(
+        float r[4][4],
+        float m1[4][4], float m2[4][4], float m3[4][4], float m4[4][4],
+        float m5[4][4], float m6[4][4], float m7[4][4])
+{
+       mul_m4_m4m4(r, m1, m2);
+       mul_m4_m4m4(r, r, m3);
+       mul_m4_m4m4(r, r, m4);
+       mul_m4_m4m4(r, r, m5);
+       mul_m4_m4m4(r, r, m6);
+       mul_m4_m4m4(r, r, m7);
+}
+void _va_mul_m4_series_9(
+        float r[4][4],
+        float m1[4][4], float m2[4][4], float m3[4][4], float m4[4][4],
+        float m5[4][4], float m6[4][4], float m7[4][4], float m8[4][4])
+{
+       mul_m4_m4m4(r, m1, m2);
+       mul_m4_m4m4(r, r, m3);
+       mul_m4_m4m4(r, r, m4);
+       mul_m4_m4m4(r, r, m5);
+       mul_m4_m4m4(r, r, m6);
+       mul_m4_m4m4(r, r, m7);
+       mul_m4_m4m4(r, r, m8);
+}
+/** \} */
+
+void mul_v2_m3v2(float r[2], float m[3][3], float v[2])
+{
+       float temp[3], warped[3];
+
+       copy_v2_v2(temp, v);
+       temp[2] = 1.0f;
+
+       mul_v3_m3v3(warped, m, temp);
+
+       r[0] = warped[0] / warped[2];
+       r[1] = warped[1] / warped[2];
 }
 
 }
 
-void mul_m4_v3(float mat[][4], float vec[3])
+void mul_m3_v2(float m[3][3], float r[2])
 {
 {
-       float x, y;
+       mul_v2_m3v2(r, m, r);
+}
+
+void mul_m4_v3(float mat[4][4], float vec[3])
+{
+       const float x = vec[0];
+       const float y = vec[1];
 
 
-       x = vec[0];
-       y = vec[1];
        vec[0] = x * mat[0][0] + y * mat[1][0] + mat[2][0] * vec[2] + mat[3][0];
        vec[1] = x * mat[0][1] + y * mat[1][1] + mat[2][1] * vec[2] + mat[3][1];
        vec[2] = x * mat[0][2] + y * mat[1][2] + mat[2][2] * vec[2] + mat[3][2];
 }
 
        vec[0] = x * mat[0][0] + y * mat[1][0] + mat[2][0] * vec[2] + mat[3][0];
        vec[1] = x * mat[0][1] + y * mat[1][1] + mat[2][1] * vec[2] + mat[3][1];
        vec[2] = x * mat[0][2] + y * mat[1][2] + mat[2][2] * vec[2] + mat[3][2];
 }
 
-void mul_v3_m4v3(float in[3], float mat[][4], const float vec[3])
+void mul_v3_m4v3(float r[3], float mat[4][4], const float vec[3])
 {
 {
-       float x, y;
+       const float x = vec[0];
+       const float y = vec[1];
 
 
-       x = vec[0];
-       y = vec[1];
-       in[0] = x * mat[0][0] + y * mat[1][0] + mat[2][0] * vec[2] + mat[3][0];
-       in[1] = x * mat[0][1] + y * mat[1][1] + mat[2][1] * vec[2] + mat[3][1];
-       in[2] = x * mat[0][2] + y * mat[1][2] + mat[2][2] * vec[2] + mat[3][2];
+       r[0] = x * mat[0][0] + y * mat[1][0] + mat[2][0] * vec[2] + mat[3][0];
+       r[1] = x * mat[0][1] + y * mat[1][1] + mat[2][1] * vec[2] + mat[3][1];
+       r[2] = x * mat[0][2] + y * mat[1][2] + mat[2][2] * vec[2] + mat[3][2];
+}
+
+void mul_v2_m4v3(float r[2], float mat[4][4], const float vec[3])
+{
+       const float x = vec[0];
+
+       r[0] = x * mat[0][0] + vec[1] * mat[1][0] + mat[2][0] * vec[2] + mat[3][0];
+       r[1] = x * mat[0][1] + vec[1] * mat[1][1] + mat[2][1] * vec[2] + mat[3][1];
+}
+
+void mul_v2_m2v2(float r[2], float mat[2][2], const float vec[2])
+{
+       const float x = vec[0];
+
+       r[0] = mat[0][0] * x + mat[1][0] * vec[1];
+       r[1] = mat[0][1] * x + mat[1][1] * vec[1];
+}
+
+void mul_m2v2(float mat[2][2], float vec[2])
+{
+       mul_v2_m2v2(vec, mat, vec);
 }
 
 /* same as mul_m4_v3() but doesnt apply translation component */
 }
 
 /* same as mul_m4_v3() but doesnt apply translation component */
-void mul_mat3_m4_v3(float mat[][4], float vec[3])
+void mul_mat3_m4_v3(float mat[4][4], float vec[3])
 {
 {
-       float x, y;
+       const float x = vec[0];
+       const float y = vec[1];
 
 
-       x = vec[0];
-       y = vec[1];
        vec[0] = x * mat[0][0] + y * mat[1][0] + mat[2][0] * vec[2];
        vec[1] = x * mat[0][1] + y * mat[1][1] + mat[2][1] * vec[2];
        vec[2] = x * mat[0][2] + y * mat[1][2] + mat[2][2] * vec[2];
 }
 
        vec[0] = x * mat[0][0] + y * mat[1][0] + mat[2][0] * vec[2];
        vec[1] = x * mat[0][1] + y * mat[1][1] + mat[2][1] * vec[2];
        vec[2] = x * mat[0][2] + y * mat[1][2] + mat[2][2] * vec[2];
 }
 
-void mul_project_m4_v3(float mat[][4], float vec[3])
+void mul_project_m4_v3(float mat[4][4], float vec[3])
 {
 {
-       const float w = vec[0] * mat[0][3] + vec[1] * mat[1][3] + vec[2] * mat[2][3] + mat[3][3];
+       const float w = mul_project_m4_v3_zfac(mat, vec);
        mul_m4_v3(mat, vec);
 
        vec[0] /= w;
        mul_m4_v3(mat, vec);
 
        vec[0] /= w;
@@ -354,13 +514,30 @@ void mul_project_m4_v3(float mat[][4], float vec[3])
        vec[2] /= w;
 }
 
        vec[2] /= w;
 }
 
-void mul_v4_m4v4(float r[4], float mat[4][4], float v[4])
+void mul_v3_project_m4_v3(float r[3], float mat[4][4], const float vec[3])
 {
 {
-       float x, y, z;
+       const float w = mul_project_m4_v3_zfac(mat, vec);
+       mul_v3_m4v3(r, mat, vec);
 
 
-       x = v[0];
-       y = v[1];
-       z = v[2];
+       r[0] /= w;
+       r[1] /= w;
+       r[2] /= w;
+}
+
+void mul_v2_project_m4_v3(float r[2], float mat[4][4], const float vec[3])
+{
+       const float w = mul_project_m4_v3_zfac(mat, vec);
+       mul_v2_m4v3(r, mat, vec);
+
+       r[0] /= w;
+       r[1] /= w;
+}
+
+void mul_v4_m4v4(float r[4], float mat[4][4], const float v[4])
+{
+       const float x = v[0];
+       const float y = v[1];
+       const float z = v[2];
 
        r[0] = x * mat[0][0] + y * mat[1][0] + z * mat[2][0] + mat[3][0] * v[3];
        r[1] = x * mat[0][1] + y * mat[1][1] + z * mat[2][1] + mat[3][1] * v[3];
 
        r[0] = x * mat[0][0] + y * mat[1][0] + z * mat[2][0] + mat[3][0] * v[3];
        r[1] = x * mat[0][1] + y * mat[1][1] + z * mat[2][1] + mat[3][1] * v[3];
@@ -375,11 +552,9 @@ void mul_m4_v4(float mat[4][4], float r[4])
 
 void mul_v4d_m4v4d(double r[4], float mat[4][4], double v[4])
 {
 
 void mul_v4d_m4v4d(double r[4], float mat[4][4], double v[4])
 {
-       double x, y, z;
-
-       x = v[0];
-       y = v[1];
-       z = v[2];
+       const double x = v[0];
+       const double y = v[1];
+       const double z = v[2];
 
        r[0] = x * (double)mat[0][0] + y * (double)mat[1][0] + z * (double)mat[2][0] + (double)mat[3][0] * v[3];
        r[1] = x * (double)mat[0][1] + y * (double)mat[1][1] + z * (double)mat[2][1] + (double)mat[3][1] * v[3];
 
        r[0] = x * (double)mat[0][0] + y * (double)mat[1][0] + z * (double)mat[2][0] + (double)mat[3][0] * v[3];
        r[1] = x * (double)mat[0][1] + y * (double)mat[1][1] + z * (double)mat[2][1] + (double)mat[3][1] * v[3];
@@ -392,13 +567,23 @@ void mul_m4_v4d(float mat[4][4], double r[4])
        mul_v4d_m4v4d(r, mat, r);
 }
 
        mul_v4d_m4v4d(r, mat, r);
 }
 
-void mul_v3_m3v3(float r[3], float M[3][3], float a[3])
+void mul_v3_m3v3(float r[3], float M[3][3], const float a[3])
 {
 {
+       BLI_assert(r != a);
+
        r[0] = M[0][0] * a[0] + M[1][0] * a[1] + M[2][0] * a[2];
        r[1] = M[0][1] * a[0] + M[1][1] * a[1] + M[2][1] * a[2];
        r[2] = M[0][2] * a[0] + M[1][2] * a[1] + M[2][2] * a[2];
 }
 
        r[0] = M[0][0] * a[0] + M[1][0] * a[1] + M[2][0] * a[2];
        r[1] = M[0][1] * a[0] + M[1][1] * a[1] + M[2][1] * a[2];
        r[2] = M[0][2] * a[0] + M[1][2] * a[1] + M[2][2] * a[2];
 }
 
+void mul_v2_m3v3(float r[2], float M[3][3], const float a[3])
+{
+       BLI_assert(r != a);
+
+       r[0] = M[0][0] * a[0] + M[1][0] * a[1] + M[2][0] * a[2];
+       r[1] = M[0][1] * a[0] + M[1][1] * a[1] + M[2][1] * a[2];
+}
+
 void mul_m3_v3(float M[3][3], float r[3])
 {
        float tmp[3];
 void mul_m3_v3(float M[3][3], float r[3])
 {
        float tmp[3];
@@ -407,12 +592,21 @@ void mul_m3_v3(float M[3][3], float r[3])
        copy_v3_v3(r, tmp);
 }
 
        copy_v3_v3(r, tmp);
 }
 
-void mul_transposed_m3_v3(float mat[][3], float vec[3])
+void mul_transposed_m3_v3(float mat[3][3], float vec[3])
+{
+       const float x = vec[0];
+       const float y = vec[1];
+
+       vec[0] = x * mat[0][0] + y * mat[0][1] + mat[0][2] * vec[2];
+       vec[1] = x * mat[1][0] + y * mat[1][1] + mat[1][2] * vec[2];
+       vec[2] = x * mat[2][0] + y * mat[2][1] + mat[2][2] * vec[2];
+}
+
+void mul_transposed_mat3_m4_v3(float mat[4][4], float vec[3])
 {
 {
-       float x, y;
+       const float x = vec[0];
+       const float y = vec[1];
 
 
-       x = vec[0];
-       y = vec[1];
        vec[0] = x * mat[0][0] + y * mat[0][1] + mat[0][2] * vec[2];
        vec[1] = x * mat[1][0] + y * mat[1][1] + mat[1][2] * vec[2];
        vec[2] = x * mat[2][0] + y * mat[2][1] + mat[2][2] * vec[2];
        vec[0] = x * mat[0][0] + y * mat[0][1] + mat[0][2] * vec[2];
        vec[1] = x * mat[1][0] + y * mat[1][1] + mat[1][2] * vec[2];
        vec[2] = x * mat[2][0] + y * mat[2][1] + mat[2][2] * vec[2];
@@ -445,18 +639,35 @@ void mul_mat3_m4_fl(float m[4][4], float f)
                        m[i][j] *= f;
 }
 
                        m[i][j] *= f;
 }
 
-void mul_m3_v3_double(float mat[][3], double vec[3])
+void negate_m3(float m[3][3])
 {
 {
-       double x, y;
+       int i, j;
+
+       for (i = 0; i < 3; i++)
+               for (j = 0; j < 3; j++)
+                       m[i][j] *= -1.0f;
+}
+
+void negate_m4(float m[4][4])
+{
+       int i, j;
+
+       for (i = 0; i < 4; i++)
+               for (j = 0; j < 4; j++)
+                       m[i][j] *= -1.0f;
+}
+
+void mul_m3_v3_double(float mat[3][3], double vec[3])
+{
+       const double x = vec[0];
+       const double y = vec[1];
 
 
-       x = vec[0];
-       y = vec[1];
        vec[0] = x * (double)mat[0][0] + y * (double)mat[1][0] + (double)mat[2][0] * vec[2];
        vec[1] = x * (double)mat[0][1] + y * (double)mat[1][1] + (double)mat[2][1] * vec[2];
        vec[2] = x * (double)mat[0][2] + y * (double)mat[1][2] + (double)mat[2][2] * vec[2];
 }
 
        vec[0] = x * (double)mat[0][0] + y * (double)mat[1][0] + (double)mat[2][0] * vec[2];
        vec[1] = x * (double)mat[0][1] + y * (double)mat[1][1] + (double)mat[2][1] * vec[2];
        vec[2] = x * (double)mat[0][2] + y * (double)mat[1][2] + (double)mat[2][2] * vec[2];
 }
 
-void add_m3_m3m3(float m1[][3], float m2[][3], float m3[][3])
+void add_m3_m3m3(float m1[3][3], float m2[3][3], float m3[3][3])
 {
        int i, j;
 
 {
        int i, j;
 
@@ -465,7 +676,7 @@ void add_m3_m3m3(float m1[][3], float m2[][3], float m3[][3])
                        m1[i][j] = m2[i][j] + m3[i][j];
 }
 
                        m1[i][j] = m2[i][j] + m3[i][j];
 }
 
-void add_m4_m4m4(float m1[][4], float m2[][4], float m3[][4])
+void add_m4_m4m4(float m1[4][4], float m2[4][4], float m3[4][4])
 {
        int i, j;
 
 {
        int i, j;
 
@@ -474,7 +685,7 @@ void add_m4_m4m4(float m1[][4], float m2[][4], float m3[][4])
                        m1[i][j] = m2[i][j] + m3[i][j];
 }
 
                        m1[i][j] = m2[i][j] + m3[i][j];
 }
 
-void sub_m3_m3m3(float m1[][3], float m2[][3], float m3[][3])
+void sub_m3_m3m3(float m1[3][3], float m2[3][3], float m3[3][3])
 {
        int i, j;
 
 {
        int i, j;
 
@@ -483,7 +694,7 @@ void sub_m3_m3m3(float m1[][3], float m2[][3], float m3[][3])
                        m1[i][j] = m2[i][j] - m3[i][j];
 }
 
                        m1[i][j] = m2[i][j] - m3[i][j];
 }
 
-void sub_m4_m4m4(float m1[][4], float m2[][4], float m3[][4])
+void sub_m4_m4m4(float m1[4][4], float m2[4][4], float m3[4][4])
 {
        int i, j;
 
 {
        int i, j;
 
@@ -492,64 +703,103 @@ void sub_m4_m4m4(float m1[][4], float m2[][4], float m3[][4])
                        m1[i][j] = m2[i][j] - m3[i][j];
 }
 
                        m1[i][j] = m2[i][j] - m3[i][j];
 }
 
-int invert_m3(float m[3][3])
+float determinant_m3_array(float m[3][3])
+{
+       return (m[0][0] * (m[1][1] * m[2][2] - m[1][2] * m[2][1]) -
+               m[1][0] * (m[0][1] * m[2][2] - m[0][2] * m[2][1]) +
+               m[2][0] * (m[0][1] * m[1][2] - m[0][2] * m[1][1]));
+}
+
+bool invert_m3_ex(float m[3][3], const float epsilon)
 {
        float tmp[3][3];
 {
        float tmp[3][3];
-       int success;
+       const bool success = invert_m3_m3_ex(tmp, m, epsilon);
 
 
-       success = invert_m3_m3(tmp, m);
        copy_m3_m3(m, tmp);
        copy_m3_m3(m, tmp);
+       return success;
+}
+
+bool invert_m3_m3_ex(float m1[3][3], float m2[3][3], const float epsilon)
+{
+       float det;
+       int a, b;
+       bool success;
+
+       BLI_assert(epsilon >= 0.0f);
+
+       /* calc adjoint */
+       adjoint_m3_m3(m1, m2);
+
+       /* then determinant old matrix! */
+       det = determinant_m3_array(m2);
+
+       success = (fabsf(det) > epsilon);
 
 
+       if (LIKELY(det != 0.0f)) {
+               det = 1.0f / det;
+               for (a = 0; a < 3; a++) {
+                       for (b = 0; b < 3; b++) {
+                               m1[a][b] *= det;
+                       }
+               }
+       }
        return success;
 }
 
        return success;
 }
 
-int invert_m3_m3(float m1[3][3], float m2[3][3])
+bool invert_m3(float m[3][3])
+{
+       float tmp[3][3];
+       const bool success = invert_m3_m3(tmp, m);
+
+       copy_m3_m3(m, tmp);
+       return success;
+}
+
+bool invert_m3_m3(float m1[3][3], float m2[3][3])
 {
        float det;
 {
        float det;
-       int a, b, success;
+       int a, b;
+       bool success;
 
        /* calc adjoint */
        adjoint_m3_m3(m1, m2);
 
        /* then determinant old matrix! */
 
        /* calc adjoint */
        adjoint_m3_m3(m1, m2);
 
        /* then determinant old matrix! */
-       det = (m2[0][0] * (m2[1][1] * m2[2][2] - m2[1][2] * m2[2][1]) -
-              m2[1][0] * (m2[0][1] * m2[2][2] - m2[0][2] * m2[2][1]) +
-              m2[2][0] * (m2[0][1] * m2[1][2] - m2[0][2] * m2[1][1]));
+       det = determinant_m3_array(m2);
 
 
-       success = (det != 0);
+       success = (det != 0.0f);
 
 
-       if (det == 0) det = 1;
-       det = 1 / det;
-       for (a = 0; a < 3; a++) {
-               for (b = 0; b < 3; b++) {
-                       m1[a][b] *= det;
+       if (LIKELY(det != 0.0f)) {
+               det = 1.0f / det;
+               for (a = 0; a < 3; a++) {
+                       for (b = 0; b < 3; b++) {
+                               m1[a][b] *= det;
+                       }
                }
        }
 
        return success;
 }
 
                }
        }
 
        return success;
 }
 
-int invert_m4(float m[4][4])
+bool invert_m4(float m[4][4])
 {
        float tmp[4][4];
 {
        float tmp[4][4];
-       int success;
+       const bool success = invert_m4_m4(tmp, m);
 
 
-       success = invert_m4_m4(tmp, m);
        copy_m4_m4(m, tmp);
        copy_m4_m4(m, tmp);
-
        return success;
 }
 
 /*
  * invertmat -
  *      computes the inverse of mat and puts it in inverse.  Returns
        return success;
 }
 
 /*
  * invertmat -
  *      computes the inverse of mat and puts it in inverse.  Returns
- *  TRUE on success (i.e. can always find a pivot) and FALSE on failure.
+ *  true on success (i.e. can always find a pivot) and false on failure.
  *  Uses Gaussian Elimination with partial (maximal column) pivoting.
  *
  *                                     Mark Segal - 1992
  */
 
  *  Uses Gaussian Elimination with partial (maximal column) pivoting.
  *
  *                                     Mark Segal - 1992
  */
 
-int invert_m4_m4(float inverse[4][4], float mat[4][4])
+bool invert_m4_m4(float inverse[4][4], float mat[4][4])
 {
        int i, j, k;
        double temp;
 {
        int i, j, k;
        double temp;
@@ -557,6 +807,8 @@ int invert_m4_m4(float inverse[4][4], float mat[4][4])
        float max;
        int maxj;
 
        float max;
        int maxj;
 
+       BLI_assert(inverse != mat);
+
        /* Set inverse to identity */
        for (i = 0; i < 4; i++)
                for (j = 0; j < 4; j++)
        /* Set inverse to identity */
        for (i = 0; i < 4; i++)
                for (j = 0; j < 4; j++)
@@ -571,11 +823,11 @@ int invert_m4_m4(float inverse[4][4], float mat[4][4])
 
        for (i = 0; i < 4; i++) {
                /* Look for row with max pivot */
 
        for (i = 0; i < 4; i++) {
                /* Look for row with max pivot */
-               max = fabs(tempmat[i][i]);
+               max = fabsf(tempmat[i][i]);
                maxj = i;
                for (j = i + 1; j < 4; j++) {
                        if (fabsf(tempmat[j][i]) > max) {
                maxj = i;
                for (j = i + 1; j < 4; j++) {
                        if (fabsf(tempmat[j][i]) > max) {
-                               max = fabs(tempmat[j][i]);
+                               max = fabsf(tempmat[j][i]);
                                maxj = j;
                        }
                }
                                maxj = j;
                        }
                }
@@ -591,15 +843,15 @@ int invert_m4_m4(float inverse[4][4], float mat[4][4])
                if (temp == 0)
                        return 0;  /* No non-zero pivot */
                for (k = 0; k < 4; k++) {
                if (temp == 0)
                        return 0;  /* No non-zero pivot */
                for (k = 0; k < 4; k++) {
-                       tempmat[i][k] = (float)(tempmat[i][k] / temp);
-                       inverse[i][k] = (float)(inverse[i][k] / temp);
+                       tempmat[i][k] = (float)((double)tempmat[i][k] / temp);
+                       inverse[i][k] = (float)((double)inverse[i][k] / temp);
                }
                for (j = 0; j < 4; j++) {
                        if (j != i) {
                                temp = tempmat[j][i];
                                for (k = 0; k < 4; k++) {
                }
                for (j = 0; j < 4; j++) {
                        if (j != i) {
                                temp = tempmat[j][i];
                                for (k = 0; k < 4; k++) {
-                                       tempmat[j][k] -= (float)(tempmat[i][k] * temp);
-                                       inverse[j][k] -= (float)(inverse[i][k] * temp);
+                                       tempmat[j][k] -= (float)((double)tempmat[i][k] * temp);
+                                       inverse[j][k] -= (float)((double)inverse[i][k] * temp);
                                }
                        }
                }
                                }
                        }
                }
@@ -609,7 +861,7 @@ int invert_m4_m4(float inverse[4][4], float mat[4][4])
 
 /****************************** Linear Algebra *******************************/
 
 
 /****************************** Linear Algebra *******************************/
 
-void transpose_m3(float mat[][3])
+void transpose_m3(float mat[3][3])
 {
        float t;
 
 {
        float t;
 
@@ -624,7 +876,38 @@ void transpose_m3(float mat[][3])
        mat[2][1] = t;
 }
 
        mat[2][1] = t;
 }
 
-void transpose_m4(float mat[][4])
+void transpose_m3_m3(float rmat[3][3], float mat[3][3])
+{
+       BLI_assert(rmat != mat);
+
+       rmat[0][0] = mat[0][0];
+       rmat[0][1] = mat[1][0];
+       rmat[0][2] = mat[2][0];
+       rmat[1][0] = mat[0][1];
+       rmat[1][1] = mat[1][1];
+       rmat[1][2] = mat[2][1];
+       rmat[2][0] = mat[0][2];
+       rmat[2][1] = mat[1][2];
+       rmat[2][2] = mat[2][2];
+}
+
+/* seems obscure but in-fact a common operation */
+void transpose_m3_m4(float rmat[3][3], float mat[4][4])
+{
+       BLI_assert(&rmat[0][0] != &mat[0][0]);
+
+       rmat[0][0] = mat[0][0];
+       rmat[0][1] = mat[1][0];
+       rmat[0][2] = mat[2][0];
+       rmat[1][0] = mat[0][1];
+       rmat[1][1] = mat[1][1];
+       rmat[1][2] = mat[2][1];
+       rmat[2][0] = mat[0][2];
+       rmat[2][1] = mat[1][2];
+       rmat[2][2] = mat[2][2];
+}
+
+void transpose_m4(float mat[4][4])
 {
        float t;
 
 {
        float t;
 
@@ -650,7 +933,39 @@ void transpose_m4(float mat[][4])
        mat[3][2] = t;
 }
 
        mat[3][2] = t;
 }
 
-void orthogonalize_m3(float mat[][3], int axis)
+void transpose_m4_m4(float rmat[4][4], float mat[4][4])
+{
+       BLI_assert(rmat != mat);
+
+       rmat[0][0] = mat[0][0];
+       rmat[0][1] = mat[1][0];
+       rmat[0][2] = mat[2][0];
+       rmat[0][3] = mat[3][0];
+       rmat[1][0] = mat[0][1];
+       rmat[1][1] = mat[1][1];
+       rmat[1][2] = mat[2][1];
+       rmat[1][3] = mat[3][1];
+       rmat[2][0] = mat[0][2];
+       rmat[2][1] = mat[1][2];
+       rmat[2][2] = mat[2][2];
+       rmat[2][3] = mat[3][2];
+       rmat[3][0] = mat[0][3];
+       rmat[3][1] = mat[1][3];
+       rmat[3][2] = mat[2][3];
+       rmat[3][3] = mat[3][3];
+}
+
+int compare_m4m4(float mat1[4][4], float mat2[4][4], float limit)
+{
+       if (compare_v4v4(mat1[0], mat2[0], limit))
+               if (compare_v4v4(mat1[1], mat2[1], limit))
+                       if (compare_v4v4(mat1[2], mat2[2], limit))
+                               if (compare_v4v4(mat1[3], mat2[3], limit))
+                                       return 1;
+       return 0;
+}
+
+void orthogonalize_m3(float mat[3][3], int axis)
 {
        float size[3];
        mat3_to_size(size, mat);
 {
        float size[3];
        mat3_to_size(size, mat);
@@ -678,6 +993,7 @@ void orthogonalize_m3(float mat[][3], int axis)
                                normalize_v3(mat[2]);
                                cross_v3_v3v3(mat[1], mat[2], mat[0]);
                        }
                                normalize_v3(mat[2]);
                                cross_v3_v3v3(mat[1], mat[2], mat[0]);
                        }
+                       break;
                case 1:
                        if (dot_v3v3(mat[1], mat[0]) < 1) {
                                cross_v3_v3v3(mat[2], mat[0], mat[1]);
                case 1:
                        if (dot_v3v3(mat[1], mat[0]) < 1) {
                                cross_v3_v3v3(mat[2], mat[0], mat[1]);
@@ -700,6 +1016,7 @@ void orthogonalize_m3(float mat[][3], int axis)
                                normalize_v3(mat[0]);
                                cross_v3_v3v3(mat[2], mat[0], mat[1]);
                        }
                                normalize_v3(mat[0]);
                                cross_v3_v3v3(mat[2], mat[0], mat[1]);
                        }
+                       break;
                case 2:
                        if (dot_v3v3(mat[2], mat[0]) < 1) {
                                cross_v3_v3v3(mat[1], mat[2], mat[0]);
                case 2:
                        if (dot_v3v3(mat[2], mat[0]) < 1) {
                                cross_v3_v3v3(mat[1], mat[2], mat[0]);
@@ -722,13 +1039,17 @@ void orthogonalize_m3(float mat[][3], int axis)
                                normalize_v3(mat[0]);
                                cross_v3_v3v3(mat[1], mat[2], mat[0]);
                        }
                                normalize_v3(mat[0]);
                                cross_v3_v3v3(mat[1], mat[2], mat[0]);
                        }
+                       break;
+               default:
+                       BLI_assert(0);
+                       break;
        }
        mul_v3_fl(mat[0], size[0]);
        mul_v3_fl(mat[1], size[1]);
        mul_v3_fl(mat[2], size[2]);
 }
 
        }
        mul_v3_fl(mat[0], size[0]);
        mul_v3_fl(mat[1], size[1]);
        mul_v3_fl(mat[2], size[2]);
 }
 
-void orthogonalize_m4(float mat[][4], int axis)
+void orthogonalize_m4(float mat[4][4], int axis)
 {
        float size[3];
        mat4_to_size(size, mat);
 {
        float size[3];
        mat4_to_size(size, mat);
@@ -756,8 +1077,8 @@ void orthogonalize_m4(float mat[][4], int axis)
                                normalize_v3(mat[2]);
                                cross_v3_v3v3(mat[1], mat[2], mat[0]);
                        }
                                normalize_v3(mat[2]);
                                cross_v3_v3v3(mat[1], mat[2], mat[0]);
                        }
+                       break;
                case 1:
                case 1:
-                       normalize_v3(mat[0]);
                        if (dot_v3v3(mat[1], mat[0]) < 1) {
                                cross_v3_v3v3(mat[2], mat[0], mat[1]);
                                normalize_v3(mat[2]);
                        if (dot_v3v3(mat[1], mat[0]) < 1) {
                                cross_v3_v3v3(mat[2], mat[0], mat[1]);
                                normalize_v3(mat[2]);
@@ -779,6 +1100,7 @@ void orthogonalize_m4(float mat[][4], int axis)
                                normalize_v3(mat[0]);
                                cross_v3_v3v3(mat[2], mat[0], mat[1]);
                        }
                                normalize_v3(mat[0]);
                                cross_v3_v3v3(mat[2], mat[0], mat[1]);
                        }
+                       break;
                case 2:
                        if (dot_v3v3(mat[2], mat[0]) < 1) {
                                cross_v3_v3v3(mat[1], mat[2], mat[0]);
                case 2:
                        if (dot_v3v3(mat[2], mat[0]) < 1) {
                                cross_v3_v3v3(mat[1], mat[2], mat[0]);
@@ -801,13 +1123,17 @@ void orthogonalize_m4(float mat[][4], int axis)
                                normalize_v3(mat[0]);
                                cross_v3_v3v3(mat[1], mat[2], mat[0]);
                        }
                                normalize_v3(mat[0]);
                                cross_v3_v3v3(mat[1], mat[2], mat[0]);
                        }
+                       break;
+               default:
+                       BLI_assert(0);
+                       break;
        }
        mul_v3_fl(mat[0], size[0]);
        mul_v3_fl(mat[1], size[1]);
        mul_v3_fl(mat[2], size[2]);
 }
 
        }
        mul_v3_fl(mat[0], size[0]);
        mul_v3_fl(mat[1], size[1]);
        mul_v3_fl(mat[2], size[2]);
 }
 
-int is_orthogonal_m3(float m[][3])
+bool is_orthogonal_m3(float m[3][3])
 {
        int i, j;
 
 {
        int i, j;
 
@@ -821,13 +1147,13 @@ int is_orthogonal_m3(float m[][3])
        return 1;
 }
 
        return 1;
 }
 
-int is_orthogonal_m4(float m[][4])
+bool is_orthogonal_m4(float m[4][4])
 {
        int i, j;
 
        for (i = 0; i < 4; i++) {
                for (j = 0; j < i; j++) {
 {
        int i, j;
 
        for (i = 0; i < 4; i++) {
                for (j = 0; j < i; j++) {
-                       if (fabsf(dot_vn_vn(m[i], m[j], 4)) > 1.5f * FLT_EPSILON)
+                       if (fabsf(dot_v4v4(m[i], m[j])) > 1.5f * FLT_EPSILON)
                                return 0;
                }
 
                                return 0;
                }
 
@@ -836,7 +1162,7 @@ int is_orthogonal_m4(float m[][4])
        return 1;
 }
 
        return 1;
 }
 
-int is_orthonormal_m3(float m[][3])
+bool is_orthonormal_m3(float m[3][3])
 {
        if (is_orthogonal_m3(m)) {
                int i;
 {
        if (is_orthogonal_m3(m)) {
                int i;
@@ -851,13 +1177,13 @@ int is_orthonormal_m3(float m[][3])
        return 0;
 }
 
        return 0;
 }
 
-int is_orthonormal_m4(float m[][4])
+bool is_orthonormal_m4(float m[4][4])
 {
        if (is_orthogonal_m4(m)) {
                int i;
 
                for (i = 0; i < 4; i++)
 {
        if (is_orthogonal_m4(m)) {
                int i;
 
                for (i = 0; i < 4; i++)
-                       if (fabsf(dot_vn_vn(m[i], m[i], 4) - 1) > 1.5f * FLT_EPSILON)
+                       if (fabsf(dot_v4v4(m[i], m[i]) - 1) > 1.5f * FLT_EPSILON)
                                return 0;
 
                return 1;
                                return 0;
 
                return 1;
@@ -866,14 +1192,13 @@ int is_orthonormal_m4(float m[][4])
        return 0;
 }
 
        return 0;
 }
 
-int is_uniform_scaled_m3(float m[][3])
+bool is_uniform_scaled_m3(float m[3][3])
 {
 {
-       const float eps = 1e-7;
+       const float eps = 1e-7f;
        float t[3][3];
        float l1, l2, l3, l4, l5, l6;
 
        float t[3][3];
        float l1, l2, l3, l4, l5, l6;
 
-       copy_m3_m3(t, m);
-       transpose_m3(t);
+       transpose_m3_m3(t, m);
 
        l1 = len_squared_v3(m[0]);
        l2 = len_squared_v3(m[1]);
 
        l1 = len_squared_v3(m[0]);
        l2 = len_squared_v3(m[1]);
@@ -889,27 +1214,34 @@ int is_uniform_scaled_m3(float m[][3])
            fabsf(l5 - l1) <= eps &&
            fabsf(l6 - l1) <= eps)
        {
            fabsf(l5 - l1) <= eps &&
            fabsf(l6 - l1) <= eps)
        {
-               return 1;
+               return true;
        }
 
        }
 
-       return 0;
+       return false;
 }
 
 }
 
-void normalize_m3(float mat[][3])
+bool is_uniform_scaled_m4(float m[4][4])
+{
+       float t[3][3];
+       copy_m3_m4(t, m);
+       return is_uniform_scaled_m3(t);
+}
+
+void normalize_m3(float mat[3][3])
 {
        normalize_v3(mat[0]);
        normalize_v3(mat[1]);
        normalize_v3(mat[2]);
 }
 
 {
        normalize_v3(mat[0]);
        normalize_v3(mat[1]);
        normalize_v3(mat[2]);
 }
 
-void normalize_m3_m3(float rmat[][3], float mat[][3])
+void normalize_m3_m3(float rmat[3][3], float mat[3][3])
 {
        normalize_v3_v3(rmat[0], mat[0]);
        normalize_v3_v3(rmat[1], mat[1]);
        normalize_v3_v3(rmat[2], mat[2]);
 }
 
 {
        normalize_v3_v3(rmat[0], mat[0]);
        normalize_v3_v3(rmat[1], mat[1]);
        normalize_v3_v3(rmat[2], mat[2]);
 }
 
-void normalize_m4(float mat[][4])
+void normalize_m4(float mat[4][4])
 {
        float len;
 
 {
        float len;
 
@@ -921,20 +1253,24 @@ void normalize_m4(float mat[][4])
        if (len != 0.0f) mat[2][3] /= len;
 }
 
        if (len != 0.0f) mat[2][3] /= len;
 }
 
-void normalize_m4_m4(float rmat[][4], float mat[][4])
+void normalize_m4_m4(float rmat[4][4], float mat[4][4])
 {
 {
-       float len;
+       copy_m4_m4(rmat, mat);
+       normalize_m4(rmat);
+}
 
 
-       len = normalize_v3_v3(rmat[0], mat[0]);
-       if (len != 0.0f) rmat[0][3] = mat[0][3] / len;
-       len = normalize_v3_v3(rmat[1], mat[1]);
-       if (len != 0.0f) rmat[1][3] = mat[1][3] / len;
-       len = normalize_v3_v3(rmat[2], mat[2]);
-       if (len != 0.0f) rmat[2][3] = mat[2][3] / len;
+void adjoint_m2_m2(float m1[2][2], float m[2][2])
+{
+       BLI_assert(m1 != m);
+       m1[0][0] =  m[1][1];
+       m1[0][1] = -m[0][1];
+       m1[1][0] = -m[1][0];
+       m1[1][1] =  m[0][0];
 }
 
 }
 
-void adjoint_m3_m3(float m1[][3], float m[][3])
+void adjoint_m3_m3(float m1[3][3], float m[3][3])
 {
 {
+       BLI_assert(m1 != m);
        m1[0][0] = m[1][1] * m[2][2] - m[1][2] * m[2][1];
        m1[0][1] = -m[0][1] * m[2][2] + m[0][2] * m[2][1];
        m1[0][2] = m[0][1] * m[1][2] - m[0][2] * m[1][1];
        m1[0][0] = m[1][1] * m[2][2] - m[1][2] * m[2][1];
        m1[0][1] = -m[0][1] * m[2][2] + m[0][2] * m[2][1];
        m1[0][2] = m[0][1] * m[1][2] - m[0][2] * m[1][1];
@@ -948,7 +1284,7 @@ void adjoint_m3_m3(float m1[][3], float m[][3])
        m1[2][2] = m[0][0] * m[1][1] - m[0][1] * m[1][0];
 }
 
        m1[2][2] = m[0][0] * m[1][1] - m[0][1] * m[1][0];
 }
 
-void adjoint_m4_m4(float out[][4], float in[][4]) /* out = ADJ(in) */
+void adjoint_m4_m4(float out[4][4], float in[4][4]) /* out = ADJ(in) */
 {
        float a1, a2, a3, a4, b1, b2, b3, b4;
        float c1, c2, c3, c4, d1, d2, d3, d4;
 {
        float a1, a2, a3, a4, b1, b2, b3, b4;
        float c1, c2, c3, c4, d1, d2, d3, d4;
@@ -1014,7 +1350,7 @@ float determinant_m3(float a1, float a2, float a3,
        return ans;
 }
 
        return ans;
 }
 
-float determinant_m4(float m[][4])
+float determinant_m4(float m[4][4])
 {
        float ans;
        float a1, a2, a3, a4, b1, b2, b3, b4, c1, c2, c3, c4, d1, d2, d3, d4;
 {
        float ans;
        float a1, a2, a3, a4, b1, b2, b3, b4, c1, c2, c3, c4, d1, d2, d3, d4;
@@ -1049,7 +1385,7 @@ float determinant_m4(float m[][4])
 
 /****************************** Transformations ******************************/
 
 
 /****************************** Transformations ******************************/
 
-void size_to_mat3(float mat[][3], const float size[3])
+void size_to_mat3(float mat[3][3], const float size[3])
 {
        mat[0][0] = size[0];
        mat[0][1] = 0.0f;
 {
        mat[0][0] = size[0];
        mat[0][1] = 0.0f;
@@ -1062,23 +1398,34 @@ void size_to_mat3(float mat[][3], const float size[3])
        mat[2][0] = 0.0f;
 }
 
        mat[2][0] = 0.0f;
 }
 
-void size_to_mat4(float mat[][4], const float size[3])
+void size_to_mat4(float mat[4][4], const float size[3])
 {
 {
-       float tmat[3][3];
-
-       size_to_mat3(tmat, size);
-       unit_m4(mat);
-       copy_m4_m3(mat, tmat);
+       mat[0][0] = size[0];
+       mat[0][1] = 0.0f;
+       mat[0][2] = 0.0f;
+       mat[0][3] = 0.0f;
+       mat[1][0] = 0.0f;
+       mat[1][1] = size[1];
+       mat[1][2] = 0.0f;
+       mat[1][3] = 0.0f;
+       mat[2][0] = 0.0f;
+       mat[2][1] = 0.0f;
+       mat[2][2] = size[2];
+       mat[2][3] = 0.0f;
+       mat[3][0] = 0.0f;
+       mat[3][1] = 0.0f;
+       mat[3][2] = 0.0f;
+       mat[3][3] = 1.0f;
 }
 
 }
 
-void mat3_to_size(float size[3], float mat[][3])
+void mat3_to_size(float size[3], float mat[3][3])
 {
        size[0] = len_v3(mat[0]);
        size[1] = len_v3(mat[1]);
        size[2] = len_v3(mat[2]);
 }
 
 {
        size[0] = len_v3(mat[0]);
        size[1] = len_v3(mat[1]);
        size[2] = len_v3(mat[2]);
 }
 
-void mat4_to_size(float size[3], float mat[][4])
+void mat4_to_size(float size[3], float mat[4][4])
 {
        size[0] = len_v3(mat[0]);
        size[1] = len_v3(mat[1]);
 {
        size[0] = len_v3(mat[0]);
        size[1] = len_v3(mat[1]);
@@ -1088,19 +1435,22 @@ void mat4_to_size(float size[3], float mat[][4])
 /* this gets the average scale of a matrix, only use when your scaling
  * data that has no idea of scale axis, examples are bone-envelope-radius
  * and curve radius */
 /* this gets the average scale of a matrix, only use when your scaling
  * data that has no idea of scale axis, examples are bone-envelope-radius
  * and curve radius */
-float mat3_to_scale(float mat[][3])
+float mat3_to_scale(float mat[3][3])
 {
        /* unit length vector */
 {
        /* unit length vector */
-       float unit_vec[3] = {0.577350269189626f, 0.577350269189626f, 0.577350269189626f};
+       float unit_vec[3];
+       copy_v3_fl(unit_vec, (float)(1.0 / M_SQRT3));
        mul_m3_v3(mat, unit_vec);
        return len_v3(unit_vec);
 }
 
        mul_m3_v3(mat, unit_vec);
        return len_v3(unit_vec);
 }
 
-float mat4_to_scale(float mat[][4])
+float mat4_to_scale(float mat[4][4])
 {
 {
-       float tmat[3][3];
-       copy_m3_m4(tmat, mat);
-       return mat3_to_scale(tmat);
+       /* unit length vector */
+       float unit_vec[3];
+       copy_v3_fl(unit_vec, (float)(1.0 / M_SQRT3));
+       mul_mat3_m4_v3(mat, unit_vec);
+       return len_v3(unit_vec);
 }
 
 void mat3_to_rot_size(float rot[3][3], float size[3], float mat3[3][3])
 }
 
 void mat3_to_rot_size(float rot[3][3], float size[3], float mat3[3][3])
@@ -1115,9 +1465,7 @@ void mat3_to_rot_size(float rot[3][3], float size[3], float mat3[3][3])
        /* note: this is a workaround for negative matrix not working for rotation conversion, FIXME */
        normalize_m3_m3(mat3_n, mat3);
        if (is_negative_m3(mat3)) {
        /* note: this is a workaround for negative matrix not working for rotation conversion, FIXME */
        normalize_m3_m3(mat3_n, mat3);
        if (is_negative_m3(mat3)) {
-               negate_v3(mat3_n[0]);
-               negate_v3(mat3_n[1]);
-               negate_v3(mat3_n[2]);
+               negate_m3(mat3_n);
        }
 
        /* rotation */
        }
 
        /* rotation */
@@ -1127,14 +1475,22 @@ void mat3_to_rot_size(float rot[3][3], float size[3], float mat3[3][3])
        /* scale */
        /* note: mat4_to_size(ob->size, mat) fails for negative scale */
        invert_m3_m3(imat3_n, mat3_n);
        /* scale */
        /* note: mat4_to_size(ob->size, mat) fails for negative scale */
        invert_m3_m3(imat3_n, mat3_n);
+
+       /* better not edit mat3 */
+#if 0
        mul_m3_m3m3(mat3, imat3_n, mat3);
 
        size[0] = mat3[0][0];
        size[1] = mat3[1][1];
        size[2] = mat3[2][2];
        mul_m3_m3m3(mat3, imat3_n, mat3);
 
        size[0] = mat3[0][0];
        size[1] = mat3[1][1];
        size[2] = mat3[2][2];
+#else
+       size[0] = dot_m3_v3_row_x(imat3_n, mat3[0]);
+       size[1] = dot_m3_v3_row_y(imat3_n, mat3[1]);
+       size[2] = dot_m3_v3_row_z(imat3_n, mat3[2]);
+#endif
 }
 
 }
 
-void mat4_to_loc_rot_size(float loc[3], float rot[3][3], float size[3], float wmat[][4])
+void mat4_to_loc_rot_size(float loc[3], float rot[3][3], float size[3], float wmat[4][4])
 {
        float mat3[3][3]; /* wmat -> 3x3 */
 
 {
        float mat3[3][3]; /* wmat -> 3x3 */
 
@@ -1145,7 +1501,32 @@ void mat4_to_loc_rot_size(float loc[3], float rot[3][3], float size[3], float wm
        copy_v3_v3(loc, wmat[3]);
 }
 
        copy_v3_v3(loc, wmat[3]);
 }
 
-void scale_m3_fl(float m[][3], float scale)
+void mat4_to_loc_quat(float loc[3], float quat[4], float wmat[4][4])
+{
+       float mat3[3][3];
+       float mat3_n[3][3]; /* normalized mat3 */
+
+       copy_m3_m4(mat3, wmat);
+       normalize_m3_m3(mat3_n, mat3);
+
+       /* so scale doesn't interfere with rotation [#24291] */
+       /* note: this is a workaround for negative matrix not working for rotation conversion, FIXME */
+       if (is_negative_m3(mat3)) {
+               negate_m3(mat3_n);
+       }
+
+       mat3_to_quat(quat, mat3_n);
+       copy_v3_v3(loc, wmat[3]);
+}
+
+void mat4_decompose(float loc[3], float quat[4], float size[3], float wmat[4][4])
+{
+       float rot[3][3];
+       mat4_to_loc_rot_size(loc, rot, size, wmat);
+       mat3_to_quat(quat, rot);
+}
+
+void scale_m3_fl(float m[3][3], float scale)
 {
        m[0][0] = m[1][1] = m[2][2] = scale;
        m[0][1] = m[0][2] = 0.0;
 {
        m[0][0] = m[1][1] = m[2][2] = scale;
        m[0][1] = m[0][2] = 0.0;
@@ -1153,7 +1534,7 @@ void scale_m3_fl(float m[][3], float scale)
        m[2][0] = m[2][1] = 0.0;
 }
 
        m[2][0] = m[2][1] = 0.0;
 }
 
-void scale_m4_fl(float m[][4], float scale)
+void scale_m4_fl(float m[4][4], float scale)
 {
        m[0][0] = m[1][1] = m[2][2] = scale;
        m[3][3] = 1.0;
 {
        m[0][0] = m[1][1] = m[2][2] = scale;
        m[3][3] = 1.0;
@@ -1163,14 +1544,14 @@ void scale_m4_fl(float m[][4], float scale)
        m[3][0] = m[3][1] = m[3][2] = 0.0;
 }
 
        m[3][0] = m[3][1] = m[3][2] = 0.0;
 }
 
-void translate_m4(float mat[][4], float Tx, float Ty, float Tz)
+void translate_m4(float mat[4][4], float Tx, float Ty, float Tz)
 {
        mat[3][0] += (Tx * mat[0][0] + Ty * mat[1][0] + Tz * mat[2][0]);
        mat[3][1] += (Tx * mat[0][1] + Ty * mat[1][1] + Tz * mat[2][1]);
        mat[3][2] += (Tx * mat[0][2] + Ty * mat[1][2] + Tz * mat[2][2]);
 }
 
 {
        mat[3][0] += (Tx * mat[0][0] + Ty * mat[1][0] + Tz * mat[2][0]);
        mat[3][1] += (Tx * mat[0][1] + Ty * mat[1][1] + Tz * mat[2][1]);
        mat[3][2] += (Tx * mat[0][2] + Ty * mat[1][2] + Tz * mat[2][2]);
 }
 
-void rotate_m4(float mat[][4], const char axis, const float angle)
+void rotate_m4(float mat[4][4], const char axis, const float angle)
 {
        int col;
        float temp[4] = {0.0f, 0.0f, 0.0f, 0.0f};
 {
        int col;
        float temp[4] = {0.0f, 0.0f, 0.0f, 0.0f};
@@ -1178,8 +1559,8 @@ void rotate_m4(float mat[][4], const char axis, const float angle)
 
        assert(axis >= 'X' && axis <= 'Z');
 
 
        assert(axis >= 'X' && axis <= 'Z');
 
-       cosine = (float)cos(angle);
-       sine = (float)sin(angle);
+       cosine = cosf(angle);
+       sine   = sinf(angle);
        switch (axis) {
                case 'X':
                        for (col = 0; col < 4; col++)
        switch (axis) {
                case 'X':
                        for (col = 0; col < 4; col++)
@@ -1210,7 +1591,36 @@ void rotate_m4(float mat[][4], const char axis, const float angle)
        }
 }
 
        }
 }
 
-void blend_m3_m3m3(float out[][3], float dst[][3], float src[][3], const float srcweight)
+void rotate_m2(float mat[2][2], const float angle)
+{
+       mat[0][0] = mat[1][1] = cosf(angle);
+       mat[0][1] = sinf(angle);
+       mat[1][0] = -mat[0][1];
+}
+
+/**
+ * Scale or rotate around a pivot point,
+ * a convenience function to avoid having to do inline.
+ *
+ * Since its common to make a scale/rotation matrix that pivots around an arbitrary point.
+ *
+ * Typical use case is to make 3x3 matrix, copy to 4x4, then pass to this function.
+ */
+void transform_pivot_set_m4(float mat[4][4], const float pivot[3])
+{
+       float tmat[4][4];
+
+       unit_m4(tmat);
+
+       copy_v3_v3(tmat[3], pivot);
+       mul_m4_m4m4(mat, tmat, mat);
+
+       /* invert the matrix */
+       negate_v3(tmat[3]);
+       mul_m4_m4m4(mat, mat, tmat);
+}
+
+void blend_m3_m3m3(float out[3][3], float dst[3][3], float src[3][3], const float srcweight)
 {
        float srot[3][3], drot[3][3];
        float squat[4], dquat[4], fquat[4];
 {
        float srot[3][3], drot[3][3];
        float squat[4], dquat[4], fquat[4];
@@ -1233,7 +1643,7 @@ void blend_m3_m3m3(float out[][3], float dst[][3], float src[][3], const float s
        mul_m3_m3m3(out, rmat, smat);
 }
 
        mul_m3_m3m3(out, rmat, smat);
 }
 
-void blend_m4_m4m4(float out[][4], float dst[][4], float src[][4], const float srcweight)
+void blend_m4_m4m4(float out[4][4], float dst[4][4], float src[4][4], const float srcweight)
 {
        float sloc[3], dloc[3], floc[3];
        float srot[3][3], drot[3][3];
 {
        float sloc[3], dloc[3], floc[3];
        float srot[3][3], drot[3][3];
@@ -1255,20 +1665,34 @@ void blend_m4_m4m4(float out[][4], float dst[][4], float src[][4], const float s
        loc_quat_size_to_mat4(out, floc, fquat, fsize);
 }
 
        loc_quat_size_to_mat4(out, floc, fquat, fsize);
 }
 
-int is_negative_m3(float mat[][3])
+bool is_negative_m3(float mat[3][3])
 {
        float vec[3];
        cross_v3_v3v3(vec, mat[0], mat[1]);
        return (dot_v3v3(vec, mat[2]) < 0.0f);
 }
 
 {
        float vec[3];
        cross_v3_v3v3(vec, mat[0], mat[1]);
        return (dot_v3v3(vec, mat[2]) < 0.0f);
 }
 
-int is_negative_m4(float mat[][4])
+bool is_negative_m4(float mat[4][4])
 {
        float vec[3];
        cross_v3_v3v3(vec, mat[0], mat[1]);
        return (dot_v3v3(vec, mat[2]) < 0.0f);
 }
 
 {
        float vec[3];
        cross_v3_v3v3(vec, mat[0], mat[1]);
        return (dot_v3v3(vec, mat[2]) < 0.0f);
 }
 
+bool is_zero_m3(float mat[3][3])
+{
+       return (is_zero_v3(mat[0]) &&
+               is_zero_v3(mat[1]) &&
+               is_zero_v3(mat[2]));
+}
+bool is_zero_m4(float mat[4][4])
+{
+       return (is_zero_v4(mat[0]) &&
+               is_zero_v4(mat[1]) &&
+               is_zero_v4(mat[2]) &&
+               is_zero_v4(mat[3]));
+}
+
 /* 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... */
 /* 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... */
@@ -1352,7 +1776,7 @@ void loc_axisangle_size_to_mat4(float mat[4][4], const float loc[3], const float
 
 /*********************************** Other ***********************************/
 
 
 /*********************************** Other ***********************************/
 
-void print_m3(const char *str, float m[][3])
+void print_m3(const char *str, float m[3][3])
 {
        printf("%s\n", str);
        printf("%f %f %f\n", m[0][0], m[1][0], m[2][0]);
 {
        printf("%s\n", str);
        printf("%f %f %f\n", m[0][0], m[1][0], m[2][0]);
@@ -1361,7 +1785,7 @@ void print_m3(const char *str, float m[][3])
        printf("\n");
 }
 
        printf("\n");
 }
 
-void print_m4(const char *str, float m[][4])
+void print_m4(const char *str, float m[4][4])
 {
        printf("%s\n", str);
        printf("%f %f %f %f\n", m[0][0], m[1][0], m[2][0], m[3][0]);
 {
        printf("%s\n", str);
        printf("%f %f %f %f\n", m[0][0], m[1][0], m[2][0], m[3][0]);
@@ -1388,7 +1812,7 @@ void svd_m4(float U[4][4], float s[4], float V[4][4], float A_[4][4])
        int m = 4;
        int n = 4;
        int maxiter = 200;
        int m = 4;
        int n = 4;
        int maxiter = 200;
-       int nu = minf(m, n);
+       int nu = min_ii(m, n);
 
        float *work = work1;
        float *e = work2;
 
        float *work = work1;
        float *e = work2;
@@ -1396,22 +1820,22 @@ void svd_m4(float U[4][4], float s[4], float V[4][4], float A_[4][4])
 
        int i = 0, j = 0, k = 0, p, pp, iter;
 
 
        int i = 0, j = 0, k = 0, p, pp, iter;
 
-       // Reduce A to bidiagonal form, storing the diagonal elements
-       // in s and the super-diagonal elements in e.
+       /* Reduce A to bidiagonal form, storing the diagonal elements
+        * in s and the super-diagonal elements in e. */
 
 
-       int nct = minf(m - 1, n);
-       int nrt = maxf(0, minf(n - 2, m));
+       int nct = min_ii(m - 1, n);
+       int nrt = max_ii(0, min_ii(n - 2, m));
 
        copy_m4_m4(A, A_);
        zero_m4(U);
        zero_v4(s);
 
 
        copy_m4_m4(A, A_);
        zero_m4(U);
        zero_v4(s);
 
-       for (k = 0; k < maxf(nct, nrt); k++) {
+       for (k = 0; k < max_ii(nct, nrt); k++) {
                if (k < nct) {
 
                if (k < nct) {
 
-                       // Compute the transformation for the k-th column and
-                       // place the k-th diagonal in s[k].
-                       // Compute 2-norm of k-th column without under/overflow.
+                       /* Compute the transformation for the k-th column and
+                        * place the k-th diagonal in s[k].
+                        * Compute 2-norm of k-th column without under/overflow. */
                        s[k] = 0;
                        for (i = k; i < m; i++) {
                                s[k] = hypotf(s[k], A[i][k]);
                        s[k] = 0;
                        for (i = k; i < m; i++) {
                                s[k] = hypotf(s[k], A[i][k]);
@@ -1432,7 +1856,7 @@ void svd_m4(float U[4][4], float s[4], float V[4][4], float A_[4][4])
                for (j = k + 1; j < n; j++) {
                        if ((k < nct) && (s[k] != 0.0f)) {
 
                for (j = k + 1; j < n; j++) {
                        if ((k < nct) && (s[k] != 0.0f)) {
 
-                               // Apply the transformation.
+                               /* Apply the transformation. */
 
                                float t = 0;
                                for (i = k; i < m; i++) {
 
                                float t = 0;
                                for (i = k; i < m; i++) {
@@ -1444,24 +1868,24 @@ void svd_m4(float U[4][4], float s[4], float V[4][4], float A_[4][4])
                                }
                        }
 
                                }
                        }
 
-                       // Place the k-th row of A into e for the
-                       // subsequent calculation of the row transformation.
+                       /* Place the k-th row of A into e for the */
+                       /* subsequent calculation of the row transformation. */
 
                        e[j] = A[k][j];
                }
                if (k < nct) {
 
 
                        e[j] = A[k][j];
                }
                if (k < nct) {
 
-                       // Place the transformation in U for subsequent back
-                       // multiplication.
+                       /* Place the transformation in U for subsequent back
+                        * multiplication. */
 
                        for (i = k; i < m; i++)
                                U[i][k] = A[i][k];
                }
                if (k < nrt) {
 
 
                        for (i = k; i < m; i++)
                                U[i][k] = A[i][k];
                }
                if (k < nrt) {
 
-                       // Compute the k-th row transformation and place the
-                       // k-th super-diagonal in e[k].
-                       // Compute 2-norm without under/overflow.
+                       /* Compute the k-th row transformation and place the
+                        * k-th super-diagonal in e[k].
+                        * Compute 2-norm without under/overflow. */
                        e[k] = 0;
                        for (i = k + 1; i < n; i++) {
                                e[k] = hypotf(e[k], e[i]);
                        e[k] = 0;
                        for (i = k + 1; i < n; i++) {
                                e[k] = hypotf(e[k], e[i]);
@@ -1481,7 +1905,7 @@ void svd_m4(float U[4][4], float s[4], float V[4][4], float A_[4][4])
                        if ((k + 1 < m) & (e[k] != 0.0f)) {
                                float invek1;
 
                        if ((k + 1 < m) & (e[k] != 0.0f)) {
                                float invek1;
 
-                               // Apply the transformation.
+                               /* Apply the transformation. */
 
                                for (i = k + 1; i < m; i++) {
                                        work[i] = 0.0f;
 
                                for (i = k + 1; i < m; i++) {
                                        work[i] = 0.0f;
@@ -1500,17 +1924,17 @@ void svd_m4(float U[4][4], float s[4], float V[4][4], float A_[4][4])
                                }
                        }
 
                                }
                        }
 
-                       // Place the transformation in V for subsequent
-                       // back multiplication.
+                       /* Place the transformation in V for subsequent
+                        * back multiplication. */
 
                        for (i = k + 1; i < n; i++)
                                V[i][k] = e[i];
                }
        }
 
 
                        for (i = k + 1; i < n; i++)
                                V[i][k] = e[i];
                }
        }
 
-       // Set up the final bidiagonal matrix or order p.
+       /* Set up the final bidiagonal matrix or order p. */
 
 
-       p = minf(n, m + 1);
+       p = min_ii(n, m + 1);
        if (nct < n) {
                s[nct] = A[nct][nct];
        }
        if (nct < n) {
                s[nct] = A[nct][nct];
        }
@@ -1522,7 +1946,7 @@ void svd_m4(float U[4][4], float s[4], float V[4][4], float A_[4][4])
        }
        e[p - 1] = 0.0f;
 
        }
        e[p - 1] = 0.0f;
 
-       // If required, generate U.
+       /* If required, generate U. */
 
        for (j = nct; j < nu; j++) {
                for (i = 0; i < m; i++) {
 
        for (j = nct; j < nu; j++) {
                for (i = 0; i < m; i++) {
@@ -1558,7 +1982,7 @@ void svd_m4(float U[4][4], float s[4], float V[4][4], float A_[4][4])
                }
        }
 
                }
        }
 
-       // If required, generate V.
+       /* If required, generate V. */
 
        for (k = n - 1; k >= 0; k--) {
                if ((k < nrt) & (e[k] != 0.0f)) {
 
        for (k = n - 1; k >= 0; k--) {
                if ((k < nrt) & (e[k] != 0.0f)) {
@@ -1579,7 +2003,7 @@ void svd_m4(float U[4][4], float s[4], float V[4][4], float A_[4][4])
                V[k][k] = 1.0f;
        }
 
                V[k][k] = 1.0f;
        }
 
-       // Main iteration loop for the singular values.
+       /* Main iteration loop for the singular values. */
 
        pp = p - 1;
        iter = 0;
 
        pp = p - 1;
        iter = 0;
@@ -1587,20 +2011,20 @@ void svd_m4(float U[4][4], float s[4], float V[4][4], float A_[4][4])
        while (p > 0) {
                int kase = 0;
 
        while (p > 0) {
                int kase = 0;
 
-               // Test for maximum iterations to avoid infinite loop
+               /* Test for maximum iterations to avoid infinite loop */
                if (maxiter == 0)
                        break;
                maxiter--;
 
                if (maxiter == 0)
                        break;
                maxiter--;
 
-               // This section of the program inspects for
-               // negligible elements in the s and e arrays.  On
-               // completion the variables kase and k are set as follows.
-
-               // kase = 1       if s(p) and e[k - 1] are negligible and k<p
-               // kase = 2       if s(k) is negligible and k<p
-               // kase = 3       if e[k - 1] is negligible, k<p, and
-               //               s(k), ..., s(p) are not negligible (qr step).
-               // kase = 4       if e(p - 1) is negligible (convergence).
+               /* This section of the program inspects for
+                * negligible elements in the s and e arrays.  On
+                * completion the variables kase and k are set as follows.
+                *
+                * kase = 1       if s(p) and e[k - 1] are negligible and k<p
+                * kase = 2       if s(k) is negligible and k<p
+                * kase = 3       if e[k - 1] is negligible, k<p, and
+                *               s(k), ..., s(p) are not negligible (qr step).
+                * kase = 4       if e(p - 1) is negligible (convergence). */
 
                for (k = p - 2; k >= -1; k--) {
                        if (k == -1) {
 
                for (k = p - 2; k >= -1; k--) {
                        if (k == -1) {
@@ -1641,11 +2065,11 @@ void svd_m4(float U[4][4], float s[4], float V[4][4], float A_[4][4])
                }
                k++;
 
                }
                k++;
 
-               // Perform the task indicated by kase.
+               /* Perform the task indicated by kase. */
 
                switch (kase) {
 
 
                switch (kase) {
 
-                       // Deflate negligible s(p).
+                       /* Deflate negligible s(p). */
 
                        case 1:
                        {
 
                        case 1:
                        {
@@ -1671,7 +2095,7 @@ void svd_m4(float U[4][4], float s[4], float V[4][4], float A_[4][4])
                                break;
                        }
 
                                break;
                        }
 
-                       // Split at negligible s(k).
+                       /* Split at negligible s(k). */
 
                        case 2:
                        {
 
                        case 2:
                        {
@@ -1695,14 +2119,14 @@ void svd_m4(float U[4][4], float s[4], float V[4][4], float A_[4][4])
                                break;
                        }
 
                                break;
                        }
 
-                       // Perform one qr step.
+                       /* Perform one qr step. */
 
                        case 3:
                        {
 
 
                        case 3:
                        {
 
-                               // Calculate the shift.
+                               /* Calculate the shift. */
 
 
-                               float scale = maxf(maxf(maxf(maxf(
+                               float scale = max_ff(max_ff(max_ff(max_ff(
                                                   fabsf(s[p - 1]), fabsf(s[p - 2])), fabsf(e[p - 2])),
                                                   fabsf(s[k])), fabsf(e[k]));
                                float invscale = 1.0f / scale;
                                                   fabsf(s[p - 1]), fabsf(s[p - 2])), fabsf(e[p - 2])),
                                                   fabsf(s[k])), fabsf(e[k]));
                                float invscale = 1.0f / scale;
@@ -1725,7 +2149,7 @@ void svd_m4(float U[4][4], float s[4], float V[4][4], float A_[4][4])
                                f = (sk + sp) * (sk - sp) + shift;
                                g = sk * ek;
 
                                f = (sk + sp) * (sk - sp) + shift;
                                g = sk * ek;
 
-                               // Chase zeros.
+                               /* Chase zeros. */
 
                                for (j = k; j < p - 1; j++) {
                                        float t = hypotf(f, g);
 
                                for (j = k; j < p - 1; j++) {
                                        float t = hypotf(f, g);
@@ -1767,12 +2191,12 @@ void svd_m4(float U[4][4], float s[4], float V[4][4], float A_[4][4])
                                iter = iter + 1;
                                break;
                        }
                                iter = iter + 1;
                                break;
                        }
-                       // Convergence.
+                       /* Convergence. */
 
                        case 4:
                        {
 
 
                        case 4:
                        {
 
-                               // Make the singular values positive.
+                               /* Make the singular values positive. */
 
                                if (s[k] <= 0.0f) {
                                        s[k] = (s[k] < 0.0f ? -s[k] : 0.0f);
 
                                if (s[k] <= 0.0f) {
                                        s[k] = (s[k] < 0.0f ? -s[k] : 0.0f);
@@ -1781,7 +2205,7 @@ void svd_m4(float U[4][4], float s[4], float V[4][4], float A_[4][4])
                                                V[i][k] = -V[i][k];
                                }
 
                                                V[i][k] = -V[i][k];
                                }
 
-                               // Order the singular values.
+                               /* Order the singular values. */
 
                                while (k < pp) {
                                        float t;
 
                                while (k < pp) {
                                        float t;
@@ -1833,5 +2257,92 @@ void pseudoinverse_m4_m4(float Ainv[4][4], float A[4][4], float epsilon)
 
        transpose_m4(V);
 
 
        transpose_m4(V);
 
-       mul_serie_m4(Ainv, U, Wm, V, NULL, NULL, NULL, NULL, NULL);
+       mul_m4_series(Ainv, U, Wm, V);
+}
+
+void pseudoinverse_m3_m3(float Ainv[3][3], float A[3][3], float epsilon)
+{
+       /* try regular inverse when possible, otherwise fall back to slow svd */
+       if (!invert_m3_m3(Ainv, A)) {
+               float tmp[4][4], tmpinv[4][4];
+
+               copy_m4_m3(tmp, A);
+               pseudoinverse_m4_m4(tmpinv, tmp, epsilon);
+               copy_m3_m4(Ainv, tmpinv);
+       }
+}
+
+bool has_zero_axis_m4(float matrix[4][4])
+{
+       return len_squared_v3(matrix[0]) < FLT_EPSILON ||
+              len_squared_v3(matrix[1]) < FLT_EPSILON ||
+              len_squared_v3(matrix[2]) < FLT_EPSILON;
+}
+
+void invert_m4_m4_safe(float Ainv[4][4], float A[4][4])
+{
+       if (!invert_m4_m4(Ainv, A)) {
+               float Atemp[4][4];
+
+               copy_m4_m4(Atemp, A);
+
+               /* Matrix is degenerate (e.g. 0 scale on some axis), ideally we should
+                * never be in this situation, but try to invert it anyway with tweak.
+                */
+               Atemp[0][0] += 1e-8f;
+               Atemp[1][1] += 1e-8f;
+               Atemp[2][2] += 1e-8f;
+
+               if (!invert_m4_m4(Ainv, Atemp)) {
+                       unit_m4(Ainv);
+               }
+       }
+}
+
+/**
+ * SpaceTransform struct encapsulates all needed data to convert between two coordinate spaces
+ * (where conversion can be represented by a matrix multiplication).
+ *
+ * A SpaceTransform is initialized using:
+ *   BLI_SPACE_TRANSFORM_SETUP(&data,  ob1, ob2)
+ *
+ * After that the following calls can be used:
+ *   BLI_space_transform_apply(&data, co);  // converts a coordinate in ob1 space to the corresponding ob2 space
+ *   BLI_space_transform_invert(&data, co);  // converts a coordinate in ob2 space to the corresponding ob1 space
+ *
+ * Same concept as BLI_space_transform_apply and BLI_space_transform_invert, but no is normalized after conversion
+ * (and not translated at all!):
+ *   BLI_space_transform_apply_normal(&data, no);
+ *   BLI_space_transform_invert_normal(&data, no);
+ *
+ */
+
+void BLI_space_transform_from_matrices(SpaceTransform *data, float local[4][4], float target[4][4])
+{
+       float itarget[4][4];
+       invert_m4_m4(itarget, target);
+       mul_m4_m4m4(data->local2target, itarget, local);
+       invert_m4_m4(data->target2local, data->local2target);
+}
+
+void BLI_space_transform_apply(const SpaceTransform *data, float co[3])
+{
+       mul_v3_m4v3(co, ((SpaceTransform *)data)->local2target, co);
+}
+
+void BLI_space_transform_invert(const SpaceTransform *data, float co[3])
+{
+       mul_v3_m4v3(co, ((SpaceTransform *)data)->target2local, co);
+}
+
+void BLI_space_transform_apply_normal(const SpaceTransform *data, float no[3])
+{
+       mul_mat3_m4_v3(((SpaceTransform *)data)->local2target, no);
+       normalize_v3(no);
+}
+
+void BLI_space_transform_invert_normal(const SpaceTransform *data, float no[3])
+{
+       mul_mat3_m4_v3(((SpaceTransform *)data)->target2local, no);
+       normalize_v3(no);
 }
 }