From 7fdf720fb164e448703c7d9f6103a3b59178189d Mon Sep 17 00:00:00 2001
From: Gaia Clary <gaia.clary@machinimatrix.org>
Date: Fri, 16 Feb 2018 12:37:36 +0100
Subject: [PATCH] Fix rotation issues due to matrix to quaternion ambiguities

Reviewers: mont29

Reviewed By: mont29

Subscribers: mont29

Differential Revision: https://developer.blender.org/D3066
---
 source/blender/collada/AnimationImporter.cpp | 14 ++++++++--
 source/blender/collada/collada_utils.cpp     | 29 ++++++++++++++++++++
 source/blender/collada/collada_utils.h       |  1 +
 3 files changed, 42 insertions(+), 2 deletions(-)

diff --git a/source/blender/collada/AnimationImporter.cpp b/source/blender/collada/AnimationImporter.cpp
index fec8d62933a..bc91b94afd9 100644
--- a/source/blender/collada/AnimationImporter.cpp
+++ b/source/blender/collada/AnimationImporter.cpp
@@ -780,6 +780,9 @@ void AnimationImporter::apply_matrix_curves(Object *ob, std::vector<FCurve *>& a
 
 	std::vector<float>::iterator it;
 
+	float qref[4];
+	unit_qt(qref);
+
 	// sample values at each frame
 	for (it = frames.begin(); it != frames.end(); it++) {
 		float fra = *it;
@@ -815,7 +818,9 @@ void AnimationImporter::apply_matrix_curves(Object *ob, std::vector<FCurve *>& a
 
 		float rot[4], loc[3], scale[3];
 
-		mat4_to_quat(rot, mat);
+		bc_rotate_from_reference_quat(rot, qref, mat);
+		copy_qt_qt(qref, rot);
+
 #if 0
 		for (int i = 0 ; i < 4;  i++) {
 			rot[i] = RAD2DEGF(rot[i]);
@@ -1190,6 +1195,9 @@ void AnimationImporter::add_bone_animation_sampled(Object *ob, std::vector<FCurv
 
 	std::sort(frames.begin(), frames.end());
 
+	float qref[4];
+	unit_qt(qref);
+
 	std::vector<float>::iterator it;
 
 	// sample values at each frame
@@ -1223,7 +1231,9 @@ void AnimationImporter::add_bone_animation_sampled(Object *ob, std::vector<FCurv
 
 		float rot[4], loc[3], scale[3];
 
-		mat4_to_quat(rot, mat);
+		bc_rotate_from_reference_quat(rot, qref, mat);
+		copy_qt_qt(qref, rot);
+
 		copy_v3_v3(loc, mat[3]);
 		mat4_to_size(scale, mat);
 
diff --git a/source/blender/collada/collada_utils.cpp b/source/blender/collada/collada_utils.cpp
index 37bd1a2a9c1..c13757fa2a4 100644
--- a/source/blender/collada/collada_utils.cpp
+++ b/source/blender/collada/collada_utils.cpp
@@ -377,6 +377,35 @@ void bc_decompose(float mat[4][4], float *loc, float eul[3], float quat[4], floa
 	}
 }
 
+/*
+* Create rotation_quaternion from a delta rotation and a reference quat
+*
+* Input:
+* mat_from: The rotation matrix before rotation
+* mat_to  : The rotation matrix after rotation
+* qref    : the quat corresponding to mat_from
+*
+* Output:
+* rot     : the calculated result (quaternion)
+*
+*/
+void bc_rotate_from_reference_quat(float quat_to[4], float quat_from[4], float mat_to[4][4])
+{
+	float qd[4];
+	float matd[4][4];
+	float mati[4][4];
+	float mat_from[4][4];
+	quat_to_mat4(mat_from, quat_from);
+
+	// Calculate the difference matrix matd between mat_from and mat_to
+	invert_m4_m4(mati, mat_from);
+	mul_m4_m4m4(matd, mati, mat_to);
+
+	mat4_to_quat(qd, matd);
+
+	mul_qt_qtqt(quat_to, quat_from, qd); // rot is the final rotation corresponding to mat_to
+}
+
 void bc_triangulate_mesh(Mesh *me)
 {
 	bool use_beauty  = false;
diff --git a/source/blender/collada/collada_utils.h b/source/blender/collada/collada_utils.h
index 93741fb0e70..ad274777e8d 100644
--- a/source/blender/collada/collada_utils.h
+++ b/source/blender/collada/collada_utils.h
@@ -92,6 +92,7 @@ extern void bc_match_scale(Object *ob, UnitConverter &bc_unit, bool scale_to_sce
 extern void bc_match_scale(std::vector<Object *> *objects_done, UnitConverter &unit_converter, bool scale_to_scene);
 
 extern void bc_decompose(float mat[4][4], float *loc, float eul[3], float quat[4], float *size);
+extern void bc_rotate_from_reference_quat(float quat_to[4], float quat_from[4], float mat_to[4][4]);
 
 extern void bc_triangulate_mesh(Mesh *me);
 extern bool bc_is_leaf_bone(Bone *bone);