Generic code for multiplying / dividing square matrices. To divide you must first calculate the inverse matrix of the divisor then use it to multiply. The code given here can be configured for single or double precision.
This can be found in the examples/math folder
Multiplication:
'Example of matrix layout mapped to an array of 4x4 cells
'
' 0 4 8 C
' 1 5 9 D
' 2 6 A E
' 3 7 B F
'
'----------------------------------------
sub MatrixMul(MatrixType *a,*b,*c, sys n)
'========================================
'
'
#if leftmatch matrixtype single
% OneStep 4
% ColStep 16
% mtype single
#endif
'
#if leftmatch matrixtype double
% OneStep 8
% ColStep 32
% mtype double
#endif
sys pa=@a, pb=@b, pc=@c
mov ecx,pa
mov edx,pb
mov eax,pc
mov esi,n
(
call column : dec esi : jg repeat
)
exit sub
column:
'======
mov edi,n
(
call cell : dec edi : jg repeat
)
add edx,ColStep
sub ecx,ColStep
ret
cell: ' row A * column B
'=======================
'matrix data is stored ascending vertically then horizontally
'thus rows are minor, columns are major
'
push ecx
push edx
push eax
mov eax,4
fldz
(
fld mtype [ecx]
fmul mtype [edx]
faddp st1
add ecx,ColStep 'next column of matrix A
add edx,OneStep 'next row of matrix B
dec eax
jnz repeat
)
pop eax
fstp mtype [eax] 'assign to next row of matrix C
'
pop edx
pop ecx
add eax,OneStep 'next cell in column of matrix C (columns then rows)
add ecx,OneStep 'next row of matrix A
ret
'
end sub
'-----------------------------------------------
sub getrow4S(MatrixType* matrix, MarixType *row)
'===============================================
'
indexbase 0
row<=matrix[0],matrix[4],matrix[8],matrix[12]
end sub
Inverse Matrix
(I 'borrowed' this algorithm from Dan Baron / thinBasic forum / PB examples)
'----------------------------------
sub MatrixInv(MatrixType *m, sys n)
'==================================
'Uses the Gauss-Jordan Method.
'Inverts matrix, m[], "in-place".
'limit to max 16x16 matrix
sys pivot,row,col,maxrow
MatrixType test,colmax,divisor,factor,temp
sys rowswitch,switchcount
sys i,j
sys rs[128] 'for limit array n=64
'
'TO RESOLVE MATRIX INDEX
'=======================
'
#indexbase 1
#define mat(row,col) m[ (col-1)*n+row ]
#define rowswitches(row,col) rs[ (col-1)*64+row ]
#define first 1
#define last n
'
'TRAVERSE COLUMNS
'================
for pivot=first to last
'
'
'GET THE MAXIMUM ELEMENT ON EACH PIVOT COL
'=========================================
'
colmax=0
maxrow=pivot
'
for row=pivot to last
test=abs (mat(row,pivot))
if test>colmax then
colmax=test
maxrow=row
end if
next
if maxrow<>pivot then
'
'
'SWAP ROWS TO GET THE MAX VALUE AT THE PIVOT
'===========================================
'
for col=first to last
temp=mat(pivot,col)
mat(pivot,col)=mat(maxrow,col)
mat(maxrow,col)=temp
next
'
'
'REMEMBER THE ROW SWAP
'=====================
'
switchcount++
rowswitches(switchcount,1)=pivot
rowswitches(switchcount,2)=maxrow
end if
'
'
'DIVIDE THE PIVOT ROW BY THE PIVOT DIAGONAL
'==========================================
'
'also setting the pivot diagonal to 1
'
divisor=mat(pivot,pivot)
mat(pivot,pivot)=1
'
for col=first to last
mat(pivot,col)=mat(pivot,col) / divisor
next
'APPLY PIVOT FACTORS TO ALL ELEMENTS (OTHER THAN PIVOT ROW)
'==========================================================
'
'also setting each pivot row element to 0
'
for row=first to last
if row<>pivot then
factor=-mat(row,pivot)
mat(row,pivot)=0
for col=first to last
mat(row,col)=mat(row,col)+mat(pivot,col) * factor
next
end if
next
next 'pivot
'
'SWAP COLUMN VALUES ACCORDING TO ORIGINAL ROW POSITIONS
'======================================================
'
for rowswitch=switchcount to 1 step-1
i=rowswitches(rowswitch,1)
j=rowswitches(rowswitch,2)
for row=first to last
temp = mat(row,i)
mat(row,i)=mat(row,j)
mat(row,j)=temp
next
next
'
'
end sub
Test
'Example of Matrix layout in an array
'
' 0 4 8 C
' 1 5 9 D
' 2 6 A E
' 3 7 B F
'
#define MatrixType double
include "MatrixMul.inc"
include "MatrixInv.inc"
'-----------------------------------------------
function showMat(MatrixType *r, sys n) as string
'===============================================
string tab.cr.pr
tab=chr 9
cr=chr(13) chr(10)
pr="Matrix:" n "x" n cr
sys i,j,m
'
'DISPLAY IN Y MINOR : X MAJOR ORDER (COLUMN MAJOR)
'-------------------------------------------------
'
for i=1 to n
m=0
for j=1 to n
pr+=str( r[m+i] ) tab
m+=n
next
pr+=cr
next
return pr cr cr
'
end function
MatrixType m[16],n[16],r[16]
'set identity matrix values
' 1 0 0 0
' 0 1 0 0
' 0 0 1 0
' 0 0 0 1
'reading vertically then left to right
m<=1,0,0,0, 0,1,0,0, 0,0,1,0, 0,0,0,1
n<=1,0,0,0, 0,1,0,0, 0,0,1,0, 0,0,0,1
'=====
'TESTS
'=====
'
string cr=chr(13)+chr(10)
string pr="MATRIX TESTS:" cr cr
'scalar mul
for i=1 to 16 : m[i]*=4 : next
'
m[4]=1
n[4]=1
MatrixMul m,n,r,4
'
pr+=showMat r,4
MatrixInv r,4
pr+="Invert "+showMat r,4
MatrixInv r,4
pr+="Invert again " ShowMat r,4
print pr
Charles
