;****************************************************************************
;
; PlayStation Maths Routines
;
;****************************************************************************

	opt	at-,w-,c+,m-

	SECTION	ASM
	INCLUDE	..\spec_psx\gp.inc
	INCLUDE	..\spec_psx\gte.inc
	INCLUDE	..\spec_psx\draw.inc

	XREF	RotMatrix
	XREF	mRotPackedYXZ

;----------------------------------------------------------------------------
; Constants
;----------------------------------------------------------------------------

SIZEOF_WORD	equ	2
SIZEOF_LONG	equ	4

NEAR_Z		equ	(32+1)

;----------------------------------------------------------------------------
; Type Definitions
;----------------------------------------------------------------------------

; -------- VECTOR3D structure

		rsreset
vx		rw	1
vy		rw	1
vz		rw	1
SIZEOF_VECTOR3D	rb	0

; -------- SVECTOR3D structure

		rsreset
svx		rh	1
svy		rh	1
svz		rh	1
SIZEOF_SVECTOR	rb	0

; -------- MATRIX3D structure

		rsreset
m00		rh	1
m01		rh	1
m02		rh	1
m10		rh	1
m11		rh	1
m12		rh	1
m20		rh	1
m21		rh	1
m22		rh	1
mPad		rh	1
mx		rw	1
my		rw	1
mz		rw	1
SIZEOF_MATRIX3D	rb	0

;----------------------------------------------------------------------------
; Externals
;----------------------------------------------------------------------------

	XREF	SQRT		; WORD SQRT[]
	XREF	rcossin_tbl	; WORD rcossin_tbl[]

	XREF	Matrix		; MATRIX3D *Matrix
	XREF	MatrixSP	; UWORD MatrixSP
	XREF	MatrixStack	; MATRIX3D MatrixStack[]
	XREF	atanTab		; WORD atanTab[]
	XREF	atanOctantTab	; LONG atanOctantTab[]

	XREF	iMatrix		; MATRIX3D *iMatrix
	XREF	iMatrixStack	; MATRIX3D iMatrixStack[]
	XREF	iRate
	XREF	iFrac
	XREF	iAmbient

;----------------------------------------------------------------------------
; Macros
;----------------------------------------------------------------------------

SINCOSLO	equ	(rcossin_tbl&$ffff)
SINCOSHI	equ	(rcossin_tbl>>16)

SCALARLO	equ	((SQRT+420)&$ffff)
SCALARHI	equ	((SQRT+420)>>16)

;----------------------------------------------------------------------------
; Public Routines
;----------------------------------------------------------------------------

	XDEF	mPushMatrix
	XDEF	mPopMatrix
	XDEF	mPushUnitMatrix
	XDEF	mTransposeMatrix
	XDEF	mTransposeMatrixGTE
	XDEF	mTranslate
	XDEF	mTranslateXYZ
	XDEF	mTranslateAbsXYZ
	XDEF	mRotX
	XDEF	mRotY
	XDEF	mRotZ
	XDEF	mRotSuperPackedYXZ
	XDEF	mLoadMatrix
	XDEF	mCopyMatrix
	XDEF	mSetTrans

	XDEF	mNormalise
	XDEF	mNormaliseXYZ
	XDEF	mSqrt
	XDEF	mGetAngle
	XDEF	mClipBoundingBox

	XDEF	InitInterpolation
	XDEF	iPushMatrix
	XDEF	iPopMatrix
	XDEF	iTranslateXYZ
	XDEF	iTranslateXYZ2
	XDEF	iRotX
	XDEF	iRotY
	XDEF	iRotZ
	XDEF	iRotYXZ
	XDEF	iRotPackedYXZ
	XDEF	iRotSuperPackedYXZ
	XDEF	iLoadMatrix

;----------------------------------------------------------------------------
;mPushMatrix
;
;Purpose:	Pushes the current matrix onto the matrix stack
;
;Inputs:	None
;
;Outputs:	None
;
;Usage:		a0-a1/t0-t7
;
;----------------------------------------------------------------------------

mPushMatrix:
	GPR.lw	a0,Matrix
	cfc2	t0,C2_R11R12		; copy current matrix...
	cfc2	t1,C2_R13R21
	cfc2	t2,C2_R22R23
	cfc2	t3,C2_R31R32
	cfc2	t4,C2_R33
	cfc2	t5,C2_TRX
	cfc2	t6,C2_TRY
	cfc2	t7,C2_TRZ

	addi	a0,SIZEOF_MATRIX3D	; ...to next matrix
	sw	t0,m00(a0)
	sw	t1,m02(a0)
	sw	t2,m11(a0)
	sw	t3,m20(a0)
	sw	t4,m22(a0)
	sw	t5,mx(a0)
	sw	t6,my(a0)
	sw	t7,mz(a0)
	jr	ra
	GPR.sw	a0,Matrix

;----------------------------------------------------------------------------
;mPopMatrix
;
;Purpose:	Pops the matrix stack into the current matrix
;
;Inputs:	None
;
;Outputs:	None
;
;Usage:		a0
;
;----------------------------------------------------------------------------

mPopMatrix:
	GPR.lw	a0,Matrix
	nop
	subi	a0,SIZEOF_MATRIX3D
	j	mLoadMatrix
	GPR.sw	a0,Matrix

;----------------------------------------------------------------------------
;mPushUnitMatrix
;
;Purpose:	Pushes a unit matrix into the current matrix
;
;Inputs:	None
;
;Outputs:	None
;
;Usage:		a0/t0-t4
;
;----------------------------------------------------------------------------

mPushUnitMatrix:
	GPR.lw	a0,Matrix
	or	t0,zero,4096
	addi	a0,SIZEOF_MATRIX3D
	GPR.sw	a0,Matrix

	move	t1,zero
	move	t2,t0
	move	t3,zero
	j	SetRotation
	move	t4,t0

;----------------------------------------------------------------------------
;mTransposeMatrix
;
;Purpose:	Transposes a matrix
;
;Inputs:	a0 = ptr to source matrix
;		a1 = ptr to destination matrix
;
;Outputs:	None
;
;Usage:		a0-a1/t1-t3
;
;----------------------------------------------------------------------------

mTransposeMatrix:
	cfc2	t0,C2_R11R12		; fetch current matrix
	cfc2	t1,C2_R13R21
	cfc2	t2,C2_R22R23
	cfc2	t3,C2_R31R32
	cfc2	t4,C2_R33

	lui	t5,$ffff
	and	t6,t0,t5		; t6 = m01
	and	t7,t2,t5		; t7 = m12
	and	t0,$ffff		; t0 = m00m10
	and	at,t1,t5
	or	t0,at

	and	t2,$ffff		; t2 = m11m21
	and	at,t3,t5
	or	t2,at

	and	t1,$ffff		; t1 = m02m12
	or	t1,t7

	and	t3,$ffff		; t3 = m20m01
	or	t3,t6

	GPR.lw	v0,Matrix
	ctc2	t0,C2_R11R12		; load transposed matrix
	ctc2	t1,C2_R13R21
	ctc2	t2,C2_R22R23
	ctc2	t3,C2_R31R32
	ctc2	t4,C2_R33

	sw	t0,m00(v0)
	sw	t1,m02(v0)
	sw	t2,m11(v0)
	sw	t3,m20(v0)
	jr	ra
	sw	t4,m22(v0)

