diff options
Diffstat (limited to 'intern/python/modules/util/vect.py')
| -rw-r--r-- | intern/python/modules/util/vect.py | 480 |
1 files changed, 480 insertions, 0 deletions
diff --git a/intern/python/modules/util/vect.py b/intern/python/modules/util/vect.py new file mode 100644 index 00000000000..3724079519b --- /dev/null +++ b/intern/python/modules/util/vect.py @@ -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() + |