Coverage for encodermap/misc/transformations.py: 9%

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

2# encodermap/misc/transformations.py

6# Produced at the Laboratory for Fluorescence Dynamics

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.

33"""Homogeneous Transformation Matrices and Quaternions.

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.

41:Author:

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

44:Organization:

45 Laboratory for Fluorescence Dynamics, University of California, Irvine

47:Version: 2013.06.29

49Requirements

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

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

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

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

54 (recommended for speedup of some functions)

56Notes

57-----

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

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

61module for a faster implementation of some functions.

63Documentation in HTML format can be generated with epydoc.

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").

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].

76Calculations are carried out with numpy.float64 precision.

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

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

81Return types are numpy arrays unless specified otherwise.

83Angles are in radians unless specified otherwise.

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

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:

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

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

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

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

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

103References

104----------

105(1) Matrices and transformations. Ronald Goldman.

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

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

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

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

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

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

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

113(5) Euler angle conversion. Ken Shoemake.

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

115(6) Arcball rotation control. Ken Shoemake.

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

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

118 vectors. James Diebel. 2006.

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

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

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

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

123(10) Quaternions. Ken Shoemake.

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

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

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

127(12) Uniform random rotations. Ken Shoemake.

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

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

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

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

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

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

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

135(16) Column Vectors vs. Row Vectors.

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

137

138Examples

139--------

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

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

142>>> I = identity_matrix()

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

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

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

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

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

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

149True

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

151>>> is_same_transform(R, Re)

152True

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

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

155True

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

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

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

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

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

161>>> Rq = quaternion_matrix(q)

162>>> is_same_transform(R, Rq)

163True

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

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

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

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

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

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

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

171True

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

173True

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

175True

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

177True

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

179>>> is_same_transform(M, M1)

180True

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

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

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

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

185True

186

187"""

188

189from __future__ import division, print_function

190

191import math

192

193import numpy

194

195__version__ = "2013.06.29"

196__docformat__ = "restructuredtext en"

197__all__ = []

198

199

200def identity_matrix():

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

202

203 >>> I = identity_matrix()

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

205 True

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

207 (4.0, 4.0)

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

209 True

210

211 """

212 return numpy.identity(4)

213

214

215def translation_matrix(direction):

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

217

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

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

220 True

221

222 """

223 M = numpy.identity(4)

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

225 return M

226

227

228def translation_from_matrix(matrix):

229 """Return translation vector from translation matrix.

230

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

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

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

234 True

235

236 """

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

238

239

240def reflection_matrix(point, normal):

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

242

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

244 >>> v0[3] = 1.

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

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

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

248 True

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

250 True

251 >>> v2 = v0.copy()

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

253 >>> v3 = v0.copy()

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

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

256 True

257

258 """

259 normal = unit_vector(normal[:3])

260 M = numpy.identity(4)

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

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

263 return M

264

265

266def reflection_from_matrix(matrix):

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

268

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

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

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

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

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

274 >>> is_same_transform(M0, M1)

275 True

276

277 """

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

279 # normal: unit eigenvector corresponding to eigenvalue -1

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

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

282 if not len(i):

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

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

285 # point: any unit eigenvector corresponding to eigenvalue 1

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

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

288 if not len(i):

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

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

291 point /= point[3]

292 return point, normal

293

294

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

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

297

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

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

300 True

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

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

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

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

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

306 >>> is_same_transform(R0, R1)

307 True

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

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

310 >>> is_same_transform(R0, R1)

311 True

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

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

314 True

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

316 ... direc, point)))

317 True

318

319 """

320 sina = math.sin(angle)

321 cosa = math.cos(angle)

322 direction = unit_vector(direction[:3])

323 # rotation matrix around unit vector

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

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

326 direction *= sina

327 R += numpy.array(

328 [

329 [0.0, -direction[2], direction[1]],

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

331 [-direction[1], direction[0], 0.0],

332 ]

333 )

334 M = numpy.identity(4)

335 M[:3, :3] = R

336 if point is not None:

337 # rotation not around origin

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

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

340 return M

341

342

343def rotation_from_matrix(matrix):

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

345

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

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

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

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

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

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

352 >>> is_same_transform(R0, R1)

353 True

354

355 """

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

357 R33 = R[:3, :3]

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

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

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

361 if not len(i):

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

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

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

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

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

367 if not len(i):

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

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

370 point /= point[3]

371 # rotation angle depending on direction

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

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

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

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

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

377 else:

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

379 angle = math.atan2(sina, cosa)

380 return angle, direction, point

381

382

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

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

385

386 Use factor -1 for point symmetry.

387

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

389 >>> v[3] = 1

390 >>> S = scale_matrix(-1.234)

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

392 True

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

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

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

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

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

398