;----------------------------------------------------------------------------
;mTranslate
;
;Purpose:	Translates the current matrix to the required position
;
;Inputs:	a0 = ptr to translation vector
;
;Outputs:	None
;
;Usage:		a0-a2
;
;----------------------------------------------------------------------------

mTranslate:
	lw	a1,vy(a0)
	lw	a2,vz(a0)
	lw	a0,vx(a0)

;----------------------------------------------------------------------------
;mTranslateXYZ
;
;Purpose:	Translates the current matrix to the required position
;
;Inputs:	a0 = x pos
;		a1 = y pos
;		a2 = z pos
;
;Outputs:	None
;
;Usage:		t0-t5
;
;----------------------------------------------------------------------------

mTranslateXYZ:
	bgez    a1,@posy
	sra     t4,a1,15
	subu    a1,zero,a1
	sra     t4,a1,15
	andi    a1,$7fff
	subu    t4,zero,t4
	j	@z
	subu    a1,zero,a1
@posy	andi    a1,a1,$7fff
@z:
	bgez    a2,@posz
	sra     t5,a2,15
	subu    a2,zero,a2
	sra     t5,a2,15
	andi    a2,a2,$7fff
	subu    t5,zero,t5
	j	@x
	subu    a2,zero,a2
@posz	andi    a2,a2,$7fff
@x:
	bgez    a0,@posx
	sra     t3,a0,15
	subu    a0,zero,a0
	sra     t3,a0,15
	andi    a0,$7fff
	subu    t3,zero,t3
	j	@doit
	subu    a0,zero,a0
@posx	andi    a0,$7fff
@doit:
	mtc2    t3,C2_IR1
	mtc2    t4,C2_IR2
	mtc2    t5,C2_IR3
	nop
	MVMVA	0,0,3,3,0		; [rt] * [ir]

	mfc2    t3,C2_MAC1
	mfc2    t4,C2_MAC2
	mtc2    a0,C2_IR1
	mtc2    a1,C2_IR2
	mtc2    a2,C2_IR3
	mfc2    t5,C2_MAC3
	MVMVA	1,0,3,0,0		; (([rt] * [ir]) >> 12) + [tr]

	bgez    t3,@lab8
	sll     t0,t3,3
	subu    t3,zero,t3
	sll     t3,3
	subu    t0,zero,t3
@lab8
	bgez    t4,@lab9
	sll     t1,t4,3
	subu    t4,zero,t4
	sll     t4,3
	subu    t1,zero,t4
@lab9
	bgez    t5,@lab10
	sll     t2,t5,3
	subu    t5,zero,t5
	sll     t5,3
	subu    t2,zero,t5
@lab10
	mfc2	t3,C2_MAC1
	mfc2	t4,C2_MAC2
	mfc2	t5,C2_MAC3
	addu	t0,t3
	addu	t1,t4
	addu	t2,t5

	GPR.lw	v0,Matrix
	ctc2	t0,C2_TRX		; load compound translation into GTE
	ctc2	t1,C2_TRY
	ctc2	t2,C2_TRZ

	sw	t0,mx(v0)		; store translation in current matrix
	sw	t1,my(v0)
	jr	ra
	sw	t2,mz(v0)

;----------------------------------------------------------------------------
;mTranslateAbsXYZ
;
;Purpose:	Translates the current matrix to the required position
;
;Inputs:	a0 = x pos
;		a1 = y pos
;		a2 = z pos
;
;Outputs:	None
;
;Usage:		a0-a3/t0-t5
;
;----------------------------------------------------------------------------

mTranslateAbsXYZ:
	GPla	a3,MatrixStack
	lw	t0,mx(a3)
	lw	t1,my(a3)
	lw	t2,mz(a3)
	sub	a0,t0
	sub	a1,t1
	sub	a2,t2

	bgez    a1,@posy
	sra     t4,a1,15
	subu    a1,zero,a1
	sra     t4,a1,15
	subu    t4,zero,t4
	andi    a1,$7fff
	j	@z
	subu    a1,zero,a1
@posy	andi    a1,$7fff
@z:
	bgez    a2,@posz
	sra     t5,a2,15
	subu    a2,zero,a2
	sra     t5,a2,15
	subu    t5,zero,t5
	andi    a2,$7fff
	j	@x
	subu    a2,zero,a2
@posz	andi    a2,$7fff
@x:
	bgez    a0,@posx
	sra     t3,a0,15
	subu    a0,zero,a0
	sra     t3,a0,15
	subu    t3,zero,t3
	andi    a0,$7fff
	j	@doit
	subu    a0,zero,a0
@posx	andi    a0,$7fff
@doit:
	mtc2    t3,C2_IR1
	mtc2    t4,C2_IR2
	mtc2    t5,C2_IR3
	nop
	MVMVA	0,0,3,3,0		; [rt] * [ir]

	mfc2    t3,C2_MAC1
	mfc2    t4,C2_MAC2
	mtc2    a0,C2_IR1
	mtc2    a1,C2_IR2
	mtc2    a2,C2_IR3
	mfc2    t5,C2_MAC3
	MVMVA	1,0,3,3,0		; ([rt] * [ir]) >> 12)

	bgez    t3,@lab8
	sll     t0,t3,3
	subu    t3,zero,t3
	sll     t3,3
	subu    t0,zero,t3
@lab8
	bgez    t4,@lab9
	sll     t1,t4,3
	subu    t4,zero,t4
	sll     t4,3
	subu    t1,zero,t4
@lab9
	bgez    t5,@lab10
	sll     t2,t5,3
	subu    t5,zero,t5
	sll     t5,3
	subu    t2,zero,t5
@lab10
	mfc2	t3,C2_MAC1
	mfc2	t4,C2_MAC2
	mfc2	t5,C2_MAC3
	addu	t0,t3
	addu	t1,t4
	addu	t2,t5

	GPR.lw	a0,Matrix

	ctc2	t0,C2_TRX		; load compound translation onto GTE
	ctc2	t1,C2_TRY
	ctc2	t2,C2_TRZ

	sw	t0,mx(a0)		; store translation in current matrix
	sw	t1,my(a0)
	jr	ra
	sw	t2,mz(a0)

;----------------------------------------------------------------------------
;mRotX
;
;Purpose:	Rotates a matrix around it's X axis
;
;Inputs:	a0 = rotation (0-65535)
;
;Outputs:	None
;
;Usage:		a0/t0-t7
;
;----------------------------------------------------------------------------

mRotX	srl	a0,2
 	andi	a0,$3ffc
	bnez	a0,@rot
	lui	t0,SINCOSHI		; t0 = sin table ptr
	jr	ra
	nop
