These matrix commands perfectly mimic b3d matrices.
Commands: CreateMatrix() - creates a 4x4 matrix (a 64-bit bank) PositionMatrix matrix,x,y,z - duh RotateMatrix matrix,pitch,yaw,roll - does what it says MatrixX(),Y(),Z() MatrixPitch(),Yaw(),Roll() - All return proper angles MoveMatrix(matrix,x,y,z) - Move a matrix. This is VERY useful, and accounts for rotation!
Background commands: SetMatrixElement row,column - rows and columns are numbered 0 to 3. GetMatrixElement(matrix,row,column) - just like getmatelement. MulMatrices(a,b,c) - multiply matrix a times b and record results in matrix c
Graphics3D 400,300,16,2
pitch#=-54
yaw#=167
roll#=32.6
x#=-240
y#=56
z#=32
mx#=21
my#=-51
mz#=72
tx#=60
ty#=0
tz#=0
Print "MATRIX"
m=creatematrix()
rotatematrix m,pitch,yaw,roll
positionmatrix m,x,y,z
movematrix m,mx,my,mz
displaymatrix m
Print ""
Print "ENTITY"
p=CreatePivot()
RotateEntity p,pitch,yaw,roll
PositionEntity p,x,y,z
MoveEntity p,mx,my,mz
displayentity p
WaitKey
End
Function DisplayEntity(matrix,spacing=10)
Print "Position: "+EntityX(matrix)+", "+EntityY(matrix)+", "+EntityZ(matrix)
Print "Rotation: "+EntityPitch(matrix)+", "+EntityYaw(matrix)+", "+EntityRoll(matrix)
Print "---------------------------------"
For r=0 To 3
s$=""
For c=0 To 2
num$=Str(GetMatElement(matrix,r,c))
s=s+num+String(" ",spacing-Len(num))
Next
Print s
Next
End Function
Function DisplayMatrix(matrix,spacing=10)
Print "Position: "+MatrixX(matrix)+", "+MatrixY(matrix)+", "+MatrixZ(matrix)
Print "Rotation: "+MatrixPitch(matrix)+", "+MatrixYaw(matrix)+", "+MatrixRoll(matrix)
Print "---------------------------------"
For r=0 To 3
s$=""
For c=0 To 2
num$=Str(GetMatrixElement(matrix,r,c))
s=s+num+String(" ",spacing-Len(num))
Next
Print s
Next
End Function
Function CreateMatrix()
matrix=CreateBank(64)
For n=0 To 15
PokeFloat matrix,n*4,0
Next
PokeFloat matrix,0,1
PokeFloat matrix,5*4,1
PokeFloat matrix,10*4,1
PokeFloat matrix,11*4,1
setmatrixelement matrix,0,3,1
;setmatrixelement matrix,1,3,1
;setmatrixelement matrix,2,3,1
;setmatrixelement matrix,3,3,1
Return matrix
End Function
Function MatrixX#(matrix)
Return GetMatrixElement(matrix,3,0)
End Function
Function MatrixY#(matrix)
Return GetMatrixElement(matrix,3,1)
End Function
Function MatrixZ#(matrix)
Return GetMatrixElement(matrix,3,2)
End Function
Function MatrixPitch#(matrix)
Return -ASin(GetMatrixElement(matrix,2,1))
End Function
Function MatrixYaw#(matrix)
Return -ATan2(GetMatrixElement(matrix,2,0),GetMatrixElement(matrix,2,2))
End Function
Function MatrixRoll#(matrix)
Return ATan2(GetMatrixElement(matrix,0,1),GetMatrixElement(matrix,1,1))
;-ATan2(m(1,0),m(1,1))
End Function
Function PositionMatrix(matrix,x#,y#,z#)
SetMatrixElement matrix,3,0,x
SetMatrixElement matrix,3,1,y
SetMatrixElement matrix,3,2,z
End Function
Function TranslateMatrix(matrix,x#,y#,z#)
SetMatrixElement matrix,3,0,x+matrixx(matrix)
SetMatrixElement matrix,3,1,y+matrixy(matrix)
SetMatrixElement matrix,3,2,z+matrixz(matrix)
End Function
Function MoveMatrix(matrix,x#,y#,z#)
mx#=matrixx(matrix)
my#=matrixy(matrix)
mz#=matrixz(matrix)
temp=copymatrix(matrix)
positionmatrix temp,0,0,0
temp2=creatematrix()
positionmatrix temp2,x,y,z
mulmatrices(temp2,temp,matrix)
setmatrixelement matrix,3,0,getmatrixelement(matrix,3,0)+mx
setmatrixelement matrix,3,1,getmatrixelement(matrix,3,1)+my
setmatrixelement matrix,3,2,getmatrixelement(matrix,3,2)+mz
FreeBank temp
FreeBank temp2
End Function
Function CopyMatrix(matrix)
m=CreateBank(64)
For n=0 To 63
PokeByte m,n,PeekByte(matrix,n)
Next
Return m
End Function
Function GetMatrixElement#(matrix,row,column)
Return PeekFloat(matrix,(row*4+column)*4)
End Function
Function SetMatrixElement#(matrix,row,column,value#)
PokeFloat matrix,(row*4+column)*4,value
End Function
Function MulMatrices(matrix1,matrix2,matrix3)
For r=0 To 3
For c=0 To 3
value#=0.0
For c0=0 To 3
a#=GetMatrixElement(matrix1,r,c0)
b#=GetMatrixElement(matrix2,c0,c)
value=value+a*b
Next
SetMatrixElement matrix3,r,c,value
Next
Next
End Function
Function RotateMatrix(matrix,pitch#,yaw#,roll#)
temp=creatematrix()
xmatrix=creatematrix()
SetMatrixElement xmatrix,0,0,1.0
SetMatrixElement xmatrix,1,1,Cos(-pitch)
SetMatrixElement xmatrix,1,2,-Sin(-pitch)
SetMatrixElement xmatrix,2,1,Sin(-pitch)
SetMatrixElement xmatrix,2,2,Cos(-pitch)
ymatrix=creatematrix()
SetMatrixElement ymatrix,0,0,Cos(yaw)
SetMatrixElement ymatrix,0,2,Sin(yaw)
SetMatrixElement ymatrix,1,1,1.0
SetMatrixElement ymatrix,2,0,-Sin(yaw)
SetMatrixElement ymatrix,2,2,Cos(yaw)
zmatrix=creatematrix()
SetMatrixElement zmatrix,0,0,Cos(-roll)
SetMatrixElement zmatrix,0,1,-Sin(-roll)
SetMatrixElement zmatrix,1,0,Sin(-roll)
SetMatrixElement zmatrix,1,1,Cos(-roll)
SetMatrixElement zmatrix,2,2,1.0
mulmatrices xmatrix,ymatrix,temp
mulmatrices zmatrix,temp,matrix
FreeBank xmatrix
FreeBank ymatrix
FreeBank zmatrix
FreeBank temp
End Function
|