Coverage for trimesh/transformations.py: 93%

1# transformations.py

3# Modified for inclusion in the `trimesh` library

4# https://github.com/mikedh/trimesh

5# -----------------------------------------------------------------------

9# Produced at the Laboratory for Fluorescence Dynamics

11#

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

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

14#

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

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

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

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

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

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

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

22# from this software without specific prior written permission.

23#

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

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

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

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

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

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

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

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

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

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

34# POSSIBILITY OF SUCH DAMAGE.

36"""Homogeneous Transformation Matrices and Quaternions.

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

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

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

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

42functions to decompose transformation matrices.

44:Author:

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

47:Organization:

48 Laboratory for Fluorescence Dynamics, University of California, Irvine

50:Version: 2017.02.17

52Requirements

53------------

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

55* `numpy 1.9 <http://www.np.org>`_

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

57 (recommended for speedup of some functions)

59Notes

60-----

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

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

64module for a faster implementation of some functions.

66Documentation in HTML format can be generated with epydoc.

68Matrices (M) can be inverted using np.linalg.inv(M), be concatenated using

69np.dot(M0, M1), or transform homogeneous coordinate arrays (v) using

70np.dot(M, v) for shape (4, *) column vectors, respectively

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

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

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

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

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

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

79Calculations are carried out with np.float64 precision.

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

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

84Return types are numpy arrays unless specified otherwise.

86Angles are in radians unless specified otherwise.

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

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

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

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

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

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

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

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

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

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

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

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

105

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

107

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

109 includes most code of this module.

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

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

112

113References

114----------

115(1) Matrices and transformations. Ronald Goldman.

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

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

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

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

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

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

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

123(5) Euler angle conversion. Ken Shoemake.

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

125(6) Arcball rotation control. Ken Shoemake.

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

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

128 vectors. James Diebel. 2006.

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

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

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

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

133(10) Quaternions. Ken Shoemake.

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

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

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

137(12) Uniform random rotations. Ken Shoemake.

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

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

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

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

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

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

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

145(16) Column Vectors vs. Row Vectors.

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

147

148Examples

149--------

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

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

152>>> I = identity_matrix()

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

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

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

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

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

158>>> np.allclose([alpha, beta, gamma], euler)

159True

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

161>>> is_same_transform(R, Re)

162True

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

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

165True

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

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

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

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

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

171>>> Rq = quaternion_matrix(q)

172>>> is_same_transform(R, Rq)

173True

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

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

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

177>>> R = random_rotation_matrix(np.random.rand(3))

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

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

180>>> np.allclose(scale, 1.23)

181True

182>>> np.allclose(trans, [1, 2, 3])

183True

184>>> np.allclose(shear, [0, np.tan(beta), 0])

185True

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

187True

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

189>>> is_same_transform(M, M1)

190True

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

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

193>>> v2 = np.dot(v0, M[:3,:3].T)

194>>> np.allclose(unit_vector(v1), unit_vector(v2))

195True

196

197"""

198

199import numpy as np

200from numpy.typing import ArrayLike, NDArray

201

202from .typed import Integer, Number, Seed

203from .util import diagonal_dot, random_generator

204

205# a cached immutable identity matrix provides a speedup sometimes

206_IDENTITY = np.eye(4)

207_IDENTITY.flags["WRITEABLE"] = False

208

209

210def identity_matrix():

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

212

213 >>> I = identity_matrix()

214 >>> np.allclose(I, np.dot(I, I))

215 True

216 >>> float(np.sum(I)), float(np.trace(I))

217 (4.0, 4.0)

218 >>> np.allclose(I, np.identity(4))

219 True

220

221 """

222 return np.identity(4)

223

224

225def translation_matrix(direction):

226 """

227 Return matrix to translate by direction vector.

228

229 >>> v = np.random.random(3) - 0.5

230 >>> np.allclose(v, translation_matrix(v)[:3, 3])

231 True

232

233 """

234 # are we 2D or 3D

235 dim = len(direction)

236

237 # start with identity matrix

238

239 if any("sympy" in str(type(v)) for v in direction):

240 # if we have been passed input values as sympy.Symbol

241 from sympy import eye

242

243 M = eye(dim + 1)

244 else:

245 M = np.eye(dim + 1)

246

247 # apply the offset

248 M[:dim, dim] = direction[:dim]

249

250 return M

251

252

253def translation_from_matrix(matrix):

254 """Return translation vector from translation matrix.

255

256 >>> v0 = np.random.random(3) - 0.5

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

258 >>> np.allclose(v0, v1)

259 True

260

261 """

262 return np.asarray(matrix)[:3, 3].copy()

263

264

265def reflection_matrix(point, normal):

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

267

268 >>> v0 = np.random.random(4) - 0.5

269 >>> v0[3] = 1.

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

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

272 >>> np.allclose(2, np.trace(R))

273 True

274 >>> np.allclose(v0, np.dot(R, v0))

275 True

276 >>> v2 = v0.copy()

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

278 >>> v3 = v0.copy()

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

280 >>> np.allclose(v2, np.dot(R, v3))

281 True

282

283 """

284 normal = unit_vector(normal[:3])

285 M = np.identity(4)

286 M[:3, :3] -= 2.0 * np.outer(normal, normal)

287 M[:3, 3] = (2.0 * np.dot(point[:3], normal)) * normal

288 return M

289

290

291def reflection_from_matrix(matrix):

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

293

294 >>> v0 = np.random.random(3) - 0.5

295 >>> v1 = np.random.random(3) - 0.5

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

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

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

299 >>> is_same_transform(M0, M1)

300 True

301

302 """

303 M = np.asarray(matrix, dtype=np.float64)

304 # normal: unit eigenvector corresponding to eigenvalue -1

305 w, V = np.linalg.eig(M[:3, :3])

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

307 if not len(i):

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

309 normal = np.real(V[:, i[0]]).squeeze()

310 # point: any unit eigenvector corresponding to eigenvalue 1

311 w, V = np.linalg.eig(M)

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

313 if not len(i):

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

315 point = np.real(V[:, i[-1]]).squeeze()

316 point /= point[3]

317 return point, normal

318

319

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

321 """

322 Return matrix to rotate about axis defined by point and

323 direction.

324

325 Parameters

326 -------------

327 angle : float, or sympy.Symbol

328 Angle, in radians or symbolic angle

329 direction : (3,) float

330 Any vector along rotation axis

331 point : (3, ) float, or None

332 Origin point of rotation axis

333

334 Returns

335 -------------

336 matrix : (4, 4) float, or (4, 4) sympy.Matrix

337 Homogeneous transformation matrix

338

339 Examples

340 -------------

341 >>> R = rotation_matrix(np.pi/2, [0, 0, 1], [1, 0, 0])

342 >>> np.allclose(np.dot(R, [0, 0, 0, 1]), [1, -1, 0, 1])

343 True

344 >>> angle = (random.random() - 0.5) * (2*np.pi)

345 >>> direc = np.random.random(3) - 0.5

346 >>> point = np.random.random(3) - 0.5

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

348 >>> R1 = rotation_matrix(angle-2*np.pi, direc, point)

349 >>> is_same_transform(R0, R1)

350 True

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

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

353 >>> is_same_transform(R0, R1)

354 True

355 >>> I = np.identity(4, np.float64)

356 >>> np.allclose(I, rotation_matrix(np.pi*2, direc))

357 True

358 >>> np.allclose(2, np.trace(rotation_matrix(np.pi/2,direc,point)))

359 True

360