@rot:
	ori	t0,SINCOSLO
	add	t0,a0
	lw	t5,0(t0)		; read sin/cos
	lui	t7,$ffff

	and	t6,t7,t5
	mtc2	t6,C2_VXY0		; v0 = 0, cos, sin
	mtc2	t5,C2_VZ0
	cfc2	t0,C2_R11R12
	cfc2	t1,C2_R13R21
	cfc2	t3,C2_R31R32
	MVMVA   1,0,0,3,0		; [rt] * [v0]

	srl	t6,t5,16
	sll	t5,16
	sub	t5,zero,t5
	mtc2	t5,C2_VXY1		; v1 = 0, -sin, cos
	mtc2	t6,C2_VZ1
	andi	t0,$ffff
	and	t1,t7
	andi	t3,$ffff
	mfc2	t4,C2_MAC1
	mfc2	t2,C2_MAC2
	mfc2	t5,C2_MAC3
	MVMVA   1,0,1,3,0		; [rt] * [v1]

	sll	t4,16
	or	t0,t4
	andi	t2,$ffff
	sll	t5,16
	or	t3,t5
	mfc2	t5,C2_MAC1
	mfc2	t6,C2_MAC2
	mfc2	t4,C2_MAC3
	andi	t5,$ffff
	or	t1,t5
	sll	t6,16
	j	SetRotation
	or	t2,t6

;----------------------------------------------------------------------------
;mRotY
;
;Purpose:	Rotates a matrix around it's Y axis
;
;Inputs:	a0 = rotation (0-65535)
;
;Outputs:	None
;
;Usage:		a0/t0-t7
;
;----------------------------------------------------------------------------

mRotY	srl	a0,2
 	andi	a0,$3ffc
	bnez	a0,@rot
	lui	t0,SINCOSHI		; t0 = sin table ptr
	jr	ra
	nop
@rot:
	ori	t0,SINCOSLO
	add	t0,a0
	lw	t5,0(t0)		; read sin/cos
	lui	t7,$ffff

	srl	t6,t5,16
	andi	t5,$ffff
	sub	t2,zero,t5
	mtc2	t6,C2_VXY0		; v0 = cos, 0, -sin
	mtc2	t2,C2_VZ0
	cfc2	t0,C2_R11R12
	cfc2	t2,C2_R22R23
	cfc2	t3,C2_R31R32
	MVMVA   1,0,0,3,0		; [rt] * [v0]

	mtc2	t5,C2_VXY1		; v1 = sin, 0, cos
	mtc2	t6,C2_VZ1
	and	t0,t7
	andi	t2,$ffff
	and	t3,t7
	mfc2	t4,C2_MAC1
	mfc2	t1,C2_MAC2
	mfc2	t5,C2_MAC3
	MVMVA   1,0,1,3,0		; [rt] * [v1]

	andi	t4,$ffff
	or	t0,t4
	sll	t1,16
	andi	t5,$ffff
	or	t3,t5
	mfc2	t5,C2_MAC1
	mfc2	t6,C2_MAC2
	mfc2	t4,C2_MAC3

	andi	t5,$ffff
	or	t1,t5
	sll	t6,16
	j	SetRotation
	or	t2,t6

;----------------------------------------------------------------------------
;mRotZ
;
;Purpose:	Rotates a matrix around it's Z axis
;
;Inputs:	a0 = rotation (0-65535)
;
;Outputs:	None
;
;Usage:		a0/t0-t6
;
;----------------------------------------------------------------------------

mRotZ	srl	a0,2
	andi	a0,$3ffc
	bnez	a0,@rot
	lui	t0,SINCOSHI		; t0 = sin table ptr
	jr	ra
	nop
@rot:
	ori	t0,SINCOSLO
	add	t0,a0
	lw	t0,0(t0)		; read sin/cos
	lui	t7,$ffff

	srl	t1,t0,16		; t1 = cos
	sll	t2,t0,16		; t2 = sin << 16
	or	t1,t2
	mtc2	t1,C2_VXY0		; v0 = cos, sin, 0
	mtc2	zero,C2_VZ0
	cfc2	t1,C2_R13R21
	cfc2	t2,C2_R22R23
	cfc2	t4,C2_R33
	MVMVA   1,0,0,3,0		; [rt] * [v0]

	and	t3,t0,t7
	andi	t0,$ffff
	sub	t0,zero,t0
	andi	t0,$ffff
	or	t0,t3
	mtc2	t0,C2_VXY1		; v1 = -sin, cos, 0
	mtc2	zero,C2_VZ1
	andi	t1,$ffff
	mfc2	t0,C2_MAC1
	mfc2	t5,C2_MAC2
	mfc2	t3,C2_MAC3
	MVMVA   1,0,1,3,0		; [rt] * [v1]

	and	t2,t7
	andi	t0,$ffff
	sll	t5,16
	or	t1,t5
	andi	t3,$ffff
	mfc2	t5,C2_MAC1
	mfc2	t6,C2_MAC2
	mfc2	a0,C2_MAC3

	sll	t5,16
	or	t0,t5
	andi	t6,$ffff
	or	t2,t6
	sll	a0,16
	j	SetRotation
	or	t3,a0

;----------------------------------------------------------------------------
;mRotSuperPackedYXZ
;
;Purpose:	Applies packed rotations
;
;Inputs:	a0 = ptr to packed rotations
;		a1 = # rotations to skip
;
;Outputs:	None
;
;Usage:		t0-t5
;
;----------------------------------------------------------------------------

mRotSuperPackedYXZ:
	lw	a2,0(a0)
	beqz	a1,@skipped
@skip	lhu	v0,0(a2)
	subi	a1,1
	andi	at,v0,$c000
	bnez	at,@1
	addi	a2,2
	addi	a2,2
@1	bnez	a1,@skip
	nop
	lhu	v0,0(a2)
@skipped:
	addi	a2,2
	srl	at,v0,14
	beqz	at,@rotxyz
	subi	at,1
	sw	a2,0(a0)
	beqz	at,@rotx
	subi	at,1
	beqz	at,@roty
@rotz:
	andi	a0,v0,1023
	j	mRotZ
	sll	a0,6
@roty:
	j	mRotY
	sll	a0,6
@rotx:
	andi	a0,v0,1023
	j	mRotX
	sll	a0,6
@rotxyz:
	lhu	at,0(a2)
	addi	a2,2
	sw	a2,0(a0)
	sll	a0,v0,16
	j	mRotPackedYXZ
	or	a0,at

;----------------------------------------------------------------------------
;mNormalise
;
;Purpose:	Normalises a vector
;
;Inputs:	a0 = ptr to input vector
;		a1 = ptr to output vector
;
;Outputs:	None
;
;Usage:		t4
;
;----------------------------------------------------------------------------

mNormalise:
	move	t0,a0
	move	t1,a1
	move	t2,ra
	lw	a0,vx(t0)
	lw	a1,vy(t0)
	jal	mNormaliseXYZ
	lw	a2,vz(t0)

	sw	a0,vx(t1)
	sw	a1,vy(t1)
	jr	t2
	sw	a2,vz(t1)

;----------------------------------------------------------------------------
;mNormaliseXYZ
;
;Purpose:	Normalises a vector
;
;Inputs:	a0 = vector x
;		a1 = vector y
;		a2 = vector z
;
;Outputs:	a0 = normal x
;		a1 = normal y
;		a2 = normal z
;
;Usage:		a0-a3/v0-v1/at
;
;----------------------------------------------------------------------------

