/src/utility/transformations.c

/* transformations.c

A Python C extension module for homogeneous transformation matrices and
quaternions.

Refer to the transformations.py module for documentation and tests.

:Author:
  `Christoph Gohlke <http://www.lfd.uci.edu/~gohlke/>`_

:Organization:
  Laboratory for Fluorescence Dynamics, University of California, Irvine

:Version: 2013.04.16

Install
-------
Use this Python distutils setup script to build the extension module::

  # setup.py
  # Usage: ``python setup.py build_ext --inplace``
  from distutils.core import setup, Extension
  import numpy
  setup(name='_transformations',
        ext_modules=[Extension('_transformations', ['transformations.c'],
                               include_dirs=[numpy.get_include()])])

License
-------
Copyright (c) 2007-2013, Christoph Gohlke
Copyright (c) 2007-2013, The Regents of the University of California
Produced at the Laboratory for Fluorescence Dynamics
All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:

* Redistributions of source code must retain the above copyright
  notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
  notice, this list of conditions and the following disclaimer in the
  documentation and/or other materials provided with the distribution.
* Neither the name of the copyright holders nor the names of any
  contributors may be used to endorse or promote products derived
  from this software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
POSSIBILITY OF SUCH DAMAGE.
*/

#define _VERSION_ "2013.04.16"

#define WIN32_LEAN_AND_MEAN

#include "Python.h"

#ifdef _WIN32
#include <windows.h>
#include <wincrypt.h>
#endif

#include "structmember.h"
#include "math.h"
#include "float.h"
#include "string.h"
#include "numpy/arrayobject.h"

#define EPSILON 8.8817841970012523e-016 /* 4.0 * DBL_EPSILON */
#define PIVOT_TOLERANCE 1.0e-14
#define DEG2RAD 0.017453292519943295769236907684886
#define TWOPI 6.283185307179586476925286766559
#ifndef M_PI
#define M_PI 3.1415926535897932384626433832795
#endif

#ifndef MAX
#define MAX(a, b) (((a) > (b)) ? (a) : (b))
#define MIN(a, b) (((a) < (b)) ? (a) : (b))
#endif

#define ISZERO(x) (((x) < EPSILON) && ((x) > -EPSILON))
#define NOTZERO(x) (((x) > EPSILON) || ((x) < -EPSILON))

/*****************************************************************************/
/* C helper functions */

/*
Return random doubles in half-open interval [0.0, 1.0).
Uses /dev/urandom or CryptoAPI. Not very fast but random.
Assumes sizeof(double) == 2*sizeof(int).
*/
int random_doubles(
    double *buffer,
    Py_ssize_t size)
{
#ifndef _WIN32
    int done;
    FILE *rfile;
    if (size < 1)
        return 0;
    rfile = fopen("/dev/urandom", "rb");
    if (rfile == NULL)
        return -1;
    done = fread(buffer, size*sizeof(double), 1, rfile);
    fclose(rfile);
#else
#pragma comment(lib,"advapi32")
    BOOL done;
    HCRYPTPROV hcryptprov;
    if (size < 1)
        return 0;
    if (!CryptAcquireContext(&hcryptprov, NULL, NULL, PROV_RSA_FULL,
                             CRYPT_VERIFYCONTEXT) || !hcryptprov)
        return -1;
    done = CryptGenRandom(hcryptprov, (DWORD)(size*sizeof(double)),
                          (unsigned char *)buffer);
    CryptReleaseContext(hcryptprov, 0);
#endif
    if (done) {
        unsigned int a, b;
        unsigned int *p = (unsigned int *)buffer;
        while (size--) {
            a = (*p++) >> 5;
            b = (*p++) >> 6;
            *buffer++ = (a * 67108864.0 + b) / 9007199254740992.0;
        }
        return 0;
    }
    return -1;
}

/*
Tridiagonal matrix from symmetric 4x4 matrix using Housholder reduction.
The input matrix is altered.
*/
int tridiagonalize_symmetric_44(
    double *matrix,      /* double[16] */
    double *diagonal,    /* double[4] */
    double *subdiagonal) /* double[3] */
{
    double t, n, g, h, v0, v1, v2;
    double *u;
    double *M = matrix;

    u = &M[1];
    t = u[1]*u[1] + u[2]*u[2];
    n = sqrt(u[0]*u[0] + t);
    if (n > EPSILON) {
        if (u[0] < 0.0)
            n = -n;
        u[0] += n;
        h = (u[0]*u[0] + t) / 2.0;
        v0 = M[5]*u[0] + M[6]*u[1]  + M[7]*u[2];
        v1 = M[6]*u[0] + M[10]*u[1] + M[11]*u[2];
        v2 = M[7]*u[0] + M[11]*u[1] + M[15]*u[2];
        v0 /= h;
        v1 /= h;
        v2 /= h;
        g = (u[0]*v0 + u[1]*v1 + u[2]*v2) / (2.0 * h);
        v0 -= g * u[0];
        v1 -= g * u[1];
        v2 -= g * u[2];
        M[5] -=  2.0*v0*u[0];
        M[10] -= 2.0*v1*u[1];
        M[15] -= 2.0*v2*u[2];
        M[6]  -= v1*u[0] + v0*u[1];
        M[7]  -= v2*u[0] + v0*u[2];
        M[11] -= v2*u[1] + v1*u[2];
        M[1] = -n;
    }

    u = &M[6];
    t = u[1]*u[1];
    n = sqrt(u[0]*u[0] + t);
    if (n > EPSILON) {
        if (u[0] < 0.0)
            n = -n;
        u[0] += n;
        h = (u[0]*u[0] + t) / 2.0;
        v0 = M[10]*u[0] + M[11]*u[1];
        v1 = M[11]*u[0] + M[15]*u[1];
        v0 /= h;
        v1 /= h;
        g = (u[0]*v0 + u[1]*v1) / (2.0 * h);
        v0 -= g * u[0];
        v1 -= g * u[1];
        M[10] -= 2.0*v0*u[0];
        M[15] -= 2.0*v1*u[1];
        M[11] -= v1*u[0] + v0*u[1];
        M[6] = -n;
    }

    diagonal[0] = M[0];
    diagonal[1] = M[5];
    diagonal[2] = M[10];
    diagonal[3] = M[15];
    subdiagonal[0] = M[1];
    subdiagonal[1] = M[6];
    subdiagonal[2] = M[11];

    return 0;
}

/*
Return largest eigenvalue of symmetric tridiagonal matrix.
Matrix Algorithms: Basic decompositions. By GW Stewart. Chapter 3.
*/
double max_eigenvalue_of_tridiag_44(
    double *diagonal,    /* double[4] */
    double *subdiagonal) /* double[3] */
{
    int count;
    double lower, upper, t0, t1, d, eps, eigenv;
    double *a = diagonal;
    double *b = subdiagonal;

    /* upper and lower bounds using Gerschgorin's theorem */
    t0 = fabs(b[0]);
    t1 = fabs(b[1]);
    lower = a[0] - t0;
    upper = a[0] + t0;
    d = a[1] - t0 - t1;
    lower = MIN(lower, d);
    d = a[1] + t0 + t1;
    upper = MAX(upper, d);
    t0 = fabs(b[2]);
    d = a[2] - t0 - t1;
    lower = MIN(lower, d);
    d = a[2] + t0 + t1;
    upper = MAX(upper, d);
    d = a[3] - t0;
    lower = MIN(lower, d);
    d = a[3] + t0;
    upper = MAX(upper, d);

    /* precision */
    eps = (4.0 * (fabs(lower) + fabs(upper))) * DBL_EPSILON;

    /* interval bisection until width is less than tolerance */
    while (fabs(upper - lower) > eps) {

        eigenv = (upper + lower) / 2.0;
        if ((eigenv == upper) || (eigenv == lower))
            return eigenv;

        /* counting pivots < 0 */
        d = a[0] - eigenv;
        count = (d < 0.0) ? 1 : 0;
        if (fabs(d) < eps)
            d = eps;
        d = a[1] - eigenv - b[0]*b[0] / d;
        if (d < 0.0)
            count++;
        if (fabs(d) < eps)
            d = eps;
        d = a[2] - eigenv - b[1]*b[1] / d;
        if (d < 0.0)
            count++;
        if (fabs(d) < eps)
            d = eps;
        d = a[3] - eigenv - b[2]*b[2] / d;
        if (d < 0.0)
            count++;

        if (count < 4)
            lower = eigenv;
        else
            upper = eigenv;
    }

    return (upper + lower) / 2.0;
}

