From 233c384c6c53f91e1405d797d9683f3296ddabc1 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sat, 14 Mar 2009 15:57:17 +0100 Subject: * host/inter, host/simu: - moved drawable and trans_matrix to the new directory structure. - new drawable behaviour. --- host/simu/utils/trans_matrix.py | 174 ++++++++++++++++++++++++++++++++++++++++ 1 file changed, 174 insertions(+) create mode 100644 host/simu/utils/trans_matrix.py (limited to 'host/simu/utils/trans_matrix.py') diff --git a/host/simu/utils/trans_matrix.py b/host/simu/utils/trans_matrix.py new file mode 100644 index 00000000..7071e188 --- /dev/null +++ b/host/simu/utils/trans_matrix.py @@ -0,0 +1,174 @@ +# inter - Robot simulation interface. {{{ +# +# Copyright (C) 2008 Nicolas Schodet +# +# APBTeam: +# Web: http://apbteam.org/ +# Email: team AT apbteam DOT org +# +# This program is free software; you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation; either version 2 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program; if not, write to the Free Software +# Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. +# +# }}} +"""TransMatrix utility.""" +from math import sin, cos, sqrt, atan2 + +class TransMatrix: + """Define a matrix to be used for transformations on the plane. + + This is a "special" kind of matrix, because the last column is omitted as + it is always (0, 0, 1).""" + + IDENTITY = ((1, 0), (0, 1), (0, 0)) + + def __init__ (self, *m): + """Initialise the matrix, with optional initial value. + + >>> TransMatrix () + ((1, 0), (0, 1), (0, 0)) + >>> TransMatrix ((1, 2), (3, 4), (5, 6)) + ((1, 2), (3, 4), (5, 6)) + """ + if m: + assert len (m) == 3 + for i in m: + assert len (i) == 2 + self.matrix = m + else: + self.matrix = self.IDENTITY + + def identity (self): + """Set to identity. + + >>> a = TransMatrix () + >>> a.translate ((2, 3)) + >>> a.identity (); a + ((1, 0), (0, 1), (0, 0)) + """ + self.matrix = self.IDENTITY + + def rotate (self, angle): + """Transform the current matrix to do a rotation. + + >>> from math import pi + >>> a = TransMatrix () + >>> a.rotate (pi / 3); a # doctest: +ELLIPSIS + ((0.5..., 0.866...), (-0.866..., 0.5...), (0.0, 0.0)) + """ + s = sin (angle) + c = cos (angle) + m = TransMatrix ((c, s), (-s, c), (0, 0)) + self *= m + + def translate (self, by): + """Transform the current matrix to do a translation. + + >>> a = TransMatrix () + >>> a.translate ((2, 3)); a + ((1, 0), (0, 1), (2, 3)) + """ + m = TransMatrix ((1, 0), (0, 1), by) + self *= m + + def scale (self, factor): + """Transform the current matrix to do a scaling. + + >>> a = TransMatrix () + >>> a.scale (2); a + ((2, 0), (0, 2), (0, 0)) + """ + m = TransMatrix ((factor, 0), (0, factor), (0, 0)) + self *= m + + def __imul__ (self, other): + """Multiply by an other matrix. + + >>> a = TransMatrix ((1, 0), (0, 1), (1, 0)) + >>> b = TransMatrix ((0, 1), (1, 0), (0, 1)) + >>> a *= b; a + ((0, 1), (1, 0), (0, 2)) + """ + s = self.matrix + o = other.matrix + self.matrix = ( + (s[0][0] * o[0][0] + s[0][1] * o[1][0], + s[0][0] * o[0][1] + s[0][1] * o[1][1]), + (s[1][0] * o[0][0] + s[1][1] * o[1][0], + s[1][0] * o[0][1] + s[1][1] * o[1][1]), + (s[2][0] * o[0][0] + s[2][1] * o[1][0] + o[2][0], + s[2][0] * o[0][1] + s[2][1] * o[1][1] + o[2][1])) + return self + + def apply (self, *args): + """Apply (multiply) the matrix to all the given arguments. + + >>> m = TransMatrix ((1, 2), (4, 8), (16, 32)) + >>> m.apply ((1, 0)) + (17, 34) + >>> m.apply ((0, 1), (1, 1)) + ((20, 40), (21, 42)) + """ + r = tuple ( + (i[0] * self.matrix[0][0] + i[1] * self.matrix[1][0] + + self.matrix[2][0], + i[0] * self.matrix[0][1] + i[1] * self.matrix[1][1] + + self.matrix[2][1]) + for i in args) + if len (args) == 1: + return r[0] + else: + return r + + def apply_angle (self, angle): + """Apply the matrix to an angle. + + >>> from math import pi + >>> a = TransMatrix () + >>> a.rotate (pi / 6) + >>> a.translate ((2, 3)) + >>> a.scale (4) + >>> a.apply_angle (pi / 6), pi / 3 # doctest: +ELLIPSIS + (1.0471..., 1.0471...) + """ + o, m = self.apply ((0, 0), (cos (angle), sin (angle))) + v = (m[0] - o[0], m[1] - o[1]) + vl = sqrt (v[0] ** 2 + v[1] ** 2) + v = (v[0] / vl, v[1] / vl) + return atan2 (v[1], v[0]) + + def apply_distance (self, distance): + """Apply the matrix to a distance. + + >>> from math import pi + >>> a = TransMatrix () + >>> a.rotate (pi / 6) + >>> a.translate ((2, 3)) + >>> a.scale (4) + >>> round (a.apply_distance (2)) + 8.0 + """ + o, m = self.apply ((0, 0), (distance, 0)) + v = (m[0] - o[0], m[1] - o[1]) + vl = sqrt (v[0] ** 2 + v[1] ** 2) + return vl + + def __repr__ (self): + return self.matrix.__repr__ () + +def _test (): + import doctest + doctest.testmod () + +if __name__ == '__main__': + _test() -- cgit v1.2.3