mNormaliseXYZ:
	mtc2	a0,C2_IR1
	mtc2	a1,C2_IR2
	mtc2	a2,C2_IR3
	ori	a3,zero,$1f
	SQR	0			; Square (5)

	mfc2	v0,C2_MAC1
	mfc2	v1,C2_MAC2
	mfc2	at,C2_MAC3
	add	v0,v1
	add	v0,at
	mtc2	v0,C2_LZCS
	mtc2	a0,C2_IR1
	mtc2	a1,C2_IR2
	mfc2	v1,C2_LZCR
	ori	at,zero,$fffe
	and	v1,at
	sub	a3,v1
	sra	a3,1			; t3 = # shifts
	add	at,v1,-$18
	bltz	at,@neg
	lui	a0,SCALARHI
	j	@scale
	sllv	v0,at
@neg:
	ori	at,zero,$18
	sub	at,v1
	srav	v0,at
@scale:
	ori	a0,SCALARLO		; shame
	addi	v0,-64
	sll	v0,1
	add	a0,v0
;	lh	v0,SCALARLO(a0)
	lh	v0,0(a0)
	mtc2	a2,C2_IR3
	mtc2	v0,C2_IR0		; load scalar
	nop
	nop
	GPF	0			; Interpolate (5)

	mfc2	a0,C2_MAC1
	mfc2	a1,C2_MAC2
	mfc2	a2,C2_MAC3
	srav	a0,a3
	srav	a1,a3
	jr	ra
	srav	a2,a3

;----------------------------------------------------------------------------
;mSqrt
;
;Purpose:	Determines the square root of a number
;
;Inputs:	a0 = number
;
;Outputs:	v0 = square root
;
;Usage:		a0/v0/t0-t2/at
;
;----------------------------------------------------------------------------

mSqrt	mtc2	a0,C2_LZCS
	beqz	a0,@exit
	ori	t0,zero,31
	mfc2	v0,C2_LZCR
	addiu	t1,zero,-2
	and	t1,v0
	sub	t0,t1
	sra	t0,1
	addi	at,t1,-$18
	bltz	at,@neg
	lui	t2,SQRT>>16
	j	@sqr
	sllv	t1,a0,at
@neg:
	ori	at,zero,$18
	sub	at,t1
	srav	t1,a0,at
@sqr:
	ori	t2,SQRT&$ffff
	addi	t1,-$40
	sll	t1,1
	addu	t2,t1
	lh	t1,0(t2)
	nop
	sllv	t1,t0
	jr	ra
	srl	v0,t1,12
@exit:
	jr	ra
	move	v0,zero

;----------------------------------------------------------------------------
;mGetAngle
;
;Purpose:	Calculates the angle between two 2d vectors
;
;Inputs:	a0 = x1
;		a1 = y1
;		a2 = x2
;		a3 = y2
;
;Outputs:	None
;
;Usage:		a0-a1/v0/t0-t1
;
;----------------------------------------------------------------------------

mGetAngle:
	sub	t0,a2,a0		; determine gradients
	sub	t1,a3,a1
	or	at,t0,t1		; exit if both gradients are zero
	beqz	at,@end
	move	v0,zero

; -------- determine octant

	bgez	t0,@y
	lui	a0,atanTab>>16
	ori	v0,4*SIZEOF_LONG
	sub	t0,zero,t0
@y:
	bgez	t1,@d
	ori	a0,atanTab&$ffff
	addi	v0,2*SIZEOF_LONG
	sub	t1,zero,t1
@d:
	sgt	at,t1,t0		; determine largest delta
	beqz	at,@pack
	lui	a1,atanOctantTab>>16
	addi	v0,1*SIZEOF_LONG
	move	at,t0
	move	t0,t1
	move	t1,at
@pack:
	srl	at,t1,16		; fit deltas into 16 bits
	beqz	at,@get
	nop
	srl	t0,1
	j	@pack
	srl	t1,1

; -------- calculate angle

@get	sll	t1,11
	divu	t1,t0
	ori	a1,atanOctantTab&$ffff
	add	a1,v0
	lw	t1,(a1)
	mflo	t0
	nop
	sll	t0,1
	add	a0,t0
	lh	t0,(a0)
	nop
	add	t0,t1
	bgez	t0,@end
	move	v0,t0
	sub	v0,zero,t0
@end:
	sub	v0,zero,v0
	jr	ra
	andi	v0,$ffff

;----------------------------------------------------------------------------
; Local Routines
;----------------------------------------------------------------------------

;----------------------------------------------------------------------------
;SetRotation
;
;Purpose:	Loads the rotation elements into the GTE and current software
;		matrices
;
;Inputs:	t0	= m00m01
;		t1	= m02m10
;		t2	= m11m12
;		t3	= m20m21
;		t4	= m22
;
;Outputs:	None
;
;Usage:		a0/t0-t4
;
;----------------------------------------------------------------------------

SetRotation:
	GPR.lw	a0,Matrix
setrot	ctc2	t0,C2_R11R12
	ctc2	t1,C2_R13R21
	ctc2	t2,C2_R22R23
	ctc2	t3,C2_R31R32
	ctc2	t4,C2_R33
	sw	t0, 0(a0)
	sw	t1, 4(a0)
	sw	t2, 8(a0)
	sw	t3,12(a0)
	jr	ra
	sw	t4,16(a0)

;----------------------------------------------------------------------------
;mLoadMatrix
;
;Purpose:	Loads a matrix onto the GTE
;
;Inputs:	a0	= ptr to matrix to load into the GTE
;
;Outputs:	None
;
;Usage:		t0-t7
;
;----------------------------------------------------------------------------

mLoadMatrix:
	lw	t0,m00(a0)
	lw	t1,m02(a0)
	lw	t2,m11(a0)
	lw	t3,m20(a0)
	lw	t4,m22(a0)
	lw	t5,mx(a0)
	lw	t6,my(a0)
	lw	t7,mz(a0)
	ctc2	t0,C2_R11R12
	ctc2	t1,C2_R13R21
	ctc2	t2,C2_R22R23
	ctc2	t3,C2_R31R32
	ctc2	t4,C2_R33
	ctc2	t5,C2_TRX
	ctc2	t6,C2_TRY
	ctc2	t7,C2_TRZ
	jr	ra
	nop

;----------------------------------------------------------------------------
;mCopyMatrix
;
;Purpose:	Copys a matrix onto the GTE & sw matrices
;
;Inputs:	a0	= ptr to matrix to copy
;
;Outputs:	None
;
;Usage:		t0-t7
;
;----------------------------------------------------------------------------

mCopyMatrix:
	move	v0,a0
	GPR.lw	a0,Matrix
	lw	t0,m00(v0)
	lw	t1,m02(v0)
	lw	t2,m11(v0)
	lw	t3,m20(v0)
	lw	t4,m22(v0)
	lw	t5,mx(v0)
	lw	t6,my(v0)
	lw	t7,mz(v0)
	ctc2	t5,C2_TRX
	ctc2	t6,C2_TRY
	ctc2	t7,C2_TRZ
	sw	t5,mx(a0)
	sw	t6,my(a0)
	j	setrot
	sw	t7,mz(a0)