/*
Eigenvector of symmetric tridiagonal 4x4 matrix using Cramer's rule.
*/
int eigenvector_of_symmetric_44(
    double *matrix, /* double[16] */
    double *vector, /* double[4]  */
    double *buffer) /* double[12] */
{
    double n, eps;
    double *M = matrix;
    double *v = vector;
    double *t = buffer;

    /* eps: minimum length of eigenvector to use */
    eps = (M[0]*M[5]*M[10]*M[15] - M[1]*M[1]*M[11]*M[11]) * 1e-6;
    eps *= eps;
    if (eps < EPSILON)
        eps = EPSILON;

    t[0] = M[10] * M[15];
    t[1] = M[11] * M[11];
    t[2] = M[6] *  M[15];
    t[3] = M[11] * M[7];
    t[4] = M[6] *  M[11];
    t[5] = M[10] * M[7];
    t[6] = M[2] *  M[15];
    t[7] = M[11] * M[3];
    t[8] = M[2] *  M[11];
    t[9] = M[10] * M[3];
    t[10] = M[2] * M[7];
    t[11] = M[6] * M[3];

    v[0] =  t[1]*M[1] + t[6]*M[6] + t[9]*M[7];
    v[0] -= t[0]*M[1] + t[7]*M[6] + t[8]*M[7];
    v[1] =  t[2]*M[1] + t[7]*M[5] + t[10]*M[7];
    v[1] -= t[3]*M[1] + t[6]*M[5] + t[11]*M[7];
    v[2] =  t[5]*M[1] + t[8]*M[5] + t[11]*M[6];
    v[2] -= t[4]*M[1] + t[9]*M[5] + t[10]*M[6];
    v[3] =  t[0]*M[5] + t[3]*M[6] + t[4]*M[7];
    v[3] -= t[1]*M[5] + t[2]*M[6] + t[5]*M[7];
    n = v[0]*v[0] + v[1]*v[1] + v[2]*v[2] + v[3]*v[3];

    if (n < eps) {
        v[0] =  t[0]*M[0] + t[7]*M[2] + t[8]*M[3];
        v[0] -= t[1]*M[0] + t[6]*M[2] + t[9]*M[3];
        v[1] =  t[3]*M[0] + t[6]*M[1] + t[11]*M[3];
        v[1] -= t[2]*M[0] + t[7]*M[1] + t[10]*M[3];
        v[2] =  t[4]*M[0] + t[9]*M[1] + t[10]*M[2];
        v[2] -= t[5]*M[0] + t[8]*M[1] + t[11]*M[2];
        v[3] =  t[1]*M[1] + t[2]*M[2] + t[5]*M[3];
        v[3] -= t[0]*M[1] + t[3]*M[2] + t[4]*M[3];
        n = v[0]*v[0] + v[1]*v[1] + v[2]*v[2] + v[3]*v[3];
    }

    if (n < eps) {
        t[0]  = M[2] * M[7];
        t[1]  = M[3] * M[6];
        t[2]  = M[1] * M[7];
        t[3]  = M[3] * M[5];
        t[4]  = M[1] * M[6];
        t[5]  = M[2] * M[5];
        t[6]  = M[0] * M[7];
        t[7]  = M[3] * M[1];
        t[8]  = M[0] * M[6];
        t[9]  = M[2] * M[1];
        t[10] = M[0] * M[5];
        t[11] = M[1] * M[1];

        v[0] =  t[1]*M[3] + t[6]*M[11] + t[9]*M[15];
        v[0] -= t[0]*M[3] + t[7]*M[11] + t[8]*M[15];
        v[1] =  t[2]*M[3] + t[7]*M[7] + t[10]*M[15];
        v[1] -= t[3]*M[3] + t[6]*M[7] + t[11]*M[15];
        v[2] =  t[5]*M[3] + t[8]*M[7] + t[11]*M[11];
        v[2] -= t[4]*M[3] + t[9]*M[7] + t[10]*M[11];
        v[3] =  t[0]*M[7] + t[3]*M[11] + t[4]*M[15];
        v[3] -= t[1]*M[7] + t[2]*M[11] + t[5]*M[15];
        n = v[0]*v[0] + v[1]*v[1] + v[2]*v[2] + v[3]*v[3];
    }

    if (n < eps) {
        v[0] =  t[8]*M[11] + t[0]*M[2] + t[7]*M[10];
        v[0] -= t[6]*M[10] + t[9]*M[11] + t[1]*M[2];
        v[1] =  t[6]*M[6] + t[11]*M[11] + t[3]*M[2];
        v[1] -= t[10]*M[11] + t[2]*M[2] + t[7]*M[6];
        v[2] =  t[10]*M[10] + t[4]*M[2] + t[9]*M[6];
        v[2] -= t[8]*M[6] + t[11]*M[10] + t[5]*M[2];
        v[3] =  t[2]*M[10] + t[5]*M[11] + t[1]*M[6];
        v[3] -= t[4]*M[11] + t[0]*M[6] + t[3]*M[10];
        n = v[0]*v[0] + v[1]*v[1] + v[2]*v[2] + v[3]*v[3];
    }

    if (n < eps)
        return -1;

    n = sqrt(n);
    v[0] /= n;
    v[1] /= n;
    v[2] /= n;
    v[3] /= n;

    return 0;
}

/*
Matrix 2x2 inversion.
*/
int invert_matrix22(
    double *matrix, /* double[4] */
    double *result) /* double[4] */
{
    double det = matrix[0]*matrix[3] - matrix[1]*matrix[2];

    if (ISZERO(det))
        return -1;

    result[0] = matrix[3] / det;
    result[1] = -matrix[1] / det;
    result[2] = -matrix[2] / det;
    result[3] = matrix[0] / det;

    return 0;
}

/*
Matrix 3x3 inversion.
*/
int invert_matrix33(
    double *matrix, /* double[9] */
    double *result) /* double[9] */
{
    int i;
    double det;
    double *M = matrix;

    result[0] = M[8]*M[4] - M[7]*M[5];
    result[1] = M[7]*M[2] - M[8]*M[1];
    result[2] = M[5]*M[1] - M[4]*M[2];
    result[3] = M[6]*M[5] - M[8]*M[3];
    result[4] = M[8]*M[0] - M[6]*M[2];
    result[5] = M[3]*M[2] - M[5]*M[0];
    result[6] = M[7]*M[3] - M[6]*M[4];
    result[7] = M[6]*M[1] - M[7]*M[0];
    result[8] = M[4]*M[0] - M[3]*M[1];

    det = M[0]*result[0] + M[3]*result[1] + M[6]*result[2];

    if (ISZERO(det))
        return -1;

    det = 1.0 / det;
    for (i = 0; i < 9; i++)
        result[i] *= det;

    return 0;
}

