332 lines
11 KiB
Cython
332 lines
11 KiB
Cython
cdef class mat4:
|
|
"""
|
|
A float 4x4 matrix.
|
|
|
|
All arrays are column-major, i.e. OpenGL style:
|
|
|
|
0 4 8 12
|
|
1 5 9 13
|
|
2 6 10 14
|
|
3 7 11 15
|
|
|
|
The matrix implements stacking useful for graphics.
|
|
"""
|
|
|
|
def __cinit__(mat4 self):
|
|
to_alloc = 16 * sizeof(float)
|
|
self.stack = <float *>malloc(to_alloc)
|
|
|
|
if not self.stack:
|
|
raise MemoryError("Unable to malloc %d B for mat4." %(to_alloc))
|
|
|
|
self.m = self.stack
|
|
self.size = 1
|
|
|
|
def _debug(mat4 self):
|
|
print("--- self.stack = %d" %(<uint64_t>self.stack))
|
|
print("--- self.m = %d (+%d)" %(<uint64_t>self.m, self.m - self.stack))
|
|
print("--- self.size = %d" %(self.size))
|
|
|
|
def __init__(mat4 self, *args):
|
|
"""
|
|
Create a ma4t.
|
|
|
|
Accepts any number of parameters between 0 and 16 to fill the
|
|
matrix from the upper left corner going down (column-wise).
|
|
"""
|
|
|
|
length = len(args)
|
|
|
|
if length == 1 and isinstance(args[0], (list, tuple)):
|
|
args = args[0]
|
|
length = len(args)
|
|
|
|
if length > 16:
|
|
raise MathError("Attempt to initialize a mat4 with %d arguments." %(length))
|
|
|
|
self.load_from(args)
|
|
|
|
def __dealloc__(mat4 self):
|
|
free(self.stack)
|
|
|
|
def __getstate__(mat4 self):
|
|
state = []
|
|
|
|
for i in range(self.m - self.stack + 16):
|
|
state.append(self.stack[i])
|
|
|
|
return state
|
|
|
|
def __setstate__(mat4 self, state):
|
|
length = len(state)
|
|
matrices = length//16
|
|
|
|
if not matrices*16 == length:
|
|
raise GeneralError("mat4 __setstate__ got %d floats as a state" %(length))
|
|
|
|
self.m = self.stack
|
|
|
|
slot_full = False
|
|
for start in range(0, length, 16):
|
|
if slot_full:
|
|
self.push()
|
|
slot_full = False
|
|
|
|
self.load_from(state[start:start+16])
|
|
slot_full = True
|
|
|
|
def __getitem__(mat4 self, int i):
|
|
if i > 16 or i < 0:
|
|
raise IndexError("element index out of range(16)")
|
|
|
|
return self.m[i]
|
|
|
|
def __setitem__(self, int i, value):
|
|
if i > 16 or i < 0:
|
|
raise IndexError("element index out of range(16)")
|
|
|
|
self.m[i] = value
|
|
|
|
def push(mat4 self):
|
|
"""
|
|
Push the current matrix into the stack and load up an empty one (a zero matrix)
|
|
"""
|
|
|
|
# self.m points to the current matrix
|
|
# self.stack points to the first matrix
|
|
# self.size how many matrices are allocated
|
|
|
|
# ensure there's room for one more
|
|
cdef unsigned int used = 1 + (self.m - self.stack) / 16
|
|
cdef unsigned int empty = self.size - used
|
|
cdef float *tmp
|
|
|
|
if not empty:
|
|
self.size += 1
|
|
to_alloc = self.size * 16 * sizeof(float)
|
|
tmp = <float *>realloc(self.stack, to_alloc)
|
|
|
|
if tmp:
|
|
self.stack = tmp
|
|
else:
|
|
raise MemoryError("Unable to malloc %d B for mat4." %(to_alloc))
|
|
|
|
# advance the pointer to the new one
|
|
self.m = self.stack + 16 * used
|
|
|
|
# at this point there's at least enough space for one matrix
|
|
# copy the old matrix into the new one
|
|
cdef uint8_t i
|
|
cdef float *old_m = self.m - 16
|
|
for i in range(16):
|
|
self.m[i] = old_m[i]
|
|
|
|
def pop(mat4 self):
|
|
"""
|
|
Pop a matrix from the stack.
|
|
"""
|
|
|
|
if self.m == self.stack:
|
|
raise IndexError("pop from an empty stack")
|
|
|
|
self.m -= 16
|
|
|
|
def get_list(mat4 self):
|
|
L = []
|
|
|
|
for i in range(16):
|
|
L.append(self.m[i])
|
|
|
|
return L
|
|
|
|
def load_from(mat4 self, L):
|
|
"""
|
|
Fill the current matrix from a either a list of values, column-major,
|
|
or another matrix. This method doesn't modify the stack, only the
|
|
current matrix is read and modified.
|
|
|
|
If the number of values isn't 16, it will be padded to 16 by zeros.
|
|
If it's larger, GeneralError will be raised.
|
|
"""
|
|
|
|
if isinstance(L, mat4):
|
|
L = L.get_list()
|
|
length = 16
|
|
else:
|
|
length = len(L)
|
|
|
|
if length > 16:
|
|
raise GeneralError("supplied list is longer than 16")
|
|
|
|
for i in range(16):
|
|
if i < length:
|
|
self.m[i] = L[i]
|
|
else:
|
|
self.m[i] = 0.0
|
|
|
|
def zero(mat4 self):
|
|
"""Fill the matrix with zeroes."""
|
|
|
|
for i in range(16):
|
|
self.m[i] = 0.0
|
|
|
|
def identity(mat4 self):
|
|
"""Make the matrix an identity."""
|
|
|
|
self.zero()
|
|
|
|
self.m[0] = 1.0
|
|
self.m[5] = 1.0
|
|
self.m[10] = 1.0
|
|
self.m[15] = 1.0
|
|
|
|
def transpose(mat4 self):
|
|
"""Transpose the matrix."""
|
|
|
|
cdef float tmp
|
|
|
|
tmp = self.m[1]
|
|
self.m[1] = self.m[4]
|
|
self.m[4] = tmp
|
|
|
|
tmp = self.m[2]
|
|
self.m[2] = self.m[8]
|
|
self.m[8] = tmp
|
|
|
|
tmp = self.m[3]
|
|
self.m[3] = self.m[12]
|
|
self.m[12] = tmp
|
|
|
|
tmp = self.m[7]
|
|
self.m[7] = self.m[13]
|
|
self.m[13] = tmp
|
|
|
|
tmp = self.m[11]
|
|
self.m[11] = self.m[14]
|
|
self.m[14] = tmp
|
|
|
|
tmp = self.m[6]
|
|
self.m[6] = self.m[9]
|
|
self.m[9] = tmp
|
|
|
|
def invert(mat4 self):
|
|
"""Invert the matrix."""
|
|
|
|
cdef float tmp[16]
|
|
cdef float det
|
|
|
|
tmp[0] = self.m[5]*self.m[10]*self.m[15] - self.m[5]*self.m[11]*self.m[14] - self.m[9]*self.m[6]*self.m[15] + self.m[9]*self.m[7]*self.m[14] + self.m[13]*self.m[6]*self.m[11] - self.m[13]*self.m[7]*self.m[10]
|
|
tmp[4] = -self.m[4]*self.m[10]*self.m[15] + self.m[4]*self.m[11]*self.m[14] + self.m[8]*self.m[6]*self.m[15] - self.m[8]*self.m[7]*self.m[14] - self.m[12]*self.m[6]*self.m[11] + self.m[12]*self.m[7]*self.m[10]
|
|
tmp[8] = self.m[4]*self.m[9]*self.m[15] - self.m[4]*self.m[11]*self.m[13] - self.m[8]*self.m[5]*self.m[15] + self.m[8]*self.m[7]*self.m[13] + self.m[12]*self.m[5]*self.m[11] - self.m[12]*self.m[7]*self.m[9]
|
|
tmp[12] = -self.m[4]*self.m[9]*self.m[14] + self.m[4]*self.m[10]*self.m[13] + self.m[8]*self.m[5]*self.m[14] - self.m[8]*self.m[6]*self.m[13] - self.m[12]*self.m[5]*self.m[10] + self.m[12]*self.m[6]*self.m[9]
|
|
|
|
det = self.m[0]*tmp[0] + self.m[1]*tmp[4] + self.m[2]*tmp[8] + self.m[3]*tmp[12]
|
|
|
|
# epsilon pulled straight out of Uranus
|
|
if det < 0.00005 and det > -0.00005:
|
|
print("det=%.1f" %(det))
|
|
return
|
|
|
|
tmp[1] = -self.m[1]*self.m[10]*self.m[15] + self.m[1]*self.m[11]*self.m[14] + self.m[9]*self.m[2]*self.m[15] - self.m[9]*self.m[3]*self.m[14] - self.m[13]*self.m[2]*self.m[11] + self.m[13]*self.m[3]*self.m[10]
|
|
tmp[5] = self.m[0]*self.m[10]*self.m[15] - self.m[0]*self.m[11]*self.m[14] - self.m[8]*self.m[2]*self.m[15] + self.m[8]*self.m[3]*self.m[14] + self.m[12]*self.m[2]*self.m[11] - self.m[12]*self.m[3]*self.m[10]
|
|
tmp[9] = -self.m[0]*self.m[9]*self.m[15] + self.m[0]*self.m[11]*self.m[13] + self.m[8]*self.m[1]*self.m[15] - self.m[8]*self.m[3]*self.m[13] - self.m[12]*self.m[1]*self.m[11] + self.m[12]*self.m[3]*self.m[9]
|
|
tmp[13] = self.m[0]*self.m[9]*self.m[14] - self.m[0]*self.m[10]*self.m[13] - self.m[8]*self.m[1]*self.m[14] + self.m[8]*self.m[2]*self.m[13] + self.m[12]*self.m[1]*self.m[10] - self.m[12]*self.m[2]*self.m[9]
|
|
tmp[2] = self.m[1]*self.m[6]*self.m[15] - self.m[1]*self.m[7]*self.m[14] - self.m[5]*self.m[2]*self.m[15] + self.m[5]*self.m[3]*self.m[14] + self.m[13]*self.m[2]*self.m[7] - self.m[13]*self.m[3]*self.m[6]
|
|
tmp[6] = -self.m[0]*self.m[6]*self.m[15] + self.m[0]*self.m[7]*self.m[14] + self.m[4]*self.m[2]*self.m[15] - self.m[4]*self.m[3]*self.m[14] - self.m[12]*self.m[2]*self.m[7] + self.m[12]*self.m[3]*self.m[6]
|
|
tmp[10] = self.m[0]*self.m[5]*self.m[15] - self.m[0]*self.m[7]*self.m[13] - self.m[4]*self.m[1]*self.m[15] + self.m[4]*self.m[3]*self.m[13] + self.m[12]*self.m[1]*self.m[7] - self.m[12]*self.m[3]*self.m[5]
|
|
tmp[14] = -self.m[0]*self.m[5]*self.m[14] + self.m[0]*self.m[6]*self.m[13] + self.m[4]*self.m[1]*self.m[14] - self.m[4]*self.m[2]*self.m[13] - self.m[12]*self.m[1]*self.m[6] + self.m[12]*self.m[2]*self.m[5]
|
|
tmp[3] = -self.m[1]*self.m[6]*self.m[11] + self.m[1]*self.m[7]*self.m[10] + self.m[5]*self.m[2]*self.m[11] - self.m[5]*self.m[3]*self.m[10] - self.m[9]*self.m[2]*self.m[7] + self.m[9]*self.m[3]*self.m[6]
|
|
tmp[7] = self.m[0]*self.m[6]*self.m[11] - self.m[0]*self.m[7]*self.m[10] - self.m[4]*self.m[2]*self.m[11] + self.m[4]*self.m[3]*self.m[10] + self.m[8]*self.m[2]*self.m[7] - self.m[8]*self.m[3]*self.m[6]
|
|
tmp[11] = -self.m[0]*self.m[5]*self.m[11] + self.m[0]*self.m[7]*self.m[9] + self.m[4]*self.m[1]*self.m[11] - self.m[4]*self.m[3]*self.m[9] - self.m[8]*self.m[1]*self.m[7] + self.m[8]*self.m[3]*self.m[5]
|
|
tmp[15] = self.m[0]*self.m[5]*self.m[10] - self.m[0]*self.m[6]*self.m[9] - self.m[4]*self.m[1]*self.m[10] + self.m[4]*self.m[2]*self.m[9] + self.m[8]*self.m[1]*self.m[6] - self.m[8]*self.m[2]*self.m[5]
|
|
|
|
det = 1.0 / det
|
|
self.m[0] = tmp[0] * det
|
|
self.m[1] = tmp[1] * det
|
|
self.m[2] = tmp[2] * det
|
|
self.m[3] = tmp[3] * det
|
|
self.m[4] = tmp[4] * det
|
|
self.m[5] = tmp[5] * det
|
|
self.m[6] = tmp[6] * det
|
|
self.m[7] = tmp[7] * det
|
|
self.m[8] = tmp[8] * det
|
|
self.m[9] = tmp[9] * det
|
|
self.m[10] = tmp[10] * det
|
|
self.m[11] = tmp[11] * det
|
|
self.m[12] = tmp[12] * det
|
|
self.m[13] = tmp[13] * det
|
|
self.m[14] = tmp[14] * det
|
|
self.m[15] = tmp[15] * det
|
|
|
|
def mulm(mat4 self, mat4 B, bint inplace=False):
|
|
"""
|
|
Return a matrix that is the result of multiplying this matrix by another.
|
|
|
|
M = self * mat4 B
|
|
"""
|
|
|
|
cdef uint8_t i
|
|
cdef mat4 tmp = mat4()
|
|
|
|
tmp.m[0] = self.m[0] * B.m[0] + self.m[4] * B.m[1] + self.m[8] * B.m[2] + self.m[12] * B.m[3]
|
|
tmp.m[1] = self.m[1] * B.m[0] + self.m[5] * B.m[1] + self.m[9] * B.m[2] + self.m[13] * B.m[3]
|
|
tmp.m[2] = self.m[2] * B.m[0] + self.m[6] * B.m[1] + self.m[10] * B.m[2] + self.m[14] * B.m[3]
|
|
tmp.m[3] = self.m[3] * B.m[0] + self.m[7] * B.m[1] + self.m[11] * B.m[2] + self.m[15] * B.m[3]
|
|
tmp.m[4] = self.m[0] * B.m[4] + self.m[4] * B.m[5] + self.m[8] * B.m[6] + self.m[12] * B.m[7]
|
|
tmp.m[5] = self.m[1] * B.m[4] + self.m[5] * B.m[5] + self.m[9] * B.m[6] + self.m[13] * B.m[7]
|
|
tmp.m[6] = self.m[2] * B.m[4] + self.m[6] * B.m[5] + self.m[10] * B.m[6] + self.m[14] * B.m[7]
|
|
tmp.m[7] = self.m[3] * B.m[4] + self.m[7] * B.m[5] + self.m[11] * B.m[6] + self.m[15] * B.m[7]
|
|
tmp.m[8] = self.m[0] * B.m[8] + self.m[4] * B.m[9] + self.m[8] * B.m[10] + self.m[12] * B.m[11]
|
|
tmp.m[9] = self.m[1] * B.m[8] + self.m[5] * B.m[9] + self.m[9] * B.m[10] + self.m[13] * B.m[11]
|
|
tmp.m[10] = self.m[2] * B.m[8] + self.m[6] * B.m[9] + self.m[10] * B.m[10] + self.m[14] * B.m[11]
|
|
tmp.m[11] = self.m[3] * B.m[8] + self.m[7] * B.m[9] + self.m[11] * B.m[10] + self.m[15] * B.m[11]
|
|
tmp.m[12] = self.m[0] * B.m[12] + self.m[4] * B.m[13] + self.m[8] * B.m[14] + self.m[12] * B.m[15]
|
|
tmp.m[13] = self.m[1] * B.m[12] + self.m[5] * B.m[13] + self.m[9] * B.m[14] + self.m[13] * B.m[15]
|
|
tmp.m[14] = self.m[2] * B.m[12] + self.m[6] * B.m[13] + self.m[10] * B.m[14] + self.m[14] * B.m[15]
|
|
tmp.m[15] = self.m[3] * B.m[12] + self.m[7] * B.m[13] + self.m[11] * B.m[14] + self.m[15] * B.m[15]
|
|
|
|
if inplace:
|
|
for i in range(16):
|
|
self.m[i] = tmp.m[i]
|
|
else:
|
|
return tmp
|
|
|
|
def mulv(mat4 self, vec3 v):
|
|
"""
|
|
Return a vec3 that is the result of multiplying this matrix by a vec3.
|
|
|
|
u = self * v
|
|
"""
|
|
|
|
cdef mat4 tmp = vec3()
|
|
|
|
tmp.v[0] = v.v[0]*self.m[0] + v.v[1]*self.m[4] + v.v[2]*self.m[8] + self.m[12]
|
|
tmp.v[1] = v.v[0]*self.m[1] + v.v[1]*self.m[5] + v.v[2]*self.m[9] + self.m[13]
|
|
tmp.v[2] = v.v[0]*self.m[2] + v.v[1]*self.m[6] + v.v[2]*self.m[10] + self.m[14]
|
|
|
|
return tmp
|
|
|
|
def mulf(mat4 self, f):
|
|
"""
|
|
Return a matrix that is the result of multiplying this matrix by a scalar.
|
|
|
|
M = self * f
|
|
"""
|
|
|
|
cdef mat4 tmp = mat4()
|
|
cdef int i
|
|
|
|
for i in range(16):
|
|
tmp.m[i] = self.m[i] * f
|
|
|
|
return tmp
|
|
|
|
def __repr__(mat4 self):
|
|
lines = []
|
|
|
|
lines.append("mat4(%.1f %.1f %.1f %.1f" %(self.m[0], self.m[4], self.m[8], self.m[12]))
|
|
lines.append(" %.1f %.1f %.1f %.1f" %(self.m[1], self.m[5], self.m[9], self.m[13]))
|
|
lines.append(" %.1f %.1f %.1f %.1f" %(self.m[2], self.m[6], self.m[10], self.m[14]))
|
|
lines.append(" %.1f %.1f %.1f %.1f)" %(self.m[3], self.m[7], self.m[11], self.m[15]))
|
|
|
|
return "\n".join(lines)
|