;----------------------------------------------------------------------------
;GetBounds
;
;Purpose:	Updates the current bounds
;
;Inputs:        a0	= left boundary
;		a1	= right boundary
;		a2	= top boundary
;		a3	= bottom boundary
;		t0	= screen xy 0
;		t1	= screen xy 1
;		t2	= screen xy 2
;		t3	= screen z 0
;		t4	= screen z 1
;		t5	= screen z 2
;		v0	= z count
;
;Outputs:	a0-a3	= updated bounds
;		v0	= updated z count
;
;Usage:		t0-t2/at
;
;Timing:	40/52
;
;----------------------------------------------------------------------------

GetBounds:
	slt	at,t0,a2		; y0 < top ?
	beqz	at,@y1
	slt	at,t0,a3		; y0 > bottom ?
	move	a2,t0
@y1	bnez	at,@y2
	slt	at,t1,a2		; y1 < top ?
	move	a3,t0
@y2	beqz	at,@y3
	slt	at,t1,a3		; y1 > bottom ?
	move	a2,t1
@y3	bnez	at,@y4
	slt	at,t2,a2		; y2 < top ?
	move	a3,t1
@y4	beqz	at,@y5
	slt	at,t2,a3		; y2 > bottom ?
	move	a2,t2
@y5	bnez	at,@x0
	sll	t0,16
	move	a3,t2
@x0:
	sll	t1,16
	sll	t2,16
	slt	at,t0,a0		; x0 < left ?
	beqz	at,@x1
	slt	at,t0,a1		; x0 > right ?
	move	a0,t0
@x1	bnez	at,@x2
	slt	at,t1,a0		; x1 < left ?
	move	a1,t0
@x2	beqz	at,@x3
	slt	at,t1,a1		; x1 > right ?
	move	a0,t1
@x3	bnez	at,@x4
	slt	at,t2,a0		; x2 < left ?
	move	a1,t1
@x4	beqz	at,@x5
	slt	at,t2,a1		; x2 > right ?
	move	a0,t2
@x5	bnez	at,@z0
	sltiu	at,t3,CLIPZ
	move	a1,t2
@z0:
	beqz	at,@z1
	sltiu	at,t4,CLIPZ
	addi	v0,1
@z1	beqz	at,@z2
	sltiu	at,t5,CLIPZ
	addi	v0,1
@z2	beqz	at,@z3
	nop
	addi	v0,1
@z3	jr	ra
	nop




mSetTrans:
	GPR.lw	v0,Matrix
	ctc2	a0,C2_TRX
	ctc2	a1,C2_TRY
	ctc2	a2,C2_TRZ
	sw	a0,mx(v0)
	sw	a1,my(v0)
	jr	ra
	sw	a2,mz(v0)

mClipBoundingBox:
	cfc2	t0,C2_TRZ
	move	v0,zero
	subi	t0,CLIPZ
	bgez	t0,@exit

	lhu	t0,0(a0)		; t0 = minx
	lhu	t3,2(a0)		; t3 = maxx
	lhu	t1,4(a0)		; t1 = miny
	lhu	t4,6(a0)		; t4 = maxy
	lhu	t2,8(a0)		; t2 = minz
	lhu	t7,10(a0)		; t7 = maxz
	sll	t1,16			; t1 = miny << 16
	sll	t4,16			; t4 = maxy << 16

	or	at,t0,t1		; v.0 = minx, miny, minz
	mtc2	at,C2_VXY0
	mtc2	t2,C2_VZ0
	or	at,t3,t1		; v.1 = maxx, miny, minz
	mtc2	at,C2_VXY1
	mtc2	t2,C2_VZ1
	or	at,t0,t4		; v.2 = minx, maxy, minz
	mtc2	at,C2_VXY2
	mtc2	t2,C2_VZ2
	move	v1,ra			; push ra
	RTPT

	or	t9,t3,t4		; t8 = maxx, maxy
	move	t8,t1			; t7 = minz
	lui	a0,$7fff		; a0 = left
	ori	a0,$ffff
	lui	a1,$8100		; a1 = right
	move	a2,a0			; a2 = top

	mfc2	t0,C2_SXY0
	mfc2	t1,C2_SXY1
	mfc2	t2,C2_SXY2
	mfc2	t3,C2_SZ1
	mfc2	t4,C2_SZ2
	mfc2	t5,C2_SZ3
	mtc2	t7,C2_VZ0		; v.3 = minx, miny, maxz
	mtc2	t7,C2_VZ1		; v.4 = maxx, miny, maxz
	mtc2	t7,C2_VZ2		; v.5 = minx, maxy, maxz
	move	a3,a1			; a3 = bottom
	RTPT

	subi	t3,NEAR_Z
	subi	t4,NEAR_Z
	subi	t5,NEAR_Z
	jal	GetBounds
	move	v0,zero			; v0 = z-clip count

	mfc2	t0,C2_SXY0
	mfc2	t1,C2_SXY1
	mfc2	t2,C2_SXY2
	mfc2	t3,C2_SZ1
	mfc2	t4,C2_SZ2
	mfc2	t5,C2_SZ3
	mtc2	t9,C2_VXY0		; v.6 = maxx, maxy, minz
	mtc2	t8,C2_VZ0
	mtc2	t9,C2_VXY1		; v.7 = maxx, maxy, maxz
	subi	t3,NEAR_Z
	RTPT

	subi	t4,NEAR_Z
	jal	GetBounds
	subi	t5,NEAR_Z

	mfc2	t0,C2_SXY0
	mfc2	t1,C2_SXY1
	mfc2	t3,C2_SZ1
	mfc2	t4,C2_SZ2
	subi	t3,NEAR_Z
	subi	t4,NEAR_Z
	move	t2,t1
	jal	GetBounds
	move	t5,t4

	move	ra,v1			; pop ra

; -------- return 0 if completely off-screen

	move	v1,v0
	beqz	v1,@exit
	move	v0,zero
	bltz	a1,@exit
	sra	a0,16
	bltz	a3,@exit
	slt	at,a0,SCREEN_WIDTH
	beqz	at,@exit
	sra	a2,16
	slt	at,a2,SCREEN_HEIGHT
	beqz	at,@exit

; -------- return -1 if clipped

	slt	at,v1,8
	beqz	at,@exit
	subi	v0,1
	bltz	a0,@exit
	slt	at,a1,SCREEN_WIDTH
	beqz	at,@exit
	slt	at,a3,SCREEN_HEIGHT
	beqz	at,@exit
	nop
	bltz	a2,@exit
	nop

; -------- return 1 if completely on-screen

	ori	v0,zero,1
@exit:
	jr	ra
	nop


mTransposeMatrixGTE:
	cfc2	t0,C2_R11R12		; fetch current matrix
	cfc2	t1,C2_R13R21
	cfc2	t2,C2_R22R23
	cfc2	t3,C2_R31R32
	cfc2	t4,C2_R33

	lui	t5,$ffff
	and	t6,t0,t5		; t6 = m01
	and	t7,t2,t5		; t7 = m12
	and	t0,$ffff		; t0 = m00m10
	and	at,t1,t5
	or	t0,at
	and	t2,$ffff		; t2 = m11m21
	and	at,t3,t5
	or	t2,at
	and	t1,$ffff		; t1 = m02m12
	or	t1,t7
	and	t3,$ffff		; t3 = m20m01
	or	t3,t6

	ctc2	t0,C2_R11R12		; load transposed matrix
	ctc2	t3,C2_R13R21
	ctc2	t2,C2_R22R23
	ctc2	t1,C2_R31R32
	ctc2	t4,C2_R33
	jr	ra
	nop

