transforms.py 64 KB

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