Coverage for larch/math/transformations.py: 10%

701 statements  

« prev     ^ index     » next       coverage.py v7.6.0, created at 2024-10-16 21:04 +0000

1# -*- coding: utf-8 -*- 

2# transformations.py 

3 

4# Copyright (c) 2006-2015, Christoph Gohlke 

5# Copyright (c) 2006-2015, The Regents of the University of California 

6# Produced at the Laboratory for Fluorescence Dynamics 

7# All rights reserved. 

8# 

9# Redistribution and use in source and binary forms, with or without 

10# modification, are permitted provided that the following conditions are met: 

11# 

12# * Redistributions of source code must retain the above copyright 

13# notice, this list of conditions and the following disclaimer. 

14# * Redistributions in binary form must reproduce the above copyright 

15# notice, this list of conditions and the following disclaimer in the 

16# documentation and/or other materials provided with the distribution. 

17# * Neither the name of the copyright holders nor the names of any 

18# contributors may be used to endorse or promote products derived 

19# from this software without specific prior written permission. 

20# 

21# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 

22# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 

23# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 

24# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 

25# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 

26# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 

27# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 

28# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 

29# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 

30# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 

31# POSSIBILITY OF SUCH DAMAGE. 

32 

33"""Homogeneous Transformation Matrices and Quaternions. 

34 

35A library for calculating 4x4 matrices for translating, rotating, reflecting, 

36scaling, shearing, projecting, orthogonalizing, and superimposing arrays of 

373D homogeneous coordinates as well as for converting between rotation matrices, 

38Euler angles, and quaternions. Also includes an Arcball control object and 

39functions to decompose transformation matrices. 

40 

41:Author: 

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

43 

44:Organization: 

45 Laboratory for Fluorescence Dynamics, University of California, Irvine 

46 

47:Version: 2015.07.18 

48 

49Requirements 

50------------ 

51* `CPython 2.7 or 3.4 <http://www.python.org>`_ 

52* `Numpy 1.9 <http://www.numpy.org>`_ 

53* `Transformations.c 2015.07.18 <http://www.lfd.uci.edu/~gohlke/>`_ 

54 (recommended for speedup of some functions) 

55 

56Notes 

57----- 

58The API is not stable yet and is expected to change between revisions. 

59 

60This Python code is not optimized for speed. Refer to the transformations.c 

61module for a faster implementation of some functions. 

62 

63Documentation in HTML format can be generated with epydoc. 

64 

65Matrices (M) can be inverted using numpy.linalg.inv(M), be concatenated using 

66numpy.dot(M0, M1), or transform homogeneous coordinate arrays (v) using 

67numpy.dot(M, v) for shape (4, *) column vectors, respectively 

68numpy.dot(v, M.T) for shape (*, 4) row vectors ("array of points"). 

69 

70This module follows the "column vectors on the right" and "row major storage" 

71(C contiguous) conventions. The translation components are in the right column 

72of the transformation matrix, i.e. M[:3, 3]. 

73The transpose of the transformation matrices may have to be used to interface 

74with other graphics systems, e.g. with OpenGL's glMultMatrixd(). See also [16]. 

75 

76Calculations are carried out with numpy.float64 precision. 

77 

78Vector, point, quaternion, and matrix function arguments are expected to be 

79"array like", i.e. tuple, list, or numpy arrays. 

80 

81Return types are numpy arrays unless specified otherwise. 

82 

83Angles are in radians unless specified otherwise. 

84 

85Quaternions w+ix+jy+kz are represented as [w, x, y, z]. 

86 

87A triple of Euler angles can be applied/interpreted in 24 ways, which can 

88be specified using a 4 character string or encoded 4-tuple: 

89 

90 *Axes 4-string*: e.g. 'sxyz' or 'ryxy' 

91 

92 - first character : rotations are applied to 's'tatic or 'r'otating frame 

93 - remaining characters : successive rotation axis 'x', 'y', or 'z' 

94 

95 *Axes 4-tuple*: e.g. (0, 0, 0, 0) or (1, 1, 1, 1) 

96 

97 - inner axis: code of axis ('x':0, 'y':1, 'z':2) of rightmost matrix. 

98 - parity : even (0) if inner axis 'x' is followed by 'y', 'y' is followed 

99 by 'z', or 'z' is followed by 'x'. Otherwise odd (1). 

100 - repetition : first and last axis are same (1) or different (0). 

101 - frame : rotations are applied to static (0) or rotating (1) frame. 

102 

103Other Python packages and modules for 3D transformations and quaternions: 

104 

105* `Transforms3d <https://pypi.python.org/pypi/transforms3d>`_ 

106 includes most code of this module. 

107* `Blender.mathutils <http://www.blender.org/api/blender_python_api>`_ 

108* `numpy-dtypes <https://github.com/numpy/numpy-dtypes>`_ 

109 

110References 

111---------- 

112(1) Matrices and transformations. Ronald Goldman. 

113 In "Graphics Gems I", pp 472-475. Morgan Kaufmann, 1990. 

114(2) More matrices and transformations: shear and pseudo-perspective. 

115 Ronald Goldman. In "Graphics Gems II", pp 320-323. Morgan Kaufmann, 1991. 

116(3) Decomposing a matrix into simple transformations. Spencer Thomas. 

117 In "Graphics Gems II", pp 320-323. Morgan Kaufmann, 1991. 

118(4) Recovering the data from the transformation matrix. Ronald Goldman. 

119 In "Graphics Gems II", pp 324-331. Morgan Kaufmann, 1991. 

120(5) Euler angle conversion. Ken Shoemake. 

121 In "Graphics Gems IV", pp 222-229. Morgan Kaufmann, 1994. 

122(6) Arcball rotation control. Ken Shoemake. 

123 In "Graphics Gems IV", pp 175-192. Morgan Kaufmann, 1994. 

124(7) Representing attitude: Euler angles, unit quaternions, and rotation 

125 vectors. James Diebel. 2006. 

126(8) A discussion of the solution for the best rotation to relate two sets 

127 of vectors. W Kabsch. Acta Cryst. 1978. A34, 827-828. 

128(9) Closed-form solution of absolute orientation using unit quaternions. 

129 BKP Horn. J Opt Soc Am A. 1987. 4(4):629-642. 

130(10) Quaternions. Ken Shoemake. 

131 http://www.sfu.ca/~jwa3/cmpt461/files/quatut.pdf 

132(11) From quaternion to matrix and back. JMP van Waveren. 2005. 

133 http://www.intel.com/cd/ids/developer/asmo-na/eng/293748.htm 

134(12) Uniform random rotations. Ken Shoemake. 

135 In "Graphics Gems III", pp 124-132. Morgan Kaufmann, 1992. 

136(13) Quaternion in molecular modeling. CFF Karney. 

137 J Mol Graph Mod, 25(5):595-604 

138(14) New method for extracting the quaternion from a rotation matrix. 

139 Itzhack Y Bar-Itzhack, J Guid Contr Dynam. 2000. 23(6): 1085-1087. 

140(15) Multiple View Geometry in Computer Vision. Hartley and Zissermann. 

141 Cambridge University Press; 2nd Ed. 2004. Chapter 4, Algorithm 4.7, p 130. 

142(16) Column Vectors vs. Row Vectors. 

143 http://steve.hollasch.net/cgindex/math/matrix/column-vec.html 

144 

145Examples 

146-------- 

147>>> alpha, beta, gamma = 0.123, -1.234, 2.345 

148>>> origin, xaxis, yaxis, zaxis = [0, 0, 0], [1, 0, 0], [0, 1, 0], [0, 0, 1] 

149>>> I = identity_matrix() 

150>>> Rx = rotation_matrix(alpha, xaxis) 

151>>> Ry = rotation_matrix(beta, yaxis) 

152>>> Rz = rotation_matrix(gamma, zaxis) 

153>>> R = concatenate_matrices(Rx, Ry, Rz) 

154>>> euler = euler_from_matrix(R, 'rxyz') 

155>>> numpy.allclose([alpha, beta, gamma], euler) 

156True 

157>>> Re = euler_matrix(alpha, beta, gamma, 'rxyz') 

158>>> is_same_transform(R, Re) 

159True 

160>>> al, be, ga = euler_from_matrix(Re, 'rxyz') 

161>>> is_same_transform(Re, euler_matrix(al, be, ga, 'rxyz')) 

162True 

163>>> qx = quaternion_about_axis(alpha, xaxis) 

164>>> qy = quaternion_about_axis(beta, yaxis) 

165>>> qz = quaternion_about_axis(gamma, zaxis) 

166>>> q = quaternion_multiply(qx, qy) 

167>>> q = quaternion_multiply(q, qz) 

168>>> Rq = quaternion_matrix(q) 

169>>> is_same_transform(R, Rq) 

170True 

171>>> S = scale_matrix(1.23, origin) 

172>>> T = translation_matrix([1, 2, 3]) 

173>>> Z = shear_matrix(beta, xaxis, origin, zaxis) 

174>>> R = random_rotation_matrix(numpy.random.rand(3)) 

175>>> M = concatenate_matrices(T, R, Z, S) 

176>>> scale, shear, angles, trans, persp = decompose_matrix(M) 

177>>> numpy.allclose(scale, 1.23) 

178True 

179>>> numpy.allclose(trans, [1, 2, 3]) 

180True 

181>>> numpy.allclose(shear, [0, math.tan(beta), 0]) 

182True 

183>>> is_same_transform(R, euler_matrix(axes='sxyz', *angles)) 

184True 

185>>> M1 = compose_matrix(scale, shear, angles, trans, persp) 

186>>> is_same_transform(M, M1) 

187True 

188>>> v0, v1 = random_vector(3), random_vector(3) 

189>>> M = rotation_matrix(angle_between_vectors(v0, v1), vector_product(v0, v1)) 

190>>> v2 = numpy.dot(v0, M[:3,:3].T) 

191>>> numpy.allclose(unit_vector(v1), unit_vector(v2)) 

192True 

193 

194""" 

195 

196from __future__ import division, print_function 

197 

198import math 

199 

200import numpy 

201 

202__version__ = '2015.07.18' 

203__docformat__ = 'restructuredtext en' 

204__all__ = () 

205 

206 

207def identity_matrix(): 

208 """Return 4x4 identity/unit matrix. 

209 

210 >>> I = identity_matrix() 

211 >>> numpy.allclose(I, numpy.dot(I, I)) 

212 True 

213 >>> numpy.sum(I), numpy.trace(I) 

214 (4.0, 4.0) 

215 >>> numpy.allclose(I, numpy.identity(4)) 

216 True 

217 

218 """ 

219 return numpy.identity(4) 

220 

221 

222def translation_matrix(direction): 

223 """Return matrix to translate by direction vector. 

224 

225 >>> v = numpy.random.random(3) - 0.5 

226 >>> numpy.allclose(v, translation_matrix(v)[:3, 3]) 

227 True 

228 

229 """ 

230 M = numpy.identity(4) 

231 M[:3, 3] = direction[:3] 

232 return M 

233 

234 

235def translation_from_matrix(matrix): 

236 """Return translation vector from translation matrix. 

237 

238 >>> v0 = numpy.random.random(3) - 0.5 

239 >>> v1 = translation_from_matrix(translation_matrix(v0)) 

240 >>> numpy.allclose(v0, v1) 

241 True 

242 

243 """ 