;----------------------------------------------------------------------------
;InitInterpolation
;
;Purpose:	Initialise the interpolation matrices
;
;Inputs:	a0	= interpolation frame
;		a1	= interpolation rate
;		a2	= ptr to interpolation stack
;
;Outputs:	None
;
;Usage:		a0-a2/t0-t7/v0
;
;----------------------------------------------------------------------------

InitInterpolation:
	cfc2	t0,C2_RBK		; save ambience
	GPR.sw	a0,iFrac
	GPR.sw	a1,iRate
	GPR.sw	a2,iMatrix
	GPR.sw	t0,iAmbient

	cfc2	t0,C2_R11R12		; fetch current matrix
	cfc2	t1,C2_R13R21
	cfc2	t2,C2_R22R23
	cfc2	t3,C2_R31R32
	cfc2	t4,C2_R33
	cfc2	t5,C2_TRX
	cfc2	t6,C2_TRY
	cfc2	t7,C2_TRZ

	ctc2	t0,C2_L11L12		; copy to GTE interpolation matrix
	ctc2	t1,C2_L13L21
	ctc2	t2,C2_L22L23
	ctc2	t3,C2_L31L32
	ctc2	t4,C2_L33
	ctc2	t5,C2_RBK
	ctc2	t6,C2_GBK
	ctc2	t7,C2_BBK

	sw	t0,m00(a2)		; copy to sw interpolation matrix
	sw	t1,m02(a2)
	sw	t2,m11(a2)
	sw	t3,m20(a2)
	sw	t4,m22(a2)
	sw	t5,mx(a2)
	sw	t6,my(a2)
	jr	ra
	sw	t7,mz(a2)

;----------------------------------------------------------------------------
;iPushMatrix
;
;Purpose:	Pushes the current draw and interpolation matrices
;
;Inputs:	None
;
;Outputs:	None
;
;Usage:		a0-a1/t0-t7
;
;----------------------------------------------------------------------------

iPushMatrix:
	GPR.lw	a0,iMatrix
	cfc2	t0,C2_L11L12		; copy current matrix...
	cfc2	t1,C2_L13L21
	cfc2	t2,C2_L22L23
	cfc2	t3,C2_L31L32
	cfc2	t4,C2_L33
	cfc2	t5,C2_RBK
	cfc2	t6,C2_GBK
	cfc2	t7,C2_BBK

	addi	a0,SIZEOF_MATRIX3D	; ...to next matrix
	sw	t0,m00(a0)
	sw	t1,m02(a0)
	sw	t2,m11(a0)
	sw	t3,m20(a0)
	sw	t4,m22(a0)
	sw	t5,mx(a0)
	sw	t6,my(a0)
	sw	t7,mz(a0)
	j	mPushMatrix
	GPR.sw	a0,iMatrix

;----------------------------------------------------------------------------
;iPopMatrix
;
;Purpose:	Pops the matrix and interpolation stack
;
;Inputs:	None
;
;Outputs:	None
;
;Usage:		a0-a2
;
;----------------------------------------------------------------------------

iPopMatrix:
	move	a2,ra			; push ra
	GPR.lw	a0,Matrix
	GPR.lw	a1,iMatrix
	subi	a0,SIZEOF_MATRIX3D
	subi	a1,SIZEOF_MATRIX3D
	GPR.sw	a0,Matrix
	jal	mLoadMatrix
	GPR.sw	a1,iMatrix

	move	ra,a2			; pop ra
	j	iLoadMatrix
	move	a0,a1

;----------------------------------------------------------------------------
;iRotX
;
;Purpose:	Rotates an interpolated matrix around it's X axis
;
;Inputs:	a0 = rotation (0-65535)
;
;Outputs:	None
;
;Usage:		a0/t0-t7
;
;----------------------------------------------------------------------------

iRotX	srl	a0,2
 	andi	a0,$3ffc
	bnez	a0,@rot
	lui	t0,SINCOSHI		; t0 = sin table ptr
	jr	ra
	nop
@rot:
	ori	t0,SINCOSLO
	add	t0,a0
	lw	t5,0(t0)		; read sin/cos
	lui	t7,$ffff

	and	t6,t7,t5
	mtc2	t6,C2_VXY0		; v0 = 0, cos, sin
	mtc2	t5,C2_VZ0
	cfc2	t0,C2_L11L12
	cfc2	t1,C2_L13L21
	cfc2	t3,C2_L31L32
	MVMVA   1,1,0,3,0		; [rt] * [v0]

	srl	t6,t5,16
	sll	t5,16
	sub	t5,zero,t5
	mtc2	t5,C2_VXY1		; v1 = 0, -sin, cos
	mtc2	t6,C2_VZ1
	andi	t0,$ffff
	and	t1,t7
	andi	t3,$ffff
	mfc2	t4,C2_MAC1
	mfc2	t2,C2_MAC2
	mfc2	t5,C2_MAC3
	MVMVA   1,1,1,3,0		; [rt] * [v1]

	sll	t4,16
	or	t0,t4
	andi	t2,$ffff
	sll	t5,16
	or	t3,t5
	mfc2	t5,C2_MAC1
	mfc2	t6,C2_MAC2
	mfc2	t4,C2_MAC3
	andi	t5,$ffff
	or	t1,t5
	sll	t6,16
	j	SetRotation_I
	or	t2,t6

;----------------------------------------------------------------------------
;iRotY
;
;Purpose:	Rotates an interpolated matrix around it's Y axis
;
;Inputs:	a0 = rotation (0-65535)
;
;Outputs:	None
;
;Usage:		a0/t0-t7
;
;----------------------------------------------------------------------------

iRotY	srl	a0,2
 	andi	a0,$3ffc
	bnez	a0,@rot
	lui	t0,SINCOSHI		; t0 = sin table ptr
	jr	ra
	nop
@rot:
	ori	t0,SINCOSLO
	add	t0,a0
	lw	t5,0(t0)		; read sin/cos
	lui	t7,$ffff

	srl	t6,t5,16
	andi	t5,$ffff
	sub	t2,zero,t5
	mtc2	t6,C2_VXY0		; v0 = cos, 0, -sin
	mtc2	t2,C2_VZ0
	cfc2	t0,C2_L11L12
	cfc2	t2,C2_L22L23
	cfc2	t3,C2_L31L32
	MVMVA   1,1,0,3,0		; [ll] * [v0]

	mtc2	t5,C2_VXY1		; v1 = sin, 0, cos
	mtc2	t6,C2_VZ1
	and	t0,t7
	andi	t2,$ffff
	and	t3,t7
	mfc2	t4,C2_MAC1
	mfc2	t1,C2_MAC2
	mfc2	t5,C2_MAC3
	MVMVA   1,1,1,3,0		; [ll] * [v1]

	andi	t4,$ffff
	or	t0,t4
	sll	t1,16
	andi	t5,$ffff
	or	t3,t5
	mfc2	t5,C2_MAC1
	mfc2	t6,C2_MAC2
	mfc2	t4,C2_MAC3

	andi	t5,$ffff
	or	t1,t5
	sll	t6,16
	j	SetRotation_I
	or	t2,t6

