archive/blender-archive
Archived
1
1
Fork 0
Code Pull Requests Packages Activity

Initial revision

Browse Source
This commit is contained in:
Hans Lambermont
2002-10-12 11:37:38 +00:00
commit 12315f4d0e
1699 changed files with 444708 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

480
intern/python/modules/util/vect.py Normal file
View File

@@ -0,0 +1,480 @@
#------------------------------------------------------------------------------
# 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()