244 return numpy.array(matrix, copy=False)[:3, 3].copy() 

245 

246 

247def reflection_matrix(point, normal): 

248 """Return matrix to mirror at plane defined by point and normal vector. 

249 

250 >>> v0 = numpy.random.random(4) - 0.5 

251 >>> v0[3] = 1. 

252 >>> v1 = numpy.random.random(3) - 0.5 

253 >>> R = reflection_matrix(v0, v1) 

254 >>> numpy.allclose(2, numpy.trace(R)) 

255 True 

256 >>> numpy.allclose(v0, numpy.dot(R, v0)) 

257 True 

258 >>> v2 = v0.copy() 

259 >>> v2[:3] += v1 

260 >>> v3 = v0.copy() 

261 >>> v2[:3] -= v1 

262 >>> numpy.allclose(v2, numpy.dot(R, v3)) 

263 True 

264 

265 """ 

266 normal = unit_vector(normal[:3]) 

267 M = numpy.identity(4) 

268 M[:3, :3] -= 2.0 * numpy.outer(normal, normal) 

269 M[:3, 3] = (2.0 * numpy.dot(point[:3], normal)) * normal 

270 return M 

271 

272 

273def reflection_from_matrix(matrix): 

274 """Return mirror plane point and normal vector from reflection matrix. 

275 

276 >>> v0 = numpy.random.random(3) - 0.5 

277 >>> v1 = numpy.random.random(3) - 0.5 

278 >>> M0 = reflection_matrix(v0, v1) 

279 >>> point, normal = reflection_from_matrix(M0) 

280 >>> M1 = reflection_matrix(point, normal) 

281 >>> is_same_transform(M0, M1) 

282 True 

283 

284 """ 

285 M = numpy.array(matrix, dtype=numpy.float64, copy=False) 

286 # normal: unit eigenvector corresponding to eigenvalue -1 

287 w, V = numpy.linalg.eig(M[:3, :3]) 

288 i = numpy.where(abs(numpy.real(w) + 1.0) < 1e-8)[0] 

289 if not len(i): 

290 raise ValueError("no unit eigenvector corresponding to eigenvalue -1") 

291 normal = numpy.real(V[:, i[0]]).squeeze() 

292 # point: any unit eigenvector corresponding to eigenvalue 1 

293 w, V = numpy.linalg.eig(M) 

294 i = numpy.where(abs(numpy.real(w) - 1.0) < 1e-8)[0] 

295 if not len(i): 

296 raise ValueError("no unit eigenvector corresponding to eigenvalue 1") 

297 point = numpy.real(V[:, i[-1]]).squeeze() 

298 point /= point[3] 

299 return point, normal 

300 

301 

302def rotation_matrix(angle, direction, point=None): 

303 """Return matrix to rotate about axis defined by point and direction. 

304 

305 >>> R = rotation_matrix(math.pi/2, [0, 0, 1], [1, 0, 0]) 

306 >>> numpy.allclose(numpy.dot(R, [0, 0, 0, 1]), [1, -1, 0, 1]) 

307 True 

308 >>> angle = (random.random() - 0.5) * (2*math.pi) 

309 >>> direc = numpy.random.random(3) - 0.5 

310 >>> point = numpy.random.random(3) - 0.5 

311 >>> R0 = rotation_matrix(angle, direc, point) 

312 >>> R1 = rotation_matrix(angle-2*math.pi, direc, point) 

313 >>> is_same_transform(R0, R1) 

314 True 

315 >>> R0 = rotation_matrix(angle, direc, point) 

316 >>> R1 = rotation_matrix(-angle, -direc, point) 

317 >>> is_same_transform(R0, R1) 

318 True 

319 >>> I = numpy.identity(4, numpy.float64) 

320 >>> numpy.allclose(I, rotation_matrix(math.pi*2, direc)) 

321 True 

322 >>> numpy.allclose(2, numpy.trace(rotation_matrix(math.pi/2, 

323 ... direc, point))) 

324 True 

325 

326 """ 

327 sina = math.sin(angle) 

328 cosa = math.cos(angle) 

329 direction = unit_vector(direction[:3]) 

330 # rotation matrix around unit vector 

331 R = numpy.diag([cosa, cosa, cosa]) 

332 R += numpy.outer(direction, direction) * (1.0 - cosa) 

333 direction *= sina 

334 R += numpy.array([[ 0.0, -direction[2], direction[1]], 

335 [ direction[2], 0.0, -direction[0]], 

336 [-direction[1], direction[0], 0.0]]) 

337 M = numpy.identity(4) 

338 M[:3, :3] = R 

339 if point is not None: 

340 # rotation not around origin 

341 point = numpy.array(point[:3], dtype=numpy.float64, copy=False) 

342 M[:3, 3] = point - numpy.dot(R, point) 

343 return M 

344 

345 

346def rotation_from_matrix(matrix): 

347 """Return rotation angle and axis from rotation matrix. 

348 

349 >>> angle = (random.random() - 0.5) * (2*math.pi) 

350 >>> direc = numpy.random.random(3) - 0.5 

351 >>> point = numpy.random.random(3) - 0.5 

352 >>> R0 = rotation_matrix(angle, direc, point) 

353 >>> angle, direc, point = rotation_from_matrix(R0) 

354 >>> R1 = rotation_matrix(angle, direc, point) 

355 >>> is_same_transform(R0, R1) 

356 True 

357 

358 """ 

359 R = numpy.array(matrix, dtype=numpy.float64, copy=False) 

360 R33 = R[:3, :3] 

361 # direction: unit eigenvector of R33 corresponding to eigenvalue of 1 

362 w, W = numpy.linalg.eig(R33.T) 

363 i = numpy.where(abs(numpy.real(w) - 1.0) < 1e-8)[0] 

364 if not len(i): 

365 raise ValueError("no unit eigenvector corresponding to eigenvalue 1") 

366 direction = numpy.real(W[:, i[-1]]).squeeze() 

367 # point: unit eigenvector of R33 corresponding to eigenvalue of 1 

368 w, Q = numpy.linalg.eig(R) 

369 i = numpy.where(abs(numpy.real(w) - 1.0) < 1e-8)[0] 

370 if not len(i): 

371 raise ValueError("no unit eigenvector corresponding to eigenvalue 1") 

372 point = numpy.real(Q[:, i[-1]]).squeeze() 

373 point /= point[3] 

374 # rotation angle depending on direction 

375 cosa = (numpy.trace(R33) - 1.0) / 2.0 

376 if abs(direction[2]) > 1e-8: 

377 sina = (R[1, 0] + (cosa-1.0)*direction[0]*direction[1]) / direction[2] 

378 elif abs(direction[1]) > 1e-8: 

379 sina = (R[0, 2] + (cosa-1.0)*direction[0]*direction[2]) / direction[1] 

380 else: 

381 sina = (R[2, 1] + (cosa-1.0)*direction[1]*direction[2]) / direction[0] 

382 angle = math.atan2(sina, cosa) 

383 return angle, direction, point 

384 

385 

386def scale_matrix(factor, origin=None, direction=None): 

387 """Return matrix to scale by factor around origin in direction. 

388 

389 Use factor -1 for point symmetry. 

390 

391 >>> v = (numpy.random.rand(4, 5) - 0.5) * 20 

392 >>> v[3] = 1 

393 >>> S = scale_matrix(-1.234) 

394 >>> numpy.allclose(numpy.dot(S, v)[:3], -1.234*v[:3]) 

395 True 

396 >>> factor = random.random() * 10 - 5 

397 >>> origin = numpy.random.random(3) - 0.5 

398 >>> direct = numpy.random.random(3) - 0.5 

399 >>> S = scale_matrix(factor, origin) 

400 >>> S = scale_matrix(factor, origin, direct) 

401 

402 """ 

403 if direction is None: 

404 # uniform scaling 

405 M = numpy.diag([factor, factor, factor, 1.0]) 

406 if origin is not None: 

407 M[:3, 3] = origin[:3] 

408 M[:3, 3] *= 1.0 - factor 

409 else: 

410 # nonuniform scaling 

411 direction = unit_vector(direction[:3]) 

412 factor = 1.0 - factor 

413 M = numpy.identity(4) 

414 M[:3, :3] -= factor * numpy.outer(direction, direction) 

415 if origin is not None: 

416 M[:3, 3] = (factor * numpy.dot(origin[:3], direction)) * direction 

417 return M 

418 

419 

420def scale_from_matrix(matrix): 

421 """Return scaling factor, origin and direction from scaling matrix. 

422 

423 >>> factor = random.random() * 10 - 5 

424 >>> origin = numpy.random.random(3) - 0.5 

425 >>> direct = numpy.random.random(3) - 0.5 

426 >>> S0 = scale_matrix(factor, origin) 

427 >>> factor, origin, direction = scale_from_matrix(S0) 

428 >>> S1 = scale_matrix(factor, origin, direction) 

429 >>> is_same_transform(S0, S1) 

430 True 

431 >>> S0 = scale_matrix(factor, origin, direct) 

432 >>> factor, origin, direction = scale_from_matrix(S0) 

433 >>> S1 = scale_matrix(factor, origin, direction) 

434 >>> is_same_transform(S0, S1) 

435 True 

436 

437 """ 

438 M = numpy.array(matrix, dtype=numpy.float64, copy=False) 

439 M33 = M[:3, :3] 

440 factor = numpy.trace(M33) - 2.0 

441 try: 

442 # direction: unit eigenvector corresponding to eigenvalue factor 

443 w, V = numpy.linalg.eig(M33) 

444 i = numpy.where(abs(numpy.real(w) - factor) < 1e-8)[0][0] 

445 direction = numpy.real(V[:, i]).squeeze() 

446 direction /= vector_norm(direction) 

447 except IndexError: 

448 # uniform scaling 

449 factor = (factor + 2.0) / 3.0 

450 direction = None 

451 # origin: any eigenvector corresponding to eigenvalue 1 

452 w, V = numpy.linalg.eig(M) 

453 i = numpy.where(abs(numpy.real(w) - 1.0) < 1e-8)[0] 

454 if not len(i): 

455 raise ValueError("no eigenvector corresponding to eigenvalue 1") 

456 origin = numpy.real(V[:, i[-1]]).squeeze() 

457 origin /= origin[3] 

458 return factor, origin, direction 

459 

460 

461def projection_matrix(point, normal, direction=None, 

462 perspective=None, pseudo=False): 