361 """

362 if "sympy" in str(type(angle)):

363 # special case sympy symbolic angles

364 import sympy as sp

365

366 symbolic = True

367 sina = sp.sin(angle)

368 cosa = sp.cos(angle)

369 else:

370 symbolic = False

371 sina = np.sin(angle)

372 cosa = np.cos(angle)

373

374 direction = unit_vector(direction[:3])

375

376 # rotation matrix around unit vector

377 M = np.diag([cosa, cosa, cosa, 1.0])

378 M[:3, :3] += np.outer(direction, direction) * (1.0 - cosa)

379

380 direction = direction * sina

381 M[:3, :3] += np.array(

382 [

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

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

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

386 ]

387 )

388

389 # if point is specified, rotation is not around origin

390 if point is not None:

391 point = np.asarray(point[:3], dtype=np.float64)

392 M[:3, 3] = point - np.dot(M[:3, :3], point)

393

394 # return symbolic angles as sympy Matrix objects

395 if symbolic:

396 return sp.Matrix(M)

397

398 return M

399

400

401def rotation_from_matrix(matrix):

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

403

404 >>> angle = (random.random() - 0.5) * (2*np.pi)

405 >>> direc = np.random.random(3) - 0.5

406 >>> point = np.random.random(3) - 0.5

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

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

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

410 >>> is_same_transform(R0, R1)

411 True

412

413 """

414 R = np.asarray(matrix, dtype=np.float64)

415 R33 = R[:3, :3]

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

417 w, W = np.linalg.eig(R33.T)

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

419 if not len(i):

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

421 direction = np.real(W[:, i[-1]]).squeeze()

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

423 w, Q = np.linalg.eig(R)

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

425 if not len(i):

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

427 point = np.real(Q[:, i[-1]]).squeeze()

428 point /= point[3]

429 # rotation angle depending on direction

430 cosa = (np.trace(R33) - 1.0) / 2.0

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

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

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

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

435 else:

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

437 angle = np.arctan2(sina, cosa)

438 return angle, direction, point

439

440

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

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

443

444 Use factor -1 for point symmetry.

445

446 >>> v = (np.random.rand(4, 5) - 0.5) * 20

447 >>> v[3] = 1

448 >>> S = scale_matrix(-1.234)

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

450 True

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

452 >>> origin = np.random.random(3) - 0.5

453 >>> direct = np.random.random(3) - 0.5

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

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

456

457 """

458 if direction is None:

459 # uniform scaling

460 M = np.diag([factor, factor, factor, 1.0])

461 if origin is not None:

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

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

464 else:

465 # nonuniform scaling

466 direction = unit_vector(direction[:3])

467 factor = 1.0 - factor

468 M = np.identity(4)

469 M[:3, :3] -= factor * np.outer(direction, direction)

470 if origin is not None:

471 M[:3, 3] = (factor * np.dot(origin[:3], direction)) * direction

472 return M

473

474

475def scale_from_matrix(matrix):

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

477

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

479 >>> origin = np.random.random(3) - 0.5

480 >>> direct = np.random.random(3) - 0.5

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

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

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

484 >>> is_same_transform(S0, S1)

485 True

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

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

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

489 >>> is_same_transform(S0, S1)

490 True

491

492 """

493 M = np.asarray(matrix, dtype=np.float64)

494 M33 = M[:3, :3]

495 factor = np.trace(M33) - 2.0

496 try:

497 # direction: unit eigenvector corresponding to eigenvalue factor

498 w, V = np.linalg.eig(M33)

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

500 direction = np.real(V[:, i]).squeeze()

501 direction /= vector_norm(direction)

502 except IndexError:

503 # uniform scaling

504 factor = (factor + 2.0) / 3.0

505 direction = None

506 # origin: any eigenvector corresponding to eigenvalue 1

507 w, V = np.linalg.eig(M)

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

509 if not len(i):

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

511 origin = np.real(V[:, i[-1]]).squeeze()

512 origin /= origin[3]

513 return factor, origin, direction

514

515

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

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

518

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

520

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

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

523

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

525 >>> np.allclose(P[1:, 1:], np.identity(4)[1:, 1:])

526 True

527 >>> point = np.random.random(3) - 0.5

528 >>> normal = np.random.random(3) - 0.5

529 >>> direct = np.random.random(3) - 0.5

530 >>> persp = np.random.random(3) - 0.5

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

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

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

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

535 >>> is_same_transform(P2, np.dot(P0, P3))

536 True

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

538 >>> v0 = (np.random.rand(4, 5) - 0.5) * 20

539 >>> v0[3] = 1

540 >>> v1 = np.dot(P, v0)

541 >>> np.allclose(v1[1], v0[1])

542 True

543 >>> np.allclose(v1[0], 3-v1[1])

544 True

545

546 """

547 M = np.identity(4)

548 point = np.asarray(point[:3], dtype=np.float64)

549 normal = unit_vector(normal[:3])

550 if perspective is not None:

551 # perspective projection

552 perspective = np.asarray(perspective[:3], dtype=np.float64)

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

554 M[:3, :3] -= np.outer(perspective, normal)

555 if pseudo:

556 # preserve relative depth

557 M[:3, :3] -= np.outer(normal, normal)

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

559 else:

560 M[:3, 3] = np.dot(point, normal) * perspective

561 M[3, :3] = -normal

562 M[3, 3] = np.dot(perspective, normal)

563 elif direction is not None:

564 # parallel projection

565 direction = np.asarray(direction[:3], dtype=np.float64)

566 scale = np.dot(direction, normal)

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

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

569 else:

570 # orthogonal projection

571 M[:3, :3] -= np.outer(normal, normal)

572 M[:3, 3] = np.dot(point, normal) * normal

573 return M

574

575

576def projection_from_matrix(matrix, pseudo=False):

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

578

579 Return values are same as arguments for projection_matrix function:

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

581

582 >>> point = np.random.random(3) - 0.5

583 >>> normal = np.random.random(3) - 0.5

584 >>> direct = np.random.random(3) - 0.5

585 >>> persp = np.random.random(3) - 0.5

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

587 >>> result = projection_from_matrix(P0)

588 >>> P1 = projection_matrix(*result)

589 >>> is_same_transform(P0, P1)

590 True

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

592 >>> result = projection_from_matrix(P0)

593 >>> P1 = projection_matrix(*result)

594 >>> is_same_transform(P0, P1)

595 True

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

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

598 >>> P1 = projection_matrix(*result)

599 >>> is_same_transform(P0, P1)

600 True

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

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

603 >>> P1 = projection_matrix(*result)

604 >>> is_same_transform(P0, P1)

605 True

606

607 """

608 M = np.asarray(matrix, dtype=np.float64)

609 M33 = M[:3, :3]

610 w, V = np.linalg.eig(M)

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

612 if not pseudo and len(i):

613 # point: any eigenvector corresponding to eigenvalue 1

614 point = np.real(V[:, i[-1]]).squeeze()

615 point /= point[3]

616 # direction: unit eigenvector corresponding to eigenvalue 0

617 w, V = np.linalg.eig(M33)

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

619 if not len(i):

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

621 direction = np.real(V[:, i[0]]).squeeze()

622 direction /= vector_norm(direction)

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

624 w, V = np.linalg.eig(M33.T)

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

626 if len(i):

627 # parallel projection

628 normal = np.real(V[:, i[0]]).squeeze()

629 normal /= vector_norm(normal)

630 return point, normal, direction, None, False

631 else:

632 # orthogonal projection, where normal equals direction vector

633 return point, direction, None, None, False

634 else:

635 # perspective projection

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

637 if not len(i):

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

639 point = np.real(V[:, i[-1]]).squeeze()

640 point /= point[3]

641 normal = -M[3, :3]

642 perspective = M[:3, 3] / np.dot(point[:3], normal)

643 if pseudo:

644 perspective -= normal

645 return point, normal, None, perspective, pseudo

646

647

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

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

650

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

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

653

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

655 inside the frustum.

656

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

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

659 orthographic canonical view volume (a box).

660

661 Homogeneous coordinates transformed by the perspective clip matrix

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

663

664 >>> frustum = np.random.rand(6)

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

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

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

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

669 >>> a = np.dot(M, [frustum[0], frustum[2], frustum[4], 1])

670 >>> np.allclose(a, [-1., -1., -1., 1.])

671 True

672 >>> b = np.dot(M, [frustum[1], frustum[3], frustum[5], 1])

673 >>> np.allclose(b, [ 1., 1., 1., 1.])

674 True

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

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

677 >>> c = v / v[3]

678 >>> np.allclose(c, [-1., -1., -1., 1.])

679 True

680 >>> v = np.dot(M, [frustum[1], frustum[3], frustum[4], 1])