;----------------------------------------------------------------------------
;iRotZ
;
;Purpose:	Rotates an interpolated matrix around it's Z axis
;
;Inputs:	a0 = rotation (0-65535)
;
;Outputs:	None
;
;Usage:		a0/t0-t6
;
;----------------------------------------------------------------------------

iRotZ	srl	a0,2
	andi	a0,$3ffc
	bnez	a0,@rot
	lui	t0,SINCOSHI		; t0 = sin table ptr
	jr	ra
	nop
@rot:
	ori	t0,SINCOSLO
	add	t0,a0
	lw	t0,0(t0)		; read sin/cos
	lui	t7,$ffff

	srl	t1,t0,16		; t1 = cos
	sll	t2,t0,16		; t2 = sin << 16
	or	t1,t2
	mtc2	t1,C2_VXY0		; v0 = cos, sin, 0
	mtc2	zero,C2_VZ0
	cfc2	t1,C2_L13L21
	cfc2	t2,C2_L22L23
	cfc2	t4,C2_L33
	MVMVA   1,1,0,3,0		; [rt] * [v0]

	and	t3,t0,t7
	andi	t0,$ffff
	sub	t0,zero,t0
	andi	t0,$ffff
	or	t0,t3
	mtc2	t0,C2_VXY1		; v1 = -sin, cos, 0
	mtc2	zero,C2_VZ1
	andi	t1,$ffff
	mfc2	t0,C2_MAC1
	mfc2	t5,C2_MAC2
	mfc2	t3,C2_MAC3
	MVMVA   1,1,1,3,0		; [rt] * [v1]

	and	t2,t7
	andi	t0,$ffff
	sll	t5,16
	or	t1,t5
	andi	t3,$ffff
	mfc2	t5,C2_MAC1
	mfc2	t6,C2_MAC2
	mfc2	t8,C2_MAC3

	sll	t5,16
	or	t0,t5
	andi	t6,$ffff
	or	t2,t6
	sll	t8,16
	j	SetRotation_I
	or	t3,t8

;----------------------------------------------------------------------------
;iRotYXZ
;
;Purpose:	Rotates the interpolation matrix
;
;Inputs:	a0 = y rotation
;		a1 = x rotation
;      		a2 = z rotation
;
;Outputs:	None
;
;Usage:		t0-t5
;
;----------------------------------------------------------------------------

iRotYXZ	move	t9,ra			; push ra
	jal	iRotY
	nop

	jal	iRotX
	move	a0,a1

	move	ra,t9			; pop ra
	j	iRotZ
	move	a0,a2

;----------------------------------------------------------------------------
;iRotPackedYXZ
;
;Purpose:	Applies packed rotations to the interpolation matrix
;
;Inputs:	a0 = packed rotation
;
;Outputs:	None
;
;Usage:		t0-t5
;
;----------------------------------------------------------------------------

iRotPackedYXZ:
	move	a1,ra			; push ra

	move	a2,a0

	srl	a0,a2,4
	jal	iRotY
	andi	a0,$ffc0

	srl	a0,a2,14
	jal	iRotX
	andi	a0,$ffc0

	sll	a0,a2,6
	andi	a0,$ffc0
	j	iRotZ
	move	ra,a1			; pop ra

;----------------------------------------------------------------------------
;iRotSuperPackedYXZ
;
;Purpose:	Applies packed rotations to the interpolation matrix
;
;Inputs:	a0 = ptr to packed rotations
;		a1 = # rotations to skip
;
;Outputs:	None
;
;Usage:		t0-t5
;
;----------------------------------------------------------------------------

iRotSuperPackedYXZ:
	lw	a2,0(a0)
	beqz	a1,@skipped
@skip	lhu	v0,0(a2)
	subi	a1,1
	andi	at,v0,$c000
	bnez	at,@1
	addi	a2,2
	addi	a2,2
@1	bnez	a1,@skip
	nop
	lhu	v0,0(a2)
@skipped:
	addi	a2,2
	srl	at,v0,14
	beqz	at,@rotxyz
	subi	at,1
	sw	a2,0(a0)
	beqz	at,@rotx
	subi	at,1
	beqz	at,@roty
@rotz:
	andi	a0,v0,1023
	j	iRotZ
	sll	a0,6
@roty:
	j	iRotY
	sll	a0,6
@rotx:
	andi	a0,v0,1023
	j	iRotX
	sll	a0,6
@rotxyz:
	lhu	at,0(a2)
	addi	a2,2
	sw	a2,0(a0)
	sll	a0,v0,16
	j	iRotPackedYXZ
	or	a0,at

;----------------------------------------------------------------------------
;iTranslateXYZ
;
;Purpose:	Translates the current and interpolation matrices to the
;		required position
;
;Inputs:	a0 = x pos
;		a1 = y pos
;		a2 = z pos
;
;Outputs:	None
;
;Usage:		t0-t5
;
;----------------------------------------------------------------------------

iTranslateXYZ:
	bgez    a0,@posx
	sra     t0,a0,15
	subu    a0,zero,a0
	sra     t0,a0,15
	andi    a0,$7fff
	subu    t0,zero,t0
	j	@y
	subu    a0,zero,a0
@posx	andi    a0,$7fff
@y:
	bgez    a1,@posy
	sra     t1,a1,15
	subu    a1,zero,a1
	sra     t1,a1,15
	andi    a1,$7fff
	subu    t1,zero,t1
	j	@z
	subu    a1,zero,a1
@posy	andi    a1,a1,$7fff
@z:
	bgez    a2,@posz
	sra     t2,a2,15
	subu    a2,zero,a2
	sra     t2,a2,15
	andi    a2,a2,$7fff
	subu    t2,zero,t2
	j	@app
	subu    a2,zero,a2
@posz	andi    a2,a2,$7fff
;t0-t2 = integer
;a0-a2 = fraction
@app:	mtc2    t0,C2_IR1
	mtc2    t1,C2_IR2
	mtc2    t2,C2_IR3
	nop
	MVMVA	0,0,3,3,0		; [rt] * [ir]

	GPR.lw	v0,Matrix
	GPR.lw	v1,iMatrix

	mtc2    a0,C2_IR1
	mtc2    a1,C2_IR2
	mtc2    a2,C2_IR3
	mfc2    t6,C2_MAC1
	mfc2    t7,C2_MAC2
	mfc2    t8,C2_MAC3
	MVMVA	1,0,3,0,0		; (([rt] * [ir]) >> 12) + [tr]

	bgez    t6,@xok
	sll     t3,t6,3
	subu    t6,zero,t6
	sll     t6,3
	subu    t3,zero,t6
@xok:
	bgez    t7,@yok
	sll     t4,t7,3
	subu    t7,zero,t7
	sll     t7,3
	subu    t4,zero,t7
@yok:
	bgez    t8,@zok
	sll     t5,t8,3
	subu    t8,zero,t8
	sll     t8,3
	subu    t5,zero,t8
