summaryrefslogtreecommitdiff
path: root/host/simu/utils/trans_matrix.py
diff options
context:
space:
mode:
Diffstat (limited to 'host/simu/utils/trans_matrix.py')
-rw-r--r--host/simu/utils/trans_matrix.py174
1 files changed, 174 insertions, 0 deletions
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()