681 >>> d = v / v[3]

682 >>> np.allclose(d, [ 1., 1., -1., 1.])

683 True

684

685 """

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

687 raise ValueError("invalid frustum")

688 if perspective:

689 if near <= _EPS:

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

691 t = 2.0 * near

692 M = [

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

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

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

696 [0.0, 0.0, -1.0, 0.0],

697 ]

698 else:

699 M = [

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

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

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

703 [0.0, 0.0, 0.0, 1.0],

704 ]

705 return np.array(M)

706

707

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

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

710

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

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

713

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

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

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

717 of P onto the shear plane.

718

719 >>> angle = (random.random() - 0.5) * 4*np.pi

720 >>> direct = np.random.random(3) - 0.5

721 >>> point = np.random.random(3) - 0.5

722 >>> normal = np.cross(direct, np.random.random(3))

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

724 >>> np.allclose(1, np.linalg.det(S))

725 True

726

727 """

728 normal = unit_vector(normal[:3])

729 direction = unit_vector(direction[:3])

730 if abs(np.dot(normal, direction)) > 1e-6:

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

732 angle = np.tan(angle)

733 M = np.identity(4)

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

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

736 return M

737

738

739def shear_from_matrix(matrix):

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

741

742 >>> angle = np.pi / 2.0

743 >>> direct = [0.0, 1.0, 0.0]

744 >>> point = [0.0, 0.0, 0.0]

745 >>> normal = np.cross(direct, np.roll(direct,1))

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

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

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

749 >>> is_same_transform(S0, S1)

750 True

751

752 """

753 M = np.asarray(matrix, dtype=np.float64)

754 M33 = M[:3, :3]

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

756 w, V = np.linalg.eig(M33)

757

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

759 if len(i) < 2:

760 raise ValueError(f"no two linear independent eigenvectors found {w}")

761 V = np.real(V[:, i]).squeeze().T

762 lenorm = -1.0

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

764 n = np.cross(V[i0], V[i1])

765 w = vector_norm(n)

766 if w > lenorm:

767 lenorm = w

768 normal = n

769 normal /= lenorm

770 # direction and angle

771 direction = np.dot(M33 - np.identity(3), normal)

772 angle = vector_norm(direction)

773 direction /= angle

774 angle = np.arctan(angle)

775 # point: eigenvector corresponding to eigenvalue 1

776 w, V = np.linalg.eig(M)

777

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

779 if not len(i):

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

781 point = np.real(V[:, i[-1]]).squeeze()

782 point /= point[3]

783 return angle, direction, point, normal

784

785

786def decompose_matrix(matrix):

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

788

789 matrix : array_like

790 Non-degenerative homogeneous transformation matrix

791

792 Return tuple of:

793 scale : vector of 3 scaling factors

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

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

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

797 perspective : perspective partition of matrix

798

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

800

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

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

803 >>> T1 = translation_matrix(trans)

804 >>> np.allclose(T0, T1)

805 True

806 >>> S = scale_matrix(0.123)

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

808 >>> bool(np.isclose(scale[0], 0.123))

809 True

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

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

812 >>> R1 = euler_matrix(*angles)

813 >>> np.allclose(R0, R1)

814 True

815

816 """

817 M = np.array(matrix, dtype=np.float64).T

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

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

820 M /= M[3, 3]

821 P = M.copy()

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

823 if not np.linalg.det(P):

824 raise ValueError("matrix is singular")

825

826 scale = np.zeros((3,))

827 shear = [0.0, 0.0, 0.0]

828 angles = [0.0, 0.0, 0.0]

829

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

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

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

833 else:

834 perspective = np.array([0.0, 0.0, 0.0, 1.0])

835

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

837 M[3, :3] = 0.0

838

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

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

841 row[0] /= scale[0]

842 shear[0] = np.dot(row[0], row[1])

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

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

845 row[1] /= scale[1]

846 shear[0] /= scale[1]

847 shear[1] = np.dot(row[0], row[2])

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

849 shear[2] = np.dot(row[1], row[2])

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

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

852 row[2] /= scale[2]

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

854

855 if np.dot(row[0], np.cross(row[1], row[2])) < 0:

856 np.negative(scale, scale)

857 np.negative(row, row)

858

859 angles[1] = np.arcsin(-row[0, 2])

860 if np.cos(angles[1]):

861 angles[0] = np.arctan2(row[1, 2], row[2, 2])

862 angles[2] = np.arctan2(row[0, 1], row[0, 0])

863 else:

864 angles[0] = np.arctan2(-row[2, 1], row[1, 1])

865 angles[2] = 0.0

866

867 return scale, shear, angles, translate, perspective

868

869

870def compose_matrix(scale=None, shear=None, angles=None, translate=None, perspective=None):

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

872

873 This is the inverse of the decompose_matrix function.

874

875 Sequence of transformations:

876 scale : vector of 3 scaling factors

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

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

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

880 perspective : perspective partition of matrix

881

882 >>> scale = np.random.random(3) - 0.5

883 >>> shear = np.random.random(3) - 0.5

884 >>> angles = (np.random.random(3) - 0.5) * (2*np.pi)

885 >>> trans = np.random.random(3) - 0.5

886 >>> persp = np.random.random(4) - 0.5

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

888 >>> result = decompose_matrix(M0)

889 >>> M1 = compose_matrix(*result)

890 >>> is_same_transform(M0, M1)

891 True

892

893 """

894 M = np.identity(4)

895 if perspective is not None:

896 P = np.identity(4)

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

898 M = np.dot(M, P)

899 if translate is not None:

900 T = np.identity(4)

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

902 M = np.dot(M, T)

903 if angles is not None:

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

905 M = np.dot(M, R)

906 if shear is not None:

907 Z = np.identity(4)

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

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

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

911 M = np.dot(M, Z)

912 if scale is not None:

913 S = np.identity(4)

914 S[0, 0] = scale[0]

915 S[1, 1] = scale[1]

916 S[2, 2] = scale[2]

917 M = np.dot(M, S)

918 M /= M[3, 3]

919 return M

920

921

922def orthogonalization_matrix(lengths, angles):

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

924

925 Angles are expected in degrees.

926

927 The de-orthogonalization matrix is the inverse.

928

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

930 >>> np.allclose(O[:3, :3], np.identity(3, float) * 10)

931 True

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

933 >>> np.allclose(np.sum(O), 43.063229)

934 True

935

936 """

937 a, b, c = lengths

938 angles = np.radians(angles)

939 sina, sinb, _ = np.sin(angles)

940 cosa, cosb, cosg = np.cos(angles)

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

942 return np.array(

943 [

944 [a * sinb * np.sqrt(1.0 - co * co), 0.0, 0.0, 0.0],

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

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

947 [0.0, 0.0, 0.0, 1.0],

948 ]

949 )

950

951

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

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

954

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

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

957

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

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

960 is returned.

961

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

963 If usesvd is True, similarity and Euclidean transformation matrices

964 are calculated by minimizing the weighted sum of squared deviations

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

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

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

968

969 The returned matrix performs rotation, translation and uniform scaling

970 (if specified).

971

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

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

974 >>> mat = affine_matrix_from_points(v0, v1)

975 >>> T = translation_matrix(np.random.random(3)-0.5)

976 >>> R = random_rotation_matrix(np.random.random(3))

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

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

979 >>> v0 = (np.random.rand(4, 100) - 0.5) * 20

980 >>> v0[3] = 1

981 >>> v1 = np.dot(M, v0)

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

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

984 >>> check = np.allclose(v1, np.dot(M, v0))

985

986 More examples in superimposition_matrix()

987