/*
Matrix 4x4 inversion.
*/
int invert_matrix44(
    double *matrix, /* double[16] */
    double *result) /* double[16] */
{
    int i;
    double det;
    double t[12];
    double *M = matrix;

    t[0] = M[10] * M[15];
    t[1] = M[14] * M[11];
    t[2] = M[6] * M[15];
    t[3] = M[14] * M[7];
    t[4] = M[6] * M[11];
    t[5] = M[10] * M[7];
    t[6] = M[2] * M[15];
    t[7] = M[14] * M[3];
    t[8] = M[2] * M[11];
    t[9] = M[10] * M[3];
    t[10] = M[2] * M[7];
    t[11] = M[6] * M[3];

    result[0] = t[0]*M[5] + t[3]*M[9] + t[4]*M[13];
    result[0] -= t[1]*M[5] + t[2]*M[9] + t[5]*M[13];
    result[1] = t[1]*M[1] + t[6]*M[9] + t[9]*M[13];
    result[1] -= t[0]*M[1] + t[7]*M[9] + t[8]*M[13];
    result[2] = t[2]*M[1] + t[7]*M[5] + t[10]*M[13];
    result[2] -= t[3]*M[1] + t[6]*M[5] + t[11]*M[13];
    result[3] = t[5]*M[1] + t[8]*M[5] + t[11]*M[9];
    result[3] -= t[4]*M[1] + t[9]*M[5] + t[10]*M[9];
    result[4] = t[1]*M[4] + t[2]*M[8] + t[5]*M[12];
    result[4] -= t[0]*M[4] + t[3]*M[8] + t[4]*M[12];
    result[5] = t[0]*M[0] + t[7]*M[8] + t[8]*M[12];
    result[5] -= t[1]*M[0] + t[6]*M[8] + t[9]*M[12];
    result[6] = t[3]*M[0] + t[6]*M[4] + t[11]*M[12];
    result[6] -= t[2]*M[0] + t[7]*M[4] + t[10]*M[12];
    result[7] = t[4]*M[0] + t[9]*M[4] + t[10]*M[8];
    result[7] -= t[5]*M[0] + t[8]*M[4] + t[11]*M[8];

    t[0] = M[8]*M[13];
    t[1] = M[12]*M[9];
    t[2] = M[4]*M[13];
    t[3] = M[12]*M[5];
    t[4] = M[4]*M[9];
    t[5] = M[8]*M[5];
    t[6] = M[0]*M[13];
    t[7] = M[12]*M[1];
    t[8] = M[0]*M[9];
    t[9] = M[8]*M[1];
    t[10] = M[0]*M[5];
    t[11] = M[4]*M[1];

    result[8] = t[0]*M[7] + t[3]*M[11] + t[4]*M[15];
    result[8] -= t[1]*M[7] + t[2]*M[11] + t[5]*M[15];
    result[9] = t[1]*M[3] + t[6]*M[11] + t[9]*M[15];
    result[9] -= t[0]*M[3] + t[7]*M[11] + t[8]*M[15];
    result[10] = t[2]*M[3] + t[7]*M[7] + t[10]*M[15];
    result[10]-= t[3]*M[3] + t[6]*M[7] + t[11]*M[15];
    result[11] = t[5]*M[3] + t[8]*M[7] + t[11]*M[11];
    result[11]-= t[4]*M[3] + t[9]*M[7] + t[10]*M[11];
    result[12] = t[2]*M[10] + t[5]*M[14] + t[1]*M[6];
    result[12]-= t[4]*M[14] + t[0]*M[6] + t[3]*M[10];
    result[13] = t[8]*M[14] + t[0]*M[2] + t[7]*M[10];
    result[13]-= t[6]*M[10] + t[9]*M[14] + t[1]*M[2];
    result[14] = t[6]*M[6] + t[11]*M[14] + t[3]*M[2];
    result[14]-= t[10]*M[14] + t[2]*M[2] + t[7]*M[6];
    result[15] = t[10]*M[10] + t[4]*M[2] + t[9]*M[6];
    result[15]-= t[8]*M[6] + t[11]*M[10] + t[5]*M[2];

    det = M[0]*result[0] + M[4]*result[1] + M[8]*result[2] + M[12]*result[3];

    if (ISZERO(det))
        return -1;

    det = 1.0 / det;
    for (i = 0; i < 16; i++)
        result[i] *= det;

    return 0;
}

/*
Invert square matrix using LU factorization with pivoting.
The input matrix is altered.
*/
int invert_matrix(
    Py_ssize_t size,
    double *matrix,     /* [size*size] */
    double *result,     /* [size*size] */
    Py_ssize_t *buffer) /* [2*size] */
{
    Py_ssize_t *seq = buffer;
    Py_ssize_t *iseq = buffer + size;
    Py_ssize_t i, j, k, ks, ps, ksk, js, p, is;
    double temp, temp1;
    double *M = matrix;

    /* sequence of pivots */
    for (k = 0; k < size; k++)
        seq[k] = k;

    /* forward solution */
    for (k = 0; k < size-1; k++) {
        ks = k*size;
        ksk = ks + k;

        /* pivoting: find maximum coefficient in column */
        p = k;
        temp = fabs(M[ksk]);
        for (i = k+1; i < size; i++) {
            temp1 = fabs(M[i*size + k]);
            if (temp < temp1) {
                temp = temp1;
                p = i;
            }
        }
        /* permutate lines with index k and p */
        if (p != k) {
            ps = p*size;
            for (i = 0; i < size; i++) {
                temp = M[ks + i];
                M[ks + i] = M[ps + i];
                M[ps + i] = temp;
            }
            i = seq[k];
            seq[k] = seq[p];
            seq[p] = i;
        }

        /* test for singular matrix */
        if (fabs(M[ksk]) < PIVOT_TOLERANCE)
            return -1;

        /* elimination */
        temp = M[ksk];
        for (j = k+1; j < size; j++) {
            M[j*size + k] /= temp;
        }
        for (j = k+1; j < size; j++) {
            js = j * size;
            temp = M[js + k];
            for(i = k+1; i < size; i++) {
                M[js + i] -= temp * M[ks + i];
            }
            M[js + k] = temp;
        }
    }

    /* Backward substitution with identity matrix */
    memset(result, 0, size*size*sizeof(double));
    for (i = 0; i < size; i++) {
        result[i*size + seq[i]] = 1.0;
        iseq[seq[i]] = i;
    }

    for (i = 0; i < size; i++) {
        is = iseq[i];
        for (k = 1; k < size; k++) {
            ks = k*size;
            temp = 0.0;
            for (j = is; j < k; j++)
                temp += M[ks + j] * result[j*size + i];
            result[ks + i] -= temp;
        }
        for (k = size-1; k >= 0; k--) {
            ks = k*size;
            temp = result[ks + i];
            for (j = k+1; j < size; j++)
                temp -= M[ks + j] * result[j*size + i];
            result[ks + i] = temp / M[ks + k];
        }
    }
    return 0;
}

