From 677dbdcc67d976cbf2040dad32b5a4600f233eaf Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sat, 15 Oct 2011 21:09:55 +0200 Subject: host/simu/utils: add vector class --- host/simu/utils/vector.py | 267 ++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 267 insertions(+) create mode 100644 host/simu/utils/vector.py (limited to 'host') 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 () -- cgit v1.2.3