463 """Return matrix to project onto plane defined by point and normal. 

464 

465 Using either perspective point, projection direction, or none of both. 

466 

467 If pseudo is True, perspective projections will preserve relative depth 

468 such that Perspective = dot(Orthogonal, PseudoPerspective). 

469 

470 >>> P = projection_matrix([0, 0, 0], [1, 0, 0]) 

471 >>> numpy.allclose(P[1:, 1:], numpy.identity(4)[1:, 1:]) 

472 True 

473 >>> point = numpy.random.random(3) - 0.5 

474 >>> normal = numpy.random.random(3) - 0.5 

475 >>> direct = numpy.random.random(3) - 0.5 

476 >>> persp = numpy.random.random(3) - 0.5 

477 >>> P0 = projection_matrix(point, normal) 

478 >>> P1 = projection_matrix(point, normal, direction=direct) 

479 >>> P2 = projection_matrix(point, normal, perspective=persp) 

480 >>> P3 = projection_matrix(point, normal, perspective=persp, pseudo=True) 

481 >>> is_same_transform(P2, numpy.dot(P0, P3)) 

482 True 

483 >>> P = projection_matrix([3, 0, 0], [1, 1, 0], [1, 0, 0]) 

484 >>> v0 = (numpy.random.rand(4, 5) - 0.5) * 20 

485 >>> v0[3] = 1 

486 >>> v1 = numpy.dot(P, v0) 

487 >>> numpy.allclose(v1[1], v0[1]) 

488 True 

489 >>> numpy.allclose(v1[0], 3-v1[1]) 

490 True 

491 

492 """ 

493 M = numpy.identity(4) 

494 point = numpy.array(point[:3], dtype=numpy.float64, copy=False) 

495 normal = unit_vector(normal[:3]) 

496 if perspective is not None: 

497 # perspective projection 

498 perspective = numpy.array(perspective[:3], dtype=numpy.float64, 

499 copy=False) 

500 M[0, 0] = M[1, 1] = M[2, 2] = numpy.dot(perspective-point, normal) 

501 M[:3, :3] -= numpy.outer(perspective, normal) 

502 if pseudo: 

503 # preserve relative depth 

504 M[:3, :3] -= numpy.outer(normal, normal) 

505 M[:3, 3] = numpy.dot(point, normal) * (perspective+normal) 

506 else: 

507 M[:3, 3] = numpy.dot(point, normal) * perspective 

508 M[3, :3] = -normal 

509 M[3, 3] = numpy.dot(perspective, normal) 

510 elif direction is not None: 

511 # parallel projection 

512 direction = numpy.array(direction[:3], dtype=numpy.float64, copy=False) 

513 scale = numpy.dot(direction, normal) 

514 M[:3, :3] -= numpy.outer(direction, normal) / scale 

515 M[:3, 3] = direction * (numpy.dot(point, normal) / scale) 

516 else: 

517 # orthogonal projection 

518 M[:3, :3] -= numpy.outer(normal, normal) 

519 M[:3, 3] = numpy.dot(point, normal) * normal 

520 return M 

521 

522 

523def projection_from_matrix(matrix, pseudo=False): 

524 """Return projection plane and perspective point from projection matrix. 

525 

526 Return values are same as arguments for projection_matrix function: 

527 point, normal, direction, perspective, and pseudo. 

528 

529 >>> point = numpy.random.random(3) - 0.5 

530 >>> normal = numpy.random.random(3) - 0.5 

531 >>> direct = numpy.random.random(3) - 0.5 

532 >>> persp = numpy.random.random(3) - 0.5 

533 >>> P0 = projection_matrix(point, normal) 

534 >>> result = projection_from_matrix(P0) 

535 >>> P1 = projection_matrix(*result) 

536 >>> is_same_transform(P0, P1) 

537 True 

538 >>> P0 = projection_matrix(point, normal, direct) 

539 >>> result = projection_from_matrix(P0) 

540 >>> P1 = projection_matrix(*result) 

541 >>> is_same_transform(P0, P1) 

542 True 

543 >>> P0 = projection_matrix(point, normal, perspective=persp, pseudo=False) 

544 >>> result = projection_from_matrix(P0, pseudo=False) 

545 >>> P1 = projection_matrix(*result) 

546 >>> is_same_transform(P0, P1) 

547 True 

548 >>> P0 = projection_matrix(point, normal, perspective=persp, pseudo=True) 

549 >>> result = projection_from_matrix(P0, pseudo=True) 

550 >>> P1 = projection_matrix(*result) 

551 >>> is_same_transform(P0, P1) 

552 True 

553 

554 """ 

555 M = numpy.array(matrix, dtype=numpy.float64, copy=False) 

556 M33 = M[:3, :3] 

557 w, V = numpy.linalg.eig(M) 

558 i = numpy.where(abs(numpy.real(w) - 1.0) < 1e-8)[0] 

559 if not pseudo and len(i): 

560 # point: any eigenvector corresponding to eigenvalue 1 

561 point = numpy.real(V[:, i[-1]]).squeeze() 

562 point /= point[3] 

563 # direction: unit eigenvector corresponding to eigenvalue 0 

564 w, V = numpy.linalg.eig(M33) 

565 i = numpy.where(abs(numpy.real(w)) < 1e-8)[0] 

566 if not len(i): 

567 raise ValueError("no eigenvector corresponding to eigenvalue 0") 

568 direction = numpy.real(V[:, i[0]]).squeeze() 

569 direction /= vector_norm(direction) 

570 # normal: unit eigenvector of M33.T corresponding to eigenvalue 0 

571 w, V = numpy.linalg.eig(M33.T) 

572 i = numpy.where(abs(numpy.real(w)) < 1e-8)[0] 

573 if len(i): 

574 # parallel projection 

575 normal = numpy.real(V[:, i[0]]).squeeze() 

576 normal /= vector_norm(normal) 

577 return point, normal, direction, None, False 

578 else: 

579 # orthogonal projection, where normal equals direction vector 

580 return point, direction, None, None, False 

581 else: 

582 # perspective projection 

583 i = numpy.where(abs(numpy.real(w)) > 1e-8)[0] 

584 if not len(i): 

585 raise ValueError( 

586 "no eigenvector not corresponding to eigenvalue 0") 

587 point = numpy.real(V[:, i[-1]]).squeeze() 

588 point /= point[3] 

589 normal = - M[3, :3] 

590 perspective = M[:3, 3] / numpy.dot(point[:3], normal) 

591 if pseudo: 

592 perspective -= normal 

593 return point, normal, None, perspective, pseudo 

594 

595 

596def clip_matrix(left, right, bottom, top, near, far, perspective=False): 

597 """Return matrix to obtain normalized device coordinates from frustum. 

598 

599 The frustum bounds are axis-aligned along x (left, right), 

600 y (bottom, top) and z (near, far). 

601 

602 Normalized device coordinates are in range [-1, 1] if coordinates are 

603 inside the frustum. 

604 

605 If perspective is True the frustum is a truncated pyramid with the 

606 perspective point at origin and direction along z axis, otherwise an 

607 orthographic canonical view volume (a box). 

608 

609 Homogeneous coordinates transformed by the perspective clip matrix 

610 need to be dehomogenized (divided by w coordinate). 

611 

612 >>> frustum = numpy.random.rand(6) 

613 >>> frustum[1] += frustum[0] 

614 >>> frustum[3] += frustum[2] 

615 >>> frustum[5] += frustum[4] 

616 >>> M = clip_matrix(perspective=False, *frustum) 

617 >>> numpy.dot(M, [frustum[0], frustum[2], frustum[4], 1]) 

618 array([-1., -1., -1., 1.]) 

619 >>> numpy.dot(M, [frustum[1], frustum[3], frustum[5], 1]) 

620 array([ 1., 1., 1., 1.]) 

621 >>> M = clip_matrix(perspective=True, *frustum) 

622 >>> v = numpy.dot(M, [frustum[0], frustum[2], frustum[4], 1]) 

623 >>> v / v[3] 

624 array([-1., -1., -1., 1.]) 

625 >>> v = numpy.dot(M, [frustum[1], frustum[3], frustum[4], 1]) 

626 >>> v / v[3] 

627 array([ 1., 1., -1., 1.]) 

628 

629 """ 

630 if left >= right or bottom >= top or near >= far: 

631 raise ValueError("invalid frustum") 

632 if perspective: 

633 if near <= _EPS: 

634 raise ValueError("invalid frustum: near <= 0") 

635 t = 2.0 * near 

636 M = [[t/(left-right), 0.0, (right+left)/(right-left), 0.0], 

637 [0.0, t/(bottom-top), (top+bottom)/(top-bottom), 0.0], 

638 [0.0, 0.0, (far+near)/(near-far), t*far/(far-near)], 

639 [0.0, 0.0, -1.0, 0.0]] 

640 else: 

641 M = [[2.0/(right-left), 0.0, 0.0, (right+left)/(left-right)], 

642 [0.0, 2.0/(top-bottom), 0.0, (top+bottom)/(bottom-top)], 

643 [0.0, 0.0, 2.0/(far-near), (far+near)/(near-far)], 

644 [0.0, 0.0, 0.0, 1.0]] 

645 return numpy.array(M) 

646 

647 

648def shear_matrix(angle, direction, point, normal): 

649 """Return matrix to shear by angle along direction vector on shear plane. 

650 

651 The shear plane is defined by a point and normal vector. The direction 

652 vector must be orthogonal to the plane's normal vector. 

653 

654 A point P is transformed by the shear matrix into P'' such that 

655 the vector P-P'' is parallel to the direction vector and its extent is 

656 given by the angle of P-P'-P'', where P' is the orthogonal projection 

657 of P onto the shear plane. 

658 

659 >>> angle = (random.random() - 0.5) * 4*math.pi 

660 >>> direct = numpy.random.random(3) - 0.5 

661 >>> point = numpy.random.random(3) - 0.5 

662 >>> normal = numpy.cross(direct, numpy.random.random(3)) 

663 >>> S = shear_matrix(angle, direct, point, normal) 

664 >>> numpy.allclose(1, numpy.linalg.det(S)) 

665 True 

666 

667 """ 

668 normal = unit_vector(normal[:3]) 

669 direction = unit_vector(direction[:3]) 

670 if abs(numpy.dot(normal, direction)) > 1e-6: 

671 raise ValueError("direction and normal vectors are not orthogonal") 

672 angle = math.tan(angle) 

673 M = numpy.identity(4) 

674 M[:3, :3] += angle * numpy.outer(direction, normal) 

675 M[:3, 3] = -angle * numpy.dot(point[:3], normal) * direction 

676 return M 

677 

678 

679def shear_from_matrix(matrix): 

680 """Return shear angle, direction and plane from shear matrix. 

681 

682 >>> angle = (random.random() - 0.5) * 4*math.pi 

683 >>> direct = numpy.random.random(3) - 0.5 

684 >>> point = numpy.random.random(3) - 0.5 

685 >>> normal = numpy.cross(direct, numpy.random.random(3)) 

686 >>> S0 = shear_matrix(angle, direct, point, normal) 

687 >>> angle, direct, point, normal = shear_from_matrix(S0) 

688 >>> S1 = shear_matrix(angle, direct, point, normal) 

689 >>> is_same_transform(S0, S1) 

690 True 

691 

692 """ 

693 M = numpy.array(matrix, dtype=numpy.float64, copy=False) 

694 M33 = M[:3, :3] 

695 # normal: cross independent eigenvectors corresponding to the eigenvalue 1 

696 w, V = numpy.linalg.eig(M33) 

697 i = numpy.where(abs(numpy.real(w) - 1.0) < 1e-4)[0] 

698 if len(i) < 2: 

699 raise ValueError("no two linear independent eigenvectors found %s" % w) 

700 V = numpy.real(V[:, i]).squeeze().T 

701 lenorm = -1.0 

702 for i0, i1 in ((0, 1), (0, 2), (1, 2)): 

703 n = numpy.cross(V[i0], V[i1]) 