988 """

989 v0 = np.array(v0, dtype=np.float64)

990 v1 = np.array(v1, dtype=np.float64)

991

992 ndims = v0.shape[0]

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

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

995

996 # move centroids to origin

997 t0 = -np.mean(v0, axis=1)

998 M0 = np.identity(ndims + 1)

999 M0[:ndims, ndims] = t0

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

1001 t1 = -np.mean(v1, axis=1)

1002 M1 = np.identity(ndims + 1)

1003 M1[:ndims, ndims] = t1

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

1005

1006 if shear:

1007 # Affine transformation

1008 A = np.concatenate((v0, v1), axis=0)

1009 u, s, vh = np.linalg.svd(A.T)

1010 vh = vh[:ndims].T

1011 B = vh[:ndims]

1012 C = vh[ndims : 2 * ndims]

1013 t = np.dot(C, np.linalg.pinv(B))

1014 t = np.concatenate((t, np.zeros((ndims, 1))), axis=1)

1015 M = np.vstack((t, ((0.0,) * ndims) + (1.0,)))

1016 elif usesvd or ndims != 3:

1017 # Rigid transformation via SVD of covariance matrix

1018 u, s, vh = np.linalg.svd(np.dot(v1, v0.T))

1019 # rotation matrix from SVD orthonormal bases

1020 R = np.dot(u, vh)

1021 if np.linalg.det(R) < 0.0:

1022 # R does not constitute right handed system

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

1024 s[-1] *= -1.0

1025 # homogeneous transformation matrix

1026 M = np.identity(ndims + 1)

1027 M[:ndims, :ndims] = R

1028 else:

1029 # Rigid transformation matrix via quaternion

1030 # compute symmetric matrix N

1031 xx, yy, zz = np.sum(v0 * v1, axis=1)

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

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

1034 N = [

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

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

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

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

1039 ]

1040 # quaternion: eigenvector corresponding to most positive eigenvalue

1041 w, V = np.linalg.eigh(N)

1042 q = V[:, np.argmax(w)]

1043 q /= vector_norm(q) # unit quaternion

1044 # homogeneous transformation matrix

1045 M = quaternion_matrix(q)

1046

1047 if scale and not shear:

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

1049 v0 *= v0

1050 v1 *= v1

1051 M[:ndims, :ndims] *= np.sqrt(np.sum(v1) / np.sum(v0))

1052

1053 # move centroids back

1054 M = np.dot(np.linalg.inv(M1), np.dot(M, M0))

1055 M /= M[ndims, ndims]

1056 return M

1057

1058

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

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

1061

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

1063

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

1065 affine_matrix_from_points function.

1066

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

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

1069

1070 >>> v0 = np.random.rand(3, 10)

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

1072 >>> np.allclose(M, np.identity(4))

1073 True

1074 >>> R = random_rotation_matrix(np.random.random(3))

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

1076 >>> v1 = np.dot(R, v0)

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

1078 >>> np.allclose(v1, np.dot(M, v0))

1079 True

1080 >>> v0 = (np.random.rand(4, 100) - 0.5) * 20

1081 >>> v0[3] = 1

1082 >>> v1 = np.dot(R, v0)

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

1084 >>> np.allclose(v1, np.dot(M, v0))

1085 True

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

1087 >>> T = translation_matrix(np.random.random(3)-0.5)

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

1089 >>> v1 = np.dot(M, v0)

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

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

1092 >>> np.allclose(v1, np.dot(M, v0))

1093 True

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

1095 >>> np.allclose(v1, np.dot(M, v0))

1096 True

1097 >>> v = np.zeros((4, 100, 3))

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

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

1100 >>> np.allclose(v1, np.dot(M, v[:, :, 0]))

1101 True

1102

1103 """

1104 v0 = np.asarray(v0, dtype=np.float64)[:3]

1105 v1 = np.asarray(v1, dtype=np.float64)[:3]

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

1107

1108

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

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

1111

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

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

1114

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

1116 >>> np.allclose(np.sum(R[0]), -1.34786452)

1117 True

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

1119 >>> np.allclose(np.sum(R[0]), -0.383436184)

1120 True

1121 >>> ai, aj, ak = (4*np.pi) * (np.random.random(3) - 0.5)

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

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

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

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

1126

1127 """

1128 try:

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

1130 except (AttributeError, KeyError):

1131 _TUPLE2AXES[axes] # validation

1132 firstaxis, parity, repetition, frame = axes

1133

1134 i = firstaxis

1135 j = _NEXT_AXIS[i + parity]

1136 k = _NEXT_AXIS[i - parity + 1]

1137

1138 if frame:

1139 ai, ak = ak, ai

1140 if parity:

1141 ai, aj, ak = -ai, -aj, -ak

1142

1143 if "sympy" in str(type(ai)):

1144 # if we have been passed input values as sympy.Symbol

1145 # use symbolic cosine and identity matrix

1146 from sympy import cos, eye, sin

1147

1148 M = eye(4)

1149 else:

1150 sin, cos = np.sin, np.cos

1151 M = np.eye(4)

1152

1153 si, sj, sk = sin(ai), sin(aj), sin(ak)

1154 ci, cj, ck = cos(ai), cos(aj), cos(ak)

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

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

1157

1158 if repetition:

1159 M[i, i] = cj

1160 M[i, j] = sj * si

1161 M[i, k] = sj * ci

1162 M[j, i] = sj * sk

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

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

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

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

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

1168 else:

1169 M[i, i] = cj * ck

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

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

1172 M[j, i] = cj * sk

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

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

1175 M[k, i] = -sj

1176 M[k, j] = cj * si

1177 M[k, k] = cj * ci

1178 return M

1179

1180

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

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

1183

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

1185

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

1187

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

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

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

1191 >>> np.allclose(R0, R1)

1192 True

1193 >>> angles = (4*np.pi) * (np.random.random(3) - 0.5)

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

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

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

1197 ... if not np.allclose(R0, R1): print(axes, "failed")

1198

1199 """

1200 try:

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

1202 except (AttributeError, KeyError):

1203 _TUPLE2AXES[axes] # validation

1204 firstaxis, parity, repetition, frame = axes

1205

1206 i = firstaxis

1207 j = _NEXT_AXIS[i + parity]

1208 k = _NEXT_AXIS[i - parity + 1]

1209

1210 M = np.asarray(matrix, dtype=np.float64)[:3, :3]

1211 if repetition:

1212 sy = np.sqrt(M[i, j] * M[i, j] + M[i, k] * M[i, k])

1213 if sy > _EPS:

1214 ax = np.arctan2(M[i, j], M[i, k])

1215 ay = np.arctan2(sy, M[i, i])

1216 az = np.arctan2(M[j, i], -M[k, i])

1217 else:

1218 ax = np.arctan2(-M[j, k], M[j, j])

1219 ay = np.arctan2(sy, M[i, i])

1220 az = 0.0

1221 else:

1222 cy = np.sqrt(M[i, i] * M[i, i] + M[j, i] * M[j, i])

1223 if cy > _EPS:

1224 ax = np.arctan2(M[k, j], M[k, k])

1225 ay = np.arctan2(-M[k, i], cy)

1226 az = np.arctan2(M[j, i], M[i, i])

1227 else:

1228 ax = np.arctan2(-M[j, k], M[j, j])

1229 ay = np.arctan2(-M[k, i], cy)

1230 az = 0.0

1231

1232 if parity:

1233 ax, ay, az = -ax, -ay, -az

1234 if frame:

1235 ax, az = az, ax

1236 return ax, ay, az

1237

1238

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

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

1241

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

1243 >>> np.allclose(angles, [0.123, 0, 0])

1244 True

1245

1246 """

1247 return euler_from_matrix(quaternion_matrix(quaternion), axes)

1248

1249

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

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

1252

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

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

1255

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

1257 >>> np.allclose(q, [0.435953, 0.310622, -0.718287, 0.444435])

1258 True

1259

1260 """

1261 try:

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

1263 except (AttributeError, KeyError):

1264 _TUPLE2AXES[axes] # validation

1265 firstaxis, parity, repetition, frame = axes

1266

1267 i = firstaxis + 1

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

1269 k = _NEXT_AXIS[i - parity] + 1

1270

1271 if frame:

1272 ai, ak = ak, ai

1273 if parity:

1274 aj = -aj

1275

1276 ai /= 2.0

1277 aj /= 2.0

1278 ak /= 2.0

1279 ci = np.cos(ai)

1280 si = np.sin(ai)

1281 cj = np.cos(aj)

1282 sj = np.sin(aj)

1283 ck = np.cos(ak)