399 """

400 if direction is None:

401 # uniform scaling

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

403 if origin is not None:

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

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

406 else:

407 # nonuniform scaling

408 direction = unit_vector(direction[:3])

409 factor = 1.0 - factor

410 M = numpy.identity(4)

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

412 if origin is not None:

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

414 return M

415

416

417def scale_from_matrix(matrix):

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

419

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

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

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

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

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

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

426 >>> is_same_transform(S0, S1)

427 True

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

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

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

431 >>> is_same_transform(S0, S1)

432 True

433

434 """

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

436 M33 = M[:3, :3]

437 factor = numpy.trace(M33) - 2.0

438 try:

439 # direction: unit eigenvector corresponding to eigenvalue factor

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

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

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

443 direction /= vector_norm(direction)

444 except IndexError:

445 # uniform scaling

446 factor = (factor + 2.0) / 3.0

447 direction = None

448 # origin: any eigenvector corresponding to eigenvalue 1

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

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

451 if not len(i):

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

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

454 origin /= origin[3]

455 return factor, origin, direction

456

457

458def projection_matrix(point, normal, direction=None, perspective=None, pseudo=False):

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

460

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

462

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

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

465

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

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

468 True

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

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

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

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

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

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

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

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

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

478 True

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

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

481 >>> v0[3] = 1

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

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

484 True

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

486 True

487

488 """

489 M = numpy.identity(4)

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

491 normal = unit_vector(normal[:3])

492 if perspective is not None:

493 # perspective projection

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

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

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

497 if pseudo:

498 # preserve relative depth

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

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

501 else:

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

503 M[3, :3] = -normal

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

505 elif direction is not None:

506 # parallel projection

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

508 scale = numpy.dot(direction, normal)

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

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

511 else:

512 # orthogonal projection

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

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

515 return M

516

517

518def projection_from_matrix(matrix, pseudo=False):

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

520

521 Return values are same as arguments for projection_matrix function:

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

523

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

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

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

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

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

529 >>> result = projection_from_matrix(P0)

530 >>> P1 = projection_matrix(*result)

531 >>> is_same_transform(P0, P1)

532 True

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

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, perspective=persp, pseudo=False)

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

540 >>> P1 = projection_matrix(*result)

541 >>> is_same_transform(P0, P1)

542 True

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

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

545 >>> P1 = projection_matrix(*result)

546 >>> is_same_transform(P0, P1)

547 True

548

549 """

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

551 M33 = M[:3, :3]

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

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

554 if not pseudo and len(i):

555 # point: any eigenvector corresponding to eigenvalue 1

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

557 point /= point[3]

558 # direction: unit eigenvector corresponding to eigenvalue 0

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

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

561 if not len(i):

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

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

564 direction /= vector_norm(direction)

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

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

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

568 if len(i):

569 # parallel projection

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

571 normal /= vector_norm(normal)

572 return point, normal, direction, None, False

573 else:

574 # orthogonal projection, where normal equals direction vector

575 return point, direction, None, None, False

576 else:

577 # perspective projection

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

579 if not len(i):

580 raise ValueError("no eigenvector not corresponding to eigenvalue 0")

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

582 point /= point[3]

583 normal = -M[3, :3]

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

585 if pseudo:

586 perspective -= normal

587 return point, normal, None, perspective, pseudo

588

589

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

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

592

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

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

595

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

597 inside the frustum.

598

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

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

601 orthographic canonical view volume (a box).

602

603 Homogeneous coordinates transformed by the perspective clip matrix

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

605

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

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

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

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

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

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

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

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

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

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

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

617 >>> v / v[3]

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

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

620 >>> v / v[3]

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

622

623 """

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

625 raise ValueError("invalid frustum")

626 if perspective:

627 if near <= _EPS:

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

629 t = 2.0 * near

630 M = [

631 [t / (left - right), 0.0, (right + left) / (right - left), 0.0],

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

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

634 [0.0, 0.0, -1.0, 0.0],

635 ]

636 else:

637 M = [

638 [2.0 / (right - left), 0.0, 0.0, (right + left) / (left - right)],

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

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

641 [0.0, 0.0, 0.0, 1.0],

642 ]

643 return numpy.array(M)

644

645

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

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

648

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

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

651

652 A point P is transformed by the shear matrix into P" such that

653 the vector P-P" is parallel to the direction vector and its extent is

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

655 of P onto the shear plane.

656

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

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

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

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

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

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

663 True

664

665 """

666 normal = unit_vector(normal[:3])

667 direction = unit_vector(direction[:3])

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

669 raise ValueError(

670 "direction and normal vectors are not orthogonal, diff is %f"

671 % abs(numpy.dot(normal, direction))

672 )

673 angle = math.tan(angle)

674 M = numpy.identity(4)

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

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

677 return M

678

679