704 w = vector_norm(n) 

705 if w > lenorm: 

706 lenorm = w 

707 normal = n 

708 normal /= lenorm 

709 # direction and angle 

710 direction = numpy.dot(M33 - numpy.identity(3), normal) 

711 angle = vector_norm(direction) 

712 direction /= angle 

713 angle = math.atan(angle) 

714 # point: eigenvector corresponding to eigenvalue 1 

715 w, V = numpy.linalg.eig(M) 

716 i = numpy.where(abs(numpy.real(w) - 1.0) < 1e-8)[0] 

717 if not len(i): 

718 raise ValueError("no eigenvector corresponding to eigenvalue 1") 

719 point = numpy.real(V[:, i[-1]]).squeeze() 

720 point /= point[3] 

721 return angle, direction, point, normal 

722 

723 

724def decompose_matrix(matrix): 

725 """Return sequence of transformations from transformation matrix. 

726 

727 matrix : array_like 

728 Non-degenerative homogeneous transformation matrix 

729 

730 Return tuple of: 

731 scale : vector of 3 scaling factors 

732 shear : list of shear factors for x-y, x-z, y-z axes 

733 angles : list of Euler angles about static x, y, z axes 

734 translate : translation vector along x, y, z axes 

735 perspective : perspective partition of matrix 

736 

737 Raise ValueError if matrix is of wrong type or degenerative. 

738 

739 >>> T0 = translation_matrix([1, 2, 3]) 

740 >>> scale, shear, angles, trans, persp = decompose_matrix(T0) 

741 >>> T1 = translation_matrix(trans) 

742 >>> numpy.allclose(T0, T1) 

743 True 

744 >>> S = scale_matrix(0.123) 

745 >>> scale, shear, angles, trans, persp = decompose_matrix(S) 

746 >>> scale[0] 

747 0.123 

748 >>> R0 = euler_matrix(1, 2, 3) 

749 >>> scale, shear, angles, trans, persp = decompose_matrix(R0) 

750 >>> R1 = euler_matrix(*angles) 

751 >>> numpy.allclose(R0, R1) 

752 True 

753 

754 """ 

755 M = numpy.array(matrix, dtype=numpy.float64, copy=True).T 

756 if abs(M[3, 3]) < _EPS: 

757 raise ValueError("M[3, 3] is zero") 

758 M /= M[3, 3] 

759 P = M.copy() 

760 P[:, 3] = 0.0, 0.0, 0.0, 1.0 

761 if not numpy.linalg.det(P): 

762 raise ValueError("matrix is singular") 

763 

764 scale = numpy.zeros((3, )) 

765 shear = [0.0, 0.0, 0.0] 

766 angles = [0.0, 0.0, 0.0] 

767 

768 if any(abs(M[:3, 3]) > _EPS): 

769 perspective = numpy.dot(M[:, 3], numpy.linalg.inv(P.T)) 

770 M[:, 3] = 0.0, 0.0, 0.0, 1.0 

771 else: 

772 perspective = numpy.array([0.0, 0.0, 0.0, 1.0]) 

773 

774 translate = M[3, :3].copy() 

775 M[3, :3] = 0.0 

776 

777 row = M[:3, :3].copy() 

778 scale[0] = vector_norm(row[0]) 

779 row[0] /= scale[0] 

780 shear[0] = numpy.dot(row[0], row[1]) 

781 row[1] -= row[0] * shear[0] 

782 scale[1] = vector_norm(row[1]) 

783 row[1] /= scale[1] 

784 shear[0] /= scale[1] 

785 shear[1] = numpy.dot(row[0], row[2]) 

786 row[2] -= row[0] * shear[1] 

787 shear[2] = numpy.dot(row[1], row[2]) 

788 row[2] -= row[1] * shear[2] 

789 scale[2] = vector_norm(row[2]) 

790 row[2] /= scale[2] 

791 shear[1:] /= scale[2] 

792 

793 if numpy.dot(row[0], numpy.cross(row[1], row[2])) < 0: 

794 numpy.negative(scale, scale) 

795 numpy.negative(row, row) 

796 

797 angles[1] = math.asin(-row[0, 2]) 

798 if math.cos(angles[1]): 

799 angles[0] = math.atan2(row[1, 2], row[2, 2]) 

800 angles[2] = math.atan2(row[0, 1], row[0, 0]) 

801 else: 

802 #angles[0] = math.atan2(row[1, 0], row[1, 1]) 

803 angles[0] = math.atan2(-row[2, 1], row[1, 1]) 

804 angles[2] = 0.0 

805 

806 return scale, shear, angles, translate, perspective 

807 

808 

809def compose_matrix(scale=None, shear=None, angles=None, translate=None, 

810 perspective=None): 

811 """Return transformation matrix from sequence of transformations. 

812 

813 This is the inverse of the decompose_matrix function. 

814 

815 Sequence of transformations: 

816 scale : vector of 3 scaling factors 

817 shear : list of shear factors for x-y, x-z, y-z axes 

818 angles : list of Euler angles about static x, y, z axes 

819 translate : translation vector along x, y, z axes 

820 perspective : perspective partition of matrix 

821 

822 >>> scale = numpy.random.random(3) - 0.5 

823 >>> shear = numpy.random.random(3) - 0.5 

824 >>> angles = (numpy.random.random(3) - 0.5) * (2*math.pi) 

825 >>> trans = numpy.random.random(3) - 0.5 

826 >>> persp = numpy.random.random(4) - 0.5 

827 >>> M0 = compose_matrix(scale, shear, angles, trans, persp) 

828 >>> result = decompose_matrix(M0) 

829 >>> M1 = compose_matrix(*result) 

830 >>> is_same_transform(M0, M1) 

831 True 

832 

833 """ 

834 M = numpy.identity(4) 

835 if perspective is not None: 

836 P = numpy.identity(4) 

837 P[3, :] = perspective[:4] 

838 M = numpy.dot(M, P) 

839 if translate is not None: 

840 T = numpy.identity(4) 

841 T[:3, 3] = translate[:3] 

842 M = numpy.dot(M, T) 

843 if angles is not None: 

844 R = euler_matrix(angles[0], angles[1], angles[2], 'sxyz') 

845 M = numpy.dot(M, R) 

846 if shear is not None: 

847 Z = numpy.identity(4) 

848 Z[1, 2] = shear[2] 

849 Z[0, 2] = shear[1] 

850 Z[0, 1] = shear[0] 

851 M = numpy.dot(M, Z) 

852 if scale is not None: 

853 S = numpy.identity(4) 

854 S[0, 0] = scale[0] 

855 S[1, 1] = scale[1] 

856 S[2, 2] = scale[2] 

857 M = numpy.dot(M, S) 

858 M /= M[3, 3] 

859 return M 

860 

861 

862def orthogonalization_matrix(lengths, angles): 

863 """Return orthogonalization matrix for crystallographic cell coordinates. 

864 

865 Angles are expected in degrees. 

866 

867 The de-orthogonalization matrix is the inverse. 

868 

869 >>> O = orthogonalization_matrix([10, 10, 10], [90, 90, 90]) 

870 >>> numpy.allclose(O[:3, :3], numpy.identity(3, float) * 10) 

871 True 

872 >>> O = orthogonalization_matrix([9.8, 12.0, 15.5], [87.2, 80.7, 69.7]) 

873 >>> numpy.allclose(numpy.sum(O), 43.063229) 

874 True 

875 

876 """ 

877 a, b, c = lengths 

878 angles = numpy.radians(angles) 

879 sina, sinb, _ = numpy.sin(angles) 

880 cosa, cosb, cosg = numpy.cos(angles) 

881 co = (cosa * cosb - cosg) / (sina * sinb) 

882 return numpy.array([ 

883 [ a*sinb*math.sqrt(1.0-co*co), 0.0, 0.0, 0.0], 

884 [-a*sinb*co, b*sina, 0.0, 0.0], 

885 [ a*cosb, b*cosa, c, 0.0], 

886 [ 0.0, 0.0, 0.0, 1.0]]) 

887 

888 

889def affine_matrix_from_points(v0, v1, shear=True, scale=True, usesvd=True): 

890 """Return affine transform matrix to register two point sets. 

891 

892 v0 and v1 are shape (ndims, *) arrays of at least ndims non-homogeneous 

893 coordinates, where ndims is the dimensionality of the coordinate space. 

894 

895 If shear is False, a similarity transformation matrix is returned. 

896 If also scale is False, a rigid/Euclidean transformation matrix 

897 is returned. 

898 

899 By default the algorithm by Hartley and Zissermann [15] is used. 

900 If usesvd is True, similarity and Euclidean transformation matrices 

901 are calculated by minimizing the weighted sum of squared deviations 

902 (RMSD) according to the algorithm by Kabsch [8]. 

903 Otherwise, and if ndims is 3, the quaternion based algorithm by Horn [9] 

904 is used, which is slower when using this Python implementation. 

905 

906 The returned matrix performs rotation, translation and uniform scaling 

907 (if specified). 

908 

909 >>> v0 = [[0, 1031, 1031, 0], [0, 0, 1600, 1600]] 

910 >>> v1 = [[675, 826, 826, 677], [55, 52, 281, 277]] 

911 >>> affine_matrix_from_points(v0, v1) 

912 array([[ 0.14549, 0.00062, 675.50008], 

913 [ 0.00048, 0.14094, 53.24971], 

914 [ 0. , 0. , 1. ]]) 

915 >>> T = translation_matrix(numpy.random.random(3)-0.5) 

916 >>> R = random_rotation_matrix(numpy.random.random(3)) 

917 >>> S = scale_matrix(random.random()) 

918 >>> M = concatenate_matrices(T, R, S) 

919 >>> v0 = (numpy.random.rand(4, 100) - 0.5) * 20 

920 >>> v0[3] = 1 

921 >>> v1 = numpy.dot(M, v0) 

922 >>> v0[:3] += numpy.random.normal(0, 1e-8, 300).reshape(3, -1) 

923 >>> M = affine_matrix_from_points(v0[:3], v1[:3]) 

924 >>> numpy.allclose(v1, numpy.dot(M, v0)) 

925 True 

926 

927 More examples in superimposition_matrix() 

928 

929 """ 

930 v0 = numpy.array(v0, dtype=numpy.float64, copy=True) 

931 v1 = numpy.array(v1, dtype=numpy.float64, copy=True) 

932 

933 ndims = v0.shape[0] 

934 if ndims < 2 or v0.shape[1] < ndims or v0.shape != v1.shape: 

935 raise ValueError("input arrays are of wrong shape or type") 

936 

937 # move centroids to origin 

938 t0 = -numpy.mean(v0, axis=1) 

939 M0 = numpy.identity(ndims+1) 

940 M0[:ndims, ndims] = t0 

941 v0 += t0.reshape(ndims, 1) 

942 t1 = -numpy.mean(v1, axis=1) 

943 M1 = numpy.identity(ndims+1) 

944 M1[:ndims, ndims] = t1 

945 v1 += t1.reshape(ndims, 1) 

946 

947 if shear: 

948 # Affine transformation 

949 A = numpy.concatenate((v0, v1), axis=0) 

950 u, s, vh = numpy.linalg.svd(A.T) 

951 vh = vh[:ndims].T 

