# Implementing Quaternions in Julia

Julia is making waves in the scientific programming community as the language to learn if you truly want to be a computational code ninja. I won’t list out all the features which make Julia such an incredilicious language to learn. You can’t go clicks on the internet without stumbling into a paean to Julia, so one more such list would be highly redundant.

I will however use one example to illustrate what makes Julia that much more expressive, convenient and, indeed, fast compared to almost any language out there for scientific computing. Mind you, I am very much a Julia novice so I’m likely to get the technical jargon wrong in parts. I pray to the Julia-Gods for forgiveness for such infractions.

We all know what complex numbers are (right?) and many people have also heard of their cousins – the quaternions. Here’s a quick reminder to jog your memory. Those who are upto speed on their complex number and quaternions can jump ahead to the juicy parts in the section on implementing quaternions in either Python or in Julia 1

# Complex Numbers

Complex numbers are “tuples” (a fancy word for a set consisting of two objects) of real numbers, i.e. given $a, b \in \mathbb{R}$, one can define a complex number:

$$z = a + \imath b,$$

which is an element of the so-called “complex plane” $\mathbb{C}$ which is nothing more than the two-dimensional Euclidean plane $\mathbb{R}^2$ with one axis identified as “real” and the other as “imaginary”. The funny looking “i” multiplying $b$ in the expression above, is called the “unit imaginary” and is supposed to represent the square of root of $-1$:

$$\imath = \sqrt{-1}.$$

At this point, you’re probably thinking “what a load of …, you can’t take square roots of negative numbers!” And, you would be right! Indeed we can’t express the square root of a negative number in terms of real numbers. But, mathematicians being the wizards they are, decided there is no law which stops us from writing down the formal expression $\imath = \sqrt{-1}$. If you can suspend disbelief for a second, and just accept this fact, then all sorts of wonderful things become possible.

Given any two complex numbers we can add, subtract or multiply them to obtain another complex number:

\begin{align}
z_1 + z_2 & = (a_1 + a_2) + \imath (b_1 + b_2) \nonumber \\
z_1 – z_2 & = (a_1 – a_2) + \imath (b_1 – b_2) \nonumber \\
z_1 z_2 & = (a_1 a_2 – b_1 b_2) + \imath (a_1 b_2 + a_2 b_2),
\end{align}

where in the last line we have used the fact that $(\imath b_1) (\imath b_2) = \imath^2 b_1 b_2 = – b_1 b_2$. We can also divide complex numbers by other complex numbers. However, in order to write the result of a division operation $z_1/z_2$ as another complex number, we have to introduce a new operation called “complex conjugation” denoted by an asterix ${}^*$. Under complex conjugation, real numbers are unchanged, but purely imaginary numbers becomes negative:

$$a^* = a; \quad (\imath b) = – \imath b; \Rightarrow z^* = (a + \imath b)^* = a – \imath b$$

where $a, b \in \mbb{R}$. The result of dividing one complex number by another can then be expressed as follows:

\begin{align}
\frac{z_1}{z_2} & = \frac{a_1 + \imath b_1}{a_2 + \imath b_2} \nonumber \\
& = \frac{(a_1 + \imath b_1)}{(a_2 + \imath b_2)} \frac{(a_2 – \imath b_2)}{(a_2 – \imath b_2)} \nonumber \nonumber \\
& = \frac{a_1 a_2 + b_1 b_2 + \imath (b_1 a_2 – b_2 a_1)}{a_2^2 + b_2^2} \nonumber \\
& = \frac{1}{|z_2|^2} z_1 z_2^*.
\end{align}

In the second line we have multiplied the numerator and denominator by $z_2^*$. In the third line we have used the fact that the multiple of a complex number with its complex conjugate yields a real number ($z z^* = a^2 + b^2 \in \mbb{R}$) to get rid of the imaginary unit in the denominator. The real and imaginary parts of $z_1/z_2$ can then be written as:

$$\mf{Re}(z_1/z_2) = \frac{a_1 a_2 + b_1 b_2}{a_2^2 + b_2^2}; \quad \mf{Im}(z_1/z_2) = \frac{b_1 a_2 – b_2 a_1}{a_2^2 + b_2^2}.$$

In this way we can perform all the standard arithmetical operations with complex numbers. We can go on to define functions on the complex plane $f: \mbb{C} \rightarrow \mbb{C}$. All the usual functions on real numbers, such as polynomials $x^n$, roots $x^{1/n}$, trignometric and logarithmic functions $\sin(x), \log(x)$, can be generalized straightforwardly to take complex numbers as their arguments instead of real numbers.

This only touches the tip of the proverbial iceberg. There is a lot more to say about complex numbers, but this introduction will suffice to enable us to talk about our true goal – the quaternions.

# Quaternions

Quaternions are just like complex numbers, with one minor difference. Instead of just one imaginary axis as in the complex numbers, quaternions have three different imaginary axes. Blew your mind, right? Well, not really. We don’t have any difficulties in extending the real line to the two or three-dimensional spaces $\mbb{R}^2$ and $\mbb{R}^n$. We are comfortable defining vectors in $\mbb{R}^n$ as tuples of $n$ real numbers: $\vect{v} = (v_1, v_2, \ldots, v_n)$ and performing all the usual arithmetical operations (except for division) with these objects. As we have just seen, we can define a new kind of axis which represents the “imaginary” dimension. Well, what’s to stop us from extending $\mbb{C}$ by adding more imaginary dimensions? Nothing!

Let’s start small and add just one extra imaginary axis. Our “triplex” numbers would then have the form:

$$\gamma = a + i b + j c,$$

where, expectedly, $i^2 = j^2 = -1$2 and $a,b,c \in \mbb{R}$. It turns out that the set of triplex numbers is not closed under the arithmetic operation of multiplication. We soldier on ahead and add one more dimension:

$$q = a + i b + j c + k d,$$

where, once again:

\label{eqn:quaternions-rule1}
i^2 = j^2 = k^2 = -1,

However, now there is a new rule in town. When multiplying any two imaginary components with each other one gets the third kind of imaginary. This can be expressed as:

\label{eqn:quaternions-rule2}
i \cdot j = k; \quad j \cdot k = i; \quad k \cdot i = j

What about $j \cdot i$ ? We can use the expression $j \cdot k = 1$ from \eqref{eqn:quaternions-rule2} above, to write:

$$j \cdot i = j \cdot j \cdot k = -k$$

We can summarize the rules for quaternion imaginaries:

OperationResult
Complex Conjugation$i^* = -i; \; j^* = -j; \; k^* = -k$
Square$i^2 = j^2 = k^2 = -1$
Complex Multiplication$i \cdot j = k; \; j \cdot k = i; \; k \cdot i = j$ with $j \cdot i = -k$, etc.

One can now use these rules, in addition to the usual rules for addition and subtraction, to perform all arithmetical operations with quaternions. Let me illustrate with the example of multiplying two quaternions $p_1$ and $p_2$ to get a third quaternion $p_3$. If we use the notation $p_n = a_n + i b_n + j c_n + k d_n$ to represent the $n{}^\text{th}$ quaternion in terms of its components, then for the components of $p_3$ in terms of the components of $p_1$ and $p_2$ we obtain the following expression:

\begin{align}
a_3 & = a_1 a_2 – b_1 b_2 – c_1 c_2 – d_1 d_2 \nonumber \\
b_3 & = a_1 b_2 + a_2 b_1 + c_1 d_2 – c_2 d_1 \nonumber \\
c_3 & = a_1 c_2 + a_2 c_1 + d_1 b_2 – d_2 b_1 \nonumber \\
d_3 & = a_1 d_2 + a_2 d_1 + b_1 c_2 – b_2 c_1
\end{align}

This introduction is sufficient to allow us to move on to discuss implementation of quaternions in the two commonly used scripting languages – Python and Julia.

# In Python

There are two ways to implement custom types in Python. One can either follow the procedure for constructing custom types which can be manipulated in the same way as core Python types such as strings and lists.

## Custom Types in Python

The following code provides the basic template for defining custom types in Python 2.7. To begin with one has to write the skeleton C code base, implement all the relevant methods and finally compile everything using Cython or some other C/Python compiler.

 // Source: https://docs.python.org/2/extending/newtypes.html #include typedef struct { PyObject_HEAD /* Type-specific fields go here. */ } noddy_NoddyObject; static PyTypeObject noddy_NoddyType = { PyVarObject_HEAD_INIT(NULL, 0) "noddy.Noddy", /* tp_name */ sizeof(noddy_NoddyObject), /* tp_basicsize */ 0, /* tp_itemsize */ 0, /* tp_dealloc */ 0, /* tp_print */ 0, /* tp_getattr */ 0, /* tp_setattr */ 0, /* tp_compare */ 0, /* tp_repr */ 0, /* tp_as_number */ 0, /* tp_as_sequence */ 0, /* tp_as_mapping */ 0, /* tp_hash */ 0, /* tp_call */ 0, /* tp_str */ 0, /* tp_getattro */ 0, /* tp_setattro */ 0, /* tp_as_buffer */ Py_TPFLAGS_DEFAULT, /* tp_flags */ "Noddy objects", /* tp_doc */ }; static PyMethodDef noddy_methods[] = { {NULL} /* Sentinel */ }; #ifndef PyMODINIT_FUNC /* declarations for DLL import/export */ #define PyMODINIT_FUNC void #endif PyMODINIT_FUNC initnoddy(void) { PyObject* m; noddy_NoddyType.tp_new = PyType_GenericNew; if (PyType_Ready(&noddy_NoddyType) < 0) return; m = Py_InitModule3("noddy", noddy_methods, "Example module that creates an extension type."); Py_INCREF(&noddy_NoddyType); PyModule_AddObject(m, "Noddy", (PyObject *)&noddy_NoddyType); }
view raw CustomTypesPy27.c hosted with ❤ by GitHub

The procedure is not too different in Python 3.7.

 // Source: https://docs.python.org/3.7/extending/newtypes.html typedef struct _typeobject { PyObject_VAR_HEAD const char *tp_name; /* For printing, in format "." */ Py_ssize_t tp_basicsize, tp_itemsize; /* For allocation */ /* Methods to implement standard operations */ destructor tp_dealloc; printfunc tp_print; getattrfunc tp_getattr; setattrfunc tp_setattr; PyAsyncMethods *tp_as_async; /* formerly known as tp_compare (Python 2) or tp_reserved (Python 3) */ reprfunc tp_repr; /* Method suites for standard classes */ PyNumberMethods *tp_as_number; PySequenceMethods *tp_as_sequence; PyMappingMethods *tp_as_mapping; /* More standard operations (here for binary compatibility) */ hashfunc tp_hash; ternaryfunc tp_call; reprfunc tp_str; getattrofunc tp_getattro; setattrofunc tp_setattro; /* Functions to access object as input/output buffer */ PyBufferProcs *tp_as_buffer; /* Flags to define presence of optional/expanded features */ unsigned long tp_flags; const char *tp_doc; /* Documentation string */ /* call function for all accessible objects */ traverseproc tp_traverse; /* delete references to contained objects */ inquiry tp_clear; /* rich comparisons */ richcmpfunc tp_richcompare; /* weak reference enabler */ Py_ssize_t tp_weaklistoffset; /* Iterators */ getiterfunc tp_iter; iternextfunc tp_iternext; /* Attribute descriptor and subclassing stuff */ struct PyMethodDef *tp_methods; struct PyMemberDef *tp_members; struct PyGetSetDef *tp_getset; struct _typeobject *tp_base; PyObject *tp_dict; descrgetfunc tp_descr_get; descrsetfunc tp_descr_set; Py_ssize_t tp_dictoffset; initproc tp_init; allocfunc tp_alloc; newfunc tp_new; freefunc tp_free; /* Low-level free-memory routine */ inquiry tp_is_gc; /* For PyObject_IS_GC */ PyObject *tp_bases; PyObject *tp_mro; /* method resolution order */ PyObject *tp_cache; PyObject *tp_subclasses; PyObject *tp_weaklist; destructor tp_del; /* Type attribute cache version tag. Added in version 2.6 */ unsigned int tp_version_tag; destructor tp_finalize; } PyTypeObject;
view raw CustomTypesPy37.c hosted with ❤ by GitHub

This is the hard way to implement a custom type in Python, but also the only way if one wants the custom type to act and behave like any other of Python’s core types. A non-trivial amount of C code, C-Python interoperability and Python code is needed for this purpose.

## Examples: numpy_quaternion.c