/*
Quaternion from matrix.
*/
int quaternion_from_matrix(
    double *matrix,     /* double[16] */
    double *quaternion) /* double[4] */
{
    double s;
    double *M = matrix;
    double *q = quaternion;

    if (ISZERO(M[15]))
        return -1;

    if ((M[0] + M[5] + M[10]) > 0.0) {
        s = 0.5 / sqrt(M[0] + M[5] + M[10] + M[15]);
        q[0] = 0.25 / s;
        q[3] = (M[4] - M[1]) * s;
        q[2] = (M[2] - M[8]) * s;
        q[1] = (M[9] - M[6]) * s;
    } else if ((M[0] > M[5]) && (M[0] > M[10])) {
        s = 0.5 / sqrt(M[0] - (M[5] + M[10]) + M[15]);
        q[1] = 0.25 / s;
        q[2] = (M[4] + M[1]) * s;
        q[3] = (M[2] + M[8]) * s;
        q[0] = (M[9] - M[6]) * s;
    } else if (M[5] > M[10]) {
        s = 0.5 / sqrt(M[5] - (M[0] + M[10]) + M[15]);
        q[2] = 0.25 / s;
        q[1] = (M[4] + M[1]) * s;
        q[0] = (M[2] - M[8]) * s;
        q[3] = (M[9] + M[6]) * s;
    } else {
        s = 0.5 / sqrt(M[10] - (M[0] + M[5]) + M[15]);
        q[3] = 0.25 / s;
        q[0] = (M[4] - M[1]) * s;
        q[1] = (M[2] + M[8]) * s;
        q[2] = (M[9] + M[6]) * s;
    }

    if (M[15] != 1.0) {
        s = 1.0 / sqrt(M[15]);
        q[0] *= s;
        q[1] *= s;
        q[2] *= s;
        q[3] *= s;
    }
    return 0;
}

/*
Quaternion to rotation matrix.
*/
int quaternion_matrix(
    double *quat,    /* double[4]  */
    double *matrix)  /* double[16] */
{
    double *M = matrix;
    double *q = quat;
    double n = sqrt(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]);

    if (n < EPSILON) {
        /* return identity matrix */
        memset(M, 0, 16*sizeof(double));
        M[0] = M[5] = M[10] = M[15] = 1.0;
    } else {
        q[0] /= n;
        q[1] /= n;
        q[2] /= n;
        q[3] /= n;
        {
            double x2 = q[1]+q[1];
            double y2 = q[2]+q[2];
            double z2 = q[3]+q[3];
            {
                double xx2 = q[1]*x2;
                double yy2 = q[2]*y2;
                double zz2 = q[3]*z2;
                M[0]  = 1.0 - yy2 - zz2;
                M[5]  = 1.0 - xx2 - zz2;
                M[10] = 1.0 - xx2 - yy2;
            }{
                double yz2 = q[2]*z2;
                double wx2 = q[0]*x2;
                M[6] = yz2 - wx2;
                M[9] = yz2 + wx2;
            }{
                double xy2 = q[1]*y2;
                double wz2 = q[0]*z2;
                M[1] = xy2 - wz2;
                M[4] = xy2 + wz2;
            }{
                double xz2 = q[1]*z2;
                double wy2 = q[0]*y2;
                M[8] = xz2 - wy2;
                M[2] = xz2 + wy2;
            }
            M[3] = M[7] = M[11] = M[12] = M[13] = M[14] = 0.0;
            M[15] = 1.0;
        }
    }
    return 0;
}

/*
Unit quaternion from unit sphere points.
*/
int quaternion_from_sphere_points(
    double *point0, /* double[3] */
    double *point1, /* double[3] */
    double *quat)   /* double[4] */
{
    quat[0] = point0[0]*point1[0] + point0[1]*point1[1] + point0[2]*point1[2];
    quat[1] = point0[1]*point1[2] - point0[2]*point1[1];
    quat[2] = point0[2]*point1[0] - point0[0]*point1[2];
    quat[3] = point0[0]*point1[1] - point0[1]*point1[0];
    return 0;
}

/*
Unit quaternion to unit sphere points.
*/
int quaternion_to_sphere_points(
    double *quat,   /* double[4] */
    double *point0, /* double[3] */
    double *point1) /* double[3] */
{
    double n = sqrt(quat[0]*quat[0] + quat[1]*quat[1]);

    if (n < EPSILON) {
        point0[0] = 0.0;
        point0[1] = 1.0;
        point0[2] = 0.0;
    } else {
        point0[0] = -quat[2] / n;
        point0[1] = quat[1] / n;
        point0[2] = 0.0;
    }

    point1[0] = quat[0]*point0[0] - quat[3]*point0[1];
    point1[1] = quat[0]*point0[1] + quat[3]*point0[0];
    point1[2] = quat[1]*point0[1] - quat[2]*point0[0];

    if (quat[0] < 0.0) {
        point0[0] = -point0[0];
        point0[1] = -point0[1];
    }

    return 0;
}

/*****************************************************************************/
/* Python functions */

/*
Numpy array converters for use with PyArg_Parse functions.
*/
static int
PyConverter_DoubleArray(
    PyObject *object,
    PyObject **address)
{
    *address = PyArray_FROM_OTF(object, NPY_DOUBLE, NPY_IN_ARRAY);
     if (*address == NULL) return NPY_FAIL;
     return NPY_SUCCEED;
}

static int
PyConverter_AnyDoubleArray(
    PyObject *object,
    PyObject **address)
{
    PyArrayObject *obj = (PyArrayObject *)object;
    if (PyArray_Check(object) && obj->descr->type_num == PyArray_DOUBLE) {
        *address = object;
        Py_INCREF(object);
        return NPY_SUCCEED;
    } else {
        *address = PyArray_FROM_OTF(object, NPY_DOUBLE, NPY_ALIGNED);
        if (*address == NULL) {
            PyErr_Format(PyExc_ValueError, "can not convert to array");
            return NPY_FAIL;
        }
        return NPY_SUCCEED;
    }
}

static int
PyConverter_DoubleArrayOrNone(
    PyObject *object,
    PyObject **address)
{
    if ((object == NULL) || (object == Py_None)) {
        *address = NULL;
    } else {
        *address = PyArray_FROM_OTF(object, NPY_DOUBLE, NPY_IN_ARRAY);
        if (*address == NULL) {
            PyErr_Format(PyExc_ValueError, "can not convert to array");
            return NPY_FAIL;
        }
    }
    return NPY_SUCCEED;
}

static int
PyConverter_DoubleMatrix44(
    PyObject *object,
    PyObject **address)
{
    PyArrayObject *obj;
    *address = PyArray_FROM_OTF(object, NPY_DOUBLE, NPY_IN_ARRAY);
    if (*address == NULL) {
        PyErr_Format(PyExc_ValueError, "can not convert to array");
        return NPY_FAIL;
    }
    obj = (PyArrayObject *) *address;
    if ((PyArray_NDIM(obj) != 2) || (PyArray_DIM(obj, 0) != 4)
        || (PyArray_DIM(obj, 1) != 4) || PyArray_ISCOMPLEX(obj)) {
        PyErr_Format(PyExc_ValueError, "not a 4x4 matrix");
        Py_DECREF(*address);
        *address = NULL;
        return NPY_FAIL;
    }
    return NPY_SUCCEED;
}

static int
PyConverter_DoubleMatrix44Copy(
    PyObject *object,
    PyObject **address)
{
    PyArrayObject *obj;
    *address = PyArray_FROM_OTF(object, NPY_DOUBLE,
                                NPY_ENSURECOPY|NPY_IN_ARRAY);
    if (*address == NULL) {
        PyErr_Format(PyExc_ValueError, "can not convert to array");
        return NPY_FAIL;
    }
    obj = (PyArrayObject *) *address;
    if ((PyArray_NDIM(obj) != 2) || (PyArray_DIM(obj, 0) != 4)
        || (PyArray_DIM(obj, 1) != 4) || PyArray_ISCOMPLEX(obj)) {
        PyErr_Format(PyExc_ValueError, "not a 4x4 matrix");
        Py_DECREF(*address);
        *address = NULL;
        return NPY_FAIL;
    }
    return NPY_SUCCEED;
}

