Coding · Research

Implementing Quaternions in Julia

Newton chilling and Einstein contemplating in IUCAA courtyard.

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.

Aryabhatta and Kepler showing off their moves in IUCAA courtyard.

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:

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

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:

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

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 <Python.h>
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 "<module>.<name>" */
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: <https://github.com/moble/quaternion/blob/master/LICENSE>
#define NPY_NO_DEPRECATED_API NPY_API_VERSION
#include <Python.h>
#include <numpy/arrayobject.h>
#include <numpy/npy_math.h>
#include <numpy/ufuncobject.h>
#include "structmember.h"
#include "quaternion.h"
// The following definitions, along with `#define NPY_PY3K 1`, can
// also be found in the header <numpy/npy_3kcompat.h>.
#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
// <https://docs.scipy.org/doc/numpy/user/c-info.ufunc-tutorial.html>
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
// <https://docs.scipy.org/doc/numpy/user/c-info.ufunc-tutorial.html>
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
// <https://docs.scipy.org/doc/numpy/user/c-info.ufunc-tutorial.html>
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
// <https://docs.scipy.org/doc/numpy/user/c-info.ufunc-tutorial.html>
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