459 lines
14 KiB
Python
459 lines
14 KiB
Python
#! /bin/env python3
|
|
# encoding: utf-8
|
|
|
|
'''
|
|
Vector And Matrix Math
|
|
|
|
Pure Python implementation.
|
|
'''
|
|
|
|
from math import sin, cos, sqrt
|
|
|
|
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).
|
|
'''
|
|
|
|
self.m =
|
|
|
|
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)
|
|
|
|
cdef class vec3:
|
|
'''
|
|
A float 3D vector.
|
|
|
|
>>> v = vec3(1, 1, 0)
|
|
>>> w = vec3(0, 1, 1)
|
|
>>> v.length
|
|
1.4142135623730951
|
|
>>> v.dot(w)
|
|
1.0
|
|
>>> v.cross(w)
|
|
vec4(1.00, 1.00, 1.00)
|
|
>>> v + w
|
|
vec4(1.00, 2.00, 1.00)
|
|
>>> w - v
|
|
vec4(-1.00, 0.00, 1.00)
|
|
|
|
'''
|
|
|
|
def __init__(vec3 self, *args):
|
|
'''
|
|
Create a vec3.
|
|
|
|
Accepts any number of parameters between 0 and 3 to fill the vector from the left.
|
|
'''
|
|
|
|
length = len(args)
|
|
|
|
if length == 1 and isinstance(args[0], (list, tuple)):
|
|
args = args[0]
|
|
length = len(args)
|
|
|
|
if length > 3:
|
|
raise MathError('Attempt to initialize a vec3 with %d arguments.' %(length))
|
|
|
|
for i in range(3):
|
|
if i < length:
|
|
self.v[i] = args[i]
|
|
else:
|
|
self.v[i] = 0.0
|
|
|
|
def __getitem__(vec3 self, int i):
|
|
if i >= 3 or i < 0:
|
|
raise IndexError('element index out of range(3)')
|
|
|
|
return self.v[i]
|
|
|
|
def __setitem__(vec3 self, int i, float value):
|
|
if i >= 3 or i < 0:
|
|
raise IndexError('element index out of range(3)')
|
|
|
|
self.v[i] = value
|
|
|
|
def __repr__(vec3 self):
|
|
return 'vec3(%.2f, %.2f, %.2f)' %(self.v[0], self.v[1], self.v[2])
|
|
|
|
def __getstate__(vec3 self):
|
|
return (self.v[0], self.v[1], self.v[2])
|
|
|
|
def __setstate__(vec3 self, state):
|
|
self.v[0] = state[0]
|
|
self.v[1] = state[1]
|
|
self.v[2] = state[2]
|
|
|
|
@property
|
|
def length(vec3 self):
|
|
'''Contains the geometric length of the vector.'''
|
|
|
|
return sqrt(self.v[0]**2 + self.v[1]**2 + self.v[2]**2)
|
|
|
|
def normalized(vec3 self):
|
|
'''Returns this vector, normalized.'''
|
|
|
|
length = self.length
|
|
|
|
return vec3(self.v[0] / length, self.v[1] / length, self.v[2] / length)
|
|
|
|
def __add__(vec3 L, vec3 R):
|
|
return vec3(L.v[0] + R.v[0], L.v[1] + R.v[1], L.v[2] + R.v[2])
|
|
|
|
def __sub__(vec3 L, vec3 R):
|
|
return vec3(L.v[0] - R.v[0], L.v[1] - R.v[1], L.v[2] - R.v[2])
|
|
|
|
def __neg__(vec3 self):
|
|
return vec3(-self.v[0], -self.v[1], -self.v[2])
|
|
|
|
def dot(vec3 L, vec3 R):
|
|
'''
|
|
Returns the dot product of the two vectors.
|
|
|
|
E.g. u.dot(v) -> u . v
|
|
'''
|
|
|
|
return L.v[0] * R.v[0] + L.v[1] * R.v[1] + L.v[2] * R.v[2]
|
|
|
|
def cross(vec3 L, vec3 R):
|
|
'''
|
|
Returns the cross product of the two vectors.
|
|
|
|
E.g. u.cross(v) -> u x v
|
|
|
|
'''
|
|
|
|
return vec3(L.v[1]*R.v[2] - L.v[2]*R.v[1], L.v[0]*R.v[2] - L.v[2]*R.v[0], L.v[0]*R.v[1] - L.v[1]*R.v[0])
|
|
|
|
def __mul__(vec3 L, R):
|
|
'''
|
|
Multiplication of a vec3 by a float.
|
|
|
|
The float has to be on the right.
|
|
'''
|
|
|
|
return vec3(L.v[0] * R, L.v[1] * R, L.v[2] * R)
|