680def shear_from_matrix(matrix):

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

682

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

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

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

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

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

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

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

690 >>> is_same_transform(S0, S1)

691 True

692

693 """

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

695 M33 = M[:3, :3]

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

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

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

699 if len(i) < 2:

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

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

702 lenorm = -1.0

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

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

705 w = vector_norm(n)

706 if w > lenorm:

707 lenorm = w

708 normal = n

709 normal /= lenorm

710 # direction and angle

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

712 angle = vector_norm(direction)

713 direction /= angle

714 angle = math.atan(angle)

715 # point: eigenvector corresponding to eigenvalue 1

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

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

718 if not len(i):

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

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

721 point /= point[3]

722 return angle, direction, point, normal

723

724

725def decompose_matrix(matrix):

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

727

728 matrix : array_like

729 Non-degenerative homogeneous transformation matrix

730

731 Return tuple of:

732 scale : vector of 3 scaling factors

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

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

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

736 perspective : perspective partition of matrix

737

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

739

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

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

742 >>> T1 = translation_matrix(trans)

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

744 True

745 >>> S = scale_matrix(0.123)

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

747 >>> scale[0]

748 0.123

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

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

751 >>> R1 = euler_matrix(*angles)

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

753 True

754

755 """

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

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

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

759 M /= M[3, 3]

760 P = M.copy()

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

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

763 raise ValueError("matrix is singular")

764

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

766 shear = [0.0, 0.0, 0.0]

767 angles = [0.0, 0.0, 0.0]

768

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

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

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

772 else:

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

774

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

776 M[3, :3] = 0.0

777

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

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

780 row[0] /= scale[0]

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

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

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

784 row[1] /= scale[1]

785 shear[0] /= scale[1]

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

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

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

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

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

791 row[2] /= scale[2]

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

793

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

795 numpy.negative(scale, scale)

796 numpy.negative(row, row)

797

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

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

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

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

802 else:

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

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

805 angles[2] = 0.0

806

807 return scale, shear, angles, translate, perspective

808

809

810def compose_matrix(

811 scale=None, shear=None, angles=None, translate=None, perspective=None

812):

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

814

815 This is the inverse of the decompose_matrix function.

816

817 Sequence of transformations:

818 scale : vector of 3 scaling factors

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

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

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

822 perspective : perspective partition of matrix

823

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

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

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

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

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

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

830 >>> result = decompose_matrix(M0)

831 >>> M1 = compose_matrix(*result)

832 >>> is_same_transform(M0, M1)

833 True

834

835 """

836 M = numpy.identity(4)

837 if perspective is not None:

838 P = numpy.identity(4)

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

840 M = numpy.dot(M, P)

841 if translate is not None:

842 T = numpy.identity(4)

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

844 M = numpy.dot(M, T)

845 if angles is not None:

846 R = euler_matrix(angles[0], angles[1], angles[2], "sxyz")

847 M = numpy.dot(M, R)

848 if shear is not None:

849 Z = numpy.identity(4)

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

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

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

853 M = numpy.dot(M, Z)

854 if scale is not None:

855 S = numpy.identity(4)

856 S[0, 0] = scale[0]

857 S[1, 1] = scale[1]

858 S[2, 2] = scale[2]

859 M = numpy.dot(M, S)

860 M /= M[3, 3]

861 return M

862

863

864def orthogonalization_matrix(lengths, angles):

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

866

867 Angles are expected in degrees.

868

869 The de-orthogonalization matrix is the inverse.

870

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

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

873 True

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

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

876 True

877

878 """

879 a, b, c = lengths

880 angles = numpy.radians(angles)

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

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

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

884 return numpy.array(

885 [

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

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

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

889 [0.0, 0.0, 0.0, 1.0],

890 ]

891 )

892

893

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

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

896

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

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

899

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

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

902 is returned.

903

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

905 If usesvd is True, similarity and Euclidean transformation matrices

906 are calculated by minimizing the weighted sum of squared deviations

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

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

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

910

911 The returned matrix performs rotation, translation and uniform scaling

912 (if specified).

913

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

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

916 >>> affine_matrix_from_points(v0, v1)

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

918 [ 0.00048, 0.14094, 53.24971],

919 [ 0. , 0. , 1. ]])

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

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

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

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

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

925 >>> v0[3] = 1

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

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

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

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

930 True

931

932 More examples in superimposition_matrix()

933