952 B = vh[:ndims] 

953 C = vh[ndims:2*ndims] 

954 t = numpy.dot(C, numpy.linalg.pinv(B)) 

955 t = numpy.concatenate((t, numpy.zeros((ndims, 1))), axis=1) 

956 M = numpy.vstack((t, ((0.0,)*ndims) + (1.0,))) 

957 elif usesvd or ndims != 3: 

958 # Rigid transformation via SVD of covariance matrix 

959 u, s, vh = numpy.linalg.svd(numpy.dot(v1, v0.T)) 

960 # rotation matrix from SVD orthonormal bases 

961 R = numpy.dot(u, vh) 

962 if numpy.linalg.det(R) < 0.0: 

963 # R does not constitute right handed system 

964 R -= numpy.outer(u[:, ndims-1], vh[ndims-1, :]*2.0) 

965 s[-1] *= -1.0 

966 # homogeneous transformation matrix 

967 M = numpy.identity(ndims+1) 

968 M[:ndims, :ndims] = R 

969 else: 

970 # Rigid transformation matrix via quaternion 

971 # compute symmetric matrix N 

972 xx, yy, zz = numpy.sum(v0 * v1, axis=1) 

973 xy, yz, zx = numpy.sum(v0 * numpy.roll(v1, -1, axis=0), axis=1) 

974 xz, yx, zy = numpy.sum(v0 * numpy.roll(v1, -2, axis=0), axis=1) 

975 N = [[xx+yy+zz, 0.0, 0.0, 0.0], 

976 [yz-zy, xx-yy-zz, 0.0, 0.0], 

977 [zx-xz, xy+yx, yy-xx-zz, 0.0], 

978 [xy-yx, zx+xz, yz+zy, zz-xx-yy]] 

979 # quaternion: eigenvector corresponding to most positive eigenvalue 

980 w, V = numpy.linalg.eigh(N) 

981 q = V[:, numpy.argmax(w)] 

982 q /= vector_norm(q) # unit quaternion 

983 # homogeneous transformation matrix 

984 M = quaternion_matrix(q) 

985 

986 if scale and not shear: 

987 # Affine transformation; scale is ratio of RMS deviations from centroid 

988 v0 *= v0 

989 v1 *= v1 

990 M[:ndims, :ndims] *= math.sqrt(numpy.sum(v1) / numpy.sum(v0)) 

991 

992 # move centroids back 

993 M = numpy.dot(numpy.linalg.inv(M1), numpy.dot(M, M0)) 

994 M /= M[ndims, ndims] 

995 return M 

996 

997 

998def superimposition_matrix(v0, v1, scale=False, usesvd=True): 

999 """Return matrix to transform given 3D point set into second point set. 

1000 

1001 v0 and v1 are shape (3, *) or (4, *) arrays of at least 3 points. 

1002 

1003 The parameters scale and usesvd are explained in the more general 

1004 affine_matrix_from_points function. 

1005 

1006 The returned matrix is a similarity or Euclidean transformation matrix. 

1007 This function has a fast C implementation in transformations.c. 

1008 

1009 >>> v0 = numpy.random.rand(3, 10) 

1010 >>> M = superimposition_matrix(v0, v0) 

1011 >>> numpy.allclose(M, numpy.identity(4)) 

1012 True 

1013 >>> R = random_rotation_matrix(numpy.random.random(3)) 

1014 >>> v0 = [[1,0,0], [0,1,0], [0,0,1], [1,1,1]] 

1015 >>> v1 = numpy.dot(R, v0) 

1016 >>> M = superimposition_matrix(v0, v1) 

1017 >>> numpy.allclose(v1, numpy.dot(M, v0)) 

1018 True 

1019 >>> v0 = (numpy.random.rand(4, 100) - 0.5) * 20 

1020 >>> v0[3] = 1 

1021 >>> v1 = numpy.dot(R, v0) 

1022 >>> M = superimposition_matrix(v0, v1) 

1023 >>> numpy.allclose(v1, numpy.dot(M, v0)) 

1024 True 

1025 >>> S = scale_matrix(random.random()) 

1026 >>> T = translation_matrix(numpy.random.random(3)-0.5) 

1027 >>> M = concatenate_matrices(T, R, S) 

1028 >>> v1 = numpy.dot(M, v0) 

1029 >>> v0[:3] += numpy.random.normal(0, 1e-9, 300).reshape(3, -1) 

1030 >>> M = superimposition_matrix(v0, v1, scale=True) 

1031 >>> numpy.allclose(v1, numpy.dot(M, v0)) 

1032 True 

1033 >>> M = superimposition_matrix(v0, v1, scale=True, usesvd=False) 

1034 >>> numpy.allclose(v1, numpy.dot(M, v0)) 

1035 True 

1036 >>> v = numpy.empty((4, 100, 3)) 

1037 >>> v[:, :, 0] = v0 

1038 >>> M = superimposition_matrix(v0, v1, scale=True, usesvd=False) 

1039 >>> numpy.allclose(v1, numpy.dot(M, v[:, :, 0])) 

1040 True 

1041 

1042 """ 

1043 v0 = numpy.array(v0, dtype=numpy.float64, copy=False)[:3] 

1044 v1 = numpy.array(v1, dtype=numpy.float64, copy=False)[:3] 

1045 return affine_matrix_from_points(v0, v1, shear=False, 

1046 scale=scale, usesvd=usesvd) 

1047 

1048 

1049def euler_matrix(ai, aj, ak, axes='sxyz'): 

1050 """Return homogeneous rotation matrix from Euler angles and axis sequence. 

1051 

1052 ai, aj, ak : Euler's roll, pitch and yaw angles 

1053 axes : One of 24 axis sequences as string or encoded tuple 

1054 

1055 >>> R = euler_matrix(1, 2, 3, 'syxz') 

1056 >>> numpy.allclose(numpy.sum(R[0]), -1.34786452) 

1057 True 

1058 >>> R = euler_matrix(1, 2, 3, (0, 1, 0, 1)) 

1059 >>> numpy.allclose(numpy.sum(R[0]), -0.383436184) 

1060 True 

1061 >>> ai, aj, ak = (4*math.pi) * (numpy.random.random(3) - 0.5) 

1062 >>> for axes in _AXES2TUPLE.keys(): 

1063 ... R = euler_matrix(ai, aj, ak, axes) 

1064 >>> for axes in _TUPLE2AXES.keys(): 

1065 ... R = euler_matrix(ai, aj, ak, axes) 

1066 

1067 """ 

1068 try: 

1069 firstaxis, parity, repetition, frame = _AXES2TUPLE[axes] 

1070 except (AttributeError, KeyError): 

1071 _TUPLE2AXES[axes] # validation 

1072 firstaxis, parity, repetition, frame = axes 

1073 

1074 i = firstaxis 

1075 j = _NEXT_AXIS[i+parity] 

1076 k = _NEXT_AXIS[i-parity+1] 

1077 

1078 if frame: 

1079 ai, ak = ak, ai 

1080 if parity: 

1081 ai, aj, ak = -ai, -aj, -ak 

1082 

1083 si, sj, sk = math.sin(ai), math.sin(aj), math.sin(ak) 

1084 ci, cj, ck = math.cos(ai), math.cos(aj), math.cos(ak) 

1085 cc, cs = ci*ck, ci*sk 

1086 sc, ss = si*ck, si*sk 

1087 

1088 M = numpy.identity(4) 

1089 if repetition: 

1090 M[i, i] = cj 

1091 M[i, j] = sj*si 

1092 M[i, k] = sj*ci 

1093 M[j, i] = sj*sk 

1094 M[j, j] = -cj*ss+cc 

1095 M[j, k] = -cj*cs-sc 

1096 M[k, i] = -sj*ck 

1097 M[k, j] = cj*sc+cs 

1098 M[k, k] = cj*cc-ss 

1099 else: 

1100 M[i, i] = cj*ck 

1101 M[i, j] = sj*sc-cs 

1102 M[i, k] = sj*cc+ss 

1103 M[j, i] = cj*sk 

1104 M[j, j] = sj*ss+cc 

1105 M[j, k] = sj*cs-sc 

1106 M[k, i] = -sj 

1107 M[k, j] = cj*si 

1108 M[k, k] = cj*ci 

1109 return M 

1110 

1111 

1112def euler_from_matrix(matrix, axes='sxyz'): 

1113 """Return Euler angles from rotation matrix for specified axis sequence. 

1114 

1115 axes : One of 24 axis sequences as string or encoded tuple 

1116 

1117 Note that many Euler angle triplets can describe one matrix. 

1118 

1119 >>> R0 = euler_matrix(1, 2, 3, 'syxz') 

1120 >>> al, be, ga = euler_from_matrix(R0, 'syxz') 

1121 >>> R1 = euler_matrix(al, be, ga, 'syxz') 

1122 >>> numpy.allclose(R0, R1) 

1123 True 

1124 >>> angles = (4*math.pi) * (numpy.random.random(3) - 0.5) 

1125 >>> for axes in _AXES2TUPLE.keys(): 

1126 ... R0 = euler_matrix(axes=axes, *angles) 

1127 ... R1 = euler_matrix(axes=axes, *euler_from_matrix(R0, axes)) 

1128 ... if not numpy.allclose(R0, R1): print(axes, "failed") 

1129 

1130 """ 

1131 try: 

1132 firstaxis, parity, repetition, frame = _AXES2TUPLE[axes.lower()] 

1133 except (AttributeError, KeyError): 

1134 _TUPLE2AXES[axes] # validation 

1135 firstaxis, parity, repetition, frame = axes 

1136 

1137 i = firstaxis 

1138 j = _NEXT_AXIS[i+parity] 

1139 k = _NEXT_AXIS[i-parity+1] 

1140 

1141 M = numpy.array(matrix, dtype=numpy.float64, copy=False)[:3, :3] 

1142 if repetition: 

1143 sy = math.sqrt(M[i, j]*M[i, j] + M[i, k]*M[i, k]) 

1144 if sy > _EPS: 

1145 ax = math.atan2( M[i, j], M[i, k]) 

1146 ay = math.atan2( sy, M[i, i]) 

1147 az = math.atan2( M[j, i], -M[k, i]) 

1148 else: 

1149 ax = math.atan2(-M[j, k], M[j, j]) 

1150 ay = math.atan2( sy, M[i, i]) 

1151 az = 0.0 

1152 else: 

1153 cy = math.sqrt(M[i, i]*M[i, i] + M[j, i]*M[j, i]) 

1154 if cy > _EPS: 

1155 ax = math.atan2( M[k, j], M[k, k]) 

1156 ay = math.atan2(-M[k, i], cy) 

1157 az = math.atan2( M[j, i], M[i, i]) 

1158 else: 

1159 ax = math.atan2(-M[j, k], M[j, j]) 

1160 ay = math.atan2(-M[k, i], cy) 

1161 az = 0.0 

1162 

1163 if parity: 

1164 ax, ay, az = -ax, -ay, -az 

1165 if frame: 

1166 ax, az = az, ax 

1167 return ax, ay, az 

1168 

1169 

1170def euler_from_quaternion(quaternion, axes='sxyz'): 