Let us look at another example which aims to extend the core capabilities of the numpy module to include quaternion arithmetic. The file shown is numpy_quaternion.c, containing the C code implementing code for construction quaternion objects in Python. A mere 1,628 lines long. And this does not include the C header files and various other supporting C and Python codes.

 // Copyright (c) 2017, Michael Boyle // See LICENSE file for details: #define NPY_NO_DEPRECATED_API NPY_API_VERSION #include #include #include #include #include "structmember.h" #include "quaternion.h" // The following definitions, along with #define NPY_PY3K 1, can // also be found in the header . #if PY_MAJOR_VERSION >= 3 #define PyUString_FromString PyUnicode_FromString static NPY_INLINE int PyInt_Check(PyObject *op) { int overflow = 0; if (!PyLong_Check(op)) { return 0; } PyLong_AsLongAndOverflow(op, &overflow); return (overflow == 0); } #define PyInt_AsLong PyLong_AsLong #else #define PyUString_FromString PyString_FromString #endif // The basic python object holding a quaternion typedef struct { PyObject_HEAD quaternion obval; } PyQuaternion; static PyTypeObject PyQuaternion_Type; // This is the crucial feature that will make a quaternion into a // built-in numpy data type. We will describe its features below. PyArray_Descr* quaternion_descr; static NPY_INLINE int PyQuaternion_Check(PyObject* object) { return PyObject_IsInstance(object,(PyObject*)&PyQuaternion_Type); } static PyObject* PyQuaternion_FromQuaternion(quaternion q) { PyQuaternion* p = (PyQuaternion*)PyQuaternion_Type.tp_alloc(&PyQuaternion_Type,0); if (p) { p->obval = q; } return (PyObject*)p; } // TODO: Add list/tuple conversions #define PyQuaternion_AsQuaternion(q, o) \ /* fprintf (stderr, "file %s, line %d., PyQuaternion_AsQuaternion\n", __FILE__, __LINE__); */ \ if(PyQuaternion_Check(o)) { \ q = ((PyQuaternion*)o)->obval; \ } else { \ PyErr_SetString(PyExc_TypeError, \ "Input object is not a quaternion."); \ return NULL; \ } #define PyQuaternion_AsQuaternionPointer(q, o) \ /* fprintf (stderr, "file %s, line %d, PyQuaternion_AsQuaternionPointer.\n", __FILE__, __LINE__); */ \ if(PyQuaternion_Check(o)) { \ q = &((PyQuaternion*)o)->obval; \ } else { \ PyErr_SetString(PyExc_TypeError, \ "Input object is not a quaternion."); \ return NULL; \ } static PyObject * pyquaternion_new(PyTypeObject *type, PyObject *NPY_UNUSED(args), PyObject *NPY_UNUSED(kwds)) { PyQuaternion* self; self = (PyQuaternion *)type->tp_alloc(type, 0); return (PyObject *)self; } static int pyquaternion_init(PyObject *self, PyObject *args, PyObject *kwds) { // "A good rule of thumb is that for immutable types, all // initialization should take place in tp_new, while for mutable // types, most initialization should be deferred to tp_init." // ---Python 2.7.8 docs Py_ssize_t size = PyTuple_Size(args); quaternion* q; PyObject* Q = {0}; q = &(((PyQuaternion*)self)->obval); if (kwds && PyDict_Size(kwds)) { PyErr_SetString(PyExc_TypeError, "quaternion constructor takes no keyword arguments"); return -1; } q->w = 0.0; q->x = 0.0; q->y = 0.0; q->z = 0.0; if(size == 0) { return 0; } else if(size == 1) { if(PyArg_ParseTuple(args, "O", &Q) && PyQuaternion_Check(Q)) { q->w = ((PyQuaternion*)Q)->obval.w; q->x = ((PyQuaternion*)Q)->obval.x; q->y = ((PyQuaternion*)Q)->obval.y; q->z = ((PyQuaternion*)Q)->obval.z; return 0; } else if(PyArg_ParseTuple(args, "d", &q->w)) { return 0; } } else if(size == 3 && PyArg_ParseTuple(args, "ddd", &q->x, &q->y, &q->z)) { return 0; } else if(size == 4 && PyArg_ParseTuple(args, "dddd", &q->w, &q->x, &q->y, &q->z)) { return 0; } PyErr_SetString(PyExc_TypeError, "quaternion constructor takes zero, one, three, or four float arguments, or a single quaternion"); return -1; } #define UNARY_BOOL_RETURNER(name) \ static PyObject* \ pyquaternion_##name(PyObject* a, PyObject* NPY_UNUSED(b)) { \ quaternion q = {0.0, 0.0, 0.0, 0.0}; \ PyQuaternion_AsQuaternion(q, a); \ return PyBool_FromLong(quaternion_##name(q)); \ } UNARY_BOOL_RETURNER(nonzero) UNARY_BOOL_RETURNER(isnan) UNARY_BOOL_RETURNER(isinf) UNARY_BOOL_RETURNER(isfinite) #define BINARY_BOOL_RETURNER(name) \ static PyObject* \ pyquaternion_##name(PyObject* a, PyObject* b) { \ quaternion p = {0.0, 0.0, 0.0, 0.0}; \ quaternion q = {0.0, 0.0, 0.0, 0.0}; \ PyQuaternion_AsQuaternion(p, a); \ PyQuaternion_AsQuaternion(q, b); \ return PyBool_FromLong(quaternion_##name(p,q)); \ } BINARY_BOOL_RETURNER(equal) BINARY_BOOL_RETURNER(not_equal) BINARY_BOOL_RETURNER(less) BINARY_BOOL_RETURNER(greater) BINARY_BOOL_RETURNER(less_equal) BINARY_BOOL_RETURNER(greater_equal) #define UNARY_FLOAT_RETURNER(name) \ static PyObject* \ pyquaternion_##name(PyObject* a, PyObject* NPY_UNUSED(b)) { \ quaternion q = {0.0, 0.0, 0.0, 0.0}; \ PyQuaternion_AsQuaternion(q, a); \ return PyFloat_FromDouble(quaternion_##name(q)); \ } UNARY_FLOAT_RETURNER(absolute) UNARY_FLOAT_RETURNER(norm) UNARY_FLOAT_RETURNER(angle) #define UNARY_QUATERNION_RETURNER(name) \ static PyObject* \ pyquaternion_##name(PyObject* a, PyObject* NPY_UNUSED(b)) { \ quaternion q = {0.0, 0.0, 0.0, 0.0}; \ PyQuaternion_AsQuaternion(q, a); \ return PyQuaternion_FromQuaternion(quaternion_##name(q)); \ } UNARY_QUATERNION_RETURNER(negative) UNARY_QUATERNION_RETURNER(conjugate) UNARY_QUATERNION_RETURNER(inverse) UNARY_QUATERNION_RETURNER(sqrt) UNARY_QUATERNION_RETURNER(log) UNARY_QUATERNION_RETURNER(exp) UNARY_QUATERNION_RETURNER(normalized) UNARY_QUATERNION_RETURNER(x_parity_conjugate) UNARY_QUATERNION_RETURNER(x_parity_symmetric_part) UNARY_QUATERNION_RETURNER(x_parity_antisymmetric_part) UNARY_QUATERNION_RETURNER(y_parity_conjugate) UNARY_QUATERNION_RETURNER(y_parity_symmetric_part) UNARY_QUATERNION_RETURNER(y_parity_antisymmetric_part) UNARY_QUATERNION_RETURNER(z_parity_conjugate) UNARY_QUATERNION_RETURNER(z_parity_symmetric_part) UNARY_QUATERNION_RETURNER(z_parity_antisymmetric_part) UNARY_QUATERNION_RETURNER(parity_conjugate) UNARY_QUATERNION_RETURNER(parity_symmetric_part) UNARY_QUATERNION_RETURNER(parity_antisymmetric_part) static PyObject* pyquaternion_positive(PyObject* self, PyObject* NPY_UNUSED(b)) { Py_INCREF(self); return self; } #define QQ_BINARY_QUATERNION_RETURNER(name) \ static PyObject* \ pyquaternion_##name(PyObject* a, PyObject* b) { \ quaternion p = {0.0, 0.0, 0.0, 0.0}; \ quaternion q = {0.0, 0.0, 0.0, 0.0}; \ PyQuaternion_AsQuaternion(p, a); \ PyQuaternion_AsQuaternion(q, b); \ return PyQuaternion_FromQuaternion(quaternion_##name(p,q)); \ } /* QQ_BINARY_QUATERNION_RETURNER(add) */ /* QQ_BINARY_QUATERNION_RETURNER(subtract) */ QQ_BINARY_QUATERNION_RETURNER(copysign) #define QQ_QS_SQ_BINARY_QUATERNION_RETURNER_FULL(fake_name, name) \ static PyObject* \ pyquaternion_##fake_name##_array_operator(PyObject* a, PyObject* b) { \ NpyIter *iter; \ NpyIter_IterNextFunc *iternext; \ PyArrayObject *op[2]; \ PyObject *ret; \ npy_uint32 flags; \ npy_uint32 op_flags[2]; \ PyArray_Descr *op_dtypes[2]; \ npy_intp itemsize, *innersizeptr, innerstride; \ char **dataptrarray; \ char *src, *dst; \ quaternion p = {0.0, 0.0, 0.0, 0.0}; \ PyQuaternion_AsQuaternion(p, a); \ flags = NPY_ITER_EXTERNAL_LOOP; \ op[0] = (PyArrayObject *) b; \ op[1] = NULL; \ op_flags[0] = NPY_ITER_READONLY; \ op_flags[1] = NPY_ITER_WRITEONLY | NPY_ITER_ALLOCATE; \ op_dtypes[0] = PyArray_DESCR((PyArrayObject*) b); \ op_dtypes[1] = quaternion_descr; \ iter = NpyIter_MultiNew(2, op, flags, NPY_KEEPORDER, NPY_NO_CASTING, op_flags, op_dtypes); \ if (iter == NULL) { \ return NULL; \ } \ iternext = NpyIter_GetIterNext(iter, NULL); \ innerstride = NpyIter_GetInnerStrideArray(iter)[0]; \ itemsize = NpyIter_GetDescrArray(iter)[1]->elsize; \ innersizeptr = NpyIter_GetInnerLoopSizePtr(iter); \ dataptrarray = NpyIter_GetDataPtrArray(iter); \ if(PyArray_EquivTypes(PyArray_DESCR((PyArrayObject*) b), quaternion_descr)) { \ npy_intp i; \ do { \ npy_intp size = *innersizeptr; \ src = dataptrarray[0]; \ dst = dataptrarray[1]; \ for(i = 0; i < size; i++, src += innerstride, dst += itemsize) { \ *((quaternion *) dst) = quaternion_##name(p, *((quaternion *) src)); \ } \ } while (iternext(iter)); \ } else if(PyArray_ISFLOAT((PyArrayObject*) b)) { \ npy_intp i; \ do { \ npy_intp size = *innersizeptr; \ src = dataptrarray[0]; \ dst = dataptrarray[1]; \ for(i = 0; i < size; i++, src += innerstride, dst += itemsize) { \ *(quaternion *) dst = quaternion_##name##_scalar(p, *((double *) src)); \ } \ } while (iternext(iter)); \ } else if(PyArray_ISINTEGER((PyArrayObject*) b)) { \ npy_intp i; \ do { \ npy_intp size = *innersizeptr; \ src = dataptrarray[0]; \ dst = dataptrarray[1]; \ for(i = 0; i < size; i++, src += innerstride, dst += itemsize) { \ *((quaternion *) dst) = quaternion_##name##_scalar(p, *((int *) src)); \ } \ } while (iternext(iter)); \ } else { \ NpyIter_Deallocate(iter); \ return NULL; \ } \ ret = (PyObject *) NpyIter_GetOperandArray(iter)[1]; \ Py_INCREF(ret); \ if (NpyIter_Deallocate(iter) != NPY_SUCCEED) { \ Py_DECREF(ret); \ return NULL; \ } \ return ret; \ } \ static PyObject* \ pyquaternion_##fake_name(PyObject* a, PyObject* b) { \ /* PyObject *a_type, *a_repr, *b_type, *b_repr, *a_repr2, *b_repr2; \ */ \ /* char* a_char, b_char, a_char2, b_char2; \ */ \ npy_int64 val64; \ npy_int32 val32; \ quaternion p = {0.0, 0.0, 0.0, 0.0}; \ if(PyArray_Check(b)) { return pyquaternion_##fake_name##_array_operator(a, b); } \ if(PyFloat_Check(a) && PyQuaternion_Check(b)) { \ return PyQuaternion_FromQuaternion(quaternion_scalar_##name(PyFloat_AsDouble(a), ((PyQuaternion*)b)->obval)); \ } \ if(PyInt_Check(a) && PyQuaternion_Check(b)) { \ return PyQuaternion_FromQuaternion(quaternion_scalar_##name(PyInt_AsLong(a), ((PyQuaternion*)b)->obval)); \ } \ PyQuaternion_AsQuaternion(p, a); \ if(PyQuaternion_Check(b)) { \ return PyQuaternion_FromQuaternion(quaternion_##name(p,((PyQuaternion*)b)->obval)); \ } else if(PyFloat_Check(b)) { \ return PyQuaternion_FromQuaternion(quaternion_##name##_scalar(p,PyFloat_AsDouble(b))); \ } else if(PyInt_Check(b)) { \ return PyQuaternion_FromQuaternion(quaternion_##name##_scalar(p,PyInt_AsLong(b))); \ } else if(PyObject_TypeCheck(b, &PyInt64ArrType_Type)) { \ PyArray_ScalarAsCtype(b, &val64); \ return PyQuaternion_FromQuaternion(quaternion_##name##_scalar(p, val64)); \ } else if(PyObject_TypeCheck(b, &PyInt32ArrType_Type)) { \ PyArray_ScalarAsCtype(b, &val32); \ return PyQuaternion_FromQuaternion(quaternion_##name##_scalar(p, val32)); \ } \ PyErr_SetString(PyExc_TypeError, "Binary operation involving quaternion and \\neither float nor quaternion."); \ return NULL; \ } #define QQ_QS_SQ_BINARY_QUATERNION_RETURNER(name) QQ_QS_SQ_BINARY_QUATERNION_RETURNER_FULL(name, name) QQ_QS_SQ_BINARY_QUATERNION_RETURNER(add) QQ_QS_SQ_BINARY_QUATERNION_RETURNER(subtract) QQ_QS_SQ_BINARY_QUATERNION_RETURNER(multiply) QQ_QS_SQ_BINARY_QUATERNION_RETURNER(divide) /* QQ_QS_SQ_BINARY_QUATERNION_RETURNER_FULL(true_divide, divide) */ /* QQ_QS_SQ_BINARY_QUATERNION_RETURNER_FULL(floor_divide, divide) */ QQ_QS_SQ_BINARY_QUATERNION_RETURNER(power) #define QQ_QS_SQ_BINARY_QUATERNION_INPLACE_FULL(fake_name, name) \ static PyObject* \ pyquaternion_inplace_##fake_name(PyObject* a, PyObject* b) { \ quaternion* p = {0}; \ /* fprintf (stderr, "file %s, line %d, pyquaternion_inplace_"#fake_name"(PyObject* a, PyObject* b).\n", __FILE__, __LINE__); \ */ \ if(PyFloat_Check(a) || PyInt_Check(a)) { \ PyErr_SetString(PyExc_TypeError, "Cannot in-place "#fake_name" a scalar by a quaternion; should be handled by python."); \ return NULL; \ } \ PyQuaternion_AsQuaternionPointer(p, a); \ if(PyQuaternion_Check(b)) { \ quaternion_inplace_##name(p,((PyQuaternion*)b)->obval); \ Py_INCREF(a); \ return a; \ } else if(PyFloat_Check(b)) { \ quaternion_inplace_##name##_scalar(p,PyFloat_AsDouble(b)); \ Py_INCREF(a); \ return a; \ } else if(PyInt_Check(b)) { \ quaternion_inplace_##name##_scalar(p,PyInt_AsLong(b)); \ Py_INCREF(a); \ return a; \ } \ PyErr_SetString(PyExc_TypeError, "Binary in-place operation involving quaternion and neither float nor quaternion."); \ return NULL; \ } #define QQ_QS_SQ_BINARY_QUATERNION_INPLACE(name) QQ_QS_SQ_BINARY_QUATERNION_INPLACE_FULL(name, name) QQ_QS_SQ_BINARY_QUATERNION_INPLACE(add) QQ_QS_SQ_BINARY_QUATERNION_INPLACE(subtract) QQ_QS_SQ_BINARY_QUATERNION_INPLACE(multiply) QQ_QS_SQ_BINARY_QUATERNION_INPLACE(divide) /* QQ_QS_SQ_BINARY_QUATERNION_INPLACE_FULL(true_divide, divide) */ /* QQ_QS_SQ_BINARY_QUATERNION_INPLACE_FULL(floor_divide, divide) */ QQ_QS_SQ_BINARY_QUATERNION_INPLACE(power) static PyObject * pyquaternion__reduce(PyQuaternion* self) { /* printf("\n\n\nI'm trying, most of all!\n\n\n"); */ return Py_BuildValue("O(OOOO)", Py_TYPE(self), PyFloat_FromDouble(self->obval.w), PyFloat_FromDouble(self->obval.x), PyFloat_FromDouble(self->obval.y), PyFloat_FromDouble(self->obval.z)); } static PyObject * pyquaternion_getstate(PyQuaternion* self, PyObject* args) { /* printf("\n\n\nI'm Trying, OKAY?\n\n\n"); */ if (!PyArg_ParseTuple(args, ":getstate")) return NULL; return Py_BuildValue("OOOO", PyFloat_FromDouble(self->obval.w), PyFloat_FromDouble(self->obval.x), PyFloat_FromDouble(self->obval.y), PyFloat_FromDouble(self->obval.z)); } static PyObject * pyquaternion_setstate(PyQuaternion* self, PyObject* args) { /* printf("\n\n\nI'm Trying, TOO!\n\n\n"); */ quaternion* q; q = &(self->obval); if (!PyArg_ParseTuple(args, "dddd:setstate", &q->w, &q->x, &q->y, &q->z)) { return NULL; } Py_INCREF(Py_None); return Py_None; } // This is an array of methods (member functions) that will be // available to use on the quaternion objects in python. This is // packaged up here, and will be used in the tp_methods field when // definining the PyQuaternion_Type below. PyMethodDef pyquaternion_methods[] = { // Unary bool returners {"nonzero", pyquaternion_nonzero, METH_NOARGS, "True if the quaternion has all zero components"}, {"isnan", pyquaternion_isnan, METH_NOARGS, "True if the quaternion has any NAN components"}, {"isinf", pyquaternion_isinf, METH_NOARGS, "True if the quaternion has any INF components"}, {"isfinite", pyquaternion_isfinite, METH_NOARGS, "True if the quaternion has all finite components"}, // Binary bool returners {"equal", pyquaternion_equal, METH_O, "True if the quaternions are PRECISELY equal"}, {"not_equal", pyquaternion_not_equal, METH_O, "True if the quaternions are not PRECISELY equal"}, {"less", pyquaternion_less, METH_O, "Strict dictionary ordering"}, {"greater", pyquaternion_greater, METH_O, "Strict dictionary ordering"}, {"less_equal", pyquaternion_less_equal, METH_O, "Dictionary ordering"}, {"greater_equal", pyquaternion_greater_equal, METH_O, "Dictionary ordering"}, // Unary float returners {"absolute", pyquaternion_absolute, METH_NOARGS, "Absolute value of quaternion"}, {"abs", pyquaternion_absolute, METH_NOARGS, "Absolute value (Euclidean norm) of quaternion"}, {"norm", pyquaternion_norm, METH_NOARGS, "Cayley norm (square of the absolute value) of quaternion"}, {"angle", pyquaternion_angle, METH_NOARGS, "Angle through which rotor rotates"}, // Unary quaternion returners // {"negative", pyquaternion_negative, METH_NOARGS, // "Return the negated quaternion"}, // {"positive", pyquaternion_positive, METH_NOARGS, // "Return the quaternion itself"}, {"conjugate", pyquaternion_conjugate, METH_NOARGS, "Return the complex conjugate of the quaternion"}, {"conj", pyquaternion_conjugate, METH_NOARGS, "Return the complex conjugate of the quaternion"}, {"inverse", pyquaternion_inverse, METH_NOARGS, "Return the inverse of the quaternion"}, {"sqrt", pyquaternion_sqrt, METH_NOARGS, "Return the square-root of the quaternion"}, {"log", pyquaternion_log, METH_NOARGS, "Return the logarithm (base e) of the quaternion"}, {"exp", pyquaternion_exp, METH_NOARGS, "Return the exponential of the quaternion (e**q)"}, {"normalized", pyquaternion_normalized, METH_NOARGS, "Return a normalized copy of the quaternion"}, {"x_parity_conjugate", pyquaternion_x_parity_conjugate, METH_NOARGS, "Reflect across y-z plane (note spinorial character)"}, {"x_parity_symmetric_part", pyquaternion_x_parity_symmetric_part, METH_NOARGS, "Part invariant under reflection across y-z plane (note spinorial character)"}, {"x_parity_antisymmetric_part", pyquaternion_x_parity_antisymmetric_part, METH_NOARGS, "Part anti-invariant under reflection across y-z plane (note spinorial character)"}, {"y_parity_conjugate", pyquaternion_y_parity_conjugate, METH_NOARGS, "Reflect across x-z plane (note spinorial character)"}, {"y_parity_symmetric_part", pyquaternion_y_parity_symmetric_part, METH_NOARGS, "Part invariant under reflection across x-z plane (note spinorial character)"}, {"y_parity_antisymmetric_part", pyquaternion_y_parity_antisymmetric_part, METH_NOARGS, "Part anti-invariant under reflection across x-z plane (note spinorial character)"}, {"z_parity_conjugate", pyquaternion_z_parity_conjugate, METH_NOARGS, "Reflect across x-y plane (note spinorial character)"}, {"z_parity_symmetric_part", pyquaternion_z_parity_symmetric_part, METH_NOARGS, "Part invariant under reflection across x-y plane (note spinorial character)"}, {"z_parity_antisymmetric_part", pyquaternion_z_parity_antisymmetric_part, METH_NOARGS, "Part anti-invariant under reflection across x-y plane (note spinorial character)"}, {"parity_conjugate", pyquaternion_parity_conjugate, METH_NOARGS, "Reflect all dimensions (note spinorial character)"}, {"parity_symmetric_part", pyquaternion_parity_symmetric_part, METH_NOARGS, "Part invariant under negation of all vectors (note spinorial character)"}, {"parity_antisymmetric_part", pyquaternion_parity_antisymmetric_part, METH_NOARGS, "Part anti-invariant under negation of all vectors (note spinorial character)"}, // Quaternion-quaternion binary quaternion returners // {"add", pyquaternion_add, METH_O, // "Componentwise addition"}, // {"subtract", pyquaternion_subtract, METH_O, // "Componentwise subtraction"}, {"copysign", pyquaternion_copysign, METH_O, "Componentwise copysign"}, // Quaternion-quaternion or quaternion-scalar binary quaternion returners // {"multiply", pyquaternion_multiply, METH_O, // "Standard (geometric) quaternion product"}, // {"divide", pyquaternion_divide, METH_O, // "Standard (geometric) quaternion division"}, // {"power", pyquaternion_power, METH_O, // "q.power(p) = (q.log() * p).exp()"}, {"__reduce__", (PyCFunction)pyquaternion__reduce, METH_NOARGS, "Return state information for pickling."}, {"__getstate__", (PyCFunction)pyquaternion_getstate, METH_VARARGS, "Return state information for pickling."}, {"__setstate__", (PyCFunction)pyquaternion_setstate, METH_VARARGS, "Reconstruct state information from pickle."}, {NULL, NULL, 0, NULL} }; static PyObject* pyquaternion_num_power(PyObject* a, PyObject* b, PyObject *c) { (void) c; return pyquaternion_power(a,b); } static PyObject* pyquaternion_num_inplace_power(PyObject* a, PyObject* b, PyObject *c) { (void) c; return pyquaternion_inplace_power(a,b); } static PyObject* pyquaternion_num_negative(PyObject* a) { return pyquaternion_negative(a,NULL); } static PyObject* pyquaternion_num_positive(PyObject* a) { return pyquaternion_positive(a,NULL); } static PyObject* pyquaternion_num_absolute(PyObject* a) { return pyquaternion_absolute(a,NULL); } static PyObject* pyquaternion_num_inverse(PyObject* a) { return pyquaternion_inverse(a,NULL); } static int pyquaternion_num_nonzero(PyObject* a) { quaternion q = ((PyQuaternion*)a)->obval; return quaternion_nonzero(q); } static PyNumberMethods pyquaternion_as_number = { pyquaternion_add, // nb_add pyquaternion_subtract, // nb_subtract pyquaternion_multiply, // nb_multiply #if PY_MAJOR_VERSION < 3 pyquaternion_divide, // nb_divide #endif 0, // nb_remainder 0, // nb_divmod pyquaternion_num_power, // nb_power pyquaternion_num_negative, // nb_negative pyquaternion_num_positive, // nb_positive pyquaternion_num_absolute, // nb_absolute pyquaternion_num_nonzero, // nb_nonzero pyquaternion_num_inverse, // nb_invert 0, // nb_lshift 0, // nb_rshift 0, // nb_and 0, // nb_xor 0, // nb_or #if PY_MAJOR_VERSION < 3 0, // nb_coerce #endif 0, // nb_int #if PY_MAJOR_VERSION >= 3 0, // nb_reserved #else 0, // nb_long #endif 0, // nb_float #if PY_MAJOR_VERSION < 3 0, // nb_oct 0, // nb_hex #endif pyquaternion_inplace_add, // nb_inplace_add pyquaternion_inplace_subtract, // nb_inplace_subtract pyquaternion_inplace_multiply, // nb_inplace_multiply #if PY_MAJOR_VERSION < 3 pyquaternion_inplace_divide, // nb_inplace_divide #endif 0, // nb_inplace_remainder pyquaternion_num_inplace_power, // nb_inplace_power 0, // nb_inplace_lshift 0, // nb_inplace_rshift 0, // nb_inplace_and 0, // nb_inplace_xor 0, // nb_inplace_or pyquaternion_divide, // nb_floor_divide pyquaternion_divide, // nb_true_divide pyquaternion_inplace_divide, // nb_inplace_floor_divide pyquaternion_inplace_divide, // nb_inplace_true_divide 0, // nb_index #if PY_MAJOR_VERSION >= 3 #if PY_MINOR_VERSION >= 5 0, // nb_matrix_multiply 0, // nb_inplace_matrix_multiply #endif #endif }; // This is an array of members (member data) that will be available to // use on the quaternion objects in python. This is packaged up here, // and will be used in the tp_members field when definining the // PyQuaternion_Type below. PyMemberDef pyquaternion_members[] = { {"real", T_DOUBLE, offsetof(PyQuaternion, obval.w), 0, "The real component of the quaternion"}, {"w", T_DOUBLE, offsetof(PyQuaternion, obval.w), 0, "The real component of the quaternion"}, {"x", T_DOUBLE, offsetof(PyQuaternion, obval.x), 0, "The first imaginary component of the quaternion"}, {"y", T_DOUBLE, offsetof(PyQuaternion, obval.y), 0, "The second imaginary component of the quaternion"}, {"z", T_DOUBLE, offsetof(PyQuaternion, obval.z), 0, "The third imaginary component of the quaternion"}, {NULL, 0, 0, 0, NULL} }; // The quaternion can be conveniently separated into two complex // numbers, which we call 'part a' and 'part b'. These are useful in // writing Wigner's D matrices directly in terms of quaternions. This // is essentially the column-vector presentation of spinors. static PyObject * pyquaternion_get_part_a(PyObject *self, void *NPY_UNUSED(closure)) { return (PyObject*) PyComplex_FromDoubles(((PyQuaternion *)self)->obval.w, ((PyQuaternion *)self)->obval.z); } static PyObject * pyquaternion_get_part_b(PyObject *self, void *NPY_UNUSED(closure)) { return (PyObject*) PyComplex_FromDoubles(((PyQuaternion *)self)->obval.y, ((PyQuaternion *)self)->obval.x); } // This will be defined as a member function on the quaternion // objects, so that calling "vec" will return a numpy array // with the last three components of the quaternion. static PyObject * pyquaternion_get_vec(PyObject *self, void *NPY_UNUSED(closure)) { quaternion *q = &((PyQuaternion *)self)->obval; int nd = 1; npy_intp dims[1] = { 3 }; int typenum = NPY_DOUBLE; PyObject* components = PyArray_SimpleNewFromData(nd, dims, typenum, &(q->x)); Py_INCREF(self); PyArray_SetBaseObject((PyArrayObject*)components, self); return components; } // This will be defined as a member function on the quaternion // objects, so that calling q.vec = [1,2,3], for example, // will set the vector components appropriately. static int pyquaternion_set_vec(PyObject *self, PyObject *value, void *NPY_UNUSED(closure)) { PyObject *element; quaternion *q = &((PyQuaternion *)self)->obval; if (value == NULL) { PyErr_SetString(PyExc_TypeError, "Cannot set quaternion to empty value"); return -1; } if (! (PySequence_Check(value) && PySequence_Size(value)==3) ) { PyErr_SetString(PyExc_TypeError, "A quaternion's vector components must be set to something of length 3"); return -1; } /* PySequence_GetItem INCREFs element. */ element = PySequence_GetItem(value, 0); if(element == NULL) { return -1; } /* Not a sequence, or other failure */ q->x = PyFloat_AsDouble(element); Py_DECREF(element); element = PySequence_GetItem(value, 1); if(element == NULL) { return -1; } /* Not a sequence, or other failure */ q->y = PyFloat_AsDouble(element); Py_DECREF(element); element = PySequence_GetItem(value, 2); if(element == NULL) { return -1; } /* Not a sequence, or other failure */ q->z = PyFloat_AsDouble(element); Py_DECREF(element); return 0; } // This will be defined as a member function on the quaternion // objects, so that calling "components" will return a numpy array // with the components of the quaternion. static PyObject * pyquaternion_get_components(PyObject *self, void *NPY_UNUSED(closure)) { quaternion *q = &((PyQuaternion *)self)->obval; int nd = 1; npy_intp dims[1] = { 4 }; int typenum = NPY_DOUBLE; PyObject* components = PyArray_SimpleNewFromData(nd, dims, typenum, &(q->w)); Py_INCREF(self); PyArray_SetBaseObject((PyArrayObject*)components, self); return components; } // This will be defined as a member function on the quaternion // objects, so that calling q.components = [1,2,3,4], for example, // will set the components appropriately. static int pyquaternion_set_components(PyObject *self, PyObject *value, void *NPY_UNUSED(closure)){ PyObject *element; quaternion *q = &((PyQuaternion *)self)->obval; if (value == NULL) { PyErr_SetString(PyExc_ValueError, "Cannot set quaternion to empty value"); return -1; } if (! (PySequence_Check(value) && PySequence_Size(value)==4) ) { PyErr_SetString(PyExc_TypeError, "A quaternion's components must be set to something of length 4"); return -1; } element = PySequence_GetItem(value, 0); if(element == NULL) { return -1; } /* Not a sequence, or other failure */ q->w = PyFloat_AsDouble(element); Py_DECREF(element); element = PySequence_GetItem(value, 1); if(element == NULL) { return -1; } /* Not a sequence, or other failure */ q->x = PyFloat_AsDouble(element); Py_DECREF(element); element = PySequence_GetItem(value, 2); if(element == NULL) { return -1; } /* Not a sequence, or other failure */ q->y = PyFloat_AsDouble(element); Py_DECREF(element); element = PySequence_GetItem(value, 3); if(element == NULL) { return -1; } /* Not a sequence, or other failure */ q->z = PyFloat_AsDouble(element); Py_DECREF(element); return 0; } // This collects the methods for getting and setting elements of the // quaternion. This is packaged up here, and will be used in the // tp_getset field when definining the PyQuaternion_Type // below. PyGetSetDef pyquaternion_getset[] = { {"a", pyquaternion_get_part_a, NULL, "The complex number (w+i*z)", NULL}, {"b", pyquaternion_get_part_b, NULL, "The complex number (y+i*x)", NULL}, {"imag", pyquaternion_get_vec, pyquaternion_set_vec, "The vector part (x,y,z) of the quaternion as a numpy array", NULL}, {"vec", pyquaternion_get_vec, pyquaternion_set_vec, "The vector part (x,y,z) of the quaternion as a numpy array", NULL}, {"components", pyquaternion_get_components, pyquaternion_set_components, "The components (w,x,y,z) of the quaternion as a numpy array", NULL}, {NULL, NULL, NULL, NULL, NULL} }; static PyObject* pyquaternion_richcompare(PyObject* a, PyObject* b, int op) { quaternion x = {0.0, 0.0, 0.0, 0.0}; quaternion y = {0.0, 0.0, 0.0, 0.0}; int result = 0; PyQuaternion_AsQuaternion(x,a); PyQuaternion_AsQuaternion(y,b); #define COMPARISONOP(py,op) case py: result = quaternion_##op(x,y); break; switch (op) { COMPARISONOP(Py_LT,less) COMPARISONOP(Py_LE,less_equal) COMPARISONOP(Py_EQ,equal) COMPARISONOP(Py_NE,not_equal) COMPARISONOP(Py_GT,greater) COMPARISONOP(Py_GE,greater_equal) }; #undef COMPARISONOP return PyBool_FromLong(result); } static long pyquaternion_hash(PyObject *o) { quaternion q = ((PyQuaternion *)o)->obval; long value = 0x456789; value = (10000004 * value) ^ _Py_HashDouble(q.w); value = (10000004 * value) ^ _Py_HashDouble(q.x); value = (10000004 * value) ^ _Py_HashDouble(q.y); value = (10000004 * value) ^ _Py_HashDouble(q.z); if (value == -1) value = -2; return value; } static PyObject * pyquaternion_repr(PyObject *o) { char str[128]; quaternion q = ((PyQuaternion *)o)->obval; sprintf(str, "quaternion(%.15g, %.15g, %.15g, %.15g)", q.w, q.x, q.y, q.z); return PyUString_FromString(str); } static PyObject * pyquaternion_str(PyObject *o) { char str[128]; quaternion q = ((PyQuaternion *)o)->obval; sprintf(str, "quaternion(%.15g, %.15g, %.15g, %.15g)", q.w, q.x, q.y, q.z); return PyUString_FromString(str); } // This establishes the quaternion as a python object (not yet a numpy // scalar type). The name may be a little counterintuitive; the idea // is that this will be a type that can be used as an array dtype. // Note that many of the slots below will be filled later, after the // corresponding functions are defined. static PyTypeObject PyQuaternion_Type = { #if PY_MAJOR_VERSION >= 3 PyVarObject_HEAD_INIT(NULL, 0) #else PyObject_HEAD_INIT(NULL) 0, // ob_size #endif "quaternion", // tp_name sizeof(PyQuaternion), // tp_basicsize 0, // tp_itemsize 0, // tp_dealloc 0, // tp_print 0, // tp_getattr 0, // tp_setattr #if PY_MAJOR_VERSION >= 3 0, // tp_reserved #else 0, // tp_compare #endif pyquaternion_repr, // tp_repr &pyquaternion_as_number, // tp_as_number 0, // tp_as_sequence 0, // tp_as_mapping pyquaternion_hash, // tp_hash 0, // tp_call pyquaternion_str, // tp_str 0, // tp_getattro 0, // tp_setattro 0, // tp_as_buffer #if PY_MAJOR_VERSION >= 3 Py_TPFLAGS_DEFAULT | Py_TPFLAGS_BASETYPE, // tp_flags #else Py_TPFLAGS_DEFAULT | Py_TPFLAGS_BASETYPE | Py_TPFLAGS_CHECKTYPES, // tp_flags #endif 0, // tp_doc 0, // tp_traverse 0, // tp_clear pyquaternion_richcompare, // tp_richcompare 0, // tp_weaklistoffset 0, // tp_iter 0, // tp_iternext pyquaternion_methods, // tp_methods pyquaternion_members, // tp_members pyquaternion_getset, // tp_getset 0, // tp_base; will be reset to &PyGenericArrType_Type after numpy import 0, // tp_dict 0, // tp_descr_get 0, // tp_descr_set 0, // tp_dictoffset pyquaternion_init, // tp_init 0, // tp_alloc pyquaternion_new, // tp_new 0, // tp_free 0, // tp_is_gc 0, // tp_bases 0, // tp_mro 0, // tp_cache 0, // tp_subclasses 0, // tp_weaklist 0, // tp_del #if PY_VERSION_HEX >= 0x02060000 0, // tp_version_tag #endif #if PY_VERSION_HEX >= 0x030400a1 0, // tp_finalize #endif }; // Functions implementing internal features. Not all of these function // pointers must be defined for a given type. The required members are // nonzero, copyswap, copyswapn, setitem, getitem, and cast. static PyArray_ArrFuncs _PyQuaternion_ArrFuncs; static npy_bool QUATERNION_nonzero (char *ip, PyArrayObject *ap) { quaternion q; quaternion zero = {0,0,0,0}; if (ap == NULL || PyArray_ISBEHAVED_RO(ap)) { q = *(quaternion *)ip; } else { PyArray_Descr *descr; descr = PyArray_DescrFromType(NPY_DOUBLE); descr->f->copyswap(&q.w, ip, !PyArray_ISNOTSWAPPED(ap), NULL); descr->f->copyswap(&q.x, ip+8, !PyArray_ISNOTSWAPPED(ap), NULL); descr->f->copyswap(&q.y, ip+16, !PyArray_ISNOTSWAPPED(ap), NULL); descr->f->copyswap(&q.z, ip+24, !PyArray_ISNOTSWAPPED(ap), NULL); Py_DECREF(descr); } return (npy_bool) !quaternion_equal(q, zero); } static void QUATERNION_copyswap(quaternion *dst, quaternion *src, int swap, void *NPY_UNUSED(arr)) { PyArray_Descr *descr; descr = PyArray_DescrFromType(NPY_DOUBLE); descr->f->copyswapn(dst, sizeof(double), src, sizeof(double), 4, swap, NULL); Py_DECREF(descr); } static void QUATERNION_copyswapn(quaternion *dst, npy_intp dstride, quaternion *src, npy_intp sstride, npy_intp n, int swap, void *NPY_UNUSED(arr)) { PyArray_Descr *descr; descr = PyArray_DescrFromType(NPY_DOUBLE); descr->f->copyswapn(&dst->w, dstride, &src->w, sstride, n, swap, NULL); descr->f->copyswapn(&dst->x, dstride, &src->x, sstride, n, swap, NULL); descr->f->copyswapn(&dst->y, dstride, &src->y, sstride, n, swap, NULL); descr->f->copyswapn(&dst->z, dstride, &src->z, sstride, n, swap, NULL); Py_DECREF(descr); } static int QUATERNION_setitem(PyObject* item, quaternion* qp, void* NPY_UNUSED(ap)) { PyObject *element; if(PyQuaternion_Check(item)) { memcpy(qp,&(((PyQuaternion *)item)->obval),sizeof(quaternion)); } else if(PySequence_Check(item) && PySequence_Length(item)==4) { element = PySequence_GetItem(item, 0); if(element == NULL) { return -1; } /* Not a sequence, or other failure */ qp->w = PyFloat_AsDouble(element); Py_DECREF(element); element = PySequence_GetItem(item, 1); if(element == NULL) { return -1; } /* Not a sequence, or other failure */ qp->x = PyFloat_AsDouble(element); Py_DECREF(element); element = PySequence_GetItem(item, 2); if(element == NULL) { return -1; } /* Not a sequence, or other failure */ qp->y = PyFloat_AsDouble(element); Py_DECREF(element); element = PySequence_GetItem(item, 3); if(element == NULL) { return -1; } /* Not a sequence, or other failure */ qp->z = PyFloat_AsDouble(element); Py_DECREF(element); } else { PyErr_SetString(PyExc_TypeError, "Unknown input to QUATERNION_setitem"); return -1; } return 0; } // When a numpy array of dtype=quaternion is indexed, this function is // called, returning a new quaternion object with a copy of the // data... sometimes... static PyObject * QUATERNION_getitem(void* data, void* NPY_UNUSED(arr)) { quaternion q; memcpy(&q,data,sizeof(quaternion)); return PyQuaternion_FromQuaternion(q); } static int QUATERNION_compare(quaternion *pa, quaternion *pb, PyArrayObject *NPY_UNUSED(ap)) { quaternion a = *pa, b = *pb; npy_bool anan, bnan; int ret; anan = quaternion_isnan(a); bnan = quaternion_isnan(b); if (anan) { ret = bnan ? 0 : -1; } else if (bnan) { ret = 1; } else if(quaternion_less(a, b)) { ret = -1; } else if(quaternion_less(b, a)) { ret = 1; } else { ret = 0; } return ret; } static int QUATERNION_argmax(quaternion *ip, npy_intp n, npy_intp *max_ind, PyArrayObject *NPY_UNUSED(aip)) { npy_intp i; quaternion mp = *ip; *max_ind = 0; if (quaternion_isnan(mp)) { // nan encountered; it's maximal return 0; } for (i = 1; i < n; i++) { ip++; //Propagate nans, similarly as max() and min() if (!(quaternion_less_equal(*ip, mp))) { // negated, for correct nan handling mp = *ip; *max_ind = i; if (quaternion_isnan(mp)) { // nan encountered, it's maximal break; } } } return 0; } static void QUATERNION_fillwithscalar(quaternion *buffer, npy_intp length, quaternion *value, void *NPY_UNUSED(ignored)) { npy_intp i; quaternion val = *value; for (i = 0; i < length; ++i) { buffer[i] = val; } } // This is a macro (followed by applications of the macro) that cast // the input types to standard quaternions with only a nonzero scalar // part. #define MAKE_T_TO_QUATERNION(TYPE, type) \ static void \ TYPE ## _to_quaternion(type *ip, quaternion *op, npy_intp n, \ PyArrayObject *NPY_UNUSED(aip), PyArrayObject *NPY_UNUSED(aop)) \ { \ while (n--) { \ op->w = (double)(*ip++); \ op->x = 0; \ op->y = 0; \ op->z = 0; \ op++; \ } \ } MAKE_T_TO_QUATERNION(FLOAT, npy_float); MAKE_T_TO_QUATERNION(DOUBLE, npy_double); MAKE_T_TO_QUATERNION(LONGDOUBLE, npy_longdouble); MAKE_T_TO_QUATERNION(BOOL, npy_bool); MAKE_T_TO_QUATERNION(BYTE, npy_byte); MAKE_T_TO_QUATERNION(UBYTE, npy_ubyte); MAKE_T_TO_QUATERNION(SHORT, npy_short); MAKE_T_TO_QUATERNION(USHORT, npy_ushort); MAKE_T_TO_QUATERNION(INT, npy_int); MAKE_T_TO_QUATERNION(UINT, npy_uint); MAKE_T_TO_QUATERNION(LONG, npy_long); MAKE_T_TO_QUATERNION(ULONG, npy_ulong); MAKE_T_TO_QUATERNION(LONGLONG, npy_longlong); MAKE_T_TO_QUATERNION(ULONGLONG, npy_ulonglong); // This is a macro (followed by applications of the macro) that cast // the input complex types to standard quaternions with only the first // two components nonzero. This doesn't make a whole lot of sense to // me, and may be removed in the future. #define MAKE_CT_TO_QUATERNION(TYPE, type) \ static void \ TYPE ## _to_quaternion(type *ip, quaternion *op, npy_intp n, \ PyArrayObject *NPY_UNUSED(aip), PyArrayObject *NPY_UNUSED(aop)) \ { \ while (n--) { \ op->w = (double)(*ip++); \ op->x = (double)(*ip++); \ op->y = 0; \ op->z = 0; \ } \ } MAKE_CT_TO_QUATERNION(CFLOAT, npy_float); MAKE_CT_TO_QUATERNION(CDOUBLE, npy_double); MAKE_CT_TO_QUATERNION(CLONGDOUBLE, npy_longdouble); static void register_cast_function(int sourceType, int destType, PyArray_VectorUnaryFunc *castfunc) { PyArray_Descr *descr = PyArray_DescrFromType(sourceType); PyArray_RegisterCastFunc(descr, destType, castfunc); PyArray_RegisterCanCast(descr, destType, NPY_NOSCALAR); Py_DECREF(descr); } // This is a macro that will be used to define the various basic unary // quaternion functions, so that they can be applied quickly to a // numpy array of quaternions. #define UNARY_GEN_UFUNC(ufunc_name, func_name, ret_type) \ static void \ quaternion_##ufunc_name##_ufunc(char** args, npy_intp* dimensions, \ npy_intp* steps, void* NPY_UNUSED(data)) { \ /* fprintf (stderr, "file %s, line %d, quaternion_%s_ufunc.\n", __FILE__, __LINE__, #ufunc_name); */ \ char *ip1 = args[0], *op1 = args[1]; \ npy_intp is1 = steps[0], os1 = steps[1]; \ npy_intp n = dimensions[0]; \ npy_intp i; \ for(i = 0; i < n; i++, ip1 += is1, op1 += os1){ \ const quaternion in1 = *(quaternion *)ip1; \ *((ret_type *)op1) = quaternion_##func_name(in1);};} #define UNARY_UFUNC(name, ret_type) \ UNARY_GEN_UFUNC(name, name, ret_type) // And these all do the work mentioned above, using the macro UNARY_UFUNC(isnan, npy_bool) UNARY_UFUNC(isinf, npy_bool) UNARY_UFUNC(isfinite, npy_bool) UNARY_UFUNC(norm, npy_double) UNARY_UFUNC(absolute, npy_double) UNARY_UFUNC(angle, npy_double) UNARY_UFUNC(sqrt, quaternion) UNARY_UFUNC(log, quaternion) UNARY_UFUNC(exp, quaternion) UNARY_UFUNC(negative, quaternion) UNARY_UFUNC(conjugate, quaternion) UNARY_GEN_UFUNC(invert, inverse, quaternion) UNARY_UFUNC(normalized, quaternion) UNARY_UFUNC(x_parity_conjugate, quaternion) UNARY_UFUNC(x_parity_symmetric_part, quaternion) UNARY_UFUNC(x_parity_antisymmetric_part, quaternion) UNARY_UFUNC(y_parity_conjugate, quaternion) UNARY_UFUNC(y_parity_symmetric_part, quaternion) UNARY_UFUNC(y_parity_antisymmetric_part, quaternion) UNARY_UFUNC(z_parity_conjugate, quaternion) UNARY_UFUNC(z_parity_symmetric_part, quaternion) UNARY_UFUNC(z_parity_antisymmetric_part, quaternion) UNARY_UFUNC(parity_conjugate, quaternion) UNARY_UFUNC(parity_symmetric_part, quaternion) UNARY_UFUNC(parity_antisymmetric_part, quaternion) // This is a macro that will be used to define the various basic binary // quaternion functions, so that they can be applied quickly to a // numpy array of quaternions. #define BINARY_GEN_UFUNC(ufunc_name, func_name, arg_type1, arg_type2, ret_type) \ static void \ quaternion_##ufunc_name##_ufunc(char** args, npy_intp* dimensions, \ npy_intp* steps, void* NPY_UNUSED(data)) { \ /* fprintf (stderr, "file %s, line %d, quaternion_%s_ufunc.\n", __FILE__, __LINE__, #ufunc_name); */ \ char *ip1 = args[0], *ip2 = args[1], *op1 = args[2]; \ npy_intp is1 = steps[0], is2 = steps[1], os1 = steps[2]; \ npy_intp n = dimensions[0]; \ npy_intp i; \ for(i = 0; i < n; i++, ip1 += is1, ip2 += is2, op1 += os1) { \ const arg_type1 in1 = *(arg_type1 *)ip1; \ const arg_type2 in2 = *(arg_type2 *)ip2; \ *((ret_type *)op1) = quaternion_##func_name(in1, in2); \ }; \ }; // A couple special-case versions of the above #define BINARY_UFUNC(name, ret_type) \ BINARY_GEN_UFUNC(name, name, quaternion, quaternion, ret_type) #define BINARY_SCALAR_UFUNC(name, ret_type) \ BINARY_GEN_UFUNC(name##_scalar, name##_scalar, quaternion, npy_double, ret_type) \ BINARY_GEN_UFUNC(scalar_##name, scalar_##name, npy_double, quaternion, ret_type) // And these all do the work mentioned above, using the macros BINARY_UFUNC(add, quaternion) BINARY_UFUNC(subtract, quaternion) BINARY_UFUNC(multiply, quaternion) BINARY_UFUNC(divide, quaternion) BINARY_GEN_UFUNC(true_divide, divide, quaternion, quaternion, quaternion) BINARY_GEN_UFUNC(floor_divide, divide, quaternion, quaternion, quaternion) BINARY_UFUNC(power, quaternion) BINARY_UFUNC(copysign, quaternion) BINARY_UFUNC(equal, npy_bool) BINARY_UFUNC(not_equal, npy_bool) BINARY_UFUNC(less, npy_bool) BINARY_UFUNC(less_equal, npy_bool) BINARY_SCALAR_UFUNC(add, quaternion) BINARY_SCALAR_UFUNC(subtract, quaternion) BINARY_SCALAR_UFUNC(multiply, quaternion) BINARY_SCALAR_UFUNC(divide, quaternion) BINARY_GEN_UFUNC(true_divide_scalar, divide_scalar, quaternion, npy_double, quaternion) BINARY_GEN_UFUNC(floor_divide_scalar, divide_scalar, quaternion, npy_double, quaternion) BINARY_GEN_UFUNC(scalar_true_divide, scalar_divide, npy_double, quaternion, quaternion) BINARY_GEN_UFUNC(scalar_floor_divide, scalar_divide, npy_double, quaternion, quaternion) BINARY_SCALAR_UFUNC(power, quaternion) BINARY_UFUNC(rotor_intrinsic_distance, npy_double) BINARY_UFUNC(rotor_chordal_distance, npy_double) BINARY_UFUNC(rotation_intrinsic_distance, npy_double) BINARY_UFUNC(rotation_chordal_distance, npy_double) // Interface to the module-level slerp function static PyObject* pyquaternion_slerp_evaluate(PyObject *NPY_UNUSED(self), PyObject *args) { double tau; PyObject* Q1 = {0}; PyObject* Q2 = {0}; PyQuaternion* Q = (PyQuaternion*)PyQuaternion_Type.tp_alloc(&PyQuaternion_Type,0); if (!PyArg_ParseTuple(args, "OOd", &Q1, &Q2, &tau)) { return NULL; } Q->obval = slerp(((PyQuaternion*)Q1)->obval, ((PyQuaternion*)Q2)->obval, tau); return (PyObject*)Q; } // Interface to the evaluate a squad interpolant at a particular time static PyObject* pyquaternion_squad_evaluate(PyObject *NPY_UNUSED(self), PyObject *args) { double tau_i; PyObject* q_i = {0}; PyObject* a_i = {0}; PyObject* b_ip1 = {0}; PyObject* q_ip1 = {0}; PyQuaternion* Q = (PyQuaternion*)PyQuaternion_Type.tp_alloc(&PyQuaternion_Type,0); if (!PyArg_ParseTuple(args, "dOOOO", &tau_i, &q_i, &a_i, &b_ip1, &q_ip1)) { return NULL; } Q->obval = squad_evaluate(tau_i, ((PyQuaternion*)q_i)->obval, ((PyQuaternion*)a_i)->obval, ((PyQuaternion*)b_ip1)->obval, ((PyQuaternion*)q_ip1)->obval); return (PyObject*)Q; } // This will be used to create the ufunc needed for slerp, which // evaluates the interpolant at a point. The method for doing this // was pieced together from examples given on the page // static void slerp_loop(char **args, npy_intp *dimensions, npy_intp* steps, void* NPY_UNUSED(data)) { npy_intp i; double tau_i; quaternion *q_1, *q_2; npy_intp is1=steps[0]; npy_intp is2=steps[1]; npy_intp is3=steps[2]; npy_intp os=steps[3]; npy_intp n=dimensions[0]; char *i1=args[0]; char *i2=args[1]; char *i3=args[2]; char *op=args[3]; for (i = 0; i < n; i++) { q_1 = (quaternion*)i1; q_2 = (quaternion*)i2; tau_i = *(double *)i3; *((quaternion *)op) = slerp(*q_1, *q_2, tau_i); i1 += is1; i2 += is2; i3 += is3; op += os; } } // This will be used to create the ufunc needed for squad, which // evaluates the interpolant at a point. The method for doing this // was pieced together from examples given on the page // static void squad_loop(char **args, npy_intp *dimensions, npy_intp* steps, void* NPY_UNUSED(data)) { npy_intp i; double tau_i; quaternion *q_i, *a_i, *b_ip1, *q_ip1; npy_intp is1=steps[0]; npy_intp is2=steps[1]; npy_intp is3=steps[2]; npy_intp is4=steps[3]; npy_intp is5=steps[4]; npy_intp os=steps[5]; npy_intp n=dimensions[0]; char *i1=args[0]; char *i2=args[1]; char *i3=args[2]; char *i4=args[3]; char *i5=args[4]; char *op=args[5]; for (i = 0; i < n; i++) { tau_i = *(double *)i1; q_i = (quaternion*)i2; a_i = (quaternion*)i3; b_ip1 = (quaternion*)i4; q_ip1 = (quaternion*)i5; *((quaternion *)op) = squad_evaluate(tau_i, *q_i, *a_i, *b_ip1, *q_ip1); i1 += is1; i2 += is2; i3 += is3; i4 += is4; i5 += is5; op += os; } } // This contains assorted other top-level methods for the module static PyMethodDef QuaternionMethods[] = { {"slerp_evaluate", pyquaternion_slerp_evaluate, METH_VARARGS, "Interpolate linearly along the geodesic between two rotors \n\n" "See also numpy.slerp_vectorized for a vectorized version of this function, and\n" "quaternion.slerp for the most useful form, which automatically finds the correct\n" "rotors to interpolate and the relative time to which they must be interpolated."}, {"squad_evaluate", pyquaternion_squad_evaluate, METH_VARARGS, "Interpolate linearly along the geodesic between two rotors\n\n" "See also numpy.squad_vectorized for a vectorized version of this function, and\n" "quaternion.squad for the most useful form, which automatically finds the correct\n" "rotors to interpolate and the relative time to which they must be interpolated."}, {NULL, NULL, 0, NULL} }; #if PY_MAJOR_VERSION >= 3 static struct PyModuleDef moduledef = { PyModuleDef_HEAD_INIT, "numpy_quaternion", NULL, -1, QuaternionMethods, NULL, NULL, NULL, NULL }; #define INITERROR return NULL // This is the initialization function that does the setup PyMODINIT_FUNC PyInit_numpy_quaternion(void) { #else #define INITERROR return // This is the initialization function that does the setup PyMODINIT_FUNC initnumpy_quaternion(void) { #endif PyObject *module; PyObject *tmp_ufunc; PyObject *slerp_evaluate_ufunc; PyObject *squad_evaluate_ufunc; int quaternionNum; int arg_types[3]; PyArray_Descr* arg_dtypes[6]; PyObject* numpy; PyObject* numpy_dict; // Initialize a (for now, empty) module #if PY_MAJOR_VERSION >= 3 module = PyModule_Create(&moduledef); #else module = Py_InitModule("numpy_quaternion", QuaternionMethods); #endif if(module==NULL) { INITERROR; } // Initialize numpy import_array(); if (PyErr_Occurred()) { INITERROR; } import_umath(); if (PyErr_Occurred()) { INITERROR; } numpy = PyImport_ImportModule("numpy"); if (!numpy) { INITERROR; } numpy_dict = PyModule_GetDict(numpy); if (!numpy_dict) { INITERROR; } // Register the quaternion array base type. Couldn't do this until // after we imported numpy (above) PyQuaternion_Type.tp_base = &PyGenericArrType_Type; if (PyType_Ready(&PyQuaternion_Type) < 0) { PyErr_Print(); PyErr_SetString(PyExc_SystemError, "Could not initialize PyQuaternion_Type."); INITERROR; } // The array functions, to be used below. This InitArrFuncs // function is a convenient way to set all the fields to zero // initially, so we don't get undefined behavior. PyArray_InitArrFuncs(&_PyQuaternion_ArrFuncs); _PyQuaternion_ArrFuncs.nonzero = (PyArray_NonzeroFunc*)QUATERNION_nonzero; _PyQuaternion_ArrFuncs.copyswap = (PyArray_CopySwapFunc*)QUATERNION_copyswap; _PyQuaternion_ArrFuncs.copyswapn = (PyArray_CopySwapNFunc*)QUATERNION_copyswapn; _PyQuaternion_ArrFuncs.setitem = (PyArray_SetItemFunc*)QUATERNION_setitem; _PyQuaternion_ArrFuncs.getitem = (PyArray_GetItemFunc*)QUATERNION_getitem; _PyQuaternion_ArrFuncs.compare = (PyArray_CompareFunc*)QUATERNION_compare; _PyQuaternion_ArrFuncs.argmax = (PyArray_ArgFunc*)QUATERNION_argmax; _PyQuaternion_ArrFuncs.fillwithscalar = (PyArray_FillWithScalarFunc*)QUATERNION_fillwithscalar; // The quaternion array descr quaternion_descr = PyObject_New(PyArray_Descr, &PyArrayDescr_Type); quaternion_descr->typeobj = &PyQuaternion_Type; quaternion_descr->kind = 'V'; quaternion_descr->type = 'q'; quaternion_descr->byteorder = '='; quaternion_descr->flags = 0; quaternion_descr->type_num = 0; // assigned at registration quaternion_descr->elsize = 8*4; quaternion_descr->alignment = 8; quaternion_descr->subarray = NULL; quaternion_descr->fields = NULL; quaternion_descr->names = NULL; quaternion_descr->f = &_PyQuaternion_ArrFuncs; quaternion_descr->metadata = NULL; quaternion_descr->c_metadata = NULL; Py_INCREF(&PyQuaternion_Type); quaternionNum = PyArray_RegisterDataType(quaternion_descr); if (quaternionNum < 0) { INITERROR; } register_cast_function(NPY_BOOL, quaternionNum, (PyArray_VectorUnaryFunc*)BOOL_to_quaternion); register_cast_function(NPY_BYTE, quaternionNum, (PyArray_VectorUnaryFunc*)BYTE_to_quaternion); register_cast_function(NPY_UBYTE, quaternionNum, (PyArray_VectorUnaryFunc*)UBYTE_to_quaternion); register_cast_function(NPY_SHORT, quaternionNum, (PyArray_VectorUnaryFunc*)SHORT_to_quaternion); register_cast_function(NPY_USHORT, quaternionNum, (PyArray_VectorUnaryFunc*)USHORT_to_quaternion); register_cast_function(NPY_INT, quaternionNum, (PyArray_VectorUnaryFunc*)INT_to_quaternion); register_cast_function(NPY_UINT, quaternionNum, (PyArray_VectorUnaryFunc*)UINT_to_quaternion); register_cast_function(NPY_LONG, quaternionNum, (PyArray_VectorUnaryFunc*)LONG_to_quaternion); register_cast_function(NPY_ULONG, quaternionNum, (PyArray_VectorUnaryFunc*)ULONG_to_quaternion); register_cast_function(NPY_LONGLONG, quaternionNum, (PyArray_VectorUnaryFunc*)LONGLONG_to_quaternion); register_cast_function(NPY_ULONGLONG, quaternionNum, (PyArray_VectorUnaryFunc*)ULONGLONG_to_quaternion); register_cast_function(NPY_FLOAT, quaternionNum, (PyArray_VectorUnaryFunc*)FLOAT_to_quaternion); register_cast_function(NPY_DOUBLE, quaternionNum, (PyArray_VectorUnaryFunc*)DOUBLE_to_quaternion); register_cast_function(NPY_LONGDOUBLE, quaternionNum, (PyArray_VectorUnaryFunc*)LONGDOUBLE_to_quaternion); register_cast_function(NPY_CFLOAT, quaternionNum, (PyArray_VectorUnaryFunc*)CFLOAT_to_quaternion); register_cast_function(NPY_CDOUBLE, quaternionNum, (PyArray_VectorUnaryFunc*)CDOUBLE_to_quaternion); register_cast_function(NPY_CLONGDOUBLE, quaternionNum, (PyArray_VectorUnaryFunc*)CLONGDOUBLE_to_quaternion); // These macros will be used below #define REGISTER_UFUNC(name) \ PyUFunc_RegisterLoopForType((PyUFuncObject *)PyDict_GetItemString(numpy_dict, #name), \ quaternion_descr->type_num, quaternion_##name##_ufunc, arg_types, NULL) #define REGISTER_SCALAR_UFUNC(name) \ PyUFunc_RegisterLoopForType((PyUFuncObject *)PyDict_GetItemString(numpy_dict, #name), \ quaternion_descr->type_num, quaternion_scalar_##name##_ufunc, arg_types, NULL) #define REGISTER_UFUNC_SCALAR(name) \ PyUFunc_RegisterLoopForType((PyUFuncObject *)PyDict_GetItemString(numpy_dict, #name), \ quaternion_descr->type_num, quaternion_##name##_scalar_ufunc, arg_types, NULL) #define REGISTER_NEW_UFUNC_GENERAL(pyname, cname, nargin, nargout, doc) \ tmp_ufunc = PyUFunc_FromFuncAndData(NULL, NULL, NULL, 0, nargin, nargout, \ PyUFunc_None, #pyname, doc, 0); \ PyUFunc_RegisterLoopForType((PyUFuncObject *)tmp_ufunc, \ quaternion_descr->type_num, quaternion_##cname##_ufunc, arg_types, NULL); \ PyDict_SetItemString(numpy_dict, #pyname, tmp_ufunc); \ Py_DECREF(tmp_ufunc) #define REGISTER_NEW_UFUNC(name, nargin, nargout, doc) \ REGISTER_NEW_UFUNC_GENERAL(name, name, nargin, nargout, doc) // quat -> bool arg_types[0] = quaternion_descr->type_num; arg_types[1] = NPY_BOOL; REGISTER_UFUNC(isnan); /* // Already works: REGISTER_UFUNC(nonzero); */ REGISTER_UFUNC(isinf); REGISTER_UFUNC(isfinite); // quat -> double arg_types[0] = quaternion_descr->type_num; arg_types[1] = NPY_DOUBLE; REGISTER_NEW_UFUNC(norm, 1, 1, "Return Cayley norm (square of the absolute value) of each quaternion.\n"); REGISTER_UFUNC(absolute); REGISTER_NEW_UFUNC_GENERAL(angle_of_rotor, angle, 1, 1, "Return angle of rotation, assuming input is a unit rotor\n"); // quat -> quat arg_types[0] = quaternion_descr->type_num; arg_types[1] = quaternion_descr->type_num; REGISTER_NEW_UFUNC_GENERAL(sqrt_of_rotor, sqrt, 1, 1, "Return square-root of rotor. Assumes input has unit norm.\n"); REGISTER_UFUNC(log); REGISTER_UFUNC(exp); REGISTER_NEW_UFUNC(normalized, 1, 1, "Normalize all quaternions in this array\n"); REGISTER_NEW_UFUNC(x_parity_conjugate, 1, 1, "Reflect across y-z plane (note spinorial character)\n"); REGISTER_NEW_UFUNC(x_parity_symmetric_part, 1, 1, "Part invariant under reflection across y-z plane (note spinorial character)\n"); REGISTER_NEW_UFUNC(x_parity_antisymmetric_part, 1, 1, "Part anti-invariant under reflection across y-z plane (note spinorial character)\n"); REGISTER_NEW_UFUNC(y_parity_conjugate, 1, 1, "Reflect across x-z plane (note spinorial character)\n"); REGISTER_NEW_UFUNC(y_parity_symmetric_part, 1, 1, "Part invariant under reflection across x-z plane (note spinorial character)\n"); REGISTER_NEW_UFUNC(y_parity_antisymmetric_part, 1, 1, "Part anti-invariant under reflection across x-z plane (note spinorial character)\n"); REGISTER_NEW_UFUNC(z_parity_conjugate, 1, 1, "Reflect across x-y plane (note spinorial character)\n"); REGISTER_NEW_UFUNC(z_parity_symmetric_part, 1, 1, "Part invariant under reflection across x-y plane (note spinorial character)\n"); REGISTER_NEW_UFUNC(z_parity_antisymmetric_part, 1, 1, "Part anti-invariant under reflection across x-y plane (note spinorial character)\n"); REGISTER_NEW_UFUNC(parity_conjugate, 1, 1, "Reflect all dimensions (note spinorial character)\n"); REGISTER_NEW_UFUNC(parity_symmetric_part, 1, 1, "Part invariant under reversal of all vectors (note spinorial character)\n"); REGISTER_NEW_UFUNC(parity_antisymmetric_part, 1, 1, "Part anti-invariant under reversal of all vectors (note spinorial character)\n"); REGISTER_UFUNC(negative); REGISTER_UFUNC(conjugate); REGISTER_UFUNC(invert); // quat, quat -> bool arg_types[0] = quaternion_descr->type_num; arg_types[1] = quaternion_descr->type_num; arg_types[2] = NPY_BOOL; REGISTER_UFUNC(equal); REGISTER_UFUNC(not_equal); REGISTER_UFUNC(less); REGISTER_UFUNC(less_equal); // quat, quat -> quat arg_types[0] = quaternion_descr->type_num; arg_types[1] = quaternion_descr->type_num; arg_types[2] = quaternion_descr->type_num; REGISTER_UFUNC(add); REGISTER_UFUNC(subtract); REGISTER_UFUNC(multiply); REGISTER_UFUNC(divide); REGISTER_UFUNC(true_divide); REGISTER_UFUNC(floor_divide); REGISTER_UFUNC(power); REGISTER_UFUNC(copysign); // double, quat -> quat arg_types[0] = NPY_DOUBLE; arg_types[1] = quaternion_descr->type_num; arg_types[2] = quaternion_descr->type_num; REGISTER_SCALAR_UFUNC(add); REGISTER_SCALAR_UFUNC(subtract); REGISTER_SCALAR_UFUNC(multiply); REGISTER_SCALAR_UFUNC(divide); REGISTER_SCALAR_UFUNC(true_divide); REGISTER_SCALAR_UFUNC(floor_divide); REGISTER_SCALAR_UFUNC(power); // quat, double -> quat arg_types[0] = quaternion_descr->type_num; arg_types[1] = NPY_DOUBLE; arg_types[2] = quaternion_descr->type_num; REGISTER_UFUNC_SCALAR(add); REGISTER_UFUNC_SCALAR(subtract); REGISTER_UFUNC_SCALAR(multiply); REGISTER_UFUNC_SCALAR(divide); REGISTER_UFUNC_SCALAR(true_divide); REGISTER_UFUNC_SCALAR(floor_divide); REGISTER_UFUNC_SCALAR(power); // quat, quat -> double arg_types[0] = quaternion_descr->type_num; arg_types[1] = quaternion_descr->type_num; arg_types[2] = NPY_DOUBLE; REGISTER_NEW_UFUNC(rotor_intrinsic_distance, 2, 1, "Distance measure intrinsic to rotor manifold"); REGISTER_NEW_UFUNC(rotor_chordal_distance, 2, 1, "Distance measure from embedding of rotor manifold"); REGISTER_NEW_UFUNC(rotation_intrinsic_distance, 2, 1, "Distance measure intrinsic to rotation manifold"); REGISTER_NEW_UFUNC(rotation_chordal_distance, 2, 1, "Distance measure from embedding of rotation manifold"); /* I think before I do the following, I'll have to update numpy_dict * somehow, presumably with something related to * PyUFunc_RegisterLoopForType. I should also do this for the * various other methods defined above. */ // Create a custom ufunc and register it for loops. The method for // doing this was pieced together from examples given on the page // arg_dtypes[0] = PyArray_DescrFromType(NPY_DOUBLE); arg_dtypes[1] = quaternion_descr; arg_dtypes[2] = quaternion_descr; arg_dtypes[3] = quaternion_descr; arg_dtypes[4] = quaternion_descr; arg_dtypes[5] = quaternion_descr; squad_evaluate_ufunc = PyUFunc_FromFuncAndData(NULL, NULL, NULL, 0, 5, 1, PyUFunc_None, "squad_vectorized", "Calculate squad from arrays of (tau, q_i, a_i, b_ip1, q_ip1)\n\n" "See quaternion.squad for an easier-to-use version of this function", 0); PyUFunc_RegisterLoopForDescr((PyUFuncObject*)squad_evaluate_ufunc, quaternion_descr, &squad_loop, arg_dtypes, NULL); PyDict_SetItemString(numpy_dict, "squad_vectorized", squad_evaluate_ufunc); Py_DECREF(squad_evaluate_ufunc); // Create a custom ufunc and register it for loops. The method for // doing this was pieced together from examples given on the page // arg_dtypes[0] = quaternion_descr; arg_dtypes[1] = quaternion_descr; arg_dtypes[2] = PyArray_DescrFromType(NPY_DOUBLE); slerp_evaluate_ufunc = PyUFunc_FromFuncAndData(NULL, NULL, NULL, 0, 3, 1, PyUFunc_None, "slerp_vectorized", "Calculate slerp from arrays of (q_1, q_2, tau)\n\n" "See quaternion.slerp for an easier-to-use version of this function", 0); PyUFunc_RegisterLoopForDescr((PyUFuncObject*)slerp_evaluate_ufunc, quaternion_descr, &slerp_loop, arg_dtypes, NULL); PyDict_SetItemString(numpy_dict, "slerp_vectorized", slerp_evaluate_ufunc); Py_DECREF(slerp_evaluate_ufunc); // Add the constant _QUATERNION_EPS to the module as quaternion._eps PyModule_AddObject(module, "_eps", PyFloat_FromDouble(_QUATERNION_EPS)); // Finally, add this quaternion object to the quaternion module itself PyModule_AddObject(module, "quaternion", (PyObject *)&PyQuaternion_Type); #if PY_MAJOR_VERSION >= 3 return module; #else return; #endif }
