# Source code for Goulib.geom3d

```#!/usr/bin/env python
# coding: utf8
"""
3D geometry
"""
from __future__ import division #"true division" everywhere

__author__ = "Alex Holkner, Philippe Guglielmetti"
__credits__ = [
'http://www.nmt.edu/tcc/help/lang/python/examples/homcoord/']

__docformat__ = 'restructuredtext'
__version__ = '\$Id\$'
__revision__ = '\$Revision\$'

import operator, six, abc

from math import pi,sin,cos,tan,acos,asin,atan2,sqrt,hypot,copysign
from .geom import Geometry,copy

# 3D Geometry
# -------------------------------------------------------------------------

"""
**3D Geometry**

The following classes are available for dealing with simple 3D geometry.
The interfaces are very similar to the 2D classes (but note that you
cannot mix and match 2D and 3D operations).

For example, to find the closest point on a line to a sphere::

>>> sphere = Sphere(Point3(1., 2., 3.,), 2.)
>>> line = Line3(Point3(0., 0., 0.), Point3(-1., -1., 0.))
>>> line.connect(sphere).p1
Point3(1.50, 1.50, 0.00)

To find the corresponding closest point on the sphere to the line::

>>> line.connect(sphere).p2
Point3(1.32, 1.68, 1.05)

XXX I have not checked if these are correct.
"""

def _connect_point3_line3(P, L):
d = L.v.mag2()
# 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 Segment3(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 Segment3(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 Segment3(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.mag2()
denom = A.v.mag2() * d4343 - d4321 ** 2
if denom == 0:
# Parallel, connect an endpoint with a line
if isinstance(B, Ray3) or isinstance(B, Segment3):
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 Segment3(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):
"""
Sphere/Line shortest joining segment
:param S: Sphere
:param L: Line
:return: LineSegment3 of minimal length
"""
d = L.v.mag2()
# 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 = L.point(u)
v = (point - S.c)
v.normalize()
v *= S.r
return Segment3(S.c+v,point)

def _connect_sphere_sphere(A, B):
v = B.c - A.c
d = v.mag()
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 Segment3(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 Segment3(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_line3_sphere(L, S):
a = L.v.mag2()
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.mag2() + \
L.p.mag2() - \
2 * S.c.dot(L.p) - \
S.r ** 2
det = b ** 2 - 4 * a * c
if det < 0:
return None
sq = sqrt(det)
u1 = (-b + sq) / (2 * a)
u2 = (-b - sq) / (2 * a)
p1 = L.point(u1)
p2 = L.point(u2)
if p1 is None:
return p2
if p2 is None:
return p1
return Segment3(p1,p2)

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.mag2()
n2_m = B.n.mag2()
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 Vector3(object):
""" Mutable 3D Vector.
See `Vector2`documentation"""

[docs]    def __init__(self, *args):
"""Constructor.
:param *args: x,y,z values
"""
if len(args) == 1:
value = args[0]
# assert(len(value) == 3)
x, y, z = value
else:
x, y,z = args
self.x = x
self.y = y
self.z = z

@property
def xyz(self):
""":return: tuple (x,y,z)"""
return (self.x, self.y, self.z)

[docs]    def __repr__(self):
return '%s%s' % (self.__class__.__name__,self.xyz)

[docs]    def __eq__(self, other):
try:
return self.xyz == other.xyz
except:
pass
# assert hasattr(other, '__len__') and len(other) == 3
return self.x == other[0] and \
self.y == other[1] and \
self.z == other[2]

[docs]    def __ne__(self, other):
return not self.__eq__(other)

[docs]    def __bool__(self):
return self.x != 0 or self.y != 0 or self.z != 0

__nonzero__=__bool__

[docs]    def __len__(self):
return 3

[docs]    def __iter__(self):
return iter(self.xyz)

try:
# 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)
except:
return Vector3(self.x + other[0],
self.y + other[1],
self.z + other[2])

try:
self.x += other.x
self.y += other.y
self.z += other.z
except:
self.x += other[0]
self.y += other[1]
self.z += other[2]
return self

[docs]    def __sub__(self, other):
try:
# 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)
except:
#assert hasattr(other, '__len__') and len(other) == 3
return Vector3(self.x - other[0],
self.y - other[1],
self.z - other[2])

[docs]    def __rsub__(self, other):
try:
return Vector3(other.x - self.x,
other.y - self.y,
other.z - self.z)
except:
# assert hasattr(other, '__len__') and len(other) == 3
return Vector3(other.x - self[0],
other.y - self[1],
other.z - self[2])

[docs]    def __mul__(self, other):
try:
# 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)
except:
# assert type(other) in (int, int, float)
return Vector3(self.x * other,
self.y * other,
self.z * other)

__rmul__ = __mul__

[docs]    def __imul__(self, other):
# assert type(other) in (int, int, float)
self.x *= other
self.y *= other
self.z *= other
return self

#geometry requires truediv even in Python2

[docs]    def __div__(self, other):
# assert type(other) in (int, int, float)
return Vector3(operator.truediv(self.x, other),
operator.truediv(self.y, other),
operator.truediv(self.z, other))

[docs]    def __rdiv__(self, other):
# assert type(other) in (int, int, float)
return Vector3(operator.truediv(other, self.x),
operator.truediv(other, self.y),
operator.truediv(other, self.z))

[docs]    def __floordiv__(self, other):
# assert type(other) in (int, int, float)
return Vector3(operator.floordiv(self.x, other),
operator.floordiv(self.y, other),
operator.floordiv(self.z, other))

[docs]    def __rfloordiv__(self, other):
# assert type(other) in (int, int, float)
return Vector3(operator.floordiv(other, self.x),
operator.floordiv(other, self.y),
operator.floordiv(other, self.z))

[docs]    def __truediv__(self, other):
# assert type(other) in (int, int, float)
return Vector3(operator.truediv(self.x, other),
operator.truediv(self.y, other),
operator.truediv(self.z, other))

[docs]    def __rtruediv__(self, other):
# assert type(other) in (int, int, float)
return Vector3(operator.truediv(other, self.x),
operator.truediv(other, self.y),
operator.truediv(other, self.z))

[docs]    def __neg__(self):
return Vector3(-self.x, -self.y, -self.z)

[docs]    def __pos__(self):
return Vector3(self)

[docs]    def __abs__(self):
return sqrt(self.mag2())

mag = __abs__

[docs]    def mag2(self):
return self.x ** 2 + self.y ** 2 + self.z ** 2

[docs]    def normalize(self):
d = self.mag()
if d:
self.x /= d
self.y /= d
self.z /= d
return self

[docs]    def normalized(self):
res=copy(self)
return res.normalize()

[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):
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"""

# 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 = sqrt(r2)
ct = cos(theta)
st = 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):
"""angle between two vectors.
:param other: Vector3
:return: float angle in radians to the other vector, or self direction if other=None
"""
return acos(self.dot(other) / (self.mag()*other.mag()))

[docs]    def project(self, other):
"""Return one vector projected on the vector other"""
n = other.normalized()
return self.dot(n)*n

[docs]class Point3(Vector3, Geometry):
"""
A point on a 3D plane.  Construct in the obvious way::

>>> p = Point3(1.0, 2.0, 3.0)
>>> p
Point3(1.00, 2.00, 3.00)

**Point3** subclasses **Vector3**, so all of **Vector3** operators and
methods apply.  In particular, subtracting two points gives a vector::

>>> Point3(1.0, 2.0, 3.0) - Point3(1.0, 0.0, -2.0)
Vector3(0.00, 2.00, 5.00)

The following methods are also defined:

``intersect(other)``
If *other* is a **Sphere**, returns ``True`` iff the point lies within
the sphere.

``connect(other)``
Returns a **LineSegment3** which is the minimum length line segment
that can connect the two shapes.  *other* may be a **Point3**, **Line3**,
**Ray3**, **LineSegment3**, **Sphere** or **Plane**.

``distance(other)``
Returns the absolute minimum distance to *other*.  Internally this
simply returns the length of the result of ``connect``.
"""

[docs]    def intersect(self, other):
"""Point3/object intersection
:return: self Point3 if on other object, None if not
"""
return self if self in other else None

[docs]    def connect(self, other):
return other._connect_point3(self)

def _connect_point3(self, other):
if self != other:
return Segment3(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(Geometry):
"""
A **Line3** is a line on a 3D plane extending to infinity in both directions;
a **Ray3** has a finite end-point and extends to infinity in a single
direction; a **LineSegment3** joins two points.

All three classes support the same constructors, operators and methods,
but may behave differently when calculating intersections etc.

You may construct a line, ray or line segment using any of:

* another line, ray or line segment
* two points
* a point and a vector
* a point, a vector and a length

For example::

>>> Line3(Point3(1.0, 1.0, 1.0), Point3(1.0, 2.0, 3.0))
Line3(<1.00, 1.00, 1.00> + u<0.00, 1.00, 2.00>)
>>> Line3(Point3(0.0, 1.0, 1.0), Vector3(1.0, 1.0, 2.0))
Line3(<0.00, 1.00, 1.00> + u<1.00, 1.00, 2.00>)
>>> Ray3(Point3(1.0, 1.0, 1.0), Vector3(1.0, 1.0, 2.0), 1.0)
Ray3(<1.00, 1.00, 1.00> + u<0.41, 0.41, 0.82>)

Internally, lines, rays and line segments store a Point3 *p* and a
Vector3 *v*.  You can also access (but not set) the two endpoints
*p1* and *p2*.  These may or may not be meaningful for all types of lines.

The following methods are supported by all three classes:

``intersect(other)``
If *other* is a **Sphere**, returns a **LineSegment3** which is the
intersection of the sphere and line, or ``None`` if there is no
intersection.

If *other* is a **Plane**, returns a **Point3** of intersection, or
``None``.

``connect(other)``
Returns a **LineSegment3** which is the minimum length line segment
that can connect the two shapes.  For two parallel lines, this
line segment may be in an arbitrary position.  *other* may be
a **Point3**, **Line3**, **Ray3**, **LineSegment3**, **Sphere** or
**Plane**.

``distance(other)``
Returns the absolute minimum distance to *other*.  Internally this
simply returns the length of the result of ``connect``.

**LineSegment3** also has a *length* property which is read-only.
"""

[docs]    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 = Point3(args[0])
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 = Point3(args[0])
self.v = Vector3(args[1] - args[0])
elif isinstance(args[0], Point3) and isinstance(args[1], Vector3):
self.p = Point3(args[0])
self.v = Vector3(args[1])
else:
raise AttributeError('%r' % (args,))
elif len(args) == 1: #copy constructor
if isinstance(args[0], Line3):
super(Line3,self).__init__(*args)
self.p = Point3(args[0])
self.v = Vector3(args[0])
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'

[docs]    def __repr__(self):
return '%s(%s,%s)' % (self.__class__.__name__,self.p,self.v)

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 point(self, u):
":return: Point3 at parameter u"
if self._u_in(u):
return self.p+u*self.v
else:
return None

[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 _u_in(self, u):
return u >= 0.0

[docs]class Segment3(Line3):
[docs]    def __repr__(self):
return '%s(%s,%s)' % (self.__class__.__name__,self.p,self.p2)

def _u_in(self, u):
return u >= 0.0 and u <= 1.0

[docs]    def __abs__(self):
return abs(self.v)

[docs]    def mag2(self):
return self.v.mag2()

[docs]    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]def Spherical(r,theta,phi):
return Vector3(r*sin(theta)*cos(phi),r*sin(theta)*sin(phi),r*cos(phi))

[docs]class Sphere(Geometry):
"""
Spheres are constructed with a center **Point3** and a radius::

>>> s = Sphere(Point3(1.0, 1.0, 1.0), 0.5)
>>> s

Internally there are two attributes: *c*, giving the center point and

The following methods are supported:

``intersect(other)``:
If *other* is a **Point3**, returns ``True`` iff the point lies
within the sphere.

If *other* is a **Line3**, **Ray3** or **LineSegment3**, returns
a **LineSegment3** giving the intersection, or ``None`` if the
line does not intersect the sphere.

``distance(other)``
Returns the absolute minimum distance to *other*.  Internally this
simply returns the length of the result of ``connect``.
"""

[docs]    def __init__(self, *args):
""":param args: can be
* Sphere
* center, point on sphere
"""
if len(args) == 1: # Circle or derived class
super(Sphere,self).__init__(*args)
self.c = Point3(args[0].c)
self.r = args[0].r
else: #2 first params are used to stay compatible with Arc2
self.c = Point3(args[0])
if isinstance(args[1],(float,int)):
self.r = args[1]
else:
p=Point3(args[1]) #one point on sphere
self.r=abs(p-self.c)

[docs]    def __repr__(self):
return '%s(%s,%g)' % (self.__class__.__name__,self.c,self.r)

[docs]    def __contains__(self,pt):
":return: True if pt is ON or IN the sphere"
return self.c.distance(pt)<=self.r+precision

[docs]    def point(self, u, v):
"""
:param v: float angle from 0 meridian
:return: Point3 on sphere at specified coordinates
"""
return self.c+Spherical(self.r,u,v)

def _apply_transform(self, t):
self.c = t * self.c

[docs]    def intersect(self, other):
return other._intersect_sphere(self)

def _intersect_line3(self, other):
return _intersect_line3_sphere(other, self)

[docs]    def connect(self, other):
"""
minimal joining segment between Sphere and other 3D Object
:param other: Point3, Line3, Sphere, Plane
:return: LineSegment3 of minimal length
"""

return other._connect_sphere(self)

def _connect_point3(self, other):
return _connect_point3_sphere(other, self)

def _connect_line3(self, sphere):
c = _connect_sphere_line3(self, sphere)
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]    def distance_on_sphere(self, phi1,theta1,phi2,theta2):
"""
:param theta1: float angle from 0 meridian
:param theta2: float angle from 0 meridian
"""
# http://www.johndcook.com/blog/python_longitude_latitude/

c = (sin(phi1)*sin(phi2)*cos(theta1 - theta2) + cos(phi1)*cos(phi2))
return self.r*acos( c )

[docs]class Plane(Geometry):
"""
Planes can be constructed with any of:

* three **Point3**'s lying on the plane
* a **Point3** on the plane and the **Vector3** normal
* a **Vector3** normal and *k*, described below.

Internally, planes are stored with the normal *n* and constant *k* such
that *n.p* = *k* for any point on the plane *p*.

The following methods are supported:

``intersect(other)``
If *other* is a **Line3**, **Ray3** or **LineSegment3**, returns a
**Point3** of intersection, or ``None`` if there is no intersection.

If *other* is a **Plane**, returns the **Line3** of intersection.

``connect(other)``
Returns a **LineSegment3** which is the minimum length line segment
that can connect the two shapes. *other* may be a **Point3**, **Line3**,
**Ray3**, **LineSegment3**, **Sphere** or **Plane**.

``distance(other)``
Returns the absolute minimum distance to *other*.  Internally this
simply returns the length of the result of ``connect``.
"""
# n.p = k, where n is normal, p is point on plane, k is constant scalar

[docs]    def __init__(self, *args):
if len(args) == 3:
self.n = (Point3(args[1]) - Point3(args[0])).cross(Point3(args[2]) - Point3(args[0]))
self.n.normalize()
self.k = self.n.dot(Point3(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')

[docs]    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)

# a b c d
# e f g h
# i j k l
# m n o p

[docs]class Matrix4(object):

[docs]    def __init__(self, *args):
self.identity()
if not args:
return
if len(args)==1:
args=args[0][:]
if len(args)==16:
self[:] = args
else:
raise RuntimeError('%s.__init__(%s) failed'%(self.__class__.__name__,object))

[docs]    def __repr__(self):
t=self.transposed() #repr is by line while [:] is by column
return ('%s%s') % (self.__class__.__name__,tuple(t))

[docs]    def __iter__(self):
return iter((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))

[docs]    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]

[docs]    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

[docs]    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
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 = copy(other)
other._apply_transform(self)
return other

[docs]    def __call__(self,other):
return self*other

[docs]    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
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):
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 = copy(self)
M.transpose()
return M

# Static constructors
[docs]    @classmethod
def new(cls, *values):
M = cls()
M[:] = values
return M

[docs]    @classmethod
def new_identity(cls):
self = cls()
return self

[docs]    @classmethod
def new_scale(cls, x, y, z):
self = cls()
self.a = x
self.f = y
self.k = z
return self

[docs]    @classmethod
def new_translate(cls, x, y, z):
self = cls()
self.d = x
self.h = y
self.l = z
return self

[docs]    @classmethod
def new_rotatex(cls, angle):
self = cls()
s = sin(angle)
c = cos(angle)
self.f = self.k = c
self.g = -s
self.j = s
return self

[docs]    @classmethod
def new_rotatey(cls, angle):
self = cls()
s = sin(angle)
c = cos(angle)
self.a = self.k = c
self.c = s
self.i = -s
return self

[docs]    @classmethod
def new_rotatez(cls, angle):
self = cls()
s = sin(angle)
c = cos(angle)
self.a = self.f = c
self.b = -s
self.e = s
return self

[docs]    @classmethod
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 = sin(angle)
c = 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

[docs]    @classmethod
# from http://www.euclideanspace.com/
ca = cos(attitude)
sa = sin(attitude)
cb = cos(bank)
sb = 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

[docs]    @classmethod
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

[docs]    @classmethod
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

[docs]    @classmethod
def new_perspective(cls, fov_y, aspect, near, far):
# from the gluPerspective man page
f = 1 / 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

[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:
"""
A quaternion represents a three-dimensional rotation or reflection
transformation.  They are the preferred way to store and manipulate
rotations in 3D applications, as they do not suffer the same numerical

The quaternion constructor initializes to the identity transform::

>>> q = Quaternion()
>>> q
Quaternion(real=1.00, imag=<0.00, 0.00, 0.00>)

**Element access**

Internally, the quaternion is stored as four attributes: ``x``, ``y`` and
``z`` forming the imaginary vector, and ``w`` the real component.

**Constructors**

Rotations can be formed using the constructors:

``new_identity()``
Equivalent to the default constructor.

``new_rotate_axis(angle, axis)``
Equivalent to the Matrix4 constructor of the same name.  *angle* is
specified in radians, *axis* is an instance of **Vector3**.  It is
not necessary to normalize the axis.  Example::

>>> q = Quaternion.new_rotate_axis(math.pi / 2, Vector3(1, 0, 0))
>>> q
Quaternion(real=0.71, imag=<0.71, 0.00, 0.00>)

Equivalent to the Matrix4 constructor of the same name.  *heading*
is a rotation around the Y axis, *attitude* around the X axis and
*bank* around the Z axis.  All angles are given in radians.  Example::

>>> q = Quaternion.new_rotate_euler(math.pi / 2, math.pi / 2, 0)
>>> q
Quaternion(real=0.50, imag=<0.50, 0.50, 0.50>)

``new_interpolate(q1, q2, t)``
Create a quaternion which gives a (SLERP) interpolated rotation
between *q1* and *q2*.  *q1* and *q2* are instances of **Quaternion**,
and *t* is a value between 0.0 and 1.0.  For example::

>>> q1 = Quaternion.new_rotate_axis(math.pi / 2, Vector3(1, 0, 0))
>>> q2 = Quaternion.new_rotate_axis(math.pi / 2, Vector3(0, 1, 0))
>>> for i in range(11):
...     print Quaternion.new_interpolate(q1, q2, i / 10.0)
...
Quaternion(real=0.71, imag=<0.71, 0.00, 0.00>)
Quaternion(real=0.75, imag=<0.66, 0.09, 0.00>)
Quaternion(real=0.78, imag=<0.61, 0.17, 0.00>)
Quaternion(real=0.80, imag=<0.55, 0.25, 0.00>)
Quaternion(real=0.81, imag=<0.48, 0.33, 0.00>)
Quaternion(real=0.82, imag=<0.41, 0.41, 0.00>)
Quaternion(real=0.81, imag=<0.33, 0.48, 0.00>)
Quaternion(real=0.80, imag=<0.25, 0.55, 0.00>)
Quaternion(real=0.78, imag=<0.17, 0.61, 0.00>)
Quaternion(real=0.75, imag=<0.09, 0.66, 0.00>)
Quaternion(real=0.71, imag=<0.00, 0.71, 0.00>)

**Operators**

Quaternions may be multiplied to compound rotations.  For example, to
rotate 90 degrees around the X axis and then 90 degrees around the Y axis::

>>> q1 = Quaternion.new_rotate_axis(math.pi / 2, Vector3(1, 0, 0))
>>> q2 = Quaternion.new_rotate_axis(math.pi / 2, Vector3(0, 1, 0))
>>> q1 * q2
Quaternion(real=0.50, imag=<0.50, 0.50, 0.50>)

Multiplying a quaternion by a vector gives a vector, transformed
appropriately::

>>> q = Quaternion.new_rotate_axis(math.pi / 2, Vector3(0, 1, 0))
>>> q * Vector3(1.0, 0, 0)
Vector3(0.00, 0.00, -1.00)

Similarly, any 3D object can be multiplied (e.g., **Point3**, **Line3**,
**Sphere**, etc)::

>>> q * Ray3(Point3(1., 1., 1.), Vector3(1., 1., 1.))
Ray3(<1.00, 1.00, -1.00> + u<1.00, 1.00, -1.00>)

As with the matrix classes, the constructors are also available as in-place
operators.  These are named ``identity``, ``rotate_euler`` and
``rotate_axis``.  For example::

>>> q1 = Quaternion()
>>> q1.rotate_euler(math.pi / 2, math.pi / 2, 0)
Quaternion(real=0.50, imag=<0.50, 0.50, 0.50>)
>>> q1
Quaternion(real=0.50, imag=<0.50, 0.50, 0.50>)

Quaternions are usually unit length, but you may wish to use sized
quaternions.  In this case, you can find the magnitude using ``abs``,
``magnitude`` and ``magnitude_squared``, as with the vector classes.
Example::

>>> q1 = Quaternion()
>>> abs(q1)
1.0
>>> q1.magnitude()
1.0

Similarly, the class implements ``normalize`` and ``normalized`` in the
same way as the vectors.

The following methods do not alter the quaternion:

``conjugated()``
Returns a quaternion that is the conjugate of the instance.  For
example::

>>> q1 = Quaternion.new_rotate_axis(math.pi / 2, Vector3(1, 0, 0))
>>> q1.conjugated()
Quaternion(real=0.71, imag=<-0.71, -0.00, -0.00>)
>>> q1
Quaternion(real=0.71, imag=<0.71, 0.00, 0.00>)

``get_angle_axis()``
Returns a tuple (angle, axis), giving the angle to rotate around an
axis equivalent to the quaternion.  For example::

>>> q1 = Quaternion.new_rotate_axis(math.pi / 2, Vector3(1, 0, 0))
>>> q1.get_angle_axis()
(1.5707963267948966, Vector3(1.00, 0.00, 0.00))

``get_matrix()``
Returns a **Matrix4** implementing the transformation of the quaternion.
For example::

>>> q1 = Quaternion.new_rotate_axis(math.pi / 2, Vector3(1, 0, 0))
>>> q1.get_matrix()
Matrix4([    1.00     0.00     0.00     0.00
0.00     0.00    -1.00     0.00
0.00     1.00     0.00     0.00
0.00     0.00     0.00     1.00])
"""
# 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

[docs]    def __init__(self, w=1, x=0, y=0, z=0):
super(Quaternion,self).__init__() #TODO: add a copy constructor one day
self.w = w
self.x = x
self.y = y
self.z = z

[docs]    def __repr__(self):
return '%s(%g,%g,%g,%g)' % (self.__class__.__name__,self.w, self.x, self.y, self.z)

[docs]    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 = copy(other)
other._apply_transform(self)
return other

[docs]    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

[docs]    def mag2(self):
return self.w ** 2 + self.x ** 2 + self.y ** 2 + self.z ** 2

[docs]    def __abs__(self):
return sqrt(self.mag2())

mag = __abs__

[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):
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.mag()
if d != 0:
self.w /= d
self.x /= d
self.y /= d
self.z /= d
return self

[docs]    def normalized(self):
res=copy(self)
return res.normalize()

[docs]    def get_angle_axis(self):
if self.w > 1:
self = self.normalized()
angle = 2 * acos(self.w)
s = 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 * atan2(self.x, self.w)
attitude = pi / 2
bank = 0
elif t < -0.4999:
heading = -2 * atan2(self.x, self.w)
attitude = -pi / 2
bank = 0
else:
sqx = self.x ** 2
sqy = self.y ** 2
sqz = self.z ** 2
heading = atan2(2 * self.y * self.w - 2 * self.x * self.z,
1 - 2 * sqy - 2 * sqz)
attitude = asin(2 * t)
bank = atan2(2 * self.x * self.w - 2 * self.y * self.z,
1 - 2 * sqx - 2 * sqz)

[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]    @classmethod
def new_identity(cls):
return cls()

[docs]    @classmethod
def new_rotate_axis(cls, angle, axis):
# assert(isinstance(axis, Vector3))
axis = axis.normalized()
s = sin(angle / 2)
Q = cls()
Q.w = cos(angle / 2)
Q.x = axis.x * s
Q.y = axis.y * s
Q.z = axis.z * s
return Q

[docs]    @classmethod
Q = cls()
c2 = cos(attitude / 2)
s2 = sin(attitude / 2)
c3 = cos(bank / 2)
s3 = 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

[docs]    @classmethod
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/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/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/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/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
)
[docs]    @classmethod
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 = 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 = 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 = sin((1 - t) * theta) / sintheta
ratio2 = 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

```