mathutils: added exponential map to Quaternion

Added conversion to and from exponential map representation. This
representation is useful for interpolation of > 2 quaternions, or in
PD controllers.

Implementation in C functions quat_to_expmap,
quat_normalized_to_expmap, and expmap_to_quat with Python API, unit
tests and documentation.

Added Quaternion.to_exponential_map() and Quaternion(3-vector) to
Python API.

Reviewers: campbellbarton

Projects: #bf_blender

Differential Revision: https://developer.blender.org/D1049

5 files changed, 117 insertions, 2 deletions

diff --git a/doc/python_api/examples/mathutils.Quaternion.py b/doc/python_api/examples/mathutils.Quaternion.py
index d8c696e6ba6..7e5538b7a96 100644
--- a/doc/python_api/examples/mathutils.Quaternion.py
+++ b/doc/python_api/examples/mathutils.Quaternion.py
@@ -21,3 +21,12 @@ print(quat_out)
 print("%.2f, %.2f, %.2f" % tuple(math.degrees(a) for a in quat_out.to_euler()))
 print("(%.2f, %.2f, %.2f), %.2f" % (quat_out.axis[:] +
                                     (math.degrees(quat_out.angle), )))
+
+# multiple rotations can be interpolated using the exponential map
+quat_c = mathutils.Quaternion((1.0, 0.0, 0.0), math.radians(15.0))
+exp_avg = (quat_a.to_exponential_map() +
+           quat_b.to_exponential_map() +
+           quat_c.to_exponential_map()) / 3.0
+quat_avg = mathutils.Quaternion(exp_avg)
+print("Average rotation:")
+print(quat_avg)
diff --git a/source/blender/blenlib/BLI_math_rotation.h b/source/blender/blenlib/BLI_math_rotation.h
index 905889a33d7..fbd026f7617 100644
--- a/source/blender/blenlib/BLI_math_rotation.h
+++ b/source/blender/blenlib/BLI_math_rotation.h
@@ -119,6 +119,11 @@ void mat4_to_axis_angle(float axis[3], float *angle, float M[4][4]);
 void axis_angle_to_mat3_single(float R[3][3], const char axis, const float angle);
 void      angle_to_mat2(float R[2][2], const float angle);
 
+/****************************** Exponential Map ******************************/
+void quat_to_expmap(float expmap[3], const float q[4]);
+void quat_normalized_to_expmap(float expmap[3], const float q[4]);
+void expmap_to_quat(float r[4], const float expmap[3]);
+
 /******************************** XYZ Eulers *********************************/
 
 void eul_to_quat(float quat[4], const float eul[3]);
diff --git a/source/blender/blenlib/intern/math_rotation.c b/source/blender/blenlib/intern/math_rotation.c
index 3ac031d7b90..3d5d47bc2e0 100644
--- a/source/blender/blenlib/intern/math_rotation.c
+++ b/source/blender/blenlib/intern/math_rotation.c
@@ -1016,6 +1016,40 @@ void angle_to_mat2(float mat[2][2], const float angle)
 	mat[1][1] =  angle_cos;
 }
 
+/****************************** Exponential Map ******************************/
+
+void quat_normalized_to_expmap(float expmap[3], const float q[4])
+{
+	float angle;
+	BLI_ASSERT_UNIT_QUAT(q);
+
+	/* Obtain axis/angle representation. */
+	quat_to_axis_angle(expmap, &angle, q);
+
+	/* Convert to exponential map. */
+	mul_v3_fl(expmap, angle);
+}
+
+void quat_to_expmap(float expmap[3], const float q[4])
+{
+	float q_no[4];
+	normalize_qt_qt(q_no, q);
+	quat_normalized_to_expmap(expmap, q_no);
+}
+
+void expmap_to_quat(float r[4], const float expmap[3])
+{
+	float axis[3];
+	float angle;
+
+	/* Obtain axis/angle representation. */
+	angle = normalize_v3_v3(axis, expmap);
+	angle = angle_wrap_rad(angle);
+
+	/* Convert to quaternion. */
+	axis_angle_to_quat(r, axis, angle);
+}
+
 /******************************** XYZ Eulers *********************************/
 
 /* XYZ order */
diff --git a/source/blender/python/mathutils/mathutils_Quaternion.c b/source/blender/python/mathutils/mathutils_Quaternion.c
index 98ee2fbd7cc..786a9324adf 100644
--- a/source/blender/python/mathutils/mathutils_Quaternion.c
+++ b/source/blender/python/mathutils/mathutils_Quaternion.c
@@ -177,6 +177,28 @@ static PyObject *Quaternion_to_axis_angle(QuaternionObject *self)
 	return ret;
 }
 