1284 sk = np.sin(ak)

1285 cc = ci * ck

1286 cs = ci * sk

1287 sc = si * ck

1288 ss = si * sk

1289

1290 q = np.zeros((4,))

1291 if repetition:

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

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

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

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

1296 else:

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

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

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

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

1301 if parity:

1302 q[j] *= -1.0

1303

1304 return q

1305

1306

1307def quaternion_about_axis(angle, axis):

1308 """Return quaternion for rotation about axis.

1309

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

1311 >>> np.allclose(q, [0.99810947, 0.06146124, 0, 0])

1312 True

1313

1314 """

1315 q = np.array([0.0, axis[0], axis[1], axis[2]])

1316 qlen = vector_norm(q)

1317 if qlen > _EPS:

1318 q *= np.sin(angle / 2.0) / qlen

1319 q[0] = np.cos(angle / 2.0)

1320 return q

1321

1322

1323def quaternion_matrix(quaternion):

1324 """

1325 Return a homogeneous rotation matrix from quaternion.

1326

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

1328 >>> np.allclose(M, rotation_matrix(0.123, [1, 0, 0]))

1329 True

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

1331 >>> np.allclose(M, np.identity(4))

1332 True

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

1334 >>> np.allclose(M, np.diag([1, -1, -1, 1]))

1335 True

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

1337 >>> np.allclose(M, np.array([np.identity(4), np.diag([1, -1, -1, 1])]))

1338 True

1339

1340

1341 """

1342 q = np.array(quaternion, dtype=np.float64).reshape((-1, 4))

1343 n = diagonal_dot(q, q)

1344 # how many entries do we have

1345 num_qs = len(n)

1346 identities = n < _EPS

1347 q[~identities, :] *= np.sqrt(2.0 / n[~identities, None])

1348 q = q[:, None, :] * q[:, :, None]

1349

1350 # store the result

1351 ret = np.zeros((num_qs, 4, 4))

1352

1353 # pack the values into the result

1354 ret[:, 0, 0] = 1.0 - q[:, 2, 2] - q[:, 3, 3]

1355 ret[:, 0, 1] = q[:, 1, 2] - q[:, 3, 0]

1356 ret[:, 0, 2] = q[:, 1, 3] + q[:, 2, 0]

1357 ret[:, 1, 0] = q[:, 1, 2] + q[:, 3, 0]

1358 ret[:, 1, 1] = 1.0 - q[:, 1, 1] - q[:, 3, 3]

1359 ret[:, 1, 2] = q[:, 2, 3] - q[:, 1, 0]

1360 ret[:, 2, 0] = q[:, 1, 3] - q[:, 2, 0]

1361 ret[:, 2, 1] = q[:, 2, 3] + q[:, 1, 0]

1362 ret[:, 2, 2] = 1.0 - q[:, 1, 1] - q[:, 2, 2]

1363 ret[:, 3, 3] = 1.0

1364 # set any identities

1365 ret[identities] = np.eye(4)[None, ...]

1366

1367 return ret.squeeze()

1368

1369

1370def quaternion_from_matrix(matrix, isprecise=False):

1371 """Return quaternion from rotation matrix.

1372

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

1374 matrix and a faster algorithm is used.

1375

1376 >>> q = quaternion_from_matrix(np.identity(4), True)

1377 >>> np.allclose(q, [1, 0, 0, 0])

1378 True

1379 >>> q = quaternion_from_matrix(np.diag([1, -1, -1, 1]))

1380 >>> np.allclose(q, [0, 1, 0, 0]) or np.allclose(q, [0, -1, 0, 0])

1381 True

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

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

1384 >>> np.allclose(q, [0.9981095, 0.0164262, 0.0328524, 0.0492786])

1385 True

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

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

1388 >>> q = quaternion_from_matrix(R)

1389 >>> np.allclose(q, [0.19069, 0.43736, 0.87485, -0.083611])

1390 True

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

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

1393 >>> q = quaternion_from_matrix(R)

1394 >>> np.allclose(q, [0.82336615, -0.13610694, 0.46344705, -0.29792603])

1395 True

1396 >>> R = random_rotation_matrix()

1397 >>> q = quaternion_from_matrix(R)

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

1399 True

1400 >>> is_same_quaternion(quaternion_from_matrix(R, isprecise=False),

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

1402 True

1403 >>> R = euler_matrix(0.0, 0.0, np.pi/2.0)

1404 >>> is_same_quaternion(quaternion_from_matrix(R, isprecise=False),

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

1406 True

1407

1408 """

1409 M = np.asarray(matrix, dtype=np.float64)[:4, :4]

1410 if isprecise:

1411 q = np.zeros((4,))

1412 t = np.trace(M)

1413 if t > M[3, 3]:

1414 q[0] = t

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

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

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

1418 else:

1419 i, j, k = 0, 1, 2

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

1421 i, j, k = 1, 2, 0

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

1423 i, j, k = 2, 0, 1

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

1425 q[i] = t

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

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

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

1429 q = q[[3, 0, 1, 2]]

1430 q *= 0.5 / np.sqrt(t * M[3, 3])

1431 else:

1432 m00 = M[0, 0]

1433 m01 = M[0, 1]

1434 m02 = M[0, 2]

1435 m10 = M[1, 0]

1436 m11 = M[1, 1]

1437 m12 = M[1, 2]

1438 m20 = M[2, 0]

1439 m21 = M[2, 1]

1440 m22 = M[2, 2]

1441 # symmetric matrix K

1442 K = np.array(

1443 [

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

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

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

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

1448 ]

1449 )

1450 K /= 3.0

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

1452 w, V = np.linalg.eigh(K)

1453 q = V[[3, 0, 1, 2], np.argmax(w)]

1454 if q[0] < 0.0:

1455 np.negative(q, q)

1456 return q

1457

1458

1459def quaternion_multiply(quaternion1, quaternion0):

1460 """Return multiplication of two quaternions.

1461

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

1463 >>> np.allclose(q, [28, -44, -14, 48])

1464 True

1465

1466 """

1467 w0, x0, y0, z0 = quaternion0

1468 w1, x1, y1, z1 = quaternion1

1469 return np.array(

1470 [

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

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

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

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

1475 ],

1476 dtype=np.float64,

1477 )

1478

1479

1480def quaternion_conjugate(quaternion):

1481 """Return conjugate of quaternion.

1482

1483 >>> q0 = random_quaternion()

1484 >>> q1 = quaternion_conjugate(q0)

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

1486 True

1487

1488 """

1489 q = np.array(quaternion, dtype=np.float64)

1490 np.negative(q[1:], q[1:])

1491 return q

1492

1493

1494def quaternion_inverse(quaternion):

1495 """Return inverse of quaternion.

1496

1497 >>> q0 = random_quaternion()

1498 >>> q1 = quaternion_inverse(q0)

1499 >>> np.allclose(quaternion_multiply(q0, q1), [1, 0, 0, 0])

1500 True

1501

1502 """

1503 q = np.array(quaternion, dtype=np.float64)

1504 np.negative(q[1:], q[1:])

1505 return q / np.dot(q, q)

1506

1507

1508def quaternion_real(quaternion):

1509 """Return real part of quaternion.

1510

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

1512 3.0

1513

1514 """

1515 return float(quaternion[0])

1516

1517

1518def quaternion_imag(quaternion):

1519 """Return imaginary part of quaternion.

1520

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

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

1523

1524 """

1525 return np.array(quaternion[1:4], dtype=np.float64)

1526

1527

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

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

1530

1531 >>> q0 = random_quaternion()

1532 >>> q1 = random_quaternion()

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

1534 >>> np.allclose(q, q0)

1535 True

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

1537 >>> np.allclose(q, q1)

1538 True

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

1540 >>> angle = np.arccos(np.dot(q0, q))

1541 >>> np.allclose(2, np.arccos(np.dot(q0, q1)) / angle) or \

1542 np.allclose(2, np.arccos(-np.dot(q0, q1)) / angle)

1543 True

1544