static int
PyConverter_DoubleVector3(
    PyObject *object,
    PyObject **address)
{
    PyArrayObject *obj;
    *address = PyArray_FROM_OTF(object, NPY_DOUBLE, NPY_IN_ARRAY);
    if (*address == NULL) {
        PyErr_Format(PyExc_ValueError, "can not convert to array");
        return NPY_FAIL;
    }
    obj = (PyArrayObject *) *address;
    if ((PyArray_NDIM(obj) != 1) || (PyArray_DIM(obj, 0) < 3)
        || PyArray_ISCOMPLEX(obj)) {
        PyErr_Format(PyExc_ValueError, "not a vector3");
        Py_DECREF(*address);
        *address = NULL;
        return NPY_FAIL;
    }
    return NPY_SUCCEED;
}

static int
PyConverter_DoubleVector4(
    PyObject *object,
    PyObject **address)
{
    PyArrayObject *obj;
    *address = PyArray_FROM_OTF(object, NPY_DOUBLE, NPY_IN_ARRAY);
    if (*address == NULL) {
        PyErr_Format(PyExc_ValueError, "can not convert to array");
        return NPY_FAIL;
    }
    obj = (PyArrayObject *) *address;
    if ((PyArray_NDIM(obj) != 1) || (PyArray_DIM(obj, 0) < 4)
        || PyArray_ISCOMPLEX(obj)) {
        PyErr_Format(PyExc_ValueError, "not a vector4");
        Py_DECREF(*address);
        *address = NULL;
        return NPY_FAIL;
    }
    return NPY_SUCCEED;
}

static int
PyConverter_DoubleVector4Copy(
    PyObject *object,
    PyObject **address)
{
    PyArrayObject *obj;
    *address = PyArray_FROM_OTF(object, NPY_DOUBLE,
                                NPY_ENSURECOPY|NPY_IN_ARRAY);
    if (*address == NULL) {
        PyErr_Format(PyExc_ValueError, "can not convert to array");
        return NPY_FAIL;
    }
    obj = (PyArrayObject *) *address;
    if ((PyArray_NDIM(obj) != 1) || (PyArray_DIM(obj, 0) < 4)
        || PyArray_ISCOMPLEX(obj)) {
        PyErr_Format(PyExc_ValueError, "not a vector4");
        Py_DECREF(*address);
        *address = NULL;
        return NPY_FAIL;
    }
    return NPY_SUCCEED;
}

static int
PyConverter_DoubleVector3OrNone(
    PyObject *object,
    PyObject **address)
{
    if ((object == NULL) || (object == Py_None)) {
        *address = NULL;
    } else {
        PyArrayObject *obj;
        *address = PyArray_FROM_OTF(object, NPY_DOUBLE, NPY_IN_ARRAY);
        if (*address == NULL) {
            PyErr_Format(PyExc_ValueError, "can not convert to array");
            return NPY_FAIL;
        }
        obj = (PyArrayObject *) *address;
        if ((PyArray_NDIM(obj) != 1) || (PyArray_DIM(obj, 0) < 3)
            || PyArray_ISCOMPLEX(obj)) {
            PyErr_Format(PyExc_ValueError, "not a vector3");
            Py_DECREF(*address);
            *address = NULL;
            return NPY_FAIL;
        }
    }
    return NPY_SUCCEED;
}

static int
PyOutputConverter_AnyDoubleArrayOrNone(
    PyObject *object,
    PyArrayObject **address)
{
    PyArrayObject *obj = (PyArrayObject *)object;
    if ((object == NULL) || (object == Py_None)) {
        *address = NULL;
        return NPY_SUCCEED;
    }
    if (PyArray_Check(object) && (obj->descr->type_num == PyArray_DOUBLE)) {
        Py_INCREF(object);
        *address = (PyArrayObject *)object;
        return NPY_SUCCEED;
    } else {
        PyErr_Format(PyExc_TypeError, "output must be array of type double");
        *address = NULL;
        return NPY_FAIL;
    }
}

/*
Return i-th element of Python sequence as long, or -1 on failure.
*/
long PySequence_GetInteger(PyObject *obj, Py_ssize_t i)
{
    long value;
    PyObject *item = PySequence_GetItem(obj, i);
    if (item == NULL ||
#if PY_MAJOR_VERSION < 3
        !PyInt_Check(item)
#else
        !PyLong_Check(item)
#endif
        ) {
        PyErr_Format(PyExc_ValueError, "expected integer number");
        Py_XDECREF(item);
        return -1;
    }

#if PY_MAJOR_VERSION < 3
    value = PyInt_AsLong(item);
#else
    value = PyLong_AsLong(item);
#endif
    Py_XDECREF(item);
    return value;
}

/*
Equivalence of transformations.
*/
char py_is_same_transform_doc[] =
    "Return True if two matrices perform same transformation.";

static PyObject *
py_is_same_transform(
    PyObject *obj,
    PyObject *args,
    PyObject *kwds)
{
    int result;
    PyArrayObject *matrix0 = NULL;
    PyArrayObject *matrix1 = NULL;
    static char *kwlist[] = {"matrix0", "matrix1", NULL};

    if (!PyArg_ParseTupleAndKeywords(args, kwds, "O&O&", kwlist,
        PyConverter_DoubleMatrix44, &matrix0,
        PyConverter_DoubleMatrix44, &matrix1)) goto _fail;

    {
        double *M0 = (double *)PyArray_DATA(matrix0);
        double *M1 = (double *)PyArray_DATA(matrix1);
        double t0 = M0[15];
        double t1 = M1[15];
        double t;
        int i;
        if (ISZERO(t0) || ISZERO(t1)) {
            result = 0;
        } else {
            result = 1;
            for (i = 0; i < 16; i++) {
                t = M1[i] / t1;
                if (fabs(M0[i]/t0 - t) > (1e-8 + 1e-5*fabs(t))) {
                    result = 0;
                    break;
                }
            }
        }
    }

    Py_DECREF(matrix0);
    Py_DECREF(matrix1);
    if (result)
        Py_RETURN_TRUE;
    else
        Py_RETURN_FALSE;

  _fail:
    Py_XDECREF(matrix0);
    Py_XDECREF(matrix1);
    return NULL;
}

/*
Identity matrix.
*/
char py_identity_matrix_doc[] = "Return identity/unit matrix.";

static PyObject *
py_identity_matrix(
    PyObject *obj,
    PyObject *args)
{
    PyArrayObject *result = NULL;
    PyArray_Descr *type = NULL;
    Py_ssize_t dims[] = {4, 4};

    type = PyArray_DescrFromType(NPY_DOUBLE);
    result = (PyArrayObject*)PyArray_Zeros(2, dims, type, 0);
    if (result == NULL) {
        PyErr_Format(PyExc_MemoryError, "unable to allocate matrix");
        goto _fail;
    }
    {
        double *M = (double *)PyArray_DATA(result);
        M[0] = M[5] = M[10] = M[15] = 1.0;
    }

    return PyArray_Return(result);

  _fail:
    Py_XDECREF(result);
    return NULL;
}

/*
Translation matrix.
*/
char py_translation_matrix_doc[] =
    "Return matrix to translate by direction vector.";

