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