# $Id: bmath.py 3739 2016-03-22 08:14:24Z bnv $
#
# Copyright and User License
# ~~~~~~~~~~~~~~~~~~~~~~~~~~
# Copyright Vasilis.Vlachoudis@cern.ch for the
# European Organization for Nuclear Research (CERN)
#
# All rights not expressly granted under this license are reserved.
#
# Installation, use, reproduction, display of the
# software ("flair"), in source and binary forms, are
# permitted free of charge on a non-exclusive basis for
# internal scientific, non-commercial and non-weapon-related
# use by non-profit organizations only.
#
# For commercial use of the software, please contact the main
# author Vasilis.Vlachoudis@cern.ch for further information.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following
# conditions are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in the
# documentation and/or other materials provided with the
# distribution.
#
# DISCLAIMER
# ~~~~~~~~~~
# THIS SOFTWARE IS PROVIDED BY THE AUTHOR "AS IS"
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT
# NOT LIMITED TO, IMPLIED WARRANTIES OF MERCHANTABILITY, OF
# SATISFACTORY QUALITY, AND FITNESS FOR A PARTICULAR PURPOSE
# OR USE ARE DISCLAIMED. THE COPYRIGHT HOLDERS AND THE
# AUTHORS MAKE NO REPRESENTATION THAT THE SOFTWARE AND
# MODIFICATIONS THEREOF, WILL NOT INFRINGE ANY PATENT,
# COPYRIGHT, TRADE SECRET OR OTHER PROPRIETARY RIGHT.
#
# LIMITATION OF LIABILITY
# ~~~~~~~~~~~~~~~~~~~~~~~
# THE COPYRIGHT HOLDERS AND THE AUTHORS SHALL HAVE NO
# LIABILITY FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL,
# CONSEQUENTIAL, EXEMPLARY, OR PUNITIVE DAMAGES OF ANY
# CHARACTER INCLUDING, WITHOUT LIMITATION, PROCUREMENT OF
# SUBSTITUTE GOODS OR SERVICES, LOSS OF USE, DATA OR PROFITS,
# OR BUSINESS INTERRUPTION, HOWEVER CAUSED AND ON ANY THEORY
# OF CONTRACT, WARRANTY, TORT (INCLUDING NEGLIGENCE), PRODUCT
# LIABILITY OR OTHERWISE, ARISING IN ANY WAY OUT OF THE USE OF
# THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
# Author: Vasilis.Vlachoudis@cern.ch
# Date: 15-May-2004
from __future__ import generators
import math
import random
import pymchelper.flair.common.rexx as rexx
__author__ = "Vasilis Vlachoudis"
__email__ = "Vasilis.Vlachoudis@cern.ch"
# Accuracy for comparison operators
_accuracy = 1E-15
# Formating
_format = "%12g"
# -------------------------------------------------------------------------------
[docs]def Cmp0(x):
return abs(x) < _accuracy
# -------------------------------------------------------------------------------
# range of floating point numbers
# -------------------------------------------------------------------------------
[docs]def frange(start, stop, step):
"""range(start,stop,step) for floating point numbers"""
x = start
if step < 0.0:
while x > stop:
yield x
x += step
else:
while x < stop:
yield x
x += step
# -------------------------------------------------------------------------------
# limit in number within a range
# -------------------------------------------------------------------------------
[docs]def limit(min_, num, max_):
'''
limit a number within a specific range
:param min_:
:param num:
:param max_:
:return:
'''
return max(min(num, max_), min_)
# -------------------------------------------------------------------------------
# dms - degrees from degrees, minutes, seconds
# -------------------------------------------------------------------------------
[docs]def dms(d, m, s):
return d + m / 60.0 + s / 3600.0
# -------------------------------------------------------------------------------
# d2s - degrees to string
# D2S(angle[,"H"|"M"|"D"|"N"])
# -------------------------------------------------------------------------------
[docs]def d2s(ang, fmt=""):
fmt.capitalize()
if ang < 0.0:
neg = "-"
ang = -ang
else:
neg = ""
ang = round(ang * 360000) / 100
SS = "%05.2f" % (math.fmod(ang, 60))
ang = int(ang / 60.0)
MM = "%02d" % (ang % 60)
HH = neg + str(ang / 60)
if fmt == "H":
return HH + "h" + MM + "m" + SS + "s"
if fmt == "M":
return HH + "h" + MM + "m"
if fmt == "D":
return HH + " " + MM + "'" + SS + '"'
if fmt == "N":
return HH + ":" + MM
return HH + ":" + MM + ":" + SS
# -------------------------------------------------------------------------------
# Format a number to fit in the minimum space
# -------------------------------------------------------------------------------
# ==============================================================================
# Dangerous dictionary that unknown keys return a user default value
# Use it with care
# ==============================================================================
[docs]class DefaultDict(dict):
def __init__(self, default=None):
self._default = default
# ----------------------------------------------------------------------
def __getitem__(self, key):
return self.get(key, self._default)
# ==============================================================================
# Unknown keys will return 0.0
# ==============================================================================
[docs]class ZeroDict(DefaultDict):
def __init__(self):
DefaultDict.__init__(self, 0.0)
# ==============================================================================
# Unknown keys will return 0.0
# ==============================================================================
[docs]class ZeroIntDict(DefaultDict):
def __init__(self):
DefaultDict.__init__(self, 0)
# ===============================================================================
# Vector class
# Inherits from List
# ===============================================================================
[docs]class Vector(list):
"""Vector class"""
# ----------------------------------------------------------------------
def __init__(self, x=3, *args):
"""Create a new vector,
Vector(size), Vector(list), Vector(x,y,z,...)"""
if isinstance(x, int) and not args:
for i in range(x):
self.append(0.0)
elif isinstance(x, list) or isinstance(x, tuple):
for i in x:
self.append(float(i))
else:
self.append(float(x))
for i in args:
self.append(float(i))
# ----------------------------------------------------------------------
[docs] def set(self, x, y, z=None):
"""Set vector"""
self[0] = x
self[1] = y
if z:
self[2] = z
# ----------------------------------------------------------------------
def __repr__(self):
return "[%s]" % (", ".join([(_format % (x)).strip() for x in self]))
# ----------------------------------------------------------------------
[docs] def eq(self, v, acc=_accuracy):
"""Test for equality with vector v within accuracy"""
if len(self) != len(v):
return False
s2 = 0.0
for i in range(len(self)):
s2 += (self[i] - v[i])**2
return s2 <= acc**2
def __eq__(self, v):
return self.eq(v)
# ----------------------------------------------------------------------
def __neg__(self):
"""Negate vector"""
new = Vector(len(self))
for i in range(len(self)):
new[i] = -self[i]
return new
# ----------------------------------------------------------------------
def __add__(self, v):
"""Add 2 vectors"""
size = min(len(self), len(v))
new = Vector(size)
for i in range(size):
new[i] = self[i] + v[i]
return new
# ----------------------------------------------------------------------
def __iadd__(self, v):
"""Add vector v to self"""
for i in range(min(len(self), len(v))):
self[i] += v[i]
return self
# ----------------------------------------------------------------------
def __sub__(self, v):
"""Subtract 2 vectors"""
size = min(len(self), len(v))
new = Vector(size)
for i in range(size):
new[i] = self[i] - v[i]
return new
# ----------------------------------------------------------------------
def __isub__(self, v):
"""Subtract vector v from self"""
for i in range(min(len(self), len(v))):
self[i] -= v[i]
return self
# ----------------------------------------------------------------------
# Scale or Dot product
# ----------------------------------------------------------------------
def __mul__(self, v):
"""scale*Vector() or Vector()*Vector() - Scale vector or dot product"""
if isinstance(v, list):
return self.dot(v)
else:
return Vector([x * v for x in self])
# ----------------------------------------------------------------------
# Scale or Dot product
# ----------------------------------------------------------------------
def __rmul__(self, v):
"""scale*Vector() or Vector()*Vector() - Scale vector or dot product"""
if isinstance(v, Vector):
return self.dot(v)
else:
return Vector([x * v for x in self])
# ----------------------------------------------------------------------
# Divide by floating point
# ----------------------------------------------------------------------
def __div__(self, b):
return Vector([x / b for x in self])
# ----------------------------------------------------------------------
def __xor__(self, v):
"""Cross product"""
return self.cross(v)
# ----------------------------------------------------------------------
[docs] def dot(self, v):
"""Dot product of 2 vectors"""
size = min(len(self), len(v))
s = 0.0
for i in range(size):
s += self[i] * v[i]
return s
# ----------------------------------------------------------------------
[docs] def cross(self, v):
"""Cross product of 2 vectors"""
if len(self) == 3:
return Vector(self[1] * v[2] - self[2] * v[1], self[2] * v[0] - self[0] * v[2],
self[0] * v[1] - self[1] * v[0])
elif len(self) == 2:
return self[0] * v[1] - self[1] * v[0]
else:
raise Exception("Cross product needs 2d or 3d vectors")
# ----------------------------------------------------------------------
[docs] def length2(self):
"""Return length squared of vector"""
s = 0.0
for i in range(len(self)):
s += self[i]**2
return s
# ----------------------------------------------------------------------
[docs] def length(self):
"""Return length of vector"""
s = 0.0
for i in range(len(self)):
s += self[i]**2
return math.sqrt(s)
__abs__ = length
# ----------------------------------------------------------------------
[docs] def arg(self):
"""return vector angle"""
return math.atan2(self[1], self[0])
# ----------------------------------------------------------------------
[docs] def norm(self):
"""Normalize vector and return length"""
s_length = self.length()
if s_length > 0.0:
invlen = 1.0 / s_length
for i in range(len(self)):
self[i] *= invlen
return s_length
normalize = norm
# ----------------------------------------------------------------------
[docs] def unit(self):
"""return a unit vector"""
v = self.clone()
v.norm()
return v
# ----------------------------------------------------------------------
[docs] def clone(self):
"""Clone vector"""
return Vector(self)
# ----------------------------------------------------------------------
[docs] def x(self):
return self[0]
[docs] def y(self):
return self[1]
[docs] def z(self):
return self[2]
# ----------------------------------------------------------------------
[docs] def orthogonal(self):
"""return a vector orthogonal to self"""
xx = abs(self.x())
yy = abs(self.y())
if len(self) >= 3:
zz = abs(self.z())
if xx < yy:
if xx < zz:
return Vector(0.0, self.z(), -self.y())
else:
return Vector(self.y(), -self.x(), 0.0)
else:
if yy < zz:
return Vector(-self.z(), 0.0, self.x())
else:
return Vector(self.y(), -self.x(), 0.0)
else:
return Vector(-self.y(), self.x())
# ----------------------------------------------------------------------
[docs] def direction(self, zero=_accuracy):
"""return containing the direction
if normalized with any of the axis"""
v = self.clone()
length = v.norm()
if abs(length) <= zero:
return "O"
if abs(v[0] - 1.0) < zero:
return "X"
elif abs(v[0] + 1.0) < zero:
return "-X"
elif abs(v[1] - 1.0) < zero:
return "Y"
elif abs(v[1] + 1.0) < zero:
return "-Y"
elif abs(v[2] - 1.0) < zero:
return "Z"
elif abs(v[2] + 1.0) < zero:
return "-Z"
else:
# nothing special about the direction, return N
return "N"
# ----------------------------------------------------------------------
# Set the vector directly in polar coordinates
# @param ma magnitude of vector
# @param ph azimuthal angle in radians
# @param th polar angle in radians
# ----------------------------------------------------------------------
[docs] def setPolar(self, ma, ph, th):
sf = math.sin(ph)
cf = math.cos(ph)
st = math.sin(th)
ct = math.cos(th)
self[0] = ma * st * cf
self[1] = ma * st * sf
self[2] = ma * ct
# ----------------------------------------------------------------------
# @return the azimuth angle.
# ----------------------------------------------------------------------
[docs] def phi(self):
if Cmp0(self.x()) and Cmp0(self.y()):
return 0.0
return math.atan2(self.y(), self.x())
# ----------------------------------------------------------------------
# @return the polar angle.
# ----------------------------------------------------------------------
[docs] def theta(self):
if Cmp0(self.x()) and Cmp0(self.y()) and Cmp0(self.z()):
return 0.0
return math.atan2(self.perp(), self.z())
# ----------------------------------------------------------------------
# @return cosine of the polar angle.
# ----------------------------------------------------------------------
[docs] def cosTheta(self):
ptot = self.length()
if Cmp0(ptot):
return 1.0
else:
return self.z() / ptot
# ----------------------------------------------------------------------
# @return the transverse component squared
# (R^2 in cylindrical coordinate system).
# ----------------------------------------------------------------------
[docs] def perp2(self):
return self.x() * self.x() + self.y() * self.y()
# ----------------------------------------------------------------------
# @return the transverse component
# (R in cylindrical coordinate system).
# ----------------------------------------------------------------------
[docs] def perp(self):
return math.sqrt(self.perp2())
# -------------------------------------------------------------------------------
# Basic 3D Vectors
# -------------------------------------------------------------------------------
Vector.O = Vector(0.0, 0.0, 0.0) # noqa: E741
Vector.X = Vector(1.0, 0.0, 0.0)
Vector.Y = Vector(0.0, 1.0, 0.0)
Vector.Z = Vector(0.0, 0.0, 1.0)
# -------------------------------------------------------------------------------
# Return a random 3D vector
# -------------------------------------------------------------------------------
[docs]def random3D():
cosTheta = 2.0 * random.random() - 1.0
sinTheta = math.sqrt(1.0 - cosTheta**2)
phi = 2.0 * math.pi * random.random()
return Vector(math.cos(phi) * sinTheta, math.sin(phi) * sinTheta, cosTheta)
# ------------------------------------------------------------------------------
# Return a random nolor
# ------------------------------------------------------------------------------
[docs]def rndColor(x):
def rnd(zw):
w = zw & 0xffff
z = (zw >> 16) & 0xffff
z = 36969 * (z & 0xffff) + (z >> 16)
w = 18000 * (w & 0xffff) + (w >> 16)
return (z << 16) + (w & 0xffff)
x = rnd(x)
R = (x % 224) + 16
x = rnd(x)
G = (x % 224) + 16
x = rnd(x)
B = (x % 224) + 16
return R << 16 | G << 8 | B
# ===============================================================================
# Matrix class
# Use 4x4 matrix for vector transformations
# ===============================================================================
[docs]class Matrix(list):
"""Matrix 4x4 used for vector transformations"""
# ----------------------------------------------------------------------
def __init__(self, rows=4, cols=-1, type=0):
"""
Matrix(rows=4, cols=-1, type=0|1)
if rows is integer then
Create a matrix rows x cols either
zero(type=0) or unary(type=1)
elif rows is a list of lists
create a matrix from a double-list
"""
if isinstance(rows, list):
lst = rows
self.rows = len(lst)
self.extend([[]] * self.rows)
if isinstance(lst[0], list):
self.cols = len(lst[0])
for i in range(self.rows):
self[i] = lst[i][:]
if len(self[i]) != self.cols:
raise Exception("Not a valid double-list for a matrix")
else:
self.cols = 1
for i in range(self.rows):
self[i] = [lst[i]]
else:
if rows < 2:
raise Exception("Array size too small")
if cols < 0:
cols = rows
self.rows = rows
self.cols = cols
self += [[]] * rows
if type == 1:
self.unary()
else:
self.zero()
# ----------------------------------------------------------------------
# Create a diagonal square matrix from a list
# ----------------------------------------------------------------------
[docs] @staticmethod
def diagonal(lst):
m = Matrix(len(lst), type=0)
i = 0
for item in lst:
m[i][i] = item
i += 1
return m
# ----------------------------------------------------------------------
# append row
# ----------------------------------------------------------------------
[docs] def append(self, col):
list.append(self, col)
self.rows += 1
# ----------------------------------------------------------------------
[docs] @staticmethod
def translate(x, y=0.0, z=0.0):
"""m = Matrix.translate(x,y,z|vector)
@return a translation matrix"""
m = Matrix(4, type=1)
if isinstance(x, list) or isinstance(x, tuple):
m[0][3] = x[0]
m[1][3] = x[1]
m[2][3] = x[2]
else:
m[0][3] = x
m[1][3] = y
m[2][3] = z
return m
# ----------------------------------------------------------------------
[docs] @staticmethod
def scale(sx, sy=None, sz=None):
"""m = Matrix.scale(scale|vector)
@return a scaling matrix"""
m = Matrix(4, type=1)
if sy is None:
sy = sx
if sz is None:
sz = sx
if isinstance(sx, list) or isinstance(sx, tuple):
m[0][0] = sx[0]
m[1][1] = sx[1]
m[2][2] = sx[2]
else:
m[0][0] = sx
m[1][1] = sy
m[2][2] = sz
return m
# ----------------------------------------------------------------------
[docs] def zero(self):
"""Zero matrix"""
for i in range(self.rows):
self[i] = [0.0] * self.cols
# ----------------------------------------------------------------------
[docs] def unary(self):
"""Unary matrix"""
self.zero()
for i in range(min(self.rows, self.cols)):
self[i][i] = 1.0
# ----------------------------------------------------------------------
# Create a transformation matrix from 3 normalized vectors
# and optionally a translation
# ----------------------------------------------------------------------
[docs] def make(self, X, Y, Z=None, T=None):
"""Create a transformation matrix from 3 normalized vectors"""
self.unary()
if self.rows in (3, 4) and self.cols == self.rows:
if Z is None:
Z = X ^ Y
Z.normalize()
for i in range(3):
self[0][i] = X[i]
self[1][i] = Y[i]
self[2][i] = Z[i]
if T is not None and self.rows == 4:
self[i][3] = T[i]
else:
raise Exception("Matrix.make() works only " "on Matrix(3x3) or Matrix(4x4)")
# ----------------------------------------------------------------------
def __str__(self):
"""Multiline string representation of matrix"""
s = ""
for i in range(self.rows):
if i == 0:
first = "/"
last = "\\"
elif i == self.rows - 1:
first = "\\"
last = "/"
else:
first = last = "|"
s += first
for j in range(self.cols):
s += " " + _format % self[i][j]
s += " " + last + "\n"
return s
# ----------------------------------------------------------------------
[docs] def writeOctave(self, filename, name):
"""Write an octave matrix file"""
f = open(filename, "w")
f.write("# bmath.Matrix\n")
f.write("# name: %s\n" % (name))
f.write("# type: matrix\n")
f.write("# rows: %d\n" % (self.rows))
f.write("# columns: %d\n" % (self.cols))
for i in range(self.rows):
for j in range(self.cols):
f.write("%s " % (str(self[i][j])))
f.write("\n")
f.close()
# ----------------------------------------------------------------------
[docs] def T(self):
"""@return transpose matrix"""
m = Matrix(self.cols, self.rows)
for i in range(self.rows):
for j in range(self.cols):
m[j][i] = self[i][j]
return m
transpose = T
# ----------------------------------------------------------------------
[docs] def trace(self):
"""Return trace of matrix (sum of diagonal elements)"""
t = 0.0
for i in range(min(self.rows, self.cols)):
t += self[i][i]
return t
# ----------------------------------------------------------------------
def __eq__(self, m):
"""Test for equality of 2 matrices"""
if self.rows != m.rows or self.cols != m.cols:
return False
for i in range(self.rows):
for j in range(self.cols):
if abs(self[i][j] - m[i][j]):
return False
return True
# ----------------------------------------------------------------------
# Create a rotation matrix around one axis
# X = 0
# Y = 1
# Z = 2
# or an arbitrary vector
# ----------------------------------------------------------------------
[docs] def rotate(self, angle, axis):
"""Add rotation elements to the matrix around one axis
Axis X=0, Y=1, Z=2, or an arbitrary one given by vector axis"""
self.unary()
c = math.cos(angle)
s = math.sin(angle)
if isinstance(axis, int):
m1 = ((axis + 1) % 3) + 1
m2 = m1 % 3
m1 -= 1
self[m1][m1] = c
self[m2][m2] = c
self[m1][m2] = -s
self[m2][m1] = s
elif isinstance(axis, Vector):
length = axis.length()
x = axis[0] / length
y = axis[1] / length
z = axis[2] / length
c1 = 1 - c
self[0][0] = x * x + (1 - x * x) * c
self[0][1] = x * y * c1 - z * s
self[0][2] = x * z * c1 + y * s
self[1][0] = x * y * c1 + z * s
self[1][1] = y * y + (1 - y * y) * c
self[1][2] = y * z * c1 - x * s
self[2][0] = x * z * c1 - y * s
self[2][1] = y * z * c1 + x * s
self[2][2] = z * z + (1 - z * z) * c
# ----------------------------------------------------------------------
[docs] @staticmethod
def rotX(angle):
"""m = Matrix.rotX(angle) - Return a rotation matrix around X"""
m = Matrix(4, type=1)
m.rotate(angle, 0)
return m
# ----------------------------------------------------------------------
[docs] @staticmethod
def rotY(angle):
"""m = Matrix.rotY(angle) - Return a rotation matrix arround Y"""
m = Matrix(4, type=1)
m.rotate(angle, 1)
return m
# ----------------------------------------------------------------------
[docs] @staticmethod
def rotZ(angle):
"""m = Matrix.rotZ(angle) - Return a rotation matrix arround Z"""
m = Matrix(4, type=1)
m.rotate(angle, 2)
return m
# ----------------------------------------------------------------------
[docs] def getEulerRotation(self):
# ROTX(x) * ROTY(y) * ROTZ(z)
# cos(z)*cos(y)
# sin(z)*cos(y)
# -sin(y)
# -sin(z)*cos(x)+cos(z)*sin(y)*sin(x)
# cos(z)*cos(x)+sin(z)*sin(y)*sin(x)
# cos(y)*sin(x)
# sin(z)*sin(x)+cos(z)*sin(y)*cos(x)
# -cos(z)*sin(x)+sin(z)*sin(y)*cos(x)
# cos(y)*cos(x)
rx = math.atan2(self[1][2], self[2][2])
ry = -math.asin(self[0][2])
rz = math.atan2(self[0][1], self[0][0])
return rx, ry, rz
# ----------------------------------------------------------------------
[docs] @staticmethod
def eulerRotation(rx, ry, rz):
# ROTX(x) * ROTY(y) * ROTZ(z)
m = Matrix(4, type=1)
cx = math.cos(rx)
cy = math.cos(ry)
cz = math.cos(rz)
sx = math.sin(rx)
sy = math.sin(ry)
sz = math.sin(rz)
row = m[0]
row[0] = cz * cy
row[1] = sz * cy
row[2] = -sy
row = m[1]
row[0] = -sz * cx + cz * sy * sx
row[1] = cz * cx + sz * sy * sx
row[2] = cy * sx
row = m[2]
row[0] = sz * sx + cz * sy * cx
row[1] = -cz * sx + sz * sy * cx
row[2] = cy * cx
return m
# ----------------------------------------------------------------------
def __add__(self, B):
"""Add 2 matrices"""
if self.rows != B.rows or self.cols != B.cols:
raise Exception("Matrix.add: matrices same size")
m = Matrix(self.rows, self.cols)
for i in range(self.rows):
mrow = m[i]
arow = self[i]
brow = B[i]
for j in range(self.cols):
mrow[j] = arow[j] + brow[j]
return m
# ----------------------------------------------------------------------
def __sub__(self, B):
"""Subtract 2 matrices"""
if self.rows != B.rows or self.cols != B.cols:
raise Exception("Matrix.add: matrices same size")
m = Matrix(self.rows, self.cols)
for i in range(self.rows):
mrow = m[i]
arow = self[i]
brow = B[i]
for j in range(self.cols):
mrow[j] = arow[j] - brow[j]
return m
# ----------------------------------------------------------------------
def __neg__(self):
"""Negate matrix"""
m = Matrix(self.rows, self.cols)
for i in range(self.rows):
mrow = m[i]
mold = self[i]
for j in range(self.cols):
mrow[j] = -mold[j]
return m
# ----------------------------------------------------------------------
def __mul__(self, B):
"""Multiply two matrices or vector
A.__mul__(B|vec) <==> A*B or A*vec"""
if isinstance(B, Matrix): # has to be a matrix of same cxN * Nxr
if self.cols != B.rows:
raise Exception("arrays don't have the correct dimensions")
r = Matrix(self.rows, B.cols)
for i in range(self.rows):
for j in range(B.cols):
s = 0.0
for k in range(self.cols):
s += self[i][k] * B[k][j]
r[i][j] = s
return r
elif isinstance(B, list): # Vector or list
vecsize = len(B)
v = Vector(vecsize)
for i in range(vecsize):
for j in range(min(self.cols, vecsize)):
v[i] += self[i][j] * B[j]
for j in range(vecsize, self.cols):
v[i] += self[i][j]
return v
else:
for row in self:
for i in range(self.cols):
row[i] *= B
return self
# -----------------------------------------------------------------------
# Special function to multiply a transformation matrix with a vector
# ignoring the translation
# -----------------------------------------------------------------------
[docs] def multNoTranslation(self, B):
"""Multiply matrix with a vector ignoring the translation part"""
if not isinstance(B, list):
raise Exception("Invalid operation")
vecsize = len(B)
v = Vector(vecsize)
for i in range(vecsize):
for j in range(min(self.cols, vecsize)):
v[i] += self[i][j] * B[j]
return v
# ----------------------------------------------------------------------
[docs] def inv(self):
"""Inverse matrix in place"""
if self.rows != self.cols:
raise Exception("inverting a non square matrix")
index = [0] * self.rows
self.__ludcmp(index)
y = Matrix(self.rows)
for j in range(self.rows):
col = [0.0] * self.rows
col[j] = 1.0
self.__lubksb(index, col)
for i in range(self.rows):
y[i][j] = col[i]
for j in range(self.rows):
self[j] = y[j]
inverse = inv
# ----------------------------------------------------------------------
[docs] def clone(self):
"""Clone matrix"""
m = Matrix(self.rows, self.cols)
for i in range(self.rows):
m[i] = self[i][:]
return m
# ----------------------------------------------------------------------
# determinant with Gauss method
# ----------------------------------------------------------------------
[docs] def det(self, eps=_accuracy):
"""determinant of square matrix using Gauss method"""
if self.rows == 2:
return self[0][0] * self[1][1] - self[1][0] * self[0][1]
elif self.rows == 3:
return self[0][0] * (self[1][1] * self[2][2] - self[2][1] * self[1][2]) \
- self[0][1] * (self[1][0] * self[2][2] - self[2][0] * self[1][2]) \
+ self[0][2] * (self[1][0] * self[2][1] - self[2][0] * self[1][1])
M = self.clone()
s = 1.0
n = M.rows
for i in range(n - 1):
# find the absolute maximum value
ma = abs(M[i][i])
k = i
for j in range(i + 1, n):
if abs(M[j][i]) > ma:
ma = abs(M[j][i])
k = j
if ma < eps:
return 0.0
# swap rows i,k
if i != k:
s = -s # Change sign of determinate
for j in range(n):
d = M[i][j]
M[i][j] = M[k][j]
M[k][j] = d
# make all the following rows with zero at the i column
for j in range(i + 1, n):
if abs(M[j][i]) < _accuracy:
continue
d = -M[i][i] / M[j][i]
s *= d
for k in range(i, n):
M[j][k] = M[i][k] + d * M[j][k]
d = M[0][0] / s
for i in range(1, n):
d *= M[i][i]
return d
determinant = det
# ----------------------------------------------------------------------
# LU decomposition.
# Parameters
# index[0:size] row permutation record
# ----------------------------------------------------------------------
def __ludcmp(self, index): # procedure expose indx.
size = self.rows
vv = [0.0] * size
for i in range(size):
big = 0.0
for j in range(size):
big = max(abs(self[i][j]), big)
if big == 0:
raise Exception("Singular matrix found")
vv[i] = 1.0 / big
for j in range(size):
for i in range(j):
s = self[i][j]
for k in range(i):
s -= self[i][k] * self[k][j]
self[i][j] = s
big = 0.0
for i in range(j, size):
s = self[i][j]
for k in range(j):
s -= self[i][k] * self[k][j]
self[i][j] = s
dum = vv[i] * abs(s)
if dum >= big:
big = dum
imax = i
if j != imax:
for k in range(size):
dum = self[imax][k]
self[imax][k] = self[j][k]
self[j][k] = dum
vv[imax] = vv[j]
index[j] = imax
if self[j][j] == 0.0:
self[j][j] = 1E-20
if j != size - 1:
dum = 1.0 / self[j][j]
for i in range(j + 1, size):
self[i][j] *= dum
# ----------------------------------------------------------------------
# backward substitution
# index[0:size] row permutation record
# col[0:size] right hand vector (?)
# ----------------------------------------------------------------------
def __lubksb(self, index, col):
ii = -1
size = self.rows
for i in range(size):
ip = index[i]
s = col[ip]
col[ip] = col[i]
if ii >= 0:
for j in range(ii, i):
s -= self[i][j] * col[j]
elif s != 0.0:
ii = i
col[i] = s
for i in range(size - 1, -1, -1):
s = col[i]
for j in range(i + 1, size):
s -= self[i][j] * col[j]
col[i] = s / self[i][i]
# -------------------------------------------------------------------------------
# Basic Matrices
# -------------------------------------------------------------------------------
Matrix.O = Matrix(4, type=0) # noqa: E741
Matrix.U = Matrix(4, type=1)
# -------------------------------------------------------------------------------
# Quaternion
#
# Note: See the following for more information on quaternions:
#
# - Shoemake, K., Animating rotation with quaternion curves, Computer
# Graphics 19, No 3 (Proc. SIGGRAPH'85), 245-254, 1985.
# - Pletinckx, D., Quaternion calculus as a basic tool in computer
# graphics, The Visual Computer 5, 2-13, 1989.
# -------------------------------------------------------------------------------
[docs]class Quaternion(list):
def __init__(self, a, b=None, c=None, d=None):
if isinstance(a, Quaternion):
self.extend(a)
elif isinstance(a, Matrix):
tr = a[0][0] + a[1][1] + a[2][2] + 1.0 # trace of matrix
if tr > 0:
S = math.sqrt(tr) * 2.0 # S=4*qw
qw = 0.25 * S
qx = (a[2][1] - a[1][2]) / S
qy = (a[0][2] - a[2][0]) / S
qz = (a[1][0] - a[0][1]) / S
elif a[0][0] > a[1][1] and a[0][0] > a[2][2]:
# S=4*qx
S = math.sqrt(1.0 + a[0][0] - a[1][1] - a[2][2]) * 2.0
qx = 0.25 * S
qy = (a[0][1] + a[1][0]) / S
qz = (a[0][2] + a[2][0]) / S
qw = (a[2][1] - a[1][2]) / S
elif a[1][1] > a[2][2]:
# S=4*qy
S = math.sqrt(1.0 + a[1][1] - a[0][0] - a[2][2]) * 2.0
qx = (a[0][1] + a[1][0]) / S
qy = 0.25 * S
qz = (a[1][2] + a[2][1]) / S
qw = (a[0][2] - a[2][0]) / S
else:
# S=4*qz
S = math.sqrt(1.0 + a[2][2] - a[0][0] - a[1][1]) * 2.0
qx = (a[0][2] + a[2][0]) / S
qy = (a[1][2] + a[2][1]) / S
qz = 0.25 * S
qw = (a[1][0] - a[0][1]) / S
self.extend([qx, qy, qz, qw])
elif isinstance(a, Vector) and isinstance(b, float):
s = math.sin(b / 2.0) / a.length()
self.append(a[0] * s)
self.append(a[1] * s)
self.append(a[2] * s)
self.append(math.cos(b / 2.0))
else:
self.extend([a, b, c, d])
# ----------------------------------------------------------------------
# Quaternions always obey: a^2 + b^2 + c^2 + d^2 = 1.0
# If they don't add up to 1.0, dividing by their magnitued will
# renormalize them.
# ----------------------------------------------------------------------
[docs] def norm(self):
"""normalize quaternion"""
mag = math.sqrt(self[0]**2 + self[1]**2 + self[2]**2 + self[3]**2)
self[0] /= mag
self[1] /= mag
self[2] /= mag
self[3] /= mag
return mag
normalize = norm
# ----------------------------------------------------------------------
[docs] def vector(self):
"""return vector of quaternion"""
return Vector(self[0], self[1], self[2])
# ----------------------------------------------------------------------
# return rotation matrix
# ----------------------------------------------------------------------
[docs] def matrix(self):
"""return rotation matrix"""
m = Matrix(4, type=1)
m[0][0] = 1.0 - 2.0 * (self[1] * self[1] + self[2] * self[2])
m[0][1] = 2.0 * (self[0] * self[1] - self[2] * self[3])
m[0][2] = 2.0 * (self[2] * self[0] + self[1] * self[3])
m[1][0] = 2.0 * (self[0] * self[1] + self[2] * self[3])
m[1][1] = 1.0 - 2.0 * (self[2] * self[2] + self[0] * self[0])
m[1][2] = 2.0 * (self[1] * self[2] - self[0] * self[3])
m[2][0] = 2.0 * (self[2] * self[0] - self[1] * self[3])
m[2][1] = 2.0 * (self[1] * self[2] + self[0] * self[3])
m[2][2] = 1.0 - 2.0 * (self[1] * self[1] + self[0] * self[0])
return m
# ----------------------------------------------------------------------
# Given two rotations, e1 and e2, expressed as quaternion rotations,
# figure out the equivalent single rotation and stuff it into dest.
# This routine also normalizes the result every RENORMCOUNT times it is
# called, to keep error from creeping in.
# ----------------------------------------------------------------------
def __add__(self, b):
v1 = self.vector()
v2 = b.vector()
t1 = v1 * b[3]
t2 = v2 * self[3]
t3 = v2.cross(v1)
tf = t1 + t2 + t3
q = Quaternion(tf, self[3] * b[3] - v1.dot(v2))
q.norm()
return q
# ----------------------------------------------------------------------
def __iadd__(self, b):
v1 = self.vector()
v2 = b.vector()
t1 = v1 * b[3]
t2 = v2 * self[3]
t3 = v2.cross(v1)
tf = t1 + t2 + t3
self[0] = tf[0]
self[1] = tf[1]
self[2] = tf[2]
self[3] = self[3] * b[3] - v1.dot(v2)
self.norm()
return self
# -------------------------------------------------------------------------------
[docs]def gauss(A, B):
"""Solve A*X = B using the Gauss elimination method"""
n = len(A)
s = [0.0] * n
X = [0.0] * n
p = list(range(n))
for i in range(n):
s[i] = max([abs(x) for x in A[i]])
for k in range(n - 1):
# select j>=k so that
# |A[p[j]][k]| / s[p[i]] >= |A[p[i]][k]| / s[p[i]] for i = k,k+1,...,n
j = k
ap = abs(A[p[j]][k]) / s[p[j]]
for i in range(k + 1, n):
api = abs(A[p[i]][k]) / s[p[i]]
if api > ap:
j = i
ap = api
if j != k:
p[k], p[j] = p[j], p[k] # Swap values
for i in range(k + 1, n):
z = A[p[i]][k] / A[p[k]][k]
A[p[i]][k] = z
for j in range(k + 1, n):
A[p[i]][j] -= z * A[p[k]][j]
for k in range(n - 1):
for i in range(k + 1, n):
B[p[i]] -= A[p[i]][k] * B[p[k]]
for i in range(n - 1, -1, -1):
X[i] = B[p[i]]
for j in range(i + 1, n):
X[i] -= A[p[i]][j] * X[j]
X[i] /= A[p[i]][i]
return X
# -------------------------------------------------------------------------------
[docs]def solveOverDetermined(A, B, W=None):
"""Solve the overdetermined linear system defined by the matrices A,B
such as A*X = B
Optionally a weight can be specified"""
if A.rows < A.cols:
raise Exception("solveOverDetermined: " "A matrix has more columns than rows")
AT = A.transpose()
if W:
Wd = Matrix.diagonal(W)
ATA = AT * Wd * A
ATB = AT * Wd * B
else:
ATA = AT * A
ATB = AT * B
ATA.inv()
RT = ATA * ATB
return [RT[i][0] for i in range(len(RT))]
# -------------------------------------------------------------------------------
[docs]def linear(X, Y):
"""
Solve linear regression y = ax + b
@return a,b,r
"""
Sx = Sy = Sx2 = Sy2 = Sxy = 0.0
for x, y in zip(X, Y):
Sx += x
Sy += y
Sx2 += x * x
Sy2 += y * y
Sxy += x * y
n = float(len(X))
try:
b = (Sxy - Sx * Sy / n) / (Sx2 - Sx * Sx / n)
a = Sy / n - b * Sx / n
r = (Sxy - Sx * Sy / n) / math.sqrt(Sx2 - Sx * Sx / n) * \
math.sqrt(Sy2 - Sy * Sy / n)
return a, b, r
except ZeroDivisionError:
return None
# -------------------------------------------------------------------------------
# Idiotimes pragmatikwv symmetrikwv pivakwv
#
# O algori8mos poy xrnsimopoieitai stnv roytiva eivai gvwstos sav
# proseggistikn me8odos Jacobi.
# O algori8mos ekmetaleyetai tnv idiotnta poy exoyv oi diagwvioi
# pivakes, dnladn pivakes me mndevika ola ta stoixeia ektos tns
# kyrias diagwvioy, va exoyv sav idiotimes ta diagwvia stoixeia.
# Me tov metasxnmatismo.
# T T
# A1 = R1 (f) A R1(f), A2 = R2 (f) A1 R2(f)
# metaballoyme syvexws tov pivaka A, mexris otoy to a8roisma olwv
# twv mn diagwviwv stoixeiwv f8asei mia ka8orismevn timn tns eklogns
# toy xrnstn n givei mndev
# Ta bnmata tns diadikasias eivai:
# 1. Avazntnsn toy apolytws megistoy mn diagwvioy stoixeioy
# Divei ta p kai q
# 2. Prosdiorismos tns gwvias peristrofns f. Divei ta sinf kai cosf
# 3. Metasxnmatismos Ai -> Ai+1
# 4. Elegxos av to a8roisma twv mn diagwviwv stoixeiwv exei f8asei tnv
# epi8ymntn timn. Eav vai tote ta diagwvia stoixeia eivai oi
# proseggiseis twv idiotimwv, eav oxi tote epistrefoyme sto 1.
# px. | 1 -2 -1 |
# A = | -2 1 -1 |
# | -1 -1 2.5|
# apolyto megisto A(1,2) = -2
# Ypologizoyme tnv gwvia f, co=cos(f), si=sin(f) kai kavoyme tov
# metasxnmatismo
# | co -si 0 | | 1 -2 -1 | | co si 0 |
# A = | si co 0 | x | -2 1 -1 | x | -si co 0 |
# | 0 0 1 | | -1 -1 2.5| | 0 0 1 |
#
#
# Oi parametroi tns roytivas eivai oi e3ns:
# A - pivakas tetragwvikos
# eps - akribeia (a8roisma tetragwvwv)
# check - av prepei va elejei tnv symmetria toy arxikoy pivaka
# n oxi
# -------------------------------------------------------------------------------
[docs]def eigenvalues(M, eps=_accuracy, check=False):
"""Return eigen values and eigen vectors of a symmetric matrix"""
n = M.rows
# elegxos av eivai symmetrikos o pivakas
if check:
if n != M.cols:
return None
for i in range(n):
for j in range(i, n):
if M[i][j] != M[j][i]:
return None
# Allocate arrays
A = M.clone()
R = Matrix(n, type=0)
RT = Matrix(n, type=0)
ZW = Matrix(n, type=0)
V = None
# kavovika 8a prepei meta apo merikes prospa8eies va tov aporiptei
while True:
# Bnma 1. Avazntnsn toy apolytws megistoy mn diagwvioy stoixeioy
p = 0
q = 1
el = abs(A[p][q])
for i in range(1, n):
for j in range(i):
if abs(A[i][j]) > el:
el = abs(A[i][j])
p = i
q = j
if el == 0:
break
# Ftiaxvei ta R, RT
for i in range(n):
for j in range(n):
R[i][j] = RT[i][j] = (i == j)
# Bnma 2. Prosdiorizei tnv gwvia f, cosf kai sinf
fi = (A[q][q] - A[p][p]) / (2.0 * A[p][q])
t = 1.0 / (fi + math.sqrt(fi * fi + 1.0))
if fi < 0:
t = -t
co = 1.0 / math.sqrt(1.0 + t * t)
si = t / math.sqrt(1.0 + t * t)
R[p][p] = R[q][q] = co
RT[p][p] = RT[q][q] = co
R[p][q] = si
R[q][p] = -si
RT[p][q] = -si
RT[q][p] = si
# Bnma 3. metasxnmatismos Ai+1 = Rt * Ai * R
# ka8os kai to ginomeno Rn*...*R2*R1 that
# gives us the eigenvectors
if V is None:
V = R.clone()
else:
V = V * R
for i in range(n):
for j in range(n):
if j not in (p, q):
ZW[i][j] = A[i][j]
else:
zw1 = 0
for k in range(n):
zw1 += A[i][k] * R[k][j]
ZW[i][j] = zw1
for i in range(n):
for j in range(n):
if i not in (p, q):
A[i][j] = ZW[i][j]
else:
zw1 = 0
for k in range(n):
zw1 += RT[i][k] * ZW[k][j]
A[i][j] = zw1
# Bnma 4. Briskoymai to a8roisma kai elegxoyme av teleiwse
zw1 = 0
k = 0
for i in range(1, n):
for j in range(i):
zw1 += A[i][j] * A[i][j]
k += 1
zw1 /= n
# Exit condition
if zw1 <= eps:
break
return ([A[i][i] for i in range(n)], V.T())
# -------------------------------------------------------------------------------
# Given a function, and given a bracketing triplet of abscissas ax,bx,cx (such
# that bx is between ax and cx, and f(bx) is less than both f(ax) and f(cx),
# this routing performs a golden section search for the minimum, isolating it
# to a fractional precision of about eps. The abscissa of the minimum is
# returned as xmin, and the minimum function value is returned as golden, the
# returned function value.
#
# @param func function to be evaluated
# @param ax triplet of abscissas ax,bx,cx
# @param bx where func(x+bx*d) < min[ func(x+ax*d), func(x+cx*d) ]
# @param cx ...
# @param x starting vector/value
# @param d direction vector/value
# @param eps accuracy of search
# -------------------------------------------------------------------------------
[docs]def goldenSectionSearch(func, ax, bx, cx, x, d=1, eps=_accuracy):
R = 0.61803399 # The golden ratio
C = (1.0 - R)
x0 = ax # At any given time we will keep track of four points
x3 = cx # x0, x1, x2, x3
if abs(cx - bx) > abs(bx - ax):
x1 = bx
x2 = bx + C * (cx - bx)
else:
x2 = bx
x1 = bx - C * (bx - ax)
f1 = func(x + x1 * d) # The initial function evaluation
f2 = func(x + x2 * d)
while abs(x3 - x0) > eps * (abs(x1) + abs(x2)):
if f2 < f1:
x0 = x1
x1 = x2
x2 = R * x1 + C * x3
f1 = f2
f2 = func(x + x2 * d)
else:
x3 = x2
x2 = x1
x1 = R * x2 + C * x0
f2 = f1
f1 = func(x + x1 * d)
if f1 < f2:
return x1
else:
return x2
# -------------------------------------------------------------------------------
# Generators for calculating a) the permutations of a sequence and
# b) the combinations and selections of a number of elements from a
# sequence. Uses Python 2.2 generators.
# Similar solutions found also in comp.lang.python
# Keywords: generator, combination, permutation, selection
#
# See also: http://aspn.activestate.com/ASPN/Cookbook/Python/Recipe/105962
# See also: http://aspn.activestate.com/ASPN/Cookbook/Python/Recipe/66463
# See also: http://aspn.activestate.com/ASPN/Cookbook/Python/Recipe/66465
# -------------------------------------------------------------------------------
[docs]def xcombinations(items, n):
if n <= 0:
yield []
else:
for i in range(len(items)):
for cc in xcombinations(items[:i] + items[i + 1:], n - 1):
yield [items[i]] + cc
[docs]def xuniqueCombinations(items, n):
if n <= 0:
yield []
else:
for i in range(len(items)):
for cc in xuniqueCombinations(items[i + 1:], n - 1):
yield [items[i]] + cc
[docs]def xselections(items, n):
if n <= 0:
yield []
else:
for i in range(len(items)):
for ss in xselections(items, n - 1):
yield [items[i]] + ss
[docs]def xpermutations(items):
return xcombinations(items, len(items))
# -------------------------------------------------------------------------------
# Conversion between rectangular and polar coordinates
# Usage:
# real, real = rect(real, real [, deg=False])
# real, real = polar(real, real [, deg=False])
# Normally, rect() and polar() uses radian for angle; but,
# if deg=True specified, degree is used instead.
# -------------------------------------------------------------------------------
# radian if deg=False; degree if deg=True
[docs]def rect(r, w, deg=False):
"""
Convert from polar (r,w) to rectangular (x,y)
x = r cos(w)
y = r sin(w)
"""
if deg:
w = math.radians(w)
return r * math.cos(w), r * math.sin(w)
# -------------------------------------------------------------------------------
# radian if deg=False; degree if deg=True
# -------------------------------------------------------------------------------
[docs]def polar(x, y, deg=False):
"""
Convert from rectangular (x,y) to polar (r,w)
r = sqrt(x^2 + y^2)
w = arctan(y/x) = [-pi,pi] = [-180,180]
"""
if deg:
return math.hypot(x, y), math.degrees(math.atan2(y, x))
else:
return math.hypot(x, y), math.atan2(y, x)
[docs]def quadratic(b, c, eps=_accuracy):
D = b * b - 4.0 * c
if D <= 0.0:
x1 = -0.5 * b # Always return this as a solution!!!
if D >= -eps * (b * b + abs(c)):
return x1, x1
else:
return None, None
else:
if b > 0.0:
bD = -b - math.sqrt(D)
else:
bD = -b + math.sqrt(D)
return 0.5 * bD, 2.0 * c / bD
# -------------------------------------------------------------------------------
# Cubic equation: y^3 + a*y^2 + b*y + c = 0 (or ax^3 + bx^2 + cx + d = 0)
#
# Normally, x^3 + ax^2 + bx + c = 0 is assumed with the 3 coefficients as
# arguments; but, if 4 arguments are present, then ax^3 + bx^2 + cx + d = 0 is
# assumed.
#
# Even though both quadratic() and cubic() functions take real arguments, they
# can be modified to accept any real or complex coefficients because the method
# of solution does not make any assumptions.
# -------------------------------------------------------------------------------
[docs]def cubic(a, b, c, d=None, eps=_accuracy):
if d is not None: # (ax^3 + bx^2 + cx + d = 0)
a, b, c = b / float(a), c / float(a), d / float(a)
Q = (a * a - 3.0 * b) / 9.0
R = (2. * a**3 - 9. * a * b + 27. * c) / 54.
R2 = R**2
Q3 = Q**3
if R2 < Q3: # the cubic has 3 real solutions
theta = math.acos(R / math.sqrt(Q3))
sqrt_Q = math.sqrt(Q)
x1 = -2. * sqrt_Q * math.cos(theta / 3.) - a / 3.
x2 = -2. * sqrt_Q * math.cos((theta + 2. * math.pi) / 3.) - a / 3.
x3 = -2. * sqrt_Q * math.cos((theta - 2. * math.pi) / 3.) - a / 3.
return x1, x2, x3
A = -math.copysign(1.0, R) * (abs(R) + math.sqrt(R2 - Q3))**(1. / 3.)
if abs(A) > eps:
B = Q / A
else:
B = 0.0
return (A + B) - a / 3., None, None
# imaginary roots
# x2 = -(A+B)/2 - a/3 + i*sqrt(3)*(A-B)
# x3 = -(A+B)/2 - a/3 - i*sqrt(3)*(A-B)
# -------------------------------------------------------------------------------
# Fit a plane to a set of points using least square fitting
# -------------------------------------------------------------------------------
[docs]def fitPlane(xyz):
# First do statistics with points
Sx = Sy = Sz = 0.0
Sx2 = Sy2 = Sz2 = 0.0
Sxy = Syz = Sxz = 0.0
for x, y, z in xyz:
Sx += x
Sy += y
Sz += z
Sx2 += x**2
Sy2 += y**2
Sz2 += z**2
Sxy += x * y
Syz += y * z
Sxz += x * z
n = float(len(xyz))
Sx /= n
Sy /= n
Sz /= n
Vx = Sx2 / n - Sx**2
Vy = Sy2 / n - Sy**2
Vz = Sz2 / n - Sz**2
# Count zero variances
nv = int(abs(Vx) <= _accuracy) + int(abs(Vy) <= _accuracy) + \
int(abs(Vz) <= _accuracy)
if nv > 1:
return None
elif nv == 1:
# Planes parallel to axes
# Try the solution of x=Xo or y=Yo or z=Zo
if abs(Vx) <= _accuracy:
return 1.0, 0.0, 0.0, -Sx
elif abs(Vy) <= _accuracy:
return 0.0, 1.0, 0.0, -Sy
else:
return 0.0, 0.0, 1.0, -Sz
# Try a generic solution
# z = ax + by + d <=> ax + by -z + d = 0
# assuming c=-1
# it can only fail on ax + by + d = 0
#
# / Sx2 Sxy Sx \ / Sxz \
# | Sxy Sy2 Sy | * X = | Syz |
# \ Sx Sy n / \ Sz /
A = Matrix([[Sx2, Sxy, Sx], [Sxy, Sy2, Sy], [Sx, Sy, n]])
B = Matrix([[Sxz], [Syz], [Sz]])
try:
A.inverse()
X = A * B
return X[0][0], X[1][0], -1.0, X[2][0]
except Exception:
pass
# Try a solution where c=0
# y = ax + d <=> ax -y +d = 0
# .
# / Sx2 Sx \ / Sxy \
# | | * X = | |
# \ Sx n / \ Sy /
A = Matrix([[Sx2, Sx], [Sx, n]])
B = Matrix([[Sxy], [Sy]])
try:
A.inverse()
X = A * B
return X[0][0], -1.0, 0.0, X[1][0]
except Exception:
return None
# -------------------------------------------------------------------------------
# Evaluating n'th degree polynomial is simple loop, starting with highest
# coefficient a[n].
# -------------------------------------------------------------------------------
[docs]def polyeval(a, x):
"""
p(x) = polyeval(a, x)
= a[0] + a[1]x + a[2]x^2 +...+ a[n-1]x^{n-1} + a[n]x^n
= a[0] + x(a[1] + x(a[2] +...+ x(a[n-1] + a[n]x)...)
"""
p = 0
a.reverse()
for coef in a:
p = p * x + coef
a.reverse()
return p
# -------------------------------------------------------------------------------
# Find the first derivative of a polynomial
# -------------------------------------------------------------------------------
[docs]def polyderiv(a):
"""
p'(x) = polyderiv(a)
= b[0] + b[1]x + b[2]x^2 +...+ b[n-2]x^{n-2} + b[n-1]x^{n-1}
where b[i] = (i+1)a[i+1]
"""
b = []
for i in range(1, len(a)):
b.append(i * a[i])
return b
# -------------------------------------------------------------------------------
# Factor out a root from n'th degree polynomial, and return the remaining
# (n-1)'th degree polynomial.
# list = polyreduce(list, number)
# -------------------------------------------------------------------------------
[docs]def polyreduce(a, root):
"""
Given x = r is a root of n'th degree polynomial p(x) = (x-r)q(x),
divide p(x) by linear factor (x-r) using the same algorithm as
polynomial evaluation. Then, return the (n-1)'th degree quotient
q(x) = polyreduce(a, r)
= c[0] + c[1]x + c[2]x^2 +...+ c[n-2]x^{n-2} + c[n-1]x^{n-1}
"""
c, p = [], 0
a.reverse()
for coef in a:
p = p * root + coef
c.append(p)
a.reverse()
c.reverse()
return c[1:]
# -------------------------------------------------------------------------------
# Conversion from integer to Roman
# -------------------------------------------------------------------------------
[docs]def int2roman(num):
"""
Convert an integer to Roman numeral
"""
if not isinstance(num, int):
raise TypeError("expected integer, got %s" % type(input))
if not 0 < num < 4000:
raise ValueError("Argument must be between 1 and 3999")
ints = (1000, 900, 500, 400, 100, 90, 50, 40, 10, 9, 5, 4, 1)
nums = ('M', 'CM', 'D', 'CD', 'C', 'XC', 'L', 'XL', 'X', 'IX', 'V', 'IV', 'I')
result = ""
for i in range(len(ints)):
count = int(num / ints[i])
result += nums[i] * count
num -= ints[i] * count
return result
# -------------------------------------------------------------------------------
# Conversion from Roman to integer
# -------------------------------------------------------------------------------
[docs]def roman2int(roman):
"""
convert a roman string to integer
"""
if not isinstance(roman, str):
raise TypeError("expected string, got %s" % type(roman))
roman = roman.upper()
nums = ('M', 'D', 'C', 'L', 'X', 'V', 'I')
ints = (1000, 500, 100, 50, 10, 5, 1)
places = []
for c in roman:
if c not in nums:
raise ValueError("input is not a valid roman numeral: %s" % roman)
for i in range(len(roman)):
c = roman[i]
value = ints[nums.index(c)]
# If the next place holds a larger number, this value is negative.
try:
nextvalue = ints[nums.index(roman[i + 1])]
if nextvalue > value:
value *= -1
except IndexError:
# there is no next place.
pass
places.append(value)
sum = 0
for n in places:
sum += n
# Easiest test for validity...
if int2roman(sum) == roman:
return sum
else:
raise ValueError('input is not a valid roman numeral: %s' % roman)
# ===============================================================================
if __name__ == "__main__":
from pymchelper.flair.common.log import say
for i in range(4, 50):
num = "-1e-%d" % (i)
say(num, repr(float(num)), format_number(num, 10))
digits = "1234567890123456789012345678"
for i in range(len(digits)):
num = digits[:i]
say("%02d %40s |%10s|" % (i, num, format_number(num, 10)))
say()
for i in range(len(digits)):
num = digits[:i] + ".123"
say("%02d %40s |%10s|" % (i, num, format_number(num, 10)))
say()
for i in range(len(digits)):
num = digits[:i] + ".1234567890"
say("%02d %40s |%10s|" % (i, num, format_number(num, 10)))
say()
for i in range(len(digits)):
num = "-" + digits[:i]
say("%02d %40s |%10s|" % (i, num, format_number(num, 10)))
say()
for i in range(len(digits)):
num = "-" + digits[:i] + ".123"
say("%02d %40s |%10s|" % (i, num, format_number(num, 10)))
say()
for i in range(len(digits)):
num = "-" + digits[:i] + ".1234567890"
say("%02d %40s |%10s|" % (i, num, format_number(num, 10)))
say(Vector.O)
a = 1.2345678901234567890
w = -1e-05
say(format_number(w, 22))
say(format_number(a, 22))
say(Vector.Y.direction(0.0000001))
a = Vector(range(5))
a.norm()
say("a=", a, "len(a)=", a.length())
b = Vector(range(5))
say("b=", b, "length(b)=", b.length(), "len(b)=", len(b))
say("matO\n", Matrix.O)
say("matU\n", Matrix.U)
M = Matrix(4)
a = Vector(0.1, 0.2, 0.3)
M.rotate(math.radians(45.0), a)
say("M\n", M)
say("a=", a)
say("M*a=", M * a)
say("M*X=", M * Vector.X)
say(d2s(dms(1, 2, 3.006)))
M = Matrix([[1.0, -2.0, -3.0, 1.0], [-2.0, 1.0, -2.0, -1.0], [-3.0, -2.0, 2.0, 2.0], [1.0, -1.0, 2.0, 3.0]])
say("M=\n", M)
say("det(M)=", M.det())
v, u = eigenvalues(M)
say("Eigenvalues=", v)
say("Eigenvectors=\n", u)
say("uT * diag(v) * u=\n", u.T() * Matrix.diagonal(v) * u)
say()
say("Permutations of 'love'")
for p in xpermutations(['l', 'o', 'v', 'e']):
say(''.join(p))
say()
say("Combinations of 2 letters from 'love'")
for c in xcombinations(['l', 'o', 'v', 'e'], 2):
say(''.join(c))
say()
say("Unique Combinations of 2 letters from 'love'")
for uc in xuniqueCombinations(['l', 'o', 'v', 'e'], 2):
say(''.join(uc))
say()
say("Selections of 2 letters from 'love'")
for s in xselections(['l', 'o', 'v', 'e'], 2):
say(''.join(s))
say()
say(map(''.join, list(xpermutations('done'))))
say()
say("cubic(x^3-3x^2-10x+24=0)=", cubic(-3.0, -10.0, 24.0))
say()
for i in range(1, 51):
say(i, int2roman(i))