static PyObject *
py_translation_matrix(
    PyObject *obj,
    PyObject *args,
    PyObject *kwds)
{
    PyArrayObject *result = NULL;
    PyArrayObject *direction = NULL;
    PyArray_Descr *type = NULL;
    Py_ssize_t dims[] = {4, 4};
    static char *kwlist[] = {"direction", NULL};

    if (!PyArg_ParseTupleAndKeywords(args, kwds, "O&", kwlist,
        PyConverter_DoubleVector3, &direction)) goto _fail;

    type = PyArray_DescrFromType(NPY_DOUBLE);
    result = (PyArrayObject*)PyArray_Zeros(2, dims, type, 0);
    if (result == NULL) {
        PyErr_Format(PyExc_MemoryError, "unable to allocate matrix");
        goto _fail;
    }
    {
        double *M = (double *)PyArray_DATA(result);
        double *d = (double *)PyArray_DATA(direction);
        M[0] = M[5] = M[10] = M[15] = 1.0;
        M[3] = d[0];
        M[7] = d[1];
        M[11] = d[2];
    }

    Py_DECREF(direction);
    return PyArray_Return(result);

  _fail:
    Py_XDECREF(direction);
    Py_XDECREF(result);
    return NULL;
}

/*
Reflection matrix.
*/
char py_reflection_matrix_doc[] =
    "Return matrix to mirror at plane defined by point and normal vector.";

static PyObject *
py_reflection_matrix(
    PyObject *obj,
    PyObject *args,
    PyObject *kwds)
{
    PyArrayObject *result = NULL;
    PyArrayObject *point = NULL;
    PyArrayObject *normal = NULL;
    Py_ssize_t dims[] = {4, 4};
    static char *kwlist[] = {"point", "normal", NULL};

    if (!PyArg_ParseTupleAndKeywords(args, kwds, "O&O&", kwlist,
        PyConverter_DoubleVector3, &point,
        PyConverter_DoubleVector3, &normal)) goto _fail;

    result = (PyArrayObject*)PyArray_SimpleNew(2, dims, NPY_DOUBLE);
    if (result == NULL) {
        PyErr_Format(PyExc_MemoryError, "unable to allocate matrix");
        goto _fail;
    }
    {
        double *M = (double *)PyArray_DATA(result);
        double *p = (double *)PyArray_DATA(point);
        double *n = (double *)PyArray_DATA(normal);
        double nx = n[0];
        double ny = n[1];
        double nz = n[2];
        double t = sqrt(nx*nx + ny*ny + nz*nz);
        if (t < EPSILON) {
            PyErr_Format(PyExc_ValueError, "invalid normal vector");
            goto _fail;
        }
        nx /= t;
        ny /= t;
        nz /= t;
        M[12] = M[13] = M[14] = 0.0;
        M[15] = 1.0;
        M[0] = 1.0 - 2.0 * nx *nx;
        M[5] = 1.0 - 2.0 * ny *ny;
        M[10] = 1.0 - 2.0 * nz *nz;
        M[1] = M[4] = -2.0 * nx *ny;
        M[2] = M[8] = -2.0 * nx *nz;
        M[6] = M[9] = -2.0 * ny *nz;
        t = 2.0 * (p[0]*nx + p[1]*ny + p[2]*nz);
        M[3] = nx * t;
        M[7] = ny * t;
        M[11] = nz * t;
    }

    Py_DECREF(point);
    Py_DECREF(normal);
    return PyArray_Return(result);

  _fail:
    Py_XDECREF(point);
    Py_XDECREF(normal);
    Py_XDECREF(result);
    return NULL;
}

/*
Rotation matrix.
*/
char py_rotation_matrix_doc[] =
    "Return matrix to rotate about axis defined by point and direction.";

static PyObject *
py_rotation_matrix(
    PyObject *obj,
    PyObject *args,
    PyObject *kwds)
{
    PyArrayObject *result = NULL;
    PyArrayObject *point = NULL;
    PyArrayObject *direction = NULL;
    Py_ssize_t dims[] = {4, 4};
    double angle;
    static char *kwlist[] = {"angle", "direction", "point", NULL};

    if (!PyArg_ParseTupleAndKeywords(args, kwds, "dO&|O&", kwlist,
        &angle,
        PyConverter_DoubleVector3, &direction,
        PyConverter_DoubleVector3OrNone, &point)) goto _fail;

    result = (PyArrayObject*)PyArray_SimpleNew(2, dims, NPY_DOUBLE);
    if (result == NULL) {
        PyErr_Format(PyExc_MemoryError, "unable to allocate matrix");
        goto _fail;
    }
    {
        double *M = (double *)PyArray_DATA(result);
        double *d = (double *)PyArray_DATA(direction);
        double dx = d[0];
        double dy = d[1];
        double dz = d[2];
        double sa = sin(angle);
        double ca = cos(angle);
        double ca1 = 1 - ca;
        double s, t;

        t = sqrt(dx*dx + dy*dy + dz*dz);
        if (t < EPSILON) {
            PyErr_Format(PyExc_ValueError, "invalid direction vector");
            goto _fail;
        }
        dx /= t;
        dy /= t;
        dz /= t;

        M[0] =  ca + dx*dx * ca1;
        M[5] =  ca + dy*dy * ca1;
        M[10] = ca + dz*dz * ca1;

        s = dz * sa;
        t = dx*dy * ca1;
        M[1] = t - s;
        M[4] = t + s;

        s = dy * sa;
        t = dx*dz * ca1;
        M[2] = t + s;
        M[8] = t - s;

        s = dx * sa;
        t = dy*dz * ca1;
        M[6] = t - s;
        M[9] = t + s;

        M[12] = M[13] = M[14] = 0.0;
        M[15] = 1.0;

        if (point != NULL) {
            double *p = (double *)PyArray_DATA(point);
            M[3]  = p[0] - (M[0]*p[0] + M[1]*p[1] + M[2]*p[2]);
            M[7]  = p[1] - (M[4]*p[0] + M[5]*p[1] + M[6]*p[2]);
            M[11] = p[2] - (M[8]*p[0] + M[9]*p[1] + M[10]*p[2]);
        } else {
            M[3] = M[7] = M[11] = 0.0;
        }
    }

    Py_XDECREF(point);
    Py_DECREF(direction);
    return PyArray_Return(result);

  _fail:
    Py_XDECREF(point);
    Py_XDECREF(direction);
    Py_XDECREF(result);
    return NULL;
}

/*
Projection matrix.
*/
char py_projection_matrix_doc[] =
    "Return matrix to project onto plane defined by point and normal.";

