Source code for honeybee_plus.vectormath.euclid

#!/usr/bin/env python
#
# euclid graphics maths module
#
# Copyright (c) 2006 Alex Holkner
# Alex.Holkner@mail.google.com
# https://code.google.com/p/pyeuclid/
#
# 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
import sys

import sys
if (sys.version_info >= (3, 0)):
    long = int

# 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


[docs]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__
[docs] def magnitude_squared(self): return self.x ** 2 + \ self.y ** 2
[docs] def normalize(self): d = self.magnitude() if d: self.x /= d self.y /= d return self
[docs] def normalized(self): d = self.magnitude() if d: return Vector2(self.x / d, self.y / d) return self.copy()
[docs] def dot(self, other): assert isinstance(other, Vector2) return self.x * other.x + \ self.y * other.y
[docs] def cross(self): return Vector2(self.y, -self.x)
[docs] 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)
[docs] def angle(self, other): """Return the angle to the vector other""" return math.acos(self.dot(other) / (self.magnitude() * other.magnitude()))
[docs] def project(self, other): """Return one vector projected on the vector other""" n = other.normalized() return self.dot(n) * n
[docs]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 @property def X(self): """Return x value.""" return self.x @property def Y(self): """Return y value.""" return self.y @property def Z(self): """Return z value.""" return self.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 __str__(self): return '%.3f %.3f %.3f' % (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__
[docs] def magnitude_squared(self): return self.x ** 2 + \ self.y ** 2 + \ self.z ** 2
[docs] def normalize(self): d = self.magnitude() if d: self.x /= d self.y /= d self.z /= d return self
[docs] def normalized(self): d = self.magnitude() if d: return Vector3(self.x / d, self.y / d, self.z / d) return self.copy()
[docs] def flip(self): """Flip vector""" # added by Mostapha Sadeghipour self.x = -self.x self.y = -self.y self.z = -self.z return self
[docs] def flipped(self): """Return flipped vector""" # added by Mostapha Sadeghipour return Vector3(-self.x, -self.y, -self.z)
[docs] def dot(self, other): assert isinstance(other, Vector3) return self.x * other.x + \ self.y * other.y + \ self.z * other.z
[docs] 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)
[docs] 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)
[docs] def rotate_around(self, axis, theta): """Return the vector rotated around axis through angle theta. Right hand rule applies. """ # Adapted from equations published by Glenn Murray. # http://inside.mines.edu/~gmurray/ArbitraryAxisRotation/ArbitraryAxisRotation.html x, y, z = self.x, self.y, self.z u, v, w = axis.x, axis.y, axis.z # Extracted common factors for simplicity and efficiency r2 = u**2 + v**2 + w**2 r = math.sqrt(r2) ct = math.cos(theta) st = math.sin(theta) / r dt = (u * x + v * y + w * z) * (1 - ct) / r2 return Vector3((u * dt + x * ct + (-w * y + v * z) * st), (v * dt + y * ct + (w * x - u * z) * st), (w * dt + z * ct + (-v * x + u * y) * st))
[docs] def angle(self, other): """Return the angle to the vector other""" return math.acos(self.dot(other) / (self.magnitude() * other.magnitude()))
[docs] def project(self, other): """Return one vector projected on the vector other""" n = other.normalized() return self.dot(n) * n
# a b c # e f g # i j k
[docs]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
[docs] 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
[docs] def scale(self, x, y): self *= Matrix3.new_scale(x, y) return self
[docs] def translate(self, x, y): self *= Matrix3.new_translate(x, y) return self
[docs] def rotate(self, angle): self *= Matrix3.new_rotate(angle) return self
# Static constructors
[docs] def new_identity(cls): self = cls() return self
new_identity = classmethod(new_identity)
[docs] def new_scale(cls, x, y): self = cls() self.a = x self.f = y return self
new_scale = classmethod(new_scale)
[docs] def new_translate(cls, x, y): self = cls() self.c = x self.g = y return self
new_translate = classmethod(new_translate)
[docs] 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)
[docs] 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)
[docs] 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
[docs]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
[docs] 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
[docs] 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
[docs] def scale(self, x, y, z): self *= Matrix4.new_scale(x, y, z) return self
[docs] def translate(self, x, y, z): self *= Matrix4.new_translate(x, y, z) return self
[docs] def rotatex(self, angle): self *= Matrix4.new_rotatex(angle) return self
[docs] def rotatey(self, angle): self *= Matrix4.new_rotatey(angle) return self
[docs] def rotatez(self, angle): self *= Matrix4.new_rotatez(angle) return self
[docs] def rotate_axis(self, angle, axis): self *= Matrix4.new_rotate_axis(angle, axis) return self
[docs] def rotate_euler(self, heading, attitude, bank): self *= Matrix4.new_rotate_euler(heading, attitude, bank) return self
[docs] def rotate_triple_axis(self, x, y, z): self *= Matrix4.new_rotate_triple_axis(x, y, z) return self
[docs] 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)
[docs] def transposed(self): M = self.copy() M.transpose() return M
# Static constructors
[docs] def new(cls, *values): M = cls() M[:] = values return M
new = classmethod(new)
[docs] def new_identity(cls): self = cls() return self
new_identity = classmethod(new_identity)
[docs] 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)
[docs] 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)
[docs] 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)
[docs] 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)
[docs] 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)
[docs] 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)
[docs] 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)
[docs] 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)
[docs] 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)
[docs] 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)
[docs] 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))
[docs] 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
[docs]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__
[docs] def magnitude_squared(self): return self.w ** 2 + \ self.x ** 2 + \ self.y ** 2 + \ self.z ** 2
[docs] def identity(self): self.w = 1 self.x = 0 self.y = 0 self.z = 0 return self
[docs] def rotate_axis(self, angle, axis): self *= Quaternion.new_rotate_axis(angle, axis) return self
[docs] def rotate_euler(self, heading, attitude, bank): self *= Quaternion.new_rotate_euler(heading, attitude, bank) return self
[docs] def rotate_matrix(self, m): self *= Quaternion.new_rotate_matrix(m) return self
[docs] def conjugated(self): Q = Quaternion() Q.w = self.w Q.x = -self.x Q.y = -self.y Q.z = -self.z return Q
[docs] def normalize(self): d = self.magnitude() if d != 0: self.w /= d self.x /= d self.y /= d self.z /= d return self
[docs] 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()
[docs] 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)
[docs] 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
[docs] 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
[docs] def new_identity(cls): return cls()
new_identity = classmethod(new_identity)
[docs] 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)
[docs] 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)
[docs] 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)
[docs] 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 # ---------------------------------------------------------------------------
[docs]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
[docs] def intersect(self, other): raise NotImplementedError
[docs] def connect(self, other): raise NotImplementedError
[docs] 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 d = v.magnitude() if A.r >= B.r and d < A.r: # centre B inside A s1, s2 = +1, +1 elif B.r > A.r and d < B.r: # centre A inside B s1, s2 = -1, -1 elif d >= A.r and d >= B.r: s1, s2 = +1, -1 v.normalize() return LineSegment2(Point2(A.c.x + s1 * v.x * A.r, A.c.y + s1 * v.y * A.r), Point2(B.c.x + s2 * v.x * B.r, B.c.y + s2 * v.y * B.r))
[docs]class Point2(Vector2, Geometry): def __repr__(self): return 'Point2(%.2f, %.2f)' % (self.x, self.y)
[docs] def intersect(self, other): return other._intersect_point2(self)
def _intersect_circle(self, other): return _intersect_point2_circle(self, other)
[docs] 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()
[docs]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 \ isinstance(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
[docs] 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)
[docs] 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)
[docs]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
[docs]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)
[docs] 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))
[docs]class Circle(Geometry): __slots__ = ['c', 'r'] def __init__(self, center, radius): assert isinstance(center, Vector2) and isinstance(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
[docs] 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)
[docs] 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 d = v.magnitude() if A.r >= B.r and d < A.r: # centre B inside A s1, s2 = +1, +1 elif B.r > A.r and d < B.r: # centre A inside B s1, s2 = -1, -1 elif d >= A.r and d >= B.r: s1, s2 = +1, -1 v.normalize() return LineSegment3(Point3(A.c.x + s1 * v.x * A.r, A.c.y + s1 * v.y * A.r, A.c.z + s1 * v.z * A.r), Point3(B.c.x + s2 * v.x * B.r, B.c.y + s2 * v.y * B.r, B.c.z + s2 * 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))
[docs]class Point3(Vector3, Geometry): def __repr__(self): return 'Point3(%.2f, %.2f, %.2f)' % (self.x, self.y, self.z)
[docs] def intersect(self, other): return other._intersect_point3(self)
def _intersect_sphere(self, other): return _intersect_point3_sphere(self, other)
[docs] 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()
[docs]class Line3: __slots__ = ['p', 'v'] def __init__(self, *args): if len(args) == 3: assert isinstance(args[0], Point3) and \ isinstance(args[1], Vector3) and \ isinstance(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
[docs] 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)
[docs] 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
[docs]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
[docs]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)
[docs] 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))
[docs]class Sphere: __slots__ = ['c', 'r'] def __init__(self, center, radius): assert isinstance(center, Vector3) and isinstance(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
[docs] 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)
[docs] 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
[docs]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 isinstance(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)
[docs] 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)
[docs] 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)