From 766755aa9c2786fc3903f88b0044db7c6270e6de Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 27 Apr 2012 11:19:53 +1000 Subject: [PATCH] autotest: removed unused files --- Tools/autotest/pysim/euclid.py | 2271 -------------------------------- Tools/autotest/pysim/fgFDM.py | 209 --- 2 files changed, 2480 deletions(-) delete mode 100644 Tools/autotest/pysim/euclid.py delete mode 100644 Tools/autotest/pysim/fgFDM.py diff --git a/Tools/autotest/pysim/euclid.py b/Tools/autotest/pysim/euclid.py deleted file mode 100644 index 876cc89738..0000000000 --- a/Tools/autotest/pysim/euclid.py +++ /dev/null @@ -1,2271 +0,0 @@ -#!/usr/bin/env python -# -# euclid graphics maths module -# -# Copyright (c) 2006 Alex Holkner -# Alex.Holkner@mail.google.com -# -# This library is free software; you can redistribute it and/or modify it -# under the terms of the GNU Lesser General Public License as published by the -# Free Software Foundation; either version 2.1 of the License, or (at your -# option) any later version. -# -# This library 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 Lesser General Public License -# for more details. -# -# You should have received a copy of the GNU Lesser General Public License -# along with this library; if not, write to the Free Software Foundation, -# Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA - -'''euclid graphics maths module - -Documentation and tests are included in the file "euclid.txt", or online -at http://code.google.com/p/pyeuclid -''' - -__docformat__ = 'restructuredtext' -__version__ = '$Id$' -__revision__ = '$Revision$' - -import math -import operator -import types - -# Some magic here. If _use_slots is True, the classes will derive from -# object and will define a __slots__ class variable. If _use_slots is -# False, classes will be old-style and will not define __slots__. -# -# _use_slots = True: Memory efficient, probably faster in future versions -# of Python, "better". -# _use_slots = False: Ordinary classes, much faster than slots in current -# versions of Python (2.4 and 2.5). -_use_slots = True - -# If True, allows components of Vector2 and Vector3 to be set via swizzling; -# e.g. v.xyz = (1, 2, 3). This is much, much slower than the more verbose -# v.x = 1; v.y = 2; v.z = 3, and slows down ordinary element setting as -# well. Recommended setting is False. -_enable_swizzle_set = False - -# Requires class to derive from object. -if _enable_swizzle_set: - _use_slots = True - -# Implement _use_slots magic. -class _EuclidMetaclass(type): - def __new__(cls, name, bases, dct): - if '__slots__' in dct: - dct['__getstate__'] = cls._create_getstate(dct['__slots__']) - dct['__setstate__'] = cls._create_setstate(dct['__slots__']) - if _use_slots: - return type.__new__(cls, name, bases + (object,), dct) - else: - if '__slots__' in dct: - del dct['__slots__'] - return types.ClassType.__new__(types.ClassType, name, bases, dct) - - @classmethod - def _create_getstate(cls, slots): - def __getstate__(self): - d = {} - for slot in slots: - d[slot] = getattr(self, slot) - return d - return __getstate__ - - @classmethod - def _create_setstate(cls, slots): - def __setstate__(self, state): - for name, value in state.items(): - setattr(self, name, value) - return __setstate__ - -__metaclass__ = _EuclidMetaclass - -class Vector2: - __slots__ = ['x', 'y'] - __hash__ = None - - def __init__(self, x=0, y=0): - self.x = x - self.y = y - - def __copy__(self): - return self.__class__(self.x, self.y) - - copy = __copy__ - - def __repr__(self): - return 'Vector2(%.2f, %.2f)' % (self.x, self.y) - - def __eq__(self, other): - if isinstance(other, Vector2): - return self.x == other.x and \ - self.y == other.y - else: - assert hasattr(other, '__len__') and len(other) == 2 - return self.x == other[0] and \ - self.y == other[1] - - def __ne__(self, other): - return not self.__eq__(other) - - def __nonzero__(self): - return self.x != 0 or self.y != 0 - - def __len__(self): - return 2 - - def __getitem__(self, key): - return (self.x, self.y)[key] - - def __setitem__(self, key, value): - l = [self.x, self.y] - l[key] = value - self.x, self.y = l - - def __iter__(self): - return iter((self.x, self.y)) - - def __getattr__(self, name): - try: - return tuple([(self.x, self.y)['xy'.index(c)] \ - for c in name]) - except ValueError: - raise AttributeError, name - - if _enable_swizzle_set: - # This has detrimental performance on ordinary setattr as well - # if enabled - def __setattr__(self, name, value): - if len(name) == 1: - object.__setattr__(self, name, value) - else: - try: - l = [self.x, self.y] - for c, v in map(None, name, value): - l['xy'.index(c)] = v - self.x, self.y = l - except ValueError: - raise AttributeError, name - - def __add__(self, other): - if isinstance(other, Vector2): - # Vector + Vector -> Vector - # Vector + Point -> Point - # Point + Point -> Vector - if self.__class__ is other.__class__: - _class = Vector2 - else: - _class = Point2 - return _class(self.x + other.x, - self.y + other.y) - else: - assert hasattr(other, '__len__') and len(other) == 2 - return Vector2(self.x + other[0], - self.y + other[1]) - __radd__ = __add__ - - def __iadd__(self, other): - if isinstance(other, Vector2): - self.x += other.x - self.y += other.y - else: - self.x += other[0] - self.y += other[1] - return self - - def __sub__(self, other): - if isinstance(other, Vector2): - # Vector - Vector -> Vector - # Vector - Point -> Point - # Point - Point -> Vector - if self.__class__ is other.__class__: - _class = Vector2 - else: - _class = Point2 - return _class(self.x - other.x, - self.y - other.y) - else: - assert hasattr(other, '__len__') and len(other) == 2 - return Vector2(self.x - other[0], - self.y - other[1]) - - - def __rsub__(self, other): - if isinstance(other, Vector2): - return Vector2(other.x - self.x, - other.y - self.y) - else: - assert hasattr(other, '__len__') and len(other) == 2 - return Vector2(other.x - self[0], - other.y - self[1]) - - def __mul__(self, other): - assert type(other) in (int, long, float) - return Vector2(self.x * other, - self.y * other) - - __rmul__ = __mul__ - - def __imul__(self, other): - assert type(other) in (int, long, float) - self.x *= other - self.y *= other - return self - - def __div__(self, other): - assert type(other) in (int, long, float) - return Vector2(operator.div(self.x, other), - operator.div(self.y, other)) - - - def __rdiv__(self, other): - assert type(other) in (int, long, float) - return Vector2(operator.div(other, self.x), - operator.div(other, self.y)) - - def __floordiv__(self, other): - assert type(other) in (int, long, float) - return Vector2(operator.floordiv(self.x, other), - operator.floordiv(self.y, other)) - - - def __rfloordiv__(self, other): - assert type(other) in (int, long, float) - return Vector2(operator.floordiv(other, self.x), - operator.floordiv(other, self.y)) - - def __truediv__(self, other): - assert type(other) in (int, long, float) - return Vector2(operator.truediv(self.x, other), - operator.truediv(self.y, other)) - - - def __rtruediv__(self, other): - assert type(other) in (int, long, float) - return Vector2(operator.truediv(other, self.x), - operator.truediv(other, self.y)) - - def __neg__(self): - return Vector2(-self.x, - -self.y) - - __pos__ = __copy__ - - def __abs__(self): - return math.sqrt(self.x ** 2 + \ - self.y ** 2) - - magnitude = __abs__ - - def magnitude_squared(self): - return self.x ** 2 + \ - self.y ** 2 - - def normalize(self): - d = self.magnitude() - if d: - self.x /= d - self.y /= d - return self - - def normalized(self): - d = self.magnitude() - if d: - return Vector2(self.x / d, - self.y / d) - return self.copy() - - def dot(self, other): - assert isinstance(other, Vector2) - return self.x * other.x + \ - self.y * other.y - - def cross(self): - return Vector2(self.y, -self.x) - - def reflect(self, normal): - # assume normal is normalized - assert isinstance(normal, Vector2) - d = 2 * (self.x * normal.x + self.y * normal.y) - return Vector2(self.x - d * normal.x, - self.y - d * normal.y) - -class Vector3: - __slots__ = ['x', 'y', 'z'] - __hash__ = None - - def __init__(self, x=0, y=0, z=0): - self.x = x - self.y = y - self.z = z - - def __copy__(self): - return self.__class__(self.x, self.y, self.z) - - copy = __copy__ - - def __repr__(self): - return 'Vector3(%.2f, %.2f, %.2f)' % (self.x, - self.y, - self.z) - - def __eq__(self, other): - if isinstance(other, Vector3): - return self.x == other.x and \ - self.y == other.y and \ - self.z == other.z - else: - assert hasattr(other, '__len__') and len(other) == 3 - return self.x == other[0] and \ - self.y == other[1] and \ - self.z == other[2] - - def __ne__(self, other): - return not self.__eq__(other) - - def __nonzero__(self): - return self.x != 0 or self.y != 0 or self.z != 0 - - def __len__(self): - return 3 - - def __getitem__(self, key): - return (self.x, self.y, self.z)[key] - - def __setitem__(self, key, value): - l = [self.x, self.y, self.z] - l[key] = value - self.x, self.y, self.z = l - - def __iter__(self): - return iter((self.x, self.y, self.z)) - - def __getattr__(self, name): - try: - return tuple([(self.x, self.y, self.z)['xyz'.index(c)] \ - for c in name]) - except ValueError: - raise AttributeError, name - - if _enable_swizzle_set: - # This has detrimental performance on ordinary setattr as well - # if enabled - def __setattr__(self, name, value): - if len(name) == 1: - object.__setattr__(self, name, value) - else: - try: - l = [self.x, self.y, self.z] - for c, v in map(None, name, value): - l['xyz'.index(c)] = v - self.x, self.y, self.z = l - except ValueError: - raise AttributeError, name - - - def __add__(self, other): - if isinstance(other, Vector3): - # Vector + Vector -> Vector - # Vector + Point -> Point - # Point + Point -> Vector - if self.__class__ is other.__class__: - _class = Vector3 - else: - _class = Point3 - return _class(self.x + other.x, - self.y + other.y, - self.z + other.z) - else: - assert hasattr(other, '__len__') and len(other) == 3 - return Vector3(self.x + other[0], - self.y + other[1], - self.z + other[2]) - __radd__ = __add__ - - def __iadd__(self, other): - if isinstance(other, Vector3): - self.x += other.x - self.y += other.y - self.z += other.z - else: - self.x += other[0] - self.y += other[1] - self.z += other[2] - return self - - def __sub__(self, other): - if isinstance(other, Vector3): - # Vector - Vector -> Vector - # Vector - Point -> Point - # Point - Point -> Vector - if self.__class__ is other.__class__: - _class = Vector3 - else: - _class = Point3 - return Vector3(self.x - other.x, - self.y - other.y, - self.z - other.z) - else: - assert hasattr(other, '__len__') and len(other) == 3 - return Vector3(self.x - other[0], - self.y - other[1], - self.z - other[2]) - - - def __rsub__(self, other): - if isinstance(other, Vector3): - return Vector3(other.x - self.x, - other.y - self.y, - other.z - self.z) - else: - assert hasattr(other, '__len__') and len(other) == 3 - return Vector3(other.x - self[0], - other.y - self[1], - other.z - self[2]) - - def __mul__(self, other): - if isinstance(other, Vector3): - # TODO component-wise mul/div in-place and on Vector2; docs. - if self.__class__ is Point3 or other.__class__ is Point3: - _class = Point3 - else: - _class = Vector3 - return _class(self.x * other.x, - self.y * other.y, - self.z * other.z) - else: - assert type(other) in (int, long, float) - return Vector3(self.x * other, - self.y * other, - self.z * other) - - __rmul__ = __mul__ - - def __imul__(self, other): - assert type(other) in (int, long, float) - self.x *= other - self.y *= other - self.z *= other - return self - - def __div__(self, other): - assert type(other) in (int, long, float) - return Vector3(operator.div(self.x, other), - operator.div(self.y, other), - operator.div(self.z, other)) - - - def __rdiv__(self, other): - assert type(other) in (int, long, float) - return Vector3(operator.div(other, self.x), - operator.div(other, self.y), - operator.div(other, self.z)) - - def __floordiv__(self, other): - assert type(other) in (int, long, float) - return Vector3(operator.floordiv(self.x, other), - operator.floordiv(self.y, other), - operator.floordiv(self.z, other)) - - - def __rfloordiv__(self, other): - assert type(other) in (int, long, float) - return Vector3(operator.floordiv(other, self.x), - operator.floordiv(other, self.y), - operator.floordiv(other, self.z)) - - def __truediv__(self, other): - assert type(other) in (int, long, float) - return Vector3(operator.truediv(self.x, other), - operator.truediv(self.y, other), - operator.truediv(self.z, other)) - - - def __rtruediv__(self, other): - assert type(other) in (int, long, float) - return Vector3(operator.truediv(other, self.x), - operator.truediv(other, self.y), - operator.truediv(other, self.z)) - - def __neg__(self): - return Vector3(-self.x, - -self.y, - -self.z) - - __pos__ = __copy__ - - def __abs__(self): - return math.sqrt(self.x ** 2 + \ - self.y ** 2 + \ - self.z ** 2) - - magnitude = __abs__ - - def magnitude_squared(self): - return self.x ** 2 + \ - self.y ** 2 + \ - self.z ** 2 - - def normalize(self): - d = self.magnitude() - if d: - self.x /= d - self.y /= d - self.z /= d - return self - - def normalized(self): - d = self.magnitude() - if d: - return Vector3(self.x / d, - self.y / d, - self.z / d) - return self.copy() - - def dot(self, other): - assert isinstance(other, Vector3) - return self.x * other.x + \ - self.y * other.y + \ - self.z * other.z - - def cross(self, other): - assert isinstance(other, Vector3) - return Vector3(self.y * other.z - self.z * other.y, - -self.x * other.z + self.z * other.x, - self.x * other.y - self.y * other.x) - - def reflect(self, normal): - # assume normal is normalized - assert isinstance(normal, Vector3) - d = 2 * (self.x * normal.x + self.y * normal.y + self.z * normal.z) - return Vector3(self.x - d * normal.x, - self.y - d * normal.y, - self.z - d * normal.z) - -# a b c -# e f g -# i j k - -class Matrix3: - __slots__ = list('abcefgijk') - - def __init__(self): - self.identity() - - def __copy__(self): - M = Matrix3() - M.a = self.a - M.b = self.b - M.c = self.c - M.e = self.e - M.f = self.f - M.g = self.g - M.i = self.i - M.j = self.j - M.k = self.k - return M - - copy = __copy__ - def __repr__(self): - return ('Matrix3([% 8.2f % 8.2f % 8.2f\n' \ - ' % 8.2f % 8.2f % 8.2f\n' \ - ' % 8.2f % 8.2f % 8.2f])') \ - % (self.a, self.b, self.c, - self.e, self.f, self.g, - self.i, self.j, self.k) - - def __getitem__(self, key): - return [self.a, self.e, self.i, - self.b, self.f, self.j, - self.c, self.g, self.k][key] - - def __setitem__(self, key, value): - L = self[:] - L[key] = value - (self.a, self.e, self.i, - self.b, self.f, self.j, - self.c, self.g, self.k) = L - - def __mul__(self, other): - if isinstance(other, Matrix3): - # Caching repeatedly accessed attributes in local variables - # apparently increases performance by 20%. Attrib: Will McGugan. - Aa = self.a - Ab = self.b - Ac = self.c - Ae = self.e - Af = self.f - Ag = self.g - Ai = self.i - Aj = self.j - Ak = self.k - Ba = other.a - Bb = other.b - Bc = other.c - Be = other.e - Bf = other.f - Bg = other.g - Bi = other.i - Bj = other.j - Bk = other.k - C = Matrix3() - C.a = Aa * Ba + Ab * Be + Ac * Bi - C.b = Aa * Bb + Ab * Bf + Ac * Bj - C.c = Aa * Bc + Ab * Bg + Ac * Bk - C.e = Ae * Ba + Af * Be + Ag * Bi - C.f = Ae * Bb + Af * Bf + Ag * Bj - C.g = Ae * Bc + Af * Bg + Ag * Bk - C.i = Ai * Ba + Aj * Be + Ak * Bi - C.j = Ai * Bb + Aj * Bf + Ak * Bj - C.k = Ai * Bc + Aj * Bg + Ak * Bk - return C - elif isinstance(other, Point2): - A = self - B = other - P = Point2(0, 0) - P.x = A.a * B.x + A.b * B.y + A.c - P.y = A.e * B.x + A.f * B.y + A.g - return P - elif isinstance(other, Vector2): - A = self - B = other - V = Vector2(0, 0) - V.x = A.a * B.x + A.b * B.y - V.y = A.e * B.x + A.f * B.y - return V - else: - other = other.copy() - other._apply_transform(self) - return other - - def __imul__(self, other): - assert isinstance(other, Matrix3) - # Cache attributes in local vars (see Matrix3.__mul__). - Aa = self.a - Ab = self.b - Ac = self.c - Ae = self.e - Af = self.f - Ag = self.g - Ai = self.i - Aj = self.j - Ak = self.k - Ba = other.a - Bb = other.b - Bc = other.c - Be = other.e - Bf = other.f - Bg = other.g - Bi = other.i - Bj = other.j - Bk = other.k - self.a = Aa * Ba + Ab * Be + Ac * Bi - self.b = Aa * Bb + Ab * Bf + Ac * Bj - self.c = Aa * Bc + Ab * Bg + Ac * Bk - self.e = Ae * Ba + Af * Be + Ag * Bi - self.f = Ae * Bb + Af * Bf + Ag * Bj - self.g = Ae * Bc + Af * Bg + Ag * Bk - self.i = Ai * Ba + Aj * Be + Ak * Bi - self.j = Ai * Bb + Aj * Bf + Ak * Bj - self.k = Ai * Bc + Aj * Bg + Ak * Bk - return self - - def identity(self): - self.a = self.f = self.k = 1. - self.b = self.c = self.e = self.g = self.i = self.j = 0 - return self - - def scale(self, x, y): - self *= Matrix3.new_scale(x, y) - return self - - def translate(self, x, y): - self *= Matrix3.new_translate(x, y) - return self - - def rotate(self, angle): - self *= Matrix3.new_rotate(angle) - return self - - # Static constructors - def new_identity(cls): - self = cls() - return self - new_identity = classmethod(new_identity) - - def new_scale(cls, x, y): - self = cls() - self.a = x - self.f = y - return self - new_scale = classmethod(new_scale) - - def new_translate(cls, x, y): - self = cls() - self.c = x - self.g = y - return self - new_translate = classmethod(new_translate) - - def new_rotate(cls, angle): - self = cls() - s = math.sin(angle) - c = math.cos(angle) - self.a = self.f = c - self.b = -s - self.e = s - return self - new_rotate = classmethod(new_rotate) - - def determinant(self): - return (self.a*self.f*self.k - + self.b*self.g*self.i - + self.c*self.e*self.j - - self.a*self.g*self.j - - self.b*self.e*self.k - - self.c*self.f*self.i) - - def inverse(self): - tmp = Matrix3() - d = self.determinant() - - if abs(d) < 0.001: - # No inverse, return identity - return tmp - else: - d = 1.0 / d - - tmp.a = d * (self.f*self.k - self.g*self.j) - tmp.b = d * (self.c*self.j - self.b*self.k) - tmp.c = d * (self.b*self.g - self.c*self.f) - tmp.e = d * (self.g*self.i - self.e*self.k) - tmp.f = d * (self.a*self.k - self.c*self.i) - tmp.g = d * (self.c*self.e - self.a*self.g) - tmp.i = d * (self.e*self.j - self.f*self.i) - tmp.j = d * (self.b*self.i - self.a*self.j) - tmp.k = d * (self.a*self.f - self.b*self.e) - - return tmp - -# a b c d -# e f g h -# i j k l -# m n o p - -class Matrix4: - __slots__ = list('abcdefghijklmnop') - - def __init__(self): - self.identity() - - def __copy__(self): - M = Matrix4() - M.a = self.a - M.b = self.b - M.c = self.c - M.d = self.d - M.e = self.e - M.f = self.f - M.g = self.g - M.h = self.h - M.i = self.i - M.j = self.j - M.k = self.k - M.l = self.l - M.m = self.m - M.n = self.n - M.o = self.o - M.p = self.p - return M - - copy = __copy__ - - - def __repr__(self): - return ('Matrix4([% 8.2f % 8.2f % 8.2f % 8.2f\n' \ - ' % 8.2f % 8.2f % 8.2f % 8.2f\n' \ - ' % 8.2f % 8.2f % 8.2f % 8.2f\n' \ - ' % 8.2f % 8.2f % 8.2f % 8.2f])') \ - % (self.a, self.b, self.c, self.d, - self.e, self.f, self.g, self.h, - self.i, self.j, self.k, self.l, - self.m, self.n, self.o, self.p) - - def __getitem__(self, key): - return [self.a, self.e, self.i, self.m, - self.b, self.f, self.j, self.n, - self.c, self.g, self.k, self.o, - self.d, self.h, self.l, self.p][key] - - def __setitem__(self, key, value): - L = self[:] - L[key] = value - (self.a, self.e, self.i, self.m, - self.b, self.f, self.j, self.n, - self.c, self.g, self.k, self.o, - self.d, self.h, self.l, self.p) = L - - def __mul__(self, other): - if isinstance(other, Matrix4): - # Cache attributes in local vars (see Matrix3.__mul__). - Aa = self.a - Ab = self.b - Ac = self.c - Ad = self.d - Ae = self.e - Af = self.f - Ag = self.g - Ah = self.h - Ai = self.i - Aj = self.j - Ak = self.k - Al = self.l - Am = self.m - An = self.n - Ao = self.o - Ap = self.p - Ba = other.a - Bb = other.b - Bc = other.c - Bd = other.d - Be = other.e - Bf = other.f - Bg = other.g - Bh = other.h - Bi = other.i - Bj = other.j - Bk = other.k - Bl = other.l - Bm = other.m - Bn = other.n - Bo = other.o - Bp = other.p - C = Matrix4() - C.a = Aa * Ba + Ab * Be + Ac * Bi + Ad * Bm - C.b = Aa * Bb + Ab * Bf + Ac * Bj + Ad * Bn - C.c = Aa * Bc + Ab * Bg + Ac * Bk + Ad * Bo - C.d = Aa * Bd + Ab * Bh + Ac * Bl + Ad * Bp - C.e = Ae * Ba + Af * Be + Ag * Bi + Ah * Bm - C.f = Ae * Bb + Af * Bf + Ag * Bj + Ah * Bn - C.g = Ae * Bc + Af * Bg + Ag * Bk + Ah * Bo - C.h = Ae * Bd + Af * Bh + Ag * Bl + Ah * Bp - C.i = Ai * Ba + Aj * Be + Ak * Bi + Al * Bm - C.j = Ai * Bb + Aj * Bf + Ak * Bj + Al * Bn - C.k = Ai * Bc + Aj * Bg + Ak * Bk + Al * Bo - C.l = Ai * Bd + Aj * Bh + Ak * Bl + Al * Bp - C.m = Am * Ba + An * Be + Ao * Bi + Ap * Bm - C.n = Am * Bb + An * Bf + Ao * Bj + Ap * Bn - C.o = Am * Bc + An * Bg + Ao * Bk + Ap * Bo - C.p = Am * Bd + An * Bh + Ao * Bl + Ap * Bp - return C - elif isinstance(other, Point3): - A = self - B = other - P = Point3(0, 0, 0) - P.x = A.a * B.x + A.b * B.y + A.c * B.z + A.d - P.y = A.e * B.x + A.f * B.y + A.g * B.z + A.h - P.z = A.i * B.x + A.j * B.y + A.k * B.z + A.l - return P - elif isinstance(other, Vector3): - A = self - B = other - V = Vector3(0, 0, 0) - V.x = A.a * B.x + A.b * B.y + A.c * B.z - V.y = A.e * B.x + A.f * B.y + A.g * B.z - V.z = A.i * B.x + A.j * B.y + A.k * B.z - return V - else: - other = other.copy() - other._apply_transform(self) - return other - - def __imul__(self, other): - assert isinstance(other, Matrix4) - # Cache attributes in local vars (see Matrix3.__mul__). - Aa = self.a - Ab = self.b - Ac = self.c - Ad = self.d - Ae = self.e - Af = self.f - Ag = self.g - Ah = self.h - Ai = self.i - Aj = self.j - Ak = self.k - Al = self.l - Am = self.m - An = self.n - Ao = self.o - Ap = self.p - Ba = other.a - Bb = other.b - Bc = other.c - Bd = other.d - Be = other.e - Bf = other.f - Bg = other.g - Bh = other.h - Bi = other.i - Bj = other.j - Bk = other.k - Bl = other.l - Bm = other.m - Bn = other.n - Bo = other.o - Bp = other.p - self.a = Aa * Ba + Ab * Be + Ac * Bi + Ad * Bm - self.b = Aa * Bb + Ab * Bf + Ac * Bj + Ad * Bn - self.c = Aa * Bc + Ab * Bg + Ac * Bk + Ad * Bo - self.d = Aa * Bd + Ab * Bh + Ac * Bl + Ad * Bp - self.e = Ae * Ba + Af * Be + Ag * Bi + Ah * Bm - self.f = Ae * Bb + Af * Bf + Ag * Bj + Ah * Bn - self.g = Ae * Bc + Af * Bg + Ag * Bk + Ah * Bo - self.h = Ae * Bd + Af * Bh + Ag * Bl + Ah * Bp - self.i = Ai * Ba + Aj * Be + Ak * Bi + Al * Bm - self.j = Ai * Bb + Aj * Bf + Ak * Bj + Al * Bn - self.k = Ai * Bc + Aj * Bg + Ak * Bk + Al * Bo - self.l = Ai * Bd + Aj * Bh + Ak * Bl + Al * Bp - self.m = Am * Ba + An * Be + Ao * Bi + Ap * Bm - self.n = Am * Bb + An * Bf + Ao * Bj + Ap * Bn - self.o = Am * Bc + An * Bg + Ao * Bk + Ap * Bo - self.p = Am * Bd + An * Bh + Ao * Bl + Ap * Bp - return self - - def transform(self, other): - A = self - B = other - P = Point3(0, 0, 0) - P.x = A.a * B.x + A.b * B.y + A.c * B.z + A.d - P.y = A.e * B.x + A.f * B.y + A.g * B.z + A.h - P.z = A.i * B.x + A.j * B.y + A.k * B.z + A.l - w = A.m * B.x + A.n * B.y + A.o * B.z + A.p - if w != 0: - P.x /= w - P.y /= w - P.z /= w - return P - - def identity(self): - self.a = self.f = self.k = self.p = 1. - self.b = self.c = self.d = self.e = self.g = self.h = \ - self.i = self.j = self.l = self.m = self.n = self.o = 0 - return self - - def scale(self, x, y, z): - self *= Matrix4.new_scale(x, y, z) - return self - - def translate(self, x, y, z): - self *= Matrix4.new_translate(x, y, z) - return self - - def rotatex(self, angle): - self *= Matrix4.new_rotatex(angle) - return self - - def rotatey(self, angle): - self *= Matrix4.new_rotatey(angle) - return self - - def rotatez(self, angle): - self *= Matrix4.new_rotatez(angle) - return self - - def rotate_axis(self, angle, axis): - self *= Matrix4.new_rotate_axis(angle, axis) - return self - - def rotate_euler(self, heading, attitude, bank): - self *= Matrix4.new_rotate_euler(heading, attitude, bank) - return self - - def rotate_triple_axis(self, x, y, z): - self *= Matrix4.new_rotate_triple_axis(x, y, z) - return self - - def transpose(self): - (self.a, self.e, self.i, self.m, - self.b, self.f, self.j, self.n, - self.c, self.g, self.k, self.o, - self.d, self.h, self.l, self.p) = \ - (self.a, self.b, self.c, self.d, - self.e, self.f, self.g, self.h, - self.i, self.j, self.k, self.l, - self.m, self.n, self.o, self.p) - - def transposed(self): - M = self.copy() - M.transpose() - return M - - # Static constructors - def new(cls, *values): - M = cls() - M[:] = values - return M - new = classmethod(new) - - def new_identity(cls): - self = cls() - return self - new_identity = classmethod(new_identity) - - def new_scale(cls, x, y, z): - self = cls() - self.a = x - self.f = y - self.k = z - return self - new_scale = classmethod(new_scale) - - def new_translate(cls, x, y, z): - self = cls() - self.d = x - self.h = y - self.l = z - return self - new_translate = classmethod(new_translate) - - def new_rotatex(cls, angle): - self = cls() - s = math.sin(angle) - c = math.cos(angle) - self.f = self.k = c - self.g = -s - self.j = s - return self - new_rotatex = classmethod(new_rotatex) - - def new_rotatey(cls, angle): - self = cls() - s = math.sin(angle) - c = math.cos(angle) - self.a = self.k = c - self.c = s - self.i = -s - return self - new_rotatey = classmethod(new_rotatey) - - def new_rotatez(cls, angle): - self = cls() - s = math.sin(angle) - c = math.cos(angle) - self.a = self.f = c - self.b = -s - self.e = s - return self - new_rotatez = classmethod(new_rotatez) - - def new_rotate_axis(cls, angle, axis): - assert(isinstance(axis, Vector3)) - vector = axis.normalized() - x = vector.x - y = vector.y - z = vector.z - - self = cls() - s = math.sin(angle) - c = math.cos(angle) - c1 = 1. - c - - # from the glRotate man page - self.a = x * x * c1 + c - self.b = x * y * c1 - z * s - self.c = x * z * c1 + y * s - self.e = y * x * c1 + z * s - self.f = y * y * c1 + c - self.g = y * z * c1 - x * s - self.i = x * z * c1 - y * s - self.j = y * z * c1 + x * s - self.k = z * z * c1 + c - return self - new_rotate_axis = classmethod(new_rotate_axis) - - def new_rotate_euler(cls, heading, attitude, bank): - # from http://www.euclideanspace.com/ - ch = math.cos(heading) - sh = math.sin(heading) - ca = math.cos(attitude) - sa = math.sin(attitude) - cb = math.cos(bank) - sb = math.sin(bank) - - self = cls() - self.a = ch * ca - self.b = sh * sb - ch * sa * cb - self.c = ch * sa * sb + sh * cb - self.e = sa - self.f = ca * cb - self.g = -ca * sb - self.i = -sh * ca - self.j = sh * sa * cb + ch * sb - self.k = -sh * sa * sb + ch * cb - return self - new_rotate_euler = classmethod(new_rotate_euler) - - def new_rotate_triple_axis(cls, x, y, z): - m = cls() - - m.a, m.b, m.c = x.x, y.x, z.x - m.e, m.f, m.g = x.y, y.y, z.y - m.i, m.j, m.k = x.z, y.z, z.z - - return m - new_rotate_triple_axis = classmethod(new_rotate_triple_axis) - - def new_look_at(cls, eye, at, up): - z = (eye - at).normalized() - x = up.cross(z).normalized() - y = z.cross(x) - - m = cls.new_rotate_triple_axis(x, y, z) - m.d, m.h, m.l = eye.x, eye.y, eye.z - return m - new_look_at = classmethod(new_look_at) - - def new_perspective(cls, fov_y, aspect, near, far): - # from the gluPerspective man page - f = 1 / math.tan(fov_y / 2) - self = cls() - assert near != 0.0 and near != far - self.a = f / aspect - self.f = f - self.k = (far + near) / (near - far) - self.l = 2 * far * near / (near - far) - self.o = -1 - self.p = 0 - return self - new_perspective = classmethod(new_perspective) - - def determinant(self): - return ((self.a * self.f - self.e * self.b) - * (self.k * self.p - self.o * self.l) - - (self.a * self.j - self.i * self.b) - * (self.g * self.p - self.o * self.h) - + (self.a * self.n - self.m * self.b) - * (self.g * self.l - self.k * self.h) - + (self.e * self.j - self.i * self.f) - * (self.c * self.p - self.o * self.d) - - (self.e * self.n - self.m * self.f) - * (self.c * self.l - self.k * self.d) - + (self.i * self.n - self.m * self.j) - * (self.c * self.h - self.g * self.d)) - - def inverse(self): - tmp = Matrix4() - d = self.determinant(); - - if abs(d) < 0.001: - # No inverse, return identity - return tmp - else: - d = 1.0 / d; - - tmp.a = d * (self.f * (self.k * self.p - self.o * self.l) + self.j * (self.o * self.h - self.g * self.p) + self.n * (self.g * self.l - self.k * self.h)); - tmp.e = d * (self.g * (self.i * self.p - self.m * self.l) + self.k * (self.m * self.h - self.e * self.p) + self.o * (self.e * self.l - self.i * self.h)); - tmp.i = d * (self.h * (self.i * self.n - self.m * self.j) + self.l * (self.m * self.f - self.e * self.n) + self.p * (self.e * self.j - self.i * self.f)); - tmp.m = d * (self.e * (self.n * self.k - self.j * self.o) + self.i * (self.f * self.o - self.n * self.g) + self.m * (self.j * self.g - self.f * self.k)); - - tmp.b = d * (self.j * (self.c * self.p - self.o * self.d) + self.n * (self.k * self.d - self.c * self.l) + self.b * (self.o * self.l - self.k * self.p)); - tmp.f = d * (self.k * (self.a * self.p - self.m * self.d) + self.o * (self.i * self.d - self.a * self.l) + self.c * (self.m * self.l - self.i * self.p)); - tmp.j = d * (self.l * (self.a * self.n - self.m * self.b) + self.p * (self.i * self.b - self.a * self.j) + self.d * (self.m * self.j - self.i * self.n)); - tmp.n = d * (self.i * (self.n * self.c - self.b * self.o) + self.m * (self.b * self.k - self.j * self.c) + self.a * (self.j * self.o - self.n * self.k)); - - tmp.c = d * (self.n * (self.c * self.h - self.g * self.d) + self.b * (self.g * self.p - self.o * self.h) + self.f * (self.o * self.d - self.c * self.p)); - tmp.g = d * (self.o * (self.a * self.h - self.e * self.d) + self.c * (self.e * self.p - self.m * self.h) + self.g * (self.m * self.d - self.a * self.p)); - tmp.k = d * (self.p * (self.a * self.f - self.e * self.b) + self.d * (self.e * self.n - self.m * self.f) + self.h * (self.m * self.b - self.a * self.n)); - tmp.o = d * (self.m * (self.f * self.c - self.b * self.g) + self.a * (self.n * self.g - self.f * self.o) + self.e * (self.b * self.o - self.n * self.c)); - - tmp.d = d * (self.b * (self.k * self.h - self.g * self.l) + self.f * (self.c * self.l - self.k * self.d) + self.j * (self.g * self.d - self.c * self.h)); - tmp.h = d * (self.c * (self.i * self.h - self.e * self.l) + self.g * (self.a * self.l - self.i * self.d) + self.k * (self.e * self.d - self.a * self.h)); - tmp.l = d * (self.d * (self.i * self.f - self.e * self.j) + self.h * (self.a * self.j - self.i * self.b) + self.l * (self.e * self.b - self.a * self.f)); - tmp.p = d * (self.a * (self.f * self.k - self.j * self.g) + self.e * (self.j * self.c - self.b * self.k) + self.i * (self.b * self.g - self.f * self.c)); - - return tmp; - - -class Quaternion: - # All methods and naming conventions based off - # http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions - - # w is the real part, (x, y, z) are the imaginary parts - __slots__ = ['w', 'x', 'y', 'z'] - - def __init__(self, w=1, x=0, y=0, z=0): - self.w = w - self.x = x - self.y = y - self.z = z - - def __copy__(self): - Q = Quaternion() - Q.w = self.w - Q.x = self.x - Q.y = self.y - Q.z = self.z - return Q - - copy = __copy__ - - def __repr__(self): - return 'Quaternion(real=%.2f, imag=<%.2f, %.2f, %.2f>)' % \ - (self.w, self.x, self.y, self.z) - - def __mul__(self, other): - if isinstance(other, Quaternion): - Ax = self.x - Ay = self.y - Az = self.z - Aw = self.w - Bx = other.x - By = other.y - Bz = other.z - Bw = other.w - Q = Quaternion() - Q.x = Ax * Bw + Ay * Bz - Az * By + Aw * Bx - Q.y = -Ax * Bz + Ay * Bw + Az * Bx + Aw * By - Q.z = Ax * By - Ay * Bx + Az * Bw + Aw * Bz - Q.w = -Ax * Bx - Ay * By - Az * Bz + Aw * Bw - return Q - elif isinstance(other, Vector3): - w = self.w - x = self.x - y = self.y - z = self.z - Vx = other.x - Vy = other.y - Vz = other.z - ww = w * w - w2 = w * 2 - wx2 = w2 * x - wy2 = w2 * y - wz2 = w2 * z - xx = x * x - x2 = x * 2 - xy2 = x2 * y - xz2 = x2 * z - yy = y * y - yz2 = 2 * y * z - zz = z * z - return other.__class__(\ - ww * Vx + wy2 * Vz - wz2 * Vy + \ - xx * Vx + xy2 * Vy + xz2 * Vz - \ - zz * Vx - yy * Vx, - xy2 * Vx + yy * Vy + yz2 * Vz + \ - wz2 * Vx - zz * Vy + ww * Vy - \ - wx2 * Vz - xx * Vy, - xz2 * Vx + yz2 * Vy + \ - zz * Vz - wy2 * Vx - yy * Vz + \ - wx2 * Vy - xx * Vz + ww * Vz) - else: - other = other.copy() - other._apply_transform(self) - return other - - def __imul__(self, other): - assert isinstance(other, Quaternion) - Ax = self.x - Ay = self.y - Az = self.z - Aw = self.w - Bx = other.x - By = other.y - Bz = other.z - Bw = other.w - self.x = Ax * Bw + Ay * Bz - Az * By + Aw * Bx - self.y = -Ax * Bz + Ay * Bw + Az * Bx + Aw * By - self.z = Ax * By - Ay * Bx + Az * Bw + Aw * Bz - self.w = -Ax * Bx - Ay * By - Az * Bz + Aw * Bw - return self - - def __abs__(self): - return math.sqrt(self.w ** 2 + \ - self.x ** 2 + \ - self.y ** 2 + \ - self.z ** 2) - - magnitude = __abs__ - - def magnitude_squared(self): - return self.w ** 2 + \ - self.x ** 2 + \ - self.y ** 2 + \ - self.z ** 2 - - def identity(self): - self.w = 1 - self.x = 0 - self.y = 0 - self.z = 0 - return self - - def rotate_axis(self, angle, axis): - self *= Quaternion.new_rotate_axis(angle, axis) - return self - - def rotate_euler(self, heading, attitude, bank): - self *= Quaternion.new_rotate_euler(heading, attitude, bank) - return self - - def rotate_matrix(self, m): - self *= Quaternion.new_rotate_matrix(m) - return self - - def conjugated(self): - Q = Quaternion() - Q.w = self.w - Q.x = -self.x - Q.y = -self.y - Q.z = -self.z - return Q - - def normalize(self): - d = self.magnitude() - if d != 0: - self.w /= d - self.x /= d - self.y /= d - self.z /= d - return self - - def normalized(self): - d = self.magnitude() - if d != 0: - Q = Quaternion() - Q.w = self.w / d - Q.x = self.x / d - Q.y = self.y / d - Q.z = self.z / d - return Q - else: - return self.copy() - - def get_angle_axis(self): - if self.w > 1: - self = self.normalized() - angle = 2 * math.acos(self.w) - s = math.sqrt(1 - self.w ** 2) - if s < 0.001: - return angle, Vector3(1, 0, 0) - else: - return angle, Vector3(self.x / s, self.y / s, self.z / s) - - def get_euler(self): - t = self.x * self.y + self.z * self.w - if t > 0.4999: - heading = 2 * math.atan2(self.x, self.w) - attitude = math.pi / 2 - bank = 0 - elif t < -0.4999: - heading = -2 * math.atan2(self.x, self.w) - attitude = -math.pi / 2 - bank = 0 - else: - sqx = self.x ** 2 - sqy = self.y ** 2 - sqz = self.z ** 2 - heading = math.atan2(2 * self.y * self.w - 2 * self.x * self.z, - 1 - 2 * sqy - 2 * sqz) - attitude = math.asin(2 * t) - bank = math.atan2(2 * self.x * self.w - 2 * self.y * self.z, - 1 - 2 * sqx - 2 * sqz) - return heading, attitude, bank - - def get_matrix(self): - xx = self.x ** 2 - xy = self.x * self.y - xz = self.x * self.z - xw = self.x * self.w - yy = self.y ** 2 - yz = self.y * self.z - yw = self.y * self.w - zz = self.z ** 2 - zw = self.z * self.w - M = Matrix4() - M.a = 1 - 2 * (yy + zz) - M.b = 2 * (xy - zw) - M.c = 2 * (xz + yw) - M.e = 2 * (xy + zw) - M.f = 1 - 2 * (xx + zz) - M.g = 2 * (yz - xw) - M.i = 2 * (xz - yw) - M.j = 2 * (yz + xw) - M.k = 1 - 2 * (xx + yy) - return M - - # Static constructors - def new_identity(cls): - return cls() - new_identity = classmethod(new_identity) - - def new_rotate_axis(cls, angle, axis): - assert(isinstance(axis, Vector3)) - axis = axis.normalized() - s = math.sin(angle / 2) - Q = cls() - Q.w = math.cos(angle / 2) - Q.x = axis.x * s - Q.y = axis.y * s - Q.z = axis.z * s - return Q - new_rotate_axis = classmethod(new_rotate_axis) - - def new_rotate_euler(cls, heading, attitude, bank): - Q = cls() - c1 = math.cos(heading / 2) - s1 = math.sin(heading / 2) - c2 = math.cos(attitude / 2) - s2 = math.sin(attitude / 2) - c3 = math.cos(bank / 2) - s3 = math.sin(bank / 2) - - Q.w = c1 * c2 * c3 - s1 * s2 * s3 - Q.x = s1 * s2 * c3 + c1 * c2 * s3 - Q.y = s1 * c2 * c3 + c1 * s2 * s3 - Q.z = c1 * s2 * c3 - s1 * c2 * s3 - return Q - new_rotate_euler = classmethod(new_rotate_euler) - - def new_rotate_matrix(cls, m): - if m[0*4 + 0] + m[1*4 + 1] + m[2*4 + 2] > 0.00000001: - t = m[0*4 + 0] + m[1*4 + 1] + m[2*4 + 2] + 1.0 - s = 0.5/math.sqrt(t) - - return cls( - s*t, - (m[1*4 + 2] - m[2*4 + 1])*s, - (m[2*4 + 0] - m[0*4 + 2])*s, - (m[0*4 + 1] - m[1*4 + 0])*s - ) - - elif m[0*4 + 0] > m[1*4 + 1] and m[0*4 + 0] > m[2*4 + 2]: - t = m[0*4 + 0] - m[1*4 + 1] - m[2*4 + 2] + 1.0 - s = 0.5/math.sqrt(t) - - return cls( - (m[1*4 + 2] - m[2*4 + 1])*s, - s*t, - (m[0*4 + 1] + m[1*4 + 0])*s, - (m[2*4 + 0] + m[0*4 + 2])*s - ) - - elif m[1*4 + 1] > m[2*4 + 2]: - t = -m[0*4 + 0] + m[1*4 + 1] - m[2*4 + 2] + 1.0 - s = 0.5/math.sqrt(t) - - return cls( - (m[2*4 + 0] - m[0*4 + 2])*s, - (m[0*4 + 1] + m[1*4 + 0])*s, - s*t, - (m[1*4 + 2] + m[2*4 + 1])*s - ) - - else: - t = -m[0*4 + 0] - m[1*4 + 1] + m[2*4 + 2] + 1.0 - s = 0.5/math.sqrt(t) - - return cls( - (m[0*4 + 1] - m[1*4 + 0])*s, - (m[2*4 + 0] + m[0*4 + 2])*s, - (m[1*4 + 2] + m[2*4 + 1])*s, - s*t - ) - new_rotate_matrix = classmethod(new_rotate_matrix) - - def new_interpolate(cls, q1, q2, t): - assert isinstance(q1, Quaternion) and isinstance(q2, Quaternion) - Q = cls() - - costheta = q1.w * q2.w + q1.x * q2.x + q1.y * q2.y + q1.z * q2.z - if costheta < 0.: - costheta = -costheta - q1 = q1.conjugated() - elif costheta > 1: - costheta = 1 - - theta = math.acos(costheta) - if abs(theta) < 0.01: - Q.w = q2.w - Q.x = q2.x - Q.y = q2.y - Q.z = q2.z - return Q - - sintheta = math.sqrt(1.0 - costheta * costheta) - if abs(sintheta) < 0.01: - Q.w = (q1.w + q2.w) * 0.5 - Q.x = (q1.x + q2.x) * 0.5 - Q.y = (q1.y + q2.y) * 0.5 - Q.z = (q1.z + q2.z) * 0.5 - return Q - - ratio1 = math.sin((1 - t) * theta) / sintheta - ratio2 = math.sin(t * theta) / sintheta - - Q.w = q1.w * ratio1 + q2.w * ratio2 - Q.x = q1.x * ratio1 + q2.x * ratio2 - Q.y = q1.y * ratio1 + q2.y * ratio2 - Q.z = q1.z * ratio1 + q2.z * ratio2 - return Q - new_interpolate = classmethod(new_interpolate) - -# Geometry -# Much maths thanks to Paul Bourke, http://astronomy.swin.edu.au/~pbourke -# --------------------------------------------------------------------------- - -class Geometry: - def _connect_unimplemented(self, other): - raise AttributeError, 'Cannot connect %s to %s' % \ - (self.__class__, other.__class__) - - def _intersect_unimplemented(self, other): - raise AttributeError, 'Cannot intersect %s and %s' % \ - (self.__class__, other.__class__) - - _intersect_point2 = _intersect_unimplemented - _intersect_line2 = _intersect_unimplemented - _intersect_circle = _intersect_unimplemented - _connect_point2 = _connect_unimplemented - _connect_line2 = _connect_unimplemented - _connect_circle = _connect_unimplemented - - _intersect_point3 = _intersect_unimplemented - _intersect_line3 = _intersect_unimplemented - _intersect_sphere = _intersect_unimplemented - _intersect_plane = _intersect_unimplemented - _connect_point3 = _connect_unimplemented - _connect_line3 = _connect_unimplemented - _connect_sphere = _connect_unimplemented - _connect_plane = _connect_unimplemented - - def intersect(self, other): - raise NotImplementedError - - def connect(self, other): - raise NotImplementedError - - def distance(self, other): - c = self.connect(other) - if c: - return c.length - return 0.0 - -def _intersect_point2_circle(P, C): - return abs(P - C.c) <= C.r - -def _intersect_line2_line2(A, B): - d = B.v.y * A.v.x - B.v.x * A.v.y - if d == 0: - return None - - dy = A.p.y - B.p.y - dx = A.p.x - B.p.x - ua = (B.v.x * dy - B.v.y * dx) / d - if not A._u_in(ua): - return None - ub = (A.v.x * dy - A.v.y * dx) / d - if not B._u_in(ub): - return None - - return Point2(A.p.x + ua * A.v.x, - A.p.y + ua * A.v.y) - -def _intersect_line2_circle(L, C): - a = L.v.magnitude_squared() - b = 2 * (L.v.x * (L.p.x - C.c.x) + \ - L.v.y * (L.p.y - C.c.y)) - c = C.c.magnitude_squared() + \ - L.p.magnitude_squared() - \ - 2 * C.c.dot(L.p) - \ - C.r ** 2 - det = b ** 2 - 4 * a * c - if det < 0: - return None - sq = math.sqrt(det) - u1 = (-b + sq) / (2 * a) - u2 = (-b - sq) / (2 * a) - if not L._u_in(u1): - u1 = max(min(u1, 1.0), 0.0) - if not L._u_in(u2): - u2 = max(min(u2, 1.0), 0.0) - - # Tangent - if u1 == u2: - return Point2(L.p.x + u1 * L.v.x, - L.p.y + u1 * L.v.y) - - return LineSegment2(Point2(L.p.x + u1 * L.v.x, - L.p.y + u1 * L.v.y), - Point2(L.p.x + u2 * L.v.x, - L.p.y + u2 * L.v.y)) - -def _connect_point2_line2(P, L): - d = L.v.magnitude_squared() - assert d != 0 - u = ((P.x - L.p.x) * L.v.x + \ - (P.y - L.p.y) * L.v.y) / d - if not L._u_in(u): - u = max(min(u, 1.0), 0.0) - return LineSegment2(P, - Point2(L.p.x + u * L.v.x, - L.p.y + u * L.v.y)) - -def _connect_point2_circle(P, C): - v = P - C.c - v.normalize() - v *= C.r - return LineSegment2(P, Point2(C.c.x + v.x, C.c.y + v.y)) - -def _connect_line2_line2(A, B): - d = B.v.y * A.v.x - B.v.x * A.v.y - if d == 0: - # Parallel, connect an endpoint with a line - if isinstance(B, Ray2) or isinstance(B, LineSegment2): - p1, p2 = _connect_point2_line2(B.p, A) - return p2, p1 - # No endpoint (or endpoint is on A), possibly choose arbitrary point - # on line. - return _connect_point2_line2(A.p, B) - - dy = A.p.y - B.p.y - dx = A.p.x - B.p.x - ua = (B.v.x * dy - B.v.y * dx) / d - if not A._u_in(ua): - ua = max(min(ua, 1.0), 0.0) - ub = (A.v.x * dy - A.v.y * dx) / d - if not B._u_in(ub): - ub = max(min(ub, 1.0), 0.0) - - return LineSegment2(Point2(A.p.x + ua * A.v.x, A.p.y + ua * A.v.y), - Point2(B.p.x + ub * B.v.x, B.p.y + ub * B.v.y)) - -def _connect_circle_line2(C, L): - d = L.v.magnitude_squared() - assert d != 0 - u = ((C.c.x - L.p.x) * L.v.x + (C.c.y - L.p.y) * L.v.y) / d - if not L._u_in(u): - u = max(min(u, 1.0), 0.0) - point = Point2(L.p.x + u * L.v.x, L.p.y + u * L.v.y) - v = (point - C.c) - v.normalize() - v *= C.r - return LineSegment2(Point2(C.c.x + v.x, C.c.y + v.y), point) - -def _connect_circle_circle(A, B): - v = B.c - A.c - v.normalize() - return LineSegment2(Point2(A.c.x + v.x * A.r, A.c.y + v.y * A.r), - Point2(B.c.x - v.x * B.r, B.c.y - v.y * B.r)) - - -class Point2(Vector2, Geometry): - def __repr__(self): - return 'Point2(%.2f, %.2f)' % (self.x, self.y) - - def intersect(self, other): - return other._intersect_point2(self) - - def _intersect_circle(self, other): - return _intersect_point2_circle(self, other) - - def connect(self, other): - return other._connect_point2(self) - - def _connect_point2(self, other): - return LineSegment2(other, self) - - def _connect_line2(self, other): - c = _connect_point2_line2(self, other) - if c: - return c._swap() - - def _connect_circle(self, other): - c = _connect_point2_circle(self, other) - if c: - return c._swap() - -class Line2(Geometry): - __slots__ = ['p', 'v'] - - def __init__(self, *args): - if len(args) == 3: - assert isinstance(args[0], Point2) and \ - isinstance(args[1], Vector2) and \ - type(args[2]) == float - self.p = args[0].copy() - self.v = args[1] * args[2] / abs(args[1]) - elif len(args) == 2: - if isinstance(args[0], Point2) and isinstance(args[1], Point2): - self.p = args[0].copy() - self.v = args[1] - args[0] - elif isinstance(args[0], Point2) and isinstance(args[1], Vector2): - self.p = args[0].copy() - self.v = args[1].copy() - else: - raise AttributeError, '%r' % (args,) - elif len(args) == 1: - if isinstance(args[0], Line2): - self.p = args[0].p.copy() - self.v = args[0].v.copy() - else: - raise AttributeError, '%r' % (args,) - else: - raise AttributeError, '%r' % (args,) - - if not self.v: - raise AttributeError, 'Line has zero-length vector' - - def __copy__(self): - return self.__class__(self.p, self.v) - - copy = __copy__ - - def __repr__(self): - return 'Line2(<%.2f, %.2f> + u<%.2f, %.2f>)' % \ - (self.p.x, self.p.y, self.v.x, self.v.y) - - p1 = property(lambda self: self.p) - p2 = property(lambda self: Point2(self.p.x + self.v.x, - self.p.y + self.v.y)) - - def _apply_transform(self, t): - self.p = t * self.p - self.v = t * self.v - - def _u_in(self, u): - return True - - def intersect(self, other): - return other._intersect_line2(self) - - def _intersect_line2(self, other): - return _intersect_line2_line2(self, other) - - def _intersect_circle(self, other): - return _intersect_line2_circle(self, other) - - def connect(self, other): - return other._connect_line2(self) - - def _connect_point2(self, other): - return _connect_point2_line2(other, self) - - def _connect_line2(self, other): - return _connect_line2_line2(other, self) - - def _connect_circle(self, other): - return _connect_circle_line2(other, self) - -class Ray2(Line2): - def __repr__(self): - return 'Ray2(<%.2f, %.2f> + u<%.2f, %.2f>)' % \ - (self.p.x, self.p.y, self.v.x, self.v.y) - - def _u_in(self, u): - return u >= 0.0 - -class LineSegment2(Line2): - def __repr__(self): - return 'LineSegment2(<%.2f, %.2f> to <%.2f, %.2f>)' % \ - (self.p.x, self.p.y, self.p.x + self.v.x, self.p.y + self.v.y) - - def _u_in(self, u): - return u >= 0.0 and u <= 1.0 - - def __abs__(self): - return abs(self.v) - - def magnitude_squared(self): - return self.v.magnitude_squared() - - def _swap(self): - # used by connect methods to switch order of points - self.p = self.p2 - self.v *= -1 - return self - - length = property(lambda self: abs(self.v)) - -class Circle(Geometry): - __slots__ = ['c', 'r'] - - def __init__(self, center, radius): - assert isinstance(center, Vector2) and type(radius) == float - self.c = center.copy() - self.r = radius - - def __copy__(self): - return self.__class__(self.c, self.r) - - copy = __copy__ - - def __repr__(self): - return 'Circle(<%.2f, %.2f>, radius=%.2f)' % \ - (self.c.x, self.c.y, self.r) - - def _apply_transform(self, t): - self.c = t * self.c - - def intersect(self, other): - return other._intersect_circle(self) - - def _intersect_point2(self, other): - return _intersect_point2_circle(other, self) - - def _intersect_line2(self, other): - return _intersect_line2_circle(other, self) - - def connect(self, other): - return other._connect_circle(self) - - def _connect_point2(self, other): - return _connect_point2_circle(other, self) - - def _connect_line2(self, other): - c = _connect_circle_line2(self, other) - if c: - return c._swap() - - def _connect_circle(self, other): - return _connect_circle_circle(other, self) - -# 3D Geometry -# ------------------------------------------------------------------------- - -def _connect_point3_line3(P, L): - d = L.v.magnitude_squared() - assert d != 0 - u = ((P.x - L.p.x) * L.v.x + \ - (P.y - L.p.y) * L.v.y + \ - (P.z - L.p.z) * L.v.z) / d - if not L._u_in(u): - u = max(min(u, 1.0), 0.0) - return LineSegment3(P, Point3(L.p.x + u * L.v.x, - L.p.y + u * L.v.y, - L.p.z + u * L.v.z)) - -def _connect_point3_sphere(P, S): - v = P - S.c - v.normalize() - v *= S.r - return LineSegment3(P, Point3(S.c.x + v.x, S.c.y + v.y, S.c.z + v.z)) - -def _connect_point3_plane(p, plane): - n = plane.n.normalized() - d = p.dot(plane.n) - plane.k - return LineSegment3(p, Point3(p.x - n.x * d, p.y - n.y * d, p.z - n.z * d)) - -def _connect_line3_line3(A, B): - assert A.v and B.v - p13 = A.p - B.p - d1343 = p13.dot(B.v) - d4321 = B.v.dot(A.v) - d1321 = p13.dot(A.v) - d4343 = B.v.magnitude_squared() - denom = A.v.magnitude_squared() * d4343 - d4321 ** 2 - if denom == 0: - # Parallel, connect an endpoint with a line - if isinstance(B, Ray3) or isinstance(B, LineSegment3): - return _connect_point3_line3(B.p, A)._swap() - # No endpoint (or endpoint is on A), possibly choose arbitrary - # point on line. - return _connect_point3_line3(A.p, B) - - ua = (d1343 * d4321 - d1321 * d4343) / denom - if not A._u_in(ua): - ua = max(min(ua, 1.0), 0.0) - ub = (d1343 + d4321 * ua) / d4343 - if not B._u_in(ub): - ub = max(min(ub, 1.0), 0.0) - return LineSegment3(Point3(A.p.x + ua * A.v.x, - A.p.y + ua * A.v.y, - A.p.z + ua * A.v.z), - Point3(B.p.x + ub * B.v.x, - B.p.y + ub * B.v.y, - B.p.z + ub * B.v.z)) - -def _connect_line3_plane(L, P): - d = P.n.dot(L.v) - if not d: - # Parallel, choose an endpoint - return _connect_point3_plane(L.p, P) - u = (P.k - P.n.dot(L.p)) / d - if not L._u_in(u): - # intersects out of range, choose nearest endpoint - u = max(min(u, 1.0), 0.0) - return _connect_point3_plane(Point3(L.p.x + u * L.v.x, - L.p.y + u * L.v.y, - L.p.z + u * L.v.z), P) - # Intersection - return None - -def _connect_sphere_line3(S, L): - d = L.v.magnitude_squared() - assert d != 0 - u = ((S.c.x - L.p.x) * L.v.x + \ - (S.c.y - L.p.y) * L.v.y + \ - (S.c.z - L.p.z) * L.v.z) / d - if not L._u_in(u): - u = max(min(u, 1.0), 0.0) - point = Point3(L.p.x + u * L.v.x, L.p.y + u * L.v.y, L.p.z + u * L.v.z) - v = (point - S.c) - v.normalize() - v *= S.r - return LineSegment3(Point3(S.c.x + v.x, S.c.y + v.y, S.c.z + v.z), - point) - -def _connect_sphere_sphere(A, B): - v = B.c - A.c - v.normalize() - return LineSegment3(Point3(A.c.x + v.x * A.r, - A.c.y + v.y * A.r, - A.c.x + v.z * A.r), - Point3(B.c.x + v.x * B.r, - B.c.y + v.y * B.r, - B.c.x + v.z * B.r)) - -def _connect_sphere_plane(S, P): - c = _connect_point3_plane(S.c, P) - if not c: - return None - p2 = c.p2 - v = p2 - S.c - v.normalize() - v *= S.r - return LineSegment3(Point3(S.c.x + v.x, S.c.y + v.y, S.c.z + v.z), - p2) - -def _connect_plane_plane(A, B): - if A.n.cross(B.n): - # Planes intersect - return None - else: - # Planes are parallel, connect to arbitrary point - return _connect_point3_plane(A._get_point(), B) - -def _intersect_point3_sphere(P, S): - return abs(P - S.c) <= S.r - -def _intersect_line3_sphere(L, S): - a = L.v.magnitude_squared() - b = 2 * (L.v.x * (L.p.x - S.c.x) + \ - L.v.y * (L.p.y - S.c.y) + \ - L.v.z * (L.p.z - S.c.z)) - c = S.c.magnitude_squared() + \ - L.p.magnitude_squared() - \ - 2 * S.c.dot(L.p) - \ - S.r ** 2 - det = b ** 2 - 4 * a * c - if det < 0: - return None - sq = math.sqrt(det) - u1 = (-b + sq) / (2 * a) - u2 = (-b - sq) / (2 * a) - if not L._u_in(u1): - u1 = max(min(u1, 1.0), 0.0) - if not L._u_in(u2): - u2 = max(min(u2, 1.0), 0.0) - return LineSegment3(Point3(L.p.x + u1 * L.v.x, - L.p.y + u1 * L.v.y, - L.p.z + u1 * L.v.z), - Point3(L.p.x + u2 * L.v.x, - L.p.y + u2 * L.v.y, - L.p.z + u2 * L.v.z)) - -def _intersect_line3_plane(L, P): - d = P.n.dot(L.v) - if not d: - # Parallel - return None - u = (P.k - P.n.dot(L.p)) / d - if not L._u_in(u): - return None - return Point3(L.p.x + u * L.v.x, - L.p.y + u * L.v.y, - L.p.z + u * L.v.z) - -def _intersect_plane_plane(A, B): - n1_m = A.n.magnitude_squared() - n2_m = B.n.magnitude_squared() - n1d2 = A.n.dot(B.n) - det = n1_m * n2_m - n1d2 ** 2 - if det == 0: - # Parallel - return None - c1 = (A.k * n2_m - B.k * n1d2) / det - c2 = (B.k * n1_m - A.k * n1d2) / det - return Line3(Point3(c1 * A.n.x + c2 * B.n.x, - c1 * A.n.y + c2 * B.n.y, - c1 * A.n.z + c2 * B.n.z), - A.n.cross(B.n)) - -class Point3(Vector3, Geometry): - def __repr__(self): - return 'Point3(%.2f, %.2f, %.2f)' % (self.x, self.y, self.z) - - def intersect(self, other): - return other._intersect_point3(self) - - def _intersect_sphere(self, other): - return _intersect_point3_sphere(self, other) - - def connect(self, other): - return other._connect_point3(self) - - def _connect_point3(self, other): - if self != other: - return LineSegment3(other, self) - return None - - def _connect_line3(self, other): - c = _connect_point3_line3(self, other) - if c: - return c._swap() - - def _connect_sphere(self, other): - c = _connect_point3_sphere(self, other) - if c: - return c._swap() - - def _connect_plane(self, other): - c = _connect_point3_plane(self, other) - if c: - return c._swap() - -class Line3: - __slots__ = ['p', 'v'] - - def __init__(self, *args): - if len(args) == 3: - assert isinstance(args[0], Point3) and \ - isinstance(args[1], Vector3) and \ - type(args[2]) == float - self.p = args[0].copy() - self.v = args[1] * args[2] / abs(args[1]) - elif len(args) == 2: - if isinstance(args[0], Point3) and isinstance(args[1], Point3): - self.p = args[0].copy() - self.v = args[1] - args[0] - elif isinstance(args[0], Point3) and isinstance(args[1], Vector3): - self.p = args[0].copy() - self.v = args[1].copy() - else: - raise AttributeError, '%r' % (args,) - elif len(args) == 1: - if isinstance(args[0], Line3): - self.p = args[0].p.copy() - self.v = args[0].v.copy() - else: - raise AttributeError, '%r' % (args,) - else: - raise AttributeError, '%r' % (args,) - - # XXX This is annoying. - #if not self.v: - # raise AttributeError, 'Line has zero-length vector' - - def __copy__(self): - return self.__class__(self.p, self.v) - - copy = __copy__ - - def __repr__(self): - return 'Line3(<%.2f, %.2f, %.2f> + u<%.2f, %.2f, %.2f>)' % \ - (self.p.x, self.p.y, self.p.z, self.v.x, self.v.y, self.v.z) - - p1 = property(lambda self: self.p) - p2 = property(lambda self: Point3(self.p.x + self.v.x, - self.p.y + self.v.y, - self.p.z + self.v.z)) - - def _apply_transform(self, t): - self.p = t * self.p - self.v = t * self.v - - def _u_in(self, u): - return True - - def intersect(self, other): - return other._intersect_line3(self) - - def _intersect_sphere(self, other): - return _intersect_line3_sphere(self, other) - - def _intersect_plane(self, other): - return _intersect_line3_plane(self, other) - - def connect(self, other): - return other._connect_line3(self) - - def _connect_point3(self, other): - return _connect_point3_line3(other, self) - - def _connect_line3(self, other): - return _connect_line3_line3(other, self) - - def _connect_sphere(self, other): - return _connect_sphere_line3(other, self) - - def _connect_plane(self, other): - c = _connect_line3_plane(self, other) - if c: - return c - -class Ray3(Line3): - def __repr__(self): - return 'Ray3(<%.2f, %.2f, %.2f> + u<%.2f, %.2f, %.2f>)' % \ - (self.p.x, self.p.y, self.p.z, self.v.x, self.v.y, self.v.z) - - def _u_in(self, u): - return u >= 0.0 - -class LineSegment3(Line3): - def __repr__(self): - return 'LineSegment3(<%.2f, %.2f, %.2f> to <%.2f, %.2f, %.2f>)' % \ - (self.p.x, self.p.y, self.p.z, - self.p.x + self.v.x, self.p.y + self.v.y, self.p.z + self.v.z) - - def _u_in(self, u): - return u >= 0.0 and u <= 1.0 - - def __abs__(self): - return abs(self.v) - - def magnitude_squared(self): - return self.v.magnitude_squared() - - def _swap(self): - # used by connect methods to switch order of points - self.p = self.p2 - self.v *= -1 - return self - - length = property(lambda self: abs(self.v)) - -class Sphere: - __slots__ = ['c', 'r'] - - def __init__(self, center, radius): - assert isinstance(center, Vector3) and type(radius) == float - self.c = center.copy() - self.r = radius - - def __copy__(self): - return self.__class__(self.c, self.r) - - copy = __copy__ - - def __repr__(self): - return 'Sphere(<%.2f, %.2f, %.2f>, radius=%.2f)' % \ - (self.c.x, self.c.y, self.c.z, self.r) - - def _apply_transform(self, t): - self.c = t * self.c - - def intersect(self, other): - return other._intersect_sphere(self) - - def _intersect_point3(self, other): - return _intersect_point3_sphere(other, self) - - def _intersect_line3(self, other): - return _intersect_line3_sphere(other, self) - - def connect(self, other): - return other._connect_sphere(self) - - def _connect_point3(self, other): - return _connect_point3_sphere(other, self) - - def _connect_line3(self, other): - c = _connect_sphere_line3(self, other) - if c: - return c._swap() - - def _connect_sphere(self, other): - return _connect_sphere_sphere(other, self) - - def _connect_plane(self, other): - c = _connect_sphere_plane(self, other) - if c: - return c - -class Plane: - # n.p = k, where n is normal, p is point on plane, k is constant scalar - __slots__ = ['n', 'k'] - - def __init__(self, *args): - if len(args) == 3: - assert isinstance(args[0], Point3) and \ - isinstance(args[1], Point3) and \ - isinstance(args[2], Point3) - self.n = (args[1] - args[0]).cross(args[2] - args[0]) - self.n.normalize() - self.k = self.n.dot(args[0]) - elif len(args) == 2: - if isinstance(args[0], Point3) and isinstance(args[1], Vector3): - self.n = args[1].normalized() - self.k = self.n.dot(args[0]) - elif isinstance(args[0], Vector3) and type(args[1]) == float: - self.n = args[0].normalized() - self.k = args[1] - else: - raise AttributeError, '%r' % (args,) - - else: - raise AttributeError, '%r' % (args,) - - if not self.n: - raise AttributeError, 'Points on plane are colinear' - - def __copy__(self): - return self.__class__(self.n, self.k) - - copy = __copy__ - - def __repr__(self): - return 'Plane(<%.2f, %.2f, %.2f>.p = %.2f)' % \ - (self.n.x, self.n.y, self.n.z, self.k) - - def _get_point(self): - # Return an arbitrary point on the plane - if self.n.z: - return Point3(0., 0., self.k / self.n.z) - elif self.n.y: - return Point3(0., self.k / self.n.y, 0.) - else: - return Point3(self.k / self.n.x, 0., 0.) - - def _apply_transform(self, t): - p = t * self._get_point() - self.n = t * self.n - self.k = self.n.dot(p) - - def intersect(self, other): - return other._intersect_plane(self) - - def _intersect_line3(self, other): - return _intersect_line3_plane(other, self) - - def _intersect_plane(self, other): - return _intersect_plane_plane(self, other) - - def connect(self, other): - return other._connect_plane(self) - - def _connect_point3(self, other): - return _connect_point3_plane(other, self) - - def _connect_line3(self, other): - return _connect_line3_plane(other, self) - - def _connect_sphere(self, other): - return _connect_sphere_plane(other, self) - - def _connect_plane(self, other): - return _connect_plane_plane(other, self) diff --git a/Tools/autotest/pysim/fgFDM.py b/Tools/autotest/pysim/fgFDM.py deleted file mode 100644 index b6885d7585..0000000000 --- a/Tools/autotest/pysim/fgFDM.py +++ /dev/null @@ -1,209 +0,0 @@ -#!/usr/bin/env python -# parse and construct FlightGear NET FDM packets -# Andrew Tridgell, November 2011 -# released under GNU GPL version 2 or later - -import struct, math - -class fgFDMError(Exception): - '''fgFDM error class''' - def __init__(self, msg): - Exception.__init__(self, msg) - self.message = 'fgFDMError: ' + msg - -class fgFDMVariable(object): - '''represent a single fgFDM variable''' - def __init__(self, index, arraylength, units): - self.index = index - self.arraylength = arraylength - self.units = units - -class fgFDMVariableList(object): - '''represent a list of fgFDM variable''' - def __init__(self): - self.vars = {} - self._nextidx = 0 - - def add(self, varname, arraylength=1, units=None): - self.vars[varname] = fgFDMVariable(self._nextidx, arraylength, units=units) - self._nextidx += arraylength - -class fgFDM(object): - '''a flightgear native FDM parser/generator''' - def __init__(self): - '''init a fgFDM object''' - self.FG_NET_FDM_VERSION = 24 - self.pack_string = '>I 4x 3d 6f 11f 3f 2f I 4I 4f 4f 4f 4f 4f 4f 4f 4f 4f I 4f I 3I 3f 3f 3f I i f 10f' - self.values = [0]*98 - - self.FG_MAX_ENGINES = 4 - self.FG_MAX_WHEELS = 3 - self.FG_MAX_TANKS = 4 - - # supported unit mappings - self.unitmap = { - ('radians', 'degrees') : math.degrees(1), - ('rps', 'dps') : math.degrees(1), - ('feet', 'meters') : 0.3048, - ('fps', 'mps') : 0.3048, - ('knots', 'mps') : 0.514444444, - ('knots', 'fps') : 0.514444444/0.3048, - ('fpss', 'mpss') : 0.3048, - ('seconds', 'minutes') : 60, - ('seconds', 'hours') : 3600, - } - - # build a mapping between variable name and index in the values array - # note that the order of this initialisation is critical - it must - # match the wire structure - self.mapping = fgFDMVariableList() - self.mapping.add('version') - - # position - self.mapping.add('longitude', units='radians') # geodetic (radians) - self.mapping.add('latitude', units='radians') # geodetic (radians) - self.mapping.add('altitude', units='meters') # above sea level (meters) - self.mapping.add('agl', units='meters') # above ground level (meters) - - # attitude - self.mapping.add('phi', units='radians') # roll (radians) - self.mapping.add('theta', units='radians') # pitch (radians) - self.mapping.add('psi', units='radians') # yaw or true heading (radians) - self.mapping.add('alpha', units='radians') # angle of attack (radians) - self.mapping.add('beta', units='radians') # side slip angle (radians) - - # Velocities - self.mapping.add('phidot', units='rps') # roll rate (radians/sec) - self.mapping.add('thetadot', units='rps') # pitch rate (radians/sec) - self.mapping.add('psidot', units='rps') # yaw rate (radians/sec) - self.mapping.add('vcas', units='fps') # calibrated airspeed - self.mapping.add('climb_rate', units='fps') # feet per second - self.mapping.add('v_north', units='fps') # north velocity in local/body frame, fps - self.mapping.add('v_east', units='fps') # east velocity in local/body frame, fps - self.mapping.add('v_down', units='fps') # down/vertical velocity in local/body frame, fps - self.mapping.add('v_wind_body_north', units='fps') # north velocity in local/body frame - self.mapping.add('v_wind_body_east', units='fps') # east velocity in local/body frame - self.mapping.add('v_wind_body_down', units='fps') # down/vertical velocity in local/body - - # Accelerations - self.mapping.add('A_X_pilot', units='fpss') # X accel in body frame ft/sec^2 - self.mapping.add('A_Y_pilot', units='fpss') # Y accel in body frame ft/sec^2 - self.mapping.add('A_Z_pilot', units='fpss') # Z accel in body frame ft/sec^2 - - # Stall - self.mapping.add('stall_warning') # 0.0 - 1.0 indicating the amount of stall - self.mapping.add('slip_deg', units='degrees') # slip ball deflection - - # Engine status - self.mapping.add('num_engines') # Number of valid engines - self.mapping.add('eng_state', self.FG_MAX_ENGINES) # Engine state (off, cranking, running) - self.mapping.add('rpm', self.FG_MAX_ENGINES) # Engine RPM rev/min - self.mapping.add('fuel_flow', self.FG_MAX_ENGINES) # Fuel flow gallons/hr - self.mapping.add('fuel_px', self.FG_MAX_ENGINES) # Fuel pressure psi - self.mapping.add('egt', self.FG_MAX_ENGINES) # Exhuast gas temp deg F - self.mapping.add('cht', self.FG_MAX_ENGINES) # Cylinder head temp deg F - self.mapping.add('mp_osi', self.FG_MAX_ENGINES) # Manifold pressure - self.mapping.add('tit', self.FG_MAX_ENGINES) # Turbine Inlet Temperature - self.mapping.add('oil_temp', self.FG_MAX_ENGINES) # Oil temp deg F - self.mapping.add('oil_px', self.FG_MAX_ENGINES) # Oil pressure psi - - # Consumables - self.mapping.add('num_tanks') # Max number of fuel tanks - self.mapping.add('fuel_quantity', self.FG_MAX_TANKS) - - # Gear status - self.mapping.add('num_wheels') - self.mapping.add('wow', self.FG_MAX_WHEELS) - self.mapping.add('gear_pos', self.FG_MAX_WHEELS) - self.mapping.add('gear_steer', self.FG_MAX_WHEELS) - self.mapping.add('gear_compression', self.FG_MAX_WHEELS) - - # Environment - self.mapping.add('cur_time', units='seconds') # current unix time - self.mapping.add('warp', units='seconds') # offset in seconds to unix time - self.mapping.add('visibility', units='meters') # visibility in meters (for env. effects) - - # Control surface positions (normalized values) - self.mapping.add('elevator') - self.mapping.add('elevator_trim_tab') - self.mapping.add('left_flap') - self.mapping.add('right_flap') - self.mapping.add('left_aileron') - self.mapping.add('right_aileron') - self.mapping.add('rudder') - self.mapping.add('nose_wheel') - self.mapping.add('speedbrake') - self.mapping.add('spoilers') - - self.set('version', self.FG_NET_FDM_VERSION) - - self._packet_size = struct.calcsize(self.pack_string) - - if len(self.values) != self.mapping._nextidx: - raise fgFDMError('Invalid variable list in initialisation') - - def packet_size(self): - '''return expected size of FG FDM packets''' - return self._packet_size - - def convert(self, value, fromunits, tounits): - '''convert a value from one set of units to another''' - if fromunits == tounits: - return value - if (fromunits,tounits) in self.unitmap: - return value * self.unitmap[(fromunits,tounits)] - if (tounits,fromunits) in self.unitmap: - return value / self.unitmap[(tounits,fromunits)] - raise fgFDMError("unknown unit mapping (%s,%s)" % (fromunits, tounits)) - - - def units(self, varname): - '''return the default units of a variable''' - if not varname in self.mapping.vars: - raise fgFDMError('Unknown variable %s' % varname) - return self.mapping.vars[varname].units - - - def variables(self): - '''return a list of available variables''' - return sorted(self.mapping.vars.keys(), - key = lambda v : self.mapping.vars[v].index) - - - def get(self, varname, idx=0, units=None): - '''get a variable value''' - if not varname in self.mapping.vars: - raise fgFDMError('Unknown variable %s' % varname) - if idx >= self.mapping.vars[varname].arraylength: - raise fgFDMError('index of %s beyond end of array idx=%u arraylength=%u' % ( - varname, idx, self.mapping.vars[varname].arraylength)) - value = self.values[self.mapping.vars[varname].index + idx] - if units: - value = self.convert(value, self.mapping.vars[varname].units, units) - return value - - def set(self, varname, value, idx=0, units=None): - '''set a variable value''' - if not varname in self.mapping.vars: - raise fgFDMError('Unknown variable %s' % varname) - if idx >= self.mapping.vars[varname].arraylength: - raise fgFDMError('index of %s beyond end of array idx=%u arraylength=%u' % ( - varname, idx, self.mapping.vars[varname].arraylength)) - if units: - value = self.convert(value, units, self.mapping.vars[varname].units) - self.values[self.mapping.vars[varname].index + idx] = value - - def parse(self, buf): - '''parse a FD FDM buffer''' - try: - t = struct.unpack(self.pack_string, buf) - except struct.error, msg: - raise fgFDMError('unable to parse - %s' % msg) - self.values = list(t) - - def pack(self): - '''pack a FD FDM buffer from current values''' - for i in range(len(self.values)): - if math.isnan(self.values[i]): - self.values[i] = 0 - return struct.pack(self.pack_string, *self.values)