static PyObject *
py_projection_matrix(
    PyObject *obj,
    PyObject *args,
    PyObject *kwds)
{
    PyArrayObject *result = NULL;
    PyArrayObject *point = NULL;
    PyArrayObject *normal = NULL;
    PyArrayObject *direction = NULL;
    PyArrayObject *perspective = NULL;
    PyObject *pseudobj = NULL;
    Py_ssize_t dims[] = {4, 4};
    int pseudo = 0;
    static char *kwlist[] = {"point", "normal", "direction",
                             "perspective", "pseudo", NULL};

    if (!PyArg_ParseTupleAndKeywords(args, kwds, "O&O&|O&O&O", kwlist,
        PyConverter_DoubleVector3, &point,
        PyConverter_DoubleVector3, &normal,
        PyConverter_DoubleVector3OrNone, &direction,
        PyConverter_DoubleVector3OrNone, &perspective,
        &pseudobj)) goto _fail;

    if (pseudobj != NULL)
        pseudo = PyObject_IsTrue(pseudobj);

    result = (PyArrayObject*)PyArray_SimpleNew(2, dims, NPY_DOUBLE);
    if (result == NULL) {
        PyErr_Format(PyExc_MemoryError, "unable to allocate matrix");
        goto _fail;
    }
    {
        double *M = (double *)PyArray_DATA(result);
        double *p = (double *)PyArray_DATA(point);
        double px = p[0];
        double py = p[1];
        double pz = p[2];
        double *n = (double *)PyArray_DATA(normal);
        double nx = n[0];
        double ny = n[1];
        double nz = n[2];
        double t = sqrt(nx*nx + ny*ny + nz*nz);
        if (t < EPSILON) {
            PyErr_Format(PyExc_ValueError, "invalid normal vector");
            goto _fail;
        }
        nx /= t;
        ny /= t;
        nz /= t;

        if (perspective) {
            /* perspective projection */
            double *d = (double *)PyArray_DATA(perspective);
            double dx = d[0];
            double dy = d[1];
            double dz = d[2];

            t = (dx-px)*nx + (dy-py)*ny + (dz-pz)*nz;
            M[0]  = t - dx*nx;
            M[5]  = t - dy*ny;
            M[10] = t - dz*nz;
            M[1] = - dx*ny;
            M[2] = - dx*nz;
            M[4] = - dy*nx;
            M[6] = - dy*nz;
            M[8] = - dz*nx;
            M[9] = - dz*ny;

            if (pseudo) {
                /* preserve relative depth */
                M[0]  -= nx*nx;
                M[5]  -= ny*ny;
                M[10] -= nz*nz;
                t = nx*ny;
                M[1] -= t;
                M[4] -= t;
                t = nx*nz;
                M[2] -= t;
                M[8] -= t;
                t = ny*nz;
                M[6] -= t;
                M[9] -= t;
                t = px*nx + py*ny + pz*nz;
                M[3]  = t * (dx+nx);
                M[7]  = t * (dy+ny);
                M[11] = t * (dz+nz);
            } else {
                t = px*nx + py*ny + pz*nz;
                M[3]  = t * dx;
                M[7]  = t * dy;
                M[11] = t * dz;
            }
            M[12] = -nx;
            M[13] = -ny;
            M[14] = -nz;
            M[15] = dx*nx + dy*ny + dz*nz;
        } else if (direction) {
            /* parallel projection */
            double *d = (double *)PyArray_DATA(direction);
            double dx = d[0];
            double dy = d[1];
            double dz = d[2];
            double scale = dx*nx + dy*ny + dz*nz;

            if (ISZERO(scale)) {
                PyErr_Format(PyExc_ValueError,
                    "normal and direction vectors are orthogonal");
                goto _fail;
            }
            scale = -1.0 / scale;
            M[0]  = 1.0 + scale * dx*nx;
            M[5]  = 1.0 + scale * dy*ny;
            M[10] = 1.0 + scale * dz*nz;
            M[1] = scale * dx*ny;
            M[2] = scale * dx*nz;
            M[4] = scale * dy*nx;
            M[6] = scale * dy*nz;
            M[8] = scale * dz*nx;
            M[9] = scale * dz*ny;
            t = (px*nx + py*ny + pz*nz) * -scale;
            M[3]  = t * dx;
            M[7]  = t * dy;
            M[11] = t * dz;
            M[12] = M[13] = M[14] = 0.0;
            M[15] = 1.0;
        } else {
            /* orthogonal projection */
            M[0]  = 1.0 - nx*nx;
            M[5]  = 1.0 - ny*ny;
            M[10] = 1.0 - nz*nz;
            M[1] = M[4] = - nx*ny;
            M[2] = M[8] = - nx*nz;
            M[6] = M[9] = - ny*nz;
            t = px*nx + py*ny + pz*nz;
            M[3]  = t * nx;
            M[7]  = t * ny;
            M[11] = t * nz;
            M[12] = M[13] = M[14] = 0.0;
            M[15] = 1.0;
        }
    }

    Py_DECREF(point);
    Py_DECREF(normal);
    Py_XDECREF(direction);
    Py_XDECREF(perspective);
    return PyArray_Return(result);

  _fail:
    Py_XDECREF(point);
    Py_XDECREF(normal);
    Py_XDECREF(direction);
    Py_XDECREF(perspective);
    Py_XDECREF(result);
    return NULL;
}

/*
Clipping matrix.
*/
char py_clip_matrix_doc[] =
    "Return matrix to obtain normalized device coordinates from frustum.";

static PyObject *
py_clip_matrix(
    PyObject *obj,
    PyObject *args,
    PyObject *kwds)
{
    PyArrayObject *result = NULL;
    PyObject *boolobj = NULL;
    Py_ssize_t dims[] = {4, 4};
    double *M;
    double left, right, bottom, top, hither, yon;
    int perspective = 0;
    static char *kwlist[] = {"left", "right", "bottom",
                             "top", "near", "far", "perspective", NULL};

    if (!PyArg_ParseTupleAndKeywords(args, kwds, "dddddd|O", kwlist,
        &left, &right, &bottom, &top, &hither, &yon, &boolobj))
        goto _fail;

    if (boolobj != NULL)
        perspective = PyObject_IsTrue(boolobj);

    if ((left >= right) || (bottom >= top) || (hither >= yon)) {
        PyErr_Format(PyExc_ValueError, "invalid frustum");
        goto _fail;
    }

    result = (PyArrayObject*)PyArray_SimpleNew(2, dims, NPY_DOUBLE);
    if (result == NULL) {
        PyErr_Format(PyExc_MemoryError, "unable to allocate matrix");
        goto _fail;
    }

    M = (double *)PyArray_DATA(result);

    if (perspective) {
        double t = 2.0 * hither;
        if (hither < EPSILON) {
            PyErr_Format(PyExc_ValueError, "invalid frustum: near <= 0");
            goto _fail;
        }
        M[1] = M[3] = M[4] = M[7] = M[8] = M[9] = M[12] = M[13] = M[15] = 0.0;
        M[14] = -1.0;
        M[0] = t / (left-right);
        M[2] = (right+left) / (right-left);
        M[5] = t / (bottom-top);
        M[6] = (top+bottom) / (top-bottom);
        M[10] = (yon+hither) / (hither-yon);
        M[11] = t*yon / (yon-hither);
    } else {
        M[1] = M[2] = M[4] = M[6] = M[8] = M[9] = M[12] = M[13] = M[14] = 0.0;
        M[15] = 1.0;
        M[0] = 2.0 / (right-left);
        M[3] = (right+left) / (left-right);
        M[5] = 2.0 / (top-bottom);
        M[7] = (top+bottom) / (bottom-top);
        M[10] = 2.0 / (yon-hither);
        M[11] = (yon+hither) / (hither-yon);
    }

    return PyArray_Return(result);

  _fail:
    Py_XDECREF(result);
    return NULL;
}

/*
Scale matrix.
*/
char py_scale_matrix_doc[] =
    "Return matrix to scale by factor around origin in direction.";