1171 """Return Euler angles from quaternion for specified axis sequence. 

1172 

1173 >>> angles = euler_from_quaternion([0.99810947, 0.06146124, 0, 0]) 

1174 >>> numpy.allclose(angles, [0.123, 0, 0]) 

1175 True 

1176 

1177 """ 

1178 return euler_from_matrix(quaternion_matrix(quaternion), axes) 

1179 

1180 

1181def quaternion_from_euler(ai, aj, ak, axes='sxyz'): 

1182 """Return quaternion from Euler angles and axis sequence. 

1183 

1184 ai, aj, ak : Euler's roll, pitch and yaw angles 

1185 axes : One of 24 axis sequences as string or encoded tuple 

1186 

1187 >>> q = quaternion_from_euler(1, 2, 3, 'ryxz') 

1188 >>> numpy.allclose(q, [0.435953, 0.310622, -0.718287, 0.444435]) 

1189 True 

1190 

1191 """ 

1192 try: 

1193 firstaxis, parity, repetition, frame = _AXES2TUPLE[axes.lower()] 

1194 except (AttributeError, KeyError): 

1195 _TUPLE2AXES[axes] # validation 

1196 firstaxis, parity, repetition, frame = axes 

1197 

1198 i = firstaxis + 1 

1199 j = _NEXT_AXIS[i+parity-1] + 1 

1200 k = _NEXT_AXIS[i-parity] + 1 

1201 

1202 if frame: 

1203 ai, ak = ak, ai 

1204 if parity: 

1205 aj = -aj 

1206 

1207 ai /= 2.0 

1208 aj /= 2.0 

1209 ak /= 2.0 

1210 ci = math.cos(ai) 

1211 si = math.sin(ai) 

1212 cj = math.cos(aj) 

1213 sj = math.sin(aj) 

1214 ck = math.cos(ak) 

1215 sk = math.sin(ak) 

1216 cc = ci*ck 

1217 cs = ci*sk 

1218 sc = si*ck 

1219 ss = si*sk 

1220 

1221 q = numpy.empty((4, )) 

1222 if repetition: 

1223 q[0] = cj*(cc - ss) 

1224 q[i] = cj*(cs + sc) 

1225 q[j] = sj*(cc + ss) 

1226 q[k] = sj*(cs - sc) 

1227 else: 

1228 q[0] = cj*cc + sj*ss 

1229 q[i] = cj*sc - sj*cs 

1230 q[j] = cj*ss + sj*cc 

1231 q[k] = cj*cs - sj*sc 

1232 if parity: 

1233 q[j] *= -1.0 

1234 

1235 return q 

1236 

1237 

1238def quaternion_about_axis(angle, axis): 

1239 """Return quaternion for rotation about axis. 

1240 

1241 >>> q = quaternion_about_axis(0.123, [1, 0, 0]) 

1242 >>> numpy.allclose(q, [0.99810947, 0.06146124, 0, 0]) 

1243 True 

1244 

1245 """ 

1246 q = numpy.array([0.0, axis[0], axis[1], axis[2]]) 

1247 qlen = vector_norm(q) 

1248 if qlen > _EPS: 

1249 q *= math.sin(angle/2.0) / qlen 

1250 q[0] = math.cos(angle/2.0) 

1251 return q 

1252 

1253 

1254def quaternion_matrix(quaternion): 

1255 """Return homogeneous rotation matrix from quaternion. 

1256 

1257 >>> M = quaternion_matrix([0.99810947, 0.06146124, 0, 0]) 

1258 >>> numpy.allclose(M, rotation_matrix(0.123, [1, 0, 0])) 

1259 True 

1260 >>> M = quaternion_matrix([1, 0, 0, 0]) 

1261 >>> numpy.allclose(M, numpy.identity(4)) 

1262 True 

1263 >>> M = quaternion_matrix([0, 1, 0, 0]) 

1264 >>> numpy.allclose(M, numpy.diag([1, -1, -1, 1])) 

1265 True 

1266 

1267 """ 

1268 q = numpy.array(quaternion, dtype=numpy.float64, copy=True) 

1269 n = numpy.dot(q, q) 

1270 if n < _EPS: 

1271 return numpy.identity(4) 

1272 q *= math.sqrt(2.0 / n) 

1273 q = numpy.outer(q, q) 

1274 return numpy.array([ 

1275 [1.0-q[2, 2]-q[3, 3], q[1, 2]-q[3, 0], q[1, 3]+q[2, 0], 0.0], 

1276 [ q[1, 2]+q[3, 0], 1.0-q[1, 1]-q[3, 3], q[2, 3]-q[1, 0], 0.0], 

1277 [ q[1, 3]-q[2, 0], q[2, 3]+q[1, 0], 1.0-q[1, 1]-q[2, 2], 0.0], 

1278 [ 0.0, 0.0, 0.0, 1.0]]) 

1279 

1280 

1281def quaternion_from_matrix(matrix, isprecise=False): 

1282 """Return quaternion from rotation matrix. 

1283 

1284 If isprecise is True, the input matrix is assumed to be a precise rotation 

1285 matrix and a faster algorithm is used. 

1286 

1287 >>> q = quaternion_from_matrix(numpy.identity(4), True) 

1288 >>> numpy.allclose(q, [1, 0, 0, 0]) 

1289 True 

1290 >>> q = quaternion_from_matrix(numpy.diag([1, -1, -1, 1])) 

1291 >>> numpy.allclose(q, [0, 1, 0, 0]) or numpy.allclose(q, [0, -1, 0, 0]) 

1292 True 

1293 >>> R = rotation_matrix(0.123, (1, 2, 3)) 

1294 >>> q = quaternion_from_matrix(R, True) 

1295 >>> numpy.allclose(q, [0.9981095, 0.0164262, 0.0328524, 0.0492786]) 

1296 True 

1297 >>> R = [[-0.545, 0.797, 0.260, 0], [0.733, 0.603, -0.313, 0], 

1298 ... [-0.407, 0.021, -0.913, 0], [0, 0, 0, 1]] 

1299 >>> q = quaternion_from_matrix(R) 

1300 >>> numpy.allclose(q, [0.19069, 0.43736, 0.87485, -0.083611]) 

1301 True 

1302 >>> R = [[0.395, 0.362, 0.843, 0], [-0.626, 0.796, -0.056, 0], 

1303 ... [-0.677, -0.498, 0.529, 0], [0, 0, 0, 1]] 

1304 >>> q = quaternion_from_matrix(R) 

1305 >>> numpy.allclose(q, [0.82336615, -0.13610694, 0.46344705, -0.29792603]) 

1306 True 

1307 >>> R = random_rotation_matrix() 

1308 >>> q = quaternion_from_matrix(R) 

1309 >>> is_same_transform(R, quaternion_matrix(q)) 

1310 True 

1311 >>> R = euler_matrix(0.0, 0.0, numpy.pi/2.0) 

1312 >>> numpy.allclose(quaternion_from_matrix(R, isprecise=False), 

1313 ... quaternion_from_matrix(R, isprecise=True)) 

1314 True 

1315 

1316 """ 

1317 M = numpy.array(matrix, dtype=numpy.float64, copy=False)[:4, :4] 

1318 if isprecise: 

1319 q = numpy.empty((4, )) 

1320 t = numpy.trace(M) 

1321 if t > M[3, 3]: 

1322 q[0] = t 

1323 q[3] = M[1, 0] - M[0, 1] 

1324 q[2] = M[0, 2] - M[2, 0] 

1325 q[1] = M[2, 1] - M[1, 2] 

1326 else: 

1327 i, j, k = 1, 2, 3 

1328 if M[1, 1] > M[0, 0]: 

1329 i, j, k = 2, 3, 1 

1330 if M[2, 2] > M[i, i]: 

1331 i, j, k = 3, 1, 2 

1332 t = M[i, i] - (M[j, j] + M[k, k]) + M[3, 3] 

1333 q[i] = t 

1334 q[j] = M[i, j] + M[j, i] 

1335 q[k] = M[k, i] + M[i, k] 

1336 q[3] = M[k, j] - M[j, k] 

1337 q *= 0.5 / math.sqrt(t * M[3, 3]) 

1338 else: 

1339 m00 = M[0, 0] 

1340 m01 = M[0, 1] 

1341 m02 = M[0, 2] 

1342 m10 = M[1, 0] 

1343 m11 = M[1, 1] 

1344 m12 = M[1, 2] 

1345 m20 = M[2, 0] 

1346 m21 = M[2, 1] 

1347 m22 = M[2, 2] 

1348 # symmetric matrix K 

1349 K = numpy.array([[m00-m11-m22, 0.0, 0.0, 0.0], 

1350 [m01+m10, m11-m00-m22, 0.0, 0.0], 

1351 [m02+m20, m12+m21, m22-m00-m11, 0.0], 

1352 [m21-m12, m02-m20, m10-m01, m00+m11+m22]]) 

1353 K /= 3.0 

1354 # quaternion is eigenvector of K that corresponds to largest eigenvalue 

1355 w, V = numpy.linalg.eigh(K) 

1356 q = V[[3, 0, 1, 2], numpy.argmax(w)] 

1357 if q[0] < 0.0: 

1358 numpy.negative(q, q) 

1359 return q 

1360 

1361 

1362def quaternion_multiply(quaternion1, quaternion0): 

1363 """Return multiplication of two quaternions. 

1364 

1365 >>> q = quaternion_multiply([4, 1, -2, 3], [8, -5, 6, 7]) 

1366 >>> numpy.allclose(q, [28, -44, -14, 48]) 

1367 True 

1368 

1369 """ 

1370 w0, x0, y0, z0 = quaternion0 

1371 w1, x1, y1, z1 = quaternion1 

1372 return numpy.array([-x1*x0 - y1*y0 - z1*z0 + w1*w0, 

1373 x1*w0 + y1*z0 - z1*y0 + w1*x0, 

1374 -x1*z0 + y1*w0 + z1*x0 + w1*y0, 

1375 x1*y0 - y1*x0 + z1*w0 + w1*z0], dtype=numpy.float64) 

1376 

1377 

1378def quaternion_conjugate(quaternion): 

1379 """Return conjugate of quaternion. 

1380 

1381 >>> q0 = random_quaternion() 

1382 >>> q1 = quaternion_conjugate(q0) 

1383 >>> q1[0] == q0[0] and all(q1[1:] == -q0[1:]) 

1384 True 

1385 

1386 """ 

1387 q = numpy.array(quaternion, dtype=numpy.float64, copy=True) 

1388 numpy.negative(q[1:], q[1:]) 

1389 return q 

1390 

1391 

1392def quaternion_inverse(quaternion): 

1393 """Return inverse of quaternion. 

1394 

1395 >>> q0 = random_quaternion() 

1396 >>> q1 = quaternion_inverse(q0) 

1397 >>> numpy.allclose(quaternion_multiply(q0, q1), [1, 0, 0, 0]) 

1398 True 

1399 

1400 """ 

1401 q = numpy.array(quaternion, dtype=numpy.float64, copy=True) 

1402 numpy.negative(q[1:], q[1:]) 

1403 return q / numpy.dot(q, q) 

1404 

1405 

1406def quaternion_real(quaternion): 

1407 """Return real part of quaternion. 

1408 

1409 >>> quaternion_real([3, 0, 1, 2]) 

1410 3.0 

1411 

1412 """ 