934 """

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

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

937

938 ndims = v0.shape[0]

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

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

941

942 # move centroids to origin

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

944 M0 = numpy.identity(ndims + 1)

945 M0[:ndims, ndims] = t0

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

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

948 M1 = numpy.identity(ndims + 1)

949 M1[:ndims, ndims] = t1

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

951

952 if shear:

953 # Affine transformation

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

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

956 vh = vh[:ndims].T

957 B = vh[:ndims]

958 C = vh[ndims : 2 * ndims]

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

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

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

962 elif usesvd or ndims != 3:

963 # Rigid transformation via SVD of covariance matrix

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

965 # rotation matrix from SVD orthonormal bases

966 R = numpy.dot(u, vh)

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

968 # R does not constitute right handed system

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

970 s[-1] *= -1.0

971 # homogeneous transformation matrix

972 M = numpy.identity(ndims + 1)

973 M[:ndims, :ndims] = R

974 else:

975 # Rigid transformation matrix via quaternion

976 # compute symmetric matrix N

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

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

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

980 N = [

981 [xx + yy + zz, 0.0, 0.0, 0.0],

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

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

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

985 ]

986 # quaternion: eigenvector corresponding to most positive eigenvalue

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

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

989 q /= vector_norm(q) # unit quaternion

990 # homogeneous transformation matrix

991 M = quaternion_matrix(q)

992

993 if scale and not shear:

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

995 v0 *= v0

996 v1 *= v1

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

998

999 # move centroids back

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

1001 M /= M[ndims, ndims]

1002 return M

1003

1004

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

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

1007

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

1009

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

1011 affine_matrix_from_points function.

1012

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

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

1015

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

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

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

1019 True

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

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

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

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

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

1025 True

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

1027 >>> v0[3] = 1

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

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

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

1031 True

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

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

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

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

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

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

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

1039 True

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

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

1042 True

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

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

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

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

1047 True

1048

1049 """

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

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

1052 return affine_matrix_from_points(v0, v1, shear=False, scale=scale, usesvd=usesvd)

1053

1054

1055def euler_matrix(ai, aj, ak, axes="sxyz"):

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

1057

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

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

1060

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

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

1063 True

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

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

1066 True

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

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

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

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

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

1072

1073 """

1074 try:

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

1076 except (AttributeError, KeyError):

1077 _TUPLE2AXES[axes] # validation

1078 firstaxis, parity, repetition, frame = axes

1079

1080 i = firstaxis

1081 j = _NEXT_AXIS[i + parity]

1082 k = _NEXT_AXIS[i - parity + 1]

1083

1084 if frame:

1085 ai, ak = ak, ai

1086 if parity:

1087 ai, aj, ak = -ai, -aj, -ak

1088

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

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

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

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

1093

1094 M = numpy.identity(4)

1095 if repetition:

1096 M[i, i] = cj

1097 M[i, j] = sj * si

1098 M[i, k] = sj * ci

1099 M[j, i] = sj * sk

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

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

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

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

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

1105 else:

1106 M[i, i] = cj * ck

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

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

1109 M[j, i] = cj * sk

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

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

1112 M[k, i] = -sj

1113 M[k, j] = cj * si

1114 M[k, k] = cj * ci

1115 return M

1116

1117

1118def euler_from_matrix(matrix, axes="sxyz"):

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

1120

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

1122

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

1124

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

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

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

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

1129 True

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

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

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

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

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

1135

1136 """

1137 try:

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

1139 except (AttributeError, KeyError):

1140 _TUPLE2AXES[axes] # validation

1141 firstaxis, parity, repetition, frame = axes

1142

1143 i = firstaxis

1144 j = _NEXT_AXIS[i + parity]

1145 k = _NEXT_AXIS[i - parity + 1]

1146

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

1148 if repetition:

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

1150 if sy > _EPS:

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

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

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

1154 else:

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

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

1157 az = 0.0

1158 else:

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

1160 if cy > _EPS:

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

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

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

1164 else:

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

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

1167 az = 0.0

1168

1169 if parity:

1170 ax, ay, az = -ax, -ay, -az

1171 if frame:

1172 ax, az = az, ax

1173 return ax, ay, az

1174

1175

1176def euler_from_quaternion(quaternion, axes="sxyz"):

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

1178

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

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

1181 True

1182

1183 """

1184 return euler_from_matrix(quaternion_matrix(quaternion), axes)

1185

1186

1187def quaternion_from_euler(ai, aj, ak, axes="sxyz"):

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

1189

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

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

1192

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

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

1195 True

1196

1197 """

1198 try:

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

1200 except (AttributeError, KeyError):

1201 _TUPLE2AXES[axes] # validation

1202 firstaxis, parity, repetition, frame = axes

1203

1204 i = firstaxis + 1

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

1206 k = _NEXT_AXIS[i - parity] + 1

1207

1208 if frame:

1209 ai, ak = ak, ai

1210 if parity:

1211 aj = -aj

1212

1213 ai /= 2.0

1214 aj /= 2.0

1215 ak /= 2.0

1216 ci = math.cos(ai)

1217 si = math.sin(ai)

1218 cj = math.cos(aj)

