From b5e6b754fb5afbf09c3989196d63d4c8c0a7892c Mon Sep 17 00:00:00 2001 From: Philip Eisenlohr Date: Thu, 3 Nov 2011 12:19:26 +0000 Subject: [PATCH] starting "lib" subfolder for shared goodies. --- {processing => lib}/msc_tools.py | 0 lib/orientation.py | 799 +++++++++++++++++++++++++++++++ lib/pathinfo | 3 + 3 files changed, 802 insertions(+) rename {processing => lib}/msc_tools.py (100%) create mode 100644 lib/orientation.py create mode 100644 lib/pathinfo diff --git a/processing/msc_tools.py b/lib/msc_tools.py similarity index 100% rename from processing/msc_tools.py rename to lib/msc_tools.py diff --git a/lib/orientation.py b/lib/orientation.py new file mode 100644 index 000000000..c4f068dfb --- /dev/null +++ b/lib/orientation.py @@ -0,0 +1,799 @@ +# code derived from http://pyeuclid.googlecode.com/svn/trunk/euclid.py + +import math,random + +# ****************************************************************************************** +class Vector3: +# ****************************************************************************************** + __slots__ = ['x', 'y', 'z'] + + def __init__(self, array): + assert hasattr(array, '__len__') and len(array) == 3 + self.x = array[0] + self.y = array[1] + self.z = array[2] + + def __copy__(self): + return self.__class__(self.x, self.y, self.z) + + copy = __copy__ + + def __repr__(self): + return 'Vector3(%.4f, %.4f, %.4f)' % (self.x, + self.y, + self.z) + + def __eq__(self, other): + if isinstance(other, Vector3): + return self.x == other.x and \ + self.y == other.y and \ + self.z == other.z + else: + assert hasattr(other, '__len__') and len(other) == 3 + return self.x == other[0] and \ + self.y == other[1] and \ + self.z == other[2] + + def __neq__(self, other): + return not self.__eq__(other) + + def __cmp__(self,other): + return cmp((self.z,self.y,self.x),(other.z,other.y,other.x)) + + def __nonzero__(self): + return self.x != 0 or self.y != 0 or self.z != 0 + + def __len__(self): + return 3 + + def __getitem__(self, key): + return (self.x, self.y, self.z)[key] + + def __setitem__(self, key, value): + l = [self.x, self.y, self.z] + l[key] = value + self.x, self.y, self.z = l + + def __iter__(self): + return iter((self.x, self.y, self.z)) + + def __getattr__(self, name): + try: + return tuple([(self.x, self.y, self.z)['xyz'.index(c)] \ + for c in name]) + except ValueError: + raise AttributeError, name + + def __add__(self, other): + if isinstance(other, Vector3): + return Vector3([self.x + other.x, + self.y + other.y, + self.z + other.z]) + else: + assert hasattr(other, '__len__') and len(other) == 3 + return Vector3([self.x + other[0], + self.y + other[1], + self.z + other[2]]) + __radd__ = __add__ + + def __iadd__(self, other): + if isinstance(other, Vector3): + self.x += other.x + self.y += other.y + self.z += other.z + else: + assert hasattr(other, '__len__') and len(other) == 3 + self.x += other[0] + self.y += other[1] + self.z += other[2] + return self + + def __sub__(self, other): + if isinstance(other, Vector3): + return Vector3([self.x - other.x, + self.y - other.y, + self.z - other.z]) + else: + assert hasattr(other, '__len__') and len(other) == 3 + return Vector3([self.x - other[0], + self.y - other[1], + self.z - other[2]]) + + + def __rsub__(self, other): + if isinstance(other, Vector3): + return Vector3([other.x - self.x, + other.y - self.y, + other.z - self.z]) + else: + assert hasattr(other, '__len__') and len(other) == 3 + return Vector3([other.x - self[0], + other.y - self[1], + other.z - self[2]]) + + def __mul__(self, other): + if isinstance(other, Vector3): + # TODO component-wise mul/div in-place and on Vector2; docs. + return Vector3([self.x * other.x, + self.y * other.y, + self.z * other.z]) + else: + assert type(other) in (int, long, float) + return Vector3([self.x * other, + self.y * other, + self.z * other]) + + __rmul__ = __mul__ + + def __imul__(self, other): + assert type(other) in (int, long, float) + self.x *= other + self.y *= other + self.z *= other + return self + + def __div__(self, other): + assert type(other) in (int, long, float) + self.x /= other + self.y /= other + self.z /= other + return self + + + def __rdiv__(self, other): + assert type(other) in (int, long, float) + return Vector3([operator.div(other, self.x), + operator.div(other, self.y), + operator.div(other, self.z)]) + + def __floordiv__(self, other): + assert type(other) in (int, long, float) + return Vector3([operator.floordiv(self.x, other), + operator.floordiv(self.y, other), + operator.floordiv(self.z, other)]) + + + def __rfloordiv__(self, other): + assert type(other) in (int, long, float) + return Vector3([operator.floordiv(other, self.x), + operator.floordiv(other, self.y), + operator.floordiv(other, self.z)]) + + def __truediv__(self, other): + assert type(other) in (int, long, float) + return Vector3([operator.truediv(self.x, other), + operator.truediv(self.y, other), + operator.truediv(self.z, other)]) + + + def __rtruediv__(self, other): + assert type(other) in (int, long, float) + return Vector3([operator.truediv(other, self.x), + operator.truediv(other, self.y), + operator.truediv(other, self.z)]) + + def __neg__(self): + return Vector3([-self.x, + -self.y, + -self.z]) + + __pos__ = __copy__ + + def __abs__(self): + return math.sqrt(self.x ** 2 + \ + self.y ** 2 + \ + self.z ** 2) + + magnitude = __abs__ + + def magnitude_squared(self): + return self.x ** 2 + \ + self.y ** 2 + \ + self.z ** 2 + + def normalize(self): + d = self.magnitude() + if d: + self.x /= d + self.y /= d + self.z /= d + return self + + def normalized(self): + d = self.magnitude() + if d: + return Vector3([self.x / d, + self.y / d, + self.z / d]) + return self.copy() + + def dot(self, other): + if isinstance(other, Vector3): + return self.x * other.x + \ + self.y * other.y + \ + self.z * other.z + else: + assert hasattr(other, '__len__') and len(other) == 3 + return self.x * other[0] + \ + self.y * other[1] + \ + self.z * other[2] + + def cross(self, other): + if isinstance(other, Vector3): + 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]) + else: + assert hasattr(other, '__len__') and len(other) == 3 + return Vector3([ self.y * other[2] - self.z * other[1], + - self.x * other[2] + self.z * other[0], + self.x * other[1] - self.y * other[0]]) + + def reflect(self, normal): + # assume normal is normalized + if isinstance(other, 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]) + else: + assert hasattr(other, '__len__') and len(other) == 3 + d = 2 * (self.x * normal[0] + self.y * normal[1] + self.z * normal[2]) + return Vector3([self.x - d * normal[0], + self.y - d * normal[1], + self.z - d * normal[2]]) + + def inSST(self,symmetry): + return {'cubic': lambda R: R.inFZ('cubic') and R.x >= R.y and R.y >= R.z and R.z >= 0.0, + 'hexagonal': lambda R: R.inFZ('hexagonal') and R.x >= math.sqrt(3)*R.y and R.y >= 0.0 and R.z >= 0.0, + 'tetragonal': lambda R: R.inFZ('tetragonal') and R.x >= R.y and R.y >= 0.0 and R.z >= 0.0, + 'orthorombic': lambda R: R.inFZ('orthorombic') and R.x >= 0.0 and R.y >= 0.0 and R.z >= 0.0, + }.get(symmetry.lower(),False)(self) + + def inFZ(self,symmetry): + return {'cubic': lambda R: math.sqrt(2.0)-1.0 >= abs(R.x) \ + and math.sqrt(2.0)-1.0 >= abs(R.y) \ + and math.sqrt(2.0)-1.0 >= abs(R.z) \ + and 1.0 >= abs(R.x) + abs(R.y) + abs(R.z), + 'hexagonal': lambda R: 1.0 >= abs(R.x) and 1.0 >= abs(R.y) and 1.0 >= abs(R.z) \ + and 2.0 >= math.sqrt(3)*abs(R.x) + abs(R.y) \ + and 2.0 >= math.sqrt(3)*abs(R.y) + abs(R.x) \ + and 2.0 >= math.sqrt(3) + abs(R.z), + 'tetragonal': lambda R: 1.0 >= abs(R.x) and 1.0 >= abs(R.y) \ + and math.sqrt(2.0) >= abs(R.x) + abs(R.y) \ + and math.sqrt(2.0) >= abs(R.z) + 1.0, + 'orthorombic': lambda R: 1.0 >= abs(R.x) and 1.0 >= abs(R.y) and 1.0 >= abs(R.z), + }.get(symmetry.lower(),False)(self) + +# ****************************************************************************************** +class Quaternion: +# ****************************************************************************************** + # 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 + __slots__ = ['w', 'x', 'y', 'z'] + + def __init__(self, quatArray=[1.0,0.0,0.0,0.0]): + self.w, \ + self.x, \ + self.y, \ + self.z = quatArray + + def __iter__(self): + return iter([self.w,self.x,self.y,self.z]) + + def __copy__(self): + Q = Quaternion([self.w,self.x,self.y,self.z]) + return Q + + copy = __copy__ + + def __repr__(self): + return 'Quaternion(real=%.4f, imag=<%.4f, %.4f, %.4f>)' % \ + (self.w, self.x, self.y, self.z) + + 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): # active rotation of vector (i.e. within fixed coordinate system) + w = self.w + x = self.x + y = self.y + z = self.z + Vx = other.x + Vy = other.y + Vz = other.z + + return other.__class__(\ + w * w * Vx + 2 * y * w * Vz - 2 * z * w * Vy + \ + x * x * Vx + 2 * y * x * Vy + 2 * z * x * Vz - \ + z * z * Vx - y * y * Vx, + 2 * x * y * Vx + y * y * Vy + 2 * z * y * Vz + \ + 2 * w * z * Vx - z * z * Vy + w * w * Vy - \ + 2 * x * w * Vz - x * x * Vy, + 2 * x * z * Vx + 2 * y * z * Vy + \ + z * z * Vz - 2 * w * y * Vx - y * y * Vz + \ + 2 * w * x * Vy - x * x * Vz + w * w * Vz) + elif isinstance(other, (int,float,long)): + Q = self.copy() + Q.w *= other + Q.x *= other + Q.y *= other + Q.z *= other + return Q + else: + return self.copy() + + def __imul__(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 + 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 + + def __div__(self, other): + if isinstance(other, (int,float,long)): + Q = self.copy() + Q.w /= other + Q.x /= other + Q.y /= other + Q.z /= other + return Q + else: + return self.copy() + + def __idiv__(self, other): + if isinstance(other, (int,float,long)): + self.w /= other + self.x /= other + self.y /= other + self.z /= other + return self + + def __div__(self, other): + if isinstance(other, (int,float,long)): + Q = self.copy() + Q.w /= other + Q.x /= other + Q.y /= other + Q.z /= other + return Q + else: + return self.copy() + + def __idiv__(self, other): + if isinstance(other, (int,float,long)): + self.w /= other + self.x /= other + self.y /= other + self.z /= other + return self + + def __add__(self, other): + if isinstance(other, Quaternion): + Q = self.copy() + Q.w += other.w + Q.x += other.x + Q.y += other.y + Q.z += other.z + return Q + else: + return self.copy() + + def __iadd__(self, other): + if isinstance(other, Quaternion): + self.w += other.w + self.x += other.x + self.y += other.y + self.z += other.z + return self + + def __sub__(self, other): + if isinstance(other, Quaternion): + Q = self.copy() + Q.w -= other.w + Q.x -= other.x + Q.y -= other.y + Q.z -= other.z + return Q + else: + return self.copy() + + def __isub__(self, other): + if isinstance(other, Quaternion): + self.w -= other.w + self.x -= other.x + self.y -= other.y + self.z -= other.z + return self + + def __neg__(self): + self.w = -self.w + self.x = -self.x + self.y = -self.y + self.z = -self.z + return self + + def __abs__(self): + return math.sqrt(self.w ** 2 + \ + self.x ** 2 + \ + self.y ** 2 + \ + self.z ** 2) + + magnitude = __abs__ + + def __eq__(self,other): + return (abs(self.w-other.w) < 1e-8 and \ + abs(self.x-other.x) < 1e-8 and \ + abs(self.y-other.y) < 1e-8 and \ + abs(self.z-other.z) < 1e-8) \ + or \ + (abs(-self.w-other.w) < 1e-8 and \ + abs(-self.x-other.x) < 1e-8 and \ + abs(-self.y-other.y) < 1e-8 and \ + abs(-self.z-other.z) < 1e-8) + + def __ne__(self,other): + return not __eq__(self,other) + + def __cmp__(self,other): + return cmp(self.Rodrigues(),other.Rodrigues()) + + def magnitude_squared(self): + return self.w ** 2 + \ + self.x ** 2 + \ + self.y ** 2 + \ + self.z ** 2 + + def identity(self): + self.w = 1 + self.x = 0 + self.y = 0 + self.z = 0 + return self + + def rotateBy_angleaxis(self, angle, axis): + self *= Quaternion.from_angleaxis(angle, axis) + return self + + def rotateBy_Eulers(self, heading, attitude, bank): + self *= Quaternion.from_Eulers(eulers, type) + return self + + def rotateBy_matrix(self, m): + self *= Quaternion.from_matrix(m) + return self + + def normalize(self): + d = self.magnitude() + if d != 0: + self /= d + return self + + def conjugate(self): + self.x = -self.x + self.y = -self.y + self.z = -self.z + return self + + def inverse(self): + d = self.magnitude() + if d != 0: + self.conjugate() + self /= d + return self + + def reduce(self, symmetry='cubic'): + for q in self.symmetricEquivalents(symmetry): + if q.inFZ(symmetry): break + self.w = q.w + self.x = q.x + self.y = q.y + self.z = q.z + return self + + def normalized(self): + return self.copy().normalize() + + def conjugated(self): + return self.copy().conjugate() + + def inversed(self): + return self.copy().inverse() + + def reduced(self): + return self.copy().reduce() + + def angleaxis(self): + if self.w > 1: + self.normalize() + angle = 2 * math.acos(self.w) + s = math.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]) + + def Rodrigues(self): + if self.w != 0.0: + return Vector3([self.x / self.w, self.y / self.w, self.z / self.w]) + else: + return Vector3([float('inf')]*3) + + def Eulers(self,type='bunge'): + angles = [0.0,0.0,0.0] + if type.lower() == 'bunge' or type.lower() == 'zxz': + angles[0] = math.atan2( self.x*self.z+self.y*self.w, + -self.y*self.z+self.x*self.w) +# angles[1] = math.acos(-self.x**2-self.y**2+self.z**2+self.w**2) + angles[1] = math.acos(1.0 - 2*(self.x**2+self.y**2)) + angles[2] = math.atan2( self.x*self.z-self.y*self.w, + +self.y*self.z+self.x*self.w) + + if angles[0] < 0.0: + angles[0] += 2*math.pi + if angles[1] < 0.0: + angles[1] += math.pi + angles[2] *= -1 + if angles[2] < 0.0: + angles[2] += 2*math.pi + return angles + + def inSST(self,symmetry): + return self.Rodrigues().inSST(symmetry) + + def inFZ(self,symmetry): + return self.Rodrigues().inFZ(symmetry) + + def symmetricEquivalents(self,symmetry): + symQuats = {'cubic': + [ + [ 1.0,0.0,0.0,0.0 ], + [ 0.0,-1.0,0.0,0.0 ], + [ 0.0,0.0,-1.0,0.0 ], + [ 0.0,0.0,0.0,1.0 ], + [ 0.0,0.0,0.5*math.sqrt(2), 0.5*math.sqrt(2) ], + [ 0.0,0.0,0.5*math.sqrt(2),-0.5*math.sqrt(2) ], + [ 0.0, 0.5*math.sqrt(2),-0.5*math.sqrt(2),0.0 ], + [ 0.0,-0.5*math.sqrt(2),-0.5*math.sqrt(2),0.0 ], + [ 0.0, 0.5*math.sqrt(2), 0.0, 0.5*math.sqrt(2) ], + [ 0.0, 0.5*math.sqrt(2), 0.0,-0.5*math.sqrt(2) ], + [ 0.5, 0.5, 0.5, 0.5 ], + [ -0.5, 0.5, 0.5, 0.5 ], + [ -0.5, 0.5, 0.5,-0.5 ], + [ -0.5, 0.5,-0.5, 0.5 ], + [ -0.5,-0.5, 0.5, 0.5 ], + [ -0.5,-0.5, 0.5,-0.5 ], + [ -0.5,-0.5,-0.5, 0.5 ], + [ -0.5, 0.5,-0.5,-0.5 ], + [ -0.5*math.sqrt(2),0.0,0.0,0.5*math.sqrt(2) ], + [ 0.5*math.sqrt(2),0.0,0.0,0.5*math.sqrt(2) ], + [ -0.5*math.sqrt(2),0.0,-0.5*math.sqrt(2),0.0 ], + [ -0.5*math.sqrt(2),0.0, 0.5*math.sqrt(2),0.0 ], + [ -0.5*math.sqrt(2), 0.5*math.sqrt(2),0.0,0.0 ], + [ -0.5*math.sqrt(2),-0.5*math.sqrt(2),0.0,0.0 ], + ], + 'hexagonal': + [ + [ 1.0,0.0,0.0,0.0], + [ 0.0,1.0,0.0,0.0], + [ 0.0,0.0,1.0,0.0], + [ 0.0,0.0,0.0,1.0], + [ 0.0, 0.5,-0.5*math.sqrt(3),0.0], + [ 0.0,-0.5,-0.5*math.sqrt(3),0.0], + [ 0.5,0.0,0.0,0.5*math.sqrt(3)], + [-0.5,0.0,0.0,0.5*math.sqrt(3)], + [-0.5*math.sqrt(3),0.0,0.0, 0.5], + [-0.5*math.sqrt(3),0.0,0.0,-0.5], + [0.0, 0.5*math.sqrt(3), 0.5,0.0], + [0.0,-0.5*math.sqrt(3), 0.5,0.0], + ], + 'tetragonal': + [ + [ 0.0,0.0,1.0,0.0 ], + [ 0.0, 0.5*math.sqrt(2),0.5*math.sqrt(2),0.0 ], + [ 0.0,-0.5*math.sqrt(2),0.5*math.sqrt(2),0.0 ], + [ 0.0,0.0,0.0,1.0 ], + [ 0.0,1.0,0.0,0.0 ], + [ 0.5*math.sqrt(2),0.0,0.0,0.5*math.sqrt(2) ], + [ -0.5*math.sqrt(2),0.0,0.0,0.5*math.sqrt(2) ], + [ 1.0,0.0,0.0,0.0 ], + ], + 'orthorombic': + [ + [ 0.0,0.0,1.0,0.0 ], + [ 0.0,0.0,0.0,1.0 ], + [ 0.0,1.0,0.0,0.0 ], + [ 1.0,0.0,0.0,0.0 ], + ], + } + equivalents = [self*Quaternion(q) for q in symQuats[symmetry.lower()]] + return equivalents + + + def disorientation(self,other,symmetry): + + QinSST = Quaternion() + breaker = False + for me in self.symmetricEquivalents(symmetry): + me.inverse() + for they in other.symmetricEquivalents(symmetry): + theQ = me * they + if theQ.w < 0.0: theQ.w = -theQ.w + if theQ.inSST(symmetry): + QinSST = theQ.copy() + breaker = True + break + if breaker: break + + return QinSST + +# # Static constructors + def new_identity(cls): + return cls() + new_identity = classmethod(new_identity) + + def from_random(cls): + r1 = random.random() + r2 = random.random() + r3 = random.random() + Q = cls() + Q.w = math.cos(2.0*math.pi*r1)*math.sqrt(r3) + Q.x = math.sin(2.0*math.pi*r2)*math.sqrt(1.0-r3) + Q.y = math.cos(2.0*math.pi*r2)*math.sqrt(1.0-r3) + Q.z = math.sin(2.0*math.pi*r1)*math.sqrt(r3) + return Q + from_random = classmethod(from_random) + + def from_Rodrigues(cls, rodrigues): + if not isinstance(rodrigues, Vector3): rodrigues = Vector3(rodrigues) + halfangle = math.atan(rodrigues.magnitude()) + c = math.cos(halfangle) + Q = cls() + Q.w = c + Q.x = rodrigues.x * c + Q.y = rodrigues.y * c + Q.z = rodrigues.z * c + return Q + from_Rodrigues = classmethod(from_Rodrigues) + + def from_angleaxis(cls, angle, axis): + assert(isinstance(axis, Vector3)) + axis = axis.normalized() + s = math.sin(angle / 2) + Q = cls() + Q.w = math.cos(angle / 2) + Q.x = axis.x * s + Q.y = axis.y * s + Q.z = axis.z * s + return Q + from_angleaxis = classmethod(from_angleaxis) + + def from_Eulers(cls, eulers, type = 'Bunge'): + c1 = math.cos(eulers[0] / 2.0) + s1 = math.sin(eulers[0] / 2.0) + c2 = math.cos(eulers[1] / 2.0) + s2 = math.sin(eulers[1] / 2.0) + c3 = math.cos(eulers[2] / 2.0) + s3 = math.sin(eulers[2] / 2.0) + Q = cls() + + if type.lower() == 'bunge' or type.lower() == 'zxz': + Q.w = c1 * c2 * c3 - s1 * c2 * s3 + Q.x = c1 * s2 * c3 + s1 * s2 * s3 + Q.y = - c1 * s2 * s3 + s1 * s2 * c3 + Q.z = c1 * c2 * s3 + s1 * c2 * c3 + else: + print 'unknown Euler convention' + 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 + from_Eulers = classmethod(from_Eulers) + + def from_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/math.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/math.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/math.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/math.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 + ) + from_matrix = classmethod(from_matrix) + + 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 = math.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 = math.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 = math.sin((1 - t) * theta) / sintheta + ratio2 = math.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 + new_interpolate = classmethod(new_interpolate) diff --git a/lib/pathinfo b/lib/pathinfo new file mode 100644 index 000000000..516fa5fb6 --- /dev/null +++ b/lib/pathinfo @@ -0,0 +1,3 @@ +ACML /opt/acml-4-4-0/ +FFTW ./ +MSC /msc/