view raw numpy_quaternion.c hosted with ❤ by GitHub

## Examples: quaternion.py

A second, somewhat simpler implementation in Python without worrying about optimizing computations using C code, defines quaternions as a an ordinary class – rather than a core Python type – with the quaternion data stored in a numpy array. Not counting the additional utility functions for interpolating between rotations and such it comes out to about 525 lines of code. While the code itself is fairly straightforward, easy to read and requires no knowledge of C or CPython, it will suffer from the performance penalties imposed due to Python’s relaxed type inference scheme.

 """ This file is part of the pyquaternion python module Author: Kieran Wynn Website: https://github.com/KieranWynn/pyquaternion Documentation: http://kieranwynn.github.io/pyquaternion/ Version: 1.0.0 License: The MIT License (MIT) Copyright (c) 2015 Kieran Wynn Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. quaternion.py - This file defines the core Quaternion class """ from __future__ import absolute_import, division, print_function # Add compatibility for Python 2.7+ from math import sqrt, pi, sin, cos, asin, acos, atan2, exp, log from copy import deepcopy import numpy as np # Numpy is required for many vector operations class Quaternion: """Class to represent a 4-dimensional complex number or quaternion. Quaternion objects can be used generically as 4D numbers, or as unit quaternions to represent rotations in 3D space. Attributes: q: Quaternion 4-vector represented as a Numpy array """ def __init__(self, *args, **kwargs): """Initialise a new Quaternion object. See Object Initialisation docs for complete behaviour: http://kieranwynn.github.io/pyquaternion/initialisation/ """ s = len(args) if s is 0: # No positional arguments supplied if len(kwargs) > 0: # Keyword arguments provided if ("scalar" in kwargs) or ("vector" in kwargs): scalar = kwargs.get("scalar", 0.0) if scalar is None: scalar = 0.0 else: scalar = float(scalar) vector = kwargs.get("vector", []) vector = self._validate_number_sequence(vector, 3) self.q = np.hstack((scalar, vector)) elif ("real" in kwargs) or ("imaginary" in kwargs): real = kwargs.get("real", 0.0) if real is None: real = 0.0 else: real = float(real) imaginary = kwargs.get("imaginary", []) imaginary = self._validate_number_sequence(imaginary, 3) self.q = np.hstack((real, imaginary)) elif ("axis" in kwargs) or ("radians" in kwargs) or ("degrees" in kwargs) or ("angle" in kwargs): try: axis = self._validate_number_sequence(kwargs["axis"], 3) except KeyError: raise ValueError( "A valid rotation 'axis' parameter must be provided to describe a meaningful rotation." ) angle = kwargs.get('radians') or self.to_radians(kwargs.get('degrees')) or kwargs.get('angle') or 0.0 self.q = Quaternion._from_axis_angle(axis, angle).q elif "array" in kwargs: self.q = self._validate_number_sequence(kwargs["array"], 4) elif "matrix" in kwargs: self.q = Quaternion._from_matrix(kwargs["matrix"]).q else: keys = sorted(kwargs.keys()) elements = [kwargs[kw] for kw in keys] if len(elements) is 1: r = float(elements[0]) self.q = np.array([r, 0.0, 0.0, 0.0]) else: self.q = self._validate_number_sequence(elements, 4) else: # Default initialisation self.q = np.array([1.0, 0.0, 0.0, 0.0]) elif s is 1: # Single positional argument supplied if isinstance(args[0], Quaternion): self.q = args[0].q return if args[0] is None: raise TypeError("Object cannot be initialised from " + str(type(args[0]))) try: r = float(args[0]) self.q = np.array([r, 0.0, 0.0, 0.0]) return except TypeError: pass # If the single argument is not scalar, it should be a sequence self.q = self._validate_number_sequence(args[0], 4) return else: # More than one positional argument supplied self.q = self._validate_number_sequence(args, 4) def __hash__(self): return hash(tuple(self.q)) def _validate_number_sequence(self, seq, n): """Validate a sequence to be of a certain length and ensure it's a numpy array of floats. Raises: ValueError: Invalid length or non-numeric value """ if seq is None: return np.zeros(n) if len(seq) is n: try: l = [float(e) for e in seq] except ValueError: raise ValueError("One or more elements in sequence <" + repr(seq) + "> cannot be interpreted as a real number") else: return np.asarray(l) elif len(seq) is 0: return np.zeros(n) else: raise ValueError("Unexpected number of elements in sequence. Got: " + str(len(seq)) + ", Expected: " + str(n) + ".") # Initialise from matrix @classmethod def _from_matrix(cls, matrix): """Initialise from matrix representation Create a Quaternion by specifying the 3x3 rotation or 4x4 transformation matrix (as a numpy array) from which the quaternion's rotation should be created. """ try: shape = matrix.shape except AttributeError: raise TypeError("Invalid matrix type: Input must be a 3x3 or 4x4 numpy array or matrix") if shape == (3, 3): R = matrix elif shape == (4,4): R = matrix[:-1][:,:-1] # Upper left 3x3 sub-matrix else: raise ValueError("Invalid matrix shape: Input must be a 3x3 or 4x4 numpy array or matrix") # Check matrix properties if not np.allclose(np.dot(R, R.conj().transpose()), np.eye(3)): raise ValueError("Matrix must be orthogonal, i.e. its transpose should be its inverse") if not np.isclose(np.linalg.det(R), 1.0): raise ValueError("Matrix must be special orthogonal i.e. its determinant must be +1.0") def decomposition_method(matrix): """ Method supposedly able to deal with non-orthogonal matrices - NON-FUNCTIONAL! Based on this method: http://arc.aiaa.org/doi/abs/10.2514/2.4654 """ x, y, z = 0, 1, 2 # indices K = np.array([ [R[x, x]-R[y, y]-R[z, z], R[y, x]+R[x, y], R[z, x]+R[x, z], R[y, z]-R[z, y]], [R[y, x]+R[x, y], R[y, y]-R[x, x]-R[z, z], R[z, y]+R[y, z], R[z, x]-R[x, z]], [R[z, x]+R[x, z], R[z, y]+R[y, z], R[z, z]-R[x, x]-R[y, y], R[x, y]-R[y, x]], [R[y, z]-R[z, y], R[z, x]-R[x, z], R[x, y]-R[y, x], R[x, x]+R[y, y]+R[z, z]] ]) K = K / 3.0 e_vals, e_vecs = np.linalg.eig(K) print('Eigenvalues:', e_vals) print('Eigenvectors:', e_vecs) max_index = np.argmax(e_vals) principal_component = e_vecs[max_index] return principal_component def trace_method(matrix): """ This code uses a modification of the algorithm described in: https://d3cw3dd2w32x2b.cloudfront.net/wp-content/uploads/2015/01/matrix-to-quat.pdf which is itself based on the method described here: http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/ Altered to work with the column vector convention instead of row vectors """ m = matrix.conj().transpose() # This method assumes row-vector and postmultiplication of that vector if m[2, 2] < 0: if m[0, 0] > m[1, 1]: t = 1 + m[0, 0] - m[1, 1] - m[2, 2] q = [m[1, 2]-m[2, 1], t, m[0, 1]+m[1, 0], m[2, 0]+m[0, 2]] else: t = 1 - m[0, 0] + m[1, 1] - m[2, 2] q = [m[2, 0]-m[0, 2], m[0, 1]+m[1, 0], t, m[1, 2]+m[2, 1]] else: if m[0, 0] < -m[1, 1]: t = 1 - m[0, 0] - m[1, 1] + m[2, 2] q = [m[0, 1]-m[1, 0], m[2, 0]+m[0, 2], m[1, 2]+m[2, 1], t] else: t = 1 + m[0, 0] + m[1, 1] + m[2, 2] q = [t, m[1, 2]-m[2, 1], m[2, 0]-m[0, 2], m[0, 1]-m[1, 0]] q = np.array(q) q *= 0.5 / sqrt(t); return q return cls(array=trace_method(R)) # Initialise from axis-angle @classmethod def _from_axis_angle(cls, axis, angle): """Initialise from axis and angle representation Create a Quaternion by specifying the 3-vector rotation axis and rotation angle (in radians) from which the quaternion's rotation should be created. Params: axis: a valid numpy 3-vector angle: a real valued angle in radians """ mag_sq = np.dot(axis, axis) if mag_sq == 0.0: raise ZeroDivisionError("Provided rotation axis has no length") # Ensure axis is in unit vector form if (abs(1.0 - mag_sq) > 1e-12): axis = axis / sqrt(mag_sq) theta = angle / 2.0 r = cos(theta) i = axis * sin(theta) return cls(r, i[0], i[1], i[2]) @classmethod def random(cls): """Generate a random unit quaternion. Uniformly distributed across the rotation space As per: http://planning.cs.uiuc.edu/node198.html """ r1, r2, r3 = np.random.random(3) q1 = sqrt(1.0 - r1) * (sin(2 * pi * r2)) q2 = sqrt(1.0 - r1) * (cos(2 * pi * r2)) q3 = sqrt(r1) * (sin(2 * pi * r3)) q4 = sqrt(r1) * (cos(2 * pi * r3)) return cls(q1, q2, q3, q4) # Representation def __str__(self): """An informal, nicely printable string representation of the Quaternion object. """ return "{:.3f} {:+.3f}i {:+.3f}j {:+.3f}k".format(self.q[0], self.q[1], self.q[2], self.q[3]) def __repr__(self): """The 'official' string representation of the Quaternion object. This is a string representation of a valid Python expression that could be used to recreate an object with the same value (given an appropriate environment) """ return "Quaternion({}, {}, {}, {})".format(repr(self.q[0]), repr(self.q[1]), repr(self.q[2]), repr(self.q[3])) def __format__(self, formatstr): """Inserts a customisable, nicely printable string representation of the Quaternion object The syntax for format_spec mirrors that of the built in format specifiers for floating point types. Check out the official Python [format specification mini-language](https://docs.python.org/3.4/library/string.html#formatspec) for details. """ if formatstr.strip() == '': # Defualt behaviour mirrors self.__str__() formatstr = '+.3f' string = \ "{:" + formatstr +"} " + \ "{:" + formatstr +"}i " + \ "{:" + formatstr +"}j " + \ "{:" + formatstr +"}k" return string.format(self.q[0], self.q[1], self.q[2], self.q[3]) # Type Conversion def __int__(self): """Implements type conversion to int. Truncates the Quaternion object by only considering the real component and rounding to the next integer value towards zero. Note: to round to the closest integer, use int(round(float(q))) """ return int(self.q[0]) def __float__(self): """Implements type conversion to float. Truncates the Quaternion object by only considering the real component. """ return self.q[0] def __complex__(self): """Implements type conversion to complex. Truncates the Quaternion object by only considering the real component and the first imaginary component. This is equivalent to a projection from the 4-dimensional hypersphere to the 2-dimensional complex plane. """ return complex(self.q[0], self.q[1]) def __bool__(self): return not (self == Quaternion(0.0)) def __nonzero__(self): return not (self == Quaternion(0.0)) def __invert__(self): return (self == Quaternion(0.0)) # Comparison def __eq__(self, other): """Returns true if the following is true for each element: absolute(a - b) <= (atol + rtol * absolute(b)) """ if isinstance(other, Quaternion): r_tol = 1.0e-13 a_tol = 1.0e-14 try: isEqual = np.allclose(self.q, other.q, rtol=r_tol, atol=a_tol) except AttributeError: raise AttributeError("Error in internal quaternion representation means it cannot be compared like a numpy array.") return isEqual return self.__eq__(self.__class__(other)) # Negation def __neg__(self): return self.__class__(array= -self.q) # Addition def __add__(self, other): if isinstance(other, Quaternion): return self.__class__(array=self.q + other.q) return self + self.__class__(other) def __iadd__(self, other): return self + other def __radd__(self, other): return self + other # Subtraction def __sub__(self, other): return self + (-other) def __isub__(self, other): return self + (-other) def __rsub__(self, other): return -(self - other) # Multiplication def __mul__(self, other): if isinstance(other, Quaternion): return self.__class__(array=np.dot(self._q_matrix(), other.q)) return self * self.__class__(other) def __imul__(self, other): return self * other def __rmul__(self, other): return self.__class__(other) * self # Division def __div__(self, other): if isinstance(other, Quaternion): if other == self.__class__(0.0): raise ZeroDivisionError("Quaternion divisor must be non-zero") return self * other.inverse return self.__div__(self.__class__(other)) def __idiv__(self, other): return self.__div__(other) def __rdiv__(self, other): return self.__class__(other) * self.inverse def __truediv__(self, other): return self.__div__(other) def __itruediv__(self, other): return self.__idiv__(other) def __rtruediv__(self, other): return self.__rdiv__(other) # Exponentiation def __pow__(self, exponent): # source: https://en.wikipedia.org/wiki/Quaternion#Exponential.2C_logarithm.2C_and_power exponent = float(exponent) # Explicitly reject non-real exponents norm = self.norm if norm > 0.0: try: n, theta = self.polar_decomposition except ZeroDivisionError: # quaternion is a real number (no vector or imaginary part) return Quaternion(scalar=self.scalar ** exponent) return (self.norm ** exponent) * Quaternion(scalar=cos(exponent * theta), vector=(n * sin(exponent * theta))) return Quaternion(self) def __ipow__(self, other): return self ** other def __rpow__(self, other): return other ** float(self) # Quaternion Features def _vector_conjugate(self): return np.hstack((self.q[0], -self.q[1:4])) def _sum_of_squares(self): return np.dot(self.q, self.q) @property def conjugate(self): """Quaternion conjugate, encapsulated in a new instance. For a unit quaternion, this is the same as the inverse. Returns: A new Quaternion object clone with its vector part negated """ return self.__class__(scalar=self.scalar, vector= -self.vector) @property def inverse(self): """Inverse of the quaternion object, encapsulated in a new instance. For a unit quaternion, this is the inverse rotation, i.e. when combined with the original rotation, will result in the null rotation. Returns: A new Quaternion object representing the inverse of this object """ ss = self._sum_of_squares() if ss > 0: return self.__class__(array=(self._vector_conjugate() / ss)) else: raise ZeroDivisionError("a zero quaternion (0 + 0i + 0j + 0k) cannot be inverted") @property def norm(self): """L2 norm of the quaternion 4-vector. This should be 1.0 for a unit quaternion (versor) Slow but accurate. If speed is a concern, consider using _fast_normalise() instead Returns: A scalar real number representing the square root of the sum of the squares of the elements of the quaternion. """ mag_squared = self._sum_of_squares() return sqrt(mag_squared) @property def magnitude(self): return self.norm def _normalise(self): """Object is guaranteed to be a unit quaternion after calling this operation UNLESS the object is equivalent to Quaternion(0) """ if not self.is_unit(): n = self.norm if n > 0: self.q = self.q / n def _fast_normalise(self): """Normalise the object to a unit quaternion using a fast approximation method if appropriate. Object is guaranteed to be a quaternion of approximately unit length after calling this operation UNLESS the object is equivalent to Quaternion(0) """ if not self.is_unit(): mag_squared = np.dot(self.q, self.q) if (mag_squared == 0): return if (abs(1.0 - mag_squared) < 2.107342e-08): mag = ((1.0 + mag_squared) / 2.0) # More efficient. Pade approximation valid if error is small else: mag = sqrt(mag_squared) # Error is too big, take the performance hit to calculate the square root properly self.q = self.q / mag @property def normalised(self): """Get a unit quaternion (versor) copy of this Quaternion object. A unit quaternion has a norm of 1.0 Returns: A new Quaternion object clone that is guaranteed to be a unit quaternion """ q = Quaternion(self) q._normalise() return q @property def polar_unit_vector(self): vector_length = np.linalg.norm(self.vector) if vector_length <= 0.0: raise ZeroDivisionError('Quaternion is pure real and does not have a unique unit vector') return self.vector / vector_length @property def polar_angle(self): return acos(self.scalar / self.norm) @property def polar_decomposition(self): """ Returns the unit vector and angle of a non-scalar quaternion according to the following decomposition q = q.norm() * (e ** (q.polar_unit_vector * q.polar_angle)) source: https://en.wikipedia.org/wiki/Polar_decomposition#Quaternion_polar_decomposition """ return self.polar_unit_vector, self.polar_angle @property def unit(self): return self.normalised def is_unit(self, tolerance=1e-14): """Determine whether the quaternion is of unit length to within a specified tolerance value. Params: tolerance: [optional] maximum absolute value by which the norm can differ from 1.0 for the object to be considered a unit quaternion. Defaults to 1e-14. Returns: True if the Quaternion object is of unit length to within the specified tolerance value. False otherwise. """ return abs(1.0 - self._sum_of_squares()) < tolerance # if _sum_of_squares is 1, norm is 1. This saves a call to sqrt() def _q_matrix(self): """Matrix representation of quaternion for multiplication purposes. """ return np.array([ [self.q[0], -self.q[1], -self.q[2], -self.q[3]], [self.q[1], self.q[0], -self.q[3], self.q[2]], [self.q[2], self.q[3], self.q[0], -self.q[1]], [self.q[3], -self.q[2], self.q[1], self.q[0]]]) def _q_bar_matrix(self): """Matrix representation of quaternion for multiplication purposes. """ return np.array([ [self.q[0], -self.q[1], -self.q[2], -self.q[3]], [self.q[1], self.q[0], self.q[3], -self.q[2]], [self.q[2], -self.q[3], self.q[0], self.q[1]], [self.q[3], self.q[2], -self.q[1], self.q[0]]]) def _rotate_quaternion(self, q): """Rotate a quaternion vector using the stored rotation. Params: q: The vector to be rotated, in quaternion form (0 + xi + yj + kz) Returns: A Quaternion object representing the rotated vector in quaternion from (0 + xi + yj + kz) """ self._normalise() return self * q * self.conjugate def rotate(self, vector): """Rotate a 3D vector by the rotation stored in the Quaternion object. Params: vector: A 3-vector specified as any ordered sequence of 3 real numbers corresponding to x, y, and z values. Some types that are recognised are: numpy arrays, lists and tuples. A 3-vector can also be represented by a Quaternion object who's scalar part is 0 and vector part is the required 3-vector. Thus it is possible to call Quaternion.rotate(q) with another quaternion object as an input. Returns: The rotated vector returned as the same type it was specified at input. Raises: TypeError: if any of the vector elements cannot be converted to a real number. ValueError: if vector cannot be interpreted as a 3-vector or a Quaternion object. """ if isinstance(vector, Quaternion): return self._rotate_quaternion(vector) q = Quaternion(vector=vector) a = self._rotate_quaternion(q).vector if isinstance(vector, list): l = [x for x in a] return l elif isinstance(vector, tuple): l = [x for x in a] return tuple(l) else: return a @classmethod def exp(cls, q): """Quaternion Exponential. Find the exponential of a quaternion amount. Params: q: the input quaternion/argument as a Quaternion object. Returns: A quaternion amount representing the exp(q). See [Source](https://math.stackexchange.com/questions/1030737/exponential-function-of-quaternion-derivation for more information and mathematical background). Note: The method can compute the exponential of any quaternion. """ tolerance = 1e-17 v_norm = np.linalg.norm(q.vector) vec = q.vector if v_norm > tolerance: vec = vec / v_norm magnitude = exp(q.scalar) return Quaternion(scalar = magnitude * cos(v_norm), vector = magnitude * sin(v_norm) * vec) @classmethod def log(cls, q): """Quaternion Logarithm. Find the logarithm of a quaternion amount. Params: q: the input quaternion/argument as a Quaternion object. Returns: A quaternion amount representing log(q) := (log(|q|), v/|v|acos(w/|q|)). Note: The method computes the logarithm of general quaternions. See [Source](https://math.stackexchange.com/questions/2552/the-logarithm-of-quaternion/2554#2554) for more details. """ v_norm = np.linalg.norm(q.vector) q_norm = q.norm tolerance = 1e-17 if q_norm < tolerance: # 0 quaternion - undefined return Quaternion(scalar=-float('inf'), vector=float('nan')*q.vector) if v_norm < tolerance: # real quaternions - no imaginary part return Quaternion(scalar=log(q_norm), vector=[0,0,0]) vec = q.vector / v_norm return Quaternion(scalar=log(q_norm), vector=acos(q.scalar/q_norm)*vec) @classmethod def exp_map(cls, q, eta): """Quaternion exponential map. Find the exponential map on the Riemannian manifold described by the quaternion space. Params: q: the base point of the exponential map, i.e. a Quaternion object eta: the argument of the exponential map, a tangent vector, i.e. a Quaternion object Returns: A quaternion p such that p is the endpoint of the geodesic starting at q in the direction of eta, having the length equal to the magnitude of eta. Note: The exponential map plays an important role in integrating orientation variations (e.g. angular velocities). This is done by projecting quaternion tangent vectors onto the quaternion manifold. """ return q * Quaternion.exp(eta) @classmethod def sym_exp_map(cls, q, eta): """Quaternion symmetrized exponential map. Find the symmetrized exponential map on the quaternion Riemannian manifold. Params: q: the base point as a Quaternion object eta: the tangent vector argument of the exponential map as a Quaternion object Returns: A quaternion p. Note: The symmetrized exponential formulation is akin to the exponential formulation for symmetric positive definite tensors [Source](http://www.academia.edu/7656761/On_the_Averaging_of_Symmetric_Positive-Definite_Tensors) """ sqrt_q = q ** 0.5 return sqrt_q * Quaternion.exp(eta) * sqrt_q @classmethod def log_map(cls, q, p): """Quaternion logarithm map. Find the logarithm map on the quaternion Riemannian manifold. Params: q: the base point at which the logarithm is computed, i.e. a Quaternion object p: the argument of the quaternion map, a Quaternion object Returns: A tangent vector having the length and direction given by the geodesic joining q and p. """ return Quaternion.log(q.inverse * p) @classmethod def sym_log_map(cls, q, p): """Quaternion symmetrized logarithm map. Find the symmetrized logarithm map on the quaternion Riemannian manifold. Params: q: the base point at which the logarithm is computed, i.e. a Quaternion object p: the argument of the quaternion map, a Quaternion object Returns: A tangent vector corresponding to the symmetrized geodesic curve formulation. Note: Information on the symmetrized formulations given in [Source](https://www.researchgate.net/publication/267191489_Riemannian_L_p_Averaging_on_Lie_Group_of_Nonzero_Quaternions). """ inv_sqrt_q = (q ** (-0.5)) return Quaternion.log(inv_sqrt_q * p * inv_sqrt_q) @classmethod def absolute_distance(cls, q0, q1): """Quaternion absolute distance. Find the distance between two quaternions accounting for the sign ambiguity. Params: q0: the first quaternion q1: the second quaternion Returns: A positive scalar corresponding to the chord of the shortest path/arc that connects q0 to q1. Note: This function does not measure the distance on the hypersphere, but it takes into account the fact that q and -q encode the same rotation. It is thus a good indicator for rotation similarities. """ q0_minus_q1 = q0 - q1 q0_plus_q1 = q0 + q1 d_minus = q0_minus_q1.norm d_plus = q0_plus_q1.norm if (d_minus < d_plus): return d_minus else: return d_plus @classmethod def distance(cls, q0, q1): """Quaternion intrinsic distance. Find the intrinsic geodesic distance between q0 and q1. Params: q0: the first quaternion q1: the second quaternion Returns: A positive amount corresponding to the length of the geodesic arc connecting q0 to q1. Note: Although the q0^(-1)*q1 != q1^(-1)*q0, the length of the path joining them is given by the logarithm of those product quaternions, the norm of which is the same. """ q = Quaternion.log_map(q0, q1) return q.norm @classmethod def sym_distance(cls, q0, q1): """Quaternion symmetrized distance. Find the intrinsic symmetrized geodesic distance between q0 and q1. Params: q0: the first quaternion q1: the second quaternion Returns: A positive amount corresponding to the length of the symmetrized geodesic curve connecting q0 to q1. Note: This formulation is more numerically stable when performing iterative gradient descent on the Riemannian quaternion manifold. However, the distance between q and -q is equal to pi, rendering this formulation not useful for measuring rotation similarities when the samples are spread over a "solid" angle of more than pi/2 radians (the spread refers to quaternions as point samples on the unit hypersphere). """ q = Quaternion.sym_log_map(q0, q1) return q.norm @classmethod def slerp(cls, q0, q1, amount=0.5): """Spherical Linear Interpolation between quaternions. Implemented as described in https://en.wikipedia.org/wiki/Slerp Find a valid quaternion rotation at a specified distance along the minor arc of a great circle passing through any two existing quaternion endpoints lying on the unit radius hypersphere. This is a class method and is called as a method of the class itself rather than on a particular instance. Params: q0: first endpoint rotation as a Quaternion object q1: second endpoint rotation as a Quaternion object amount: interpolation parameter between 0 and 1. This describes the linear placement position of the result along the arc between endpoints; 0 being at q0 and 1 being at q1. Defaults to the midpoint (0.5). Returns: A new Quaternion object representing the interpolated rotation. This is guaranteed to be a unit quaternion. Note: This feature only makes sense when interpolating between unit quaternions (those lying on the unit radius hypersphere). Calling this method will implicitly normalise the endpoints to unit quaternions if they are not already unit length. """ # Ensure quaternion inputs are unit quaternions and 0 <= amount <=1 q0._fast_normalise() q1._fast_normalise() amount = np.clip(amount, 0, 1) dot = np.dot(q0.q, q1.q) # If the dot product is negative, slerp won't take the shorter path. # Note that v1 and -v1 are equivalent when the negation is applied to all four components. # Fix by reversing one quaternion if (dot < 0.0): q0.q = -q0.q dot = -dot # sin_theta_0 can not be zero if (dot > 0.9995): qr = Quaternion(q0.q + amount*(q1.q - q0.q)) qr._fast_normalise() return qr theta_0 = np.arccos(dot) # Since dot is in range [0, 0.9995], np.arccos() is safe sin_theta_0 = np.sin(theta_0) theta = theta_0*amount sin_theta = np.sin(theta) s0 = np.cos(theta) - dot * sin_theta / sin_theta_0 s1 = sin_theta / sin_theta_0 qr = Quaternion((s0 * q0.q) + (s1 * q1.q)) qr._fast_normalise() return qr @classmethod def intermediates(cls, q0, q1, n, include_endpoints=False): """Generator method to get an iterable sequence of n evenly spaced quaternion rotations between any two existing quaternion endpoints lying on the unit radius hypersphere. This is a convenience function that is based on Quaternion.slerp() as defined above. This is a class method and is called as a method of the class itself rather than on a particular instance. Params: q_start: initial endpoint rotation as a Quaternion object q_end: final endpoint rotation as a Quaternion object n: number of intermediate quaternion objects to include within the interval include_endpoints: [optional] if set to True, the sequence of intermediates will be 'bookended' by q_start and q_end, resulting in a sequence length of n + 2. If set to False, endpoints are not included. Defaults to False. Yields: A generator object iterating over a sequence of intermediate quaternion objects. Note: This feature only makes sense when interpolating between unit quaternions (those lying on the unit radius hypersphere). Calling this method will implicitly normalise the endpoints to unit quaternions if they are not already unit length. """ step_size = 1.0 / (n + 1) if include_endpoints: steps = [i * step_size for i in range(0, n + 2)] else: steps = [i * step_size for i in range(1, n + 1)] for step in steps: yield cls.slerp(q0, q1, step) def derivative(self, rate): """Get the instantaneous quaternion derivative representing a quaternion rotating at a 3D rate vector rate Params: rate: numpy 3-array (or array-like) describing rotation rates about the global x, y and z axes respectively. Returns: A unit quaternion describing the rotation rate """ rate = self._validate_number_sequence(rate, 3) return 0.5 * self * Quaternion(vector=rate) def integrate(self, rate, timestep): """Advance a time varying quaternion to its value at a time timestep in the future. The Quaternion object will be modified to its future value. It is guaranteed to remain a unit quaternion. Params: rate: numpy 3-array (or array-like) describing rotation rates about the global x, y and z axes respectively. timestep: interval over which to integrate into the future. Assuming *now* is T=0, the integration occurs over the interval T=0 to T=timestep. Smaller intervals are more accurate when rate changes over time. Note: The solution is closed form given the assumption that rate is constant over the interval of length timestep. """ self._fast_normalise() rate = self._validate_number_sequence(rate, 3) rotation_vector = rate * timestep rotation_norm = np.linalg.norm(rotation_vector) if rotation_norm > 0: axis = rotation_vector / rotation_norm angle = rotation_norm q2 = Quaternion(axis=axis, angle=angle) self.q = (self * q2).q self._fast_normalise() @property def rotation_matrix(self): """Get the 3x3 rotation matrix equivalent of the quaternion rotation. Returns: A 3x3 orthogonal rotation matrix as a 3x3 Numpy array Note: This feature only makes sense when referring to a unit quaternion. Calling this method will implicitly normalise the Quaternion object to a unit quaternion if it is not already one. """ self._normalise() product_matrix = np.dot(self._q_matrix(), self._q_bar_matrix().conj().transpose()) return product_matrix[1:][:,1:] @property def transformation_matrix(self): """Get the 4x4 homogeneous transformation matrix equivalent of the quaternion rotation. Returns: A 4x4 homogeneous transformation matrix as a 4x4 Numpy array Note: This feature only makes sense when referring to a unit quaternion. Calling this method will implicitly normalise the Quaternion object to a unit quaternion if it is not already one. """ t = np.array([[0.0], [0.0], [0.0]]) Rt = np.hstack([self.rotation_matrix, t]) return np.vstack([Rt, np.array([0.0, 0.0, 0.0, 1.0])]) @property def yaw_pitch_roll(self): """Get the equivalent yaw-pitch-roll angles aka. intrinsic Tait-Bryan angles following the z-y'-x'' convention Returns: yaw: rotation angle around the z-axis in radians, in the range [-pi, pi] pitch: rotation angle around the y'-axis in radians, in the range [-pi/2, -pi/2] roll: rotation angle around the x''-axis in radians, in the range [-pi, pi] The resulting rotation_matrix would be R = R_x(roll) R_y(pitch) R_z(yaw) Note: This feature only makes sense when referring to a unit quaternion. Calling this method will implicitly normalise the Quaternion object to a unit quaternion if it is not already one. """ self._normalise() yaw = np.arctan2(2*(self.q[0]*self.q[3] - self.q[1]*self.q[2]), 1 - 2*(self.q[2]**2 + self.q[3]**2)) pitch = np.arcsin(2*(self.q[0]*self.q[2] + self.q[3]*self.q[1])) roll = np.arctan2(2*(self.q[0]*self.q[1] - self.q[2]*self.q[3]), 1 - 2*(self.q[1]**2 + self.q[2]**2)) return yaw, pitch, roll