1 /* transformations.c
2
3 A Python/Numpy C extension module for homogeneous transformation matrices
4 and quaternions.
5
6 Refer to the transformations.py module for documentation and tests.
7
8 Tested on Python 2.6 and 3.1, 32-bit and 64-bit.
9
10 Authors:
11 Christoph Gohlke <http://www.lfd.uci.edu/~gohlke/>,
12 Laboratory for Fluorescence Dynamics. University of California, Irvine.
13
14 Copyright (c) 2007-2010, The Regents of the University of California
15 All rights reserved.
16
17 Redistribution and use in source and binary forms, with or without
18 modification, are permitted provided that the following conditions are met:
19
20 * Redistributions of source code must retain the above copyright
21 notice, this list of conditions and the following disclaimer.
22 * Redistributions in binary form must reproduce the above copyright
23 notice, this list of conditions and the following disclaimer in the
24 documentation and/or other materials provided with the distribution.
25 * Neither the name of the copyright holders nor the names of any
26 contributors may be used to endorse or promote products derived
27 from this software without specific prior written permission.
28
29 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
30 AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
31 IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
32 ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
33 LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
34 CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
35 SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
36 INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
37 CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
38 ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
39 POSSIBILITY OF SUCH DAMAGE.
40 */
41
42 /*****************************************************************************/
43 /* setup.py
44
45 """A Python script to build the _transformations extension module.
46
47 Usage:: ``python setup.py build_ext --inplace``
48
49 """
50
51 from distutils.core import setup, Extension
52 import numpy
53
54 setup(name='_transformations', ext_modules=[
55 Extension('_transformations', ['transformations.c'],
56 include_dirs=[numpy.get_include()], extra_compile_args=[])],)
57
58 ******************************************************************************/
59
60 #define _VERSION_ "2010.04.10"
61
62 #define WIN32_LEAN_AND_MEAN
63 #ifdef _WIN32
64 #include <windows.h>
65 #include <wincrypt.h>
66 #endif
67
68 #include "Python.h"
69 #include "structmember.h"
70 #include "math.h"
71 #include "float.h"
72 #include "string.h"
73 #include "numpy/arrayobject.h"
74
75 #define EPSILON 8.8817841970012523e-016 /* 4.0 * DBL_EPSILON */
76 #define PIVOT_TOLERANCE 1.0e-14
77 #define DEG2RAD 0.017453292519943295769236907684886
78 #define TWOPI 6.283185307179586476925286766559
79 #ifndef M_PI
80 #define M_PI 3.1415926535897932384626433832795
81 #endif
82
83 #ifndef MAX
84 #define MAX(a, b) (((a) > (b)) ? (a) : (b))
85 #define MIN(a, b) (((a) < (b)) ? (a) : (b))
86 #endif
87
88 #define ISZERO(x) (((x) < EPSILON) && ((x) > -EPSILON))
89 #define NOTZERO(x) (((x) > EPSILON) || ((x) < -EPSILON))
90
91 /*****************************************************************************/
92 /* C helper functions */
93
94 /*
95 Return random doubles in half-open interval [0.0, 1.0).
96 Uses /dev/urandom or CryptoAPI. Not very fast but random.
97 Assumes sizeof(double) == 2*sizeof(int).
98 */
random_doubles(double * buffer,Py_ssize_t size)99 int random_doubles(
100 double *buffer,
101 Py_ssize_t size)
102 {
103 #ifndef _WIN32
104 int done;
105 FILE *rfile;
106 if (size < 1)
107 return 0;
108 rfile = fopen("/dev/urandom", "rb");
109 if (rfile == NULL)
110 return -1;
111 done = fread(buffer, size*sizeof(double), 1, rfile);
112 fclose(rfile);
113 #else
114 #pragma comment(lib,"advapi32")
115 BOOL done;
116 HCRYPTPROV hcryptprov;
117 if (size < 1)
118 return 0;
119 if (!CryptAcquireContext(&hcryptprov, NULL, NULL, PROV_RSA_FULL,
120 CRYPT_VERIFYCONTEXT) || !hcryptprov)
121 return -1;
122 done = CryptGenRandom(hcryptprov, (DWORD)(size*sizeof(double)),
123 (unsigned char *)buffer);
124 CryptReleaseContext(hcryptprov, 0);
125 #endif
126 if (done) {
127 unsigned int a, b;
128 unsigned int *p = (unsigned int *)buffer;
129 while (size--) {
130 a = (*p++) >> 5;
131 b = (*p++) >> 6;
132 *buffer++ = (a * 67108864.0 + b) / 9007199254740992.0;
133 }
134 return 0;
135 }
136 return -1;
137 }
138
139 /*
140 Tridiagonal matrix from symmetric 4x4 matrix using Housholder reduction.
141 The input matrix is altered.
142 */
tridiagonalize_symmetric_44(double * matrix,double * diagonal,double * subdiagonal)143 int tridiagonalize_symmetric_44(
144 double *matrix, /* double[16] */
145 double *diagonal, /* double[4] */
146 double *subdiagonal) /* double[3] */
147 {
148 double t, n, g, h;
149 double v0, v1, v2;
150 double *M = matrix;
151 double *u;
152
153 u = &M[1];
154 t = u[1]*u[1] + u[2]*u[2];
155 n = sqrt(u[0]*u[0] + t);
156 if (n > EPSILON) {
157 if (u[0] < 0.0)
158 n = -n;
159 u[0] += n;
160 h = (u[0]*u[0] + t) / 2.0;
161 v0 = M[5]*u[0] + M[6]*u[1] + M[7]*u[2];
162 v1 = M[6]*u[0] + M[10]*u[1] + M[11]*u[2];
163 v2 = M[7]*u[0] + M[11]*u[1] + M[15]*u[2];
164 v0 /= h;
165 v1 /= h;
166 v2 /= h;
167 g = (u[0]*v0 + u[1]*v1 + u[2]*v2) / (2.0 * h);
168 v0 -= g * u[0];
169 v1 -= g * u[1];
170 v2 -= g * u[2];
171 M[5] -= 2.0*v0*u[0];
172 M[10] -= 2.0*v1*u[1];
173 M[15] -= 2.0*v2*u[2];
174 M[6] -= v1*u[0] + v0*u[1];
175 M[7] -= v2*u[0] + v0*u[2];
176 M[11] -= v2*u[1] + v1*u[2];
177 M[1] = -n;
178 }
179
180 u = &M[6];
181 t = u[1]*u[1];
182 n = sqrt(u[0]*u[0] + t);
183 if (n > EPSILON) {
184 if (u[0] < 0.0)
185 n = -n;
186 u[0] += n;
187 h = (u[0]*u[0] + t) / 2.0;
188 v0 = M[10]*u[0] + M[11]*u[1];
189 v1 = M[11]*u[0] + M[15]*u[1];
190 v0 /= h;
191 v1 /= h;
192 g = (u[0]*v0 + u[1]*v1) / (2.0 * h);
193 v0 -= g * u[0];
194 v1 -= g * u[1];
195 M[10] -= 2.0*v0*u[0];
196 M[15] -= 2.0*v1*u[1];
197 M[11] -= v1*u[0] + v0*u[1];
198 M[6] = -n;
199 }
200
201 diagonal[0] = M[0];
202 diagonal[1] = M[5];
203 diagonal[2] = M[10];
204 diagonal[3] = M[15];
205 subdiagonal[0] = M[1];
206 subdiagonal[1] = M[6];
207 subdiagonal[2] = M[11];
208
209 return 0;
210 }
211
212 /*
213 Return largest eigenvalue of symmetric tridiagonal matrix.
214 Matrix Algorithms: Basic decompositions. By GW Stewart. Chapter 3.
215 */
max_eigenvalue_of_tridiag_44(double * diagonal,double * subdiagonal)216 double max_eigenvalue_of_tridiag_44(
217 double *diagonal, /* double[4] */
218 double *subdiagonal) /* double[3] */
219 {
220 int count;
221 double lower, upper, t0, t1, d, eps, eigenv;
222 double *a = diagonal;
223 double *b = subdiagonal;
224
225 /* upper and lower bounds using Gerschgorin's theorem */
226 t0 = fabs(b[0]);
227 t1 = fabs(b[1]);
228 lower = a[0] - t0;
229 upper = a[0] + t0;
230 d = a[1] - t0 - t1;
231 lower = MIN(lower, d);
232 d = a[1] + t0 + t1;
233 upper = MAX(upper, d);
234 t0 = fabs(b[2]);
235 d = a[2] - t0 - t1;
236 lower = MIN(lower, d);
237 d = a[2] + t0 + t1;
238 upper = MAX(upper, d);
239 d = a[3] - t0;
240 lower = MIN(lower, d);
241 d = a[3] + t0;
242 upper = MAX(upper, d);
243
244 /* precision */
245 eps = (4.0 * (fabs(lower) + fabs(upper))) * DBL_EPSILON;
246
247 /* interval bisection until width is less than tolerance */
248 while (fabs(upper - lower) > eps) {
249
250 eigenv = (upper + lower) / 2.0;
251 if ((eigenv == upper) || (eigenv == lower))
252 return eigenv;
253
254 /* counting pivots < 0 */
255 d = a[0] - eigenv;
256 count = (d < 0.0) ? 1 : 0;
257 if (fabs(d) < eps)
258 d = eps;
259 d = a[1] - eigenv - b[0]*b[0] / d;
260 if (d < 0.0)
261 count++;
262 if (fabs(d) < eps)
263 d = eps;
264 d = a[2] - eigenv - b[1]*b[1] / d;
265 if (d < 0.0)
266 count++;
267 if (fabs(d) < eps)
268 d = eps;
269 d = a[3] - eigenv - b[2]*b[2] / d;
270 if (d < 0.0)
271 count++;
272
273 if (count < 4)
274 lower = eigenv;
275 else
276 upper = eigenv;
277 }
278
279 return (upper + lower) / 2.0;
280 }
281
282 /*
283 Eigenvector of symmetric tridiagonal 4x4 matrix using Cramer's rule.
284 */
eigenvector_of_symmetric_44(double * matrix,double * vector,double * buffer)285 int eigenvector_of_symmetric_44(
286 double *matrix, /* double[16] */
287 double *vector, /* double[4] */
288 double *buffer) /* double[12] */
289 {
290 double n, eps;
291 double *M = matrix;
292 double *v = vector;
293 double *t = buffer;
294
295 /* eps: minimum length of eigenvector to use */
296 eps = (M[0]*M[5]*M[10]*M[15] - M[1]*M[1]*M[11]*M[11]) * 1e-6;
297 eps *= eps;
298 if (eps < EPSILON)
299 eps = EPSILON;
300
301 t[0] = M[10] * M[15];
302 t[1] = M[11] * M[11];
303 t[2] = M[6] * M[15];
304 t[3] = M[11] * M[7];
305 t[4] = M[6] * M[11];
306 t[5] = M[10] * M[7];
307 t[6] = M[2] * M[15];
308 t[7] = M[11] * M[3];
309 t[8] = M[2] * M[11];
310 t[9] = M[10] * M[3];
311 t[10] = M[2] * M[7];
312 t[11] = M[6] * M[3];
313
314 v[0] = t[1]*M[1] + t[6]*M[6] + t[9]*M[7];
315 v[0] -= t[0]*M[1] + t[7]*M[6] + t[8]*M[7];
316 v[1] = t[2]*M[1] + t[7]*M[5] + t[10]*M[7];
317 v[1] -= t[3]*M[1] + t[6]*M[5] + t[11]*M[7];
318 v[2] = t[5]*M[1] + t[8]*M[5] + t[11]*M[6];
319 v[2] -= t[4]*M[1] + t[9]*M[5] + t[10]*M[6];
320 v[3] = t[0]*M[5] + t[3]*M[6] + t[4]*M[7];
321 v[3] -= t[1]*M[5] + t[2]*M[6] + t[5]*M[7];
322 n = v[0]*v[0] + v[1]*v[1] + v[2]*v[2] + v[3]*v[3];
323
324 if (n < eps) {
325 v[0] = t[0]*M[0] + t[7]*M[2] + t[8]*M[3];
326 v[0] -= t[1]*M[0] + t[6]*M[2] + t[9]*M[3];
327 v[1] = t[3]*M[0] + t[6]*M[1] + t[11]*M[3];
328 v[1] -= t[2]*M[0] + t[7]*M[1] + t[10]*M[3];
329 v[2] = t[4]*M[0] + t[9]*M[1] + t[10]*M[2];
330 v[2] -= t[5]*M[0] + t[8]*M[1] + t[11]*M[2];
331 v[3] = t[1]*M[1] + t[2]*M[2] + t[5]*M[3];
332 v[3] -= t[0]*M[1] + t[3]*M[2] + t[4]*M[3];
333 n = v[0]*v[0] + v[1]*v[1] + v[2]*v[2] + v[3]*v[3];
334 }
335
336 if (n < eps) {
337 t[0] = M[2] * M[7];
338 t[1] = M[3] * M[6];
339 t[2] = M[1] * M[7];
340 t[3] = M[3] * M[5];
341 t[4] = M[1] * M[6];
342 t[5] = M[2] * M[5];
343 t[6] = M[0] * M[7];
344 t[7] = M[3] * M[1];
345 t[8] = M[0] * M[6];
346 t[9] = M[2] * M[1];
347 t[10] = M[0] * M[5];
348 t[11] = M[1] * M[1];
349
350 v[0] = t[1]*M[3] + t[6]*M[11] + t[9]*M[15];
351 v[0] -= t[0]*M[3] + t[7]*M[11] + t[8]*M[15];
352 v[1] = t[2]*M[3] + t[7]*M[7] + t[10]*M[15];
353 v[1] -= t[3]*M[3] + t[6]*M[7] + t[11]*M[15];
354 v[2] = t[5]*M[3] + t[8]*M[7] + t[11]*M[11];
355 v[2] -= t[4]*M[3] + t[9]*M[7] + t[10]*M[11];
356 v[3] = t[0]*M[7] + t[3]*M[11] + t[4]*M[15];
357 v[3] -= t[1]*M[7] + t[2]*M[11] + t[5]*M[15];
358 n = v[0]*v[0] + v[1]*v[1] + v[2]*v[2] + v[3]*v[3];
359 }
360
361 if (n < eps) {
362 v[0] = t[8]*M[11] + t[0]*M[2] + t[7]*M[10];
363 v[0] -= t[6]*M[10] + t[9]*M[11] + t[1]*M[2];
364 v[1] = t[6]*M[6] + t[11]*M[11] + t[3]*M[2];
365 v[1] -= t[10]*M[11] + t[2]*M[2] + t[7]*M[6];
366 v[2] = t[10]*M[10] + t[4]*M[2] + t[9]*M[6];
367 v[2] -= t[8]*M[6] + t[11]*M[10] + t[5]*M[2];
368 v[3] = t[2]*M[10] + t[5]*M[11] + t[1]*M[6];
369 v[3] -= t[4]*M[11] + t[0]*M[6] + t[3]*M[10];
370 n = v[0]*v[0] + v[1]*v[1] + v[2]*v[2] + v[3]*v[3];
371 }
372
373 if (n < eps)
374 return -1;
375
376 n = sqrt(n);
377 v[0] /= n;
378 v[1] /= n;
379 v[2] /= n;
380 v[3] /= n;
381
382 return 0;
383 }
384
385 /*
386 Matrix 2x2 inversion.
387 */
invert_matrix22(double * matrix,double * result)388 int invert_matrix22(
389 double *matrix, /* double[4] */
390 double *result) /* double[4] */
391 {
392 double det = matrix[0]*matrix[3] - matrix[1]*matrix[2];
393
394 if (ISZERO(det))
395 return -1;
396
397 result[0] = matrix[3] / det;
398 result[1] = -matrix[1] / det;
399 result[2] = -matrix[2] / det;
400 result[3] = matrix[0] / det;
401
402 return 0;
403 }
404
405 /*
406 Matrix 3x3 inversion.
407 */
invert_matrix33(double * matrix,double * result)408 int invert_matrix33(
409 double *matrix, /* double[9] */
410 double *result) /* double[9] */
411 {
412 double *M = matrix;
413 double det;
414 int i;
415
416 result[0] = M[8]*M[4] - M[7]*M[5];
417 result[1] = M[7]*M[2] - M[8]*M[1];
418 result[2] = M[5]*M[1] - M[4]*M[2];
419 result[3] = M[6]*M[5] - M[8]*M[3];
420 result[4] = M[8]*M[0] - M[6]*M[2];
421 result[5] = M[3]*M[2] - M[5]*M[0];
422 result[6] = M[7]*M[3] - M[6]*M[4];
423 result[7] = M[6]*M[1] - M[7]*M[0];
424 result[8] = M[4]*M[0] - M[3]*M[1];
425
426 det = M[0]*result[0] + M[3]*result[1] + M[6]*result[2];
427
428 if (ISZERO(det))
429 return -1;
430
431 det = 1.0 / det;
432 for (i = 0; i < 9; i++)
433 result[i] *= det;
434
435 return 0;
436 }
437
438 /*
439 Matrix 4x4 inversion.
440 */
invert_matrix44(double * matrix,double * result)441 int invert_matrix44(
442 double *matrix, /* double[16] */
443 double *result) /* double[16] */
444 {
445 double *M = matrix;
446 double t[12];
447 double det;
448 int i;
449
450 t[0] = M[10] * M[15];
451 t[1] = M[14] * M[11];
452 t[2] = M[6] * M[15];
453 t[3] = M[14] * M[7];
454 t[4] = M[6] * M[11];
455 t[5] = M[10] * M[7];
456 t[6] = M[2] * M[15];
457 t[7] = M[14] * M[3];
458 t[8] = M[2] * M[11];
459 t[9] = M[10] * M[3];
460 t[10] = M[2] * M[7];
461 t[11] = M[6] * M[3];
462
463 result[0] = t[0]*M[5] + t[3]*M[9] + t[4]*M[13];
464 result[0] -= t[1]*M[5] + t[2]*M[9] + t[5]*M[13];
465 result[1] = t[1]*M[1] + t[6]*M[9] + t[9]*M[13];
466 result[1] -= t[0]*M[1] + t[7]*M[9] + t[8]*M[13];
467 result[2] = t[2]*M[1] + t[7]*M[5] + t[10]*M[13];
468 result[2] -= t[3]*M[1] + t[6]*M[5] + t[11]*M[13];
469 result[3] = t[5]*M[1] + t[8]*M[5] + t[11]*M[9];
470 result[3] -= t[4]*M[1] + t[9]*M[5] + t[10]*M[9];
471 result[4] = t[1]*M[4] + t[2]*M[8] + t[5]*M[12];
472 result[4] -= t[0]*M[4] + t[3]*M[8] + t[4]*M[12];
473 result[5] = t[0]*M[0] + t[7]*M[8] + t[8]*M[12];
474 result[5] -= t[1]*M[0] + t[6]*M[8] + t[9]*M[12];
475 result[6] = t[3]*M[0] + t[6]*M[4] + t[11]*M[12];
476 result[6] -= t[2]*M[0] + t[7]*M[4] + t[10]*M[12];
477 result[7] = t[4]*M[0] + t[9]*M[4] + t[10]*M[8];
478 result[7] -= t[5]*M[0] + t[8]*M[4] + t[11]*M[8];
479
480 t[0] = M[8]*M[13];
481 t[1] = M[12]*M[9];
482 t[2] = M[4]*M[13];
483 t[3] = M[12]*M[5];
484 t[4] = M[4]*M[9];
485 t[5] = M[8]*M[5];
486 t[6] = M[0]*M[13];
487 t[7] = M[12]*M[1];
488 t[8] = M[0]*M[9];
489 t[9] = M[8]*M[1];
490 t[10] = M[0]*M[5];
491 t[11] = M[4]*M[1];
492
493 result[8] = t[0]*M[7] + t[3]*M[11] + t[4]*M[15];
494 result[8] -= t[1]*M[7] + t[2]*M[11] + t[5]*M[15];
495 result[9] = t[1]*M[3] + t[6]*M[11] + t[9]*M[15];
496 result[9] -= t[0]*M[3] + t[7]*M[11] + t[8]*M[15];
497 result[10] = t[2]*M[3] + t[7]*M[7] + t[10]*M[15];
498 result[10]-= t[3]*M[3] + t[6]*M[7] + t[11]*M[15];
499 result[11] = t[5]*M[3] + t[8]*M[7] + t[11]*M[11];
500 result[11]-= t[4]*M[3] + t[9]*M[7] + t[10]*M[11];
501 result[12] = t[2]*M[10] + t[5]*M[14] + t[1]*M[6];
502 result[12]-= t[4]*M[14] + t[0]*M[6] + t[3]*M[10];
503 result[13] = t[8]*M[14] + t[0]*M[2] + t[7]*M[10];
504 result[13]-= t[6]*M[10] + t[9]*M[14] + t[1]*M[2];
505 result[14] = t[6]*M[6] + t[11]*M[14] + t[3]*M[2];
506 result[14]-= t[10]*M[14] + t[2]*M[2] + t[7]*M[6];
507 result[15] = t[10]*M[10] + t[4]*M[2] + t[9]*M[6];
508 result[15]-= t[8]*M[6] + t[11]*M[10] + t[5]*M[2];
509
510 det = M[0]*result[0] + M[4]*result[1] + M[8]*result[2] + M[12]*result[3];
511
512 if (ISZERO(det))
513 return -1;
514
515 det = 1.0 / det;
516 for (i = 0; i < 16; i++)
517 result[i] *= det;
518
519 return 0;
520 }
521
522 /*
523 Invert square matrix using LU factorization with pivoting.
524 The input matrix is altered.
525 */
invert_matrix(Py_ssize_t size,double * matrix,double * result,Py_ssize_t * buffer)526 int invert_matrix(
527 Py_ssize_t size,
528 double *matrix, /* [size*size] */
529 double *result, /* [size*size] */
530 Py_ssize_t *buffer) /* [2*size] */
531 {
532 double temp, temp1;
533 double *M = matrix;
534 Py_ssize_t *seq = buffer;
535 Py_ssize_t *iseq = buffer + size;
536 Py_ssize_t i, j, k, ks, ps, ksk, js, p, is;
537
538 /* sequence of pivots */
539 for (k = 0; k < size; k++)
540 seq[k] = k;
541
542 /* forward solution */
543 for (k = 0; k < size-1; k++) {
544 ks = k*size;
545 ksk = ks + k;
546
547 /* pivoting: find maximum coefficient in column */
548 p = k;
549 temp = fabs(M[ksk]);
550 for (i = k+1; i < size; i++) {
551 temp1 = fabs(M[i*size + k]);
552 if (temp < temp1) {
553 temp = temp1;
554 p = i;
555 }
556 }
557 /* permutate lines with index k and p */
558 if (p != k) {
559 ps = p*size;
560 for (i = 0; i < size; i++) {
561 temp = M[ks + i];
562 M[ks + i] = M[ps + i];
563 M[ps + i] = temp;
564 }
565 i = seq[k];
566 seq[k] = seq[p];
567 seq[p] = i;
568 }
569
570 /* test for singular matrix */
571 if (fabs(M[ksk]) < PIVOT_TOLERANCE)
572 return -1;
573
574 /* elimination */
575 temp = M[ksk];
576 for (j = k+1; j < size; j++) {
577 M[j*size + k] /= temp;
578 }
579 for (j = k+1; j < size; j++) {
580 js = j * size;
581 temp = M[js + k];
582 for(i = k+1; i < size; i++) {
583 M[js + i] -= temp * M[ks + i];
584 }
585 M[js + k] = temp;
586 }
587 }
588
589 /* Backward substitution with identity matrix */
590 memset(result, 0, size*size*sizeof(double));
591 for (i = 0; i < size; i++) {
592 result[i*size + seq[i]] = 1.0;
593 iseq[seq[i]] = i;
594 }
595
596 for (i = 0; i < size; i++) {
597 is = iseq[i];
598 for (k = 1; k < size; k++) {
599 ks = k*size;
600 temp = 0.0;
601 for (j = is; j < k; j++)
602 temp += M[ks + j] * result[j*size + i];
603 result[ks + i] -= temp;
604 }
605 for (k = size-1; k >= 0; k--) {
606 ks = k*size;
607 temp = result[ks + i];
608 for (j = k+1; j < size; j++)
609 temp -= M[ks + j] * result[j*size + i];
610 result[ks + i] = temp / M[ks + k];
611 }
612 }
613 return 0;
614 }
615
616 /*
617 Quaternion from matrix.
618 */
quaternion_from_matrix(double * matrix,double * quaternion)619 int quaternion_from_matrix(
620 double *matrix, /* double[16] */
621 double *quaternion) /* double[4] */
622 {
623 double *M = matrix;
624 double *q = quaternion;
625 double s;
626
627 if (ISZERO(M[15]))
628 return -1;
629
630 if ((M[0] + M[5] + M[10]) > 0.0) {
631 s = 0.5 / sqrt(M[0] + M[5] + M[10] + M[15]);
632 q[0] = 0.25 / s;
633 q[3] = (M[4] - M[1]) * s;
634 q[2] = (M[2] - M[8]) * s;
635 q[1] = (M[9] - M[6]) * s;
636 } else if ((M[0] > M[5]) && (M[0] > M[10])) {
637 s = 0.5 / sqrt(M[0] - (M[5] + M[10]) + M[15]);
638 q[1] = 0.25 / s;
639 q[2] = (M[4] + M[1]) * s;
640 q[3] = (M[2] + M[8]) * s;
641 q[0] = (M[9] - M[6]) * s;
642 } else if (M[5] > M[10]) {
643 s = 0.5 / sqrt(M[5] - (M[0] + M[10]) + M[15]);
644 q[2] = 0.25 / s;
645 q[1] = (M[4] + M[1]) * s;
646 q[0] = (M[2] - M[8]) * s;
647 q[3] = (M[9] + M[6]) * s;
648 } else {
649 s = 0.5 / sqrt(M[10] - (M[0] + M[5]) + M[15]);
650 q[3] = 0.25 / s;
651 q[0] = (M[4] - M[1]) * s;
652 q[1] = (M[2] + M[8]) * s;
653 q[2] = (M[9] + M[6]) * s;
654 }
655
656 if (M[15] != 1.0) {
657 s = 1.0 / sqrt(M[15]);
658 q[0] *= s;
659 q[1] *= s;
660 q[2] *= s;
661 q[3] *= s;
662 }
663 return 0;
664 }
665
666 /*
667 Quaternion to rotation matrix.
668 */
quaternion_matrix(double * quat,double * matrix)669 int quaternion_matrix(
670 double *quat, /* double[4] */
671 double *matrix) /* double[16] */
672 {
673 double *M = matrix;
674 double *q = quat;
675 double n = sqrt(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]);
676
677 if (n < EPSILON) {
678 /* return identity matrix */
679 memset(M, 0, 16*sizeof(double));
680 M[0] = M[5] = M[10] = M[15] = 1.0;
681 } else {
682 q[0] /= n;
683 q[1] /= n;
684 q[2] /= n;
685 q[3] /= n;
686 {
687 double x2 = q[1]+q[1];
688 double y2 = q[2]+q[2];
689 double z2 = q[3]+q[3];
690 {
691 double xx2 = q[1]*x2;
692 double yy2 = q[2]*y2;
693 double zz2 = q[3]*z2;
694 M[0] = 1.0 - yy2 - zz2;
695 M[5] = 1.0 - xx2 - zz2;
696 M[10] = 1.0 - xx2 - yy2;
697 }{
698 double yz2 = q[2]*z2;
699 double wx2 = q[0]*x2;
700 M[6] = yz2 - wx2;
701 M[9] = yz2 + wx2;
702 }{
703 double xy2 = q[1]*y2;
704 double wz2 = q[0]*z2;
705 M[1] = xy2 - wz2;
706 M[4] = xy2 + wz2;
707 }{
708 double xz2 = q[1]*z2;
709 double wy2 = q[0]*y2;
710 M[8] = xz2 - wy2;
711 M[2] = xz2 + wy2;
712 }
713 M[3] = M[7] = M[11] = M[12] = M[13] = M[14] = 0.0;
714 M[15] = 1.0;
715 }
716 }
717 return 0;
718 }
719
720 /*
721 Unit quaternion from unit sphere points.
722 */
quaternion_from_sphere_points(double * point0,double * point1,double * quat)723 int quaternion_from_sphere_points(
724 double *point0, /* double[3] */
725 double *point1, /* double[3] */
726 double *quat) /* double[4] */
727 {
728 quat[0] = point0[0]*point1[0] + point0[1]*point1[1] + point0[2]*point1[2];
729 quat[1] = point0[1]*point1[2] - point0[2]*point1[1];
730 quat[2] = point0[2]*point1[0] - point0[0]*point1[2];
731 quat[3] = point0[0]*point1[1] - point0[1]*point1[0];
732 return 0;
733 }
734
735 /*
736 Unit quaternion to unit sphere points.
737 */
quaternion_to_sphere_points(double * quat,double * point0,double * point1)738 int quaternion_to_sphere_points(
739 double *quat, /* double[4] */
740 double *point0, /* double[3] */
741 double *point1) /* double[3] */
742 {
743 double n = sqrt(quat[0]*quat[0] + quat[1]*quat[1]);
744
745 if (n < EPSILON) {
746 point0[0] = 0.0;
747 point0[1] = 1.0;
748 point0[2] = 0.0;
749 } else {
750 point0[0] = -quat[2] / n;
751 point0[1] = quat[1] / n;
752 point0[2] = 0.0;
753 }
754
755 point1[0] = quat[0]*point0[0] - quat[3]*point0[1];
756 point1[1] = quat[0]*point0[1] + quat[3]*point0[0];
757 point1[2] = quat[1]*point0[1] - quat[2]*point0[0];
758
759 if (quat[0] < 0.0) {
760 point0[0] = -point0[0];
761 point0[1] = -point0[1];
762 }
763
764 return 0;
765 }
766
767 /*****************************************************************************/
768 /* Python functions */
769
770 /*
771 Numpy array converters for use with PyArg_Parse functions.
772 */
773 static int
PyConverter_DoubleArray(PyObject * object,PyObject ** address)774 PyConverter_DoubleArray(
775 PyObject *object,
776 PyObject **address)
777 {
778 *address = PyArray_FROM_OTF(object, NPY_DOUBLE, NPY_IN_ARRAY);
779 if (*address == NULL) return NPY_FAIL;
780 return NPY_SUCCEED;
781 }
782
783 static int
PyConverter_AnyDoubleArray(PyObject * object,PyObject ** address)784 PyConverter_AnyDoubleArray(
785 PyObject *object,
786 PyObject **address)
787 {
788 PyArrayObject *obj = (PyArrayObject *)object;
789 if (PyArray_Check(object) && obj->descr->type_num == PyArray_DOUBLE) {
790 *address = object;
791 Py_INCREF(object);
792 return NPY_SUCCEED;
793 } else {
794 *address = PyArray_FROM_OTF(object, NPY_DOUBLE, NPY_ALIGNED);
795 if (*address == NULL) {
796 PyErr_Format(PyExc_ValueError, "can not convert to array");
797 return NPY_FAIL;
798 }
799 return NPY_SUCCEED;
800 }
801 }
802
803 static int
PyConverter_DoubleArrayOrNone(PyObject * object,PyObject ** address)804 PyConverter_DoubleArrayOrNone(
805 PyObject *object,
806 PyObject **address)
807 {
808 if ((object == NULL) || (object == Py_None)) {
809 *address = NULL;
810 } else {
811 *address = PyArray_FROM_OTF(object, NPY_DOUBLE, NPY_IN_ARRAY);
812 if (*address == NULL) {
813 PyErr_Format(PyExc_ValueError, "can not convert to array");
814 return NPY_FAIL;
815 }
816 }
817 return NPY_SUCCEED;
818 }
819
820 static int
PyConverter_DoubleMatrix44(PyObject * object,PyObject ** address)821 PyConverter_DoubleMatrix44(
822 PyObject *object,
823 PyObject **address)
824 {
825 PyArrayObject *obj;
826 *address = PyArray_FROM_OTF(object, NPY_DOUBLE, NPY_IN_ARRAY);
827 if (*address == NULL) {
828 PyErr_Format(PyExc_ValueError, "can not convert to array");
829 return NPY_FAIL;
830 }
831 obj = (PyArrayObject *) *address;
832 if ((PyArray_NDIM(obj) != 2) || (PyArray_DIM(obj, 0) != 4)
833 || (PyArray_DIM(obj, 1) != 4) || PyArray_ISCOMPLEX(obj)) {
834 PyErr_Format(PyExc_ValueError, "not a 4x4 matrix");
835 Py_DECREF(*address);
836 *address = NULL;
837 return NPY_FAIL;
838 }
839 return NPY_SUCCEED;
840 }
841
842 static int
PyConverter_DoubleMatrix44Copy(PyObject * object,PyObject ** address)843 PyConverter_DoubleMatrix44Copy(
844 PyObject *object,
845 PyObject **address)
846 {
847 PyArrayObject *obj;
848 *address = PyArray_FROM_OTF(object, NPY_DOUBLE,
849 NPY_ENSURECOPY|NPY_IN_ARRAY);
850 if (*address == NULL) {
851 PyErr_Format(PyExc_ValueError, "can not convert to array");
852 return NPY_FAIL;
853 }
854 obj = (PyArrayObject *) *address;
855 if ((PyArray_NDIM(obj) != 2) || (PyArray_DIM(obj, 0) != 4)
856 || (PyArray_DIM(obj, 1) != 4) || PyArray_ISCOMPLEX(obj)) {
857 PyErr_Format(PyExc_ValueError, "not a 4x4 matrix");
858 Py_DECREF(*address);
859 *address = NULL;
860 return NPY_FAIL;
861 }
862 return NPY_SUCCEED;
863 }
864
865 static int
PyConverter_DoubleVector3(PyObject * object,PyObject ** address)866 PyConverter_DoubleVector3(
867 PyObject *object,
868 PyObject **address)
869 {
870 PyArrayObject *obj;
871 *address = PyArray_FROM_OTF(object, NPY_DOUBLE, NPY_IN_ARRAY);
872 if (*address == NULL) {
873 PyErr_Format(PyExc_ValueError, "can not convert to array");
874 return NPY_FAIL;
875 }
876 obj = (PyArrayObject *) *address;
877 if ((PyArray_NDIM(obj) != 1) || (PyArray_DIM(obj, 0) < 3)
878 || PyArray_ISCOMPLEX(obj)) {
879 PyErr_Format(PyExc_ValueError, "not a vector3");
880 Py_DECREF(*address);
881 *address = NULL;
882 return NPY_FAIL;
883 }
884 return NPY_SUCCEED;
885 }
886
887 static int
PyConverter_DoubleVector4(PyObject * object,PyObject ** address)888 PyConverter_DoubleVector4(
889 PyObject *object,
890 PyObject **address)
891 {
892 PyArrayObject *obj;
893 *address = PyArray_FROM_OTF(object, NPY_DOUBLE, NPY_IN_ARRAY);
894 if (*address == NULL) {
895 PyErr_Format(PyExc_ValueError, "can not convert to array");
896 return NPY_FAIL;
897 }
898 obj = (PyArrayObject *) *address;
899 if ((PyArray_NDIM(obj) != 1) || (PyArray_DIM(obj, 0) < 4)
900 || PyArray_ISCOMPLEX(obj)) {
901 PyErr_Format(PyExc_ValueError, "not a vector4");
902 Py_DECREF(*address);
903 *address = NULL;
904 return NPY_FAIL;
905 }
906 return NPY_SUCCEED;
907 }
908
909 static int
PyConverter_DoubleVector4Copy(PyObject * object,PyObject ** address)910 PyConverter_DoubleVector4Copy(
911 PyObject *object,
912 PyObject **address)
913 {
914 PyArrayObject *obj;
915 *address = PyArray_FROM_OTF(object, NPY_DOUBLE,
916 NPY_ENSURECOPY|NPY_IN_ARRAY);
917 if (*address == NULL) {
918 PyErr_Format(PyExc_ValueError, "can not convert to array");
919 return NPY_FAIL;
920 }
921 obj = (PyArrayObject *) *address;
922 if ((PyArray_NDIM(obj) != 1) || (PyArray_DIM(obj, 0) < 4)
923 || PyArray_ISCOMPLEX(obj)) {
924 PyErr_Format(PyExc_ValueError, "not a vector4");
925 Py_DECREF(*address);
926 *address = NULL;
927 return NPY_FAIL;
928 }
929 return NPY_SUCCEED;
930 }
931
932 static int
PyConverter_DoubleVector3OrNone(PyObject * object,PyObject ** address)933 PyConverter_DoubleVector3OrNone(
934 PyObject *object,
935 PyObject **address)
936 {
937 if ((object == NULL) || (object == Py_None)) {
938 *address = NULL;
939 } else {
940 PyArrayObject *obj;
941 *address = PyArray_FROM_OTF(object, NPY_DOUBLE, NPY_IN_ARRAY);
942 if (*address == NULL) {
943 PyErr_Format(PyExc_ValueError, "can not convert to array");
944 return NPY_FAIL;
945 }
946 obj = (PyArrayObject *) *address;
947 if ((PyArray_NDIM(obj) != 1) || (PyArray_DIM(obj, 0) < 3)
948 || PyArray_ISCOMPLEX(obj)) {
949 PyErr_Format(PyExc_ValueError, "not a vector3");
950 Py_DECREF(*address);
951 *address = NULL;
952 return NPY_FAIL;
953 }
954 }
955 return NPY_SUCCEED;
956 }
957
958 static int
PyOutputConverter_AnyDoubleArrayOrNone(PyObject * object,PyArrayObject ** address)959 PyOutputConverter_AnyDoubleArrayOrNone(
960 PyObject *object,
961 PyArrayObject **address)
962 {
963 PyArrayObject *obj = (PyArrayObject *)object;
964 if ((object == NULL) || (object == Py_None)) {
965 *address = NULL;
966 return NPY_SUCCEED;
967 }
968 if (PyArray_Check(object) && (obj->descr->type_num == PyArray_DOUBLE)) {
969 Py_INCREF(object);
970 *address = (PyArrayObject *)object;
971 return NPY_SUCCEED;
972 } else {
973 PyErr_Format(PyExc_TypeError, "output must be array of type double");
974 *address = NULL;
975 return NPY_FAIL;
976 }
977 }
978
979 /*
980 Return i-th element of Python sequence as long, or -1 on failure.
981 */
PySequence_GetInteger(PyObject * obj,Py_ssize_t i)982 long PySequence_GetInteger(PyObject *obj, Py_ssize_t i)
983 {
984 long value;
985 PyObject *item = PySequence_GetItem(obj, i);
986 if (item == NULL ||
987 #if PY_MAJOR_VERSION < 3
988 !PyInt_Check(item)
989 #else
990 !PyLong_Check(item)
991 #endif
992 ) {
993 PyErr_Format(PyExc_ValueError, "expected integer number");
994 Py_XDECREF(item);
995 return -1;
996 }
997
998 #if PY_MAJOR_VERSION < 3
999 value = PyInt_AsLong(item);
1000 #else
1001 value = PyLong_AsLong(item);
1002 #endif
1003 Py_XDECREF(item);
1004 return value;
1005 }
1006
1007 /*
1008 Equivalence of transformations.
1009 */
1010 char py_is_same_transform_doc[] =
1011 "Return True if two matrices perform same transformation.";
1012
1013 static PyObject *
py_is_same_transform(PyObject * obj,PyObject * args,PyObject * kwds)1014 py_is_same_transform(
1015 PyObject *obj,
1016 PyObject *args,
1017 PyObject *kwds)
1018 {
1019 PyArrayObject *matrix0 = NULL;
1020 PyArrayObject *matrix1 = NULL;
1021 int result;
1022 static char *kwlist[] = {"matrix0", "matrix1", NULL};
1023
1024 if (!PyArg_ParseTupleAndKeywords(args, kwds, "O&O&", kwlist,
1025 PyConverter_DoubleMatrix44, &matrix0,
1026 PyConverter_DoubleMatrix44, &matrix1)) goto _fail;
1027
1028 {
1029 double *M0 = (double *)PyArray_DATA(matrix0);
1030 double *M1 = (double *)PyArray_DATA(matrix1);
1031 double t0 = M0[15];
1032 double t1 = M1[15];
1033 double t;
1034 int i;
1035 if (ISZERO(t0) || ISZERO(t1)) {
1036 result = 0;
1037 } else {
1038 result = 1;
1039 for (i = 0; i < 16; i++) {
1040 t = M1[i] / t1;
1041 if (fabs(M0[i]/t0 - t) > (1e-8 + 1e-5*fabs(t))) {
1042 result = 0;
1043 break;
1044 }
1045 }
1046 }
1047 }
1048
1049 Py_DECREF(matrix0);
1050 Py_DECREF(matrix1);
1051 if (result)
1052 Py_RETURN_TRUE;
1053 else
1054 Py_RETURN_FALSE;
1055
1056 _fail:
1057 Py_XDECREF(matrix0);
1058 Py_XDECREF(matrix1);
1059 return NULL;
1060 }
1061
1062 /*
1063 Identity matrix.
1064 */
1065 char py_identity_matrix_doc[] = "Return identity/unit matrix.";
1066
1067 static PyObject *
py_identity_matrix(PyObject * obj,PyObject * args)1068 py_identity_matrix(
1069 PyObject *obj,
1070 PyObject *args)
1071 {
1072 PyArrayObject *result = NULL;
1073 PyArray_Descr *type = NULL;
1074 Py_ssize_t dims[] = {4, 4};
1075
1076 type = PyArray_DescrFromType(NPY_DOUBLE);
1077 result = (PyArrayObject*)PyArray_Zeros(2, dims, type, 0);
1078 if (result == NULL) {
1079 PyErr_Format(PyExc_MemoryError, "unable to allocate matrix");
1080 goto _fail;
1081 }
1082 {
1083 double *M = (double *)PyArray_DATA(result);
1084 M[0] = M[5] = M[10] = M[15] = 1.0;
1085 }
1086
1087 return PyArray_Return(result);
1088
1089 _fail:
1090 Py_XDECREF(result);
1091 return NULL;
1092 }
1093
1094 /*
1095 Translation matrix.
1096 */
1097 char py_translation_matrix_doc[] =
1098 "Return matrix to translate by direction vector.";
1099
1100 static PyObject *
py_translation_matrix(PyObject * obj,PyObject * args,PyObject * kwds)1101 py_translation_matrix(
1102 PyObject *obj,
1103 PyObject *args,
1104 PyObject *kwds)
1105 {
1106 PyArrayObject *result = NULL;
1107 PyArrayObject *direction = NULL;
1108 PyArray_Descr *type = NULL;
1109 Py_ssize_t dims[] = {4, 4};
1110 static char *kwlist[] = {"direction", NULL};
1111
1112 if (!PyArg_ParseTupleAndKeywords(args, kwds, "O&", kwlist,
1113 PyConverter_DoubleVector3, &direction)) goto _fail;
1114
1115 type = PyArray_DescrFromType(NPY_DOUBLE);
1116 result = (PyArrayObject*)PyArray_Zeros(2, dims, type, 0);
1117 if (result == NULL) {
1118 PyErr_Format(PyExc_MemoryError, "unable to allocate matrix");
1119 goto _fail;
1120 }
1121 {
1122 double *M = (double *)PyArray_DATA(result);
1123 double *d = (double *)PyArray_DATA(direction);
1124 M[0] = M[5] = M[10] = M[15] = 1.0;
1125 M[3] = d[0];
1126 M[7] = d[1];
1127 M[11] = d[2];
1128 }
1129
1130 Py_DECREF(direction);
1131 return PyArray_Return(result);
1132
1133 _fail:
1134 Py_XDECREF(direction);
1135 Py_XDECREF(result);
1136 return NULL;
1137 }
1138
1139 /*
1140 Reflection matrix.
1141 */
1142 char py_reflection_matrix_doc[] =
1143 "Return matrix to mirror at plane defined by point and normal vector.";
1144
1145 static PyObject *
py_reflection_matrix(PyObject * obj,PyObject * args,PyObject * kwds)1146 py_reflection_matrix(
1147 PyObject *obj,
1148 PyObject *args,
1149 PyObject *kwds)
1150 {
1151 PyArrayObject *result = NULL;
1152 PyArrayObject *point = NULL;
1153 PyArrayObject *normal = NULL;
1154 Py_ssize_t dims[] = {4, 4};
1155 static char *kwlist[] = {"point", "normal", NULL};
1156
1157 if (!PyArg_ParseTupleAndKeywords(args, kwds, "O&O&", kwlist,
1158 PyConverter_DoubleVector3, &point,
1159 PyConverter_DoubleVector3, &normal)) goto _fail;
1160
1161 result = (PyArrayObject*)PyArray_SimpleNew(2, dims, NPY_DOUBLE);
1162 if (result == NULL) {
1163 PyErr_Format(PyExc_MemoryError, "unable to allocate matrix");
1164 goto _fail;
1165 }
1166 {
1167 double *M = (double *)PyArray_DATA(result);
1168 double *p = (double *)PyArray_DATA(point);
1169 double *n = (double *)PyArray_DATA(normal);
1170 double nx = n[0];
1171 double ny = n[1];
1172 double nz = n[2];
1173 double t = sqrt(nx*nx + ny*ny + nz*nz);
1174 if (t < EPSILON) {
1175 PyErr_Format(PyExc_ValueError, "invalid normal vector");
1176 goto _fail;
1177 }
1178 nx /= t;
1179 ny /= t;
1180 nz /= t;
1181 M[12] = M[13] = M[14] = 0.0;
1182 M[15] = 1.0;
1183 M[0] = 1.0 - 2.0 * nx *nx;
1184 M[5] = 1.0 - 2.0 * ny *ny;
1185 M[10] = 1.0 - 2.0 * nz *nz;
1186 M[1] = M[4] = -2.0 * nx *ny;
1187 M[2] = M[8] = -2.0 * nx *nz;
1188 M[6] = M[9] = -2.0 * ny *nz;
1189 t = 2.0 * (p[0]*nx + p[1]*ny + p[2]*nz);
1190 M[3] = nx * t;
1191 M[7] = ny * t;
1192 M[11] = nz * t;
1193 }
1194
1195 Py_DECREF(point);
1196 Py_DECREF(normal);
1197 return PyArray_Return(result);
1198
1199 _fail:
1200 Py_XDECREF(point);
1201 Py_XDECREF(normal);
1202 Py_XDECREF(result);
1203 return NULL;
1204 }
1205
1206 /*
1207 Rotation matrix.
1208 */
1209 char py_rotation_matrix_doc[] =
1210 "Return matrix to rotate about axis defined by point and direction.";
1211
1212 static PyObject *
py_rotation_matrix(PyObject * obj,PyObject * args,PyObject * kwds)1213 py_rotation_matrix(
1214 PyObject *obj,
1215 PyObject *args,
1216 PyObject *kwds)
1217 {
1218 PyArrayObject *result = NULL;
1219 PyArrayObject *point = NULL;
1220 PyArrayObject *direction = NULL;
1221 Py_ssize_t dims[] = {4, 4};
1222 double angle;
1223 static char *kwlist[] = {"angle", "direction", "point", NULL};
1224
1225 if (!PyArg_ParseTupleAndKeywords(args, kwds, "dO&|O&", kwlist,
1226 &angle,
1227 PyConverter_DoubleVector3, &direction,
1228 PyConverter_DoubleVector3OrNone, &point)) goto _fail;
1229
1230 result = (PyArrayObject*)PyArray_SimpleNew(2, dims, NPY_DOUBLE);
1231 if (result == NULL) {
1232 PyErr_Format(PyExc_MemoryError, "unable to allocate matrix");
1233 goto _fail;
1234 }
1235 {
1236 double *M = (double *)PyArray_DATA(result);
1237 double *d = (double *)PyArray_DATA(direction);
1238 double dx = d[0];
1239 double dy = d[1];
1240 double dz = d[2];
1241 double sa = sin(angle);
1242 double ca = cos(angle);
1243 double ca1 = 1 - ca;
1244 double s, t;
1245
1246 t = sqrt(dx*dx + dy*dy + dz*dz);
1247 if (t < EPSILON) {
1248 PyErr_Format(PyExc_ValueError, "invalid direction vector");
1249 goto _fail;
1250 }
1251 dx /= t;
1252 dy /= t;
1253 dz /= t;
1254
1255 M[0] = ca + dx*dx * ca1;
1256 M[5] = ca + dy*dy * ca1;
1257 M[10] = ca + dz*dz * ca1;
1258
1259 s = dz * sa;
1260 t = dx*dy * ca1;
1261 M[1] = t - s;
1262 M[4] = t + s;
1263
1264 s = dy * sa;
1265 t = dx*dz * ca1;
1266 M[2] = t + s;
1267 M[8] = t - s;
1268
1269 s = dx * sa;
1270 t = dy*dz * ca1;
1271 M[6] = t - s;
1272 M[9] = t + s;
1273
1274 M[12] = M[13] = M[14] = 0.0;
1275 M[15] = 1.0;
1276
1277 if (point != NULL) {
1278 double *p = (double *)PyArray_DATA(point);
1279 M[3] = p[0] - (M[0]*p[0] + M[1]*p[1] + M[2]*p[2]);
1280 M[7] = p[1] - (M[4]*p[0] + M[5]*p[1] + M[6]*p[2]);
1281 M[11] = p[2] - (M[8]*p[0] + M[9]*p[1] + M[10]*p[2]);
1282 } else {
1283 M[3] = M[7] = M[11] = 0.0;
1284 }
1285 }
1286
1287 Py_XDECREF(point);
1288 Py_DECREF(direction);
1289 return PyArray_Return(result);
1290
1291 _fail:
1292 Py_XDECREF(point);
1293 Py_XDECREF(direction);
1294 Py_XDECREF(result);
1295 return NULL;
1296 }
1297
1298 /*
1299 Projection matrix.
1300 */
1301 char py_projection_matrix_doc[] =
1302 "Return matrix to project onto plane defined by point and normal.";
1303
1304 static PyObject *
py_projection_matrix(PyObject * obj,PyObject * args,PyObject * kwds)1305 py_projection_matrix(
1306 PyObject *obj,
1307 PyObject *args,
1308 PyObject *kwds)
1309 {
1310 PyArrayObject *result = NULL;
1311 PyArrayObject *point = NULL;
1312 PyArrayObject *normal = NULL;
1313 PyArrayObject *direction = NULL;
1314 PyArrayObject *perspective = NULL;
1315 PyObject *pseudobj = NULL;
1316 Py_ssize_t dims[] = {4, 4};
1317 int pseudo = 0;
1318 static char *kwlist[] = {"point", "normal", "direction",
1319 "perspective", "pseudo", NULL};
1320
1321 if (!PyArg_ParseTupleAndKeywords(args, kwds, "O&O&|O&O&O", kwlist,
1322 PyConverter_DoubleVector3, &point,
1323 PyConverter_DoubleVector3, &normal,
1324 PyConverter_DoubleVector3OrNone, &direction,
1325 PyConverter_DoubleVector3OrNone, &perspective,
1326 &pseudobj)) goto _fail;
1327
1328 if (pseudobj != NULL)
1329 pseudo = PyObject_IsTrue(pseudobj);
1330
1331 result = (PyArrayObject*)PyArray_SimpleNew(2, dims, NPY_DOUBLE);
1332 if (result == NULL) {
1333 PyErr_Format(PyExc_MemoryError, "unable to allocate matrix");
1334 goto _fail;
1335 }
1336 {
1337 double *M = (double *)PyArray_DATA(result);
1338 double *p = (double *)PyArray_DATA(point);
1339 double px = p[0];
1340 double py = p[1];
1341 double pz = p[2];
1342 double *n = (double *)PyArray_DATA(normal);
1343 double nx = n[0];
1344 double ny = n[1];
1345 double nz = n[2];
1346 double t = sqrt(nx*nx + ny*ny + nz*nz);
1347 if (t < EPSILON) {
1348 PyErr_Format(PyExc_ValueError, "invalid normal vector");
1349 goto _fail;
1350 }
1351 nx /= t;
1352 ny /= t;
1353 nz /= t;
1354
1355 if (perspective) {
1356 /* perspective projection */
1357 double *d = (double *)PyArray_DATA(perspective);
1358 double dx = d[0];
1359 double dy = d[1];
1360 double dz = d[2];
1361
1362 t = (dx-px)*nx + (dy-py)*ny + (dz-pz)*nz;
1363 M[0] = t - dx*nx;
1364 M[5] = t - dy*ny;
1365 M[10] = t - dz*nz;
1366 M[1] = - dx*ny;
1367 M[2] = - dx*nz;
1368 M[4] = - dy*nx;
1369 M[6] = - dy*nz;
1370 M[8] = - dz*nx;
1371 M[9] = - dz*ny;
1372
1373 if (pseudo) {
1374 /* preserve relative depth */
1375 M[0] -= nx*nx;
1376 M[5] -= ny*ny;
1377 M[10] -= nz*nz;
1378 t = nx*ny;
1379 M[1] -= t;
1380 M[4] -= t;
1381 t = nx*nz;
1382 M[2] -= t;
1383 M[8] -= t;
1384 t = ny*nz;
1385 M[6] -= t;
1386 M[9] -= t;
1387 t = px*nx + py*ny + pz*nz;
1388 M[3] = t * (dx+nx);
1389 M[7] = t * (dy+ny);
1390 M[11] = t * (dz+nz);
1391 } else {
1392 t = px*nx + py*ny + pz*nz;
1393 M[3] = t * dx;
1394 M[7] = t * dy;
1395 M[11] = t * dz;
1396 }
1397 M[12] = -nx;
1398 M[13] = -ny;
1399 M[14] = -nz;
1400 M[15] = dx*nx + dy*ny + dz*nz;
1401 } else if (direction) {
1402 /* parallel projection */
1403 double *d = (double *)PyArray_DATA(direction);
1404 double dx = d[0];
1405 double dy = d[1];
1406 double dz = d[2];
1407 double scale = dx*nx + dy*ny + dz*nz;
1408
1409 if (ISZERO(scale)) {
1410 PyErr_Format(PyExc_ValueError,
1411 "normal and direction vectors are orthogonal");
1412 goto _fail;
1413 }
1414 scale = -1.0 / scale;
1415 M[0] = 1.0 + scale * dx*nx;
1416 M[5] = 1.0 + scale * dy*ny;
1417 M[10] = 1.0 + scale * dz*nz;
1418 M[1] = scale * dx*ny;
1419 M[2] = scale * dx*nz;
1420 M[4] = scale * dy*nx;
1421 M[6] = scale * dy*nz;
1422 M[8] = scale * dz*nx;
1423 M[9] = scale * dz*ny;
1424 t = (px*nx + py*ny + pz*nz) * -scale;
1425 M[3] = t * dx;
1426 M[7] = t * dy;
1427 M[11] = t * dz;
1428 M[12] = M[13] = M[14] = 0.0;
1429 M[15] = 1.0;
1430 } else {
1431 /* orthogonal projection */
1432 M[0] = 1.0 - nx*nx;
1433 M[5] = 1.0 - ny*ny;
1434 M[10] = 1.0 - nz*nz;
1435 M[1] = M[4] = - nx*ny;
1436 M[2] = M[8] = - nx*nz;
1437 M[6] = M[9] = - ny*nz;
1438 t = px*nx + py*ny + pz*nz;
1439 M[3] = t * nx;
1440 M[7] = t * ny;
1441 M[11] = t * nz;
1442 M[12] = M[13] = M[14] = 0.0;
1443 M[15] = 1.0;
1444 }
1445 }
1446
1447 Py_DECREF(point);
1448 Py_DECREF(normal);
1449 Py_XDECREF(direction);
1450 Py_XDECREF(perspective);
1451 return PyArray_Return(result);
1452
1453 _fail:
1454 Py_XDECREF(point);
1455 Py_XDECREF(normal);
1456 Py_XDECREF(direction);
1457 Py_XDECREF(perspective);
1458 Py_XDECREF(result);
1459 return NULL;
1460 }
1461
1462 /*
1463 Clipping matrix.
1464 */
1465 char py_clip_matrix_doc[] =
1466 "Return matrix to obtain normalized device coordinates from frustrum.";
1467
1468 static PyObject *
py_clip_matrix(PyObject * obj,PyObject * args,PyObject * kwds)1469 py_clip_matrix(
1470 PyObject *obj,
1471 PyObject *args,
1472 PyObject *kwds)
1473 {
1474 PyArrayObject *result = NULL;
1475 PyObject *boolobj = NULL;
1476 Py_ssize_t dims[] = {4, 4};
1477 double *M;
1478 double left, right, bottom, top, hither, yon;
1479 int perspective = 0;
1480 static char *kwlist[] = {"left", "right", "bottom",
1481 "top", "near", "far", "perspective", NULL};
1482
1483 if (!PyArg_ParseTupleAndKeywords(args, kwds, "dddddd|O", kwlist,
1484 &left, &right, &bottom, &top, &hither, &yon, &boolobj))
1485 goto _fail;
1486
1487 if (boolobj != NULL)
1488 perspective = PyObject_IsTrue(boolobj);
1489
1490 if ((left >= right) || (bottom >= top) || (hither >= yon)) {
1491 PyErr_Format(PyExc_ValueError, "invalid frustrum");
1492 goto _fail;
1493 }
1494
1495 result = (PyArrayObject*)PyArray_SimpleNew(2, dims, NPY_DOUBLE);
1496 if (result == NULL) {
1497 PyErr_Format(PyExc_MemoryError, "unable to allocate matrix");
1498 goto _fail;
1499 }
1500
1501 M = (double *)PyArray_DATA(result);
1502
1503 if (perspective) {
1504 double t = 2.0 * hither;
1505 if (hither < EPSILON) {
1506 PyErr_Format(PyExc_ValueError, "invalid frustrum: near <= 0");
1507 goto _fail;
1508 }
1509 M[1] = M[3] = M[4] = M[7] = M[8] = M[9] = M[12] = M[13] = M[15] = 0.0;
1510 M[14] = -1.0;
1511 M[0] = t / (left-right);
1512 M[2] = (right+left) / (right-left);
1513 M[5] = t / (bottom-top);
1514 M[6] = (top+bottom) / (top-bottom);
1515 M[10] = (yon+hither) / (hither-yon);
1516 M[11] = t*yon / (yon-hither);
1517 } else {
1518 M[1] = M[2] = M[4] = M[6] = M[8] = M[9] = M[12] = M[13] = M[14] = 0.0;
1519 M[15] = 1.0;
1520 M[0] = 2.0 / (right-left);
1521 M[3] = (right+left) / (left-right);
1522 M[5] = 2.0 / (top-bottom);
1523 M[7] = (top+bottom) / (bottom-top);
1524 M[10] = 2.0 / (yon-hither);
1525 M[11] = (yon+hither) / (hither-yon);
1526 }
1527
1528 return PyArray_Return(result);
1529
1530 _fail:
1531 Py_XDECREF(result);
1532 return NULL;
1533 }
1534
1535 /*
1536 Scale matrix.
1537 */
1538 char py_scale_matrix_doc[] =
1539 "Return matrix to scale by factor around origin in direction.";
1540
1541 static PyObject *
py_scale_matrix(PyObject * obj,PyObject * args,PyObject * kwds)1542 py_scale_matrix(
1543 PyObject *obj,
1544 PyObject *args,
1545 PyObject *kwds)
1546 {
1547 PyArrayObject *result = NULL;
1548 PyArrayObject *origin = NULL;
1549 PyArrayObject *direction = NULL;
1550 Py_ssize_t dims[] = {4, 4};
1551 double factor;
1552 static char *kwlist[] = {"factor", "origin", "direction", NULL};
1553
1554 if (!PyArg_ParseTupleAndKeywords(args, kwds, "d|O&O&", kwlist,
1555 &factor,
1556 PyConverter_DoubleVector3OrNone, &origin,
1557 PyConverter_DoubleVector3OrNone, &direction)) goto _fail;
1558
1559 result = (PyArrayObject*)PyArray_SimpleNew(2, dims, NPY_DOUBLE);
1560 if (result == NULL) {
1561 PyErr_Format(PyExc_MemoryError, "unable to allocate matrix");
1562 goto _fail;
1563 }
1564 {
1565 double *M = (double *)PyArray_DATA(result);
1566 if (direction == NULL) {
1567 memset(M, 0, 16*sizeof(double));
1568 M[0] = M[5] = M[10] = factor;
1569 M[15] = 1.0;
1570 if (origin != NULL) {
1571 double *p = (double *)PyArray_DATA(origin);
1572 factor = 1.0 - factor;
1573 M[3] = factor * p[0];
1574 M[7] = factor * p[1];
1575 M[11] = factor * p[2];
1576 }
1577 } else {
1578 double *d = (double *)PyArray_DATA(direction);
1579 double dx = d[0];
1580 double dy = d[1];
1581 double dz = d[2];
1582 factor = 1.0 - factor;
1583 M[0] = 1.0 - factor * dx*dx;
1584 M[5] = 1.0 - factor * dy*dy;
1585 M[10] = 1.0 - factor * dz*dz;
1586 factor *= -1.0;
1587 M[1] = M[4] = factor * dx*dy;
1588 M[2] = M[8] = factor * dx*dz;
1589 M[6] = M[9] = factor * dy*dz;
1590 M[12] = M[13] = M[14] = 0.0;
1591 M[15] = 1.0;
1592 if (origin != NULL) {
1593 double *p = (double *)PyArray_DATA(origin);
1594 factor *= - (p[0]*dx + p[1]*dy + p[2]*dz);
1595 M[3] = factor * dx;
1596 M[7] = factor * dy;
1597 M[11] = factor * dz;
1598 } else {
1599 M[3] = M[7] = M[11] = 0.0;
1600 }
1601 }
1602 }
1603
1604 Py_XDECREF(origin);
1605 Py_XDECREF(direction);
1606 return PyArray_Return(result);
1607
1608 _fail:
1609 Py_XDECREF(origin);
1610 Py_XDECREF(direction);
1611 Py_XDECREF(result);
1612 return NULL;
1613 }
1614
1615 /*
1616 Shearing matrix.
1617 */
1618 char py_shear_matrix_doc[] =
1619 "Return matrix to shear by angle along direction vector on shear plane.";
1620
1621 static PyObject *
py_shear_matrix(PyObject * obj,PyObject * args,PyObject * kwds)1622 py_shear_matrix(
1623 PyObject *obj,
1624 PyObject *args,
1625 PyObject *kwds)
1626 {
1627 PyArrayObject *result = NULL;
1628 PyArrayObject *direction = NULL;
1629 PyArrayObject *point = NULL;
1630 PyArrayObject *normal = NULL;
1631 Py_ssize_t dims[] = {4, 4};
1632 double angle;
1633 static char *kwlist[] = {"angle", "direction", "point", "normal", NULL};
1634
1635 if (!PyArg_ParseTupleAndKeywords(args, kwds, "dO&O&O&", kwlist,
1636 &angle,
1637 PyConverter_DoubleVector3, &direction,
1638 PyConverter_DoubleVector3, &point,
1639 PyConverter_DoubleVector3, &normal)) goto _fail;
1640
1641 result = (PyArrayObject*)PyArray_SimpleNew(2, dims, NPY_DOUBLE);
1642 if (result == NULL) {
1643 PyErr_Format(PyExc_MemoryError, "unable to allocate matrix");
1644 goto _fail;
1645 }
1646 {
1647 double *M = (double *)PyArray_DATA(result);
1648 double *p = (double *)PyArray_DATA(point);
1649 double *d = (double *)PyArray_DATA(direction);
1650 double dx = d[0];
1651 double dy = d[1];
1652 double dz = d[2];
1653 double *n = (double *)PyArray_DATA(normal);
1654 double nx = n[0];
1655 double ny = n[1];
1656 double nz = n[2];
1657 double t;
1658
1659 t = sqrt(dx*dx + dy*dy + dz*dz);
1660 if (t < EPSILON) {
1661 PyErr_Format(PyExc_ValueError, "invalid direction vector");
1662 goto _fail;
1663 }
1664 dx /= t;
1665 dy /= t;
1666 dz /= t;
1667
1668 t = sqrt(nx*nx + ny*ny + nz*nz);
1669 if (t < EPSILON) {
1670 PyErr_Format(PyExc_ValueError, "invalid normal vector");
1671 goto _fail;
1672 }
1673 nx /= t;
1674 ny /= t;
1675 nz /= t;
1676
1677 if (fabs(nx*dx + ny*dy + nz*dz) > 1e-6) {
1678 PyErr_Format(PyExc_ValueError,
1679 "direction and normal vectors are not orthogonal");
1680 goto _fail;
1681 }
1682
1683 angle = tan(angle);
1684
1685 M[0] = 1.0 + angle * dx*nx;
1686 M[5] = 1.0 + angle * dy*ny;
1687 M[10] = 1.0 + angle * dz*nz;
1688 M[1] = angle * dx*ny;
1689 M[2] = angle * dx*nz;
1690 M[4] = angle * dy*nx;
1691 M[6] = angle * dy*nz;
1692 M[8] = angle * dz*nx;
1693 M[9] = angle * dz*ny;
1694 M[12] = M[13] = M[14] = 0.0;
1695 M[15] = 1.0;
1696
1697 t = -angle * (p[0]*nx + p[1]*ny + p[2]*nz);
1698 M[3] = t * dx;
1699 M[7] = t * dy;
1700 M[11] = t * dz;
1701 }
1702
1703 Py_DECREF(direction);
1704 Py_DECREF(point);
1705 Py_DECREF(normal);
1706 return PyArray_Return(result);
1707
1708 _fail:
1709 Py_XDECREF(direction);
1710 Py_XDECREF(point);
1711 Py_XDECREF(normal);
1712 Py_XDECREF(result);
1713 return NULL;
1714 }
1715
1716 /*
1717 Superimposition matrix.
1718 */
1719 char py_superimposition_matrix_doc[] =
1720 "Return matrix to transform given vector set into second vector set.";
1721
1722 static PyObject *
py_superimposition_matrix(PyObject * obj,PyObject * args,PyObject * kwds)1723 py_superimposition_matrix(
1724 PyObject *obj,
1725 PyObject *args,
1726 PyObject *kwds)
1727 {
1728 PyThreadState *_save = NULL;
1729 PyArrayObject *result = NULL;
1730 PyArrayObject *v0 = NULL;
1731 PyArrayObject *v1 = NULL;
1732 PyObject *usesvdobj = NULL;
1733 PyObject *scalingobj = NULL;
1734 double *buffer = NULL;
1735 Py_ssize_t dims[] = {4, 4};
1736 int scaling = 0;
1737
1738 static char *kwlist[] = {"v0", "v1", "scaling", "usesvd", NULL};
1739
1740 if (!PyArg_ParseTupleAndKeywords(args, kwds, "O&O&|OO", kwlist,
1741 PyConverter_AnyDoubleArray, &v0,
1742 PyConverter_AnyDoubleArray, &v1,
1743 &scalingobj, &usesvdobj)) goto _fail;
1744
1745 if (scalingobj != NULL)
1746 scaling = PyObject_IsTrue(scalingobj);
1747
1748 if (!PyArray_SAMESHAPE(v0, v1)) {
1749 PyErr_Format(PyExc_ValueError, "shape of vector sets must match");
1750 goto _fail;
1751 }
1752
1753 if ((PyArray_NDIM(v0) != 2) || PyArray_DIM(v0, 0) < 3) {
1754 PyErr_Format(PyExc_ValueError,
1755 "vector sets are of wrong shape or type");
1756 goto _fail;
1757 }
1758
1759 result = (PyArrayObject*)PyArray_SimpleNew(2, dims, NPY_DOUBLE);
1760 if (result == NULL) {
1761 PyErr_Format(PyExc_MemoryError, "unable to allocate matrix");
1762 goto _fail;
1763 }
1764
1765 buffer = (double *)PyMem_Malloc(42 * sizeof(double));
1766 if (!buffer) {
1767 PyErr_Format(PyExc_MemoryError, "unable to allocate buffer");
1768 goto _fail;
1769 }
1770 {
1771 Py_ssize_t i, j;
1772 double v0t[3], v1t[3];
1773 double *q = buffer;
1774 double *N = (buffer + 4);
1775 double *M = (double *)PyArray_DATA(result);
1776
1777 Py_ssize_t size = PyArray_DIM(v0, 1);
1778 Py_ssize_t v0s0 = PyArray_STRIDE(v0, 0);
1779 Py_ssize_t v0s1 = PyArray_STRIDE(v0, 1);
1780 Py_ssize_t v1s0 = PyArray_STRIDE(v1, 0);
1781 Py_ssize_t v1s1 = PyArray_STRIDE(v1, 1);
1782
1783 /* displacements of v0 & v1 centroids from origin */
1784 {
1785 double t;
1786 if (v0s1 == sizeof(double)) {
1787 double *p;
1788 for (j = 0; j < 3; j++) {
1789 t = 0.0;
1790 p = (double*)((char *)PyArray_DATA(v0) + j*v0s0);
1791 for (i = 0; i < size; i++) {
1792 t += p[i];
1793 }
1794 v0t[j] = t / (double)size;
1795 }
1796 } else {
1797 char *p;
1798 for (j = 0; j < 3; j++) {
1799 t = 0.0;
1800 p = (char *)PyArray_DATA(v0) + j*v0s0;
1801 for (i = 0; i < size; i++) {
1802 t += *(double*)p;
1803 p += v0s1;
1804 }
1805 v0t[j] = t / (double)size;
1806 }
1807 }
1808 if (v1s1 == sizeof(double)) {
1809 double *p;
1810 for (j = 0; j < 3; j++) {
1811 t = 0.0;
1812 p = (double*)((char *)PyArray_DATA(v1) + j*v1s0);
1813 for (i = 0; i < size; i++) {
1814 t += p[i];
1815 }
1816 v1t[j] = t / (double)size;
1817 }
1818 } else {
1819 char *p;
1820 for (j = 0; j < 3; j++) {
1821 t = 0.0;
1822 p = (char *)PyArray_DATA(v1) + j*v1s0;
1823 for (i = 0; i < size; i++) {
1824 t += *(double*)p;
1825 p += v1s1;
1826 }
1827 v1t[j] = t / (double)size;
1828 }
1829 }
1830 }
1831 /* symmetric matrix N */
1832 {
1833 double xx, yy, zz, xy, yz, zx, xz, yx, zy;
1834 xx = yy = zz = xy = yz = zx = xz = yx = zy = 0.0;
1835
1836 if ((v0s1 == sizeof(double)) && (v1s1 == sizeof(double))) {
1837 double t, v0x, v0y, v0z;
1838 double *v0px = (double *)PyArray_DATA(v0);
1839 double *v0py = (double *)((char *)PyArray_DATA(v0) + v0s0);
1840 double *v0pz = (double *)((char *)PyArray_DATA(v0) + v0s0*2);
1841 double *v1px = (double *)PyArray_DATA(v1);
1842 double *v1py = (double *)((char *)PyArray_DATA(v1) + v1s0);
1843 double *v1pz = (double *)((char *)PyArray_DATA(v1) + v1s0*2);
1844
1845 #pragma vector always
1846 for (i = 0; i < size; i++) {
1847 v0x = v0px[i] - v0t[0];
1848 v0y = v0py[i] - v0t[1];
1849 v0z = v0pz[i] - v0t[2];
1850
1851 t = v1px[i] - v1t[0];
1852 xx += v0x * t;
1853 yx += v0y * t;
1854 zx += v0z * t;
1855 t = v1py[i] - v1t[1];
1856 xy += v0x * t;
1857 yy += v0y * t;
1858 zy += v0z * t;
1859 t = v1pz[i] - v1t[2];
1860 xz += v0x * t;
1861 yz += v0y * t;
1862 zz += v0z * t;
1863 }
1864 } else {
1865 double t, v1x, v1y, v1z;
1866 char *v0p = PyArray_DATA(v0);
1867 char *v1p = PyArray_DATA(v1);
1868
1869 for (i = 0; i < size; i++) {
1870 v1x = (*(double*)(v1p)) - v1t[0];
1871 v1y = (*(double*)(v1p + v1s0)) - v1t[1];
1872 v1z = (*(double*)(v1p + v1s0 + v1s0)) - v1t[2];
1873
1874 t = (*(double*)(v0p)) - v0t[0];
1875 xx += t * v1x;
1876 xy += t * v1y;
1877 xz += t * v1z;
1878 t = (*(double*)(v0p + v0s0)) - v0t[1];
1879 yx += t * v1x;
1880 yy += t * v1y;
1881 yz += t * v1z;
1882 t = (*(double*)(v0p + v0s0 + v0s0)) - v0t[2];
1883 zx += t * v1x;
1884 zy += t * v1y;
1885 zz += t * v1z;
1886
1887 v0p += v0s1;
1888 v1p += v1s1;
1889 }
1890 }
1891
1892 _save = PyEval_SaveThread();
1893
1894 N[0] = xx + yy + zz;
1895 N[5] = xx - yy - zz;
1896 N[10] = -xx + yy - zz;
1897 N[15] = -xx - yy + zz;
1898 N[1] = N[4] = yz - zy;
1899 N[2] = N[8] = zx - xz;
1900 N[3] = N[12] = xy - yx;
1901 N[6] = N[9] = xy + yx;
1902 N[7] = N[13] = zx + xz;
1903 N[11] = N[14] = yz + zy;
1904 }
1905 /* quaternion q: eigenvector corresponding to most positive */
1906 /* eigenvalue of N. */
1907 {
1908 double l;
1909 double *a = (buffer + 20);
1910 double *b = (buffer + 24);
1911 double *t = (buffer + 27);
1912
1913 for (i = 0; i < 16; i++)
1914 M[i] = N[i];
1915
1916 if (tridiagonalize_symmetric_44(M, a, b) != 0) {
1917 PyEval_RestoreThread(_save);
1918 PyErr_Format(PyExc_ValueError,
1919 "tridiagonalize_symmetric_44() failed");
1920 goto _fail;
1921 }
1922
1923 l = max_eigenvalue_of_tridiag_44(a, b);
1924 N[0] -= l;
1925 N[5] -= l;
1926 N[10] -= l;
1927 N[15] -= l;
1928
1929 if (eigenvector_of_symmetric_44(N, q, t) != 0) {
1930 PyEval_RestoreThread(_save);
1931 PyErr_Format(PyExc_ValueError,
1932 "eigenvector_of_symmetric_44() failed");
1933 goto _fail;
1934 }
1935
1936 l = q[3];
1937 q[3] = q[2];
1938 q[2] = q[1];
1939 q[1] = q[0];
1940 q[0] = l;
1941 }
1942
1943 if (quaternion_matrix(q, M) != 0) {
1944 PyEval_RestoreThread(_save);
1945 PyErr_Format(PyExc_ValueError, "quaternion_matrix() failed");
1946 goto _fail;
1947 }
1948
1949 PyEval_RestoreThread(_save);
1950
1951 if (scaling) {
1952 /* scaling factor = sqrt(sum(v1) / sum(v0) */
1953 double t, dt;
1954 double v0s = 0.0;
1955 double v1s = 0.0;
1956
1957 if (v0s1 == sizeof(double)) {
1958 double *p;
1959 for (j = 0; j < 3; j++) {
1960 p = (double*)((char *)PyArray_DATA(v0) + j*v0s0);
1961 dt = v0t[j];
1962 #pragma vector always
1963 for (i = 0; i < size; i++) {
1964 t = p[i] - dt;
1965 v0s += t*t;
1966 }
1967 }
1968 } else {
1969 char *p;
1970 for (j = 0; j < 3; j++) {
1971 p = (char *)PyArray_DATA(v0) + j*v0s0;
1972 dt = v0t[j];
1973 for (i = 0; i < size; i++) {
1974 t = (*(double*)p) - dt;
1975 v0s += t*t;
1976 p += v0s1;
1977 }
1978 }
1979 }
1980
1981 if (v1s1 == sizeof(double)) {
1982 double *p;
1983 for (j = 0; j < 3; j++) {
1984 p = (double*)((char *)PyArray_DATA(v1) + j*v1s0);
1985 dt = v1t[j];
1986 #pragma vector always
1987 for (i = 0; i < size; i++) {
1988 t = p[i] - dt;
1989 v1s += t*t;
1990 }
1991 }
1992 } else {
1993 char *p;
1994 for (j = 0; j < 3; j++) {
1995 p = (char *)PyArray_DATA(v1) + j*v1s0;
1996 dt = v1t[j];
1997 for (i = 0; i < size; i++) {
1998 t = (*(double*)p) - dt;
1999 v1s += t*t;
2000 p += v1s1;
2001 }
2002 }
2003 }
2004
2005 t = sqrt(v1s / v0s);
2006 M[0] *= t;
2007 M[1] *= t;
2008 M[2] *= t;
2009 M[4] *= t;
2010 M[5] *= t;
2011 M[6] *= t;
2012 M[8] *= t;
2013 M[9] *= t;
2014 M[10] *= t;
2015 }
2016
2017 /* translation */
2018 M[3] = v1t[0] - M[0]*v0t[0] - M[1]*v0t[1] - M[2]*v0t[2];
2019 M[7] = v1t[1] - M[4]*v0t[0] - M[5]*v0t[1] - M[6]*v0t[2];
2020 M[11] = v1t[2] - M[8]*v0t[0] - M[9]*v0t[1] - M[10]*v0t[2];
2021 }
2022
2023 PyMem_Free(buffer);
2024 Py_DECREF(v0);
2025 Py_DECREF(v1);
2026 return PyArray_Return(result);
2027
2028 _fail:
2029 PyMem_Free(buffer);
2030 Py_XDECREF(v0);
2031 Py_XDECREF(v1);
2032 Py_XDECREF(result);
2033 return NULL;
2034 }
2035
2036 /*
2037 Orthogonalization matrix.
2038 */
2039 char py_orthogonalization_matrix_doc[] =
2040 "Return orthogonalization matrix for crystallographic cell coordinates.";
2041
2042 static PyObject *
py_orthogonalization_matrix(PyObject * obj,PyObject * args,PyObject * kwds)2043 py_orthogonalization_matrix(
2044 PyObject *obj,
2045 PyObject *args,
2046 PyObject *kwds)
2047 {
2048 PyArrayObject *result = NULL;
2049 PyArrayObject *lengths = NULL;
2050 PyArrayObject *angles = NULL;
2051 Py_ssize_t dims[] = {4, 4};
2052 static char *kwlist[] = {"lengths", "angles", NULL};
2053
2054 if (!PyArg_ParseTupleAndKeywords(args, kwds, "O&O&", kwlist,
2055 PyConverter_DoubleVector3, &lengths,
2056 PyConverter_DoubleVector3, &angles)) goto _fail;
2057
2058 result = (PyArrayObject*)PyArray_SimpleNew(2, dims, NPY_DOUBLE);
2059 if (result == NULL) {
2060 PyErr_Format(PyExc_MemoryError, "unable to allocate matrix");
2061 goto _fail;
2062 }
2063 {
2064 double *M = (double *)PyArray_DATA(result);
2065 double *a = (double *)PyArray_DATA(angles);
2066 double *l = (double *)PyArray_DATA(lengths);
2067 double la = l[0];
2068 double lb = l[1];
2069 double sa = sin(a[0] * DEG2RAD);
2070 double ca = cos(a[0] * DEG2RAD);
2071 double sb = sin(a[1] * DEG2RAD);
2072 double cb = cos(a[1] * DEG2RAD);
2073 double cg = cos(a[2] * DEG2RAD);
2074 double t = ca * cb - cg;
2075
2076 if ((fabs(sa*sb) < EPSILON) || (fabs(t-sa*sb) < EPSILON)) {
2077 PyErr_Format(PyExc_ValueError, "invalid cell geometry");
2078 goto _fail;
2079 }
2080 t /= (sa * sb);
2081 M[15] = 1.0;
2082 M[1] = M[2] = M[3] = M[6] = M[7] = M[11] = M[12] = M[13] = M[14] = 0.0;
2083 M[0] = la * sb * sqrt(1.0-t*t);
2084 M[4] = -la * sb * t;
2085 M[5] = lb * sa;
2086 M[8] = la * cb;
2087 M[9] = lb * ca;
2088 M[10] = l[2];
2089 }
2090
2091 Py_DECREF(lengths);
2092 Py_DECREF(angles);
2093 return PyArray_Return(result);
2094
2095 _fail:
2096 Py_XDECREF(lengths);
2097 Py_XDECREF(angles);
2098 Py_XDECREF(result);
2099 return NULL;
2100 }
2101
2102 /*
2103 Map Euler axes strings and tuples to inner axis, parity, repetition, and frame.
2104 */
axis2tuple(PyObject * axes,int * firstaxis,int * parity,int * repetition,int * frame)2105 static int axis2tuple(
2106 PyObject *axes,
2107 int *firstaxis,
2108 int *parity,
2109 int *repetition,
2110 int *frame
2111 )
2112 {
2113 *firstaxis = 0; *parity = 0; *repetition = 0; *frame = 0;
2114 if (axes == NULL)
2115 return 0;
2116
2117 /* axes strings */
2118 #if PY_MAJOR_VERSION < 3
2119 if (PyString_Check(axes) && (PyString_Size(axes) == 4)) {
2120 char *s = PyString_AS_STRING(axes);
2121 #else
2122 if (PyUnicode_Check(axes) && (PyUnicode_GetSize(axes) == 4)) {
2123 char *s = PyBytes_AsString(PyUnicode_AsASCIIString(axes));
2124 #endif
2125 int hash = *((int *)s);
2126 switch (hash)
2127 {
2128 case 2054781043:
2129 case 1937275258: /* sxyz */
2130 *firstaxis = 0; *parity = 0; *repetition = 0; *frame = 0; break;
2131 case 2054781042:
2132 case 1920498042: /* rxyz */
2133 *firstaxis = 2; *parity = 1; *repetition = 0; *frame = 1; break;
2134 case 2037938802:
2135 case 1920628857: /* rzxy */
2136 *firstaxis = 1; *parity = 1; *repetition = 0; *frame = 1; break;
2137 case 2054716018:
2138 case 1920628858: /* rzxz */
2139 *firstaxis = 2; *parity = 0; *repetition = 1; *frame = 1; break;
2140 case 2054716019:
2141 case 1937406074: /* szxz */
2142 *firstaxis = 2; *parity = 0; *repetition = 1; *frame = 0; break;
2143 case 2037938803:
2144 case 1937406073: /* szxy */
2145 *firstaxis = 2; *parity = 0; *repetition = 0; *frame = 0; break;
2146 case 2038069618:
2147 case 1920563833: /* ryzy */
2148 *firstaxis = 1; *parity = 0; *repetition = 1; *frame = 1; break;
2149 case 2021292402:
2150 case 1920563832: /* ryzx */
2151 *firstaxis = 0; *parity = 1; *repetition = 0; *frame = 1; break;
2152 case 2054715762:
2153 case 1920563322: /* ryxz */
2154 *firstaxis = 2; *parity = 0; *repetition = 0; *frame = 1; break;
2155 case 2037938546:
2156 case 1920563321: /* ryxy */
2157 *firstaxis = 1; *parity = 1; *repetition = 1; *frame = 1; break;
2158 case 2021292146:
2159 case 1920498296: /* rxzx */
2160 *firstaxis = 0; *parity = 1; *repetition = 1; *frame = 1; break;
2161 case 2038069362:
2162 case 1920498297: /* rxzy */
2163 *firstaxis = 1; *parity = 0; *repetition = 0; *frame = 1; break;
2164 case 2021226611:
2165 case 1937275256: /* sxyx */
2166 *firstaxis = 0; *parity = 0; *repetition = 1; *frame = 0; break;
2167 case 2038069363:
2168 case 1937275513: /* sxzy */
2169 *firstaxis = 0; *parity = 1; *repetition = 0; *frame = 0; break;
2170 case 2054781554:
2171 case 1920629114: /* rzyz */
2172 *firstaxis = 2; *parity = 1; *repetition = 1; *frame = 1; break;
2173 case 2021227122:
2174 case 1920629112: /* rzyx */
2175 *firstaxis = 0; *parity = 0; *repetition = 0; *frame = 1; break;
2176 case 2021227123:
2177 case 1937406328: /* szyx */
2178 *firstaxis = 2; *parity = 1; *repetition = 0; *frame = 0; break;
2179 case 2054781555:
2180 case 1937406330: /* szyz */
2181 *firstaxis = 2; *parity = 1; *repetition = 1; *frame = 0; break;
2182 case 2021226610:
2183 case 1920498040: /* rxyx */
2184 *firstaxis = 0; *parity = 0; *repetition = 1; *frame = 1; break;
2185 case 2021292403:
2186 case 1937341048: /* syzx */
2187 *firstaxis = 1; *parity = 0; *repetition = 0; *frame = 0; break;
2188 case 2038069619:
2189 case 1937341049: /* syzy */
2190 *firstaxis = 1; *parity = 0; *repetition = 1; *frame = 0; break;
2191 case 2037938547:
2192 case 1937340537: /* syxy */
2193 *firstaxis = 1; *parity = 1; *repetition = 1; *frame = 0; break;
2194 case 2054715763:
2195 case 1937340538: /* syxz */
2196 *firstaxis = 1; *parity = 1; *repetition = 0; *frame = 0; break;
2197 case 2021292147:
2198 case 1937275512: /* sxzx */
2199 *firstaxis = 0; *parity = 1; *repetition = 1; *frame = 0; break;
2200 default:
2201 PyErr_Format(PyExc_ValueError, "invalid axes string");
2202 return -1;
2203 }
2204 return 0;
2205 }
2206
2207 if (PySequence_Check(axes) && (PySequence_Size(axes) == 4)) {
2208 /* axes tuples */
2209 *firstaxis = (int)PySequence_GetInteger(axes, 0);
2210 *parity = (int)PySequence_GetInteger(axes, 1);
2211 *repetition = (int)PySequence_GetInteger(axes, 2);
2212 *frame = (int)PySequence_GetInteger(axes, 3);
2213 if (((*firstaxis != 0) && (*firstaxis != 1) && (*firstaxis != 2)) ||
2214 ((*parity != 0) && (*parity != 1)) ||
2215 ((*repetition != 0) && (*repetition != 1)) ||
2216 ((*frame != 0) && (*frame != 1))) {
2217 PyErr_Format(PyExc_ValueError, "invalid axes sequence");
2218 return -1;
2219 }
2220 return 0;
2221 }
2222
2223 PyErr_Format(PyExc_ValueError, "invalid axes type or shape");
2224 return -1;
2225 }
2226
2227 /*
2228 Matrix from Euler angles.
2229 */
2230 char py_euler_matrix_doc[] =
2231 "Return homogeneous rotation matrix from Euler angles and axis sequence.";
2232
2233 static PyObject *
2234 py_euler_matrix(
2235 PyObject *obj,
2236 PyObject *args,
2237 PyObject *kwds)
2238 {
2239 PyArrayObject *result = NULL;
2240 PyObject *axes = NULL;
2241 Py_ssize_t dims[] = {4, 4};
2242 int next_axis[] = {1, 2, 0, 1};
2243 double ai, aj, ak;
2244 int firstaxis = 0;
2245 int parity = 0;
2246 int repetition = 0;
2247 int frame = 0;
2248 static char *kwlist[] = {"ai", "aj", "ak", "axes", NULL};
2249
2250 if (!PyArg_ParseTupleAndKeywords(args, kwds, "ddd|O", kwlist,
2251 &ai, &aj, &ak, &axes)) goto _fail;
2252
2253 if (axes != NULL) Py_INCREF(axes);
2254
2255 result = (PyArrayObject*)PyArray_SimpleNew(2, dims, NPY_DOUBLE);
2256 if (result == NULL) {
2257 PyErr_Format(PyExc_MemoryError, "unable to allocate matrix");
2258 goto _fail;
2259 }
2260
2261 if (axis2tuple(axes, &firstaxis, &parity, &repetition, &frame) != 0)
2262 goto _fail;
2263 Py_XDECREF(axes);
2264
2265 {
2266 double *M = (double *)PyArray_DATA(result);
2267 int i = firstaxis;
2268 int j = next_axis[i+parity];
2269 int k = next_axis[i-parity+1];
2270 double t;
2271 double si, sj, sk, ci, cj, ck, cc, cs, sc, ss;
2272
2273 if (frame) {
2274 t = ai;
2275 ai = ak;
2276 ak = t;
2277 }
2278
2279 if (parity) {
2280 ai = -ai;
2281 aj = -aj;
2282 ak = -ak;
2283 }
2284
2285 si = sin(ai);
2286 sj = sin(aj);
2287 sk = sin(ak);
2288 ci = cos(ai);
2289 cj = cos(aj);
2290 ck = cos(ak);
2291 cc = ci*ck;
2292 cs = ci*sk;
2293 sc = si*ck;
2294 ss = si*sk;
2295
2296 if (repetition) {
2297 M[4*i+i] = cj;
2298 M[4*i+j] = sj*si;
2299 M[4*i+k] = sj*ci;
2300 M[4*j+i] = sj*sk;
2301 M[4*j+j] = -cj*ss+cc;
2302 M[4*j+k] = -cj*cs-sc;
2303 M[4*k+i] = -sj*ck;
2304 M[4*k+j] = cj*sc+cs;
2305 M[4*k+k] = cj*cc-ss;
2306 } else {
2307 M[4*i+i] = cj*ck;
2308 M[4*i+j] = sj*sc-cs;
2309 M[4*i+k] = sj*cc+ss;
2310 M[4*j+i] = cj*sk;
2311 M[4*j+j] = sj*ss+cc;
2312 M[4*j+k] = sj*cs-sc;
2313 M[4*k+i] = -sj;
2314 M[4*k+j] = cj*si;
2315 M[4*k+k] = cj*ci;
2316 }
2317
2318 M[3] = M[7] = M[11] = M[12] = M[13] = M[14] = 0.0;
2319 M[15] = 1.0;
2320 }
2321
2322 return PyArray_Return(result);
2323
2324 _fail:
2325 Py_XDECREF(axes);
2326 Py_XDECREF(result);
2327 return NULL;
2328 }
2329
2330 /*
2331 Euler angles from matrix.
2332 */
2333 char py_euler_from_matrix_doc[] =
2334 "Return Euler angles from rotation matrix for specified axis sequence.";
2335
2336 static PyObject *
2337 py_euler_from_matrix(
2338 PyObject *obj,
2339 PyObject *args,
2340 PyObject *kwds)
2341 {
2342 PyArrayObject *matrix = NULL;
2343 PyObject *axes = NULL;
2344 int next_axis[] = {1, 2, 0, 1};
2345 double ai = 0.0;
2346 double aj = 0.0;
2347 double ak = 0.0;
2348 int firstaxis = 0;
2349 int parity = 0;
2350 int repetition = 0;
2351 int frame = 0;
2352 static char *kwlist[] = {"matrix", "axes", NULL};
2353
2354 if (!PyArg_ParseTupleAndKeywords(args, kwds, "O&|O", kwlist,
2355 PyConverter_DoubleMatrix44, &matrix, &axes)) goto _fail;
2356
2357 if (axes != NULL)
2358 Py_INCREF(axes);
2359
2360 if (axis2tuple(axes, &firstaxis, &parity, &repetition, &frame) != 0)
2361 goto _fail;
2362
2363 {
2364 double *M = (double *)PyArray_DATA(matrix);
2365 int i = firstaxis;
2366 int j = next_axis[i+parity];
2367 int k = next_axis[i-parity+1];
2368 double x, y, t;
2369
2370 if (repetition) {
2371 x = M[4*i+j];
2372 y = M[4*i+k];
2373 t = sqrt(x*x + y*y);
2374 if (t > EPSILON) {
2375 ai = atan2( M[4*i+j], M[4*i+k]);
2376 aj = atan2( t, M[4*i+i]);
2377 ak = atan2( M[4*j+i], -M[4*k+i]);
2378 } else {
2379 ai = atan2(-M[4*j+k], M[4*j+j]);
2380 ai = atan2( t, M[4*i+i]);
2381 }
2382 } else {
2383 x = M[4*i+i];
2384 y = M[4*j+i];
2385 t = sqrt(x*x + y*y);
2386 if (t > EPSILON) {
2387 ai = atan2( M[4*k+j], M[4*k+k]);
2388 aj = atan2(-M[4*k+i], t);
2389 ak = atan2( M[4*j+i], M[4*i+i]);
2390 } else {
2391 ai = atan2(-M[4*j+k], M[4*j+j]);
2392 ai = atan2(-M[4*k+i], t);
2393 }
2394 }
2395 if (parity) {
2396 ai = -ai;
2397 aj = -aj;
2398 ak = -ak;
2399 }
2400 if (frame) {
2401 t = ai;
2402 ai = ak;
2403 ak = t;
2404 }
2405 }
2406
2407 Py_XDECREF(axes);
2408 Py_DECREF(matrix);
2409 return Py_BuildValue("(d,d,d)", ai, aj, ak);
2410
2411 _fail:
2412 Py_XDECREF(axes);
2413 Py_XDECREF(matrix);
2414 return NULL;
2415 }
2416
2417 /*
2418 Quaternion from Euler angles.
2419 */
2420 char py_quaternion_from_euler_doc[] =
2421 "Return quaternion from Euler angles and axis sequence.";
2422
2423 static PyObject *
2424 py_quaternion_from_euler(
2425 PyObject *obj,
2426 PyObject *args,
2427 PyObject *kwds)
2428 {
2429 PyArrayObject *result = NULL;
2430 PyObject *axes = NULL;
2431 Py_ssize_t dims = 4;
2432 int next_axis[] = {1, 2, 0, 1};
2433 double ai, aj, ak;
2434 int firstaxis = 0;
2435 int parity = 0;
2436 int repetition = 0;
2437 int frame = 0;
2438 static char *kwlist[] = {"ai", "aj", "ak", "axes", NULL};
2439
2440 if (!PyArg_ParseTupleAndKeywords(args, kwds, "ddd|O", kwlist,
2441 &ai, &aj, &ak, &axes)) goto _fail;
2442
2443 if (axes != NULL)
2444 Py_INCREF(axes);
2445
2446 result = (PyArrayObject*)PyArray_SimpleNew(1, &dims, NPY_DOUBLE);
2447 if (result == NULL) {
2448 PyErr_Format(PyExc_MemoryError, "unable to allocate quaternion");
2449 goto _fail;
2450 }
2451
2452 if (axis2tuple(axes, &firstaxis, &parity, &repetition, &frame) != 0)
2453 goto _fail;
2454
2455 {
2456 double *q = (double *)PyArray_DATA(result);
2457 int i = firstaxis + 1;
2458 int j = next_axis[i+parity-1] + 1;
2459 int k = next_axis[i-parity] + 1;
2460 double t;
2461 double si, sj, sk, ci, cj, ck, cc, cs, sc, ss;
2462
2463 if (frame) {
2464 t = ai;
2465 ai = ak;
2466 ak = t;
2467 }
2468
2469 if (parity) {
2470 aj = -aj;
2471 }
2472
2473 ai /= 2.0;
2474 aj /= 2.0;
2475 ak /= 2.0;
2476
2477 si = sin(ai);
2478 sj = sin(aj);
2479 sk = sin(ak);
2480 ci = cos(ai);
2481 cj = cos(aj);
2482 ck = cos(ak);
2483 cc = ci*ck;
2484 cs = ci*sk;
2485 sc = si*ck;
2486 ss = si*sk;
2487
2488 if (repetition) {
2489 q[i] = cj*(cs + sc);
2490 q[k] = sj*(cs - sc);
2491 q[j] = sj*(cc + ss);
2492 q[0] = cj*(cc - ss);
2493 } else {
2494 q[i] = cj*sc - sj*cs;
2495 q[k] = cj*cs - sj*sc;
2496 q[j] = cj*ss + sj*cc;
2497 q[0] = cj*cc + sj*ss;
2498 }
2499
2500 if (parity) {
2501 q[j] *= -1.0;
2502 }
2503 }
2504
2505 Py_XDECREF(axes);
2506 return PyArray_Return(result);
2507
2508 _fail:
2509 Py_XDECREF(axes);
2510 Py_XDECREF(result);
2511 return NULL;
2512 }
2513
2514 /*
2515 Quaternion about axis.
2516 */
2517 char py_quaternion_about_axis_doc[] =
2518 "Return quaternion for rotation about axis.";
2519
2520 static PyObject *
2521 py_quaternion_about_axis(
2522 PyObject *obj,
2523 PyObject *args,
2524 PyObject *kwds)
2525 {
2526 PyArrayObject *axis = NULL;
2527 PyArrayObject *result = NULL;
2528 double angle;
2529 Py_ssize_t dims = 4;
2530 static char *kwlist[] = {"angle", "axis", NULL};
2531
2532 if (!PyArg_ParseTupleAndKeywords(args, kwds, "dO&", kwlist,
2533 &angle,
2534 PyConverter_DoubleVector3, &axis)) goto _fail;
2535
2536 result = (PyArrayObject*)PyArray_SimpleNew(1, &dims, NPY_DOUBLE);
2537 if (result == NULL) {
2538 PyErr_Format(PyExc_MemoryError, "unable to allocate quaternion");
2539 goto _fail;
2540 }
2541 {
2542 double *q = (double *)PyArray_DATA(result);
2543 double *a = (double *)PyArray_DATA(axis);
2544 double t = sqrt(a[0]*a[0] + a[1]*a[1] + a[2]*a[2]);
2545
2546 if (t > EPSILON) {
2547 t = sin(angle / 2.0) / t;
2548 q[1] = a[0] * t;
2549 q[2] = a[1] * t;
2550 q[3] = a[2] * t;
2551 } else {
2552 q[1] = a[0];
2553 q[2] = a[1];
2554 q[3] = a[2];
2555 }
2556 q[0] = cos(angle / 2.0);
2557 }
2558
2559 Py_DECREF(axis);
2560 return PyArray_Return(result);
2561
2562 _fail:
2563 Py_XDECREF(result);
2564 Py_XDECREF(axis);
2565 return NULL;
2566 }
2567
2568 /*
2569 Quaternion from rotation matrix.
2570 */
2571 char py_quaternion_from_matrix_doc[] =
2572 "Return quaternion from rotation matrix.";
2573
2574 static PyObject *
2575 py_quaternion_from_matrix(
2576 PyObject *obj,
2577 PyObject *args,
2578 PyObject *kwds)
2579 {
2580 PyThreadState *_save = NULL;
2581 PyArrayObject *matrix = NULL;
2582 PyArrayObject *result = NULL;
2583 PyObject *boolobj = NULL;
2584 Py_ssize_t dims = 4;
2585 static char *kwlist[] = {"matrix", "isprecise", NULL};
2586 double *buffer = NULL;
2587 int isprecise = 0;
2588
2589 if (!PyArg_ParseTupleAndKeywords(args, kwds, "O&|O", kwlist,
2590 PyConverter_DoubleMatrix44, &matrix, &boolobj)) goto _fail;
2591
2592 result = (PyArrayObject*)PyArray_SimpleNew(1, &dims, NPY_DOUBLE);
2593 if (result == NULL) {
2594 PyErr_Format(PyExc_MemoryError, "unable to allocate quaternion");
2595 goto _fail;
2596 }
2597
2598 if (boolobj != NULL)
2599 isprecise = PyObject_IsTrue(boolobj);
2600
2601 if (isprecise) {
2602 /* precise rotation matrix */
2603 double *q = (double *)PyArray_DATA(result);
2604 double *M = (double *)PyArray_DATA(matrix);
2605 if (quaternion_from_matrix(M, q) != 0) {
2606 PyEval_RestoreThread(_save);
2607 PyErr_Format(PyExc_ValueError,
2608 "quaternion_from_matrix() failed");
2609 goto _fail;
2610 }
2611 } else {
2612 double *q = (double *)PyArray_DATA(result);
2613 double *M = (double *)PyArray_DATA(matrix);
2614 double *K, *N, *a, *b, *t;
2615 double l;
2616 int i;
2617
2618 buffer = (double *)PyMem_Malloc(52 * sizeof(double));
2619 if (!buffer) {
2620 PyErr_Format(PyExc_MemoryError, "unable to allocate buffer");
2621 goto _fail;
2622 }
2623 K = buffer;
2624 N = (buffer + 16);
2625 a = (buffer + 32);
2626 b = (buffer + 36);
2627 t = (buffer + 40);
2628
2629 /* symmetric matrix K */
2630 K[0] = (M[0] - M[5] - M[10]) / 3.0;
2631 K[5] = (M[5] - M[0] - M[10]) / 3.0;
2632 K[10] = (M[10] - M[0] - M[5]) / 3.0;
2633 K[15] = (M[0] + M[5] + M[10]) / 3.0;
2634 K[1] = K[4] = (M[4] + M[1]) / 3.0;
2635 K[2] = K[8] = (M[8] + M[2]) / 3.0;
2636 K[3] = K[12] = (M[9] - M[6]) / 3.0;
2637 K[6] = K[9] = (M[9] + M[6]) / 3.0;
2638 K[7] = K[13] = (M[2] - M[8]) / 3.0;
2639 K[11] = K[14] = (M[4] - M[1]) / 3.0;
2640
2641 _save = PyEval_SaveThread();
2642
2643 /* quaternion q: eigenvector corresponding to most positive */
2644 /* eigenvalue of K. */
2645 for (i = 0; i < 16; i++) {
2646 N[i] = K[i];
2647 }
2648
2649 if (tridiagonalize_symmetric_44(N, a, b) != 0) {
2650 PyEval_RestoreThread(_save);
2651 PyErr_Format(PyExc_ValueError,
2652 "tridiagonalize_symmetric_44() failed");
2653 goto _fail;
2654 }
2655
2656 l = max_eigenvalue_of_tridiag_44(a, b);
2657 K[0] -= l;
2658 K[5] -= l;
2659 K[10] -= l;
2660 K[15] -= l;
2661
2662 if (eigenvector_of_symmetric_44(K, q, t) != 0) {
2663 PyEval_RestoreThread(_save);
2664 PyErr_Format(PyExc_ValueError,
2665 "eigenvector_of_symmetric_44() failed");
2666 goto _fail;
2667 }
2668
2669 l = q[0];
2670 q[0] = q[2];
2671 q[2] = l;
2672 l = q[1];
2673 q[1] = q[3];
2674 q[3] = l;
2675
2676 if (q[0] < 0.0) {
2677 q[0] = -q[0];
2678 q[1] = -q[1];
2679 q[2] = -q[2];
2680 q[3] = -q[3];
2681 }
2682 PyEval_RestoreThread(_save);
2683 }
2684
2685 PyMem_Free(buffer);
2686 Py_DECREF(matrix);
2687 return PyArray_Return(result);
2688
2689 _fail:
2690 PyMem_Free(buffer);
2691 Py_XDECREF(result);
2692 Py_XDECREF(matrix);
2693 return NULL;
2694 }
2695
2696 /*
2697 Rotation matrix from quaternion.
2698 */
2699 char py_quaternion_matrix_doc[] =
2700 "Return rotation matrix from quaternion.";
2701
2702 static PyObject *
2703 py_quaternion_matrix(
2704 PyObject *obj,
2705 PyObject *args,
2706 PyObject *kwds)
2707 {
2708 PyArrayObject *quaternion = NULL;
2709 PyArrayObject *result = NULL;
2710 Py_ssize_t dims[] = {4, 4};
2711 static char *kwlist[] = {"quaternion" , NULL};
2712
2713 if (!PyArg_ParseTupleAndKeywords(args, kwds, "O&", kwlist,
2714 PyConverter_DoubleVector4, &quaternion)) goto _fail;
2715
2716 result = (PyArrayObject*)PyArray_SimpleNew(2, dims, NPY_DOUBLE);
2717 if (result == NULL) {
2718 PyErr_Format(PyExc_MemoryError, "unable to allocate matrix");
2719 goto _fail;
2720 }
2721
2722 if (quaternion_matrix((double *)PyArray_DATA(quaternion),
2723 (double *)PyArray_DATA(result)) != 0) {
2724 PyErr_Format(PyExc_ValueError, "quaternion_matrix failed");
2725 goto _fail;
2726 }
2727
2728 Py_DECREF(quaternion);
2729 return PyArray_Return(result);
2730
2731 _fail:
2732 Py_XDECREF(result);
2733 Py_XDECREF(quaternion);
2734 return NULL;
2735 }
2736
2737 /*
2738 Multiply two quaternions.
2739 */
2740 char py_quaternion_multiply_doc[] = "Multiply two quaternions.";
2741
2742 static PyObject *
2743 py_quaternion_multiply(
2744 PyObject *obj,
2745 PyObject *args,
2746 PyObject *kwds)
2747 {
2748 PyArrayObject *quaternion0 = NULL;
2749 PyArrayObject *quaternion1 = NULL;
2750 PyArrayObject *result = NULL;
2751 Py_ssize_t dims = 4;
2752 static char *kwlist[] = {"quaternion1", "quaternion0", NULL};
2753
2754 if (!PyArg_ParseTupleAndKeywords(args, kwds, "O&O&", kwlist,
2755 PyConverter_DoubleVector4, &quaternion1,
2756 PyConverter_DoubleVector4, &quaternion0)) goto _fail;
2757
2758 result = (PyArrayObject*)PyArray_SimpleNew(1, &dims, NPY_DOUBLE);
2759 if (result == NULL) {
2760 PyErr_Format(PyExc_MemoryError, "unable to allocate quaternion");
2761 goto _fail;
2762 }
2763 {
2764 double *q0 = (double *)PyArray_DATA(quaternion0);
2765 double *q1 = (double *)PyArray_DATA(quaternion1);
2766 double *qq = (double *)PyArray_DATA(result);
2767 qq[0] = -q1[1]*q0[1] - q1[2]*q0[2] - q1[3]*q0[3] + q1[0]*q0[0];
2768 qq[1] = q1[1]*q0[0] + q1[2]*q0[3] - q1[3]*q0[2] + q1[0]*q0[1];
2769 qq[2] = -q1[1]*q0[3] + q1[2]*q0[0] + q1[3]*q0[1] + q1[0]*q0[2];
2770 qq[3] = q1[1]*q0[2] - q1[2]*q0[1] + q1[3]*q0[0] + q1[0]*q0[3];
2771 }
2772
2773 Py_DECREF(quaternion0);
2774 Py_DECREF(quaternion1);
2775 return PyArray_Return(result);
2776
2777 _fail:
2778 Py_XDECREF(result);
2779 Py_XDECREF(quaternion0);
2780 Py_XDECREF(quaternion1);
2781 return NULL;
2782 }
2783
2784 /*
2785 Quaternion conjugate.
2786 */
2787 char py_quaternion_conjugate_doc[] = "Return conjugate of quaternion.";
2788
2789 static PyObject *
2790 py_quaternion_conjugate(
2791 PyObject *obj,
2792 PyObject *args,
2793 PyObject *kwds)
2794 {
2795 PyArrayObject *quaternion = NULL;
2796 PyArrayObject *result = NULL;
2797 Py_ssize_t dims = 4;
2798 static char *kwlist[] = {"quaternion", NULL};
2799
2800 if (!PyArg_ParseTupleAndKeywords(args, kwds, "O&", kwlist,
2801 PyConverter_DoubleVector4, &quaternion)) goto _fail;
2802
2803 result = (PyArrayObject*)PyArray_SimpleNew(1, &dims, NPY_DOUBLE);
2804 if (result == NULL) {
2805 PyErr_Format(PyExc_MemoryError, "unable to allocate quaternion");
2806 goto _fail;
2807 }
2808 {
2809 double *q0 = (double *)PyArray_DATA(quaternion);
2810 double *q1 = (double *)PyArray_DATA(result);
2811 q1[0] = q0[0];
2812 q1[1] = -q0[1];
2813 q1[2] = -q0[2];
2814 q1[3] = -q0[3];
2815 }
2816
2817 Py_DECREF(quaternion);
2818 return PyArray_Return(result);
2819
2820 _fail:
2821 Py_XDECREF(result);
2822 Py_XDECREF(quaternion);
2823 return NULL;
2824 }
2825
2826 /*
2827 Quaternion inverse.
2828 */
2829 char py_quaternion_inverse_doc[] = "Return inverse of quaternion.";
2830
2831 static PyObject *
2832 py_quaternion_inverse(
2833 PyObject *obj,
2834 PyObject *args,
2835 PyObject *kwds)
2836 {
2837 PyArrayObject *quaternion = NULL;
2838 PyArrayObject *result = NULL;
2839 Py_ssize_t dims = 4;
2840 static char *kwlist[] = {"quaternion", NULL};
2841
2842 if (!PyArg_ParseTupleAndKeywords(args, kwds, "O&", kwlist,
2843 PyConverter_DoubleVector4, &quaternion)) goto _fail;
2844
2845 result = (PyArrayObject*)PyArray_SimpleNew(1, &dims, NPY_DOUBLE);
2846 if (result == NULL) {
2847 PyErr_Format(PyExc_MemoryError, "unable to allocate quaternion");
2848 goto _fail;
2849 }
2850 {
2851 double *r = (double *)PyArray_DATA(result);
2852 double *q = (double *)PyArray_DATA(quaternion);
2853 double n = q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3];
2854
2855 if (n < EPSILON) {
2856 PyErr_Format(PyExc_ValueError, "not a valid quaternion");
2857 goto _fail;
2858 }
2859
2860 r[0] = q[0] / n;
2861 r[1] = -q[1] / n;
2862 r[2] = -q[2] / n;
2863 r[3] = -q[3] / n;
2864 }
2865
2866 Py_DECREF(quaternion);
2867 return PyArray_Return(result);
2868
2869 _fail:
2870 Py_XDECREF(result);
2871 Py_XDECREF(quaternion);
2872 return NULL;
2873 }
2874
2875 /*
2876 Quaternion spherical linear interpolation.
2877 */
2878 char py_quaternion_slerp_doc[] =
2879 "Return spherical linear interpolation between two quaternions.";
2880
2881 static PyObject *
2882 py_quaternion_slerp(
2883 PyObject *obj,
2884 PyObject *args,
2885 PyObject *kwds)
2886 {
2887 PyObject *boolobj = NULL;
2888 PyArrayObject *quaternion0 = NULL;
2889 PyArrayObject *quaternion1 = NULL;
2890 PyArrayObject *result = NULL;
2891 Py_ssize_t dims = 4;
2892 int shortestpath = 1;
2893 int spin = 0;
2894 double fraction = 0.0;
2895 static char *kwlist[] = {"quat0", "quat1", "fraction",
2896 "spin", "shortestpath", NULL};
2897
2898 if (!PyArg_ParseTupleAndKeywords(args, kwds, "O&O&d|iO", kwlist,
2899 PyConverter_DoubleVector4, &quaternion0,
2900 PyConverter_DoubleVector4, &quaternion1,
2901 &fraction, &spin, &boolobj)) goto _fail;
2902
2903 if (boolobj != NULL)
2904 shortestpath = PyObject_IsTrue(boolobj);
2905
2906 result = (PyArrayObject*)PyArray_SimpleNew(1, &dims, NPY_DOUBLE);
2907 if (result == NULL) {
2908 PyErr_Format(PyExc_MemoryError, "unable to allocate quaternion");
2909 goto _fail;
2910 }
2911 {
2912 double *q = (double *)PyArray_DATA(result);
2913 double *q0 = (double *)PyArray_DATA(quaternion0);
2914 double *q1 = (double *)PyArray_DATA(quaternion1);
2915 double n;
2916
2917 n = sqrt(q0[0]*q0[0] + q0[1]*q0[1] + q0[2]*q0[2] + q0[3]*q0[3]);
2918 if (n < EPSILON) {
2919 PyErr_Format(PyExc_ValueError, "quaternion0 not valid");
2920 goto _fail;
2921 }
2922 q[0] = q0[0] / n;
2923 q[1] = q0[1] / n;
2924 q[2] = q0[2] / n;
2925 q[3] = q0[3] / n;
2926
2927 n = sqrt(q1[0]*q1[0] + q1[1]*q1[1] + q1[2]*q1[2] + q1[3]*q1[3]);
2928 if (n < EPSILON) {
2929 PyErr_Format(PyExc_ValueError, "quaternion1 not valid");
2930 goto _fail;
2931 }
2932
2933 if (fabs(fabs(fraction) - 1.0) < EPSILON) {
2934 q[0] = q1[0] / n;
2935 q[1] = q1[1] / n;
2936 q[2] = q1[2] / n;
2937 q[3] = q1[3] / n;
2938 } else if (NOTZERO(fraction)) {
2939 int flip = 0;
2940 double a = (q[0]*q1[0] + q[1]*q1[1] + q[2]*q1[2] + q[3]*q1[3]) / n;
2941 if (fabs(fabs(a) - 1.0) > EPSILON) {
2942 if (shortestpath && (a < 0.0)) {
2943 a = -a;
2944 flip = 1;
2945 }
2946 a = acos(a) + spin * M_PI;
2947 if (NOTZERO(a)) {
2948 double s = 1.0 / sin(a);
2949 double f0 = sin((1.0 - fraction) * a) * s;
2950 double f1 = sin(fraction * a) * s / n;
2951 if (flip) f1 = -f1;
2952 q[0] = q[0] * f0 + q1[0] * f1;
2953 q[1] = q[1] * f0 + q1[1] * f1;
2954 q[2] = q[2] * f0 + q1[2] * f1;
2955 q[3] = q[3] * f0 + q1[3] * f1;
2956 }
2957 }
2958 }
2959 }
2960
2961 Py_DECREF(quaternion0);
2962 Py_DECREF(quaternion1);
2963 return PyArray_Return(result);
2964
2965 _fail:
2966 Py_XDECREF(result);
2967 Py_DECREF(quaternion0);
2968 Py_DECREF(quaternion1);
2969 return NULL;
2970 }
2971
2972 /*
2973 Random quaternion.
2974 */
2975 char py_random_quaternion_doc[] =
2976 "Return uniform random unit quaternion.";
2977
2978 static PyObject *
2979 py_random_quaternion(
2980 PyObject *obj,
2981 PyObject *args,
2982 PyObject *kwds)
2983 {
2984 PyArrayObject *result = NULL;
2985 PyArrayObject *arand = NULL;
2986 Py_ssize_t dims = 4;
2987 static char *kwlist[] = {"rand", NULL};
2988
2989 if (!PyArg_ParseTupleAndKeywords(args, kwds, "|O&", kwlist,
2990 PyConverter_DoubleVector3OrNone, &arand)) goto _fail;
2991
2992 result = (PyArrayObject*)PyArray_SimpleNew(1, &dims, NPY_DOUBLE);
2993 if (result == NULL) {
2994 PyErr_Format(PyExc_MemoryError, "unable to allocate quaternion");
2995 goto _fail;
2996 }
2997 {
2998 double *q = (double *)PyArray_DATA(result);
2999 double r0, r1, r2, t;
3000
3001 if (arand == NULL) {
3002 double r[3];
3003 if (random_doubles(&r[0], 3) != 0) {
3004 PyErr_Format(PyExc_ValueError, "random_numbers() failed");
3005 goto _fail;
3006 }
3007 r0 = r[0];
3008 r1 = r[1];
3009 r2 = r[2];
3010 } else {
3011 double *r = (double *)PyArray_DATA(arand);
3012 r0 = r[0];
3013 r1 = r[1];
3014 r2 = r[2];
3015 }
3016 t = TWOPI * r1;
3017 r1 = sqrt(1.0 - r0);
3018 q[1] = sin(t) * r1;
3019 q[2] = cos(t) * r1;
3020 t = TWOPI * r2;
3021 r2 = sqrt(r0);
3022 q[3] = sin(t) * r2;
3023 q[0] = cos(t) * r2;
3024 }
3025
3026 Py_XDECREF(arand);
3027 return PyArray_Return(result);
3028
3029 _fail:
3030 Py_XDECREF(arand);
3031 Py_XDECREF(result);
3032 return NULL;
3033 }
3034
3035 /*
3036 Random rotation matrix.
3037 */
3038 char py_random_rotation_matrix_doc[] =
3039 "Return uniform random rotation matrix.";
3040
3041 static PyObject *
3042 py_random_rotation_matrix(
3043 PyObject *obj,
3044 PyObject *args,
3045 PyObject *kwds)
3046 {
3047 PyArrayObject *result = NULL;
3048 PyArrayObject *arand = NULL;
3049 Py_ssize_t dims[] = {4, 4};
3050 static char *kwlist[] = {"rand", NULL};
3051
3052 if (!PyArg_ParseTupleAndKeywords(args, kwds, "|O&", kwlist,
3053 PyConverter_DoubleVector3OrNone, &arand)) goto _fail;
3054
3055 result = (PyArrayObject*)PyArray_SimpleNew(2, dims, NPY_DOUBLE);
3056 if (result == NULL) {
3057 PyErr_Format(PyExc_MemoryError, "unable to allocate matrix");
3058 goto _fail;
3059 }
3060 {
3061 double *M = (double *)PyArray_DATA(result);
3062 double r0, r1, r2, t, qx, qy, qz, qw;
3063
3064 if (arand == NULL) {
3065 double r[3];
3066 if (random_doubles(&r[0], 3) != 0) {
3067 PyErr_Format(PyExc_ValueError, "random_numbers() failed");
3068 goto _fail;
3069 }
3070 r0 = r[0];
3071 r1 = r[1];
3072 r2 = r[2];
3073 } else {
3074 double *r = (double *)PyArray_DATA(arand);
3075 r0 = r[0];
3076 r1 = r[1];
3077 r2 = r[2];
3078 }
3079 t = TWOPI * r1;
3080 r1 = sqrt(1.0 - r0);
3081 qx = sin(t) * r1;
3082 qy = cos(t) * r1;
3083 t = TWOPI * r2;
3084 r2 = sqrt(r0);
3085 qz = sin(t) * r2;
3086 qw = cos(t) * r2;
3087
3088 {
3089 double x2 = qx+qx;
3090 double y2 = qy+qy;
3091 double z2 = qz+qz;
3092 {
3093 double xx2 = qx*x2;
3094 double yy2 = qy*y2;
3095 double zz2 = qz*z2;
3096 M[0] = 1.0 - yy2 - zz2;
3097 M[5] = 1.0 - xx2 - zz2;
3098 M[10] = 1.0 - xx2 - yy2;
3099 }{
3100 double yz2 = qy*z2;
3101 double wx2 = qw*x2;
3102 M[6] = yz2 - wx2;
3103 M[9] = yz2 + wx2;
3104 }{
3105 double xy2 = qx*y2;
3106 double wz2 = qw*z2;
3107 M[1] = xy2 - wz2;
3108 M[4] = xy2 + wz2;
3109 }{
3110 double xz2 = qx*z2;
3111 double wy2 = qw*y2;
3112 M[8] = xz2 - wy2;
3113 M[2] = xz2 + wy2;
3114 }
3115 M[3] = M[7] = M[11] = M[12] = M[13] = M[14] = 0.0;
3116 M[15] = 1.0;
3117 }
3118 }
3119
3120 Py_XDECREF(arand);
3121 return PyArray_Return(result);
3122
3123 _fail:
3124 Py_XDECREF(arand);
3125 Py_XDECREF(result);
3126 return NULL;
3127 }
3128
3129 /*
3130 Matrix inversion.
3131 Significantly faster than numpy.linalg.inv() for small sizes.
3132 */
3133 char py_inverse_matrix_doc[] = "Return inverse of symmetric matrix.";
3134
3135 static PyObject *
3136 py_inverse_matrix(
3137 PyObject *obj,
3138 PyObject *args,
3139 PyObject *kwds)
3140 {
3141 PyObject *object;
3142 PyArrayObject *result = NULL;
3143 PyArrayObject *matrix = NULL;
3144 Py_ssize_t dims[2];
3145 Py_ssize_t size = 0;
3146 static char *kwlist[] = {"matrix", NULL};
3147 int iscopy = 0;
3148
3149 if (!PyArg_ParseTupleAndKeywords(args, kwds, "O", kwlist, &object))
3150 goto _fail;
3151
3152 matrix = (PyArrayObject *)PyArray_FROM_OTF(object, NPY_DOUBLE,
3153 NPY_IN_ARRAY);
3154 if (matrix == NULL) {
3155 PyErr_Format(PyExc_ValueError, "not an array");
3156 goto _fail;
3157 }
3158 iscopy = ((PyObject *)matrix != object);
3159
3160 size = PyArray_DIM(matrix, 0);
3161 if ((size != PyArray_DIM(matrix, 1)) || (size < 1)) {
3162 PyErr_Format(PyExc_ValueError, "not a symmetric matrix");
3163 goto _fail;
3164 }
3165
3166 dims[0] = dims[1] = size;
3167 result = (PyArrayObject*)PyArray_SimpleNew(2, dims, NPY_DOUBLE);
3168 if (result == NULL) {
3169 PyErr_Format(PyExc_MemoryError, "unable to allocate matrix");
3170 goto _fail;
3171 }
3172 {
3173 int error = 1;
3174 double *M = (double *)PyArray_DATA(matrix);
3175 double *Minv = (double *)PyArray_DATA(result);
3176
3177 switch (size) {
3178 case 4:
3179 error = invert_matrix44(M, Minv);
3180 break;
3181 case 3:
3182 error = invert_matrix33(M, Minv);
3183 break;
3184 case 2:
3185 error = invert_matrix22(M, Minv);
3186 break;
3187 case 1:
3188 error = ISZERO(M[0]);
3189 if (error == 0)
3190 Minv[0] = 1.0 / M[0];
3191 break;
3192 default: {
3193 void *buffer;
3194 if (iscopy)
3195 buffer = PyMem_Malloc(size*2*sizeof(Py_ssize_t));
3196 else
3197 buffer = PyMem_Malloc(size*2*sizeof(Py_ssize_t) +
3198 size*size*sizeof(double));
3199 if (buffer == NULL) {
3200 PyErr_Format(PyExc_MemoryError,
3201 "unable to allocate buffer");
3202 goto _fail;
3203 }
3204 if (!iscopy) {
3205 M = (double *)((Py_ssize_t *)buffer + 2*size);
3206 memcpy(M, (double *)PyArray_DATA(matrix),
3207 size*size*sizeof(double));
3208 }
3209 Py_BEGIN_ALLOW_THREADS
3210 error = invert_matrix(size, M, Minv, (Py_ssize_t *)buffer);
3211 Py_END_ALLOW_THREADS
3212 PyMem_Free(buffer);
3213 }
3214 }
3215
3216 if (error != 0) {
3217 PyErr_Format(PyExc_ValueError, "non-singular matrix");
3218 goto _fail;
3219 }
3220 }
3221
3222 Py_XDECREF(matrix);
3223 return PyArray_Return(result);
3224
3225 _fail:
3226 Py_XDECREF(matrix);
3227 Py_XDECREF(result);
3228 return NULL;
3229 }
3230
3231 /*
3232 Arcball: map window to sphere coordinates.
3233 */
3234 char py_arcball_map_to_sphere_doc[] =
3235 "Return unit sphere coordinates from window coordinates.";
3236
3237 static PyObject *
3238 py_arcball_map_to_sphere(
3239 PyObject *obj,
3240 PyObject *args,
3241 PyObject *kwds)
3242 {
3243 PyObject *point = NULL;
3244 PyObject *center = NULL;
3245 PyArrayObject *result = NULL;
3246 Py_ssize_t dims = 3;
3247 double p[] = {0.0, 0.0};
3248 double c[] = {0.0, 0.0};
3249 double radius;
3250 static char *kwlist[] = {"point", "center", "radius", NULL};
3251
3252 if (!PyArg_ParseTupleAndKeywords(args, kwds, "OOd", kwlist,
3253 &point, ¢er, &radius)) goto _fail;
3254
3255 result = (PyArrayObject*)PyArray_SimpleNew(1, &dims, NPY_DOUBLE);
3256 if (result == NULL) {
3257 PyErr_Format(PyExc_MemoryError, "unable to allocate vector");
3258 goto _fail;
3259 }
3260
3261 if (PySequence_Check(point) && (PySequence_Size(point) > 1)) {
3262 PyObject* o;
3263 o = PySequence_GetItem(point, 0);
3264 if (o != NULL) {
3265 p[0] = PyFloat_AsDouble(o);
3266 }
3267 Py_XDECREF(o);
3268 o = PySequence_GetItem(point, 1);
3269 if (o != NULL) {
3270 p[1] = PyFloat_AsDouble(o);
3271 }
3272 Py_XDECREF(o);
3273 } else {
3274 PyErr_Format(PyExc_ValueError, "invalid point");
3275 goto _fail;
3276 }
3277
3278 if (PySequence_Check(center) && (PySequence_Size(center) > 1)) {
3279 PyObject* o;
3280 o = PySequence_GetItem(center, 0);
3281 if (o != NULL) {
3282 c[0] = PyFloat_AsDouble(o);
3283 }
3284 Py_XDECREF(o);
3285 o = PySequence_GetItem(center, 1);
3286 if (o != NULL) {
3287 c[1] = PyFloat_AsDouble(o);
3288 }
3289 Py_XDECREF(o);
3290 } else {
3291 PyErr_Format(PyExc_ValueError, "invalid center");
3292 goto _fail;
3293 }
3294 {
3295 double *v = (double *)PyArray_DATA(result);
3296 double n;
3297
3298 v[0] = (p[0] - c[0]) / radius;
3299 v[1] = (c[1] - p[1]) / radius;
3300
3301 n = v[0]*v[0] + v[1]*v[1];
3302
3303 if (n > 1.0) {
3304 n = sqrt(n);
3305 v[0] /= n;
3306 v[1] /= n;
3307 v[2] = 0.0;
3308 } else {
3309 v[2] = sqrt(1.0 - n);
3310 }
3311 }
3312
3313 return PyArray_Return(result);
3314
3315 _fail:
3316 Py_XDECREF(result);
3317 return NULL;
3318 }
3319
3320 /*
3321 Arcball: constrain point to axis.
3322 */
3323 char py_arcball_constrain_to_axis_doc[] =
3324 "Return sphere point perpendicular to axis.";
3325
3326 static PyObject *
3327 py_arcball_constrain_to_axis(
3328 PyObject *obj,
3329 PyObject *args,
3330 PyObject *kwds)
3331 {
3332 PyArrayObject *point = NULL;
3333 PyArrayObject *axis = NULL;
3334 PyArrayObject *result = NULL;
3335 Py_ssize_t dims = 3;
3336 static char *kwlist[] = {"point", "axis", NULL};
3337
3338 if (!PyArg_ParseTupleAndKeywords(args, kwds, "O&O&", kwlist,
3339 PyConverter_DoubleVector3, &point,
3340 PyConverter_DoubleVector3, &axis)) goto _fail;
3341
3342 result = (PyArrayObject*)PyArray_SimpleNew(1, &dims, NPY_DOUBLE);
3343 if (result == NULL) {
3344 PyErr_Format(PyExc_MemoryError, "unable to allocate vector");
3345 goto _fail;
3346 }
3347 {
3348 double *v = (double *)PyArray_DATA(result);
3349 double *a = (double *)PyArray_DATA(axis);
3350 double *p = (double *)PyArray_DATA(point);
3351 double n = p[0]*a[0] + p[1]*a[1] + p[2]*a[2];
3352
3353 v[0] = p[0] - a[0]*n;
3354 v[1] = p[1] - a[1]*n;
3355 v[2] = p[2] - a[2]*n;
3356
3357 n = sqrt(v[0]*v[0] + v[1]*v[1] + v[2]*v[2]);
3358
3359 if (n > EPSILON) {
3360 v[0] /= n;
3361 v[1] /= n;
3362 v[2] /= n;
3363 } else if (a[2] == 1.0) {
3364 v[0] = 1.0;
3365 v[1] = 0.0;
3366 v[2] = 0.0;
3367 } else {
3368 n = sqrt(a[0]*a[0] + a[1]*a[1]);
3369 v[0] = -a[1] / n;
3370 v[1] = a[0] / n;
3371 v[2] = 0.0;
3372 }
3373 }
3374
3375 Py_DECREF(axis);
3376 Py_DECREF(point);
3377 return PyArray_Return(result);
3378
3379 _fail:
3380 Py_XDECREF(axis);
3381 Py_XDECREF(point);
3382 Py_XDECREF(result);
3383 return NULL;
3384 }
3385
3386 /*
3387 Vector length along axis of ndarray.
3388 */
3389 char py_vector_norm_doc[] =
3390 "Return length, i.e. eucledian norm, of ndarray along axis.";
3391
3392 static PyObject *
3393 py_vector_norm(
3394 PyObject *obj,
3395 PyObject *args,
3396 PyObject *kwds)
3397 {
3398 PyArrayObject *data = NULL;
3399 PyArrayObject *out = NULL;
3400 PyArrayObject *oout = NULL;
3401 PyArrayIterObject *dit = NULL;
3402 PyArrayIterObject *oit = NULL;
3403 Py_ssize_t newshape[NPY_MAXDIMS];
3404 int axis = NPY_MAXDIMS;
3405
3406 static char *kwlist[] = {"data", "axis", "out", NULL};
3407
3408 if (!PyArg_ParseTupleAndKeywords(args, kwds, "O&|O&O&", kwlist,
3409 PyConverter_AnyDoubleArray, &data,
3410 PyArray_AxisConverter, &axis,
3411 PyOutputConverter_AnyDoubleArrayOrNone, &oout)) goto _fail;
3412
3413 if (axis == NPY_MAXDIMS ) {
3414 /* iterate over all elements */
3415 double len = 0.0;
3416
3417 if (oout != NULL) {
3418 PyErr_Format(PyExc_ValueError,
3419 "axis needs to be specified when output array is given");
3420 goto _fail;
3421 }
3422
3423 if ((PyArray_NDIM(data) == 1) &&
3424 (PyArray_STRIDE(data, 0) == sizeof(double))) {
3425 Py_ssize_t i;
3426 double* dptr = PyArray_DATA(data);
3427 #pragma vector always
3428 for (i = 0; i < PyArray_DIM(data, 0); i++) {
3429 len += dptr[i] * dptr[i];
3430 }
3431 } else {
3432 double t;
3433 dit = (PyArrayIterObject *) PyArray_IterNew((PyObject *)data);
3434 if (dit == NULL) {
3435 PyErr_Format(PyExc_ValueError, "failed to create iterator");
3436 goto _fail;
3437 }
3438 while (dit->index < dit->size) {
3439 t = *((double *)dit->dataptr);
3440 len += t*t;
3441 PyArray_ITER_NEXT(dit);
3442 }
3443 Py_DECREF(dit);
3444 }
3445 len = sqrt(len);
3446
3447 Py_DECREF(data);
3448 return PyFloat_FromDouble(len);
3449
3450 } else { /* iterate over elements of specified axis */
3451 Py_ssize_t dstride, s, size;
3452 Py_ssize_t i, j;
3453 int n = PyArray_NDIM(data);
3454
3455 /* calculate shape of output array */
3456 if (axis < 0) {
3457 axis += n;
3458 }
3459 if ((axis < 0) || (axis >= n)) {
3460 PyErr_Format(PyExc_ValueError, "invalid axis");
3461 goto _fail;
3462 }
3463
3464 j = 0;
3465 for (i = 0; i < n; i++) {
3466 if (i != axis)
3467 newshape[j++] = PyArray_DIM(data, i);
3468 }
3469
3470 if (oout == NULL) {
3471 /* create a new output array */
3472 out = (PyArrayObject*)PyArray_SimpleNew(n-1, newshape,
3473 NPY_DOUBLE);
3474 if (out == NULL) {
3475 PyErr_Format(PyExc_MemoryError, "failed to allocate array");
3476 goto _fail;
3477 }
3478 } else {
3479 /* validate given output array */
3480 if (PyArray_NDIM(data) != (PyArray_NDIM(oout)+1)) {
3481 PyErr_Format(PyExc_ValueError,
3482 "size of output must match data");
3483 goto _fail;
3484 }
3485 j = 0;
3486 for (i = 0; i < n; i++) {
3487 if ((i != axis) && (PyArray_DIM(data, i) != newshape[j++])) {
3488 PyErr_Format(PyExc_ValueError, "incorrect output size");
3489 goto _fail;
3490 }
3491 }
3492 out = oout;
3493 }
3494
3495 /* iterate data over all but specified axis */
3496 dit = (PyArrayIterObject *) PyArray_IterAllButAxis((PyObject *)data,
3497 &axis);
3498 oit = (PyArrayIterObject *) PyArray_IterNew((PyObject *)out);
3499 dstride = PyArray_STRIDE(data, axis);
3500 size = PyArray_DIM(data, axis);
3501
3502 if (dstride == sizeof(double)) {
3503 double *dptr;
3504 double len;
3505 while (dit->index < dit->size) {
3506 dptr = (double *)dit->dataptr;
3507 len = 0.0;
3508 #pragma vector always
3509 for (s = 0; s < size; s++) {
3510 len += dptr[s]*dptr[s];
3511 }
3512 *((double *)oit->dataptr) = sqrt(len);
3513 PyArray_ITER_NEXT(oit);
3514 PyArray_ITER_NEXT(dit);
3515 }
3516 } else {
3517 char *dptr;
3518 double t, len;
3519 while (dit->index < dit->size) {
3520 dptr = dit->dataptr;
3521 len = 0.0;
3522 s = size;
3523 while (s--) {
3524 t = *((double*) dptr);
3525 len += t*t;
3526 dptr += dstride;
3527 }
3528 *((double *)oit->dataptr) = sqrt(len);
3529 PyArray_ITER_NEXT(oit);
3530 PyArray_ITER_NEXT(dit);
3531 }
3532 }
3533 Py_DECREF(oit);
3534 Py_DECREF(dit);
3535 Py_DECREF(data);
3536
3537 /* Return output vector if not provided as argument */
3538 if (oout == NULL) {
3539 return PyArray_Return(out);
3540 } else {
3541 Py_DECREF(oout);
3542 Py_INCREF(Py_None);
3543 return Py_None;
3544 }
3545 }
3546
3547 _fail:
3548 Py_XDECREF(oit);
3549 Py_XDECREF(dit);
3550 Py_XDECREF(data);
3551 Py_XDECREF((oout == NULL) ? out : oout);
3552 return NULL;
3553 }
3554
3555 /*
3556 Normalize ndarray by vector length along axis.
3557 */
3558 char py_unit_vector_doc[] =
3559 "Return ndarray normalized by length, i.e. eucledian norm, along axis.";
3560
3561 static PyObject *
3562 py_unit_vector(
3563 PyObject *obj,
3564 PyObject *args,
3565 PyObject *kwds)
3566 {
3567 PyArrayObject *data = NULL;
3568 PyArrayObject *out = NULL;
3569 PyArrayObject *oout = NULL;
3570 PyArrayIterObject *dit = NULL;
3571 PyArrayIterObject *oit = NULL;
3572 Py_ssize_t dstride, ostride;
3573 int axis = NPY_MAXDIMS;
3574
3575 static char *kwlist[] = {"data", "axis", "out", NULL};
3576
3577 if (!PyArg_ParseTupleAndKeywords(args, kwds, "O&|O&O&", kwlist,
3578 PyConverter_AnyDoubleArray, &data,
3579 PyArray_AxisConverter, &axis,
3580 PyOutputConverter_AnyDoubleArrayOrNone, &oout)) goto _fail;
3581
3582 if (oout == NULL) {
3583 /* create a new output array */
3584 out = (PyArrayObject*)PyArray_SimpleNew(PyArray_NDIM(data),
3585 PyArray_DIMS(data), NPY_DOUBLE);
3586 if (out == NULL) {
3587 PyErr_Format(PyExc_ValueError, "failed to create output array");
3588 goto _fail;
3589 }
3590 } else {
3591 /* check shape of provided output array */
3592 if (!PyArray_SAMESHAPE(data, oout)) {
3593 PyErr_Format(PyExc_ValueError, "shape of output must match data");
3594 goto _fail;
3595 }
3596 out = oout;
3597 }
3598
3599 if (axis == NPY_MAXDIMS) {
3600 /* iterate over all elements */
3601 if ((PyArray_NDIM(data) == 1) &&
3602 (PyArray_STRIDE(data, 0) == sizeof(double)) &&
3603 (PyArray_STRIDE(out, 0) == sizeof(double))) {
3604
3605 Py_ssize_t i, size = PyArray_DIM(data, 0);
3606 double *dptr = (double *)PyArray_DATA(data);
3607 double *optr = (double *)PyArray_DATA(out);
3608 double len = 0.0;
3609
3610 #pragma vector always
3611 for (i = 0; i < size; i++) {
3612 len += dptr[i]*dptr[i];
3613 }
3614 len = 1.0 / sqrt(len);
3615 #pragma vector always
3616 for (i = 0; i < size; i++) {
3617 optr[i] = dptr[i] * len;
3618 }
3619 } else {
3620 double t, len = 0.0;
3621 dit = (PyArrayIterObject *)PyArray_IterNew((PyObject *)data);
3622 oit = (PyArrayIterObject *)PyArray_IterNew((PyObject *)out);
3623 if (dit == NULL || oit == NULL) {
3624 PyErr_Format(PyExc_ValueError,
3625 "failed to create iterator(s)");
3626 goto _fail;
3627 }
3628 while (dit->index < dit->size) {
3629 t = *((double *)dit->dataptr);
3630 len += t*t;
3631 PyArray_ITER_NEXT(dit);
3632 }
3633 Py_DECREF(dit);
3634 len = 1.0 / sqrt(len);
3635 dit = (PyArrayIterObject *) PyArray_IterNew((PyObject *)data);
3636 if (dit == NULL) {
3637 PyErr_Format(PyExc_ValueError, "failed to create iterator");
3638 goto _fail;
3639 }
3640 while (dit->index < dit->size) {
3641 *((double *)oit->dataptr) = *((double *)dit->dataptr) * len;
3642 PyArray_ITER_NEXT(dit);
3643 PyArray_ITER_NEXT(oit);
3644 }
3645 }
3646 }
3647 else {
3648 /* iterate over elements of specified axis */
3649 Py_ssize_t size, s;
3650
3651 if (axis < 0) {
3652 axis += PyArray_NDIM(data);
3653 }
3654 if ((axis < 0) || (axis >= PyArray_NDIM(data))) {
3655 PyErr_Format(PyExc_ValueError, "invalid axis");
3656 goto _fail;
3657 }
3658
3659 dit = (PyArrayIterObject *) PyArray_IterAllButAxis((PyObject *)data,
3660 &axis);
3661 oit = (PyArrayIterObject *) PyArray_IterAllButAxis((PyObject *)out,
3662 &axis);
3663 if (dit == NULL || oit == NULL) {
3664 PyErr_Format(PyExc_ValueError, "failed to create iterator(s)");
3665 goto _fail;
3666 }
3667 dstride = PyArray_STRIDE(data, axis);
3668 ostride = PyArray_STRIDE(out, axis);
3669 size = PyArray_DIM(data, axis);
3670
3671 if ((dstride == sizeof(double)) && (ostride == sizeof(double))) {
3672 Py_ssize_t i;
3673 double len;
3674 double *optr, *dptr;
3675 while (dit->index < dit->size) {
3676 len = 0.0;
3677 optr = (double *)oit->dataptr;
3678 dptr = (double *)dit->dataptr;
3679 #pragma vector always
3680 for (i = 0; i < size; i++) {
3681 len += dptr[i]*dptr[i];
3682 }
3683 len = 1.0 / sqrt(len);
3684 #pragma vector always
3685 for (i = 0; i < size; i++) {
3686 optr[i] = dptr[i] * len;
3687 }
3688 PyArray_ITER_NEXT(oit);
3689 PyArray_ITER_NEXT(dit);
3690 }
3691 } else {
3692 double t, len;
3693 char *optr, *dptr;
3694
3695 while (dit->index < dit->size) {
3696 len = 0.0;
3697 dptr = dit->dataptr;
3698 s = size;
3699 while (s--) {
3700 t = *((double*) dptr);
3701 len += t*t;
3702 dptr += dstride;
3703 }
3704 len = 1.0 / sqrt(len);
3705 dptr = dit->dataptr;
3706 optr = oit->dataptr;
3707 s = size;
3708 while (s--) {
3709 *((double*) optr) = *((double*) dptr) * len;
3710 optr += ostride;
3711 dptr += dstride;
3712 }
3713 PyArray_ITER_NEXT(oit);
3714 PyArray_ITER_NEXT(dit);
3715 }
3716 }
3717 }
3718
3719 Py_XDECREF(oit);
3720 Py_XDECREF(dit);
3721 Py_DECREF(data);
3722
3723 /* Return output vector if not provided as argument */
3724 if (oout == NULL) {
3725 return PyArray_Return(out);
3726 } else {
3727 Py_DECREF(oout);
3728 Py_INCREF(Py_None);
3729 return Py_None;
3730 }
3731
3732 _fail:
3733 Py_XDECREF(oit);
3734 Py_XDECREF(dit);
3735 Py_XDECREF(data);
3736 Py_XDECREF((oout == NULL) ? out : oout);
3737 return NULL;
3738 }
3739
3740 /*
3741 Random vector.
3742 */
3743 char py_random_vector_doc[] =
3744 "Return array of random doubles in half-open interval [0.0, 1.0).";
3745
3746 static PyObject *
3747 py_random_vector(
3748 PyObject *obj,
3749 PyObject *args,
3750 PyObject *kwds)
3751 {
3752 PyArrayObject *result = NULL;
3753 Py_ssize_t size = 0;
3754 int error ;
3755 static char *kwlist[] = {"size", NULL};
3756
3757 if (!PyArg_ParseTupleAndKeywords(args, kwds, "n", kwlist, &size))
3758 goto _fail;
3759
3760 result = (PyArrayObject*)PyArray_SimpleNew(1, &size, NPY_DOUBLE);
3761 if (result == NULL) {
3762 PyErr_Format(PyExc_MemoryError, "unable to allocate array");
3763 goto _fail;
3764 }
3765
3766 Py_BEGIN_ALLOW_THREADS
3767 error = random_doubles((double *)PyArray_DATA(result), size);
3768 Py_END_ALLOW_THREADS
3769
3770 if (error != 0) {
3771 PyErr_Format(PyExc_ValueError, "random_doubles() failed");
3772 goto _fail;
3773 }
3774
3775 return PyArray_Return(result);
3776
3777 _fail:
3778 Py_XDECREF(result);
3779 return NULL;
3780 }
3781
3782 /*
3783 Tridiagonal matrix.
3784 */
3785 char py_tridiagonalize_symmetric_44_doc[] =
3786 "Turn symmetric 4x4 matrix into tridiagonal matrix.";
3787
3788 static PyObject *
3789 py_tridiagonalize_symmetric_44(
3790 PyObject *obj,
3791 PyObject *args,
3792 PyObject *kwds)
3793 {
3794 PyArrayObject *matrix = NULL;
3795 PyArrayObject *diagonal = NULL;
3796 PyArrayObject *subdiagonal = NULL;
3797 Py_ssize_t dims = 4;
3798 int error;
3799 static char *kwlist[] = {"matrix", NULL};
3800
3801 if (!PyArg_ParseTupleAndKeywords(args, kwds, "O&", kwlist,
3802 PyConverter_DoubleMatrix44Copy, &matrix)) goto _fail;
3803
3804 diagonal = (PyArrayObject*)PyArray_SimpleNew(1, &dims, NPY_DOUBLE);
3805 if (diagonal == NULL) {
3806 PyErr_Format(PyExc_MemoryError, "unable to allocate diagonal");
3807 goto _fail;
3808 }
3809
3810 dims = 3;
3811 subdiagonal = (PyArrayObject*)PyArray_SimpleNew(1, &dims, NPY_DOUBLE);
3812 if (subdiagonal == NULL) {
3813 PyErr_Format(PyExc_MemoryError, "unable to allocate subdiagonal");
3814 goto _fail;
3815 }
3816
3817 Py_BEGIN_ALLOW_THREADS
3818 error = tridiagonalize_symmetric_44(
3819 (double *)PyArray_DATA(matrix),
3820 (double *)PyArray_DATA(diagonal),
3821 (double *)PyArray_DATA(subdiagonal));
3822 Py_END_ALLOW_THREADS
3823
3824 if (error != 0) {
3825 PyErr_Format(PyExc_ValueError,
3826 "tridiagonalize_symmetric_44() failed");
3827 goto _fail;
3828 }
3829
3830 Py_DECREF(matrix);
3831 return Py_BuildValue("(N,N)", diagonal, subdiagonal);
3832
3833 _fail:
3834 Py_XDECREF(matrix);
3835 Py_XDECREF(diagonal);
3836 Py_XDECREF(subdiagonal);
3837 return NULL;
3838 }
3839
3840 /*
3841 Eigenvalue of tridiagonal matrix.
3842 */
3843 char py_max_eigenvalue_of_tridiag_44_doc[] =
3844 "Return largest eigenvalue of symmetric tridiagonal 4x4 matrix.";
3845
3846 static PyObject *
3847 py_max_eigenvalue_of_tridiag_44(
3848 PyObject *obj,
3849 PyObject *args,
3850 PyObject *kwds)
3851 {
3852 PyArrayObject *diagonal = NULL;
3853 PyArrayObject *subdiagonal = NULL;
3854 double result;
3855 static char *kwlist[] = {"diagonal", "subdiagonal", NULL};
3856
3857 if (!PyArg_ParseTupleAndKeywords(args, kwds, "O&O&", kwlist,
3858 PyConverter_DoubleVector4, &diagonal,
3859 PyConverter_DoubleVector3, &subdiagonal)) goto _fail;
3860
3861 result = max_eigenvalue_of_tridiag_44(
3862 (double *)PyArray_DATA(diagonal),
3863 (double *)PyArray_DATA(subdiagonal));
3864
3865 Py_DECREF(diagonal);
3866 Py_DECREF(subdiagonal);
3867 return PyFloat_FromDouble(result);
3868
3869 _fail:
3870 Py_XDECREF(diagonal);
3871 Py_XDECREF(subdiagonal);
3872 return NULL;
3873 }
3874
3875 /*
3876 Eigenvector of symmetric matrix.
3877 */
3878 char py_eigenvector_of_symmetric_44_doc[] =
3879 "Return eigenvector of eigenvalue of symmetric tridiagonal 4x4 matrix.";
3880
3881 static PyObject *
3882 py_eigenvector_of_symmetric_44(
3883 PyObject *obj,
3884 PyObject *args,
3885 PyObject *kwds)
3886 {
3887 PyArrayObject *matrix = NULL;
3888 PyArrayObject *result = NULL;
3889 Py_ssize_t dims = 4;
3890 int error;
3891 double *M;
3892 double *buffer = NULL;
3893 double eigenvalue;
3894 static char *kwlist[] = {"matrix", "eigenvalue", NULL};
3895
3896 if (!PyArg_ParseTupleAndKeywords(args, kwds, "O&d", kwlist,
3897 PyConverter_DoubleMatrix44Copy, &matrix, &eigenvalue)) goto _fail;
3898
3899 result = (PyArrayObject*)PyArray_SimpleNew(1, &dims, NPY_DOUBLE);
3900 if (result == NULL) {
3901 PyErr_Format(PyExc_MemoryError, "unable to allocate eigenvector");
3902 goto _fail;
3903 }
3904
3905 buffer = PyMem_Malloc(12 * sizeof(double));
3906 if (buffer == NULL) {
3907 PyErr_Format(PyExc_MemoryError, "unable to allocate buffer");
3908 goto _fail;
3909 }
3910
3911 M = (double *)PyArray_DATA(matrix);
3912 M[0] -= eigenvalue;
3913 M[5] -= eigenvalue;
3914 M[10] -= eigenvalue;
3915 M[15] -= eigenvalue;
3916
3917 Py_BEGIN_ALLOW_THREADS
3918 error = eigenvector_of_symmetric_44(
3919 M, (double *)PyArray_DATA(result), buffer);
3920 Py_END_ALLOW_THREADS
3921
3922 if (error != 0) {
3923 PyErr_Format(PyExc_ValueError, "no eigenvector found");
3924 goto _fail;
3925 }
3926
3927 PyMem_Free(buffer);
3928 Py_DECREF(matrix);
3929 return PyArray_Return(result);
3930
3931 _fail:
3932 PyMem_Free(buffer);
3933 Py_XDECREF(matrix);
3934 Py_XDECREF(result);
3935 return NULL;
3936 }
3937
3938 /*****************************************************************************/
3939 /* Create Python module */
3940
3941 char module_doc[] =
3942 "Homogeneous Transformation Matrices and Quaternions.\n\n"
3943 "Refer to the transformations.py module for documentation and tests.\n\n"
3944 "Authors:\n Christoph Gohlke <http://www.lfd.uci.edu/~gohlke/>\n"
3945 " Laboratory for Fluorescence Dynamics, University of California, Irvine."
3946 "\n\nVersion: %s\n";
3947
3948 static PyMethodDef module_methods[] = {
3949 {"is_same_transform",
3950 (PyCFunction)py_is_same_transform,
3951 METH_VARARGS|METH_KEYWORDS, py_is_same_transform_doc},
3952 {"identity_matrix",
3953 (PyCFunction)py_identity_matrix, METH_NOARGS,
3954 py_identity_matrix_doc},
3955 {"translation_matrix",
3956 (PyCFunction)py_translation_matrix,
3957 METH_VARARGS|METH_KEYWORDS, py_translation_matrix_doc},
3958 {"reflection_matrix",
3959 (PyCFunction)py_reflection_matrix,
3960 METH_VARARGS|METH_KEYWORDS, py_reflection_matrix_doc},
3961 {"rotation_matrix",
3962 (PyCFunction)py_rotation_matrix,
3963 METH_VARARGS|METH_KEYWORDS, py_rotation_matrix_doc},
3964 {"scale_matrix",
3965 (PyCFunction)py_scale_matrix,
3966 METH_VARARGS|METH_KEYWORDS, py_scale_matrix_doc},
3967 {"projection_matrix",
3968 (PyCFunction)py_projection_matrix,
3969 METH_VARARGS|METH_KEYWORDS, py_projection_matrix_doc},
3970 {"clip_matrix",
3971 (PyCFunction)py_clip_matrix,
3972 METH_VARARGS|METH_KEYWORDS, py_clip_matrix_doc},
3973 {"shear_matrix",
3974 (PyCFunction)py_shear_matrix,
3975 METH_VARARGS|METH_KEYWORDS, py_shear_matrix_doc},
3976 {"superimposition_matrix",
3977 (PyCFunction)py_superimposition_matrix,
3978 METH_VARARGS|METH_KEYWORDS, py_superimposition_matrix_doc},
3979 {"orthogonalization_matrix",
3980 (PyCFunction)py_orthogonalization_matrix,
3981 METH_VARARGS|METH_KEYWORDS, py_orthogonalization_matrix_doc},
3982 {"euler_matrix",
3983 (PyCFunction)py_euler_matrix,
3984 METH_VARARGS|METH_KEYWORDS, py_euler_matrix_doc},
3985 {"euler_from_matrix",
3986 (PyCFunction)py_euler_from_matrix,
3987 METH_VARARGS|METH_KEYWORDS, py_euler_from_matrix_doc},
3988 {"quaternion_from_euler",
3989 (PyCFunction)py_quaternion_from_euler,
3990 METH_VARARGS|METH_KEYWORDS, py_quaternion_from_euler_doc},
3991 {"quaternion_about_axis",
3992 (PyCFunction)py_quaternion_about_axis,
3993 METH_VARARGS|METH_KEYWORDS, py_quaternion_about_axis_doc},
3994 {"quaternion_multiply",
3995 (PyCFunction)py_quaternion_multiply,
3996 METH_VARARGS|METH_KEYWORDS, py_quaternion_multiply_doc},
3997 {"quaternion_matrix",
3998 (PyCFunction)py_quaternion_matrix,
3999 METH_VARARGS|METH_KEYWORDS, py_quaternion_matrix_doc},
4000 {"quaternion_from_matrix",
4001 (PyCFunction)py_quaternion_from_matrix,
4002 METH_VARARGS|METH_KEYWORDS, py_quaternion_from_matrix_doc},
4003 {"quaternion_conjugate",
4004 (PyCFunction)py_quaternion_conjugate,
4005 METH_VARARGS|METH_KEYWORDS, py_quaternion_conjugate_doc},
4006 {"quaternion_inverse",
4007 (PyCFunction)py_quaternion_inverse,
4008 METH_VARARGS|METH_KEYWORDS, py_quaternion_inverse_doc},
4009 {"quaternion_slerp",
4010 (PyCFunction)py_quaternion_slerp,
4011 METH_VARARGS|METH_KEYWORDS, py_quaternion_slerp_doc},
4012 {"random_quaternion",
4013 (PyCFunction)py_random_quaternion,
4014 METH_VARARGS|METH_KEYWORDS, py_random_quaternion_doc},
4015 {"random_rotation_matrix",
4016 (PyCFunction)py_random_rotation_matrix,
4017 METH_VARARGS|METH_KEYWORDS, py_random_rotation_matrix_doc},
4018 {"arcball_map_to_sphere",
4019 (PyCFunction)py_arcball_map_to_sphere,
4020 METH_VARARGS|METH_KEYWORDS, py_arcball_map_to_sphere_doc},
4021 {"arcball_constrain_to_axis",
4022 (PyCFunction)py_arcball_constrain_to_axis,
4023 METH_VARARGS|METH_KEYWORDS, py_arcball_constrain_to_axis_doc},
4024 {"vector_norm",
4025 (PyCFunction)py_vector_norm,
4026 METH_VARARGS|METH_KEYWORDS, py_vector_norm_doc},
4027 {"unit_vector",
4028 (PyCFunction)py_unit_vector,
4029 METH_VARARGS|METH_KEYWORDS, py_unit_vector_doc},
4030 {"random_vector",
4031 (PyCFunction)py_random_vector,
4032 METH_VARARGS|METH_KEYWORDS, py_random_vector_doc},
4033 {"inverse_matrix",
4034 (PyCFunction)py_inverse_matrix,
4035 METH_VARARGS|METH_KEYWORDS, py_inverse_matrix_doc},
4036 {"_tridiagonalize_symmetric_44",
4037 (PyCFunction)py_tridiagonalize_symmetric_44,
4038 METH_VARARGS|METH_KEYWORDS, py_tridiagonalize_symmetric_44_doc},
4039 {"_max_eigenvalue_of_tridiag_44",
4040 (PyCFunction)py_max_eigenvalue_of_tridiag_44,
4041 METH_VARARGS|METH_KEYWORDS, py_max_eigenvalue_of_tridiag_44_doc},
4042 {"_eigenvector_of_symmetric_44",
4043 (PyCFunction)py_eigenvector_of_symmetric_44,
4044 METH_VARARGS|METH_KEYWORDS, py_eigenvector_of_symmetric_44_doc},
4045 {NULL, NULL, 0, NULL} /* Sentinel */
4046 };
4047
4048 #if PY_MAJOR_VERSION >= 3
4049
4050 struct module_state {
4051 PyObject *error;
4052 };
4053
4054 #define GETSTATE(m) ((struct module_state*)PyModule_GetState(m))
4055
4056 static int module_traverse(PyObject *m, visitproc visit, void *arg) {
4057 Py_VISIT(GETSTATE(m)->error);
4058 return 0;
4059 }
4060
4061 static int module_clear(PyObject *m) {
4062 Py_CLEAR(GETSTATE(m)->error);
4063 return 0;
4064 }
4065
4066 static struct PyModuleDef moduledef = {
4067 PyModuleDef_HEAD_INIT,
4068 "_transformations",
4069 NULL,
4070 sizeof(struct module_state),
4071 module_methods,
4072 NULL,
4073 module_traverse,
4074 module_clear,
4075 NULL
4076 };
4077
4078 #define INITERROR return NULL
4079
4080 PyMODINIT_FUNC
4081 PyInit__transformations(void)
4082
4083 #else
4084
4085 #define INITERROR return
4086
4087 PyMODINIT_FUNC
4088 init_transformations(void)
4089 #endif
4090 {
4091 PyObject *module;
4092
4093 char *doc = (char *)PyMem_Malloc(sizeof(module_doc) + sizeof(_VERSION_));
4094 sprintf(doc, module_doc, _VERSION_);
4095
4096 #if PY_MAJOR_VERSION >= 3
4097 moduledef.m_doc = doc;
4098 module = PyModule_Create(&moduledef);
4099 #else
4100 module = Py_InitModule3("_transformations", module_methods, doc);
4101 #endif
4102
4103 PyMem_Free(doc);
4104
4105 if (module == NULL)
4106 INITERROR;
4107
4108 if (_import_array() < 0) {
4109 Py_DECREF(module);
4110 INITERROR;
4111 }
4112
4113 {
4114 #if PY_MAJOR_VERSION < 3
4115 PyObject *s = PyString_FromString(_VERSION_);
4116 #else
4117 PyObject *s = PyUnicode_FromString(_VERSION_);
4118 #endif
4119 PyObject *dict = PyModule_GetDict(module);
4120 PyDict_SetItemString(dict, "__version__", s);
4121 Py_DECREF(s);
4122 }
4123
4124 #if PY_MAJOR_VERSION >= 3
4125 return module;
4126 #endif
4127 }
4128
4129