1545 """

1546 q0 = unit_vector(quat0[:4])

1547 q1 = unit_vector(quat1[:4])

1548 if fraction == 0.0:

1549 return q0

1550 elif fraction == 1.0:

1551 return q1

1552 d = np.dot(q0, q1)

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

1554 return q0

1555 if shortestpath and d < 0.0:

1556 # invert rotation

1557 d = -d

1558 np.negative(q1, q1)

1559 angle = np.arccos(d) + spin * np.pi

1560 if abs(angle) < _EPS:

1561 return q0

1562 isin = 1.0 / np.sin(angle)

1563 q0 *= np.sin((1.0 - fraction) * angle) * isin

1564 q1 *= np.sin(fraction * angle) * isin

1565 q0 += q1

1566 return q0

1567

1568

1569def random_quaternion(rand=None, num=1, seed: Seed = None):

1570 """Return uniform random unit quaternion.

1571

1572 rand: array like or None

1573 Three independent random variables that are uniformly distributed

1574 between 0 and 1.

1575 seed : None or int

1576 Seed for deterministic results, otherwise OS entropy.

1577

1578 >>> q = random_quaternion()

1579 >>> np.allclose(1, vector_norm(q))

1580 True

1581 >>> q = random_quaternion(num=10)

1582 >>> np.allclose(1, vector_norm(q, axis=1))

1583 True

1584 >>> q = random_quaternion(random_vector(3))

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

1586 (1, True)

1587 >>> bool(np.allclose(random_quaternion(seed=0), random_quaternion(seed=0)))

1588 True

1589

1590 """

1591 if rand is None:

1592 rand = random_generator(seed).random(3 * num).reshape((3, -1))

1593 else:

1594 assert rand.shape[0] == 3

1595 r1 = np.sqrt(1.0 - rand[0])

1596 r2 = np.sqrt(rand[0])

1597 pi2 = np.pi * 2.0

1598 t1 = pi2 * rand[1]

1599 t2 = pi2 * rand[2]

1600 return np.array(

1601 [np.cos(t2) * r2, np.sin(t1) * r1, np.cos(t1) * r1, np.sin(t2) * r2]

1602 ).T.squeeze()

1603

1604

1605def random_rotation_matrix(

1606 rand: ArrayLike | None = None,

1607 num: Integer = 1,

1608 translate: Number | None = None,

1609 seed: Seed = None,

1610):

1611 """

1612 Return uniform random rotation matrix.

1613

1614 Parameters

1615 ------------

1616 rand : (3,)

1617 Three independent random variables that are uniformly distributed

1618 between 0 and 1 for each returned quaternion.

1619 num

1620 Number of matrices to return.

1621 translate

1622 If passed the rotation matrix will include translation

1623 that is random and between positive and negative half this value.

1624 seed

1625 Seed for deterministic results, otherwise OS entropy.

1626

1627 >>> R = random_rotation_matrix()

1628 >>> np.allclose(np.dot(R.T, R), np.identity(4))

1629 True

1630 >>> R = random_rotation_matrix(num=10)

1631 >>> np.allclose(np.einsum('...ji,...jk->...ik', R, R), np.identity(4))

1632 True

1633 >>> bool(np.allclose(random_rotation_matrix(seed=0, translate=10),

1634 ... random_rotation_matrix(seed=0, translate=10)))

1635 True

1636

1637 """

1638 # thread one generator through so a passed `seed` doesn't hand the

1639 # translation the same values as the quaternion below

1640 random = random_generator(seed)

1641 matrix = quaternion_matrix(random_quaternion(rand=rand, num=num, seed=random))

1642 if translate:

1643 # apply random translation with the order of magnitude requested

1644 matrix[:3, 3] = (random.random(3) - 0.5) * float(translate)

1645

1646 return matrix

1647

1648

1649class Arcball:

1650 """Virtual Trackball Control.

1651

1652 >>> ball = Arcball()

1653 >>> ball = Arcball(initial=np.identity(4))

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

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

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

1657 >>> R = ball.matrix()

1658 >>> np.allclose(np.sum(R), 3.90583455)

1659 True

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

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

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

1663 >>> ball.constrain = True

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

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

1666 >>> R = ball.matrix()

1667 >>> np.allclose(np.sum(R), 0.2055924)

1668 True

1669 >>> ball.next()

1670

1671 """

1672

1673 def __init__(self, initial=None):

1674 """Initialize virtual trackball control.

1675

1676 initial : quaternion or rotation matrix

1677

1678 """

1679 self._axis = None

1680 self._axes = None

1681 self._radius = 1.0

1682 self._center = [0.0, 0.0]

1683 self._vdown = np.array([0.0, 0.0, 1.0])

1684 self._constrain = False

1685 if initial is None:

1686 self._qdown = np.array([1.0, 0.0, 0.0, 0.0])

1687 else:

1688 initial = np.array(initial, dtype=np.float64)

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

1690 self._qdown = quaternion_from_matrix(initial)

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

1692 initial /= vector_norm(initial)

1693 self._qdown = initial

1694 else:

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

1696 self._qnow = self._qpre = self._qdown

1697

1698 def place(self, center, radius):

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

1700

1701 center : sequence[2]

1702 Window coordinates of trackball center.

1703 radius : float

1704 Radius of trackball in window coordinates.

1705

1706 """

1707 self._radius = float(radius)

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

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

1710

1711 def setaxes(self, *axes):

1712 """Set axes to constrain rotations."""

1713 if axes is None:

1714 self._axes = None

1715 else:

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

1717

1718 @property

1719 def constrain(self):

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

1721 return self._constrain

1722

1723 @constrain.setter

1724 def constrain(self, value):

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

1726 self._constrain = bool(value)

1727

1728 def down(self, point):

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

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

1731 self._qdown = self._qpre = self._qnow

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

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

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

1735 else:

1736 self._axis = None

1737

1738 def drag(self, point):

1739 """Update current cursor window coordinates."""

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

1741 if self._axis is not None:

1742 vnow = arcball_constrain_to_axis(vnow, self._axis)

1743 self._qpre = self._qnow

1744 t = np.cross(self._vdown, vnow)

1745 if np.dot(t, t) < _EPS:

1746 self._qnow = self._qdown

1747 else:

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

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

1750

1751 def next(self, acceleration=0.0):

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

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

1754 self._qpre, self._qnow = self._qnow, q

1755

1756 def matrix(self):

1757 """Return homogeneous rotation matrix."""

1758 return quaternion_matrix(self._qnow)

1759

1760

1761def arcball_map_to_sphere(point, center, radius):

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

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

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

1765 n = v0 * v0 + v1 * v1

1766 if n > 1.0:

1767 # position outside of sphere

1768 n = np.sqrt(n)

1769 return np.array([v0 / n, v1 / n, 0.0])

1770 else:

1771 return np.array([v0, v1, np.sqrt(1.0 - n)])

1772

1773

1774def arcball_constrain_to_axis(point, axis):

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

1776 v = np.array(point, dtype=np.float64)

1777 a = np.array(axis, dtype=np.float64)

1778 v -= a * np.dot(a, v) # on plane

1779 n = vector_norm(v)

1780 if n > _EPS:

1781 if v[2] < 0.0:

1782 np.negative(v, v)

1783 v /= n

1784 return v

1785 if a[2] == 1.0:

1786 return np.array([1.0, 0.0, 0.0])

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

1788

1789

1790def arcball_nearest_axis(point, axes):

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

1792 point = np.asarray(point, dtype=np.float64)

1793 nearest = None

1794 mx = -1.0

1795 for axis in axes:

1796 t = np.dot(arcball_constrain_to_axis(point, axis), point)

1797 if t > mx:

1798 nearest = axis

1799 mx = t

1800 return nearest

1801

1802

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

1804_EPS = np.finfo(float).eps * 4.0

1805

1806# axis sequences for Euler angles

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

1808

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

1810_AXES2TUPLE = {

1811 "sxyz": (0, 0, 0, 0),

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

1830 "ryxy": (1, 1, 1, 1),

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

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

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

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

1835}

1836

1837_TUPLE2AXES = {v: k for k, v in _AXES2TUPLE.items()}

1838

1839

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

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

1842

