Commit: 9fa628f35be31a18edfdb1e1fca8a6bd3b6b453c Author: Sybren A. Stüvel Date: Sun Feb 1 11:58:10 2015 +0100 Branches: master https://developer.blender.org/rB9fa628f35be31a18edfdb1e1fca8a6bd3b6b453c
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 =================================================================== M doc/python_api/examples/mathutils.Quaternion.py M source/blender/blenlib/BLI_math_rotation.h M source/blender/blenlib/intern/math_rotation.c M source/blender/python/mathutils/mathutils_Quaternion.c M tests/python/bl_pyapi_mathutils.py =================================================================== diff --git a/doc/python_api/examples/mathutils.Quaternion.py b/doc/python_api/examples/mathutils.Quaternion.py index d8c696e..7e5538b 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 905889a..fbd026f 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 3ac031d..3d5d47b 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 98ee2fb..786a932 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 85232e4..b7f61df 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 _______________________________________________ Bf-blender-cvs mailing list Bf-blender-cvs@blender.org http://lists.blender.org/mailman/listinfo/bf-blender-cvs