static PyObject *
py_scale_matrix(
    PyObject *obj,
    PyObject *args,
    PyObject *kwds)
{
    PyArrayObject *result = NULL;
    PyArrayObject *origin = NULL;
    PyArrayObject *direction = NULL;
    Py_ssize_t dims[] = {4, 4};
    double factor;
    static char *kwlist[] = {"factor", "origin", "direction", NULL};

    if (!PyArg_ParseTupleAndKeywords(args, kwds, "d|O&O&", kwlist,
        &factor,
        PyConverter_DoubleVector3OrNone, &origin,
        PyConverter_DoubleVector3OrNone, &direction)) goto _fail;

    result = (PyArrayObject*)PyArray_SimpleNew(2, dims, NPY_DOUBLE);
    if (result == NULL) {
        PyErr_Format(PyExc_MemoryError, "unable to allocate matrix");
        goto _fail;
    }
    {
        double *M = (double *)PyArray_DATA(result);
        if (direction == NULL) {
            memset(M, 0, 16*sizeof(double));
            M[0] = M[5] = M[10] = factor;
            M[15] = 1.0;
            if (origin != NULL) {
                double *p = (double *)PyArray_DATA(origin);
                factor = 1.0 - factor;
                M[3]  = factor * p[0];
                M[7]  = factor * p[1];
                M[11] = factor * p[2];
            }
        } else {
            double *d = (double *)PyArray_DATA(direction);
            double dx = d[0];
            double dy = d[1];
            double dz = d[2];
            factor = 1.0 - factor;
            M[0]  = 1.0 - factor * dx*dx;
            M[5]  = 1.0 - factor * dy*dy;
            M[10] = 1.0 - factor * dz*dz;
            factor *= -1.0;
            M[1] = M[4] = factor * dx*dy;
            M[2] = M[8] = factor * dx*dz;
            M[6] = M[9] = factor * dy*dz;
            M[12] = M[13] = M[14] = 0.0;
            M[15] = 1.0;
            if (origin != NULL) {
                double *p = (double *)PyArray_DATA(origin);
                factor *= - (p[0]*dx + p[1]*dy + p[2]*dz);
                M[3]  = factor * dx;
                M[7]  = factor * dy;
                M[11] = factor * dz;
            } else {
                M[3] = M[7] = M[11] = 0.0;
            }
        }
    }

    Py_XDECREF(origin);
    Py_XDECREF(direction);
    return PyArray_Return(result);

  _fail:
    Py_XDECREF(origin);
    Py_XDECREF(direction);
    Py_XDECREF(result);
    return NULL;
}

/*
Shearing matrix.
*/
char py_shear_matrix_doc[] =
    "Return matrix to shear by angle along direction vector on shear plane.";

static PyObject *
py_shear_matrix(
    PyObject *obj,
    PyObject *args,
    PyObject *kwds)
{
    PyArrayObject *result = NULL;
    PyArrayObject *direction = NULL;
    PyArrayObject *point = NULL;
    PyArrayObject *normal = NULL;
    Py_ssize_t dims[] = {4, 4};
    double angle;
    static char *kwlist[] = {"angle", "direction", "point", "normal", NULL};

    if (!PyArg_ParseTupleAndKeywords(args, kwds, "dO&O&O&", kwlist,
        &angle,
        PyConverter_DoubleVector3, &direction,
        PyConverter_DoubleVector3, &point,
        PyConverter_DoubleVector3, &normal)) goto _fail;

    result = (PyArrayObject*)PyArray_SimpleNew(2, dims, NPY_DOUBLE);
    if (result == NULL) {
        PyErr_Format(PyExc_MemoryError, "unable to allocate matrix");
        goto _fail;
    }
    {
        double *M = (double *)PyArray_DATA(result);
        double *p = (double *)PyArray_DATA(point);
        double *d = (double *)PyArray_DATA(direction);
        double dx = d[0];
        double dy = d[1];
        double dz = d[2];
        double *n = (double *)PyArray_DATA(normal);
        double nx = n[0];
        double ny = n[1];
        double nz = n[2];
        double t;

        t = sqrt(dx*dx + dy*dy + dz*dz);
        if (t < EPSILON) {
            PyErr_Format(PyExc_ValueError, "invalid direction vector");
            goto _fail;
        }
        dx /= t;
        dy /= t;
        dz /= t;

        t = sqrt(nx*nx + ny*ny + nz*nz);
        if (t < EPSILON) {
            PyErr_Format(PyExc_ValueError, "invalid normal vector");
            goto _fail;
        }
        nx /= t;
        ny /= t;
        nz /= t;

        if (fabs(nx*dx + ny*dy + nz*dz) > 1e-6) {
            PyErr_Format(PyExc_ValueError,
                "direction and normal vectors are not orthogonal");
            goto _fail;
        }

        angle = tan(angle);

        M[0]  = 1.0 + angle * dx*nx;
        M[5]  = 1.0 + angle * dy*ny;
        M[10] = 1.0 + angle * dz*nz;
        M[1] = angle * dx*ny;
        M[2] = angle * dx*nz;
        M[4] = angle * dy*nx;
        M[6] = angle * dy*nz;
        M[8] = angle * dz*nx;
        M[9] = angle * dz*ny;
        M[12] = M[13] = M[14] = 0.0;
        M[15] = 1.0;

        t = -angle * (p[0]*nx + p[1]*ny + p[2]*nz);
        M[3]  = t * dx;
        M[7]  = t * dy;
        M[11] = t * dz;
    }

    Py_DECREF(direction);
    Py_DECREF(point);
    Py_DECREF(normal);
    return PyArray_Return(result);

  _fail:
    Py_XDECREF(direction);
    Py_XDECREF(point);
    Py_XDECREF(normal);
    Py_XDECREF(result);
    return NULL;
}

/*
Superimposition matrix.
*/
char py_superimposition_matrix_doc[] =
    "Return matrix to transform given vector set into second vector set.";

static PyObject *
py_superimposition_matrix(
    PyObject *obj,
    PyObject *args,
    PyObject *kwds)
{
    PyThreadState *_save = NULL;
    PyArrayObject *result = NULL;
    PyArrayObject *v0 = NULL;
    PyArrayObject *v1 = NULL;
    PyObject *usesvdobj = NULL;
    PyObject *scalingobj = NULL;
    double *buffer = NULL;
    Py_ssize_t dims[] = {4, 4};
    int scaling = 0;

    static char *kwlist[] = {"v0", "v1", "scale", "usesvd", NULL};

    if (!PyArg_ParseTupleAndKeywords(args, kwds, "O&O&|OO", kwlist,
        PyConverter_AnyDoubleArray, &v0,
        PyConverter_AnyDoubleArray, &v1,
        &scalingobj, &usesvdobj)) goto _fail;

    if (scalingobj != NULL)
        scaling = PyObject_IsTrue(scalingobj);

    if (!PyArray_SAMESHAPE(v0, v1)) {
        PyErr_Format(PyExc_ValueError, "shape of vector sets must match");
        goto _fail;
    }

    if ((PyArray_NDIM(v0) != 2) || PyArray_DIM(v0, 0) < 3) {
        PyErr_Format(PyExc_ValueError,
            "vector sets are of wrong shape or type");
        goto _fail;
    }

    result = (PyArrayObject*)PyArray_SimpleNew(2, dims, NPY_DOUBLE);
    if (result == NULL) {
        PyErr_Format(PyExc_MemoryError, "unable to allocate matrix");
        goto _fail;
    }

    buffer = (double *)PyMem_Malloc(42 * sizeof(double));
    if (!buffer) {
        PyErr_Format(PyExc_MemoryError, "unable to allocate buffer");
        goto _fail;
    }
    {
        Py_ssize_t i, j;
        double v0t[3], v1t[3];
        double *q = buffer;
        double *N = (buffer + 4);
        double *M = (double *)PyArray_DATA(result);

        Py_ssize_t size = PyArray_DIM(v0, 1);
        Py_ssize_t v0s0 = PyArray_STRIDE(v0, 0);
        Py_ssize_t v0s1 = PyArray_STRIDE(v0, 1);
        Py_ssize_t v1s0 = PyArray_STRIDE(v1, 0);
        Py_ssize_t v1s1 = PyArray_STRIDE(v1, 1);

        /* displacements of v0 & v1 centroids from origin */
        {
            double t;
            if (v0s1 == sizeof(double)) {
                double *p;
                for (j = 0; j < 3; j++) {
                    t = 0.0;
                    p = (double*)((char *)PyArray_DATA(v0) + j*v0s0);
                    for (i = 0; i < size; i++) {
                        t += p[i];
                    }
                    v0t[j] = t / (double)size;
                }
            } else {
                char *p;
                for (j = 0; j < 3; j++) {
                    t = 0.0;