# -*- test-case-name: mv3d.test.util.test_math3d -*- # Copyright (C) 2006-2012 Mortal Coil Games # See LICENSE for details. """ Vector Math Functions-- should be compiled into C++ Some stuff borrowed from: http://www.geometrictools.com/Mathematics.html """ import math from twisted.spread.pb import setUnjellyableForClass try: from ogre.renderer.OGRE import Vector3 as oVector3 from ogre.renderer.OGRE import Matrix3 as oMatrix3 from ogre.renderer.OGRE import Quaternion as oQuaternion except ImportError: class oVector3: """ Empty class because we don't have ogre """ class oMatrix3: """ Empty class because we don't have ogre """ class oQuaternion: """ Empty class because we don't have ogre """ from mv3d.net.pb import Copyable class VectorError(Exception): """ An error to note that there was some problem with a vector """ class QuaternionError(Exception): """ An error to note that there was some problem with a vector """ class MatrixError(Exception): """ An error to note that there was some problem with a vector """ class Vector(Copyable, object): """ A class that represents a vector """ vector = (0, 0, 0) def __init__(self, *args): """ Initialize the Vector Usage: Vector() Vector(x, y, z) Vector((x, y, z)) Vector(otherVector) Vector(OGRE.Vector3) """ if len(args) == 1: if isinstance(args[0], Vector): self.vector = args[0].vector return elif isinstance(args[0], oVector3): self.vector = (args[0].x, args[0].y, args[0].z) return elif isinstance(args[0], tuple): self.vector = args[0] return try: if len(args[0]) == 3: self.vector = (args[0][0], args[0][1], args[0][2]) return except TypeError: pass elif len(args) == 3: self.vector = (float(args[0]), float(args[1]), float(args[2])) return elif len(args) == 0: return raise VectorError("Invalid args to constructor: %s" % str(args)) def __hash__(self): return hash("%f%f%f" % self.vector) def __cmp__(self, other): return self.length() - Vector(other).length() def __eq__(self, other): try: other = Vector(other) except: return False return self.vector == other.vector def __req__(self, other): try: other = Vector(other) except: return False return self.vector == other.vector def __ne__(self, other): try: other = Vector(other) except: return True return self.vector != other.vector def __rne__(self, other): try: other = Vector(other) except: return True return self.vector != other.vector def __add__(self, other): other = Vector(other) return Vector(self.vector[0] + other.vector[0], self.vector[1] + other.vector[1], self.vector[2] + other.vector[2]) def __radd__(self, other): other = Vector(other) return Vector(self.vector[0] + other.vector[0], self.vector[1] + other.vector[1], self.vector[2] + other.vector[2]) def __sub__(self, other): other = Vector(other) return Vector(self.vector[0] - other.vector[0], self.vector[1] - other.vector[1], self.vector[2] - other.vector[2]) def __rsub__(self, other): other = Vector(other) return Vector(other.vector[0] - self.vector[0], other.vector[1] - self.vector[1], other.vector[2] - self.vector[2]) def __mul__(self, other): """ Dot Product, scale, or rotate """ if isinstance(other, Vector) or isinstance(other, tuple): return self.dotProduct(other) elif isinstance(other, float) or isinstance(other, int): return self.scale(other) elif isinstance(other, Matrix) or isinstance(other, Quaternion): return other.rotate(self) raise VectorError("Unsupported operand for *: %s" % str(other)) def __pow__(self, other): """ Cross Product """ return self.crossProduct(other) def __div__(self, other): """ Scale by 1/other """ return self.scale(1.0 / other) def __neg__(self): return Vector(-self.vector[0], -self.vector[1], -self.vector[2]) def __len__(self): return len(self.vector) def __getitem__(self, k): """ Return x, y, or z depending on k """ if isinstance(k, int): return self.vector[k] if k == "x": return self.vector[0] if k == "y": return self.vector[1] if k == "z": return self.vector[2] raise KeyError("Vector doesn't have a %s" % k) def __setitem__(self, key, value): """ Set one component of the vector """ if key in ["x", 0]: self.vector = (value, self.vector[1], self.vector[2]) elif key in ["y", 1]: self.vector = (self.vector[0], value, self.vector[2]) elif key in ["z", 2]: self.vector = (self.vector[0], self.vector[1], value) else: raise KeyError("Vector doesn't have a %s" % key) def __iter__(self): """ Return the iterator """ return self.vector.__iter__() def __str__(self): """ Return the vector as (x, y, z) """ return "(" + ", ".join([str(item) for item in self]) + ")" def __repr__(self): """ Return a useful repr. """ return "<(%s) %s.%s object at %s>" % ( ", ".join([str(item) for item in self]), self.__class__.__module__, self.__class__.__name__, hex(id(self))) def _getX(self): return self[0] def _setX(self, value): self[0] = value x = property(_getX, _setX) def _getY(self): return self[1] def _setY(self, value): self[1] = value y = property(_getY, _setY) def _getZ(self): return self[2] def _setZ(self, value): self[2] = value z = property(_getZ, _setZ) def distance(self, other=None): """ Calculate the distance between the two points. If Other is none, (0,0,0) is used """ if other is None: other = Vector() else: other = Vector(other) a = self.vector b = other.vector return math.sqrt((a[0] - b[0]) ** 2 + (a[1] - b[1]) ** 2 + (a[2] - b[2]) ** 2) def squaredDistance(self, other=None): """ Calculate the distance**2 between this and other. If other isn't specified then assume 0,0,0 """ if other is None: other = Vector() else: other = Vector(other) a = self.vector b = other.vector return (a[0] - b[0]) ** 2 + (a[1] - b[1]) ** 2 + (a[2] - b[2]) ** 2 def length(self): """ Returns the length of the vector.. i.e. distance from 0,0,0 """ return self.distance() def scale(self, amount): """ Scale the vector by amount """ return Vector(tuple([t * amount for t in self.vector])) def normalize(self): """ normalize the vector """ len_ = self.length() if len_ < 0.00000001: return Vector(0, 0, 0) return Vector(tuple([coord / len_ for coord in self.vector])) def dotProduct(self, b): """ Return the dot product of this vector and another """ a = self.vector return a[0] * b[0] + a[1] * b[1] + a[2] * b[2] def crossProduct(self, b): """ Returns the cross product of this vector and another """ a = self.vector return Vector(a[1] * b[2] - a[2] * b[1], a[2] * b[0] - a[0] * b[2], a[0] * b[1] - a[1] * b[0]) def lerp(self, other, amount): """ Linear interpolate between this vector and other by amount percent (0-1) Returns a new Vector """ return self + (other - self) * amount def angleBetween(self, other): """ Gets the angle between 2 vectors """ # based on the ogre.Vector3 method lenProduct = self.length() * other.length() if lenProduct < 0.00000001: lenProduct = 0.00000001 f = self.dotProduct(other) / lenProduct f = clamp(f, -1.0, 1.0) return math.acos(f) @classmethod def zero(cls): """ Returns a zero'd vector """ return cls(0, 0, 0) @classmethod def up(cls): """ Returns a upward-facing vector """ return cls(0, 1, 0) @classmethod def down(cls): """ Returns a downward-facing vector """ return cls(0, -1, 0) @classmethod def left(cls): """ Returns a left-facing vector """ return cls(-1, 0, 0) @classmethod def right(cls): """ Returns a right-facing vector """ return cls(1, 0, 0) @classmethod def forward(cls): """ Returns a forward-facing vector """ return cls(0, 0, 1) @classmethod def back(cls): """ Returns a backwards-facing vector """ return cls(0, 0, -1) @classmethod def unitScale(cls): """ Convenience method that returns a (1, 1, 1) vector """ return cls(1, 1, 1) class Quaternion(Copyable): """ This class represents a quaternion """ quat = (1, 0, 0, 0) def __init__(self, *args): """ Usage is as follows: Quaternion(w,x,y,z) Quaternion(w, (x,y,z)) Quaternion((w,x,y,z)) Quaternion(w, Vector) Quaternion(Quaternion) Quaternion(OGRE.Quaternion) Quaternion(Matrix) Quaternion() """ if not len(args): return elif len(args) == 4: self.quat = tuple(args) return elif len(args) == 2: self.quat = (args[0], args[1][0], args[1][1], args[1][2]) return elif len(args) == 1: if isinstance(args[0], Quaternion): self.quat = args[0].quat return elif isinstance(args[0], Matrix): self.quat = self.fromMatrix(args[0]).quat return elif ((isinstance(args[0], tuple) or isinstance(args[0], list)) and len(args[0]) == 4): self.quat = tuple(args[0]) return elif isinstance(args[0], oQuaternion): self.quat = (args[0].w, args[0].x, args[0].y, args[0].z) return try: if len(args[0]) == 4: self.quat = (args[0][0], args[0][1], args[0][2], args[0][3]) return except TypeError: pass try: self.quat = (args[0].w, args[0].x, args[0].y, args[0].z) except AttributeError: pass try: self.quat = (args[0].w, args[0].x, args[0].y, args[0].z) except AttributeError: pass raise QuaternionError("Invalid arguments in constructor %s" % args) def __len__(self): return len(self.quat) def __getitem__(self, itm): if isinstance(itm, int): return self.quat[itm] elif isinstance(itm, str): return self.quat[["w", "x", "y", "z"].index(itm)] raise QuaternionError("Invalid arguments to getitem") def __cmp__(self, other): return (abs(self[0] - other[0]) + abs(self[1] - other[1]) + abs(self[2] - other[2]) + abs(self[3] - other[3])) def __eq__(self, other): other = Quaternion(other) return self.quat == other.quat def __ne__(self, other): other = Quaternion(other) return self.quat != other.quat def __add__(self, b): """ Add this to another quaternion and return a new quaternion as the result """ a = self return Quaternion(a[0] + b[0], a[1] + b[1], a[2] + b[2], a[3] + b[3]) def __sub__(self, b): """ Subtract another quaternion from this and return a new quaternion as the result """ a = self return Quaternion(a[0] - b[0], a[1] - b[1], a[2] - b[2], a[3] - b[3]) def __mul__(self, b): """ Quaternion multiplication. Or if b is a Vector, rotate it. """ if isinstance(b, float): return self.scale(b) if len(b) == 3: return self.rotate(b) a = self r = (a[0] * b[0] - a[1] * b[1] - a[2] * b[2] - a[3] * b[3], a[0] * b[1] + a[1] * b[0] + a[2] * b[3] - a[3] * b[2], a[0] * b[2] + a[2] * b[0] + a[3] * b[1] - a[1] * b[3], a[0] * b[3] + a[3] * b[0] + a[1] * b[2] - a[2] * b[1]) return Quaternion(r) def __div__(self, s): return self.scale(s) def __pow__(self, o): return self.dotProduct(o) def __neg__(self): return self.negate() def __iter__(self): """ Return the iterator """ return self.quat.__iter__() def __str__(self): """ Stringify it """ return str(self.quat) def _getW(self): return self[0] def _setW(self, value): self[0] = value w = property(_getW, _setW) def _getX(self): return self[1] def _setX(self, value): self[1] = value x = property(_getX, _setX) def _getY(self): return self[2] def _setY(self, value): self[2] = value y = property(_getY, _setY) def _getZ(self): return self[3] def _setZ(self, value): self[3] = value z = property(_getZ, _setZ) def rotate(self, v): """ Rotate this vector by us """ a = self.quat t2, t3, t4 = tuple([a[0] * a[x + 1] for x in range(3)]) t5, t6, t7 = a[1] * -a[1], a[1] * a[2], a[1] * a[3] t8, t9, t10 = a[2] * -a[2], a[2] * a[3], a[3] * -a[3] return Vector(2 * ((t8 + t10) * v[0] + (t6 - t4) * v[1] + (t3 + t7) * v[2]) + v[0], 2 * ((t4 + t6) * v[0] + (t5 + t10) * v[1] + (t9 - t2) * v[2]) + v[1], 2 * ((t7 - t3) * v[0] + (t2 + t9) * v[1] + (t5 + t8) * v[2]) + v[2]) def scale(self, s): """ You need to add a docstring here ! """ a = self if s == 0: return a i = 1.0 / s return Quaternion(a[0] * i, a[1] * i, a[2] * i, a[3] * i) def length(self): """ You need to add a docstring here ! """ q = self #print "QL=",q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3] return math.sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]) def negate(self): """ You need to add a docstring here ! """ a = self return Quaternion(-a[0], -a[1], -a[2], -a[3]) def fromMatrix(self, m): """ You need to add a docstring here ! """ trace = 1 + m[0] + m[4] + m[8] if trace > 0: root = math.sqrt(trace) * 2 return Quaternion((0.25 * root, (m[7] - m[5]) / root, (m[2] - m[6]) / root, (m[3] - m[1]) / root)) assert trace == 0 if m[0] > m[4] and m[0] > m[8]: root = math.sqrt(1 + m[0] - m[4] - m[8]) * 2 r = ((m[7] - m[5]) / root, 0.25 * root, (m[4] + m[1]) / root, (m[2] + m[6]) / root) elif m[4] > m[8]: root = math.sqrt(1.0 + m[4] - m[0] - m[8]) * 2 r = ((m[2] - m[6]) / root, (m[3] + m[1]) / root, 0.25 * root, (m[7] + m[5]) / root) else: root = math.sqrt(1.0 + m[8] - m[0] - m[4]) * 2 r = ((m[4] - m[1]) / root, (m[2] + m[6]) / root, (m[7] + m[5]) / root, 0.25 * root) return Quaternion(r) @classmethod def fromAxisAngle(cls, ax, an): """ You need to add a docstring here ! """ q = [0, 0, 0, 0] fHalfAngle = 0.5 * an fSin = math.sin(fHalfAngle) q = (math.cos(fHalfAngle), fSin * ax[0], fSin * ax[1], fSin * ax[2]) return Quaternion(q).normalize() @classmethod def fromEuler(cls, *a): """ Return a quaternion generated from the given euler angles """ if isinstance(a[0], (tuple, list, Vector)): a = a[0] return Quaternion(Matrix().fromEuler(a)) @classmethod def zero(cls): """ Returns a zeroed quaternion """ return cls(0, 0, 0, 0) @classmethod def identity(cls): """ Returns the identity quaternion """ return cls(1, 0, 0, 0) def toEuler(self): """ Return rx, ry, rz in euler angles """ return Matrix(self).toEuler() def dotProduct(self, b): """ You need to add a docstring here ! """ a = self fDot = 0.0 for i in range(4): fDot += a[i] * b[i] return fDot def normalize(self): """ You need to add a docstring here ! """ a = self r = [0, 0, 0, 0] fLength = a.length() if fLength > 0: fInvLength = 1.0 / fLength for i in range(4): r[i] = a[i] * fInvLength else: fLength = 0.0 for i in range(4): r[i] = 0.0 return Quaternion(tuple(r)) def inverse(self): """ You need to add a docstring here ! """ q = self r = [0, 0, 0, 0] fNorm = 0.0 for i in range(4): fNorm += q[i] * q[i] if fNorm > 0.0: fInvNorm = 1.0 / fNorm r[0] = q[0] * fInvNorm r[1] = -q[1] * fInvNorm r[2] = -q[2] * fInvNorm r[3] = -q[3] * fInvNorm else: raise QuaternionError("Bad Quaternion inverse") return Quaternion(tuple(r)) def slerp(self, b, f): """ Spherical linear interpolation between this quaternion and b by f percent (0-1). """ a = self b = Quaternion(b) r = [0, 0, 0, 0] fCos = a.dotProduct(b) fAngle = math.acos(fCos) if abs(fAngle) > 1.0 - 0.001: fSin = math.sin(fAngle) fInvSin = 1.0 / fSin fCoeff0 = math.sin((1.0 - f) * fAngle) * fInvSin fCoeff1 = math.sin(f * fAngle) * fInvSin r = a.scale(fCoeff0) + b.scale(fCoeff1) else: r = b return Quaternion(tuple(r)) def between(self, b): """ // If V1 and V2 are not parallel, the axis of rotation is the unit-length // vector U = Cross(V1,V2)/Length(Cross(V1,V2)). The angle of rotation, // A, is the angle between V1 and V2. The quaternion for the rotation is // q = cos(A/2) + sin(A/2)*(ux*i+uy*j+uz*k) where U = (ux,uy,uz). // // (1) Rather than extract A = acos(Dot(V1,V2)), multiply by 1/2, then // compute sin(A/2) and cos(A/2), we reduce the computational costs by // computing the bisector B = (V1+V2)/Length(V1+V2), so cos(A/2) = // Dot(V1,B). // // (2) The rotation axis is U = Cross(V1,B)/Length(Cross(V1,B)), but // Length(Cross(V1,B)) = Length(V1)*Length(B)*sin(A/2) = sin(A/2), in // which case sin(A/2)*(ux*i+uy*j+uz*k) = (cx*i+cy*j+cz*k) where // C = Cross(V1,B). // // If V1 = V2, then B = V1, cos(A/2) = 1, and U = (0,0,0). If V1 = -V2, // then B = 0. This can happen even if V1 is approximately -V2 using // floating point arithmetic, since Vector3::normalize checks for // closeness to zero and returns the zero vector accordingly. The test // for exactly zero is usually not recommend for floating point // arithmetic, but the implementation of Vector3::normalize guarantees // the comparison is robust. In this case, the A = pi and any axis // perpendicular to V1 may be used as the rotation axis. """ a = self r = [0, 0, 0, 0] kBisector = a + b kBisector = kBisector.normalize() fCosHalfAngle = a.dotProduct(kBisector) kCross = [0, 0, 0] r[0] = fCosHalfAngle; if fCosHalfAngle != 0.0: kCross = a.crossProduct(kBisector) r[1] = kCross[0] r[2] = kCross[1] r[3] = kCross[2] else: if abs(a[0]) >= abs(a[1]): fInvLength = invsqrt(a[0] * a[0] + a[2] * a[2]) r[1] = -a[2] * fInvLength r[2] = 0.0 r[3] = +a[0] * fInvLength else: fInvLength = invsqrt(a[1] * a[1] + a[2] * a[2]) r[1] = 0.0 r[2] = +a[2] * fInvLength r[3] = -a[1] * fInvLength return Quaternion(tuple(r)) @classmethod def fromTargetPosition(cls, targetPosition, upVector=(0, 1.0, 0)): """ Set the rotation to look at the target position. """ return cls().fromMatrix(Matrix().fromTargetPosition(targetPosition, upVector)) class Matrix(Copyable): """ This class represents a rotation matrix """ mat = (1, 0, 0, 0, 1, 0, 0, 0, 1) def __init__(self, *args): """ Usage is as follows: Matrix(x1,y1,z1,x2,y2,z2,x3,y3,z3) Matrix((x1,y1,z1),(x2,y2,z2),(x3,y3,x3)) Matrix(Vector, Vector, Vector) Matrix(Matrix) Matrix(Quaternion) Matrix(OGRE.Matrix3) Matrix() """ if not len(args): return elif len(args) == 1: if isinstance(args[0], Matrix): self.mat = tuple(args[0].mat) return if isinstance(args[0], Quaternion): self.fromQuaternion(args[0]) return if isinstance(args[0], tuple) or isinstance(args[0], list): self.mat = tuple(args[0]) return if isinstance(args[0], oMatrix3): m = [] for c in range(3): for r in range(3): m.append(args[0].GetColumn(c)[r]) return elif len(args) == 3: mat = list([x for x in args[0]]) + list([x for x in args[1]]) + list([x for x in args[2]]) self.mat = tuple(mat) return elif len(args) == 9: self.mat = tuple(args) return raise MatrixError("Invalid argument(s) to constructor %s" % str(args)) def __str__(self): return ( "" % self.mat) def __len__(self): return len(self.mat) def __getitem__(self, *args): if len(args) == 1: if isinstance(args[0], slice): return self.mat[args[0]] if isinstance(args[0], int): return self.mat[args[0]] if isinstance(args[0], tuple): return self.mat[args[0][0] * 3 + args[0][1]] return self.mat[["x1", "y1", "z1", "x2", "y2", "z2", "x3", "y3", "z3"].index(args[0])] elif len(args) == 2: return self.mat[args[0] * 3 + args[1]] raise MatrixError("Invalid arguments to getitem!") def __eq__(self, other): if isinstance(other, tuple): return self.mat == other elif isinstance(other, Matrix): return self.mat == other.mat raise MatrixError("Invalid operand for == %s" % str(other)) def __ne__(self, other): if isinstance(other, tuple): return self.mat != other elif isinstance(other, Matrix): return self.mat != other.mat raise MatrixError("Invalid operand for !=") def __mul__(self, other): if isinstance(other, Vector) or (isinstance(other, tuple) and len(other) == 3): return self.rotateVector(other) elif isinstance(other, Matrix) or (isinstance(other, tuple) and len(other) == 9): return self.rotateMatrix(other) raise MatrixError("Invalid operand for *") def rotateVector(self, a): """ Rotate a vector by this matrix """ r = self.mat x = r[0] * a[0] + r[1] * a[1] + r[2] * a[2] y = r[3] * a[0] + r[4] * a[1] + r[5] * a[2] z = r[6] * a[0] + r[7] * a[1] + r[8] * a[2] return Vector(x, y, z) rotate = rotateVector def rotateMatrix(self, b): """ Rotate this matrix by another """ a = self.mat res = [0, 0, 0, 0, 0, 0, 0, 0, 0] for r in range(3): for c in range(3): i = r * 3 + c res[i] = 0.0 for m in range(3): res[i] += a[r * 3 + m] * b[m * 3 + c] return Matrix(*res) def fromQuaternion(self, q): """ Set this matrix equal to the quaternion """ a = tuple(q) x, y, z = 2.0 * a[1], 2.0 * a[2], 2.0 * a[3] wx, wy, wz = x * a[0], y * a[0], z * a[0] xx, xy, xz = x * a[1], y * a[1], z * a[1] yy, yz, zz = y * a[2], z * a[2], z * a[3] self.mat = (1.0 - (yy + zz), xy - wz, xz + wy, xy + wz, 1.0 - (xx + zz), yz - wx, xz - wy, yz + wx, 1.0 - (xx + yy)) return self def fromEuler(self, *a): """ Set this matrix by euler angle stored in a (x,y,z) """ if len(a) == 1: if isinstance(a[0], tuple) or isinstance(a[0], Vector): x, y, z = tuple(a[0]) elif len(a) == 3: x, y, z = a A = math.cos(x) B = math.sin(x) C = math.cos(y) D = math.sin(y) E = math.cos(z) F = math.sin(z) AE = A * E AF = A * F BE = B * E BF = B * F self.mat = (C * E, -C * F, D, AF + BE * D, AE - BF * D, -B * C, BF - AE * D, BE + AF * D, A * C) return self def determinant(self): a = self.mat return (a[0] * a[4] * a[8] + a[1] * a[5] * a[6] + a[2] * a[3] * a[7] - a[6] * a[4] * a[2] - a[7] * a[5] * a[0] - a[8] * a[3] * a[1]) def inverse(self): d = self.determinant() d = 1 / d a = self.mat return Matrix( d * (a[4] * a[8] - a[5] * a[7]), # 11 d * (a[2] * a[7] - a[1] * a[8]), # 12 d * (a[1] * a[5] - a[2] * a[4]), # 13 d * (a[5] * a[6] - a[3] * a[8]), # 21 d * (a[0] * a[8] - a[2] * a[6]), # 22 d * (a[2] * a[3] - a[0] * a[5]), # 23 d * (a[3] * a[7] - a[4] * a[6]), # 31 d * (a[1] * a[6] - a[0] * a[7]), # 32 d * (a[0] * a[4] - a[1] * a[3])) # 33 def toEuler(self): a = self.mat if a[3] > 0.998: return (3.14159 / 2.0, math.atan2(a[2], a[8]), 0) if a[3] < -0.998: return (-3.14159 / 2.0, math.atan2(a[2], a[8]), 0) return (math.atan2(-a[5], a[4]), math.atan2(-a[6], a[0]), math.asin(a[3])) @classmethod def fromTargetPosition(cls, targetPosition, upVector=(0, 1.0, 0)): """ Set the rotation to look at the target position. """ upVector = Vector(upVector) forward = Vector(targetPosition).normalize() if forward == upVector * -1.0: return cls().fromEuler((3.14159 / -2.0, 0, 0)) side = forward.crossProduct(upVector.normalize()) up = side.crossProduct(forward) return cls([side[0], up[0], -forward[0], side[1], up[1], -forward[1], side[2], up[2], -forward[2]]) def invsqrt(a): """ Returns 1 over the square root of a """ return 1.0 / math.sqrt(a) def distance(a, b=(0, 0, 0)): """ Calculate the distance between the two points. If Other is none, (0,0,0) is used """ return Vector(a).distance(b) def addVectors(a, b): """ Add two vectors """ return Vector(a) + Vector(b) def subtractVectors(a, b): """ Subtract two vectors """ return Vector(a) - Vector(b) def scaleVector(a, s): """ You need to add a docstring here ! """ return Vector(a) * s def normalizeVector(a): """ You need to add a docstring here ! """ return Vector(a).normalize() def isInsideBox(v, c): """ v= vector (x,y,z) c= box ( (minx, miny, minz), (maxx, maxy, maxz) ) return 1 if v is inside of c """ # print v,c if v[0] < c[0][0] or v[0] > c[1][0]: return 0 if v[1] < c[0][1] or v[1] > c[1][1]: return 0 if v[2] < c[0][2] or v[2] > c[1][2]: return 0 return 1 def rotateVector(a, r): """ Rotate vector a by matrix r """ if len(r) == 9: return Vector(a) * Matrix(r) if len(r) == 4: return Vector(a) * Quaternion(r) def multiplyQuaternions(a, b): """ You need to add a docstring here ! """ r = [] r.append(a[0] * b[0] - a[1] * b[1] - a[2] * b[2] - a[3] * b[3]) r.append(a[0] * b[1] + a[1] * b[0] + a[2] * b[3] - a[3] * b[2]) r.append(a[0] * b[2] + a[2] * b[0] + a[3] * b[1] - a[1] * b[3]) r.append(a[0] * b[3] + a[3] * b[0] + a[1] * b[2] - a[2] * b[1]) return tuple(r) def quaternionFromMatrix(m): """ You need to add a docstring here ! """ r = [0, 0, 0, 0] trace = m[0] + m[4] + m[8] if trace > 0: root = math.sqrt(trace + 1) r[0] = root * 0.5 r[1] = (m[7] - m[5]) * root r[2] = (m[2] - m[6]) * root r[3] = (m[3] - m[1]) * root else: raise ValueError("Bad Rotation Mat") """ i=0 if m[4] > m[0] i=1 if m[8] > m[i*3+i] i = 2; j = ms_iNext[i]; k = ms_iNext[j]; fRoot = Math::Sqrt(rkRot(i,i)-rkRot(j,j)-rkRot(k,k)+(Real)1.0); Real* apfQuat[3] = { &m_afTuple[1], &m_afTuple[2], &m_afTuple[3] }; *apfQuat[i] = ((Real)0.5)*fRoot; fRoot = ((Real)0.5)/fRoot; m_afTuple[0] = (rkRot(k,j)-rkRot(j,k))*fRoot; *apfQuat[j] = (rkRot(j,i)+rkRot(i,j))*fRoot; *apfQuat[k] = (rkRot(k,i)+rkRot(i,k))*fRoot;""" return tuple(r) def normalizeQuaternion(a): """ You need to add a docstring here ! """ r = [0, 0, 0, 0] a = Quaternion(a) fLength = a.length() if fLength > 0: fInvLength = 1.0 / fLength #print "FIRN",fInvLength for i in range(4): r[i] = a[i] * fInvLength else: fLength = 0.0 for i in range(4): r[i] = 0.0 return tuple(r) def matrixFromEuler(*args): """ Returns a matrix generated from a euler angle """ return Matrix().fromEuler(*args) def clamp(val, minVal, maxVal): """ Clamp a value within an inclusive range """ if minVal > maxVal: raise ValueError("Minimum value must be less than the maximum value") return max(min(val, maxVal), minVal) def clampAngle(self, angle, min, max): """ Clamp an angle value """ if angle < -360.0: angle += 360.0 if angle > 360.0: angle -= 360.0 return clamp(angle, min, max) def lerp(minVal, maxVal, term): """ linear interpolation """ return (maxVal - minVal) * term + minVal setUnjellyableForClass(Vector, Vector) setUnjellyableForClass(Matrix, Matrix) setUnjellyableForClass(Quaternion, Quaternion)