# simu - Robot simulation. {{{ # # 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 line is omitted as it is always (0, 0, 1). First index is column, last index is line. Transformations are done from the last added to the first added, so that it can be incrementally constructed for objects contained in others.""" 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 self.stack = [ ] 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)) """ 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), (1, 1)) """ s = self.matrix o = other.matrix self.matrix = ( (s[0][0] * o[0][0] + s[1][0] * o[0][1], s[0][1] * o[0][0] + s[1][1] * o[0][1]), (s[0][0] * o[1][0] + s[1][0] * o[1][1], s[0][1] * o[1][0] + s[1][1] * o[1][1]), (s[0][0] * o[2][0] + s[1][0] * o[2][1] + s[2][0], s[0][1] * o[2][0] + s[1][1] * o[2][1] + s[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 ( (self.matrix[0][0] * i[0] + self.matrix[1][0] * i[1] + self.matrix[2][0], self.matrix[0][1] * i[0] + self.matrix[1][1] * i[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 push (self): """Save the current value to be poped later. >>> a = TransMatrix () >>> a.translate ((2, 3)); a ((1, 0), (0, 1), (2, 3)) >>> a.push () >>> a.scale (2); a ((2, 0), (0, 2), (2, 3)) >>> a.pop () >>> a ((1, 0), (0, 1), (2, 3)) """ self.stack.append (self.matrix) def pop (self): """Restore saved value, see push.""" self.matrix = self.stack.pop () def __repr__ (self): return self.matrix.__repr__ () def _test (): import doctest doctest.testmod () if __name__ == '__main__': _test()