1219 sj = math.sin(aj)

1220 ck = math.cos(ak)

1221 sk = math.sin(ak)

1222 cc = ci * ck

1223 cs = ci * sk

1224 sc = si * ck

1225 ss = si * sk

1226

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

1228 if repetition:

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

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

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

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

1233 else:

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

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

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

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

1238 if parity:

1239 q[j] *= -1.0

1240

1241 return q

1242

1243

1244def quaternion_about_axis(angle, axis):

1245 """Return quaternion for rotation about axis.

1246

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

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

1249 True

1250

1251 """

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

1253 qlen = vector_norm(q)

1254 if qlen > _EPS:

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

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

1257 return q

1258

1259

1260def quaternion_matrix(quaternion):

1261 """Return homogeneous rotation matrix from quaternion.

1262

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

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

1265 True

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

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

1268 True

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

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

1271 True

1272

1273 """

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

1275 n = numpy.dot(q, q)

1276 if n < _EPS:

1277 return numpy.identity(4)

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

1279 q = numpy.outer(q, q)

1280 return numpy.array(

1281 [

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

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

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

1285 [0.0, 0.0, 0.0, 1.0],

1286 ]

1287 )

1288

1289

1290def quaternion_from_matrix(matrix, isprecise=False):

1291 """Return quaternion from rotation matrix.

1292

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

1294 matrix and a faster algorithm is used.

1295

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

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

1298 True

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

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

1301 True

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

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

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

1305 True

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

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

1308 >>> q = quaternion_from_matrix(R)

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

1310 True

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

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

1313 >>> q = quaternion_from_matrix(R)

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

1315 True

1316 >>> R = random_rotation_matrix()

1317 >>> q = quaternion_from_matrix(R)

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

1319 True

1320

1321 """

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

1323 if isprecise:

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

1325 t = numpy.trace(M)

1326 if t > M[3, 3]:

1327 q[0] = t

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

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

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

1331 else:

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

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

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

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

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

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

1338 q[i] = t

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

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

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

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

1343 else:

1344 m00 = M[0, 0]

1345 m01 = M[0, 1]

1346 m02 = M[0, 2]

1347 m10 = M[1, 0]

1348 m11 = M[1, 1]

1349 m12 = M[1, 2]

1350 m20 = M[2, 0]

1351 m21 = M[2, 1]

1352 m22 = M[2, 2]

1353 # symmetric matrix K

1354 K = numpy.array(

1355 [

1356 [m00 - m11 - m22, 0.0, 0.0, 0.0],

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

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

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

1360 ]

1361 )

1362 K /= 3.0

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

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

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

1366 if q[0] < 0.0:

1367 numpy.negative(q, q)

1368 return q

1369

1370

1371def quaternion_multiply(quaternion1, quaternion0):

1372 """Return multiplication of two quaternions.

1373

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

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

1376 True

1377

1378 """

1379 w0, x0, y0, z0 = quaternion0

1380 w1, x1, y1, z1 = quaternion1

1381 return numpy.array(

1382 [

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

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

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

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

1387 ],

1388 dtype=numpy.float64,

1389 )

1390

1391

1392def quaternion_conjugate(quaternion):

1393 """Return conjugate of quaternion.

1394

1395 >>> q0 = random_quaternion()

1396 >>> q1 = quaternion_conjugate(q0)

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

1398 True

1399

1400 """

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

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

1403 return q

1404

1405

1406def quaternion_inverse(quaternion):

1407 """Return inverse of quaternion.

1408

1409 >>> q0 = random_quaternion()

1410 >>> q1 = quaternion_inverse(q0)

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

1412 True

1413

1414 """

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

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

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

1418

1419

1420def quaternion_real(quaternion):

1421 """Return real part of quaternion.

1422

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

1424 3.0

1425

1426 """

1427 return float(quaternion[0])

1428

1429

1430def quaternion_imag(quaternion):

1431 """Return imaginary part of quaternion.

1432

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

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

1435

1436 """

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

1438

1439

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

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

1442

1443 >>> q0 = random_quaternion()

1444 >>> q1 = random_quaternion()

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

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

1447 True

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

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

1450 True

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

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

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

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

1455 True

1456

1457 """

1458 q0 = unit_vector(quat0[:4])

1459 q1 = unit_vector(quat1[:4])

1460 if fraction == 0.0:

1461 return q0

1462 elif fraction == 1.0:

1463 return q1

1464 d = numpy.dot(q0, q1)

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

1466 return q0

1467 if shortestpath and d < 0.0:

1468 # invert rotation

1469 d = -d

1470 numpy.negative(q1, q1)

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

1472 if abs(angle) < _EPS:

1473 return q0

1474 isin = 1.0 / math.sin(angle)

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

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

1477 q0 += q1

1478 return q0

1479

1480

1481def random_quaternion(rand=None):

1482 """Return uniform random unit quaternion.

