1 files changed, 95 insertions, 10 deletions

diff --git a/source/blender/collada/BCSampleData.cpp b/source/blender/collada/BCSampleData.cpp
index 868526928b0..cf3668a08af 100644
--- a/source/blender/collada/BCSampleData.cpp
+++ b/source/blender/collada/BCSampleData.cpp
@@ -20,10 +20,6 @@
 #include "BCSampleData.h"
 #include "collada_utils.h"
 
-BCSample::BCSample(Object *ob) : obmat(ob)
-{
-}
-
 BCSample::~BCSample()
 {
   BCBoneMatrixMap::iterator it;
@@ -43,6 +39,11 @@ void BCSample::add_bone_matrix(Bone *bone, Matrix &mat)
   bonemats[bone] = matrix;
 }
 
+BCMatrix::BCMatrix(const BCMatrix &mat)
+{
+  set_transform(mat.matrix);
+}
+
 BCMatrix::BCMatrix(Matrix &mat)
 {
   set_transform(mat);
@@ -53,11 +54,68 @@ BCMatrix::BCMatrix(Object *ob)
   set_transform(ob);
 }
 
-void BCMatrix::set_transform(Matrix &mat)
+BCMatrix::BCMatrix()
 {
-  copy_m4_m4(matrix, mat);
-  mat4_decompose(this->loc, this->q, this->size, mat);
-  quat_to_eul(this->rot, this->q);
+  unit();
+}
+
+BCMatrix::BCMatrix(BC_global_forward_axis global_forward_axis, BC_global_up_axis global_up_axis)
+{
+  float mrot[3][3];
+  float mat[4][4];
+  mat3_from_axis_conversion(
+      BC_DEFAULT_FORWARD, BC_DEFAULT_UP, global_forward_axis, global_up_axis, mrot);
+
+  transpose_m3(mrot); // TODO: Verify that mat3_from_axis_conversion() returns a transposed matrix
+  copy_m4_m3(mat, mrot);
+  set_transform(mat);
+}
+
+void BCMatrix::add_transform(const Matrix &mat, bool inverse)
+{
+  add_transform(this->matrix, mat, this->matrix, inverse);
+}
+
+void BCMatrix::add_transform(const BCMatrix &mat, bool inverse)
+{
+  add_transform(this->matrix, mat.matrix, this->matrix, inverse);
+}
+
+void BCMatrix::apply_transform(const BCMatrix &mat, bool inverse)
+{
+  apply_transform(this->matrix, mat.matrix, this->matrix, inverse);
+}
+
+void BCMatrix::add_transform(Matrix &to, const Matrix &transform, const Matrix &from, bool inverse)
+{
+  if (inverse) {
+    Matrix globinv;
+    invert_m4_m4(globinv, transform);
+    add_transform(to, globinv, from, /*inverse=*/false);
+  }
+  else {
+    mul_m4_m4m4(to, transform, from);
+  }
+}
+
+void BCMatrix::apply_transform(Matrix &to, const Matrix &transform, const Matrix &from, bool inverse)
+{
+  Matrix globinv;
+  invert_m4_m4(globinv, transform);
+  if (inverse) {
+    add_transform(to, globinv, from, /*inverse=*/false);
+  }
+  else {
+    mul_m4_m4m4(to, transform, from);
+    mul_m4_m4m4(to, to, globinv);
+  }
+}
+
+void BCMatrix::add_inverted_transform(Matrix &to, const Matrix &transform, const Matrix &from)
+{
+  Matrix workmat;
+  invert_m4_m4(workmat, transform);
+  mul_m4_m4m4(to, workmat, from);
 }
 
 void BCMatrix::set_transform(Object *ob)
@@ -71,6 +129,13 @@ void BCMatrix::set_transform(Object *ob)
   quat_to_compatible_eul(this->rot, ob->rot, this->q);
 }
 
+void BCMatrix::set_transform(Matrix &mat)
+{
+  copy_m4_m4(matrix, mat);
+  mat4_decompose(this->loc, this->q, this->size, mat);
+  quat_to_eul(this->rot, this->q);
+}
+
 const BCMatrix *BCSample::get_matrix(Bone *bone) const
 {
   BCBoneMatrixMap::const_iterator it = bonemats.find(bone);
@@ -126,12 +191,28 @@ void BCMatrix::sanitize(Matrix &mat, int precision)
 
 void BCMatrix::unit()
 {
-  unit_m4(matrix);
+  unit_m4(this->matrix);
+  mat4_decompose(this->loc, this->q, this->size, this->matrix);
+  quat_to_eul(this->rot, this->q);
 }
 
 /* We need double here because the OpenCollada API needs it.
  * precision = -1 indicates to not limit the precision. */
-void BCMatrix::get_matrix(double (&mat)[4][4], const bool transposed, const int precision) const
+void BCMatrix::get_matrix(DMatrix &mat, const bool transposed, const int precision) const
+{
+  for (int i = 0; i < 4; i++)
+    for (int j = 0; j < 4; j++) {
+      float val = (transposed) ? matrix[j][i] : matrix[i][j];
+      if (precision >= 0)
+        val = floor((val * pow(10, precision) + 0.5)) / pow(10, precision);
+      mat[i][j] = val;
+    }
+}
+
+void BCMatrix::get_matrix(Matrix &mat,
+                          const bool transposed,
+                          const int precision,
+                          const bool inverted) const
 {
   for (int i = 0; i < 4; i++)
     for (int j = 0; j < 4; j++) {
@@ -140,6 +221,10 @@ void BCMatrix::get_matrix(double (&mat)[4][4], const bool transposed, const int
         val = floor((val * pow(10, precision) + 0.5)) / pow(10, precision);
       mat[i][j] = val;
     }
+
+  if (inverted) {
+    invert_m4(mat);
+  }
 }
 
 const bool BCMatrix::in_range(const BCMatrix &other, float distance) const