From 088780409606b08be90b5ae7b06c60ca122db7ee Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 2 Dec 2011 15:13:50 +1100 Subject: [PATCH] autotest: imported python quadcopter model as sim_quad.py this allows us to keep it in sync with the main SITL code --- Tools/autotest/pysim/aircraft.py | 62 + Tools/autotest/pysim/euclid.py | 2271 ++++++++++++++++++++++++++++ Tools/autotest/pysim/fgFDM.py | 209 +++ Tools/autotest/pysim/fg_display.py | 75 + Tools/autotest/pysim/mavextra.py | 34 + Tools/autotest/pysim/quadcopter.py | 88 ++ Tools/autotest/pysim/sim_quad.py | 172 +++ 7 files changed, 2911 insertions(+) create mode 100644 Tools/autotest/pysim/aircraft.py create mode 100644 Tools/autotest/pysim/euclid.py create mode 100644 Tools/autotest/pysim/fgFDM.py create mode 100755 Tools/autotest/pysim/fg_display.py create mode 100644 Tools/autotest/pysim/mavextra.py create mode 100755 Tools/autotest/pysim/quadcopter.py create mode 100755 Tools/autotest/pysim/sim_quad.py diff --git a/Tools/autotest/pysim/aircraft.py b/Tools/autotest/pysim/aircraft.py new file mode 100644 index 0000000000..e7a7d09aa1 --- /dev/null +++ b/Tools/autotest/pysim/aircraft.py @@ -0,0 +1,62 @@ +import euclid, math, util + + +class Aircraft(object): + '''a basic aircraft class''' + def __init__(self): + self.home_latitude = 0 + self.home_longitude = 0 + self.home_altitude = 0 + self.ground_level = 0 + self.frame_height = 0.0 + + self.latitude = self.home_latitude + self.longitude = self.home_longitude + self.altitude = self.home_altitude + + self.pitch = 0.0 # degrees + self.roll = 0.0 # degrees + self.yaw = 0.0 # degrees + self.pitch_rate = 0.0 # degrees/s + self.roll_rate = 0.0 # degrees/s + self.yaw_rate = 0.0 # degrees/s + self.velocity = euclid.Vector3(0, 0, 0) # m/s, North, East, Up + self.position = euclid.Vector3(0, 0, 0) # m North, East, Up + self.accel = euclid.Vector3(0, 0, 0) # m/s/s North, East, Up + self.mass = 0.0 + self.update_frequency = 50 # in Hz + self.gravity = 9.8 # m/s/s + self.accelerometer = euclid.Vector3(0, 0, -self.gravity) + + def normalise(self): + '''normalise roll, pitch and yaw + + roll between -180 and 180 + pitch between -180 and 180 + yaw between 0 and 360 + + ''' + def norm(angle, min, max): + while angle > max: + angle -= 360 + while angle < min: + angle += 360 + return angle + self.roll = norm(self.roll, -180, 180) + self.pitch = norm(self.pitch, -180, 180) + self.yaw = norm(self.yaw, 0, 360) + + def update_position(self): + '''update lat/lon/alt from position''' + + radius_of_earth = 6378100.0 # in meters + dlat = math.degrees(math.atan(self.position.x/radius_of_earth)) + dlon = math.degrees(math.atan(self.position.y/radius_of_earth)) + + self.altitude = self.home_altitude + self.position.z + self.latitude = self.home_latitude + dlat + self.longitude = self.home_longitude + dlon + + # work out what the accelerometer would see + self.accelerometer = util.RPY_to_XYZ(self.roll, self.pitch, self.yaw, 1) * -self.gravity + self.accelerometer -= self.accel diff --git a/Tools/autotest/pysim/euclid.py b/Tools/autotest/pysim/euclid.py new file mode 100644 index 0000000000..876cc89738 --- /dev/null +++ b/Tools/autotest/pysim/euclid.py @@ -0,0 +1,2271 @@ +#!/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 new file mode 100644 index 0000000000..b6885d7585 --- /dev/null +++ b/Tools/autotest/pysim/fgFDM.py @@ -0,0 +1,209 @@ +#!/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) diff --git a/Tools/autotest/pysim/fg_display.py b/Tools/autotest/pysim/fg_display.py new file mode 100755 index 0000000000..78bad227ad --- /dev/null +++ b/Tools/autotest/pysim/fg_display.py @@ -0,0 +1,75 @@ +#!/usr/bin/env python + +import socket, struct, time, math +import fgFDM + +class udp_socket(object): + '''a UDP socket''' + def __init__(self, device, blocking=True, input=True): + a = device.split(':') + if len(a) != 2: + print("UDP ports must be specified as host:port") + sys.exit(1) + self.port = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + if input: + self.port.bind((a[0], int(a[1]))) + self.destination_addr = None + else: + self.destination_addr = (a[0], int(a[1])) + if not blocking: + self.port.setblocking(0) + self.last_address = None + + def recv(self,n=1000): + try: + data, self.last_address = self.port.recvfrom(n) + except socket.error as e: + if e.errno in [ 11, 35 ]: + return "" + raise + return data + + def write(self, buf): + try: + if self.destination_addr: + self.port.sendto(buf, self.destination_addr) + else: + self.port.sendto(buf, self.last_addr) + except socket.error: + pass + + + +def ft2m(x): + return x * 0.3048 + +def m2ft(x): + return x / 0.3048 + +def kt2mps(x): + return x * 0.514444444 + +def mps2kt(x): + return x / 0.514444444 + +udp = udp_socket("127.0.0.1:5123") +fgout = udp_socket("127.0.0.1:5124", input=False) + +tlast = time.time() +count = 0 + +fg = fgFDM.fgFDM() + +while True: + buf = udp.recv(1000) + fg.parse(buf) + fgout.write(fg.pack()) + count += 1 + if time.time() - tlast > 1.0: + print("%u FPS len=%u" % (count, len(buf))) + count = 0 + tlast = time.time() + print(fg.get('latitude', units='degrees'), + fg.get('longitude', units='degrees'), + fg.get('altitude', units='meters'), + fg.get('vcas', units='mps')) diff --git a/Tools/autotest/pysim/mavextra.py b/Tools/autotest/pysim/mavextra.py new file mode 100644 index 0000000000..db4cf6f65e --- /dev/null +++ b/Tools/autotest/pysim/mavextra.py @@ -0,0 +1,34 @@ +#!/usr/bin/env python +''' +useful extra functions for use by mavlink clients + +Copyright Andrew Tridgell 2011 +Released under GNU GPL version 3 or later +''' + +from math import * + + +def norm_heading(RAW_IMU, ATTITUDE, declination): + '''calculate heading from RAW_IMU and ATTITUDE''' + xmag = RAW_IMU.xmag + ymag = RAW_IMU.ymag + zmag = RAW_IMU.zmag + pitch = ATTITUDE.pitch + roll = ATTITUDE.roll + + headX = xmag*cos(pitch) + ymag*sin(roll)*sin(pitch) + zmag*cos(roll)*sin(pitch) + headY = ymag*cos(roll) - zmag*sin(roll) + heading = atan2(-headY, headX) + heading = fmod(degrees(heading) + declination + 360, 360) + return heading + +def TrueHeading(SERVO_OUTPUT_RAW): + rc3_min = 1060 + rc3_max = 1850 + p = float(SERVO_OUTPUT_RAW.servo3_raw - rc3_min) / (rc3_max - rc3_min) + return 172 + (1.0-p)*(326 - 172) + +def kmh(mps): + '''convert m/s to Km/h''' + return mps*3.6 diff --git a/Tools/autotest/pysim/quadcopter.py b/Tools/autotest/pysim/quadcopter.py new file mode 100755 index 0000000000..43c734e3cc --- /dev/null +++ b/Tools/autotest/pysim/quadcopter.py @@ -0,0 +1,88 @@ +#!/usr/bin/env python + +from aircraft import Aircraft +import euclid, util, time + + +class QuadCopter(Aircraft): + '''a QuadCopter''' + def __init__(self): + Aircraft.__init__(self) + self.motor_speed = [ 0.0, 0.0, 0.0, 0.0 ] + self.mass = 1.0 # Kg + self.hover_throttle = 0.37 + self.terminal_velocity = 30.0 + self.frame_height = 0.1 + + # scaling from total motor power to Newtons. Allows the copter + # to hover against gravity when each motor is at hover_throttle + self.thrust_scale = (self.mass * self.gravity) / (4.0 * self.hover_throttle) + + self.last_time = time.time() + + def update(self, servos): + for i in range(0, 4): + if servos[i] <= 0.0: + self.motor_speed[i] = 0 + else: + self.motor_speed[i] = servos[i] + + m = self.motor_speed + + # how much time has passed? + t = time.time() + delta_time = t - self.last_time + self.last_time = t + + # rotational acceleration, in degrees/s/s + roll_accel = (m[1] - m[0]) * 5000.0 + pitch_accel = (m[2] - m[3]) * 5000.0 + yaw_accel = -((m[2]+m[3]) - (m[0]+m[1])) * 400.0 + + # update rotational rates + self.roll_rate += roll_accel * delta_time + self.pitch_rate += pitch_accel * delta_time + self.yaw_rate += yaw_accel * delta_time + + # update rotation + self.roll += self.roll_rate * delta_time + self.pitch += self.pitch_rate * delta_time + self.yaw += self.yaw_rate * delta_time + + # air resistance + air_resistance = - self.velocity * (self.gravity/self.terminal_velocity) + + # normalise rotations + self.normalise() + + thrust = (m[0] + m[1] + m[2] + m[3]) * self.thrust_scale # Newtons + accel = thrust / self.mass + + accel3D = util.RPY_to_XYZ(self.roll, self.pitch, self.yaw, accel) + accel3D += euclid.Vector3(0, 0, -self.gravity) + accel3D += air_resistance + + # new velocity vector + self.velocity += accel3D * delta_time + self.accel = accel3D + + # new position vector + old_position = self.position.copy() + self.position += self.velocity * delta_time + + # constrain height to the ground + if self.position.z + self.home_altitude < self.ground_level + self.frame_height: + if old_position.z + self.home_altitude > self.ground_level + self.frame_height: + print("Hit ground at %f m/s" % (-self.velocity.z)) + self.velocity = euclid.Vector3(0, 0, 0) + self.roll_rate = 0 + self.pitch_rate = 0 + self.yaw_rate = 0 + self.roll = 0 + self.pitch = 0 + self.accel = euclid.Vector3(0, 0, 0) + self.position = euclid.Vector3(self.position.x, self.position.y, + self.ground_level + self.frame_height - self.home_altitude) + + # update lat/lon/altitude + self.update_position() diff --git a/Tools/autotest/pysim/sim_quad.py b/Tools/autotest/pysim/sim_quad.py new file mode 100755 index 0000000000..95d4de6615 --- /dev/null +++ b/Tools/autotest/pysim/sim_quad.py @@ -0,0 +1,172 @@ +#!/usr/bin/env python + +from quadcopter import QuadCopter +import euclid, util, time, os, sys, math +import socket, struct +import select, fgFDM + +# find the mavlink.py module +for d in [ 'pymavlink', + os.path.join(os.path.dirname(os.path.realpath(__file__)), '../pymavlink') ]: + if os.path.exists(d): + sys.path.insert(0, d) +import mavlink + + +def sim_send(m, a, r): + '''send flight information to mavproxy and flightgear''' + global fdm + + fdm.set('latitude', a.latitude, units='degrees') + fdm.set('longitude', a.longitude, units='degrees') + fdm.set('altitude', a.altitude, units='meters') + fdm.set('phi', a.roll, units='degrees') + fdm.set('theta', a.pitch, units='degrees') + fdm.set('psi', a.yaw, units='degrees') + fdm.set('phidot', a.roll_rate, units='dps') + fdm.set('thetadot', a.pitch_rate, units='dps') + fdm.set('psidot', a.yaw_rate, units='dps') + fdm.set('vcas', math.sqrt(a.velocity.x*a.velocity.x + a.velocity.y*a.velocity.y), units='mps') + fdm.set('v_north', a.velocity.x, units='mps') + fdm.set('v_east', a.velocity.y, units='mps') + fdm.set('num_engines', 4) + for i in range(4): + fdm.set('rpm', 1000*m[i], idx=i) + fg_out.send(fdm.pack()) + + buf = struct.pack('>ddddddddddddddddI', + a.latitude, a.longitude, util.m2ft(a.altitude), a.yaw, + util.m2ft(a.velocity.x), util.m2ft(a.velocity.y), + util.m2ft(a.accelerometer.x), util.m2ft(a.accelerometer.y), util.m2ft(a.accelerometer.z), + a.roll_rate, a.pitch_rate, a.yaw_rate, + a.roll, a.pitch, a.yaw, + util.m2ft(math.sqrt(a.velocity.x*a.velocity.x + a.velocity.y*a.velocity.y)), + 0x4c56414d) + sim_out.send(buf) + + +def sim_recv(m, a, r): + '''receive control information from SITL''' + while True: + fd = sim_in.fileno() + rin = [fd] + try: + (rin, win, xin) = select.select(rin, [], [], 1.0) + except select.error: + util.check_parent() + continue + if fd in rin: + break + util.check_parent() + buf = sim_in.recv(32) + if len(buf) != 32: + return + (m0, m1, m2, m3, + r[0], r[1], r[2], r[3], r[4], r[5], r[6], r[7]) = struct.unpack('>ffffHHHHHHHH', buf) + m[0] = m0 + m[1] = m1 + m[2] = m2 + m[3] = m3 + + +def interpret_address(addrstr): + '''interpret a IP:port string''' + a = addrstr.split(':') + a[1] = int(a[1]) + return tuple(a) + +################## +# main program +from optparse import OptionParser +parser = OptionParser("sim_quad.py [options]") +parser.add_option("--fgout", dest="fgout", help="flightgear output (IP:port)", default="127.0.0.1:5503") +parser.add_option("--simin", dest="simin", help="SIM input (IP:port)", default="127.0.0.1:5502") +parser.add_option("--simout", dest="simout", help="SIM output (IP:port)", default="127.0.0.1:5501") +parser.add_option("--home", dest="home", type='string', default=None, help="home lat,lng,alt,hdg (required)") +parser.add_option("--rate", dest="rate", type='int', help="SIM update rate", default=50) + +(opts, args) = parser.parse_args() + +for m in [ 'home' ]: + if not opts.__dict__[m]: + print("Missing required option '%s'" % m) + parser.print_help() + sys.exit(1) + +parent_pid = os.getppid() + +# UDP socket addresses +fg_out_address = interpret_address(opts.fgout) +sim_out_address = interpret_address(opts.simout) +sim_in_address = interpret_address(opts.simin) + +# setup output to flightgear +fg_out = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) +fg_out.connect(fg_out_address) +fg_out.setblocking(0) + +# setup input from SITL +sim_in = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) +sim_in.bind(sim_in_address) +sim_in.setblocking(0) + +# setup output to SITL +sim_out = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) +sim_out.connect(sim_out_address) +sim_out.setblocking(0) + +# FG FDM object +fdm = fgFDM.fgFDM() + +# create the quadcopter model +a = QuadCopter() +a.update_frequency = opts.rate + +# motors initially off +m = [0, 0, 0, 0] + +# raw PWM +r = [0, 0, 0, 0, 0, 0, 0, 0] + +lastt = time.time() +frame_count = 0 + +# parse home +v = opts.home.split(',') +if len(v) != 4: + print("home should be lat,lng,alt,hdg") + sys.exit(1) +a.home_latitude = float(v[0]) +a.home_longitude = float(v[1]) +a.home_altitude = float(v[2]) +a.latitude = a.home_latitude +a.longitude = a.home_longitude +a.altitude = a.home_altitude +a.yaw = float(v[3]) +a.ground_level = a.home_altitude +a.position.z = 0 + + +print("Starting at lat=%f lon=%f alt=%.1f heading=%.1f" % ( + a.home_latitude, + a.home_longitude, + a.home_altitude, + a.yaw)) + +while True: + sim_recv(m, a, r) + + # allow for adding inbalance in flight + m2 = m[:] + + a.update(m2) + sim_send(m, a, r) + frame_count += 1 + t = time.time() + if t - lastt > 1.0: + print("%.2f fps zspeed=%.2f zaccel=%.2f h=%.1f a=%.1f yaw=%.1f yawrate=%.1f" % ( + frame_count/(t-lastt), + a.velocity.z, a.accel.z, a.position.z, a.altitude, + a.yaw, a.yaw_rate)) + lastt = t + frame_count = 0