summaryrefslogtreecommitdiffhomepage
diff options
context:
space:
mode:
authorNicolas Schodet2011-10-15 21:09:55 +0200
committerNicolas Schodet2011-10-15 21:09:55 +0200
commit677dbdcc67d976cbf2040dad32b5a4600f233eaf (patch)
treef000a26f5bd054a670ec7a987ca4ac72849bce09
parent456d1d6670a1a1cf6a01dbbf718a84cfa0a0f34c (diff)
host/simu/utils: add vector class
-rw-r--r--host/simu/utils/vector.py267
1 files changed, 267 insertions, 0 deletions
diff --git a/host/simu/utils/vector.py b/host/simu/utils/vector.py
new file mode 100644
index 00000000..e5a8bd05
--- /dev/null
+++ b/host/simu/utils/vector.py
@@ -0,0 +1,267 @@
+# simu - Robot simulation. {{{
+# encoding: utf-8
+#
+# Copyright (C) 2011 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.
+#
+# }}}
+"""2D vector."""
+import math
+
+__all__ = ('vector')
+
+class vector (object):
+ """2D vector, with useful operators."""
+
+ __slots__ = ('x', 'y')
+
+ def __init__ (self, *xy):
+ """Initialise the vector with given coordinates.
+
+ >>> vector (1, 2)
+ vector(1.0, 2.0)
+ >>> vector ((1, 2))
+ vector(1.0, 2.0)
+ >>> vector (vector (1, 2))
+ vector(1.0, 2.0)
+ >>> vector ("hello")
+ Traceback (most recent call last):
+ ...
+ TypeError
+ >>> vector ("ab")
+ Traceback (most recent call last):
+ ...
+ ValueError: invalid literal for float(): a
+ """
+ if len (xy) == 2:
+ self.x, self.y = float (xy[0]), float (xy[1])
+ elif len (xy) == 1:
+ v = xy[0]
+ if isinstance (v, vector):
+ self.x, self.y = v.x, v.y
+ elif len (v) == 2:
+ self.x, self.y = float (v[0]), float (v[1])
+ else:
+ raise TypeError
+ else:
+ raise TypeError
+
+ def __len__ (self):
+ """Return vector length, always 2.
+
+ >>> len (vector (1, 2))
+ 2
+ """
+ return 2
+
+ def __repr__ (self):
+ """Return text representation.
+
+ >>> vector (1, 2)
+ vector(1.0, 2.0)
+ """
+ return 'vector(%r, %r)' % (self.x, self.y)
+
+ def __str__ (self):
+ """Return informal text representation.
+
+ >>> str (vector (1, 2))
+ '(1.0, 2.0)'
+ """
+ return '(%s, %s)' % (self.x, self.y)
+
+ def __getitem__ (self, index):
+ """Return a coordinate by its index, for compatibility with
+ sequences."""
+ return (self.x, self.y)[index]
+
+ def __eq__ (self, other):
+ """Test for equality.
+
+ >>> vector (1, 2) == vector (1, 3)
+ False
+ >>> vector (1, 2) == vector (2, 2)
+ False
+ >>> vector (1, 2) == vector (1, 2)
+ True
+ """
+ return (self.x, self.y) == (other.x, other.y)
+
+ def __ne__ (self, other):
+ """Test for inequality.
+
+ >>> vector (1, 2) != vector (1, 2)
+ False
+ """
+ return not (self == other)
+
+ def __cmp__ (self, other):
+ """No comparison apart from equality test.
+
+ >>> vector (1, 2) < vector (1, 2)
+ Traceback (most recent call last):
+ ...
+ TypeError
+ """
+ raise TypeError
+
+ def __add__ (self, other):
+ """Addition.
+
+ >>> vector (1, 2) + vector (3, 4)
+ vector(4.0, 6.0)
+ """
+ return vector (self.x + other.x, self.y + other.y)
+
+ def __sub__ (self, other):
+ """Subtraction.
+
+ >>> vector (1, 2) - vector (3, 4)
+ vector(-2.0, -2.0)
+ """
+ return vector (self.x - other.x, self.y - other.y)
+
+ def __mul__ (self, other):
+ """Multiplication or dot product.
+
+ >>> vector (1, 2) * 2
+ vector(2.0, 4.0)
+ >>> 3 * vector (1, 2)
+ vector(3.0, 6.0)
+ >>> vector (1, 2) * vector (3, 4)
+ 11.0
+ """
+ if isinstance (other, vector):
+ return self.x * other.x + self.y * other.y
+ else:
+ return vector (self.x * other, self.y * other)
+ __rmul__ = __mul__
+
+ def __div__ (self, other):
+ """Division.
+
+ >>> vector (1, 2) / 2
+ vector(0.5, 1.0)
+ >>> vector (1, 2) / vector (3, 4)
+ Traceback (most recent call last):
+ ...
+ TypeError: unsupported operand type(s) for /: 'float' and 'vector'
+ >>> 3.0 / vector (1, 2)
+ Traceback (most recent call last):
+ ...
+ TypeError: unsupported operand type(s) for /: 'float' and 'vector'
+ """
+ return vector (self.x / other, self.y / other)
+ __truediv__ = __div__
+
+ def __neg__ (self):
+ """Negate.
+
+ >>> -vector (1.0, 2.0)
+ vector(-1.0, -2.0)
+ """
+ return vector (-self.x, -self.y)
+
+ def __pos__ (self):
+ """No-op.
+
+ >>> +vector (1.0, 2.0)
+ vector(1.0, 2.0)
+ """
+ return self
+
+ def __abs__ (self):
+ """Norm.
+
+ >>> abs (vector (3.0, 4.0))
+ 5.0
+ >>> vector (3.0, 4.0).norm ()
+ 5.0
+ """
+ return math.hypot (self.x, self.y)
+ norm = __abs__
+
+ def norm_squared (self):
+ """Norm squared.
+
+ >>> vector (3.0, 4.0).norm_squared ()
+ 25.0
+ """
+ return self.x ** 2 + self.y ** 2
+
+ def unit (self):
+ """Return normalized vector.
+
+ >>> print vector (3.0, 4.0).unit ()
+ (0.6, 0.8)
+ """
+ norm = math.hypot (self.x, self.y)
+ return vector (self.x / norm, self.y / norm)
+
+ def normal (self):
+ """Return the vector rotated by pi/2.
+
+ >>> vector (1, 2).normal ()
+ vector(-2.0, 1.0)
+ """
+ return vector (-self.y, self.x)
+
+ def angle (self):
+ """Return angle from (1, 0).
+
+ >>> vector (1, 1).angle ()
+ 0.78539816339744828
+ >>> vector (0, -1).angle ()
+ -1.5707963267948966
+ >>> vector (-1, -1).angle ()
+ -2.3561944901923448
+ """
+ return math.atan2 (self.y, self.x)
+
+ @staticmethod
+ def polar (angle, norm):
+ """Return a vector constructed from polar coordinates (angle, norm).
+
+ >>> print vector.polar (math.pi / 4, math.sqrt (2))
+ (1.0, 1.0)
+ """
+ return vector (norm * math.cos (angle), norm * math.sin (angle))
+
+if __name__ == '__main__':
+ def _test ():
+ import doctest
+ doctest.testmod ()
+ def _perf_test ():
+ import timeit
+ def do (title, stmt, setup = None):
+ n = 10000
+ if setup is None:
+ setup = 'import vector; v = vector.vector (1.2, 3.4)'
+ t = timeit.timeit (stmt, setup, number = n)
+ print title, '%.3f µs' % (1e6 * t / n)
+ do ('init from floats', 'v = vector.vector (1.2, 3.4)')
+ do ('init from tuple', 'v = vector.vector ((1.2, 3.4))')
+ do ('init from vector', 'v2 = vector.vector (v)')
+ do ('init from polar', 'v = vector.vector.polar (1.0, 1.0)')
+ do ('abs', 'abs (v)')
+ do ('norm_squared', 'v.norm_squared()')
+ do ('unit', 'v.unit()')
+ _test()
+ _perf_test ()