## generalized matrix transformation operations for 3D homogeneous
## coordinates in yapCAD
## Copyright (c) 2020 Richard W. DeVaul
## Copyright (c) 2020 yapCAD contributors
## All rights reserved
# Permission is hereby granted, free of charge, to any person
# obtaining a copy of this software and associated documentation files
# (the "Software"), to deal in the Software without restriction,
# including without limitation the rights to use, copy, modify, merge,
# publish, distribute, sublicense, and/or sell copies of the Software,
# and to permit persons to whom the Software is furnished to do so,
# subject to the following conditions:
#
# The above copyright notice and this permission notice shall be
# included in all copies or substantial portions of the Software.
#
# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
# EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
# MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
# NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
# BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
# ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
# CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
# SOFTWARE.
from math import *
import yapcad.geom as geom
## a matrix is represented as a list of four four vectors. In a
## matrix, vectors represent rows unless the transpose property is
## true. Because vectors are represented as lists (not as instances
## of a class with meta-info) we assume that operations like Mx imply
## a column vector and that xM imply a row vector.
## There are two classes defined here: Matrix and MatrixStack. Matrix
## is a relatively lightweight class that provides foundation
## matrix-matrix and matrix-vector operations. MatrixStack is a class
## that captures a series of transformations and their inverses. Note
## that inverses are generally determined analytically through
## composition, though we don't dismiss the possibility of a numeric
## inversion.
## FIXME: only one class is defined for now
[docs]
class Matrix:
"""4x4 transformation matrix class for transforming homogemenous 3D coordinates"""
def __init__(self,a=False,trans=False):
self.m = [[1,0,0,0],
[0,1,0,0],
[0,0,1,0],
[0,0,0,1]]
self.trans=False
if isinstance(a,Matrix):
for i in range(4):
self.setrow(i,a.getrow(i))
elif isinstance(a,(tuple,list)):
if len(a) == 4:
r1 =a[0]
r2 =a[1]
r3 =a[2]
r4 =a[3]
if len(r1) == len(r2) == len(r3) == len(r4) == 4:
for i in range(4):
for j in range(4):
x =a[i][j]
if (not isinstance(x,bool)) and \
isinstance(x,(int,float)):
self.m[i][j]=x
else:
raise ValueError('bad element in matrix initialization: {}'.format(x))
elif len(a)==16:
for i in range(4):
for j in range(4):
ind=i*4+j
x = a[ind]
if (not isinstance(x,bool)) and \
isinstance(x,(int,float)):
self.m[i][j]=x
else:
raise ValueError('bad element in matrix initialization: {}'.format(x))
elif a == False:
pass
else:
raise ValueError('bad thing used in attempt to initialize matrix: ()'.format(a))
self.trans=trans
def __repr__(self):
return "Matrix({},{},{},{},{})".format(self.m[0],self.m[1],
self.m[2],self.m[3],self.trans)
#return value indexed by i,j
[docs]
def get(self,i,j):
if i < 0 or i > 3 or j < 0 or j > 3:
raise ValueError('bad index passed to get: {},{}'.format(i,j))
if self.trans:
return self.m[j][i]
else:
return self.m[i][j]
#set value indexed by i,j
[docs]
def set(self,i,j,x):
if i < 0 or i > 3 or j < 0 or j > 3:
raise ValueError('bad index passed to set: {},{}'.format(i,j))
if geom.isgoodnum(x):
if self.trans:
self.m[j][i]=x
else:
self.m[i][j]=x
else:
raise ValueError('bad value passed to set: {}'.format(x))
[docs]
def getrow(self,i):
if i < 0 or i > 3:
raise ValueError('bad row passed to getrow: {}'.format(i))
if self.trans:
return [self.m[0][i],
self.m[1][i],
self.m[2][i],
self.m[3][i]]
else:
return self.m[i]
[docs]
def getcol(self,j):
if j < 0 or j > 3:
raise ValueError('bad column passed to getcol: {}'.format(j))
if not self.trans:
return [self.m[0][j],
self.m[1][j],
self.m[2][j],
self.m[3][j]]
else:
return self.m[j]
[docs]
def setrow(self,i,x):
if not geom.isvect(x):
raise ValueError('bad non-vector passed to setrow: {}'.format(x))
if i < 0 or i > 3:
raise ValueError('bad row index passed to setrow: {}'.format(i))
if self.trans:
self.m[0][i] = x[0]
self.m[1][i] = x[1]
self.m[2][i] = x[2]
self.m[3][i] = x[3]
else:
self.m[i] = x
[docs]
def setcol(self,j,x):
if not geom.isvect(x):
raise ValueError('bad non-vector passed to setcol: {}'.format(x))
if j < 0 or j > 3:
raise ValueError('bad column index passed to setcol: {}'.format(j))
if not self.trans:
self.m[0][j] = x[0]
self.m[1][j] = x[1]
self.m[2][j] = x[2]
self.m[3][j] = x[3]
else:
self.m[j] = x
# matrix multiply. If x is a matrix, compute MX. If x is a
# vector, compute Mx. If x is a scalar, compute xM. If x isn't any
# of these, a ValueError is raised. Respects transpose flag.
[docs]
def mul(self,x):
"""Return the product of this matrix and ``x``.
``x`` may be another :class:`Matrix`, a 4D vector, or a scalar. In
each case the appropriate matrix multiplication is performed. If
``x`` is none of these types, a :class:`ValueError` is raised.
"""
if isinstance(x,Matrix):
result = Matrix()
for i in range(4):
for j in range(4):
result.set(i,j,
geom.dot4(self.getrow(i),x.getcol(j)))
return result
elif geom.isvect(x):
result = geom.vect()
for i in range(4):
result[i]=geom.dot4(self.getrow(i),x)
return result
elif geom.isgoodnum(x):
result = Matrix()
for i in range(4):
result.setrow(i,geom.scale4(self.getrow(i),x))
return result
raise ValueError('bad thing passed to mul(): {}'.format(x))
# return the generalized 4x4 arbitrary axis rotation matrix
[docs]
def Rotation(axis,angle,inverse=False):
m = geom.mag(axis)
u = axis
if m < geom.epsilon:
raise ValueError('zero-length rotation axis not allowed')
if not geom.close(m,1.0):
u = geom.geom.scale3(axis,1.0/m)
if inverse:
angle *= -1.0
rad = (angle%360.0)*geom.pi2/360.0
ux = u[0]
uy = u[1]
uz = u[2]
cang = cos(rad)
cmin = 1.0-cang
sang = sin(rad)
smin = 1.0-sang
# # see https://en.wikipedia.org/wiki/Rotation_matrix
## THIS TURNS OUT TO BE WRONG! Bad wikipedia! no biscuit
# R = [[cang + ux*ux*cmin, ux*uy*cmin-uz*sang, ux*uz*cmin+uy*sang,0],
# [uy*ux*cmin+uz*smin, cang + uy*uy*cmin, uy*uz*cmin - uz*smin,0],
# [uz*ux*cmin-uy*smin, uz*uy*cmin+ux*smin, cang+uz*uz*cmin,0],
# [0,0,0,1]]
# see http://www.opengl-tutorial.org/assets/faq_quaternions/index.html#Q38
## This is correct
R = [[cang + ux*ux*cmin, ux*uy*cmin-uz*sang, ux*uz*cmin+uy*sang,0],
[uy*ux*cmin+uz*sang, cang + uy*uy*cmin, uy*uz*cmin - ux*sang,0],
[uz*ux*cmin-uy*sang, uz*uy*cmin+ux*sang, cang+uz*uz*cmin,0],
[0,0,0,1]]
return Matrix(R)
[docs]
def Translation(delta,inverse=False):
if inverse:
delta = geom.scale3(delta, -1.0)
dx = delta[0]
dy = delta[1]
dz = delta[2]
T = [[1,0,0,dx],
[0,1,0,dy],
[0,0,1,dz],
[0,0,0,1]]
return Matrix(T)
[docs]
def Scale(x,y=False,z=False,inverse=False):
sx = sy = sz = 1.0
if geom.isgoodnum(x):
sx = x
if geom.isgoodnum(y) and geom.isgoodnum(z):
sy = y
sz = z
else:
sy = sz = x
elif geom.isvect(x):
sx = x[0]
sy = x[1]
sz = x[2]
else:
raise ValueError('bad scaling values passed to Scale')
if inverse:
sx = 1.0/sx
sy = 1.0/sy
sz = 1.0/sz
S = [[sx,0,0,0],
[0,sy,0,0],
[0,0,sz,0],
[0,0,0,1.0]]
return Matrix(S)