1483

1484 rand: array like or None

1485 Three independent random variables that are uniformly distributed

1486 between 0 and 1.

1487

1488 >>> q = random_quaternion()

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

1490 True

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

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

1493 (1, True)

1494

1495 """

1496 if rand is None:

1497 rand = numpy.random.rand(3)

1498 else:

1499 assert len(rand) == 3

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

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

1502 pi2 = math.pi * 2.0

1503 t1 = pi2 * rand[1]

1504 t2 = pi2 * rand[2]

1505 return numpy.array(

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

1507 )

1508

1509

1510def random_rotation_matrix(rand=None):

1511 """Return uniform random rotation matrix.

1512

1513 rand: array like

1514 Three independent random variables that are uniformly distributed

1515 between 0 and 1 for each returned quaternion.

1516

1517 >>> R = random_rotation_matrix()

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

1519 True

1520

1521 """

1522 return quaternion_matrix(random_quaternion(rand))

1523

1524

1525class Arcball(object):

1526 """Virtual Trackball Control.

1527

1528 >>> ball = Arcball()

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

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

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

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

1533 >>> R = ball.matrix()

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

1535 True

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

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

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

1539 >>> ball.constrain = True

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

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

1542 >>> R = ball.matrix()

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

1544 True

1545 >>> ball.next()

1546

1547 """

1548

1549 def __init__(self, initial=None):

1550 """Initialize virtual trackball control.

1551

1552 initial : quaternion or rotation matrix

1553

1554 """

1555 self._axis = None

1556 self._axes = None

1557 self._radius = 1.0

1558 self._center = [0.0, 0.0]

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

1560 self._constrain = False

1561 if initial is None:

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

1563 else:

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

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

1566 self._qdown = quaternion_from_matrix(initial)

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

1568 initial /= vector_norm(initial)

1569 self._qdown = initial

1570 else:

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

1572 self._qnow = self._qpre = self._qdown

1573

1574 def place(self, center, radius):

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

1576

1577 center : sequence[2]

1578 Window coordinates of trackball center.

1579 radius : float

1580 Radius of trackball in window coordinates.

1581

1582 """

1583 self._radius = float(radius)

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

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

1586

1587 def setaxes(self, *axes):

1588 """Set axes to constrain rotations."""

1589 if axes is None:

1590 self._axes = None

1591 else:

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

1593

1594 @property

1595 def constrain(self):

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

1597 return self._constrain

1598

1599 @constrain.setter

1600 def constrain(self, value):

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

1602 self._constrain = bool(value)

1603

1604 def down(self, point):

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

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

1607 self._qdown = self._qpre = self._qnow

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

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

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

1611 else:

1612 self._axis = None

1613

1614 def drag(self, point):

1615 """Update current cursor window coordinates."""

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

1617 if self._axis is not None:

1618 vnow = arcball_constrain_to_axis(vnow, self._axis)

1619 self._qpre = self._qnow

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

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

1622 self._qnow = self._qdown

1623 else:

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

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

1626

1627 def next(self, acceleration=0.0):

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

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

1630 self._qpre, self._qnow = self._qnow, q

1631

1632 def matrix(self):

1633 """Return homogeneous rotation matrix."""

1634 return quaternion_matrix(self._qnow)

1635

1636

1637def arcball_map_to_sphere(point, center, radius):

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

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

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

1641 n = v0 * v0 + v1 * v1

1642 if n > 1.0:

1643 # position outside of sphere

1644 n = math.sqrt(n)

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

1646 else:

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

1648

1649

1650def arcball_constrain_to_axis(point, axis):

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

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

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

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

1655 n = vector_norm(v)

1656 if n > _EPS:

1657 if v[2] < 0.0:

1658 numpy.negative(v, v)

1659 v /= n

1660 return v

1661 if a[2] == 1.0:

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

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

1664

1665

1666def arcball_nearest_axis(point, axes):

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

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

1669 nearest = None

1670 mx = -1.0

1671 for axis in axes:

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

1673 if t > mx:

1674 nearest = axis

1675 mx = t

1676 return nearest

1677

1678

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

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

1681

1682# axis sequences for Euler angles

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

1684

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

