1# -*- coding: utf-8 -*- 2# transformations.py 3 4# Copyright (c) 2006, Christoph Gohlke 5# Copyright (c) 2006-2009, The Regents of the University of California 6# All rights reserved. 7# 8# Redistribution and use in source and binary forms, with or without 9# modification, are permitted provided that the following conditions are met: 10# 11# * Redistributions of source code must retain the above copyright 12# notice, this list of conditions and the following disclaimer. 13# * Redistributions in binary form must reproduce the above copyright 14# notice, this list of conditions and the following disclaimer in the 15# documentation and/or other materials provided with the distribution. 16# * Neither the name of the copyright holders nor the names of any 17# contributors may be used to endorse or promote products derived 18# from this software without specific prior written permission. 19# 20# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 23# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 24# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 25# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 26# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 27# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 28# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 29# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30# POSSIBILITY OF SUCH DAMAGE. 31 32"""Homogeneous Transformation Matrices and Quaternions. 33 34A library for calculating 4x4 matrices for translating, rotating, reflecting, 35scaling, shearing, projecting, orthogonalizing, and superimposing arrays of 363D homogeneous coordinates as well as for converting between rotation matrices, 37Euler angles, and quaternions. Also includes an Arcball control object and 38functions to decompose transformation matrices. 39 40:Authors: 41 `Christoph Gohlke <http://www.lfd.uci.edu/~gohlke/>`__, 42 Laboratory for Fluorescence Dynamics, University of California, Irvine 43 44:Version: 20090418 45 46Requirements 47------------ 48 49* `Python 2.6 <http://www.python.org>`__ 50* `Numpy 1.3 <http://numpy.scipy.org>`__ 51* `transformations.c 20090418 <http://www.lfd.uci.edu/~gohlke/>`__ 52 (optional implementation of some functions in C) 53 54Notes 55----- 56 57Matrices (M) can be inverted using numpy.linalg.inv(M), concatenated using 58numpy.dot(M0, M1), or used to transform homogeneous coordinates (v) using 59numpy.dot(M, v) for shape (4, \*) "point of arrays", respectively 60numpy.dot(v, M.T) for shape (\*, 4) "array of points". 61 62Calculations are carried out with numpy.float64 precision. 63 64This Python implementation is not optimized for speed. 65 66Vector, point, quaternion, and matrix function arguments are expected to be 67"array like", i.e. tuple, list, or numpy arrays. 68 69Return types are numpy arrays unless specified otherwise. 70 71Angles are in radians unless specified otherwise. 72 73Quaternions ix+jy+kz+w are represented as [x, y, z, w]. 74 75Use the transpose of transformation matrices for OpenGL glMultMatrixd(). 76 77A triple of Euler angles can be applied/interpreted in 24 ways, which can 78be specified using a 4 character string or encoded 4-tuple: 79 80 *Axes 4-string*: e.g. 'sxyz' or 'ryxy' 81 82 - first character : rotations are applied to 's'tatic or 'r'otating frame 83 - remaining characters : successive rotation axis 'x', 'y', or 'z' 84 85 *Axes 4-tuple*: e.g. (0, 0, 0, 0) or (1, 1, 1, 1) 86 87 - inner axis: code of axis ('x':0, 'y':1, 'z':2) of rightmost matrix. 88 - parity : even (0) if inner axis 'x' is followed by 'y', 'y' is followed 89 by 'z', or 'z' is followed by 'x'. Otherwise odd (1). 90 - repetition : first and last axis are same (1) or different (0). 91 - frame : rotations are applied to static (0) or rotating (1) frame. 92 93References 94---------- 95 96(1) Matrices and transformations. Ronald Goldman. 97 In "Graphics Gems I", pp 472-475. Morgan Kaufmann, 1990. 98(2) More matrices and transformations: shear and pseudo-perspective. 99 Ronald Goldman. In "Graphics Gems II", pp 320-323. Morgan Kaufmann, 1991. 100(3) Decomposing a matrix into simple transformations. Spencer Thomas. 101 In "Graphics Gems II", pp 320-323. Morgan Kaufmann, 1991. 102(4) Recovering the data from the transformation matrix. Ronald Goldman. 103 In "Graphics Gems II", pp 324-331. Morgan Kaufmann, 1991. 104(5) Euler angle conversion. Ken Shoemake. 105 In "Graphics Gems IV", pp 222-229. Morgan Kaufmann, 1994. 106(6) Arcball rotation control. Ken Shoemake. 107 In "Graphics Gems IV", pp 175-192. Morgan Kaufmann, 1994. 108(7) Representing attitude: Euler angles, unit quaternions, and rotation 109 vectors. James Diebel. 2006. 110(8) A discussion of the solution for the best rotation to relate two sets 111 of vectors. W Kabsch. Acta Cryst. 1978. A34, 827-828. 112(9) Closed-form solution of absolute orientation using unit quaternions. 113 BKP Horn. J Opt Soc Am A. 1987. 4(4), 629-642. 114(10) Quaternions. Ken Shoemake. 115 http://www.sfu.ca/~jwa3/cmpt461/files/quatut.pdf 116(11) From quaternion to matrix and back. JMP van Waveren. 2005. 117 http://www.intel.com/cd/ids/developer/asmo-na/eng/293748.htm 118(12) Uniform random rotations. Ken Shoemake. 119 In "Graphics Gems III", pp 124-132. Morgan Kaufmann, 1992. 120 121 122Examples 123-------- 124 125>>> alpha, beta, gamma = 0.123, -1.234, 2.345 126>>> origin, xaxis, yaxis, zaxis = (0, 0, 0), (1, 0, 0), (0, 1, 0), (0, 0, 1) 127>>> I = identity_matrix() 128>>> Rx = rotation_matrix(alpha, xaxis) 129>>> Ry = rotation_matrix(beta, yaxis) 130>>> Rz = rotation_matrix(gamma, zaxis) 131>>> R = concatenate_matrices(Rx, Ry, Rz) 132>>> euler = euler_from_matrix(R, 'rxyz') 133>>> numpy.allclose([alpha, beta, gamma], euler) 134True 135>>> Re = euler_matrix(alpha, beta, gamma, 'rxyz') 136>>> is_same_transform(R, Re) 137True 138>>> al, be, ga = euler_from_matrix(Re, 'rxyz') 139>>> is_same_transform(Re, euler_matrix(al, be, ga, 'rxyz')) 140True 141>>> qx = quaternion_about_axis(alpha, xaxis) 142>>> qy = quaternion_about_axis(beta, yaxis) 143>>> qz = quaternion_about_axis(gamma, zaxis) 144>>> q = quaternion_multiply(qx, qy) 145>>> q = quaternion_multiply(q, qz) 146>>> Rq = quaternion_matrix(q) 147>>> is_same_transform(R, Rq) 148True 149>>> S = scale_matrix(1.23, origin) 150>>> T = translation_matrix((1, 2, 3)) 151>>> Z = shear_matrix(beta, xaxis, origin, zaxis) 152>>> R = random_rotation_matrix(numpy.random.rand(3)) 153>>> M = concatenate_matrices(T, R, Z, S) 154>>> scale, shear, angles, trans, persp = decompose_matrix(M) 155>>> numpy.allclose(scale, 1.23) 156True 157>>> numpy.allclose(trans, (1, 2, 3)) 158True 159>>> numpy.allclose(shear, (0, math.tan(beta), 0)) 160True 161>>> is_same_transform(R, euler_matrix(axes='sxyz', *angles)) 162True 163>>> M1 = compose_matrix(scale, shear, angles, trans, persp) 164>>> is_same_transform(M, M1) 165True 166 167""" 168 169from __future__ import division 170 171import warnings 172import math 173 174import numpy 175 176# Documentation in HTML format can be generated with Epydoc 177__docformat__ = "restructuredtext en" 178 179 180def identity_matrix(): 181 """Return 4x4 identity/unit matrix. 182 183 >>> I = identity_matrix() 184 >>> numpy.allclose(I, numpy.dot(I, I)) 185 True 186 >>> numpy.sum(I), numpy.trace(I) 187 (4.0, 4.0) 188 >>> numpy.allclose(I, numpy.identity(4, dtype=numpy.float64)) 189 True 190 191 """ 192 return numpy.identity(4, dtype=numpy.float64) 193 194 195def translation_matrix(direction): 196 """Return matrix to translate by direction vector. 197 198 >>> v = numpy.random.random(3) - 0.5 199 >>> numpy.allclose(v, translation_matrix(v)[:3, 3]) 200 True 201 202 """ 203 M = numpy.identity(4) 204 M[:3, 3] = direction[:3] 205 return M 206 207 208def translation_from_matrix(matrix): 209 """Return translation vector from translation matrix. 210 211 >>> v0 = numpy.random.random(3) - 0.5 212 >>> v1 = translation_from_matrix(translation_matrix(v0)) 213 >>> numpy.allclose(v0, v1) 214 True 215 216 """ 217 return numpy.array(matrix, copy=False)[:3, 3].copy() 218 219 220def reflection_matrix(point, normal): 221 """Return matrix to mirror at plane defined by point and normal vector. 222 223 >>> v0 = numpy.random.random(4) - 0.5 224 >>> v0[3] = 1.0 225 >>> v1 = numpy.random.random(3) - 0.5 226 >>> R = reflection_matrix(v0, v1) 227 >>> numpy.allclose(2., numpy.trace(R)) 228 True 229 >>> numpy.allclose(v0, numpy.dot(R, v0)) 230 True 231 >>> v2 = v0.copy() 232 >>> v2[:3] += v1 233 >>> v3 = v0.copy() 234 >>> v2[:3] -= v1 235 >>> numpy.allclose(v2, numpy.dot(R, v3)) 236 True 237 238 """ 239 normal = unit_vector(normal[:3]) 240 M = numpy.identity(4) 241 M[:3, :3] -= 2.0 * numpy.outer(normal, normal) 242 M[:3, 3] = (2.0 * numpy.dot(point[:3], normal)) * normal 243 return M 244 245 246def reflection_from_matrix(matrix): 247 """Return mirror plane point and normal vector from reflection matrix. 248 249 >>> v0 = numpy.random.random(3) - 0.5 250 >>> v1 = numpy.random.random(3) - 0.5 251 >>> M0 = reflection_matrix(v0, v1) 252 >>> point, normal = reflection_from_matrix(M0) 253 >>> M1 = reflection_matrix(point, normal) 254 >>> is_same_transform(M0, M1) 255 True 256 257 """ 258 M = numpy.array(matrix, dtype=numpy.float64, copy=False) 259 # normal: unit eigenvector corresponding to eigenvalue -1 260 l, V = numpy.linalg.eig(M[:3, :3]) 261 i = numpy.where(abs(numpy.real(l) + 1.0) < 1e-8)[0] 262 if not len(i): 263 raise ValueError("no unit eigenvector corresponding to eigenvalue -1") 264 normal = numpy.real(V[:, i[0]]).squeeze() 265 # point: any unit eigenvector corresponding to eigenvalue 1 266 l, V = numpy.linalg.eig(M) 267 i = numpy.where(abs(numpy.real(l) - 1.0) < 1e-8)[0] 268 if not len(i): 269 raise ValueError("no unit eigenvector corresponding to eigenvalue 1") 270 point = numpy.real(V[:, i[-1]]).squeeze() 271 point /= point[3] 272 return point, normal 273 274 275def rotation_matrix(angle, direction, point=None): 276 """Return matrix to rotate about axis defined by point and direction. 277 278 >>> angle = (random.random() - 0.5) * (2*math.pi) 279 >>> direc = numpy.random.random(3) - 0.5 280 >>> point = numpy.random.random(3) - 0.5 281 >>> R0 = rotation_matrix(angle, direc, point) 282 >>> R1 = rotation_matrix(angle-2*math.pi, direc, point) 283 >>> is_same_transform(R0, R1) 284 True 285 >>> R0 = rotation_matrix(angle, direc, point) 286 >>> R1 = rotation_matrix(-angle, -direc, point) 287 >>> is_same_transform(R0, R1) 288 True 289 >>> I = numpy.identity(4, numpy.float64) 290 >>> numpy.allclose(I, rotation_matrix(math.pi*2, direc)) 291 True 292 >>> numpy.allclose(2., numpy.trace(rotation_matrix(math.pi/2, 293 ... direc, point))) 294 True 295 296 """ 297 sina = math.sin(angle) 298 cosa = math.cos(angle) 299 direction = unit_vector(direction[:3]) 300 # rotation matrix around unit vector 301 R = numpy.array(((cosa, 0.0, 0.0), 302 (0.0, cosa, 0.0), 303 (0.0, 0.0, cosa)), dtype=numpy.float64) 304 R += numpy.outer(direction, direction) * (1.0 - cosa) 305 direction *= sina 306 R += numpy.array((( 0.0, -direction[2], direction[1]), 307 ( direction[2], 0.0, -direction[0]), 308 (-direction[1], direction[0], 0.0)), 309 dtype=numpy.float64) 310 M = numpy.identity(4) 311 M[:3, :3] = R 312 if point is not None: 313 # rotation not around origin 314 point = numpy.array(point[:3], dtype=numpy.float64, copy=False) 315 M[:3, 3] = point - numpy.dot(R, point) 316 return M 317 318 319def rotation_from_matrix(matrix): 320 """Return rotation angle and axis from rotation matrix. 321 322 >>> angle = (random.random() - 0.5) * (2*math.pi) 323 >>> direc = numpy.random.random(3) - 0.5 324 >>> point = numpy.random.random(3) - 0.5 325 >>> R0 = rotation_matrix(angle, direc, point) 326 >>> angle, direc, point = rotation_from_matrix(R0) 327 >>> R1 = rotation_matrix(angle, direc, point) 328 >>> is_same_transform(R0, R1) 329 True 330 331 """ 332 R = numpy.array(matrix, dtype=numpy.float64, copy=False) 333 R33 = R[:3, :3] 334 # direction: unit eigenvector of R33 corresponding to eigenvalue of 1 335 l, W = numpy.linalg.eig(R33.T) 336 i = numpy.where(abs(numpy.real(l) - 1.0) < 1e-8)[0] 337 if not len(i): 338 raise ValueError("no unit eigenvector corresponding to eigenvalue 1") 339 direction = numpy.real(W[:, i[-1]]).squeeze() 340 # point: unit eigenvector of R33 corresponding to eigenvalue of 1 341 l, Q = numpy.linalg.eig(R) 342 i = numpy.where(abs(numpy.real(l) - 1.0) < 1e-8)[0] 343 if not len(i): 344 raise ValueError("no unit eigenvector corresponding to eigenvalue 1") 345 point = numpy.real(Q[:, i[-1]]).squeeze() 346 point /= point[3] 347 # rotation angle depending on direction 348 cosa = (numpy.trace(R33) - 1.0) / 2.0 349 if abs(direction[2]) > 1e-8: 350 sina = (R[1, 0] + (cosa-1.0)*direction[0]*direction[1]) / direction[2] 351 elif abs(direction[1]) > 1e-8: 352 sina = (R[0, 2] + (cosa-1.0)*direction[0]*direction[2]) / direction[1] 353 else: 354 sina = (R[2, 1] + (cosa-1.0)*direction[1]*direction[2]) / direction[0] 355 angle = math.atan2(sina, cosa) 356 return angle, direction, point 357 358 359def scale_matrix(factor, origin=None, direction=None): 360 """Return matrix to scale by factor around origin in direction. 361 362 Use factor -1 for point symmetry. 363 364 >>> v = (numpy.random.rand(4, 5) - 0.5) * 20.0 365 >>> v[3] = 1.0 366 >>> S = scale_matrix(-1.234) 367 >>> numpy.allclose(numpy.dot(S, v)[:3], -1.234*v[:3]) 368 True 369 >>> factor = random.random() * 10 - 5 370 >>> origin = numpy.random.random(3) - 0.5 371 >>> direct = numpy.random.random(3) - 0.5 372 >>> S = scale_matrix(factor, origin) 373 >>> S = scale_matrix(factor, origin, direct) 374 375 """ 376 if direction is None: 377 # uniform scaling 378 M = numpy.array(((factor, 0.0, 0.0, 0.0), 379 (0.0, factor, 0.0, 0.0), 380 (0.0, 0.0, factor, 0.0), 381 (0.0, 0.0, 0.0, 1.0)), dtype=numpy.float64) 382 if origin is not None: 383 M[:3, 3] = origin[:3] 384 M[:3, 3] *= 1.0 - factor 385 else: 386 # nonuniform scaling 387 direction = unit_vector(direction[:3]) 388 factor = 1.0 - factor 389 M = numpy.identity(4) 390 M[:3, :3] -= factor * numpy.outer(direction, direction) 391 if origin is not None: 392 M[:3, 3] = (factor * numpy.dot(origin[:3], direction)) * direction 393 return M 394 395 396def scale_from_matrix(matrix): 397 """Return scaling factor, origin and direction from scaling matrix. 398 399 >>> factor = random.random() * 10 - 5 400 >>> origin = numpy.random.random(3) - 0.5 401 >>> direct = numpy.random.random(3) - 0.5 402 >>> S0 = scale_matrix(factor, origin) 403 >>> factor, origin, direction = scale_from_matrix(S0) 404 >>> S1 = scale_matrix(factor, origin, direction) 405 >>> is_same_transform(S0, S1) 406 True 407 >>> S0 = scale_matrix(factor, origin, direct) 408 >>> factor, origin, direction = scale_from_matrix(S0) 409 >>> S1 = scale_matrix(factor, origin, direction) 410 >>> is_same_transform(S0, S1) 411 True 412 413 """ 414 M = numpy.array(matrix, dtype=numpy.float64, copy=False) 415 M33 = M[:3, :3] 416 factor = numpy.trace(M33) - 2.0 417 try: 418 # direction: unit eigenvector corresponding to eigenvalue factor 419 l, V = numpy.linalg.eig(M33) 420 i = numpy.where(abs(numpy.real(l) - factor) < 1e-8)[0][0] 421 direction = numpy.real(V[:, i]).squeeze() 422 direction /= vector_norm(direction) 423 except IndexError: 424 # uniform scaling 425 factor = (factor + 2.0) / 3.0 426 direction = None 427 # origin: any eigenvector corresponding to eigenvalue 1 428 l, V = numpy.linalg.eig(M) 429 i = numpy.where(abs(numpy.real(l) - 1.0) < 1e-8)[0] 430 if not len(i): 431 raise ValueError("no eigenvector corresponding to eigenvalue 1") 432 origin = numpy.real(V[:, i[-1]]).squeeze() 433 origin /= origin[3] 434 return factor, origin, direction 435 436 437def projection_matrix(point, normal, direction=None, 438 perspective=None, pseudo=False): 439 """Return matrix to project onto plane defined by point and normal. 440 441 Using either perspective point, projection direction, or none of both. 442 443 If pseudo is True, perspective projections will preserve relative depth 444 such that Perspective = dot(Orthogonal, PseudoPerspective). 445 446 >>> P = projection_matrix((0, 0, 0), (1, 0, 0)) 447 >>> numpy.allclose(P[1:, 1:], numpy.identity(4)[1:, 1:]) 448 True 449 >>> point = numpy.random.random(3) - 0.5 450 >>> normal = numpy.random.random(3) - 0.5 451 >>> direct = numpy.random.random(3) - 0.5 452 >>> persp = numpy.random.random(3) - 0.5 453 >>> P0 = projection_matrix(point, normal) 454 >>> P1 = projection_matrix(point, normal, direction=direct) 455 >>> P2 = projection_matrix(point, normal, perspective=persp) 456 >>> P3 = projection_matrix(point, normal, perspective=persp, pseudo=True) 457 >>> is_same_transform(P2, numpy.dot(P0, P3)) 458 True 459 >>> P = projection_matrix((3, 0, 0), (1, 1, 0), (1, 0, 0)) 460 >>> v0 = (numpy.random.rand(4, 5) - 0.5) * 20.0 461 >>> v0[3] = 1.0 462 >>> v1 = numpy.dot(P, v0) 463 >>> numpy.allclose(v1[1], v0[1]) 464 True 465 >>> numpy.allclose(v1[0], 3.0-v1[1]) 466 True 467 468 """ 469 M = numpy.identity(4) 470 point = numpy.array(point[:3], dtype=numpy.float64, copy=False) 471 normal = unit_vector(normal[:3]) 472 if perspective is not None: 473 # perspective projection 474 perspective = numpy.array(perspective[:3], dtype=numpy.float64, 475 copy=False) 476 M[0, 0] = M[1, 1] = M[2, 2] = numpy.dot(perspective-point, normal) 477 M[:3, :3] -= numpy.outer(perspective, normal) 478 if pseudo: 479 # preserve relative depth 480 M[:3, :3] -= numpy.outer(normal, normal) 481 M[:3, 3] = numpy.dot(point, normal) * (perspective+normal) 482 else: 483 M[:3, 3] = numpy.dot(point, normal) * perspective 484 M[3, :3] = -normal 485 M[3, 3] = numpy.dot(perspective, normal) 486 elif direction is not None: 487 # parallel projection 488 direction = numpy.array(direction[:3], dtype=numpy.float64, copy=False) 489 scale = numpy.dot(direction, normal) 490 M[:3, :3] -= numpy.outer(direction, normal) / scale 491 M[:3, 3] = direction * (numpy.dot(point, normal) / scale) 492 else: 493 # orthogonal projection 494 M[:3, :3] -= numpy.outer(normal, normal) 495 M[:3, 3] = numpy.dot(point, normal) * normal 496 return M 497 498 499def projection_from_matrix(matrix, pseudo=False): 500 """Return projection plane and perspective point from projection matrix. 501 502 Return values are same as arguments for projection_matrix function: 503 point, normal, direction, perspective, and pseudo. 504 505 >>> point = numpy.random.random(3) - 0.5 506 >>> normal = numpy.random.random(3) - 0.5 507 >>> direct = numpy.random.random(3) - 0.5 508 >>> persp = numpy.random.random(3) - 0.5 509 >>> P0 = projection_matrix(point, normal) 510 >>> result = projection_from_matrix(P0) 511 >>> P1 = projection_matrix(*result) 512 >>> is_same_transform(P0, P1) 513 True 514 >>> P0 = projection_matrix(point, normal, direct) 515 >>> result = projection_from_matrix(P0) 516 >>> P1 = projection_matrix(*result) 517 >>> is_same_transform(P0, P1) 518 True 519 >>> P0 = projection_matrix(point, normal, perspective=persp, pseudo=False) 520 >>> result = projection_from_matrix(P0, pseudo=False) 521 >>> P1 = projection_matrix(*result) 522 >>> is_same_transform(P0, P1) 523 True 524 >>> P0 = projection_matrix(point, normal, perspective=persp, pseudo=True) 525 >>> result = projection_from_matrix(P0, pseudo=True) 526 >>> P1 = projection_matrix(*result) 527 >>> is_same_transform(P0, P1) 528 True 529 530 """ 531 M = numpy.array(matrix, dtype=numpy.float64, copy=False) 532 M33 = M[:3, :3] 533 l, V = numpy.linalg.eig(M) 534 i = numpy.where(abs(numpy.real(l) - 1.0) < 1e-8)[0] 535 if not pseudo and len(i): 536 # point: any eigenvector corresponding to eigenvalue 1 537 point = numpy.real(V[:, i[-1]]).squeeze() 538 point /= point[3] 539 # direction: unit eigenvector corresponding to eigenvalue 0 540 l, V = numpy.linalg.eig(M33) 541 i = numpy.where(abs(numpy.real(l)) < 1e-8)[0] 542 if not len(i): 543 raise ValueError("no eigenvector corresponding to eigenvalue 0") 544 direction = numpy.real(V[:, i[0]]).squeeze() 545 direction /= vector_norm(direction) 546 # normal: unit eigenvector of M33.T corresponding to eigenvalue 0 547 l, V = numpy.linalg.eig(M33.T) 548 i = numpy.where(abs(numpy.real(l)) < 1e-8)[0] 549 if len(i): 550 # parallel projection 551 normal = numpy.real(V[:, i[0]]).squeeze() 552 normal /= vector_norm(normal) 553 return point, normal, direction, None, False 554 else: 555 # orthogonal projection, where normal equals direction vector 556 return point, direction, None, None, False 557 else: 558 # perspective projection 559 i = numpy.where(abs(numpy.real(l)) > 1e-8)[0] 560 if not len(i): 561 raise ValueError( 562 "no eigenvector not corresponding to eigenvalue 0") 563 point = numpy.real(V[:, i[-1]]).squeeze() 564 point /= point[3] 565 normal = - M[3, :3] 566 perspective = M[:3, 3] / numpy.dot(point[:3], normal) 567 if pseudo: 568 perspective -= normal 569 return point, normal, None, perspective, pseudo 570 571 572def clip_matrix(left, right, bottom, top, near, far, perspective=False): 573 """Return matrix to obtain normalized device coordinates from frustrum. 574 575 The frustrum bounds are axis-aligned along x (left, right), 576 y (bottom, top) and z (near, far). 577 578 Normalized device coordinates are in range [-1, 1] if coordinates are 579 inside the frustrum. 580 581 If perspective is True the frustrum is a truncated pyramid with the 582 perspective point at origin and direction along z axis, otherwise an 583 orthographic canonical view volume (a box). 584 585 Homogeneous coordinates transformed by the perspective clip matrix 586 need to be dehomogenized (divided by w coordinate). 587 588 >>> frustrum = numpy.random.rand(6) 589 >>> frustrum[1] += frustrum[0] 590 >>> frustrum[3] += frustrum[2] 591 >>> frustrum[5] += frustrum[4] 592 >>> M = clip_matrix(*frustrum, perspective=False) 593 >>> numpy.dot(M, [frustrum[0], frustrum[2], frustrum[4], 1.0]) 594 array([-1., -1., -1., 1.]) 595 >>> numpy.dot(M, [frustrum[1], frustrum[3], frustrum[5], 1.0]) 596 array([ 1., 1., 1., 1.]) 597 >>> M = clip_matrix(*frustrum, perspective=True) 598 >>> v = numpy.dot(M, [frustrum[0], frustrum[2], frustrum[4], 1.0]) 599 >>> v / v[3] 600 array([-1., -1., -1., 1.]) 601 >>> v = numpy.dot(M, [frustrum[1], frustrum[3], frustrum[4], 1.0]) 602 >>> v / v[3] 603 array([ 1., 1., -1., 1.]) 604 605 """ 606 if left >= right or bottom >= top or near >= far: 607 raise ValueError("invalid frustrum") 608 if perspective: 609 if near <= _EPS: 610 raise ValueError("invalid frustrum: near <= 0") 611 t = 2.0 * near 612 M = ((-t/(right-left), 0.0, (right+left)/(right-left), 0.0), 613 (0.0, -t/(top-bottom), (top+bottom)/(top-bottom), 0.0), 614 (0.0, 0.0, -(far+near)/(far-near), t*far/(far-near)), 615 (0.0, 0.0, -1.0, 0.0)) 616 else: 617 M = ((2.0/(right-left), 0.0, 0.0, (right+left)/(left-right)), 618 (0.0, 2.0/(top-bottom), 0.0, (top+bottom)/(bottom-top)), 619 (0.0, 0.0, 2.0/(far-near), (far+near)/(near-far)), 620 (0.0, 0.0, 0.0, 1.0)) 621 return numpy.array(M, dtype=numpy.float64) 622 623 624def shear_matrix(angle, direction, point, normal): 625 """Return matrix to shear by angle along direction vector on shear plane. 626 627 The shear plane is defined by a point and normal vector. The direction 628 vector must be orthogonal to the plane's normal vector. 629 630 A point P is transformed by the shear matrix into P" such that 631 the vector P-P" is parallel to the direction vector and its extent is 632 given by the angle of P-P'-P", where P' is the orthogonal projection 633 of P onto the shear plane. 634 635 >>> angle = (random.random() - 0.5) * 4*math.pi 636 >>> direct = numpy.random.random(3) - 0.5 637 >>> point = numpy.random.random(3) - 0.5 638 >>> normal = numpy.cross(direct, numpy.random.random(3)) 639 >>> S = shear_matrix(angle, direct, point, normal) 640 >>> numpy.allclose(1.0, numpy.linalg.det(S)) 641 True 642 643 """ 644 normal = unit_vector(normal[:3]) 645 direction = unit_vector(direction[:3]) 646 if abs(numpy.dot(normal, direction)) > 1e-6: 647 raise ValueError("direction and normal vectors are not orthogonal") 648 angle = math.tan(angle) 649 M = numpy.identity(4) 650 M[:3, :3] += angle * numpy.outer(direction, normal) 651 M[:3, 3] = -angle * numpy.dot(point[:3], normal) * direction 652 return M 653 654 655def shear_from_matrix(matrix): 656 """Return shear angle, direction and plane from shear matrix. 657 658 >>> angle = (random.random() - 0.5) * 4*math.pi 659 >>> direct = numpy.random.random(3) - 0.5 660 >>> point = numpy.random.random(3) - 0.5 661 >>> normal = numpy.cross(direct, numpy.random.random(3)) 662 >>> S0 = shear_matrix(angle, direct, point, normal) 663 >>> angle, direct, point, normal = shear_from_matrix(S0) 664 >>> S1 = shear_matrix(angle, direct, point, normal) 665 >>> is_same_transform(S0, S1) 666 True 667 668 """ 669 M = numpy.array(matrix, dtype=numpy.float64, copy=False) 670 M33 = M[:3, :3] 671 # normal: cross independent eigenvectors corresponding to the eigenvalue 1 672 l, V = numpy.linalg.eig(M33) 673 i = numpy.where(abs(numpy.real(l) - 1.0) < 1e-4)[0] 674 if len(i) < 2: 675 raise ValueError("No two linear independent eigenvectors found %s" % l) 676 V = numpy.real(V[:, i]).squeeze().T 677 lenorm = -1.0 678 for i0, i1 in ((0, 1), (0, 2), (1, 2)): 679 n = numpy.cross(V[i0], V[i1]) 680 l = vector_norm(n) 681 if l > lenorm: 682 lenorm = l 683 normal = n 684 normal /= lenorm 685 # direction and angle 686 direction = numpy.dot(M33 - numpy.identity(3), normal) 687 angle = vector_norm(direction) 688 direction /= angle 689 angle = math.atan(angle) 690 # point: eigenvector corresponding to eigenvalue 1 691 l, V = numpy.linalg.eig(M) 692 i = numpy.where(abs(numpy.real(l) - 1.0) < 1e-8)[0] 693 if not len(i): 694 raise ValueError("no eigenvector corresponding to eigenvalue 1") 695 point = numpy.real(V[:, i[-1]]).squeeze() 696 point /= point[3] 697 return angle, direction, point, normal 698 699 700def decompose_matrix(matrix): 701 """Return sequence of transformations from transformation matrix. 702 703 matrix : array_like 704 Non-degenerative homogeneous transformation matrix 705 706 Return tuple of: 707 scale : vector of 3 scaling factors 708 shear : list of shear factors for x-y, x-z, y-z axes 709 angles : list of Euler angles about static x, y, z axes 710 translate : translation vector along x, y, z axes 711 perspective : perspective partition of matrix 712 713 Raise ValueError if matrix is of wrong type or degenerative. 714 715 >>> T0 = translation_matrix((1, 2, 3)) 716 >>> scale, shear, angles, trans, persp = decompose_matrix(T0) 717 >>> T1 = translation_matrix(trans) 718 >>> numpy.allclose(T0, T1) 719 True 720 >>> S = scale_matrix(0.123) 721 >>> scale, shear, angles, trans, persp = decompose_matrix(S) 722 >>> scale[0] 723 0.123 724 >>> R0 = euler_matrix(1, 2, 3) 725 >>> scale, shear, angles, trans, persp = decompose_matrix(R0) 726 >>> R1 = euler_matrix(*angles) 727 >>> numpy.allclose(R0, R1) 728 True 729 730 """ 731 M = numpy.array(matrix, dtype=numpy.float64, copy=True).T 732 if abs(M[3, 3]) < _EPS: 733 raise ValueError("M[3, 3] is zero") 734 M /= M[3, 3] 735 P = M.copy() 736 P[:, 3] = 0, 0, 0, 1 737 if not numpy.linalg.det(P): 738 raise ValueError("Matrix is singular") 739 740 scale = numpy.zeros((3, ), dtype=numpy.float64) 741 shear = [0, 0, 0] 742 angles = [0, 0, 0] 743 744 if any(abs(M[:3, 3]) > _EPS): 745 perspective = numpy.dot(M[:, 3], numpy.linalg.inv(P.T)) 746 M[:, 3] = 0, 0, 0, 1 747 else: 748 perspective = numpy.array((0, 0, 0, 1), dtype=numpy.float64) 749 750 translate = M[3, :3].copy() 751 M[3, :3] = 0 752 753 row = M[:3, :3].copy() 754 scale[0] = vector_norm(row[0]) 755 row[0] /= scale[0] 756 shear[0] = numpy.dot(row[0], row[1]) 757 row[1] -= row[0] * shear[0] 758 scale[1] = vector_norm(row[1]) 759 row[1] /= scale[1] 760 shear[0] /= scale[1] 761 shear[1] = numpy.dot(row[0], row[2]) 762 row[2] -= row[0] * shear[1] 763 shear[2] = numpy.dot(row[1], row[2]) 764 row[2] -= row[1] * shear[2] 765 scale[2] = vector_norm(row[2]) 766 row[2] /= scale[2] 767 shear[1:] /= scale[2] 768 769 if numpy.dot(row[0], numpy.cross(row[1], row[2])) < 0: 770 scale *= -1 771 row *= -1 772 773 angles[1] = math.asin(-row[0, 2]) 774 if math.cos(angles[1]): 775 angles[0] = math.atan2(row[1, 2], row[2, 2]) 776 angles[2] = math.atan2(row[0, 1], row[0, 0]) 777 else: 778 #angles[0] = math.atan2(row[1, 0], row[1, 1]) 779 angles[0] = math.atan2(-row[2, 1], row[1, 1]) 780 angles[2] = 0.0 781 782 return scale, shear, angles, translate, perspective 783 784 785def compose_matrix(scale=None, shear=None, angles=None, translate=None, 786 perspective=None): 787 """Return transformation matrix from sequence of transformations. 788 789 This is the inverse of the decompose_matrix function. 790 791 Sequence of transformations: 792 scale : vector of 3 scaling factors 793 shear : list of shear factors for x-y, x-z, y-z axes 794 angles : list of Euler angles about static x, y, z axes 795 translate : translation vector along x, y, z axes 796 perspective : perspective partition of matrix 797 798 >>> scale = numpy.random.random(3) - 0.5 799 >>> shear = numpy.random.random(3) - 0.5 800 >>> angles = (numpy.random.random(3) - 0.5) * (2*math.pi) 801 >>> trans = numpy.random.random(3) - 0.5 802 >>> persp = numpy.random.random(4) - 0.5 803 >>> M0 = compose_matrix(scale, shear, angles, trans, persp) 804 >>> result = decompose_matrix(M0) 805 >>> M1 = compose_matrix(*result) 806 >>> is_same_transform(M0, M1) 807 True 808 809 """ 810 M = numpy.identity(4) 811 if perspective is not None: 812 P = numpy.identity(4) 813 P[3, :] = perspective[:4] 814 M = numpy.dot(M, P) 815 if translate is not None: 816 T = numpy.identity(4) 817 T[:3, 3] = translate[:3] 818 M = numpy.dot(M, T) 819 if angles is not None: 820 R = euler_matrix(angles[0], angles[1], angles[2], 'sxyz') 821 M = numpy.dot(M, R) 822 if shear is not None: 823 Z = numpy.identity(4) 824 Z[1, 2] = shear[2] 825 Z[0, 2] = shear[1] 826 Z[0, 1] = shear[0] 827 M = numpy.dot(M, Z) 828 if scale is not None: 829 S = numpy.identity(4) 830 S[0, 0] = scale[0] 831 S[1, 1] = scale[1] 832 S[2, 2] = scale[2] 833 M = numpy.dot(M, S) 834 M /= M[3, 3] 835 return M 836 837 838def orthogonalization_matrix(lengths, angles): 839 """Return orthogonalization matrix for crystallographic cell coordinates. 840 841 Angles are expected in degrees. 842 843 The de-orthogonalization matrix is the inverse. 844 845 >>> O = orthogonalization_matrix((10., 10., 10.), (90., 90., 90.)) 846 >>> numpy.allclose(O[:3, :3], numpy.identity(3, float) * 10) 847 True 848 >>> O = orthogonalization_matrix([9.8, 12.0, 15.5], [87.2, 80.7, 69.7]) 849 >>> numpy.allclose(numpy.sum(O), 43.063229) 850 True 851 852 """ 853 a, b, c = lengths 854 angles = numpy.radians(angles) 855 sina, sinb, _ = numpy.sin(angles) 856 cosa, cosb, cosg = numpy.cos(angles) 857 co = (cosa * cosb - cosg) / (sina * sinb) 858 return numpy.array(( 859 ( a*sinb*math.sqrt(1.0-co*co), 0.0, 0.0, 0.0), 860 (-a*sinb*co, b*sina, 0.0, 0.0), 861 ( a*cosb, b*cosa, c, 0.0), 862 ( 0.0, 0.0, 0.0, 1.0)), 863 dtype=numpy.float64) 864 865 866def superimposition_matrix(v0, v1, scaling=False, usesvd=True): 867 """Return matrix to transform given vector set into second vector set. 868 869 v0 and v1 are shape (3, \*) or (4, \*) arrays of at least 3 vectors. 870 871 If usesvd is True, the weighted sum of squared deviations (RMSD) is 872 minimized according to the algorithm by W. Kabsch [8]. Otherwise the 873 quaternion based algorithm by B. Horn [9] is used (slower when using 874 this Python implementation). 875 876 The returned matrix performs rotation, translation and uniform scaling 877 (if specified). 878 879 >>> v0 = numpy.random.rand(3, 10) 880 >>> M = superimposition_matrix(v0, v0) 881 >>> numpy.allclose(M, numpy.identity(4)) 882 True 883 >>> R = random_rotation_matrix(numpy.random.random(3)) 884 >>> v0 = ((1,0,0), (0,1,0), (0,0,1), (1,1,1)) 885 >>> v1 = numpy.dot(R, v0) 886 >>> M = superimposition_matrix(v0, v1) 887 >>> numpy.allclose(v1, numpy.dot(M, v0)) 888 True 889 >>> v0 = (numpy.random.rand(4, 100) - 0.5) * 20.0 890 >>> v0[3] = 1.0 891 >>> v1 = numpy.dot(R, v0) 892 >>> M = superimposition_matrix(v0, v1) 893 >>> numpy.allclose(v1, numpy.dot(M, v0)) 894 True 895 >>> S = scale_matrix(random.random()) 896 >>> T = translation_matrix(numpy.random.random(3)-0.5) 897 >>> M = concatenate_matrices(T, R, S) 898 >>> v1 = numpy.dot(M, v0) 899 >>> v0[:3] += numpy.random.normal(0.0, 1e-9, 300).reshape(3, -1) 900 >>> M = superimposition_matrix(v0, v1, scaling=True) 901 >>> numpy.allclose(v1, numpy.dot(M, v0)) 902 True 903 >>> M = superimposition_matrix(v0, v1, scaling=True, usesvd=False) 904 >>> numpy.allclose(v1, numpy.dot(M, v0)) 905 True 906 >>> v = numpy.empty((4, 100, 3), dtype=numpy.float64) 907 >>> v[:, :, 0] = v0 908 >>> M = superimposition_matrix(v0, v1, scaling=True, usesvd=False) 909 >>> numpy.allclose(v1, numpy.dot(M, v[:, :, 0])) 910 True 911 912 """ 913 v0 = numpy.array(v0, dtype=numpy.float64, copy=False)[:3] 914 v1 = numpy.array(v1, dtype=numpy.float64, copy=False)[:3] 915 916 if v0.shape != v1.shape or v0.shape[1] < 3: 917 raise ValueError("Vector sets are of wrong shape or type.") 918 919 # move centroids to origin 920 t0 = numpy.mean(v0, axis=1) 921 t1 = numpy.mean(v1, axis=1) 922 v0 = v0 - t0.reshape(3, 1) 923 v1 = v1 - t1.reshape(3, 1) 924 925 if usesvd: 926 # Singular Value Decomposition of covariance matrix 927 u, s, vh = numpy.linalg.svd(numpy.dot(v1, v0.T)) 928 # rotation matrix from SVD orthonormal bases 929 R = numpy.dot(u, vh) 930 if numpy.linalg.det(R) < 0.0: 931 # R does not constitute right handed system 932 R -= numpy.outer(u[:, 2], vh[2, :]*2.0) 933 s[-1] *= -1.0 934 # homogeneous transformation matrix 935 M = numpy.identity(4) 936 M[:3, :3] = R 937 else: 938 # compute symmetric matrix N 939 xx, yy, zz = numpy.sum(v0 * v1, axis=1) 940 xy, yz, zx = numpy.sum(v0 * numpy.roll(v1, -1, axis=0), axis=1) 941 xz, yx, zy = numpy.sum(v0 * numpy.roll(v1, -2, axis=0), axis=1) 942 N = ((xx+yy+zz, yz-zy, zx-xz, xy-yx), 943 (yz-zy, xx-yy-zz, xy+yx, zx+xz), 944 (zx-xz, xy+yx, -xx+yy-zz, yz+zy), 945 (xy-yx, zx+xz, yz+zy, -xx-yy+zz)) 946 # quaternion: eigenvector corresponding to most positive eigenvalue 947 l, V = numpy.linalg.eig(N) 948 q = V[:, numpy.argmax(l)] 949 q /= vector_norm(q) # unit quaternion 950 q = numpy.roll(q, -1) # move w component to end 951 # homogeneous transformation matrix 952 M = quaternion_matrix(q) 953 954 # scale: ratio of rms deviations from centroid 955 if scaling: 956 v0 *= v0 957 v1 *= v1 958 M[:3, :3] *= math.sqrt(numpy.sum(v1) / numpy.sum(v0)) 959 960 # translation 961 M[:3, 3] = t1 962 T = numpy.identity(4) 963 T[:3, 3] = -t0 964 M = numpy.dot(M, T) 965 return M 966 967 968def euler_matrix(ai, aj, ak, axes='sxyz'): 969 """Return homogeneous rotation matrix from Euler angles and axis sequence. 970 971 ai, aj, ak : Euler's roll, pitch and yaw angles 972 axes : One of 24 axis sequences as string or encoded tuple 973 974 >>> R = euler_matrix(1, 2, 3, 'syxz') 975 >>> numpy.allclose(numpy.sum(R[0]), -1.34786452) 976 True 977 >>> R = euler_matrix(1, 2, 3, (0, 1, 0, 1)) 978 >>> numpy.allclose(numpy.sum(R[0]), -0.383436184) 979 True 980 >>> ai, aj, ak = (4.0*math.pi) * (numpy.random.random(3) - 0.5) 981 >>> for axes in _AXES2TUPLE.keys(): 982 ... R = euler_matrix(ai, aj, ak, axes) 983 >>> for axes in _TUPLE2AXES.keys(): 984 ... R = euler_matrix(ai, aj, ak, axes) 985 986 """ 987 try: 988 firstaxis, parity, repetition, frame = _AXES2TUPLE[axes] 989 except (AttributeError, KeyError): 990 _ = _TUPLE2AXES[axes] 991 firstaxis, parity, repetition, frame = axes 992 993 i = firstaxis 994 j = _NEXT_AXIS[i+parity] 995 k = _NEXT_AXIS[i-parity+1] 996 997 if frame: 998 ai, ak = ak, ai 999 if parity: 1000 ai, aj, ak = -ai, -aj, -ak 1001 1002 si, sj, sk = math.sin(ai), math.sin(aj), math.sin(ak) 1003 ci, cj, ck = math.cos(ai), math.cos(aj), math.cos(ak) 1004 cc, cs = ci*ck, ci*sk 1005 sc, ss = si*ck, si*sk 1006 1007 M = numpy.identity(4) 1008 if repetition: 1009 M[i, i] = cj 1010 M[i, j] = sj*si 1011 M[i, k] = sj*ci 1012 M[j, i] = sj*sk 1013 M[j, j] = -cj*ss+cc 1014 M[j, k] = -cj*cs-sc 1015 M[k, i] = -sj*ck 1016 M[k, j] = cj*sc+cs 1017 M[k, k] = cj*cc-ss 1018 else: 1019 M[i, i] = cj*ck 1020 M[i, j] = sj*sc-cs 1021 M[i, k] = sj*cc+ss 1022 M[j, i] = cj*sk 1023 M[j, j] = sj*ss+cc 1024 M[j, k] = sj*cs-sc 1025 M[k, i] = -sj 1026 M[k, j] = cj*si 1027 M[k, k] = cj*ci 1028 return M 1029 1030 1031def euler_from_matrix(matrix, axes='sxyz'): 1032 """Return Euler angles from rotation matrix for specified axis sequence. 1033 1034 axes : One of 24 axis sequences as string or encoded tuple 1035 1036 Note that many Euler angle triplets can describe one matrix. 1037 1038 >>> R0 = euler_matrix(1, 2, 3, 'syxz') 1039 >>> al, be, ga = euler_from_matrix(R0, 'syxz') 1040 >>> R1 = euler_matrix(al, be, ga, 'syxz') 1041 >>> numpy.allclose(R0, R1) 1042 True 1043 >>> angles = (4.0*math.pi) * (numpy.random.random(3) - 0.5) 1044 >>> for axes in _AXES2TUPLE.keys(): 1045 ... R0 = euler_matrix(axes=axes, *angles) 1046 ... R1 = euler_matrix(axes=axes, *euler_from_matrix(R0, axes)) 1047 ... if not numpy.allclose(R0, R1): print axes, "failed" 1048 1049 """ 1050 try: 1051 firstaxis, parity, repetition, frame = _AXES2TUPLE[axes.lower()] 1052 except (AttributeError, KeyError): 1053 _ = _TUPLE2AXES[axes] 1054 firstaxis, parity, repetition, frame = axes 1055 1056 i = firstaxis 1057 j = _NEXT_AXIS[i+parity] 1058 k = _NEXT_AXIS[i-parity+1] 1059 1060 M = numpy.array(matrix, dtype=numpy.float64, copy=False)[:3, :3] 1061 if repetition: 1062 sy = math.sqrt(M[i, j]*M[i, j] + M[i, k]*M[i, k]) 1063 if sy > _EPS: 1064 ax = math.atan2( M[i, j], M[i, k]) 1065 ay = math.atan2( sy, M[i, i]) 1066 az = math.atan2( M[j, i], -M[k, i]) 1067 else: 1068 ax = math.atan2(-M[j, k], M[j, j]) 1069 ay = math.atan2( sy, M[i, i]) 1070 az = 0.0 1071 else: 1072 cy = math.sqrt(M[i, i]*M[i, i] + M[j, i]*M[j, i]) 1073 if cy > _EPS: 1074 ax = math.atan2( M[k, j], M[k, k]) 1075 ay = math.atan2(-M[k, i], cy) 1076 az = math.atan2( M[j, i], M[i, i]) 1077 else: 1078 ax = math.atan2(-M[j, k], M[j, j]) 1079 ay = math.atan2(-M[k, i], cy) 1080 az = 0.0 1081 1082 if parity: 1083 ax, ay, az = -ax, -ay, -az 1084 if frame: 1085 ax, az = az, ax 1086 return ax, ay, az 1087 1088 1089def euler_from_quaternion(quaternion, axes='sxyz'): 1090 """Return Euler angles from quaternion for specified axis sequence. 1091 1092 >>> angles = euler_from_quaternion([0.06146124, 0, 0, 0.99810947]) 1093 >>> numpy.allclose(angles, [0.123, 0, 0]) 1094 True 1095 1096 """ 1097 return euler_from_matrix(quaternion_matrix(quaternion), axes) 1098 1099 1100def quaternion_from_euler(ai, aj, ak, axes='sxyz'): 1101 """Return quaternion from Euler angles and axis sequence. 1102 1103 ai, aj, ak : Euler's roll, pitch and yaw angles 1104 axes : One of 24 axis sequences as string or encoded tuple 1105 1106 >>> q = quaternion_from_euler(1, 2, 3, 'ryxz') 1107 >>> numpy.allclose(q, [0.310622, -0.718287, 0.444435, 0.435953]) 1108 True 1109 1110 """ 1111 try: 1112 firstaxis, parity, repetition, frame = _AXES2TUPLE[axes.lower()] 1113 except (AttributeError, KeyError): 1114 _ = _TUPLE2AXES[axes] 1115 firstaxis, parity, repetition, frame = axes 1116 1117 i = firstaxis 1118 j = _NEXT_AXIS[i+parity] 1119 k = _NEXT_AXIS[i-parity+1] 1120 1121 if frame: 1122 ai, ak = ak, ai 1123 if parity: 1124 aj = -aj 1125 1126 ai /= 2.0 1127 aj /= 2.0 1128 ak /= 2.0 1129 ci = math.cos(ai) 1130 si = math.sin(ai) 1131 cj = math.cos(aj) 1132 sj = math.sin(aj) 1133 ck = math.cos(ak) 1134 sk = math.sin(ak) 1135 cc = ci*ck 1136 cs = ci*sk 1137 sc = si*ck 1138 ss = si*sk 1139 1140 quaternion = numpy.empty((4, ), dtype=numpy.float64) 1141 if repetition: 1142 quaternion[i] = cj*(cs + sc) 1143 quaternion[j] = sj*(cc + ss) 1144 quaternion[k] = sj*(cs - sc) 1145 quaternion[3] = cj*(cc - ss) 1146 else: 1147 quaternion[i] = cj*sc - sj*cs 1148 quaternion[j] = cj*ss + sj*cc 1149 quaternion[k] = cj*cs - sj*sc 1150 quaternion[3] = cj*cc + sj*ss 1151 if parity: 1152 quaternion[j] *= -1 1153 1154 return quaternion 1155 1156 1157def quaternion_about_axis(angle, axis): 1158 """Return quaternion for rotation about axis. 1159 1160 >>> q = quaternion_about_axis(0.123, (1, 0, 0)) 1161 >>> numpy.allclose(q, [0.06146124, 0, 0, 0.99810947]) 1162 True 1163 1164 """ 1165 quaternion = numpy.zeros((4, ), dtype=numpy.float64) 1166 quaternion[:3] = axis[:3] 1167 qlen = vector_norm(quaternion) 1168 if qlen > _EPS: 1169 quaternion *= math.sin(angle/2.0) / qlen 1170 quaternion[3] = math.cos(angle/2.0) 1171 return quaternion 1172 1173 1174def quaternion_matrix(quaternion): 1175 """Return homogeneous rotation matrix from quaternion. 1176 1177 >>> R = quaternion_matrix([0.06146124, 0, 0, 0.99810947]) 1178 >>> numpy.allclose(R, rotation_matrix(0.123, (1, 0, 0))) 1179 True 1180 1181 """ 1182 q = numpy.array(quaternion[:4], dtype=numpy.float64, copy=True) 1183 nq = numpy.dot(q, q) 1184 if nq < _EPS: 1185 return numpy.identity(4) 1186 q *= math.sqrt(2.0 / nq) 1187 q = numpy.outer(q, q) 1188 return numpy.array(( 1189 (1.0-q[1, 1]-q[2, 2], q[0, 1]-q[2, 3], q[0, 2]+q[1, 3], 0.0), 1190 ( q[0, 1]+q[2, 3], 1.0-q[0, 0]-q[2, 2], q[1, 2]-q[0, 3], 0.0), 1191 ( q[0, 2]-q[1, 3], q[1, 2]+q[0, 3], 1.0-q[0, 0]-q[1, 1], 0.0), 1192 ( 0.0, 0.0, 0.0, 1.0) 1193 ), dtype=numpy.float64) 1194 1195 1196def quaternion_from_matrix(matrix): 1197 """Return quaternion from rotation matrix. 1198 1199 >>> R = rotation_matrix(0.123, (1, 2, 3)) 1200 >>> q = quaternion_from_matrix(R) 1201 >>> numpy.allclose(q, [0.0164262, 0.0328524, 0.0492786, 0.9981095]) 1202 True 1203 1204 """ 1205 q = numpy.empty((4, ), dtype=numpy.float64) 1206 M = numpy.array(matrix, dtype=numpy.float64, copy=False)[:4, :4] 1207 t = numpy.trace(M) 1208 if t > M[3, 3]: 1209 q[3] = t 1210 q[2] = M[1, 0] - M[0, 1] 1211 q[1] = M[0, 2] - M[2, 0] 1212 q[0] = M[2, 1] - M[1, 2] 1213 else: 1214 i, j, k = 0, 1, 2 1215 if M[1, 1] > M[0, 0]: 1216 i, j, k = 1, 2, 0 1217 if M[2, 2] > M[i, i]: 1218 i, j, k = 2, 0, 1 1219 t = M[i, i] - (M[j, j] + M[k, k]) + M[3, 3] 1220 q[i] = t 1221 q[j] = M[i, j] + M[j, i] 1222 q[k] = M[k, i] + M[i, k] 1223 q[3] = M[k, j] - M[j, k] 1224 q *= 0.5 / math.sqrt(t * M[3, 3]) 1225 return q 1226 1227 1228def quaternion_multiply(quaternion1, quaternion0): 1229 """Return multiplication of two quaternions. 1230 1231 >>> q = quaternion_multiply([1, -2, 3, 4], [-5, 6, 7, 8]) 1232 >>> numpy.allclose(q, [-44, -14, 48, 28]) 1233 True 1234 1235 """ 1236 x0, y0, z0, w0 = quaternion0 1237 x1, y1, z1, w1 = quaternion1 1238 return numpy.array(( 1239 x1*w0 + y1*z0 - z1*y0 + w1*x0, 1240 -x1*z0 + y1*w0 + z1*x0 + w1*y0, 1241 x1*y0 - y1*x0 + z1*w0 + w1*z0, 1242 -x1*x0 - y1*y0 - z1*z0 + w1*w0), dtype=numpy.float64) 1243 1244 1245def quaternion_conjugate(quaternion): 1246 """Return conjugate of quaternion. 1247 1248 >>> q0 = random_quaternion() 1249 >>> q1 = quaternion_conjugate(q0) 1250 >>> q1[3] == q0[3] and all(q1[:3] == -q0[:3]) 1251 True 1252 1253 """ 1254 return numpy.array((-quaternion[0], -quaternion[1], 1255 -quaternion[2], quaternion[3]), dtype=numpy.float64) 1256 1257 1258def quaternion_inverse(quaternion): 1259 """Return inverse of quaternion. 1260 1261 >>> q0 = random_quaternion() 1262 >>> q1 = quaternion_inverse(q0) 1263 >>> numpy.allclose(quaternion_multiply(q0, q1), [0, 0, 0, 1]) 1264 True 1265 1266 """ 1267 return quaternion_conjugate(quaternion) / numpy.dot(quaternion, quaternion) 1268 1269 1270def quaternion_slerp(quat0, quat1, fraction, spin=0, shortestpath=True): 1271 """Return spherical linear interpolation between two quaternions. 1272 1273 >>> q0 = random_quaternion() 1274 >>> q1 = random_quaternion() 1275 >>> q = quaternion_slerp(q0, q1, 0.0) 1276 >>> numpy.allclose(q, q0) 1277 True 1278 >>> q = quaternion_slerp(q0, q1, 1.0, 1) 1279 >>> numpy.allclose(q, q1) 1280 True 1281 >>> q = quaternion_slerp(q0, q1, 0.5) 1282 >>> angle = math.acos(numpy.dot(q0, q)) 1283 >>> numpy.allclose(2.0, math.acos(numpy.dot(q0, q1)) / angle) or \ 1284 numpy.allclose(2.0, math.acos(-numpy.dot(q0, q1)) / angle) 1285 True 1286 1287 """ 1288 q0 = unit_vector(quat0[:4]) 1289 q1 = unit_vector(quat1[:4]) 1290 if fraction == 0.0: 1291 return q0 1292 elif fraction == 1.0: 1293 return q1 1294 d = numpy.dot(q0, q1) 1295 if abs(abs(d) - 1.0) < _EPS: 1296 return q0 1297 if shortestpath and d < 0.0: 1298 # invert rotation 1299 d = -d 1300 q1 *= -1.0 1301 angle = math.acos(d) + spin * math.pi 1302 if abs(angle) < _EPS: 1303 return q0 1304 isin = 1.0 / math.sin(angle) 1305 q0 *= math.sin((1.0 - fraction) * angle) * isin 1306 q1 *= math.sin(fraction * angle) * isin 1307 q0 += q1 1308 return q0 1309 1310 1311def random_quaternion(rand=None): 1312 """Return uniform random unit quaternion. 1313 1314 rand: array like or None 1315 Three independent random variables that are uniformly distributed 1316 between 0 and 1. 1317 1318 >>> q = random_quaternion() 1319 >>> numpy.allclose(1.0, vector_norm(q)) 1320 True 1321 >>> q = random_quaternion(numpy.random.random(3)) 1322 >>> q.shape 1323 (4,) 1324 1325 """ 1326 if rand is None: 1327 rand = numpy.random.rand(3) 1328 else: 1329 assert len(rand) == 3 1330 r1 = numpy.sqrt(1.0 - rand[0]) 1331 r2 = numpy.sqrt(rand[0]) 1332 pi2 = math.pi * 2.0 1333 t1 = pi2 * rand[1] 1334 t2 = pi2 * rand[2] 1335 return numpy.array((numpy.sin(t1)*r1, 1336 numpy.cos(t1)*r1, 1337 numpy.sin(t2)*r2, 1338 numpy.cos(t2)*r2), dtype=numpy.float64) 1339 1340 1341def random_rotation_matrix(rand=None): 1342 """Return uniform random rotation matrix. 1343 1344 rnd: array like 1345 Three independent random variables that are uniformly distributed 1346 between 0 and 1 for each returned quaternion. 1347 1348 >>> R = random_rotation_matrix() 1349 >>> numpy.allclose(numpy.dot(R.T, R), numpy.identity(4)) 1350 True 1351 1352 """ 1353 return quaternion_matrix(random_quaternion(rand)) 1354 1355 1356class Arcball(object): 1357 """Virtual Trackball Control. 1358 1359 >>> ball = Arcball() 1360 >>> ball = Arcball(initial=numpy.identity(4)) 1361 >>> ball.place([320, 320], 320) 1362 >>> ball.down([500, 250]) 1363 >>> ball.drag([475, 275]) 1364 >>> R = ball.matrix() 1365 >>> numpy.allclose(numpy.sum(R), 3.90583455) 1366 True 1367 >>> ball = Arcball(initial=[0, 0, 0, 1]) 1368 >>> ball.place([320, 320], 320) 1369 >>> ball.setaxes([1,1,0], [-1, 1, 0]) 1370 >>> ball.setconstrain(True) 1371 >>> ball.down([400, 200]) 1372 >>> ball.drag([200, 400]) 1373 >>> R = ball.matrix() 1374 >>> numpy.allclose(numpy.sum(R), 0.2055924) 1375 True 1376 >>> ball.next() 1377 1378 """ 1379 1380 def __init__(self, initial=None): 1381 """Initialize virtual trackball control. 1382 1383 initial : quaternion or rotation matrix 1384 1385 """ 1386 self._axis = None 1387 self._axes = None 1388 self._radius = 1.0 1389 self._center = [0.0, 0.0] 1390 self._vdown = numpy.array([0, 0, 1], dtype=numpy.float64) 1391 self._constrain = False 1392 1393 if initial is None: 1394 self._qdown = numpy.array([0, 0, 0, 1], dtype=numpy.float64) 1395 else: 1396 initial = numpy.array(initial, dtype=numpy.float64) 1397 if initial.shape == (4, 4): 1398 self._qdown = quaternion_from_matrix(initial) 1399 elif initial.shape == (4, ): 1400 initial /= vector_norm(initial) 1401 self._qdown = initial 1402 else: 1403 raise ValueError("initial not a quaternion or matrix.") 1404 1405 self._qnow = self._qpre = self._qdown 1406 1407 def place(self, center, radius): 1408 """Place Arcball, e.g. when window size changes. 1409 1410 center : sequence[2] 1411 Window coordinates of trackball center. 1412 radius : float 1413 Radius of trackball in window coordinates. 1414 1415 """ 1416 self._radius = float(radius) 1417 self._center[0] = center[0] 1418 self._center[1] = center[1] 1419 1420 def setaxes(self, *axes): 1421 """Set axes to constrain rotations.""" 1422 if axes is None: 1423 self._axes = None 1424 else: 1425 self._axes = [unit_vector(axis) for axis in axes] 1426 1427 def setconstrain(self, constrain): 1428 """Set state of constrain to axis mode.""" 1429 self._constrain = constrain == True 1430 1431 def getconstrain(self): 1432 """Return state of constrain to axis mode.""" 1433 return self._constrain 1434 1435 def down(self, point): 1436 """Set initial cursor window coordinates and pick constrain-axis.""" 1437 self._vdown = arcball_map_to_sphere(point, self._center, self._radius) 1438 self._qdown = self._qpre = self._qnow 1439 1440 if self._constrain and self._axes is not None: 1441 self._axis = arcball_nearest_axis(self._vdown, self._axes) 1442 self._vdown = arcball_constrain_to_axis(self._vdown, self._axis) 1443 else: 1444 self._axis = None 1445 1446 def drag(self, point): 1447 """Update current cursor window coordinates.""" 1448 vnow = arcball_map_to_sphere(point, self._center, self._radius) 1449 1450 if self._axis is not None: 1451 vnow = arcball_constrain_to_axis(vnow, self._axis) 1452 1453 self._qpre = self._qnow 1454 1455 t = numpy.cross(self._vdown, vnow) 1456 if numpy.dot(t, t) < _EPS: 1457 self._qnow = self._qdown 1458 else: 1459 q = [t[0], t[1], t[2], numpy.dot(self._vdown, vnow)] 1460 self._qnow = quaternion_multiply(q, self._qdown) 1461 1462 def next(self, acceleration=0.0): 1463 """Continue rotation in direction of last drag.""" 1464 q = quaternion_slerp(self._qpre, self._qnow, 2.0+acceleration, False) 1465 self._qpre, self._qnow = self._qnow, q 1466 1467 def matrix(self): 1468 """Return homogeneous rotation matrix.""" 1469 return quaternion_matrix(self._qnow) 1470 1471 1472def arcball_map_to_sphere(point, center, radius): 1473 """Return unit sphere coordinates from window coordinates.""" 1474 v = numpy.array(((point[0] - center[0]) / radius, 1475 (center[1] - point[1]) / radius, 1476 0.0), dtype=numpy.float64) 1477 n = v[0]*v[0] + v[1]*v[1] 1478 if n > 1.0: 1479 v /= math.sqrt(n) # position outside of sphere 1480 else: 1481 v[2] = math.sqrt(1.0 - n) 1482 return v 1483 1484 1485def arcball_constrain_to_axis(point, axis): 1486 """Return sphere point perpendicular to axis.""" 1487 v = numpy.array(point, dtype=numpy.float64, copy=True) 1488 a = numpy.array(axis, dtype=numpy.float64, copy=True) 1489 v -= a * numpy.dot(a, v) # on plane 1490 n = vector_norm(v) 1491 if n > _EPS: 1492 if v[2] < 0.0: 1493 v *= -1.0 1494 v /= n 1495 return v 1496 if a[2] == 1.0: 1497 return numpy.array([1, 0, 0], dtype=numpy.float64) 1498 return unit_vector([-a[1], a[0], 0]) 1499 1500 1501def arcball_nearest_axis(point, axes): 1502 """Return axis, which arc is nearest to point.""" 1503 point = numpy.array(point, dtype=numpy.float64, copy=False) 1504 nearest = None 1505 mx = -1.0 1506 for axis in axes: 1507 t = numpy.dot(arcball_constrain_to_axis(point, axis), point) 1508 if t > mx: 1509 nearest = axis 1510 mx = t 1511 return nearest 1512 1513 1514# epsilon for testing whether a number is close to zero 1515_EPS = numpy.finfo(float).eps * 4.0 1516 1517# axis sequences for Euler angles 1518_NEXT_AXIS = [1, 2, 0, 1] 1519 1520# map axes strings to/from tuples of inner axis, parity, repetition, frame 1521_AXES2TUPLE = { 1522 'sxyz': (0, 0, 0, 0), 'sxyx': (0, 0, 1, 0), 'sxzy': (0, 1, 0, 0), 1523 'sxzx': (0, 1, 1, 0), 'syzx': (1, 0, 0, 0), 'syzy': (1, 0, 1, 0), 1524 'syxz': (1, 1, 0, 0), 'syxy': (1, 1, 1, 0), 'szxy': (2, 0, 0, 0), 1525 'szxz': (2, 0, 1, 0), 'szyx': (2, 1, 0, 0), 'szyz': (2, 1, 1, 0), 1526 'rzyx': (0, 0, 0, 1), 'rxyx': (0, 0, 1, 1), 'ryzx': (0, 1, 0, 1), 1527 'rxzx': (0, 1, 1, 1), 'rxzy': (1, 0, 0, 1), 'ryzy': (1, 0, 1, 1), 1528 'rzxy': (1, 1, 0, 1), 'ryxy': (1, 1, 1, 1), 'ryxz': (2, 0, 0, 1), 1529 'rzxz': (2, 0, 1, 1), 'rxyz': (2, 1, 0, 1), 'rzyz': (2, 1, 1, 1)} 1530 1531_TUPLE2AXES = dict((v, k) for k, v in _AXES2TUPLE.items()) 1532 1533# helper functions 1534 1535def vector_norm(data, axis=None, out=None): 1536 """Return length, i.e. eucledian norm, of ndarray along axis. 1537 1538 >>> v = numpy.random.random(3) 1539 >>> n = vector_norm(v) 1540 >>> numpy.allclose(n, numpy.linalg.norm(v)) 1541 True 1542 >>> v = numpy.random.rand(6, 5, 3) 1543 >>> n = vector_norm(v, axis=-1) 1544 >>> numpy.allclose(n, numpy.sqrt(numpy.sum(v*v, axis=2))) 1545 True 1546 >>> n = vector_norm(v, axis=1) 1547 >>> numpy.allclose(n, numpy.sqrt(numpy.sum(v*v, axis=1))) 1548 True 1549 >>> v = numpy.random.rand(5, 4, 3) 1550 >>> n = numpy.empty((5, 3), dtype=numpy.float64) 1551 >>> vector_norm(v, axis=1, out=n) 1552 >>> numpy.allclose(n, numpy.sqrt(numpy.sum(v*v, axis=1))) 1553 True 1554 >>> vector_norm([]) 1555 0.0 1556 >>> vector_norm([1.0]) 1557 1.0 1558 1559 """ 1560 data = numpy.array(data, dtype=numpy.float64, copy=True) 1561 if out is None: 1562 if data.ndim == 1: 1563 return math.sqrt(numpy.dot(data, data)) 1564 data *= data 1565 out = numpy.atleast_1d(numpy.sum(data, axis=axis)) 1566 numpy.sqrt(out, out) 1567 return out 1568 else: 1569 data *= data 1570 numpy.sum(data, axis=axis, out=out) 1571 numpy.sqrt(out, out) 1572 1573 1574def unit_vector(data, axis=None, out=None): 1575 """Return ndarray normalized by length, i.e. eucledian norm, along axis. 1576 1577 >>> v0 = numpy.random.random(3) 1578 >>> v1 = unit_vector(v0) 1579 >>> numpy.allclose(v1, v0 / numpy.linalg.norm(v0)) 1580 True 1581 >>> v0 = numpy.random.rand(5, 4, 3) 1582 >>> v1 = unit_vector(v0, axis=-1) 1583 >>> v2 = v0 / numpy.expand_dims(numpy.sqrt(numpy.sum(v0*v0, axis=2)), 2) 1584 >>> numpy.allclose(v1, v2) 1585 True 1586 >>> v1 = unit_vector(v0, axis=1) 1587 >>> v2 = v0 / numpy.expand_dims(numpy.sqrt(numpy.sum(v0*v0, axis=1)), 1) 1588 >>> numpy.allclose(v1, v2) 1589 True 1590 >>> v1 = numpy.empty((5, 4, 3), dtype=numpy.float64) 1591 >>> unit_vector(v0, axis=1, out=v1) 1592 >>> numpy.allclose(v1, v2) 1593 True 1594 >>> list(unit_vector([])) 1595 [] 1596 >>> list(unit_vector([1.0])) 1597 [1.0] 1598 1599 """ 1600 if out is None: 1601 data = numpy.array(data, dtype=numpy.float64, copy=True) 1602 if data.ndim == 1: 1603 data /= math.sqrt(numpy.dot(data, data)) 1604 return data 1605 else: 1606 if out is not data: 1607 out[:] = numpy.array(data, copy=False) 1608 data = out 1609 length = numpy.atleast_1d(numpy.sum(data*data, axis)) 1610 numpy.sqrt(length, length) 1611 if axis is not None: 1612 length = numpy.expand_dims(length, axis) 1613 data /= length 1614 if out is None: 1615 return data 1616 1617 1618def random_vector(size): 1619 """Return array of random doubles in the half-open interval [0.0, 1.0). 1620 1621 >>> v = random_vector(10000) 1622 >>> numpy.all(v >= 0.0) and numpy.all(v < 1.0) 1623 True 1624 >>> v0 = random_vector(10) 1625 >>> v1 = random_vector(10) 1626 >>> numpy.any(v0 == v1) 1627 False 1628 1629 """ 1630 return numpy.random.random(size) 1631 1632 1633def inverse_matrix(matrix): 1634 """Return inverse of square transformation matrix. 1635 1636 >>> M0 = random_rotation_matrix() 1637 >>> M1 = inverse_matrix(M0.T) 1638 >>> numpy.allclose(M1, numpy.linalg.inv(M0.T)) 1639 True 1640 >>> for size in range(1, 7): 1641 ... M0 = numpy.random.rand(size, size) 1642 ... M1 = inverse_matrix(M0) 1643 ... if not numpy.allclose(M1, numpy.linalg.inv(M0)): print size 1644 1645 """ 1646 return numpy.linalg.inv(matrix) 1647 1648 1649def concatenate_matrices(*matrices): 1650 """Return concatenation of series of transformation matrices. 1651 1652 >>> M = numpy.random.rand(16).reshape((4, 4)) - 0.5 1653 >>> numpy.allclose(M, concatenate_matrices(M)) 1654 True 1655 >>> numpy.allclose(numpy.dot(M, M.T), concatenate_matrices(M, M.T)) 1656 True 1657 1658 """ 1659 M = numpy.identity(4) 1660 for i in matrices: 1661 M = numpy.dot(M, i) 1662 return M 1663 1664 1665def is_same_transform(matrix0, matrix1): 1666 """Return True if two matrices perform same transformation. 1667 1668 >>> is_same_transform(numpy.identity(4), numpy.identity(4)) 1669 True 1670 >>> is_same_transform(numpy.identity(4), random_rotation_matrix()) 1671 False 1672 1673 """ 1674 matrix0 = numpy.array(matrix0, dtype=numpy.float64, copy=True) 1675 matrix0 /= matrix0[3, 3] 1676 matrix1 = numpy.array(matrix1, dtype=numpy.float64, copy=True) 1677 matrix1 /= matrix1[3, 3] 1678 return numpy.allclose(matrix0, matrix1) 1679 1680 1681def _import_module(module_name, warn=True, prefix='_py_', ignore='_'): 1682 """Try import all public attributes from module into global namespace. 1683 1684 Existing attributes with name clashes are renamed with prefix. 1685 Attributes starting with underscore are ignored by default. 1686 1687 Return True on successful import. 1688 1689 """ 1690 try: 1691 module = __import__(module_name) 1692 except ImportError: 1693 if warn: 1694 warnings.warn("Failed to import module " + module_name) 1695 else: 1696 for attr in dir(module): 1697 if ignore and attr.startswith(ignore): 1698 continue 1699 if prefix: 1700 if attr in globals(): 1701 globals()[prefix + attr] = globals()[attr] 1702 elif warn: 1703 warnings.warn("No Python implementation of " + attr) 1704 globals()[attr] = getattr(module, attr) 1705 return True 1706