481 lines
12 KiB
Python
481 lines
12 KiB
Python
#------------------------------------------------------------------------------
|
|
# simple 3D vector / matrix class
|
|
#
|
|
# (c) 9.1999, Martin Strubel // onk@section5.de
|
|
# updated 4.2001
|
|
#
|
|
# This module consists of a rather low level command oriented
|
|
# and a more OO oriented part for 3D vector/matrix manipulation
|
|
#
|
|
# For documentation, please look at the EXAMPLE code below - execute by:
|
|
#
|
|
# > python vect.py
|
|
#
|
|
#
|
|
# permission to use in scientific and free programs granted
|
|
# In doubt, please contact author.
|
|
#
|
|
# history:
|
|
#
|
|
# 1.5: Euler/Rotation matrix support moved here
|
|
# 1.4: high level Vector/Matrix classes extended/improved
|
|
#
|
|
|
|
"""Vector and matrix math module
|
|
|
|
Version 1.5
|
|
by onk@section5.de
|
|
|
|
This is a lightweight 3D matrix and vector module, providing basic vector
|
|
and matrix math plus a more object oriented layer.
|
|
|
|
For examples, look at vect.test()
|
|
"""
|
|
|
|
VERSION = 1.5
|
|
|
|
TOLERANCE = 0.0000001
|
|
|
|
VectorType = 'Vector3'
|
|
MatrixType = 'Matrix3'
|
|
FloatType = type(1.0)
|
|
|
|
def dot(x, y):
|
|
"(x,y) - Returns the dot product of vector 'x' and 'y'"
|
|
return (x[0] * y[0] + x[1] * y[1] + x[2] * y[2])
|
|
|
|
def cross(x, y):
|
|
"(x,y) - Returns the cross product of vector 'x' and 'y'"
|
|
return (x[1] * y[2] - x[2] * y[1],
|
|
x[2] * y[0] - x[0] * y[2],
|
|
x[0] * y[1] - x[1] * y[0])
|
|
|
|
def matrix():
|
|
"Returns Unity matrix"
|
|
return ((1.0, 0.0, 0.0), (0.0, 1.0, 0.0), (0.0, 0.0, 1.0))
|
|
|
|
def matxvec(m, x):
|
|
"y = matxvec(m,x) - Returns product of Matrix 'm' and vector 'x'"
|
|
vx = m[0][0] * x[0] + m[1][0] * x[1] + m[2][0] * x[2]
|
|
vy = m[0][1] * x[0] + m[1][1] * x[1] + m[2][1] * x[2]
|
|
vz = m[0][2] * x[0] + m[1][2] * x[1] + m[2][2] * x[2]
|
|
return (vx, vy, vz)
|
|
|
|
def matfromnormal(z, y = (0.0, 1.0, 0.0)):
|
|
"""(z, y) - returns transformation matrix for local coordinate system
|
|
where 'z' = local z, with optional *up* axis 'y'"""
|
|
y = norm3(y)
|
|
x = cross(y, z)
|
|
y = cross(z, x)
|
|
return (x, y, z)
|
|
|
|
def matxmat(m, n):
|
|
"(m,n) - Returns matrix product of 'm' and 'n'"
|
|
return (matxvec(m, n[0]), matxvec(m, n[1]), matxvec(m, n[2]))
|
|
|
|
def len(x):
|
|
"(x) - Returns the length of vector 'x'"
|
|
import math
|
|
return math.sqrt(x[0]*x[0] + x[1]*x[1] + x[2]*x[2])
|
|
|
|
len3 = len # compatiblity reasons
|
|
|
|
def norm3(x):
|
|
"(x) - Returns the vector 'x' normed to 1.0"
|
|
import math
|
|
r = math.sqrt(x[0]*x[0] + x[1]*x[1] + x[2]*x[2])
|
|
return (x[0]/r, x[1]/r, x[2]/r)
|
|
|
|
def add3(x, y):
|
|
"(x,y) - Returns vector ('x' + 'y')"
|
|
return (x[0]+y[0], x[1]+y[1], x[2]+y[2])
|
|
|
|
def sub3(x, y):
|
|
"(x,y) - Returns vector ('x' - 'y')"
|
|
return ((x[0] - y[0]), (x[1] - y[1]), (x[2] - y[2]))
|
|
|
|
def dist3(x, y):
|
|
"(x,y) - Returns euclidian distance from Point 'x' to 'y'"
|
|
return len3(sub3(x, y))
|
|
|
|
def scale3(s, x):
|
|
"(s,x) - Returns the vector 'x' scaled by 's'"
|
|
return (s*x[0], s*x[1], s*x[2])
|
|
|
|
def scalemat(s,m):
|
|
"(s,m) - Returns the Matrix 'm' scaled by 's'"
|
|
return (scale3(s, m[0]), scale3(s, m[1]), scale3(s,m[2]))
|
|
|
|
def invmatdet(m):
|
|
"""n, det = invmat(m) - Inverts matrix without determinant correction.
|
|
Inverse matrix 'n' and Determinant 'det' are returned"""
|
|
|
|
# Matrix: (row vectors)
|
|
# 00 10 20
|
|
# 01 11 21
|
|
# 02 12 22
|
|
|
|
wk = [0.0, 0.0, 0.0]
|
|
|
|
t = m[1][1] * m[2][2] - m[1][2] * m[2][1]
|
|
wk[0] = t
|
|
det = t * m[0][0]
|
|
|
|
t = m[2][1] * m[0][2] - m[0][1] * m[2][2]
|
|
wk[1] = t
|
|
det = det + t * m[1][0]
|
|
|
|
t = m[0][1] * m[1][2] - m[1][1] * m[0][2]
|
|
wk[2] = t
|
|
det = det + t * m[2][0]
|
|
|
|
v0 = (wk[0], wk[1], wk[2])
|
|
|
|
t = m[2][0] * m[1][2] - m[1][0] * m[2][2]
|
|
wk[0] = t
|
|
det = det + t * m[0][1]
|
|
|
|
t = m[0][0] * m[2][2] - m[0][2] * m[2][0]
|
|
wk[1] = t
|
|
det = det + t * m[1][1]
|
|
|
|
t = m[1][0] * m[0][2] - m[0][0] * m[1][2]
|
|
wk[2] = t
|
|
det = det + t * m[2][1]
|
|
|
|
v1 = (wk[0], wk[1], wk[2])
|
|
|
|
t = m[1][0] * m[2][1] - m[1][1] * m[2][0]
|
|
wk[0] = t
|
|
det = det + t * m[0][2]
|
|
|
|
t = m[2][0] * m[0][1] - m[0][0] * m[2][1]
|
|
wk[1] = t
|
|
det = det + t * m[1][2]
|
|
|
|
t = m[0][0] * m[1][1] - m[1][0] * m[0][1]
|
|
wk[2] = t
|
|
det = det + t * m[2][2]
|
|
|
|
v2 = (wk[0], wk[1], wk[2])
|
|
# det = 3 * determinant
|
|
return ((v0,v1,v2), det/3.0)
|
|
|
|
def invmat(m):
|
|
"(m) - Inverts the 3x3 matrix 'm', result in 'n'"
|
|
n, det = invmatdet(m)
|
|
if det < 0.000001:
|
|
raise ZeroDivisionError, "minor rank matrix"
|
|
d = 1.0/det
|
|
return (scale3(d, n[0]),
|
|
scale3(d, n[1]),
|
|
scale3(d, n[2]))
|
|
|
|
def transmat(m):
|
|
# can be used to invert orthogonal rotation matrices
|
|
"(m) - Returns transposed matrix of 'm'"
|
|
return ((m[0][0], m[1][0], m[2][0]),
|
|
(m[0][1], m[1][1], m[2][1]),
|
|
(m[0][2], m[1][2], m[2][2]))
|
|
|
|
def coplanar(verts):
|
|
"checks whether list of 4 vertices is coplanar"
|
|
v1 = verts[0]
|
|
v2 = verts[1]
|
|
a = sub3(v2, v1)
|
|
v1 = verts[1]
|
|
v2 = verts[2]
|
|
b = sub3(v2, v1)
|
|
if dot(cross(a,b), sub3(verts[3] - verts[2])) < 0.0001:
|
|
return 1
|
|
return 0
|
|
|
|
################################################################################
|
|
# Matrix / Vector highlevel
|
|
# (and slower)
|
|
# TODO: include better type checks !
|
|
|
|
class Vector:
|
|
"""Vector class
|
|
|
|
This vector class provides vector operations as addition, multiplication, etc.
|
|
|
|
Usage::
|
|
|
|
v = Vector(x, y, z)
|
|
|
|
where 'x', 'y', 'z' are float values, representing coordinates.
|
|
Note: This datatype emulates a float triple."""
|
|
|
|
def __init__(self, x = 0.0, y = 0.0, z = 0.0):
|
|
# don't change these to lists, very ugly referencing details...
|
|
self.v = (x, y, z)
|
|
# ... can lead to same data being shared by several matrices..
|
|
# (unless you want this to happen)
|
|
self.type = VectorType
|
|
|
|
def __neg__(self):
|
|
return self.new(-self.v[0], -self.v[1], -self.v[2])
|
|
|
|
def __getitem__(self, i):
|
|
"Tuple emulation"
|
|
return self.v[i]
|
|
|
|
# def __setitem__(self, i, arg):
|
|
# self.v[i] = arg
|
|
|
|
def new(self, *args):
|
|
return Vector(args[0], args[1], args[2])
|
|
|
|
def __cmp__(self, v):
|
|
"Comparison only supports '=='"
|
|
if self[0] == v[0] and self[1] == v[1] and self[1] == v[1]:
|
|
return 0
|
|
return 1
|
|
|
|
def __add__(self, v):
|
|
"Addition of 'Vector' objects"
|
|
return self.new(self[0] + v[0],
|
|
self[1] + v[1],
|
|
self[2] + v[2])
|
|
|
|
def __sub__(self, v):
|
|
"Subtraction of 'Vector' objects"
|
|
return self.new(self[0] - v[0],
|
|
self[1] - v[1],
|
|
self[2] - v[2])
|
|
|
|
def __rmul__(self, s): # scaling by s
|
|
return self.new(s * self[0], s * self[1], s * self[2])
|
|
|
|
def __mul__(self, t): # dot product
|
|
"""Left multiplikation supports:
|
|
|
|
- scaling with a float value
|
|
|
|
- Multiplikation with *Matrix* object"""
|
|
|
|
if type(t) == FloatType:
|
|
return self.__rmul__(t)
|
|
elif t.type == MatrixType:
|
|
return Matrix(self[0] * t[0], self[1] * t[1], self[2] * t[2])
|
|
else:
|
|
return dot(self, t)
|
|
|
|
def cross(self, v):
|
|
"(Vector v) - returns the cross product of 'self' with 'v'"
|
|
return self.new(self[1] * v[2] - self[2] * v[1],
|
|
self[2] * v[0] - self[0] * v[2],
|
|
self[0] * v[1] - self[1] * v[0])
|
|
|
|
def __repr__(self):
|
|
return "(%.3f, %.3f, %.3f)" % (self.v[0], self.v[1], self.v[2])
|
|
|
|
class Matrix(Vector):
|
|
"""Matrix class
|
|
|
|
This class is representing a vector of Vectors.
|
|
|
|
Usage::
|
|
|
|
M = Matrix(v1, v2, v3)
|
|
|
|
where 'v'n are Vector class instances.
|
|
Note: This datatype emulates a 3x3 float array."""
|
|
|
|
def __init__(self, v1 = Vector(1.0, 0.0, 0.0),
|
|
v2 = Vector(0.0, 1.0, 0.0),
|
|
v3 = Vector(0.0, 0.0, 1.0)):
|
|
self.v = [v1, v2, v3]
|
|
self.type = MatrixType
|
|
|
|
def __setitem__(self, i, arg):
|
|
self.v[i] = arg
|
|
|
|
def new(self, *args):
|
|
return Matrix(args[0], args[1], args[2])
|
|
|
|
def __repr__(self):
|
|
return "Matrix:\n %s\n %s\n %s\n" % (self.v[0], self.v[1], self.v[2])
|
|
|
|
def __mul__(self, m):
|
|
"""Left multiplication supported with:
|
|
|
|
- Scalar (float)
|
|
|
|
- Matrix
|
|
|
|
- Vector: row_vector * matrix; same as self.transposed() * vector
|
|
"""
|
|
try:
|
|
if type(m) == FloatType:
|
|
return self.__rmul__(m)
|
|
if m.type == MatrixType:
|
|
M = matxmat(self, m)
|
|
return self.new(Vector(M[0][0], M[0][1], M[0][2]),
|
|
Vector(M[1][0], M[1][1], M[1][2]),
|
|
Vector(M[2][0], M[2][1], M[2][2]))
|
|
if m.type == VectorType:
|
|
v = matxvec(self, m)
|
|
return Vector(v[0], v[1], v[2])
|
|
except:
|
|
raise TypeError, "bad multiplicator type"
|
|
|
|
def inverse(self):
|
|
"""returns the matrix inverse"""
|
|
M = invmat(self)
|
|
return self.new(Vector(M[0][0], M[0][1], M[0][2]),
|
|
Vector(M[1][0], M[1][1], M[1][2]),
|
|
Vector(M[2][0], M[2][1], M[2][2]))
|
|
|
|
def transposed(self):
|
|
"returns the transposed matrix"
|
|
M = self
|
|
return self.new(Vector(M[0][0], M[1][0], M[2][0]),
|
|
Vector(M[1][0], M[1][1], M[2][1]),
|
|
Vector(M[2][0], M[1][2], M[2][2]))
|
|
|
|
def det(self):
|
|
"""returns the determinant"""
|
|
M, det = invmatdet(self)
|
|
return det
|
|
|
|
def tr(self):
|
|
"""returns trace (sum of diagonal elements) of matrix"""
|
|
return self.v[0][0] + self.v[1][1] + self.v[2][2]
|
|
|
|
def __rmul__(self, m):
|
|
"Right multiplication supported with scalar"
|
|
if type(m) == FloatType:
|
|
return self.new(m * self[0],
|
|
m * self[1],
|
|
m * self[2])
|
|
else:
|
|
raise TypeError, "bad multiplicator type"
|
|
|
|
def __div__(self, m):
|
|
"""Division supported with:
|
|
|
|
- Scalar
|
|
|
|
- Matrix: a / b equivalent b.inverse * a
|
|
"""
|
|
if type(m) == FloatType:
|
|
m = 1.0 /m
|
|
return m * self
|
|
elif m.type == MatrixType:
|
|
return self.inverse() * m
|
|
else:
|
|
raise TypeError, "bad multiplicator type"
|
|
|
|
def __rdiv__(self, m):
|
|
"Right division of matrix equivalent to multiplication with matrix.inverse()"
|
|
return m * self.inverse()
|
|
|
|
def asEuler(self):
|
|
"""returns Matrix 'self' as Eulers. Note that this not the only result, due to
|
|
the nature of sin() and cos(). The Matrix MUST be a rotation matrix, i.e. orthogonal and
|
|
normalized."""
|
|
from math import cos, sin, acos, asin, atan2, atan
|
|
mat = self.v
|
|
sy = mat[0][2]
|
|
# for numerical stability:
|
|
if sy > 1.0:
|
|
if sy > 1.0 + TOLERANCE:
|
|
raise RuntimeError, "FATAL: bad matrix given"
|
|
else:
|
|
sy = 1.0
|
|
phi_y = -asin(sy)
|
|
|
|
if abs(sy) > (1.0 - TOLERANCE):
|
|
# phi_x can be arbitrarely chosen, we set it = 0.0
|
|
phi_x = 0.0
|
|
sz = mat[1][0]
|
|
cz = mat[2][0]
|
|
phi_z = atan(sz/cz)
|
|
else:
|
|
cy = cos(phi_y)
|
|
cz = mat[0][0] / cy
|
|
sz = mat[0][1] / cy
|
|
phi_z = atan2(sz, cz)
|
|
|
|
sx = mat[1][2] / cy
|
|
cx = mat[2][2] / cy
|
|
phi_x = atan2(sx, cx)
|
|
return phi_x, phi_y, phi_z
|
|
|
|
Ex = Vector(1.0, 0.0, 0.0)
|
|
Ey = Vector(0.0, 1.0, 0.0)
|
|
Ez = Vector(0.0, 0.0, 1.0)
|
|
|
|
One = Matrix(Ex, Ey, Ez)
|
|
orig = (0.0, 0.0, 0.0)
|
|
|
|
def rotmatrix(phi_x, phi_y, phi_z, reverse = 0):
|
|
"""Creates rotation matrix from euler angles. Rotations are applied in order
|
|
X, then Y, then Z. If the reverse is desired, you have to transpose the matrix after."""
|
|
from math import sin, cos
|
|
s = sin(phi_z)
|
|
c = cos(phi_z)
|
|
matz = Matrix(Vector(c, s, 0.0), Vector(-s, c, 0.0), Ez)
|
|
|
|
s = sin(phi_y)
|
|
c = cos(phi_y)
|
|
maty = Matrix(Vector(c, 0.0, -s), Ey, Vector(s, 0.0, c))
|
|
|
|
s = sin(phi_x)
|
|
c = cos(phi_x)
|
|
matx = Matrix(Ex, Vector(0.0, c, s), Vector(0.0, -s, c))
|
|
|
|
return matz * maty * matx
|
|
|
|
|
|
def test():
|
|
"The module test"
|
|
print "********************"
|
|
print "VECTOR TEST"
|
|
print "********************"
|
|
|
|
a = Vector(1.1, 0.0, 0.0)
|
|
b = Vector(0.0, 2.0, 0.0)
|
|
|
|
print "vectors: a = %s, b = %s" % (a, b)
|
|
print "dot:", a * a
|
|
print "scalar:", 4.0 * a
|
|
print "scalar:", a * 4.0
|
|
print "cross:", a.cross(b)
|
|
print "add:", a + b
|
|
print "sub:", a - b
|
|
print "sub:", b - a
|
|
print
|
|
print "********************"
|
|
print "MATRIX TEST"
|
|
print "********************"
|
|
c = a.cross(b)
|
|
m = Matrix(a, b, c)
|
|
v = Vector(1.0, 2.0, 3.0)
|
|
E = One
|
|
print "Original", m
|
|
print "det", m.det()
|
|
print "add", m + m
|
|
print "scalar", 0.5 * m
|
|
print "sub", m - 0.5 * m
|
|
print "vec mul", v * m
|
|
print "mul vec", m * v
|
|
n = m * m
|
|
print "mul:", n
|
|
print "matrix div (mul inverse):", n / m
|
|
print "scal div (inverse):", 1.0 / m
|
|
print "mat * inverse", m * m.inverse()
|
|
print "mat * inverse (/-notation):", m * (1.0 / m)
|
|
print "div scal", m / 2.0
|
|
|
|
# matrices with rang < dimension have det = 0.0
|
|
m = Matrix(a, 2.0 * a, c)
|
|
print "minor rang", m
|
|
print "det:", m.det()
|
|
|
|
if __name__ == '__main__':
|
|
test()
|
|
|