+PyDoc_STRVAR(Quaternion_to_exponential_map_doc,
+".. method:: to_exponential_map()\n"
+"\n"
+"   Return the exponential map representation of the quaternion.\n"
+"\n"
+"   This representation consist of the rotation axis multiplied by the rotation angle."
+"   Such a representation is useful for interpolation between multiple orientations.\n"
+"\n"
+"   :return: exponential map.\n"
+"   :rtype: :class:`Vector` of size 3\n"
+);
+static PyObject *Quaternion_to_exponential_map(QuaternionObject *self)
+{
+	float expmap[3];
+
+	if (BaseMath_ReadCallback(self) == -1)
+		return NULL;
+
+	quat_to_expmap(expmap, self->quat);
+	return Vector_CreatePyObject(expmap, 3, NULL);
+}
+
 PyDoc_STRVAR(Quaternion_cross_doc,
 ".. method:: cross(other)\n"
 "\n"
@@ -1077,9 +1099,24 @@ static PyObject *Quaternion_new(PyTypeObject *type, PyObject *args, PyObject *kw
 		case 0:
 			break;
 		case 1:
-			if (mathutils_array_parse(quat, QUAT_SIZE, QUAT_SIZE, seq, "mathutils.Quaternion()") == -1)
+		{
+			int size;
+
+			if ((size = mathutils_array_parse(quat, 3, QUAT_SIZE, seq, "mathutils.Quaternion()")) == -1) {
 				return NULL;
+			}
+
+			if (size == 4) {
+				/* 4d: Quaternion (common case) */
+			}
+			else {
+				/* 3d: Interpret as exponential map */
+				BLI_assert(size == 3);
+				expmap_to_quat(quat, quat);
+			}
+
 			break;
+		}
 		case 2:
 		{
 			float axis[3];
@@ -1156,6 +1193,7 @@ static struct PyMethodDef Quaternion_methods[] = {
 	{"to_euler", (PyCFunction) Quaternion_to_euler, METH_VARARGS, Quaternion_to_euler_doc},
 	{"to_matrix", (PyCFunction) Quaternion_to_matrix, METH_NOARGS, Quaternion_to_matrix_doc},
 	{"to_axis_angle", (PyCFunction) Quaternion_to_axis_angle, METH_NOARGS, Quaternion_to_axis_angle_doc},
+	{"to_exponential_map", (PyCFunction) Quaternion_to_exponential_map, METH_NOARGS, Quaternion_to_exponential_map_doc},
 
 	/* operation between 2 or more types  */
 	{"cross", (PyCFunction) Quaternion_cross, METH_O, Quaternion_cross_doc},
diff --git a/tests/python/bl_pyapi_mathutils.py b/tests/python/bl_pyapi_mathutils.py
index 85232e465d7..b7f61df0e40 100644
--- a/tests/python/bl_pyapi_mathutils.py
+++ b/tests/python/bl_pyapi_mathutils.py
@@ -2,7 +2,7 @@
 
 # ./blender.bin --background -noaudio --python tests/python/bl_pyapi_mathutils.py -- --verbose
 import unittest
-from mathutils import Matrix, Vector
+from mathutils import Matrix, Vector, Quaternion
 from mathutils import kdtree
 import math
 
@@ -210,6 +210,35 @@ class VectorTesting(unittest.TestCase):
                 self.assertAlmostEqual(v.angle(v.orthogonal()), angle_90d)
 
 
+class QuaternionTesting(unittest.TestCase):
+
+    def test_to_expmap(self):
+        q = Quaternion((0, 0, 1), math.radians(90))
+
+        e = q.to_exponential_map()
+        self.assertAlmostEqual(e.x, 0)
+        self.assertAlmostEqual(e.y, 0)
+        self.assertAlmostEqual(e.z, math.radians(90), 6)
+
+    def test_expmap_axis_normalization(self):
+        q = Quaternion((1, 1, 0), 2)
+        e = q.to_exponential_map()
+
+        self.assertAlmostEqual(e.x, 2 * math.sqrt(0.5), 6)
+        self.assertAlmostEqual(e.y, 2 * math.sqrt(0.5), 6)
+        self.assertAlmostEqual(e.z, 0)
+
+    def test_from_expmap(self):
+        e = Vector((1, 1, 0))
+        q = Quaternion(e)
+        axis, angle = q.to_axis_angle()
+
+        self.assertAlmostEqual(angle, math.sqrt(2), 6)
+        self.assertAlmostEqual(axis.x, math.sqrt(0.5), 6)
+        self.assertAlmostEqual(axis.y, math.sqrt(0.5), 6)
+        self.assertAlmostEqual(axis.z, 0)
+
+
 class KDTreeTesting(unittest.TestCase):
 
     @staticmethod