1413 return float(quaternion[0]) 

1414 

1415 

1416def quaternion_imag(quaternion): 

1417 """Return imaginary part of quaternion. 

1418 

1419 >>> quaternion_imag([3, 0, 1, 2]) 

1420 array([ 0., 1., 2.]) 

1421 

1422 """ 

1423 return numpy.array(quaternion[1:4], dtype=numpy.float64, copy=True) 

1424 

1425 

1426def quaternion_slerp(quat0, quat1, fraction, spin=0, shortestpath=True): 

1427 """Return spherical linear interpolation between two quaternions. 

1428 

1429 >>> q0 = random_quaternion() 

1430 >>> q1 = random_quaternion() 

1431 >>> q = quaternion_slerp(q0, q1, 0) 

1432 >>> numpy.allclose(q, q0) 

1433 True 

1434 >>> q = quaternion_slerp(q0, q1, 1, 1) 

1435 >>> numpy.allclose(q, q1) 

1436 True 

1437 >>> q = quaternion_slerp(q0, q1, 0.5) 

1438 >>> angle = math.acos(numpy.dot(q0, q)) 

1439 >>> numpy.allclose(2, math.acos(numpy.dot(q0, q1)) / angle) or \ 

1440 numpy.allclose(2, math.acos(-numpy.dot(q0, q1)) / angle) 

1441 True 

1442 

1443 """ 

1444 q0 = unit_vector(quat0[:4]) 

1445 q1 = unit_vector(quat1[:4]) 

1446 if fraction == 0.0: 

1447 return q0 

1448 elif fraction == 1.0: 

1449 return q1 

1450 d = numpy.dot(q0, q1) 

1451 if abs(abs(d) - 1.0) < _EPS: 

1452 return q0 

1453 if shortestpath and d < 0.0: 

1454 # invert rotation 

1455 d = -d 

1456 numpy.negative(q1, q1) 

1457 angle = math.acos(d) + spin * math.pi 

1458 if abs(angle) < _EPS: 

1459 return q0 

1460 isin = 1.0 / math.sin(angle) 

1461 q0 *= math.sin((1.0 - fraction) * angle) * isin 

1462 q1 *= math.sin(fraction * angle) * isin 

1463 q0 += q1 

1464 return q0 

1465 

1466 

1467def random_quaternion(rand=None): 

1468 """Return uniform random unit quaternion. 

1469 

1470 rand: array like or None 

1471 Three independent random variables that are uniformly distributed 

1472 between 0 and 1. 

1473 

1474 >>> q = random_quaternion() 

1475 >>> numpy.allclose(1, vector_norm(q)) 

1476 True 

1477 >>> q = random_quaternion(numpy.random.random(3)) 

1478 >>> len(q.shape), q.shape[0]==4 

1479 (1, True) 

1480 

1481 """ 

1482 if rand is None: 

1483 rand = numpy.random.rand(3) 

1484 else: 

1485 assert len(rand) == 3 

1486 r1 = numpy.sqrt(1.0 - rand[0]) 

1487 r2 = numpy.sqrt(rand[0]) 

1488 pi2 = math.pi * 2.0 

1489 t1 = pi2 * rand[1] 

1490 t2 = pi2 * rand[2] 

1491 return numpy.array([numpy.cos(t2)*r2, numpy.sin(t1)*r1, 

1492 numpy.cos(t1)*r1, numpy.sin(t2)*r2]) 

1493 

1494 

1495def random_rotation_matrix(rand=None): 

1496 """Return uniform random rotation matrix. 

1497 

1498 rand: array like 

1499 Three independent random variables that are uniformly distributed 

1500 between 0 and 1 for each returned quaternion. 

1501 

1502 >>> R = random_rotation_matrix() 

1503 >>> numpy.allclose(numpy.dot(R.T, R), numpy.identity(4)) 

1504 True 

1505 

1506 """ 

1507 return quaternion_matrix(random_quaternion(rand)) 

1508 

1509 

1510class Arcball(object): 

1511 """Virtual Trackball Control. 

1512 

1513 >>> ball = Arcball() 

1514 >>> ball = Arcball(initial=numpy.identity(4)) 

1515 >>> ball.place([320, 320], 320) 

1516 >>> ball.down([500, 250]) 

1517 >>> ball.drag([475, 275]) 

1518 >>> R = ball.matrix() 

1519 >>> numpy.allclose(numpy.sum(R), 3.90583455) 

1520 True 

1521 >>> ball = Arcball(initial=[1, 0, 0, 0]) 

1522 >>> ball.place([320, 320], 320) 

1523 >>> ball.setaxes([1, 1, 0], [-1, 1, 0]) 

1524 >>> ball.constrain = True 

1525 >>> ball.down([400, 200]) 

1526 >>> ball.drag([200, 400]) 

1527 >>> R = ball.matrix() 

1528 >>> numpy.allclose(numpy.sum(R), 0.2055924) 

1529 True 

1530 >>> ball.next() 

1531 

1532 """ 

1533 def __init__(self, initial=None): 

1534 """Initialize virtual trackball control. 

1535 

1536 initial : quaternion or rotation matrix 

1537 

1538 """ 

1539 self._axis = None 

1540 self._axes = None 

1541 self._radius = 1.0 

1542 self._center = [0.0, 0.0] 

1543 self._vdown = numpy.array([0.0, 0.0, 1.0]) 

1544 self._constrain = False 

1545 if initial is None: 

1546 self._qdown = numpy.array([1.0, 0.0, 0.0, 0.0]) 

1547 else: 

1548 initial = numpy.array(initial, dtype=numpy.float64) 

1549 if initial.shape == (4, 4): 

1550 self._qdown = quaternion_from_matrix(initial) 

1551 elif initial.shape == (4, ): 

1552 initial /= vector_norm(initial) 

1553 self._qdown = initial 

1554 else: 

1555 raise ValueError("initial not a quaternion or matrix") 

1556 self._qnow = self._qpre = self._qdown 

1557 

1558 def place(self, center, radius): 

1559 """Place Arcball, e.g. when window size changes. 

1560 

1561 center : sequence[2] 

1562 Window coordinates of trackball center. 

1563 radius : float 

1564 Radius of trackball in window coordinates. 

1565 

1566 """ 

1567 self._radius = float(radius) 

1568 self._center[0] = center[0] 

1569 self._center[1] = center[1] 

1570 

1571 def setaxes(self, *axes): 

1572 """Set axes to constrain rotations.""" 

1573 if axes is None: 

1574 self._axes = None 

1575 else: 

1576 self._axes = [unit_vector(axis) for axis in axes] 

1577 

1578 @property 

1579 def constrain(self): 

1580 """Return state of constrain to axis mode.""" 

1581 return self._constrain 

1582 

1583 @constrain.setter 

1584 def constrain(self, value): 

1585 """Set state of constrain to axis mode.""" 

1586 self._constrain = bool(value) 

1587 

1588 def down(self, point): 

1589 """Set initial cursor window coordinates and pick constrain-axis.""" 

1590 self._vdown = arcball_map_to_sphere(point, self._center, self._radius) 

1591 self._qdown = self._qpre = self._qnow 

1592 if self._constrain and self._axes is not None: 

1593 self._axis = arcball_nearest_axis(self._vdown, self._axes) 

1594 self._vdown = arcball_constrain_to_axis(self._vdown, self._axis) 

1595 else: 

1596 self._axis = None 

1597 

1598 def drag(self, point): 

1599 """Update current cursor window coordinates.""" 

1600 vnow = arcball_map_to_sphere(point, self._center, self._radius) 

1601 if self._axis is not None: 

1602 vnow = arcball_constrain_to_axis(vnow, self._axis) 

1603 self._qpre = self._qnow 

1604 t = numpy.cross(self._vdown, vnow) 

1605 if numpy.dot(t, t) < _EPS: 

1606 self._qnow = self._qdown 

1607 else: 

1608 q = [numpy.dot(self._vdown, vnow), t[0], t[1], t[2]] 

1609 self._qnow = quaternion_multiply(q, self._qdown) 

1610 

1611 def next(self, acceleration=0.0): 

1612 """Continue rotation in direction of last drag.""" 

1613 q = quaternion_slerp(self._qpre, self._qnow, 2.0+acceleration, False) 

1614 self._qpre, self._qnow = self._qnow, q 

1615 

1616 def matrix(self): 

1617 """Return homogeneous rotation matrix.""" 

1618 return quaternion_matrix(self._qnow) 

1619 

1620 

1621def arcball_map_to_sphere(point, center, radius): 

1622 """Return unit sphere coordinates from window coordinates.""" 

1623 v0 = (point[0] - center[0]) / radius 

1624 v1 = (center[1] - point[1]) / radius 

1625 n = v0*v0 + v1*v1 

1626 if n > 1.0: 

1627 # position outside of sphere 

1628 n = math.sqrt(n) 

1629 return numpy.array([v0/n, v1/n, 0.0]) 

1630 else: 

1631 return numpy.array([v0, v1, math.sqrt(1.0 - n)]) 

1632 

1633 

1634def arcball_constrain_to_axis(point, axis): 

1635 """Return sphere point perpendicular to axis.""" 

1636 v = numpy.array(point, dtype=numpy.float64, copy=True) 

1637 a = numpy.array(axis, dtype=numpy.float64, copy=True) 

1638 v -= a * numpy.dot(a, v) # on plane 

1639 n = vector_norm(v) 

1640 if n > _EPS: 

1641 if v[2] < 0.0: 

1642 numpy.negative(v, v) 

1643 v /= n 

1644 return v 

1645 if a[2] == 1.0: 

1646 return numpy.array([1.0, 0.0, 0.0]) 

1647 return unit_vector([-a[1], a[0], 0.0]) 

1648 

1649 

1650def arcball_nearest_axis(point, axes): 

1651 """Return axis, which arc is nearest to point.""" 

1652 point = numpy.array(point, dtype=numpy.float64, copy=False) 

1653 nearest = None 

1654 mx = -1.0 

1655 for axis in axes: 

1656 t = numpy.dot(arcball_constrain_to_axis(point, axis), point) 

1657 if t > mx: 

1658 nearest = axis 

1659 mx = t 

1660 return nearest 

1661 

1662 

1663# epsilon for testing whether a number is close to zero 

1664_EPS = numpy.finfo(float).eps * 4.0 

1665 

1666# axis sequences for Euler angles 

1667_NEXT_AXIS = [1, 2, 0, 1] 

1668 

1669# map axes strings to/from tuples of inner axis, parity, repetition, frame 

1670_AXES2TUPLE = { 

1671 'sxyz': (0, 0, 0, 0), 'sxyx': (0, 0, 1, 0), 'sxzy': (0, 1, 0, 0), 

1672 'sxzx': (0, 1, 1, 0), 'syzx': (1, 0, 0, 0), 'syzy': (1, 0, 1, 0), 

1673 'syxz': (1, 1, 0, 0), 'syxy': (1, 1, 1, 0), 'szxy': (2, 0, 0, 0), 

1674 'szxz': (2, 0, 1, 0), 'szyx': (2, 1, 0, 0), 'szyz': (2, 1, 1, 0), 

1675 'rzyx': (0, 0, 0, 1), 'rxyx': (0, 0, 1, 1), 'ryzx': (0, 1, 0, 1), 

1676 'rxzx': (0, 1, 1, 1), 'rxzy': (1, 0, 0, 1), 'ryzy': (1, 0, 1, 1), 

1677 'rzxy': (1, 1, 0, 1), 'ryxy': (1, 1, 1, 1), 'ryxz': (2, 0, 0, 1), 

1678 'rzxz': (2, 0, 1, 1), 'rxyz': (2, 1, 0, 1), 'rzyz': (2, 1, 1, 1)} 