1843 >>> v = np.random.random(3)

1844 >>> n = vector_norm(v)

1845 >>> np.allclose(n, np.linalg.norm(v))

1846 True

1847 >>> v = np.random.rand(6, 5, 3)

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

1849 >>> np.allclose(n, np.sqrt(np.sum(v*v, axis=2)))

1850 True

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

1852 >>> np.allclose(n, np.sqrt(np.sum(v*v, axis=1)))

1853 True

1854 >>> v = np.random.rand(5, 4, 3)

1855 >>> n = np.zeros((5, 3))

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

1857 >>> np.allclose(n, np.sqrt(np.sum(v*v, axis=1)))

1858 True

1859 >>> float(vector_norm([]))

1860 0.0

1861 >>> float(vector_norm([1]))

1862 1.0

1863

1864 """

1865 data = np.array(data, dtype=np.float64)

1866 if out is None:

1867 if data.ndim == 1:

1868 return np.sqrt(np.dot(data, data))

1869 data *= data

1870 out = np.atleast_1d(np.sum(data, axis=axis))

1871 np.sqrt(out, out)

1872 return out

1873 else:

1874 data *= data

1875 np.sum(data, axis=axis, out=out)

1876 np.sqrt(out, out)

1877

1878

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

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

1881

1882 >>> v0 = np.random.random(3)

1883 >>> v1 = unit_vector(v0)

1884 >>> np.allclose(v1, v0 / np.linalg.norm(v0))

1885 True

1886 >>> v0 = np.random.rand(5, 4, 3)

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

1888 >>> v2 = v0 / np.expand_dims(np.sqrt(np.sum(v0*v0, axis=2)), 2)

1889 >>> np.allclose(v1, v2)

1890 True

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

1892 >>> v2 = v0 / np.expand_dims(np.sqrt(np.sum(v0*v0, axis=1)), 1)

1893 >>> np.allclose(v1, v2)

1894 True

1895 >>> v1 = np.zeros((5, 4, 3))

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

1897 >>> np.allclose(v1, v2)

1898 True

1899 >>> list(unit_vector([]))

1900 []

1901 >>> [float(i) for i in unit_vector([1])]

1902 [1.0]

1903

1904 """

1905 if out is None:

1906 data = np.array(data, dtype=np.float64)

1907 if data.ndim == 1:

1908 data /= np.sqrt(np.dot(data, data))

1909 return data

1910 else:

1911 if out is not data:

1912 out[:] = np.asarray(data)

1913 data = out

1914 length = np.atleast_1d(np.sum(data * data, axis))

1915 np.sqrt(length, length)

1916 if axis is not None:

1917 length = np.expand_dims(length, axis)

1918 data /= length

1919 if out is None:

1920 return data

1921

1922

1923def random_vector(size, seed: Seed = None):

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

1925

1926 >>> v = random_vector(10000)

1927 >>> bool(np.all(v >= 0) and np.all(v < 1))

1928 True

1929 >>> v0 = random_vector(10)

1930 >>> v1 = random_vector(10)

1931 >>> bool(np.any(v0 == v1))

1932 False

1933 >>> bool(np.allclose(random_vector(10, seed=0), random_vector(10, seed=0)))

1934 True

1935

1936 """

1937 return random_generator(seed).random(size)

1938

1939

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

1941 """Return vector perpendicular to vectors.

1942

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

1944 >>> np.allclose(v, [0, 0, 6])

1945 True

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

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

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

1949 >>> np.allclose(v, [[0, 0, 0, 0], [0, 0, 6, 6], [0, -6, 0, -6]])

1950 True

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

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

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

1954 >>> np.allclose(v, [[0, 0, 6], [0, -6, 0], [6, 0, 0], [0, -6, 6]])

1955 True

1956

1957 """

1958 return np.cross(v0, v1, axis=axis)

1959

1960

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

1962 """Return angle between vectors.

1963

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

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

1966

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

1968 >>> np.allclose(a, np.pi)

1969 True

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

1971 >>> np.allclose(a, 0)

1972 True

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

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

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

1976 >>> np.allclose(a, [0, 1.5708, 1.5708, 0.95532])

1977 True

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

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

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

1981 >>> np.allclose(a, [1.5708, 1.5708, 1.5708, 0.95532])

1982 True

1983

1984 """

1985 v0 = np.asarray(v0, dtype=np.float64)

1986 v1 = np.asarray(v1, dtype=np.float64)

1987 dot = np.sum(v0 * v1, axis=axis)

1988 dot /= vector_norm(v0, axis=axis) * vector_norm(v1, axis=axis)

1989

1990 # clip off floating point error to avoid `nan` in the arccos`

1991 dot = np.clip(dot, -1.0, 1.0)

1992 return np.arccos(dot if directed else np.fabs(dot))

1993

1994

1995def inverse_matrix(matrix):

1996 """Return inverse of square transformation matrix.

1997

1998 >>> M0 = random_rotation_matrix()

1999 >>> M1 = inverse_matrix(M0.T)

2000 >>> np.allclose(M1, np.linalg.inv(M0.T))

2001 True

2002 >>> for size in range(1, 7):

2003 ... M0 = np.random.rand(size, size)

2004 ... M1 = inverse_matrix(M0)

2005 ... if not np.allclose(M1, np.linalg.inv(M0)): print(size)

2006

2007 """

2008 return np.linalg.inv(matrix)

2009

2010

2011def concatenate_matrices(*matrices):

2012 """Return concatenation of series of transformation matrices.

2013

2014 >>> M = np.random.rand(16).reshape((4, 4)) - 0.5

2015 >>> np.allclose(M, concatenate_matrices(M))

2016 True

2017 >>> np.allclose(np.dot(M, M.T), concatenate_matrices(M, M.T))

2018 True

2019

2020 """

2021 M = np.identity(4)

2022 for i in matrices:

2023 M = np.dot(M, i)

2024 return M

2025

2026

2027def is_same_transform(matrix0, matrix1):

2028 """Return True if two matrices perform same transformation.

2029

2030 >>> is_same_transform(np.identity(4), np.identity(4))

2031 True

2032 >>> is_same_transform(np.identity(4), random_rotation_matrix())

2033 False

2034

2035 """

2036 matrix0 = np.array(matrix0, dtype=np.float64)

2037 matrix0 /= matrix0[3, 3]

2038 matrix1 = np.array(matrix1, dtype=np.float64)

2039 matrix1 /= matrix1[3, 3]

2040 return np.allclose(matrix0, matrix1)

2041

2042

2043def is_same_quaternion(q0, q1):

2044 """Return True if two quaternions are equal."""

2045 q0 = np.array(q0)

2046 q1 = np.array(q1)

2047 return np.allclose(q0, q1) or np.allclose(q0, -q1)

2048

2049

2050def transform_around(matrix, point):

2051 """

2052 Given a transformation matrix, apply its rotation

2053 around a point in space.

2054

2055 Parameters

2056 ----------

2057 matrix: (4,4) or (3, 3) float, transformation matrix

2058 point: (3,) or (2,) float, point in space

2059

2060 Returns

2061 ---------

2062 result: (4,4) transformation matrix

2063 """

2064 point = np.asanyarray(point)

2065 matrix = np.asanyarray(matrix)

2066 dim = len(point)

2067 if matrix.shape != (dim + 1, dim + 1):

2068 raise ValueError("matrix must be (d+1, d+1)")

2069

2070 translate = np.eye(dim + 1)

2071 translate[:dim, dim] = -point

2072 result = np.dot(matrix, translate)

2073 translate[:dim, dim] = point

2074 result = np.dot(translate, result)

2075

2076 return result

2077

2078

2079def planar_matrix(offset=None, theta=None, point=None, scale=None):

2080 """

2081 2D homogeonous transformation matrix.

2082

2083 Parameters

2084 ----------

2085 offset : (2,) float

2086 XY offset

2087 theta : float

2088 Rotation around Z in radians

2089 point : (2, ) float

2090 Point to rotate around

2091 scale : (2,) float or None

2092 Scale to apply

2093

2094 Returns

2095 ----------

2096 matrix : (3, 3) flat