@zok:
	mtc2    t0,C2_IR1
	mtc2    t1,C2_IR2
	mtc2    t2,C2_IR3
	mfc2	t6,C2_MAC1
	mfc2	t7,C2_MAC2
	mfc2	t8,C2_MAC3
	MVMVA	0,1,3,3,0		; [ll] * [ir]

	addu	t3,t6
	addu	t4,t7
	addu	t5,t8
	sw	t3,mx(v0)		; store translation in current matrix
	sw	t4,my(v0)
	sw	t5,mz(v0)
	ctc2	t3,C2_TRX
	ctc2	t4,C2_TRY
	ctc2	t5,C2_TRZ

	mtc2    a0,C2_IR1
	mtc2    a1,C2_IR2
	mtc2    a2,C2_IR3
	mfc2    t6,C2_MAC1
	mfc2    t7,C2_MAC2
	mfc2    t8,C2_MAC3
	MVMVA	1,1,3,3,0		; (([ll] * [ir]) >> 12)

	bgez    t6,@xok2
	sll     t3,t6,3
	subu    t6,zero,t6
	sll     t6,3
	subu    t3,zero,t6
@xok2:
	bgez    t7,@yok2
	sll     t4,t7,3
	subu    t7,zero,t7
	sll     t7,3
	subu    t4,zero,t7
@yok2:
	bgez    t8,@zok2
	sll     t5,t8,3
	subu    t8,zero,t8
	sll     t8,3
	subu    t5,zero,t8
@zok2:
	mfc2    t6,C2_MAC1
	mfc2    t7,C2_MAC2
	mfc2    t8,C2_MAC3
	add	t3,t6
	add	t4,t7
	add	t5,t8

	cfc2	t0,C2_RBK
	cfc2	t1,C2_GBK
	cfc2	t2,C2_BBK
	add	t0,t3
	add	t1,t4
	add	t2,t5

	ctc2	t0,C2_RBK		; store interpolated translation
	ctc2	t1,C2_GBK
	ctc2	t2,C2_BBK
	sw	t0,mx(v1)
	sw	t1,my(v1)
	jr	ra
	sw	t2,mz(v1)

;----------------------------------------------------------------------------
;iTranslateXYZ2
;
;Purpose:	Translates the current and interpolation matrices to the
;		required positions
;
;Inputs:	a0 	= x pos
;		a1 	= y pos
;		a2 	= z pos
;		a3 	= x2 pos
;		16(sp)	= y2 pos
;		20(sp)	= z2 pos
;
;Outputs:	None
;
;Usage:		t0-t5
;
;----------------------------------------------------------------------------

iTranslateXYZ2:
	move	t9,ra			; push ra
	jal	mTranslateXYZ		; translate current matrix
	nop
	lw	a1,16(sp)
	lw	a2,20(sp)

	bgez    a3,@posx
	sra     t0,a3,15
	subu    a3,zero,a3
	sra     t0,a3,15
	andi    a3,$7fff
	subu    t0,zero,t0
	j	@y
	subu    a3,zero,a3
@posx	andi    a3,$7fff
@y:
	bgez    a1,@posy
	sra     t1,a1,15
	subu    a1,zero,a1
	sra     t1,a1,15
	andi    a1,$7fff
	subu    t1,zero,t1
	j	@z
	subu    a1,zero,a1
@posy	andi    a1,a1,$7fff
@z:
	bgez    a2,@posz
	sra     t2,a2,15
	subu    a2,zero,a2
	sra     t2,a2,15
	andi    a2,a2,$7fff
	subu    t2,zero,t2
	j	@app
	subu    a2,zero,a2
@posz	andi    a2,a2,$7fff
@app:
	mtc2    t0,C2_IR1
	mtc2    t1,C2_IR2
	mtc2    t2,C2_IR3
	move	ra,t9			; pop ra
	MVMVA	0,1,3,3,0		; [ll] * [ir]

	GPR.lw	v0,iMatrix

	mtc2    a3,C2_IR1
	mtc2    a1,C2_IR2
	mtc2    a2,C2_IR3
	mfc2    t6,C2_MAC1
	mfc2    t7,C2_MAC2
	mfc2    t8,C2_MAC3
	MVMVA	1,1,3,3,0		; (([ll] * [ir]) >> 12)

	bgez    t6,@xok
	sll     t3,t6,3
	subu    t6,zero,t6
	sll     t6,3
	subu    t3,zero,t6
@xok:
	bgez    t7,@yok
	sll     t4,t7,3
	subu    t7,zero,t7
	sll     t7,3
	subu    t4,zero,t7
@yok:
	bgez    t8,@zok
	sll     t5,t8,3
	subu    t8,zero,t8
	sll     t8,3
	subu    t5,zero,t8
@zok:
	mfc2    t6,C2_MAC1
	mfc2    t7,C2_MAC2
	mfc2    t8,C2_MAC3
	addu	t3,t6
	addu	t4,t7
	addu	t5,t8

	cfc2	t0,C2_RBK		; add initial translation
	cfc2	t1,C2_GBK
	cfc2	t2,C2_BBK
	addu	t0,t3
	addu	t1,t4
	addu	t2,t5

	ctc2	t0,C2_RBK		; store interpolated translation
	ctc2	t1,C2_GBK
	ctc2	t2,C2_BBK
	sw	t0,mx(v0)
	sw	t1,my(v0)
	jr	ra
	sw	t2,mz(v0)

;----------------------------------------------------------------------------
;iLoadMatrix
;
;Purpose:	Loads an interpolation matrix onto the GTE
;
;Inputs:	a0	= ptr to matrix to load into the GTE
;
;Outputs:	None
;
;Usage:		t0-t7
;
;----------------------------------------------------------------------------

iLoadMatrix:
	lw	t0,m00(a0)
	lw	t1,m02(a0)
	lw	t2,m11(a0)
	lw	t3,m20(a0)
	lw	t4,m22(a0)
	lw	t5,mx(a0)
	lw	t6,my(a0)
	lw	t7,mz(a0)
	ctc2	t0,C2_L11L12
	ctc2	t1,C2_L13L21
	ctc2	t2,C2_L22L23
	ctc2	t3,C2_L31L32
	ctc2	t4,C2_L33
	ctc2	t5,C2_RBK
	ctc2	t6,C2_GBK
	ctc2	t7,C2_BBK
	jr	ra
	nop

;----------------------------------------------------------------------------
;SetRotation_I
;
;Purpose:	Loads the rotation elements into the GTE and current software
;		interpolation matrices
;
;Inputs:	t0	= m00m01
;		t1	= m02m10
;		t2	= m11m12
;		t3	= m20m21
;		t4	= m22
;
;Outputs:	None
;
;Usage:		a0/t0-t4
;
;----------------------------------------------------------------------------

SetRotation_I:
	GPR.lw	a0,iMatrix
	ctc2	t0,C2_L11L12
	ctc2	t1,C2_L13L21
	ctc2	t2,C2_L22L23
	ctc2	t3,C2_L31L32
	ctc2	t4,C2_L33
	sw	t0, 0(a0)
	sw	t1, 4(a0)
	sw	t2, 8(a0)
	sw	t3,12(a0)
	jr	ra
	sw	t4,16(a0)

	XDEF	mResetCols

mResetCols:
	GPR.lw	v0,iAmbient
	nop
	ctc2	v0,C2_RBK
	ctc2	v0,C2_GBK
	ctc2	v0,C2_BBK
	jr	ra
	nop