1686_AXES2TUPLE = {

1687 "sxyz": (0, 0, 0, 0),

1688 "sxyx": (0, 0, 1, 0),

1689 "sxzy": (0, 1, 0, 0),

1690 "sxzx": (0, 1, 1, 0),

1691 "syzx": (1, 0, 0, 0),

1692 "syzy": (1, 0, 1, 0),

1693 "syxz": (1, 1, 0, 0),

1694 "syxy": (1, 1, 1, 0),

1695 "szxy": (2, 0, 0, 0),

1696 "szxz": (2, 0, 1, 0),

1697 "szyx": (2, 1, 0, 0),

1698 "szyz": (2, 1, 1, 0),

1699 "rzyx": (0, 0, 0, 1),

1700 "rxyx": (0, 0, 1, 1),

1701 "ryzx": (0, 1, 0, 1),

1702 "rxzx": (0, 1, 1, 1),

1703 "rxzy": (1, 0, 0, 1),

1704 "ryzy": (1, 0, 1, 1),

1705 "rzxy": (1, 1, 0, 1),

1706 "ryxy": (1, 1, 1, 1),

1707 "ryxz": (2, 0, 0, 1),

1708 "rzxz": (2, 0, 1, 1),

1709 "rxyz": (2, 1, 0, 1),

1710 "rzyz": (2, 1, 1, 1),

1711}

1712

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

1714

1715

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

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

1718

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

1720 >>> n = vector_norm(v)

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

1722 True

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

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

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

1726 True

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

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

1729 True

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

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

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

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

1734 True

1735 >>> vector_norm([])

1736 0.0

1737 >>> vector_norm([1])

1738 1.0

1739

1740 """

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

1742 if out is None:

1743 if data.ndim == 1:

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

1745 data *= data

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

1747 numpy.sqrt(out, out)

1748 return out

1749 else:

1750 data *= data

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

1752 numpy.sqrt(out, out)

1753

1754

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

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

1757

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

1759 >>> v1 = unit_vector(v0)

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

1761 True

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

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

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

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

1766 True

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

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

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

1770 True

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

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

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

1774 True

1775 >>> list(unit_vector([]))

1776 []

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

1778 [1.0]

1779

1780 """

1781 if out is None:

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

1783 if data.ndim == 1:

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

1785 return data

1786 else:

1787 if out is not data:

1788 out[:] = numpy.array(data, copy=False)

1789 data = out

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

1791 numpy.sqrt(length, length)

1792 if axis is not None:

1793 length = numpy.expand_dims(length, axis)

1794 data /= length

1795 if out is None:

1796 return data

1797

1798

1799def random_vector(size):

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

1801

1802 >>> v = random_vector(10000)

1803 >>> numpy.all(v >= 0) and numpy.all(v < 1)

1804 True

1805 >>> v0 = random_vector(10)

1806 >>> v1 = random_vector(10)

1807 >>> numpy.any(v0 == v1)

1808 False

1809

1810 """

1811 return numpy.random.random(size)

1812

1813

1814def vector_product(v0, v1, axis=0):

1815 """Return vector perpendicular to vectors.

1816

1817 >>> v = vector_product([2, 0, 0], [0, 3, 0])

1818 >>> numpy.allclose(v, [0, 0, 6])

1819 True

1820 >>> v0 = [[2, 0, 0, 2], [0, 2, 0, 2], [0, 0, 2, 2]]

1821 >>> v1 = [[3], [0], [0]]

1822 >>> v = vector_product(v0, v1)

1823 >>> numpy.allclose(v, [[0, 0, 0, 0], [0, 0, 6, 6], [0, -6, 0, -6]])

1824 True

1825 >>> v0 = [[2, 0, 0], [2, 0, 0], [0, 2, 0], [2, 0, 0]]

1826 >>> v1 = [[0, 3, 0], [0, 0, 3], [0, 0, 3], [3, 3, 3]]

1827 >>> v = vector_product(v0, v1, axis=1)

1828 >>> numpy.allclose(v, [[0, 0, 6], [0, -6, 0], [6, 0, 0], [0, -6, 6]])

1829 True

1830

1831 """

1832 return numpy.cross(v0, v1, axis=axis)

1833

1834

1835def angle_between_vectors(v0, v1, directed=True, axis=0):

1836 """Return angle between vectors.

1837

1838 If directed is False, the input vectors are interpreted as undirected axes,

1839 i.e. the maximum angle is pi/2.

1840

1841 >>> a = angle_between_vectors([1, -2, 3], [-1, 2, -3])

1842 >>> numpy.allclose(a, math.pi)

1843 True

1844 >>> a = angle_between_vectors([1, -2, 3], [-1, 2, -3], directed=False)

1845 >>> numpy.allclose(a, 0)

1846 True

1847 >>> v0 = [[2, 0, 0, 2], [0, 2, 0, 2], [0, 0, 2, 2]]

1848 >>> v1 = [[3], [0], [0]]

1849 >>> a = angle_between_vectors(v0, v1)

1850 >>> numpy.allclose(a, [0, 1.5708, 1.5708, 0.95532])

1851 True

1852 >>> v0 = [[2, 0, 0], [2, 0, 0], [0, 2, 0], [2, 0, 0]]