2097 Homogeneous 2D transformation matrix

2098 """

2099 if offset is None:

2100 offset = [0.0, 0.0]

2101 if theta is None:

2102 theta = 0.0

2103 offset = np.asanyarray(offset, dtype=np.float64)

2104 theta = float(theta)

2105 if not np.isfinite(theta):

2106 raise ValueError("theta must be finite angle!")

2107 if offset.shape != (2,):

2108 raise ValueError("offset must be length 2!")

2109

2110 T = np.eye(3, dtype=np.float64)

2111 s = np.sin(theta)

2112 c = np.cos(theta)

2113

2114 T[0, :2] = [c, s]

2115 T[1, :2] = [-s, c]

2116 T[:2, 2] = offset

2117

2118 if point is not None:

2119 T = transform_around(matrix=T, point=point)

2120

2121 if scale is not None:

2122 S = np.eye(3)

2123 S[:2, :2] *= scale

2124 T = np.dot(S, T)

2125

2126 return T

2127

2128

2129def planar_matrix_to_3D(matrix_2D):

2130 """

2131 Given a 2D homogeneous rotation matrix convert it to a 3D rotation

2132 matrix that is rotating around the Z axis

2133

2134 Parameters

2135 ----------

2136 matrix_2D: (3,3) float, homogeneous 2D rotation matrix

2137

2138 Returns

2139 ----------

2140 matrix_3D: (4,4) float, homogeneous 3D rotation matrix

2141 """

2142

2143 matrix_2D = np.asanyarray(matrix_2D, dtype=np.float64)

2144 if matrix_2D.shape != (3, 3):

2145 raise ValueError("Homogeneous 2D transformation matrix required!")

2146

2147 matrix_3D = np.eye(4)

2148 # translation

2149 matrix_3D[:2, 3] = matrix_2D[:2, 2]

2150 # rotation from 2D to around Z

2151 matrix_3D[:2, :2] = matrix_2D[:2, :2]

2152

2153 return matrix_3D

2154

2155

2156def spherical_matrix(theta, phi, axes="sxyz"):

2157 """

2158 Give a spherical coordinate vector, find the rotation that will

2159 transform a [0,0,1] vector to those coordinates

2160

2161 Parameters

2162 -----------

2163 theta: float, rotation angle in radians

2164 phi: float, rotation angle in radians

2165

2166 Returns

2167 ----------

2168 matrix: (4,4) rotation matrix where the following will

2169 be a cartesian vector in the direction of the

2170 input spherical coordinates:

2171 np.dot(matrix, [0,0,1,0])

2172

2173 """

2174 result = euler_matrix(0.0, phi, theta, axes=axes)

2175 return result

2176

2177

2178def transform_points(

2179 points: ArrayLike, matrix: ArrayLike, translate: bool = True

2180) -> NDArray[np.float64]:

2181 """

2182 Returns points rotated by a homogeneous

2183 transformation matrix.

2184

2185 If points are (n, 2) matrix must be (3, 3)

2186 If points are (n, 3) matrix must be (4, 4)

2187

2188 Parameters

2189 ----------

2190 points : (n, dim) float

2191 Points where `dim` is 2 or 3.

2192 matrix : (3, 3) or (4, 4) float

2193 Homogeneous rotation matrix.

2194 translate : bool

2195 Apply translation from matrix or not.

2196

2197 Returns

2198 ----------

2199 transformed : (n, dim) float

2200 Transformed points.

2201 """

2202 points = np.asanyarray(points, dtype=np.float64)

2203 if len(points) == 0 or matrix is None:

2204 return points.copy()

2205

2206 # check the matrix against the points

2207 matrix = np.asanyarray(matrix, dtype=np.float64)

2208 # shorthand the shape

2209 count, dim = points.shape

2210

2211 # quickly check to see if we've been passed an identity matrix

2212 if np.abs(matrix - _IDENTITY[: dim + 1, : dim + 1]).max() < 1e-8:

2213 return np.ascontiguousarray(points.copy())

2214

2215 if translate:

2216 # apply translation and rotation

2217 stack = np.column_stack((points, np.ones(count)))

2218 return np.dot(matrix, stack.T).T[:, :dim]

2219

2220 # only apply the rotation

2221 return np.dot(matrix[:dim, :dim], points.T).T

2222

2223

2224def fix_rigid(matrix, max_deviance=1e-5):

2225 """

2226 If a homogeneous transformation matrix is *almost* a rigid

2227 transform but many matrix-multiplies have accumulated some

2228 floating point error try to restore the matrix using SVD.

2229

2230 Parameters

2231 -----------

2232 matrix : (4, 4) or (3, 3) float

2233 Homogeneous transformation matrix.

2234 max_deviance : float

2235 Do not alter the matrix if it is not rigid by more

2236 than this amount.

2237

2238 Returns

2239 ----------

2240 repaired : (4, 4) or (3, 3) float

2241 Repaired homogeneous transformation matrix

2242 """

2243 dim = matrix.shape[0] - 1

2244 check = np.abs(

2245 np.dot(matrix[:dim, :dim], matrix[:dim, :dim].T) - _IDENTITY[:dim, :dim]

2246 ).max()

2247 # if the matrix differs by more than float-zero and less

2248 # than the threshold try to repair the matrix with SVD

2249 if check > 1e-13 and check < max_deviance:

2250 # reconstruct the rotation from the SVD

2251 U, _, V = np.linalg.svd(matrix[:dim, :dim])

2252 repaired = np.eye(dim + 1)

2253 repaired[:dim, :dim] = np.dot(U, V)

2254 # copy in the translation

2255 repaired[:dim, dim] = matrix[:dim, dim]

2256 # should be within tolerance of the original matrix

2257 assert np.allclose(repaired, matrix, atol=max_deviance)

2258 return repaired

2259

2260 return matrix

2261

2262

2263def is_rigid(matrix, epsilon=1e-8):

2264 """

2265 Check to make sure a homogeonous transformation

2266 matrix is a rigid transform.

2267

2268 Parameters

2269 -----------

2270 matrix : (4, 4) float

2271 A transformation matrix

2272

2273 Returns

2274 -----------

2275 check : bool

2276 True if matrix is a a transform with

2277 only translation, scale, and rotation

2278 """

2279

2280 matrix = np.asanyarray(matrix, dtype=np.float64)

2281

2282 if matrix.shape != (4, 4):

2283 return False

2284

2285 # make sure last row has no scaling

2286 if np.ptp(matrix[-1] - [0, 0, 0, 1]) > epsilon:

2287 return False

2288

2289 # check dot product of rotation against transpose

2290 check = np.dot(matrix[:3, :3], matrix[:3, :3].T) - _IDENTITY[:3, :3]

2291

2292 return np.ptp(check) < epsilon

2293

2294

2295def scale_and_translate(scale=None, translate=None):

2296 """

2297 Optimized version of `compose_matrix` for just

2298 scaling then translating.

2299

2300 Scalar args are broadcast to arrays of shape (3,)

2301

2302 Parameters

2303 --------------

2304 scale : float or (3,) float

2305 Scale factor

2306 translate : float or (3,) float

2307 Translation

2308 """

2309 M = np.eye(4)

2310 if np.any(scale != 1):

2311 M[:3, :3] *= scale

2312 if translate is not None:

2313 M[:3, 3] = translate

2314 return M

2315

2316

2317def flips_winding(matrix):

2318 """

2319 Check to see if a matrix will invert triangles.

2320

2321 Parameters

2322 -------------

2323 matrix : (4, 4) float

2324 Homogeneous transformation matrix

2325

2326 Returns

2327 --------------

2328 flip : bool

2329 True if matrix will flip winding of triangles.

2330 """

2331 # get input as numpy array

2332 matrix = np.asanyarray(matrix, dtype=np.float64)

2333 if matrix.shape not in ((3, 3), (4, 4)):

2334 raise ValueError(f"matrix must be (3, 3) or (4, 4), not {matrix.shape!s}")

2335 # a transform flips winding exactly when its determinant is negative

2336 return bool(np.linalg.det(matrix[:3, :3]) < 0.0)