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
Table of Contents
Complex Numbers
Complex numbers are “tuples” (a fancy word for a set consisting of two objects) of real numbers, i.e. given $ a, b \in \mathbb{R} $, one can define a complex number:
$$ z = a + \imath b, $$
which is an element of the so-called “complex plane” $ \mathbb{C} $ which is nothing more than the two-dimensional Euclidean plane $ \mathbb{R}^2 $ with one axis identified as “real” and the other as “imaginary”. The funny looking “i” multiplying $ b $ in the expression above, is called the “unit imaginary” and is supposed to represent the square of root of $-1$:
$$ \imath = \sqrt{-1}. $$
At this point, you’re probably thinking “what a load of …, you can’t take square roots of negative numbers!” And, you would be right! Indeed we can’t express the square root of a negative number in terms of real numbers. But, mathematicians being the wizards they are, decided there is no law which stops us from writing down the formal expression $ \imath = \sqrt{-1} $. If you can suspend disbelief for a second, and just accept this fact, then all sorts of wonderful things become possible.
Given any two complex numbers we can add, subtract or multiply them to obtain another complex number:
\begin{align}
z_1 + z_2 & = (a_1 + a_2) + \imath (b_1 + b_2) \nonumber \\
z_1 – z_2 & = (a_1 – a_2) + \imath (b_1 – b_2) \nonumber \\
z_1 z_2 & = (a_1 a_2 – b_1 b_2) + \imath (a_1 b_2 + a_2 b_2),
\end{align}
where in the last line we have used the fact that $ (\imath b_1) (\imath b_2) = \imath^2 b_1 b_2 = – b_1 b_2 $. We can also divide complex numbers by other complex numbers. However, in order to write the result of a division operation $ z_1/z_2 $ as another complex number, we have to introduce a new operation called “complex conjugation” denoted by an asterix ${}^*$. Under complex conjugation, real numbers are unchanged, but purely imaginary numbers becomes negative:
$$ a^* = a; \quad (\imath b) = – \imath b; \Rightarrow z^* = (a + \imath b)^* = a – \imath b $$
where $ a, b \in \mbb{R} $. The result of dividing one complex number by another can then be expressed as follows:
\begin{align}
\frac{z_1}{z_2} & = \frac{a_1 + \imath b_1}{a_2 + \imath b_2} \nonumber \\
& = \frac{(a_1 + \imath b_1)}{(a_2 + \imath b_2)} \frac{(a_2 – \imath b_2)}{(a_2 – \imath b_2)} \nonumber \nonumber \\
& = \frac{a_1 a_2 + b_1 b_2 + \imath (b_1 a_2 – b_2 a_1)}{a_2^2 + b_2^2} \nonumber \\
& = \frac{1}{|z_2|^2} z_1 z_2^*.
\end{align}
In the second line we have multiplied the numerator and denominator by $ z_2^* $. In the third line we have used the fact that the multiple of a complex number with its complex conjugate yields a real number ($ z z^* = a^2 + b^2 \in \mbb{R} $) to get rid of the imaginary unit in the denominator. The real and imaginary parts of $ z_1/z_2 $ can then be written as:
$$ \mf{Re}(z_1/z_2) = \frac{a_1 a_2 + b_1 b_2}{a_2^2 + b_2^2}; \quad \mf{Im}(z_1/z_2) = \frac{b_1 a_2 – b_2 a_1}{a_2^2 + b_2^2}. $$
In this way we can perform all the standard arithmetical operations with complex numbers. We can go on to define functions on the complex plane $ f: \mbb{C} \rightarrow \mbb{C} $. All the usual functions on real numbers, such as polynomials $x^n$, roots $x^{1/n}$, trignometric and logarithmic functions $\sin(x), \log(x)$, can be generalized straightforwardly to take complex numbers as their arguments instead of real numbers.
This only touches the tip of the proverbial iceberg. There is a lot more to say about complex numbers, but this introduction will suffice to enable us to talk about our true goal – the quaternions.
Quaternions
Quaternions are just like complex numbers, with one minor difference. Instead of just one imaginary axis as in the complex numbers, quaternions have three different imaginary axes. Blew your mind, right? Well, not really. We don’t have any difficulties in extending the real line to the two or three-dimensional spaces $ \mbb{R}^2 $ and $ \mbb{R}^n$. We are comfortable defining vectors in $ \mbb{R}^n$ as tuples of $n$ real numbers: $ \vect{v} = (v_1, v_2, \ldots, v_n) $ and performing all the usual arithmetical operations (except for division) with these objects. As we have just seen, we can define a new kind of axis which represents the “imaginary” dimension. Well, what’s to stop us from extending $ \mbb{C} $ by adding more imaginary dimensions? Nothing!
Let’s start small and add just one extra imaginary axis. Our “triplex” numbers would then have the form:
$$ \gamma = a + i b + j c, $$
where, expectedly, $ i^2 = j^2 = -1 $2 and $ a,b,c \in \mbb{R} $. It turns out that the set of triplex numbers is not closed under the arithmetic operation of multiplication. We soldier on ahead and add one more dimension:
$$ q = a + i b + j c + k d, $$
where, once again:
\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:
Operation | Result |
---|---|
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); | |
} |
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; |
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 | |
} |
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 | |
def _wrap_angle(self, theta): | |
"""Helper method: Wrap any angle to lie between -pi and pi | |
Odd multiples of pi are wrapped to +pi (as opposed to -pi) | |
""" | |
result = ((theta + pi) % (2*pi)) - pi | |
if result == -pi: result = pi | |
return result | |
def get_axis(self, undefined=np.zeros(3)): | |
"""Get the axis or vector about which the quaternion rotation occurs | |
For a null rotation (a purely real quaternion), the rotation angle will | |
always be `0`, but the rotation axis is undefined. | |
It is by default assumed to be `[0, 0, 0]`. | |
Params: | |
undefined: [optional] specify the axis vector that should define a null rotation. | |
This is geometrically meaningless, and could be any of an infinite set of vectors, | |
but can be specified if the default (`[0, 0, 0]`) causes undesired behaviour. | |
Returns: | |
A Numpy unit 3-vector describing the Quaternion object's axis of rotation. | |
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. | |
""" | |
tolerance = 1e-17 | |
self._normalise() | |
norm = np.linalg.norm(self.vector) | |
if norm < tolerance: | |
# Here there are an infinite set of possible axes, use what has been specified as an undefined axis. | |
return undefined | |
else: | |
return self.vector / norm | |
@property | |
def axis(self): | |
return self.get_axis() | |
@property | |
def angle(self): | |
"""Get the angle (in radians) describing the magnitude of the quaternion rotation about its rotation axis. | |
This is guaranteed to be within the range (-pi:pi) with the direction of | |
rotation indicated by the sign. | |
When a particular rotation describes a 180 degree rotation about an arbitrary | |
axis vector `v`, the conversion to axis / angle representation may jump | |
discontinuously between all permutations of `(-pi, pi)` and `(-v, v)`, | |
each being geometrically equivalent (see Note in documentation). | |
Returns: | |
A real number in the range (-pi:pi) describing the angle of rotation | |
in radians about a Quaternion object's axis of rotation. | |
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() | |
norm = np.linalg.norm(self.vector) | |
return self._wrap_angle(2.0 * atan2(norm,self.scalar)) | |
@property | |
def degrees(self): | |
return self.to_degrees(self.angle) | |
@property | |
def radians(self): | |
return self.angle | |
@property | |
def scalar(self): | |
""" Return the real or scalar component of the quaternion object. | |
Returns: | |
A real number i.e. float | |
""" | |
return self.q[0] | |
@property | |
def vector(self): | |
""" Return the imaginary or vector component of the quaternion object. | |
Returns: | |
A numpy 3-array of floats. NOT guaranteed to be a unit vector | |
""" | |
return self.q[1:4] | |
@property | |
def real(self): | |
return self.scalar | |
@property | |
def imaginary(self): | |
return self.vector | |
@property | |
def w(self): | |
return self.q[0] | |
@property | |
def x(self): | |
return self.q[1] | |
@property | |
def y(self): | |
return self.q[2] | |
@property | |
def z(self): | |
return self.q[3] | |
@property | |
def elements(self): | |
""" Return all the elements of the quaternion object. | |
Returns: | |
A numpy 4-array of floats. NOT guaranteed to be a unit vector | |
""" | |
return self.q | |
def __getitem__(self, index): | |
index = int(index) | |
return self.q[index] | |
def __setitem__(self, index, value): | |
index = int(index) | |
self.q[index] = float(value) | |
def __copy__(self): | |
result = self.__class__(self.q) | |
return result | |
def __deepcopy__(self, memo): | |
result = self.__class__(deepcopy(self.q, memo)) | |
memo[id(self)] = result | |
return result | |
@staticmethod | |
def to_degrees(angle_rad): | |
if angle_rad is not None: | |
return float(angle_rad) / pi * 180.0 | |
@staticmethod | |
def to_radians(angle_deg): | |
if angle_deg is not None: | |
return float(angle_deg) / 180.0 * pi |
In Julia
Let us look at the same functionality implemented in Julia. Only 296 lines long. No need for separate C code in order to get C-like speeds. Syntax is simpler than the equivalent Python implementation. Operator overloading methods can be written in a very simple manner. For instance the operator +
for adding two quaternion types has a single line definition:
(+)(q::Quaternion, w::Quaternion) = Quaternion(q.s + w.s, q.v1 + w.v1, q.v2 + w.v2, q.v3 + w.v3)
# Original source: https://github.com/JuliaGeometry/Quaternions.jl | |
struct Quaternion{T<:Real} <: Number | |
s::T | |
v1::T | |
v2::T | |
v3::T | |
norm::Bool | |
end | |
(::Type{Quaternion{T}})(x::Real) where {T} = Quaternion{T}(x, 0, 0, 0, false) | |
(::Type{Quaternion{T}})(q::Quaternion{T}) where {T<:Real} = q | |
(::Type{Quaternion{T}})(q::Quaternion) where {T<:Real} = Quaternion{T}(q.s, q.v1, q.v2, q.v3, q.norm) | |
Quaternion(s::Real, v1::Real, v2::Real, v3::Real, n::Bool = false) = | |
Quaternion(promote(s, v1, v2, v3)..., n) | |
Quaternion(x::Real) = Quaternion(x, zero(x), zero(x), zero(x), abs(x) == one(x)) | |
Quaternion(z::Complex) = Quaternion(z.re, z.im, zero(z.re), zero(z.re), abs(z) == one(z.re)) | |
Quaternion(s::Real, a::Vector) = Quaternion(s, a[1], a[2], a[3]) | |
Quaternion(a::Vector) = Quaternion(0, a[1], a[2], a[3]) | |
convert(::Type{Quaternion{T}}, x::Real) where {T} = | |
Quaternion(convert(T, x), convert(T, 0), convert(T, 0), convert(T, 0)) | |
convert(::Type{Quaternion{T}}, z::Complex) where {T} = | |
Quaternion(convert(T, real(z)), convert(T, imag(z)), convert(T, 0), convert(T, 0)) | |
convert(::Type{Quaternion{T}}, q::Quaternion{T}) where {T <: Real} = q | |
convert(::Type{Quaternion{T}}, q::Quaternion) where {T} = | |
Quaternion(convert(T, q.s), convert(T, q.v1), convert(T, q.v2), convert(T, q.v3), q.norm) | |
promote_rule(::Type{Quaternion{T}}, ::Type{T}) where {T <: Real} = Quaternion{T} | |
promote_rule(::Type{Quaternion}, ::Type{T}) where {T <: Real} = Quaternion | |
promote_rule(::Type{Quaternion{T}}, ::Type{S}) where {T <: Real, S <: Real} = Quaternion{promote_type(T, S)} | |
promote_rule(::Type{Complex{T}}, ::Type{Quaternion{S}}) where {T <: Real, S <: Real} = Quaternion{promote_type(T, S)} | |
promote_rule(::Type{Quaternion{T}}, ::Type{Quaternion{S}}) where {T <: Real, S <: Real} = Quaternion{promote_type(T, S)} | |
quat(p, v1, v2, v3, n = false) = Quaternion(p, v1, v2, v3, n) | |
quat(x) = Quaternion(x) | |
quat(s, a) = Quaternion(s, a) | |
function show(io::IO, q::Quaternion) | |
pm(x) = x < 0 ? " - $(-x)" : " + $x" | |
print(io, q.s, pm(q.v1), "im", pm(q.v2), "jm", pm(q.v3), "km") | |
end | |
real(::Type{Quaternion{T}}) where {T} = T | |
real(q::Quaternion) = q.s | |
imag(q::Quaternion) = [q.v1, q.v2, q.v3] | |
(/)(q::Quaternion, x::Real) = Quaternion(q.s / x, q.v1 / x, q.v2 / x, q.v3 / x) | |
conj(q::Quaternion) = Quaternion(q.s, -q.v1, -q.v2, -q.v3, q.norm) | |
abs(q::Quaternion) = sqrt(q.s * q.s + q.v1 * q.v1 + q.v2 * q.v2 + q.v3 * q.v3) | |
abs_imag(q::Quaternion) = sqrt(q.v1 * q.v1 + q.v2 * q.v2 + q.v3 * q.v3) | |
abs2(q::Quaternion) = q.s * q.s + q.v1 * q.v1 + q.v2 * q.v2 + q.v3 * q.v3 | |
inv(q::Quaternion) = q.norm ? conj(q) : conj(q) / abs2(q) | |
isfinite(q::Quaternion) = q.norm ? true : (isfinite(q.s) && isfinite(q.v1) && isfinite(q.v2) && isfinite(q.v3)) | |
function normalize(q::Quaternion) | |
if (q.norm) | |
return q | |
end | |
q = q / abs(q) | |
Quaternion(q.s, q.v1, q.v2, q.v3, true) | |
end | |
function normalizea(q::Quaternion) | |
if (q.norm) | |
return (q, one(q.s)) | |
end | |
a = abs(q) | |
q = q / a | |
(Quaternion(q.s, q.v1, q.v2, q.v3, true), a) | |
end | |
function normalizeq(q::Quaternion) | |
a = abs(q) | |
if a > 0 | |
q = q / a | |
Quaternion(q.s, q.v1, q.v2, q.v3, true) | |
else | |
Quaternion(0.0, 1.0, 0.0, 0.0, true) | |
end | |
end | |
(-)(q::Quaternion) = Quaternion(-q.s, -q.v1, -q.v2, -q.v3, q.norm) | |
(+)(q::Quaternion, w::Quaternion) = | |
Quaternion(q.s + w.s, q.v1 + w.v1, q.v2 + w.v2, q.v3 + w.v3) | |
(-)(q::Quaternion, w::Quaternion) = | |
Quaternion(q.s - w.s, q.v1 - w.v1, q.v2 - w.v2, q.v3 - w.v3) | |
(*)(q::Quaternion, w::Quaternion) = Quaternion(q.s * w.s - q.v1 * w.v1 - q.v2 * w.v2 - q.v3 * w.v3, | |
q.s * w.v1 + q.v1 * w.s + q.v2 * w.v3 - q.v3 * w.v2, | |
q.s * w.v2 - q.v1 * w.v3 + q.v2 * w.s + q.v3 * w.v1, | |
q.s * w.v3 + q.v1 * w.v2 - q.v2 * w.v1 + q.v3 * w.s, | |
q.norm && w.norm) | |
(/)(q::Quaternion, w::Quaternion) = q * inv(w) | |
angleaxis(q::Quaternion) = angle(q), axis(q) | |
angle(q::Quaternion) = 2 * atan(√(q.v1^2 + q.v2^2 + q.v3^2), q.s) | |
function axis(q::Quaternion) | |
q = normalize(q) | |
s = sin(angle(q) / 2) | |
abs(s) > 0 ? | |
[q.v1, q.v2, q.v3] / s : | |
[1.0, 0.0, 0.0] | |
end | |
argq(q::Quaternion) = normalizeq(Quaternion(0, q.v1, q.v2, q.v3)) | |
function exp(q::Quaternion) | |
s = q.s | |
se = exp(s) | |
scale = se | |
th = abs_imag(q) | |
if th > 0 | |
scale *= sin(th) / th | |
end | |
Quaternion(se * cos(th), scale * q.v1, scale * q.v2, scale * q.v3, abs(s) < eps(typeof(s))) | |
end | |
function log(q::Quaternion) | |
q, a = normalizea(q) | |
s = q.s | |
M = abs_imag(q) | |
th = atan(M, s) | |
if M > 0 | |
M = th / M | |
return Quaternion(log(a), q.v1 * M, q.v2 * M, q.v3 * M) | |
else | |
return Quaternion(log(a), th, 0.0, 0.0) | |
end | |
end | |
function sin(q::Quaternion) | |
L = argq(q) | |
return (exp(L * q) - exp(-L * q)) / (2 * L) | |
end | |
function cos(q::Quaternion) | |
L = argq(q) | |
return (exp(L * q) + exp(-L * q)) / 2 | |
end | |
(^)(q::Quaternion, w::Quaternion) = exp(w * log(q)) | |
sqrt(q::Quaternion) = exp(0.5 * log(q)) | |
function linpol(p::Quaternion, q::Quaternion, t::Real) | |
p = normalize(p) | |
q = normalize(q) | |
qm = -q | |
if abs(p - q) > abs(p - qm) | |
q = qm | |
end | |
c = p.s * q.s + p.v1 * q.v1 + p.v2 * q.v2 + p.v3 * q.v3 | |
if c > - 1.0 | |
if c < 1.0 | |
o = acos(c) | |
s = sin(o) | |
sp = sin((1 - t) * o) / s | |
sq = sin(t * o) / s | |
else | |
sp = 1 - t | |
sq = t | |
end | |
Quaternion(sp * p.s + sq * q.s, | |
sp * p.v1 + sq * q.v1, | |
sp * p.v2 + sq * q.v2, | |
sp * p.v3 + sq * q.v3, true) | |
else | |
s = p.v3 | |
v1 = -p.v2 | |
v2 = p.v1 | |
v3 = -p.s | |
sp = sin((0.5 - t) * pi) | |
sq = sin(t * pi) | |
Quaternion(s, | |
sp * p.v1 + sq * v1, | |
sp * p.v2 + sq * v2, | |
sp * p.v3 + sq * v3, true) | |
end | |
end | |
quatrand() = quat(randn(), randn(), randn(), randn()) | |
nquatrand() = normalize(quatrand()) | |
## Rotations | |
function qrotation(axis::Vector{T}, theta) where {T <: Real} | |
if length(axis) != 3 | |
error("Must be a 3-vector") | |
end | |
u = normalize(axis) | |
thetaT = convert(eltype(u), theta) | |
s = sin(thetaT / 2) | |
Quaternion(cos(thetaT / 2), s * u[1], s * u[2], s * u[3], true) | |
end | |
# Variant of the above where norm(rotvec) encodes theta | |
function qrotation(rotvec::Vector{T}) where {T <: Real} | |
if length(rotvec) != 3 | |
error("Must be a 3-vector") | |
end | |
theta = norm(rotvec) | |
if theta > 0 | |
s = sin(theta / 2) / theta # divide by theta to make rotvec a unit vector | |
return Quaternion(cos(theta / 2), s * rotvec[1], s * rotvec[2], s * rotvec[3], true) | |
end | |
Quaternion(one(T), zero(T), zero(T), zero(T), true) | |
end | |
function qrotation(dcm::Matrix{T}) where {T<:Real} | |
# See https://arxiv.org/pdf/math/0701759.pdf | |
a2 = 1 + dcm[1,1] + dcm[2,2] + dcm[3,3] | |
b2 = 1 + dcm[1,1] - dcm[2,2] - dcm[3,3] | |
c2 = 1 - dcm[1,1] + dcm[2,2] - dcm[3,3] | |
d2 = 1 - dcm[1,1] - dcm[2,2] + dcm[3,3] | |
if a2 >= max(b2, c2, d2) | |
a = sqrt(a2)/2 | |
return Quaternion(a, (dcm[3,2]-dcm[2,3])/4a, (dcm[1,3]-dcm[3,1])/4a, (dcm[2,1]-dcm[1,2])/4a) | |
elseif b2 >= max(c2, d2) | |
b = sqrt(b2)/2 | |
return Quaternion((dcm[3,2]-dcm[2,3])/4b, b, (dcm[2,1]+dcm[1,2])/4b, (dcm[1,3]+dcm[3,1])/4b) | |
elseif c2 >= d2 | |
c = sqrt(c2)/2 | |
return Quaternion((dcm[1,3]-dcm[3,1])/4c, (dcm[2,1]+dcm[1,2])/4c, c, (dcm[3,2]+dcm[2,3])/4c) | |
else | |
d = sqrt(d2)/2 | |
return Quaternion((dcm[2,1]-dcm[1,2])/4d, (dcm[1,3]+dcm[3,1])/4d, (dcm[3,2]+dcm[2,3])/4d, d) | |
end | |
end | |
function qrotation(dcm::Matrix{T}, qa::Quaternion) where {T<:Real} | |
q = qrotation(dcm) | |
abs(q-qa) < abs(q+qa) ? q : -q | |
end | |
rotationmatrix(q::Quaternion) = rotationmatrix_normalized(normalize(q)) | |
function rotationmatrix_normalized(q::Quaternion) | |
sx, sy, sz = 2q.s * q.v1, 2q.s * q.v2, 2q.s * q.v3 | |
xx, xy, xz = 2q.v1^2, 2q.v1 * q.v2, 2q.v1 * q.v3 | |
yy, yz, zz = 2q.v2^2, 2q.v2 * q.v3, 2q.v3^2 | |
[1 - (yy + zz) xy - sz xz + sy; | |
xy + sz 1 - (xx + zz) yz - sx; | |
xz - sy yz + sx 1 - (xx + yy)] | |
end | |
function normalize(v::Vector{T}) where {T} | |
nv = norm(v) | |
if nv > 0 | |
return v / nv | |
end | |
zeros(promote_type(T, typeof(nv)), length(v)) | |
end | |
function slerp(qa::Quaternion{T}, qb::Quaternion{T}, t::T) where {T} | |
# http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/slerp/ | |
coshalftheta = qa.s * qb.s + qa.v1 * qb.v1 + qa.v2 * qb.v2 + qa.v3 * qb.v3; | |
if coshalftheta < 0 | |
qm = -qb | |
coshalftheta = -coshalftheta | |
else | |
qm = qb | |
end | |
abs(coshalftheta) >= 1.0 && return qa | |
halftheta = acos(coshalftheta) | |
sinhalftheta = sqrt(one(T) - coshalftheta * coshalftheta) | |
if abs(sinhalftheta) < 0.001 | |
return Quaternion( | |
T(0.5) * (qa.s + qb.s), | |
T(0.5) * (qa.v1 + qb.v1), | |
T(0.5) * (qa.v2 + qb.v2), | |
T(0.5) * (qa.v3 + qb.v3), | |
) | |
end | |
ratio_a = sin((one(T) - t) * halftheta) / sinhalftheta | |
ratio_b = sin(t * halftheta) / sinhalftheta | |
Quaternion( | |
qa.s * ratio_a + qm.s * ratio_b, | |
qa.v1 * ratio_a + qm.v1 * ratio_b, | |
qa.v2 * ratio_a + qm.v2 * ratio_b, | |
qa.v3 * ratio_a + qm.v3 * ratio_b, | |
) | |
end |
Julia vs Python
I ran some sample code to see what the differences between the Julia and Python implementations would arise. In particular I defined a simple method which takes a quaternion, a positive integer $n$ and a mathematical function (such as $\exp, \sin, \log$, etc) as inputs and recursively calls the function on the given quaternion $n$ times. Here is the definition of the method in Python
def recursive_func(q, ntimes, func):
for i in range(ntimes):
q = func(q)
print(q)
return
and in Julia:
function recursive_func(q::Quaternion, n::Int, func)
for _ in 1:n
q = func(q)
print(q,"\n")
end
end
Math Overflow Errors
One very interesting aspect is that whereas in Python when the result of a computation exceeds the numeric bounds of the system, the program throws an exception and crashes:
recursive_func(Quaternion.random(), 10, Quaternion.exp) | |
0.401 +0.349i +0.310j +0.182k | |
1.309 +0.500i +0.443j +0.260k | |
2.792 +1.698i +1.504j +0.884k | |
-12.401 +7.391i +6.550j +3.847k | |
-0.000 -0.000i -0.000j -0.000k | |
1.000 -0.000i -0.000j -0.000k | |
2.718 -0.000i -0.000j -0.000k | |
15.154 -0.000i -0.000j -0.000k | |
3814029.270 -415.916i -368.566j -216.488k | |
--------------------------------------------------------------------------- | |
OverflowError Traceback (most recent call last) | |
<ipython-input-16-7b7585737b8a> in <module> | |
----> 1 recursive_func(Quaternion.random(), 10, Quaternion.exp) | |
<ipython-input-15-bee1b782b391> in recursive_func(q, ntimes, func) | |
1 def recursive_func(q, ntimes, func): | |
2 for i in range(ntimes): | |
----> 3 q = func(q) | |
4 print(q) | |
5 return | |
~/ownCloud/root/code/ipython-notebooks/research/julia/pyquaternion/pyquaternion/quaternion.py in exp(cls, q) | |
642 if v_norm > tolerance: | |
643 vec = vec / v_norm | |
--> 644 magnitude = exp(q.scalar) | |
645 return Quaternion(scalar = magnitude * cos(v_norm), vector = magnitude * sin(v_norm) * vec) | |
646 | |
OverflowError: math range error |
whereas in Julia it just continues as if nothing wrong had happened:
recursive_func(rand_q[1], 20, exp) | |
Quaternion{Float64}(0.5640308130993734, 0.4169392317621881, 0.4389478445487356, 0.6341786661544735, false) | |
Quaternion{Float64}(1.124340976491199, 0.6425229055599401, 0.6764392098021417, 0.9772990598642272, false) | |
Quaternion{Float64}(0.6707861642901156, 1.4286500959270507, 1.5040630203377152, 2.173025091467643, false) | |
Quaternion{Float64}(-1.9373469633419282, 0.12737241681125427, 0.1340959150760675, 0.1937377517985787, false) | |
Quaternion{Float64}(0.1389481946590188, 0.01813389606006181, 0.01909111444176842, 0.02758226892429555, false) | |
Quaternion{Float64}(1.1482292501280715, 0.020831968042115342, 0.0219316072300202, 0.0316861276174094, false) | |
Quaternion{Float64}(3.149581083449106, 0.0656539740048246, 0.06911959388823821, 0.09986191389604829, false) | |
Quaternion{Float64}(23.10433983849076, 1.5266032791987882, 1.6071867740849768, 2.3220151945344782, false) | |
Quaternion{Float64}(-1.0791074299020746e10, -3.525745663212402e8, -3.711856168471235e8, -5.362778341691994e8, false) | |
Quaternion{Float64}(-0.0, -0.0, -0.0, -0.0, false) | |
Quaternion{Float64}(1.0, -0.0, -0.0, -0.0, true) | |
Quaternion{Float64}(2.718281828459045, -0.0, -0.0, -0.0, false) | |
Quaternion{Float64}(15.154262241479262, -0.0, -0.0, -0.0, false) | |
Quaternion{Float64}(3.814279104760214e6, -0.0, -0.0, -0.0, false) | |
Quaternion{Float64}(Inf, NaN, NaN, NaN, false) | |
Quaternion{Float64}(NaN, NaN, NaN, NaN, false) | |
Quaternion{Float64}(NaN, NaN, NaN, NaN, false) | |
Quaternion{Float64}(NaN, NaN, NaN, NaN, false) | |
Quaternion{Float64}(NaN, NaN, NaN, NaN, false) | |
Quaternion{Float64}(NaN, NaN, NaN, NaN, false) |
Now, of course, this isn’t exactly a good thing. In general when your computation gives you values which are out of bounds you want it to not continue as usual. Though, arguably, the Julia behavior is better in some situations when you don’t care much about out-of-bounds errors but are more concerned about not having the program crash halfway through.
Floating Point Precision
Recursively applying the $ \log $ function to a random quaternion in Python yields the following output:
recursive_func(Quaternion.random(), 100, Quaternion.log) | |
0.000 +0.218i -0.344j -0.265k | |
-0.722 +0.706i -1.112j -0.856k | |
0.547 +0.900i -1.417j -1.090k | |
0.730 +0.586i -0.923j -0.710k | |
0.402 +0.477i -0.751j -0.578k | |
0.126 +0.543i -0.856j -0.659k | |
0.195 +0.659i -1.039j -0.799k | |
0.392 +0.647i -1.019j -0.784k | |
0.400 +0.586i -0.924j -0.711k | |
0.311 +0.572i -0.902j -0.694k | |
0.271 +0.598i -0.943j -0.725k | |
0.307 +0.616i -0.970j -0.746k | |
0.339 +0.607i -0.956j -0.736k | |
0.331 +0.595i -0.938j -0.722k | |
0.311 +0.596i -0.939j -0.722k | |
0.309 +0.602i -0.949j -0.730k | |
0.319 +0.604i -0.952j -0.732k | |
0.323 +0.601i -0.947j -0.729k | |
0.320 +0.599i -0.944j -0.727k | |
0.316 +0.600i -0.946j -0.728k | |
0.317 +0.602i -0.948j -0.729k | |
0.319 +0.602i -0.948j -0.729k | |
0.319 +0.601i -0.947j -0.728k | |
0.318 +0.601i -0.946j -0.728k | |
0.318 +0.601i -0.947j -0.728k | |
0.318 +0.601i -0.947j -0.729k | |
0.318 +0.601i -0.947j -0.729k | |
0.318 +0.601i -0.947j -0.728k | |
0.318 +0.601i -0.947j -0.728k | |
0.318 +0.601i -0.947j -0.729k | |
0.318 +0.601i -0.947j -0.729k | |
0.318 +0.601i -0.947j -0.729k | |
0.318 +0.601i -0.947j -0.729k | |
0.318 +0.601i -0.947j -0.729k |
Note the limited floating point precision of the output and also that sometime into the calculation the output seems to stabilize around a fixed point. Let’s look at the same function applied to the same quaternion input value in Julia:
recursive_func(q, 100, log) | |
Quaternion{Float64}(0.0, 0.2138427021361156, 0.6498259226581785, 0.04673029529787405, false) | |
Quaternion{Float64}(-0.37731329782035117, 0.489868364488579, 1.4886136340169336, 0.10704921468429084, false) | |
Quaternion{Float64}(0.4796303624933924, 0.5633858732517517, 1.7120188869731483, 0.12311473789246809, false) | |
Quaternion{Float64}(0.6254684847358931, 0.40893747680176057, 1.242680580247547, 0.0893636717943734, false) | |
Quaternion{Float64}(0.3735054584912868, 0.35106899793994956, 1.066829652001861, 0.07671787617620018, false) | |
Quaternion{Float64}(0.1706470491382181, 0.3899604970562924, 1.1850132703548995, 0.08521669900311565, false) | |
Quaternion{Float64}(0.2327178376780377, 0.44757009502094197, 1.360077510459367, 0.09780592228727658, false) | |
Quaternion{Float64}(0.37425627242318393, 0.4397353421632598, 1.3362692371178646, 0.09609382123838361, false) | |
Quaternion{Float64}(0.3776581104899513, 0.40895937970377383, 1.2427471388598696, 0.08936845815869039, false) | |
Quaternion{Float64}(0.31090093152853704, 0.40242197459078016, 1.2228812502094477, 0.0879398619598939, false) | |
Quaternion{Float64}(0.2831608568053628, 0.4161356905635626, 1.264554536443051, 0.09093667218833638, false) | |
Quaternion{Float64}(0.3104808513233154, 0.42465717422354976, 1.2904496496568891, 0.09279884210963556, false) | |
Quaternion{Float64}(0.3340695336036758, 0.41995607134023355, 1.2761639223996764, 0.09177152659328365, false) | |
Quaternion{Float64}(0.32745828905964813, 0.41403302039710715, 1.2581649352674498, 0.09047718305538509, false) | |
Quaternion{Float64}(0.3129203889966859, 0.4144534937312651, 1.25944267104984, 0.09056906761760679, false) | |
Quaternion{Float64}(0.311386355942643, 0.4177513693201955, 1.2694642664841538, 0.09128974079741105, false) | |
Quaternion{Float64}(0.3186456968788053, 0.41863957621914916, 1.2721633525967888, 0.09148383753421385, false) | |
Quaternion{Float64}(0.3218637131935187, 0.41718708043753666, 1.2677495035291564, 0.09116642872804162, false) | |
Quaternion{Float64}(0.3191187911313047, 0.41623360609777327, 1.2648520824978169, 0.09095806932643001, false) | |
Quaternion{Float64}(0.3164892498533091, 0.4166775584788587, 1.2662011664869983, 0.09105508468238596, false) | |
Quaternion{Float64}(0.317054633113361, 0.41733360761104704, 1.2681947707969645, 0.09119844879708726, false) | |
Quaternion{Float64}(0.31863911536780903, 0.41731880617627404, 1.2681497921471816, 0.09119521429148635, false) | |
Quaternion{Float64}(0.31887176042403637, 0.41696678027062145, 1.2670800546408032, 0.09111828730562561, false) | |
Quaternion{Float64}(0.31811243187512667, 0.4168561076998873, 1.266743742460533, 0.09109410241710653, false) | |
Quaternion{Float64}(0.31773318001482903, 0.4170050654871516, 1.2671963958855939, 0.09112665363964648, false) | |
Quaternion{Float64}(0.31800748141371743, 0.4171138753649327, 1.2675270477082519, 0.09115043148041094, false) | |
Quaternion{Float64}(0.3183005734059548, 0.4170716351432334, 1.267398687980448, 0.09114120087298903, false) | |
Quaternion{Float64}(0.31825406326657096, 0.4169998423095536, 1.2671805236759373, 0.09112551223697474, false) | |
Quaternion{Float64}(0.3180833078791223, 0.4169980145490542, 1.2671749694711827, 0.09112511282288661, false) | |
Quaternion{Float64}(0.3180504013681607, 0.41703539792424593, 1.2672885701974799, 0.09113328208068446, false) | |
Quaternion{Float64}(0.318129705801897, 0.41704895514151064, 1.2673297679149127, 0.09113624469179937, false) | |
Quaternion{Float64}(0.3181738237438287, 0.4170337335864272, 1.2672835126021171, 0.09113291837878128, false) | |
Quaternion{Float64}(0.31814670852856086, 0.4170214332012489, 1.2672461341502566, 0.0911302304187718, false) | |
Quaternion{Float64}(0.3181142274023023, 0.41702534676476843, 1.2672580267002456, 0.09113108563607455, false) | |
Quaternion{Float64}(0.31811764001151954, 0.41703317508020965, 1.2672818154115442, 0.09113279633037741, false) | |
Quaternion{Float64}(0.3181359807845624, 0.4170337398944662, 1.267283531771007, 0.09113291975725488, false) | |
Quaternion{Float64}(0.3181403506683638, 0.41702978683721686, 1.267271519207339, 0.09113205590952593, false) | |
Quaternion{Float64}(0.31813211519911805, 0.4170281567121847, 1.2672665655779072, 0.09113169968402297, false) | |
Quaternion{Float64}(0.31812702900406814, 0.41702969996821443, 1.2672712552295222, 0.09113203692634768, false) | |
Quaternion{Float64}(0.3181296749814361, 0.4170310824455512, 1.267275456306304, 0.09113233903426882, false) | |
Quaternion{Float64}(0.3181332579789174, 0.41703073119894324, 1.2672743889369302, 0.09113226227758014, false) | |
Quaternion{Float64}(0.31813306412936865, 0.41702988121903445, 1.2672718060151238, 0.09113207653445336, false) | |
Quaternion{Float64}(0.31813110249399523, 0.41702978088837955, 1.267271501129992, 0.09113205460954742, false) | |
Quaternion{Float64}(0.3181305445031431, 0.41703019696768384, 1.2672727655131815, 0.0911321455338977, false) | |
Quaternion{Float64}(0.3181313948275616, 0.4170303901854323, 1.2672733526639255, 0.09113218775709422, false) | |
Quaternion{Float64}(0.3181319765021371, 0.417030235034959, 1.2672728811921452, 0.09113215385260572, false) | |
Quaternion{Float64}(0.31813172233439296, 0.4170300805240387, 1.2672724116638407, 0.09113212008787651, false) | |
Quaternion{Float64}(0.31813132888185675, 0.41703011060795825, 1.2672725030829564, 0.09113212666200986, false) | |
Quaternion{Float64}(0.31813133090804147, 0.41703020251629513, 1.2672727823743195, 0.09113214674641619, false) | |
Quaternion{Float64}(0.31813153983170894, 0.417030217544262, 1.2672728280413559, 0.09113215003042506, false) | |
Quaternion{Float64}(0.3181316091149064, 0.41703017396095676, 1.2672726956002613, 0.09113214050631831, false) | |
Quaternion{Float64}(0.3181315218699095, 0.4170301513303609, 1.2672726268303305, 0.09113213556093362, false) | |
Quaternion{Float64}(0.31813145582063, 0.41703016677663046, 1.2672726737685063, 0.0911321389363527, false) | |
Quaternion{Float64}(0.31813147975421563, 0.4170301839558242, 1.2672727259726981, 0.09113214269046165, false) | |
Quaternion{Float64}(0.318131522771595, 0.4170301815657699, 1.2672727187097932, 0.09113214216817148, false) | |
Quaternion{Float64}(0.3181315245905677, 0.41703017166853923, 1.2672726886340555, 0.09113214000536439, false) | |
Quaternion{Float64}(0.31813150243545035, 0.41703016960059547, 1.267272682349981, 0.09113213955346389, false) | |
Quaternion{Float64}(0.3181314940119317, 0.41703017414248317, 1.2672726961518845, 0.09113214054598666, false) | |
Quaternion{Float64}(0.31813150290125214, 0.41703017676647003, 1.2672727041256646, 0.0911321411193973, false) | |
Quaternion{Float64}(0.3181315103530424, 0.41703017524623537, 1.2672726995059704, 0.09113214078718575, false) | |
Quaternion{Float64}(0.31813150815763297, 0.4170301733455036, 1.2672726937300203, 0.09113214037182552, false) | |
Quaternion{Float64}(0.31813150347433916, 0.4170301735100363, 1.2672726942300028, 0.09113214040778027, false) | |
Quaternion{Float64}(0.31813150305918386, 0.41703017457143704, 1.267272697455391, 0.09113214063972445, false) | |
Quaternion{Float64}(0.3181315053980903, 0.417030174841785, 1.2672726982769253, 0.09113214069880264, false) | |
Quaternion{Float64}(0.31813150640545146, 0.4170301743710613, 1.2672726968464887, 0.09113214059593705, false) | |
Quaternion{Float64}(0.31813150550677766, 0.41703017406945775, 1.2672726959299747, 0.09113214053002866, false) | |
Quaternion{Float64}(0.3181315046709843, 0.41703017421703026, 1.2672726963784184, 0.09113214056227716, false) | |
Quaternion{Float64}(0.3181315048651666, 0.4170301744263544, 1.2672726970145136, 0.09113214060802004, false) | |
Quaternion{Float64}(0.3181315053729158, 0.41703017441873963, 1.2672726969913737, 0.091132140606356, false) | |
Quaternion{Float64}(0.3181315054411271, 0.41703017430538714, 1.2672726966469177, 0.09113214058158547, false) | |
Quaternion{Float64}(0.31813150519536304, 0.41703017427124556, 1.2672726965431682, 0.09113214057412464, false) | |
Quaternion{Float64}(0.31813150507649923, 0.41703017431974204, 1.2672726966905394, 0.09113214058472241, false) | |
Quaternion{Float64}(0.31813150516654626, 0.41703017435414336, 1.2672726967950783, 0.09113214059224001, false) | |
Quaternion{Float64}(0.3181315052597806, 0.41703017434006046, 1.2672726967522832, 0.09113214058916252, false) | |
Quaternion{Float64}(0.3181315052435185, 0.4170301743171106, 1.267272696682543, 0.09113214058414737, false) | |
Quaternion{Float64}(0.3181315051886964, 0.41703017431683576, 1.2672726966817078, 0.0911321405840873, false) | |
Quaternion{Float64}(0.318131505178842, 0.4170301743288898, 1.2672726967183376, 0.09113214058672142, false) | |
Quaternion{Float64}(0.3181315052045388, 0.4170301743330944, 1.267272696731115, 0.09113214058764026, false) | |
Quaternion{Float64}(0.31813150521840805, 0.4170301743281306, 1.267272696716031, 0.09113214058655554, false) | |
Quaternion{Float64}(0.3181315052094782, 0.4170301743242335, 1.2672726967041885, 0.09113214058570393, false) | |
Quaternion{Float64}(0.3181315051991303, 0.41703017432554834, 1.267272696708184, 0.09113214058599126, false) | |
Quaternion{Float64}(0.3181315052003719, 0.4170301743280538, 1.2672726967157975, 0.09113214058653876, false) | |
Quaternion{Float64}(0.31813150520626693, 0.4170301743282016, 1.2672726967162464, 0.09113214058657107, false) | |
Quaternion{Float64}(0.3181315052075948, 0.41703017432692535, 1.2672726967123682, 0.09113214058629218, false) | |
Quaternion{Float64}(0.31813150520492195, 0.41703017432641737, 1.2672726967108245, 0.09113214058618119, false) | |
Quaternion{Float64}(0.3181315052033192, 0.41703017432692185, 1.2672726967123575, 0.09113214058629142, false) | |
Quaternion{Float64}(0.3181315052041941, 0.4170301743273605, 1.2672726967136905, 0.09113214058638727, false) | |
Quaternion{Float64}(0.31813150520533695, 0.41703017432724127, 1.267272696713328, 0.09113214058636122, false) | |
Quaternion{Float64}(0.3181315052052588, 0.417030174326969, 1.2672726967125005, 0.09113214058630172, false) | |
Quaternion{Float64}(0.31813150520462746, 0.41703017432694034, 1.2672726967124135, 0.09113214058629546, false) | |
Quaternion{Float64}(0.31813150520445627, 0.4170301743270749, 1.2672726967128223, 0.09113214058632485, false) | |
Quaternion{Float64}(0.31813150520473266, 0.4170301743271354, 1.2672726967130061, 0.09113214058633808, false) | |
Quaternion{Float64}(0.3181315052049165, 0.41703017432708456, 1.2672726967128516, 0.09113214058632696, false) | |
Quaternion{Float64}(0.31813150520483213, 0.4170301743270355, 1.2672726967127024, 0.09113214058631623, false) |
By default, Julia uses
Float64
types for floating point calculations, though that can be adjusted according to need. It is clear that Julia is working at a much higher floating point precision than the equivalent Python code. Even though the output appears to stabilize in Julia too, one can see that it doesn’t exactly reach a fixed point because of the floating point accuracy in Julia.
Runtimes
The computations above in Python and Julia took ~ 30 ms and 141 ms respectively. It would appear that the Python code is three times faster than the Julia code. Then again, this could be just because of the reduced precision arithmetic in the Python computation.
Conclusion
Julia does indeed have deep structural advantages over Python as a fast scriptable language for numerical computation. The ability to easily define custom types, nearly arbitrary precision code built into the core and speed (allegedly) are some of the features which distinguish Julia from Python. That being said, with the results shown above it is hard to unambiguously agree with the premise laid out in the introduction that Julia is inarguably better and faster than Python and other languages for numerical computation.
In a follow-up post on “Dual Numbers and Automatic Differentiation in Julia” I will cover an application of Julia which, to the best of my knowledge, has no parallel in Python and which, I hope, will conclusively demonstrate Julia’s technical superiority over Python.
Of course, Python is not going anywhere anytime soon. From statistical physics we know that states which cost the lowest in energy have the greatest chance of being occupied. So it is in life and coding. It requires a far lower expenditure of time and energy to learn Python than it does for Julia. As long as this aspect does not change, regardless of its overwhelming technical advantages, Julia will remain a niche language used mostly for numerical computing and data analysis while Python continues to be used for the vast majority of non-numerical coding applications.
-
Note to self. In Markdown, one can easily add links to other sections of the document. For example the section Julia can be linked to using the code:
[Julia](#in-julia)
. In addition you need to add a header id as explained in the online Markdown extra reference. For instance, here the code for doing so is:# In Julia {#in-julia}
. The part in curly braces is the header id which is used for hot-linking. ↩ -
Sadly, for the quaternions we will no longer be able to use the symbol $\imath$ for the unit imaginary and have to go back to the lowly $ i$. The reason being, that while $\LaTeX$ provides for
\imath
($\imath$) and\jmath
($\jmath$) commands, there is no\kmath
command. So, if we don’t want one of our imaginary letters to look out of whack with the other two we have to drop the fancy lettering. ↩