1679 

1680_TUPLE2AXES = dict((v, k) for k, v in _AXES2TUPLE.items()) 

1681 

1682 

1683def vector_norm(data, axis=None, out=None): 

1684 """Return length, i.e. Euclidean norm, of ndarray along axis. 

1685 

1686 >>> v = numpy.random.random(3) 

1687 >>> n = vector_norm(v) 

1688 >>> numpy.allclose(n, numpy.linalg.norm(v)) 

1689 True 

1690 >>> v = numpy.random.rand(6, 5, 3) 

1691 >>> n = vector_norm(v, axis=-1) 

1692 >>> numpy.allclose(n, numpy.sqrt(numpy.sum(v*v, axis=2))) 

1693 True 

1694 >>> n = vector_norm(v, axis=1) 

1695 >>> numpy.allclose(n, numpy.sqrt(numpy.sum(v*v, axis=1))) 

1696 True 

1697 >>> v = numpy.random.rand(5, 4, 3) 

1698 >>> n = numpy.empty((5, 3)) 

1699 >>> vector_norm(v, axis=1, out=n) 

1700 >>> numpy.allclose(n, numpy.sqrt(numpy.sum(v*v, axis=1))) 

1701 True 

1702 >>> vector_norm([]) 

1703 0.0 

1704 >>> vector_norm([1]) 

1705 1.0 

1706 

1707 """ 

1708 data = numpy.array(data, dtype=numpy.float64, copy=True) 

1709 if out is None: 

1710 if data.ndim == 1: 

1711 return math.sqrt(numpy.dot(data, data)) 

1712 data *= data 

1713 out = numpy.atleast_1d(numpy.sum(data, axis=axis)) 

1714 numpy.sqrt(out, out) 

1715 return out 

1716 else: 

1717 data *= data 

1718 numpy.sum(data, axis=axis, out=out) 

1719 numpy.sqrt(out, out) 

1720 

1721 

1722def unit_vector(data, axis=None, out=None): 

1723 """Return ndarray normalized by length, i.e. Euclidean norm, along axis. 

1724 

1725 >>> v0 = numpy.random.random(3) 

1726 >>> v1 = unit_vector(v0) 

1727 >>> numpy.allclose(v1, v0 / numpy.linalg.norm(v0)) 

1728 True 

1729 >>> v0 = numpy.random.rand(5, 4, 3) 

1730 >>> v1 = unit_vector(v0, axis=-1) 

1731 >>> v2 = v0 / numpy.expand_dims(numpy.sqrt(numpy.sum(v0*v0, axis=2)), 2) 

1732 >>> numpy.allclose(v1, v2) 

1733 True 

1734 >>> v1 = unit_vector(v0, axis=1) 

1735 >>> v2 = v0 / numpy.expand_dims(numpy.sqrt(numpy.sum(v0*v0, axis=1)), 1) 

1736 >>> numpy.allclose(v1, v2) 

1737 True 

1738 >>> v1 = numpy.empty((5, 4, 3)) 

1739 >>> unit_vector(v0, axis=1, out=v1) 

1740 >>> numpy.allclose(v1, v2) 

1741 True 

1742 >>> list(unit_vector([])) 

1743 [] 

1744 >>> list(unit_vector([1])) 

1745 [1.0] 

1746 

1747 """ 

1748 if out is None: 

1749 data = numpy.array(data, dtype=numpy.float64, copy=True) 

1750 if data.ndim == 1: 

1751 data /= math.sqrt(numpy.dot(data, data)) 

1752 return data 

1753 else: 

1754 if out is not data: 

1755 out = numpy.array(data, copy=False) 

1756 data = out 

1757 length = numpy.atleast_1d(numpy.sum(data*data, axis)) 

1758 numpy.sqrt(length, length) 

1759 if axis is not None: 

1760 length = numpy.expand_dims(length, axis) 

1761 data /= length 

1762 if out is None: 

1763 return data 

1764 

1765 

1766def random_vector(size): 

1767 """Return array of random doubles in the half-open interval [0.0, 1.0). 

1768 

1769 >>> v = random_vector(10000) 

1770 >>> numpy.all(v >= 0) and numpy.all(v < 1) 

1771 True 

1772 >>> v0 = random_vector(10) 

1773 >>> v1 = random_vector(10) 

1774 >>> numpy.any(v0 == v1) 

1775 False 

1776 

1777 """ 

1778 return numpy.random.random(size) 

1779 

1780 

1781def vector_product(v0, v1, axis=0): 

1782 """Return vector perpendicular to vectors. 

1783 

1784 >>> v = vector_product([2, 0, 0], [0, 3, 0]) 

1785 >>> numpy.allclose(v, [0, 0, 6]) 

1786 True 

1787 >>> v0 = [[2, 0, 0, 2], [0, 2, 0, 2], [0, 0, 2, 2]] 

1788 >>> v1 = [[3], [0], [0]] 

1789 >>> v = vector_product(v0, v1) 

1790 >>> numpy.allclose(v, [[0, 0, 0, 0], [0, 0, 6, 6], [0, -6, 0, -6]]) 

1791 True 

1792 >>> v0 = [[2, 0, 0], [2, 0, 0], [0, 2, 0], [2, 0, 0]] 

1793 >>> v1 = [[0, 3, 0], [0, 0, 3], [0, 0, 3], [3, 3, 3]] 

1794 >>> v = vector_product(v0, v1, axis=1) 

1795 >>> numpy.allclose(v, [[0, 0, 6], [0, -6, 0], [6, 0, 0], [0, -6, 6]]) 

1796 True 

1797 

1798 """ 

1799 return numpy.cross(v0, v1, axis=axis) 

1800 

1801 

1802def angle_between_vectors(v0, v1, directed=True, axis=0): 

1803 """Return angle between vectors. 

1804 

1805 If directed is False, the input vectors are interpreted as undirected axes, 

1806 i.e. the maximum angle is pi/2. 

1807 

1808 >>> a = angle_between_vectors([1, -2, 3], [-1, 2, -3]) 

1809 >>> numpy.allclose(a, math.pi) 

1810 True 

1811 >>> a = angle_between_vectors([1, -2, 3], [-1, 2, -3], directed=False) 

1812 >>> numpy.allclose(a, 0) 

1813 True 

1814 >>> v0 = [[2, 0, 0, 2], [0, 2, 0, 2], [0, 0, 2, 2]] 

1815 >>> v1 = [[3], [0], [0]] 

1816 >>> a = angle_between_vectors(v0, v1) 

1817 >>> numpy.allclose(a, [0, 1.5708, 1.5708, 0.95532]) 

1818 True 

1819 >>> v0 = [[2, 0, 0], [2, 0, 0], [0, 2, 0], [2, 0, 0]] 

1820 >>> v1 = [[0, 3, 0], [0, 0, 3], [0, 0, 3], [3, 3, 3]] 

1821 >>> a = angle_between_vectors(v0, v1, axis=1) 

1822 >>> numpy.allclose(a, [1.5708, 1.5708, 1.5708, 0.95532]) 

1823 True 

1824 

1825 """ 

1826 v0 = numpy.array(v0, dtype=numpy.float64, copy=False) 

1827 v1 = numpy.array(v1, dtype=numpy.float64, copy=False) 

1828 dot = numpy.sum(v0 * v1, axis=axis) 

1829 dot /= vector_norm(v0, axis=axis) * vector_norm(v1, axis=axis) 

1830 return numpy.arccos(dot if directed else numpy.fabs(dot)) 

1831 

1832 

1833def inverse_matrix(matrix): 

1834 """Return inverse of square transformation matrix. 

1835 

1836 >>> M0 = random_rotation_matrix() 

1837 >>> M1 = inverse_matrix(M0.T) 

1838 >>> numpy.allclose(M1, numpy.linalg.inv(M0.T)) 

1839 True 

1840 >>> for size in range(1, 7): 

1841 ... M0 = numpy.random.rand(size, size) 

1842 ... M1 = inverse_matrix(M0) 

1843 ... if not numpy.allclose(M1, numpy.linalg.inv(M0)): print(size) 

1844 

1845 """ 

1846 return numpy.linalg.inv(matrix) 

1847 

1848 

1849def concatenate_matrices(*matrices): 

1850 """Return concatenation of series of transformation matrices. 

1851 

1852 >>> M = numpy.random.rand(16).reshape((4, 4)) - 0.5 

1853 >>> numpy.allclose(M, concatenate_matrices(M)) 

1854 True 

1855 >>> numpy.allclose(numpy.dot(M, M.T), concatenate_matrices(M, M.T)) 

1856 True 

1857 

1858 """ 

1859 M = numpy.identity(4) 

1860 for i in matrices: 

1861 M = numpy.dot(M, i) 

1862 return M 

1863 

1864 

1865def is_same_transform(matrix0, matrix1): 

1866 """Return True if two matrices perform same transformation. 

1867 

1868 >>> is_same_transform(numpy.identity(4), numpy.identity(4)) 

1869 True 

1870 >>> is_same_transform(numpy.identity(4), random_rotation_matrix()) 

1871 False 

1872 

1873 """ 

1874 matrix0 = numpy.array(matrix0, dtype=numpy.float64, copy=True) 

1875 matrix0 /= matrix0[3, 3] 

1876 matrix1 = numpy.array(matrix1, dtype=numpy.float64, copy=True) 

1877 matrix1 /= matrix1[3, 3] 

1878 return numpy.allclose(matrix0, matrix1) 

1879 

1880 

1881def _import_module(name, package=None, warn=True, prefix='_py_', ignore='_'): 

1882 """Try import all public attributes from module into global namespace. 

1883 

1884 Existing attributes with name clashes are renamed with prefix. 

1885 Attributes starting with underscore are ignored by default. 

1886 

1887 Return True on successful import. 

1888 

1889 """ 

1890 import warnings 

1891 from importlib import import_module 

1892 try: 

1893 if not package: 

1894 module = import_module(name) 

1895 else: 

1896 module = import_module('.' + name, package=package) 

1897 except ImportError: 

1898 if warn: 

1899 warnings.warn("failed to import module %s" % name) 

1900 else: 

1901 for attr in dir(module): 

1902 if ignore and attr.startswith(ignore): 

1903 continue 

1904 if prefix: 

1905 if attr in globals(): 

1906 globals()[prefix + attr] = globals()[attr] 

1907 elif warn: 

1908 warnings.warn("no Python implementation of " + attr) 

1909 globals()[attr] = getattr(module, attr) 

1910 return True 

1911 

1912# _import_module('_transformations') 

1913 

1914# if __name__ == "__main__": 

1915# import doctest 

1916# import random # used in doctests 

1917# numpy.set_printoptions(suppress=True, precision=5) 

1918# doctest.testmod() 

1919#