1853 >>> v1 = [[0, 3, 0], [0, 0, 3], [0, 0, 3], [3, 3, 3]]

1854 >>> a = angle_between_vectors(v0, v1, axis=1)

1855 >>> numpy.allclose(a, [1.5708, 1.5708, 1.5708, 0.95532])

1856 True

1857

1858 """

1859 v0 = numpy.array(v0, dtype=numpy.float64, copy=False)

1860 v1 = numpy.array(v1, dtype=numpy.float64, copy=False)

1861 dot = numpy.sum(v0 * v1, axis=axis)

1862 dot /= vector_norm(v0, axis=axis) * vector_norm(v1, axis=axis)

1863 return numpy.arccos(dot if directed else numpy.fabs(dot))

1864

1865

1866def inverse_matrix(matrix):

1867 """Return inverse of square transformation matrix.

1868

1869 >>> M0 = random_rotation_matrix()

1870 >>> M1 = inverse_matrix(M0.T)

1871 >>> numpy.allclose(M1, numpy.linalg.inv(M0.T))

1872 True

1873 >>> for size in range(1, 7):

1874 ... M0 = numpy.random.rand(size, size)

1875 ... M1 = inverse_matrix(M0)

1876 ... if not numpy.allclose(M1, numpy.linalg.inv(M0)): print(size)

1877

1878 """

1879 return numpy.linalg.inv(matrix)

1880

1881

1882def concatenate_matrices(*matrices):

1883 """Return concatenation of series of transformation matrices.

1884

1885 >>> M = numpy.random.rand(16).reshape((4, 4)) - 0.5

1886 >>> numpy.allclose(M, concatenate_matrices(M))

1887 True

1888 >>> numpy.allclose(numpy.dot(M, M.T), concatenate_matrices(M, M.T))

1889 True

1890

1891 """

1892 M = numpy.identity(4)

1893 for i in matrices:

1894 M = numpy.dot(M, i)

1895 return M

1896

1897

1898def apply_transform(matrix, points):

1899 """Return 3D cartesian points after applying transform.

1900

1901 The points are converted to 3D Homogeneous, the transformation

1902 is applied, and then they are converted back to Cartesian"""

1903 if points.shape[0] != 3:

1904 raise ValueError("points must be 3xN")

1905 points = numpy.vstack([points, numpy.ones((1, points.shape[1]))])

1906 tformed_points = numpy.dot(matrix, points)

1907 tformed_points = tformed_points[:3, :] / tformed_points[3, :]

1908 return tformed_points

1909

1910

1911def is_same_transform(matrix0, matrix1):

1912 """Return True if two matrices perform same transformation.

1913

1914 >>> is_same_transform(numpy.identity(4), numpy.identity(4))

1915 True

1916 >>> is_same_transform(numpy.identity(4), random_rotation_matrix())

1917 False

1918

1919 """

1920 matrix0 = numpy.array(matrix0, dtype=numpy.float64, copy=True)

1921 matrix0 /= matrix0[3, 3]

1922 matrix1 = numpy.array(matrix1, dtype=numpy.float64, copy=True)

1923 matrix1 /= matrix1[3, 3]

1924 return numpy.allclose(matrix0, matrix1)

1925

1926

1927def _import_module(name, package=None, warn=False, prefix="_py_", ignore="_"):

1928 """Try import all public attributes from module into global namespace.

1929

1930 Existing attributes with name clashes are renamed with prefix.

1931 Attributes starting with underscore are ignored by default.

1932

1933 Return True on successful import.

1934

1935 """

1936 import warnings

1937 from importlib import import_module

1938

1939 try:

1940 if not package: 1940 ↛ 1943line 1940 didn't jump to line 1943, because the condition on line 1940 was never false

1941 module = import_module(name)

1942 else:

1943 module = import_module("." + name, package=package)

1944 except ImportError:

1945 if warn: 1945 ↛ 1946line 1945 didn't jump to line 1946, because the condition on line 1945 was never true

1946 warnings.warn("failed to import module %s" % name)

1947 else:

1948 for attr in dir(module):

1949 if ignore and attr.startswith(ignore):

1950 continue

1951 if prefix:

1952 if attr in globals():

1953 globals()[prefix + attr] = globals()[attr]

1954 elif warn:

1955 warnings.warn("no Python implementation of " + attr)

1956 globals()[attr] = getattr(module, attr)

1957 return True

1958

1959

1960# print("about to do import_module, id(inverse_matrix) = %x" % id(inverse_matrix))

1961_import_module("transformations._transformations")

1962# print("after done import_module, id(inverse_matrix) = %x" % id(inverse_matrix))

1963

1964if __name__ == "__main__":

1965 import doctest

1966 import random # used in doctests

1967

1968 numpy.set_printoptions(suppress=True, precision=5)

1969 doctest.testmod()