! Simple matrix math module
!
! This module consists of a number of matrix operations that operate on an
! "internal" matrix and potentially one "external" matrix. The internal
! matrix is represented by floating point registers and the external one by
! an array of floats.
!
! This is based on code originally by Marcus Comstedt.
!
! $Id$

! Copy the internal matrix out to a memory one
! Warning: DOES NOT WORK
.globl _mat_store
_mat_store:
	add	#4*16,r4		! Start at the end
	frchg
	fmov.s	fr15,@-r4
	fmov.s	fr14,@-r4
	fmov.s	fr13,@-r4
	fmov.s	fr12,@-r4
	fmov.s	fr11,@-r4
	fmov.s	fr10,@-r4
	fmov.s	fr9,@-r4
	fmov.s	fr8,@-r4
	fmov.s	fr7,@-r4
	fmov.s	fr6,@-r4
	fmov.s	fr5,@-r4
	fmov.s	fr4,@-r4
	fmov.s	fr3,@-r4
	fmov.s	fr2,@-r4
	fmov.s	fr1,@-r4
	fmov.s	fr0,@-r4
	rts
	frchg

! Copy a memory matrix into the internal one
! Warning: DOES NOT WORK
.globl _mat_load
_mat_load:
	frchg
	fmov.s	@r4+,fr0
	fmov.s	@r4+,fr1
	fmov.s	@r4+,fr2
	fmov.s	@r4+,fr3
	fmov.s	@r4+,fr4
	fmov.s	@r4+,fr5
	fmov.s	@r4+,fr6
	fmov.s	@r4+,fr7
	fmov.s	@r4+,fr8
	fmov.s	@r4+,fr9
	fmov.s	@r4+,fr10
	fmov.s	@r4+,fr11
	fmov.s	@r4+,fr12
	fmov.s	@r4+,fr13
	fmov.s	@r4+,fr14
	fmov.s	@r4+,fr15
	rts
	frchg

! Clear internal to an identity matrix
.globl _mat_identity
_mat_identity:
! Load up identity numbers as 'doubles'
	fldi0	fr0		! entirely zero
	fldi0	fr1
	fldi1	fr2		! first float is one
	fldi0	fr3
	fldi0	fr4
	fldi1	fr5		! second float is one
	fschg
	fmov	dr2,xd0		! Setup matrix
	fmov	dr0,xd2
	fmov	dr4,xd4
	fmov	dr0,xd6
	fmov	dr0,xd8
	fmov	dr2,xd10
	fmov	dr0,xd12
	fmov	dr4,xd14
	rts
	fschg


! "Apply" a matrix: multiply a matrix onto the "internal" one
.globl _mat_apply
_mat_apply:
	fmov.s		fr12,@-r15
	fmov.s		fr13,@-r15
	fmov.s		fr14,@-r15
	fmov.s		fr15,@-r15
	fmov.s		@r4+,fr0	! Load up first row
	fmov.s		@r4+,fr1
	fmov.s		@r4+,fr2
	fmov.s		@r4+,fr3
	ftrv		xmtrx,fv0
	fmov.s		@r4+,fr4
	fmov.s		@r4+,fr5
	fmov.s		@r4+,fr6
	fmov.s		@r4+,fr7
	ftrv		xmtrx,fv4
	fmov.s		@r4+,fr8
	fmov.s		@r4+,fr9
	fmov.s		@r4+,fr10
	fmov.s		@r4+,fr11
	ftrv		xmtrx,fv8
	fmov.s		@r4+,fr12
	fmov.s		@r4+,fr13
	fmov.s		@r4+,fr14
	fmov.s		@r4+,fr15
	ftrv		xmtrx,fv12
	fschg
	fmov		dr0,xd0		! Copy the resulting matrix back to internal
	fmov		dr2,xd2
	fmov		dr4,xd4
	fmov		dr6,xd6
	fmov		dr8,xd8
	fmov		dr10,xd10
	fmov		dr12,xd12
	fmov		dr14,xd14
	fschg
	fmov.s		@r15+,fr15
	fmov.s		@r15+,fr14
	fmov.s		@r15+,fr13
	rts
	fmov.s		@r15+,fr12


! Transform zero or more sets of vectors using the current internal
! matrix. Each vector is three floats long.
!   
! r4: Input vectors
! r5: Output vectors
! r6: Number of vectors
! r7: Vector stride (bytes between vectors)
!
.globl _mat_transform
_mat_transform:
	mov.l	r8,@-r15	! Save r8
	mov	r5,r0		! Destination -> r0
	pref	@r4		! Prefetch the first vector
	mov	#4,r1		! Y offset
	mov	#8,r2		! Z offset
	mov	r4,r3		! Src -> r3
	add	r7,r3		! Find next vector now
	mov	r7,r8		! Stride - 12 -> r8
	add	#-12,r8
.loop:
	fmov.s	@r4+,fr0	! Load the vector
	fmov.s	@r4+,fr1
	fmov.s	@r4+,fr2
	fldi1	fr3		! w = 1
	pref	@r3		! Prefetch the next vector
	ftrv	xmtrx,fv0	! Do the product
	add	r7,r3		! next prefetch loc
	add	r8,r4		! next read loc
	dt	r6		! Count down the loop
	fdiv	fr3,fr0		! Normalize the result (X)
	fmov.s	fr0,@r0		! Save X
	fdiv	fr3,fr1		! Normalize Y
	fmov.s	fr1,@(r0,r1)	! Save Y
	fdiv	fr3,fr2		! Normalize Z
	
!	fldi1	fr1		! Calculate -1/z
!	fneg	fr1
!	fdiv	fr2,fr1
!	fmov.s	fr1,@(r0,r2)	! Save into Z
	
	fmov.s	fr2,@(r0,r2)

	bf/s	.loop		! loop around
	add	r7,r0

	rts
	mov.l	@r15+,r8	! Restore r8





