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
This commit is contained in:
parent
8c7e1b648b
commit
9fa628f35b
|
@ -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)
|
||||
|
|
|
@ -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]);
|
||||
|
|
|
@ -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 */
|
||||
|
|
|
@ -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},
|
||||
|
|
|
@ -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
|
||||
|
|
Loading…
Reference in New Issue