					EXPORT	SIN_MULTIPLIER
					EXPORT	SIN_ANGLE
					EXPORT	CALC_SIN

					EXPORT	COS_MULTIPLIER
					EXPORT	COS_ANGLE
					EXPORT	CALC_COS

COS_MULTIPLIER		FDB		0		; Input multiplier.				Output Integer part of the product
COS_ANGLE			FCB		0		; Input angle 0 to 256 scale.	Output below-decimal part of the product

CALC_COS			PSHS	B,X
					LDB		#$40
					SUBB	COS_ANGLE,PCR
					STB		SIN_ANGLE,PCR
					LDX		COS_MULTIPLIER,PCR
					STX		SIN_MULTIPLIER,PCR
					BSR		CALC_SIN

					LDX		SIN_MULTIPLIER,PCR
					LDB		SIN_ANGLE,PCR
					STX		COS_MULTIPLIER,PCR
					STB		COS_ANGLE,PCR
					PULS	B,X
					RTS


SIN_MULTIPLIER		FDB		0		; Input multiplier.				Output Integer part of the product
SIN_ANGLE			FCB		0		; Input angle 0 to 256 scale.	Output below-decimal part of the product

CALC_SIN			PSHS	A,B,X,U

					CLRA

					TST		SIN_MULTIPLIER,PCR
					BPL		CALC_SIN_MULTIPLIER_POSITIVE
					LDD		#0
					SUBD	SIN_MULTIPLIER,PCR
					STD		SIN_MULTIPLIER,PCR
					LDA		#$FF
CALC_SIN_MULTIPLIER_POSITIVE
					; A is 0 or FF depending on the sign of the multiplier.

					LDB		SIN_ANGLE,PCR
					BPL		CALC_SIN_POSITIVEANGLE
					COMA
					NEGB
CALC_SIN_POSITIVEANGLE

					STA		SIN_SIGN,PCR
					CMPB	#$40
					BNE		CALC_SIGN_NOT_ONE
					CLR		SIN_ANGLE,PCR
					BRA		CALC_SIN_ADJUSTSIGN
CALC_SIGN_NOT_ONE	
					LEAX	SINETABLE,PCR
					ABX

					LDA		,X
					LDB		SIN_MULTIPLIER+1,PCR
					MUL
					STD		SIN_MULTIPLIER+1,PCR

					LDA		,X
					LDB		SIN_MULTIPLIER,PCR
					MUL

					ADDB	SIN_MULTIPLIER+1,PCR
					ADCA	#0
					STD		SIN_MULTIPLIER,PCR

CALC_SIN_ADJUSTSIGN	LDA		SIN_SIGN,PCR
					BPL		CALC_SIN_END

					LDA		SIN_ANGLE,PCR
					COMA
					INCA
					STA		SIN_ANGLE,PCR

					LDD		SIN_MULTIPLIER,PCR
					SBCB	#0
					SBCA	#0
					COMA
					COMB
					STD		SIN_MULTIPLIER,PCR

CALC_SIN_END		PULS	A,B,X,U
					RTS
SIN_SIGN			FCB		0



SINETABLE
			FCB		0
			FCB		6
			FCB		12
			FCB		18
			FCB		25
			FCB		31
			FCB		37
			FCB		43
			FCB		49
			FCB		56
			FCB		62
			FCB		68
			FCB		74
			FCB		80
			FCB		86
			FCB		92
			FCB		97
			FCB		103
			FCB		109
			FCB		115
			FCB		120
			FCB		126
			FCB		131
			FCB		136
			FCB		142
			FCB		147
			FCB		152
			FCB		157
			FCB		162
			FCB		167
			FCB		171
			FCB		176
			FCB		181
			FCB		185
			FCB		189
			FCB		193
			FCB		197
			FCB		201
			FCB		205
			FCB		209
			FCB		212
			FCB		216
			FCB		219
			FCB		222
			FCB		225
			FCB		228
			FCB		231
			FCB		234
			FCB		236
			FCB		238
			FCB		241
			FCB		243
			FCB		244
			FCB		246
			FCB		248
			FCB		249
			FCB		251
			FCB		252
			FCB		253
			FCB		254
			FCB		254
			FCB		255
			FCB		255
			FCB		255
			FCB		255
			FCB		255
			FCB		255
			FCB		255
			FCB		254
			FCB		254
			FCB		253
			FCB		252
			FCB		251
			FCB		249
			FCB		248
			FCB		246
			FCB		244
			FCB		243
			FCB		241
			FCB		238
			FCB		236
			FCB		234
			FCB		231
			FCB		228
			FCB		225
			FCB		222
			FCB		219
			FCB		216
			FCB		212
			FCB		209
			FCB		205
			FCB		201
			FCB		197
			FCB		193
			FCB		189
			FCB		185
			FCB		181
			FCB		176
			FCB		171
			FCB		167
			FCB		162
			FCB		157
			FCB		152
			FCB		147
			FCB		142
			FCB		136
			FCB		131
			FCB		126
			FCB		120
			FCB		115
			FCB		109
			FCB		103
			FCB		97
			FCB		92
			FCB		86
			FCB		80
			FCB		74
			FCB		68
			FCB		62
			FCB		56
			FCB		49
			FCB		43
			FCB		37
			FCB		31
			FCB		25
			FCB		18
			FCB		12
			FCB		6
			FCB		0


			; 2D Rotation
ROTATE2D_X	FDB		0
ROTATE2D_Y	FDB		0
ROTATE2D_A	FCB		0

ROTATE2D
				PSHS	A,B,X,Y,U

				; X'=Xcos-Ysin
				; Y'=Xsin+Ycos

				LDX		ROTATE2D_X,PCR
				LDY		ROTATE2D_Y,PCR
				LDB		ROTATE2D_A,PCR

				STX		COS_MULTIPLIER,PCR
				STB		COS_ANGLE,PCR
				LBSR	CALC_COS
				LDU		COS_MULTIPLIER,PCR
				STU		ROTATE2D_NEWX,PCR
				LDA		COS_ANGLE,PCR
				STA		ROTATE2D_NEWX+2,PCR

				NEGB
				STB		SIN_ANGLE,PCR
				NEGB
				STY		SIN_MULTIPLIER,PCR
				LBSR	CALC_SIN
				LDU		SIN_MULTIPLIER,PCR
				STU		ROTATE2D_NEWX+3,PCR
				LDA		SIN_ANGLE,PCR
				STA		ROTATE2D_NEWX+5,PCR

				STB		SIN_ANGLE,PCR
				STX		SIN_MULTIPLIER,PCR
				LBSR	CALC_SIN
				LDU		SIN_MULTIPLIER,PCR
				STU		ROTATE2D_NEWY  ,PCR
				LDA		SIN_ANGLE,PCR
				STA		ROTATE2D_NEWY+2,PCR

				STY		COS_MULTIPLIER,PCR
				STB		COS_ANGLE,PCR
				LBSR	CALC_COS
				LDU		COS_MULTIPLIER,PCR
				STU		ROTATE2D_NEWY+3,PCR
				LDA		COS_ANGLE,PCR
				STA		ROTATE2D_NEWY+5,PCR

				LDA		ROTATE2D_NEWX+5,PCR
				ADDA	ROTATE2D_NEWX+2,PCR
				LDD		ROTATE2D_NEWX,PCR
				ADCB	ROTATE2D_NEWX+4,PCR
				ADCA	ROTATE2D_NEWX+3,PCR
				STD		ROTATE2D_X,PCR

				LDA		ROTATE2D_NEWY+5,PCR
				ADDA	ROTATE2D_NEWY+2,PCR
				LDD		ROTATE2D_NEWY,PCR
				ADCB	ROTATE2D_NEWY+4,PCR
				ADCA	ROTATE2D_NEWY+3,PCR
				STD		ROTATE2D_Y,PCR

				PULS	A,B,X,Y,U
				RTS

ROTATE2D_NEWX	FDB		0
				FCB		0
				FDB		0
				FCB		0
ROTATE2D_NEWY	FDB		0
				FCB		0
				FDB		0
				FCB		0


TRANS3D_MULTI_NVTX	FCB		0
TRANS3D_MULTI_SRC	FDB		0
TRANS3D_MULTI_DST	FDB		0
TRANS3D_MULTI_OBJX	EQU		TRANS3D_OBJX
TRANS3D_MULTI_OBJY	EQU		TRANS3D_OBJY
TRANS3D_MULTI_OBJZ	EQU		TRANS3D_OBJZ
TRANS3D_MULTI_OBJH	EQU		TRANS3D_OBJH
TRANS3D_MULTI_OBJP	EQU		TRANS3D_OBJP
TRANS3D_MULTI_OBJB	EQU		TRANS3D_OBJB
TRANS3D_MULTI_VIEWX	EQU		TRANS3D_VIEWX
TRANS3D_MULTI_VIEWY	EQU		TRANS3D_VIEWY
TRANS3D_MULTI_VIEWZ	EQU		TRANS3D_VIEWZ
TRANS3D_MULTI_VIEWH	EQU		TRANS3D_VIEWH
TRANS3D_MULTI_VIEWP	EQU		TRANS3D_VIEWP
TRANS3D_MULTI_VIEWB	EQU		TRANS3D_VIEWB

TRANS3D_MULTI		PSHS	A,B,X,Y,U
					LDX		TRANS3D_MULTI_SRC,PCR
					LDU		TRANS3D_MULTI_DST,PCR
					LDA		TRANS3D_MULTI_NVTX,PCR
					BEQ		TRANS3D_MULTI_END

TRANS3D_MULTI_LOOP
					LDY		,X++
					STY		TRANS3D_X,PCR
					LDY		,X++
					STY		TRANS3D_Y,PCR
					LDY		,X++
					STY		TRANS3D_Z,PCR
					BSR		TRANS3D
					LDY		TRANS3D_X,PCR
					STY		,U++
					LDY		TRANS3D_Y,PCR
					STY		,U++
					LDY		TRANS3D_Z,PCR
					STY		,U++

					DECA
					BNE		TRANS3D_MULTI_LOOP

TRANS3D_MULTI_END
					PULS	A,B,X,Y,U
					RTS


TRANS3D_X			FDB		0
TRANS3D_Y			FDB		0
TRANS3D_Z			FDB		0
TRANS3D_OBJX		FDB		0
TRANS3D_OBJY		FDB		0
TRANS3D_OBJZ		FDB		0
TRANS3D_OBJH		FCB		0
TRANS3D_OBJP		FCB		0
TRANS3D_OBJB		FCB		0
TRANS3D_VIEWX		FDB		0
TRANS3D_VIEWY		FDB		0
TRANS3D_VIEWZ		FDB		0
TRANS3D_VIEWH		FCB		0
TRANS3D_VIEWP		FCB		0
TRANS3D_VIEWB		FCB		0

					FCB		0
					; This is a not-so-elegant way of preventing
					; TRANS3D from going berserk.
					; I was STX ing to VIEWB and destroyed first byte
					; of the function.

TRANS3D				PSHS	A,B,X,Y,U

					; Rotate by OBJ B,P,H -> Translate by OBJ X,Y,Z
					; Translate by -VIEW X,Y,Z -> Rotate by -VIEW H,P,B

					LEAX	TRANS3D_X,PCR
					LEAY	TRANS3D_Y,PCR
					LDA		TRANS3D_OBJB,PCR
					BSR		TRANS3D_ROTATION

					LEAX	TRANS3D_Y,PCR
					LEAY	TRANS3D_Z,PCR
					LDA		TRANS3D_OBJP,PCR
					BSR		TRANS3D_ROTATION

					LEAX	TRANS3D_Z,PCR
					LEAY	TRANS3D_X,PCR
					LDA		TRANS3D_OBJH,PCR
					BSR		TRANS3D_ROTATION

					LEAX	TRANS3D_X,PCR
					LEAU	TRANS3D_OBJX,PCR
					LDD		,X
					ADDD	,U++
					STD		,X++
					LDD		,X
					ADDD	,U++
					STD		,X++
					LDD		,X
					ADDD	,U++
					STD		,X++

					LEAX	TRANS3D_X,PCR
					LEAU	TRANS3D_VIEWX,PCR
					LDD		,X
					SUBD	,U++
					STD		,X++
					LDD		,X
					SUBD	,U++
					STD		,X++
					LDD		,X
					SUBD	,U++
					STD		,X++

					LEAX	TRANS3D_Z,PCR
					LEAY	TRANS3D_X,PCR
					LDA		TRANS3D_VIEWH,PCR
					NEGA
					BSR		TRANS3D_ROTATION

					LEAX	TRANS3D_Y,PCR
					LEAY	TRANS3D_Z,PCR
					LDA		TRANS3D_VIEWP,PCR
					NEGA
					BSR		TRANS3D_ROTATION

					LEAX	TRANS3D_X,PCR
					LEAY	TRANS3D_Y,PCR
					LDA		TRANS3D_VIEWB,PCR
					NEGA
					BSR		TRANS3D_ROTATION

					PULS	A,B,X,Y,U,PC


					; This function is also used from other functions.
TRANS3D_ROTATION	STA		ROTATE2D_A,PCR
					LDD		,X
					STD		ROTATE2D_X,PCR
					LDD		,Y
					STD		ROTATE2D_Y,PCR
					LBSR	ROTATE2D
					LDD		ROTATE2D_X,PCR
					STD		,X
					LDD		ROTATE2D_Y,PCR
					STD		,Y
					RTS



PERSPECTIVE_VTXPTR	FDB		0
PERSPECTIVE_CX		FDB		0
PERSPECTIVE_CY		FDB		0
PERSPECTIVE_XSHIFT	FCB		8
PERSPECTIVE_YSHIFT	FCB		7

PERSPECTIVE			PSHS	A,B,X,Y,U

					LDU		PERSPECTIVE_VTXPTR,PCR

					LDB		,U	; HIBYTE of X
					SEX
					STA		DIV32_16_NUMER_HI  ,PCR
					STA		DIV32_16_NUMER_HI+1,PCR
					LDD		,U
					STD		DIV32_16_NUMER_LO  ,PCR

					LDX		#0
					LDB		PERSPECTIVE_XSHIFT,PCR
					ABX
PERSPECTIVE_XSHIFT_LOOP
					LDD		DIV32_16_NUMER_LO,PCR
					ASLB
					ROLA
					STD		DIV32_16_NUMER_LO,PCR
					LDD		DIV32_16_NUMER_HI,PCR
					ROLB
					ROLA
					STD		DIV32_16_NUMER_HI,PCR
					LEAX	-1,X
					BNE		PERSPECTIVE_XSHIFT_LOOP

					LDD		#0
					SUBD	4,U
					STD		DIV32_16_DENOM,PCR
					LBSR	DIV32_16
					LDD		DIV32_16_DENOM,PCR
					ADDD	PERSPECTIVE_CX,PCR
					STD		,U

					LDB		2,U ; HIBYTE of Y
					SEX
					STA		DIV32_16_NUMER_HI  ,PCR
					STA		DIV32_16_NUMER_HI+1,PCR
					LDD		2,U ; Y
					STD		DIV32_16_NUMER_LO  ,PCR

					; X is already zero
					LDB		PERSPECTIVE_YSHIFT,PCR
					ABX
PERSPECTIVE_YSHIFT_LOOP
					LDD		DIV32_16_NUMER_LO,PCR
					ASLB
					ROLA
					STD		DIV32_16_NUMER_LO,PCR
					LDD		DIV32_16_NUMER_HI,PCR
					ROLB
					ROLA
					STD		DIV32_16_NUMER_HI,PCR
					LEAX	-1,X
					BNE		PERSPECTIVE_YSHIFT_LOOP

					LDD		#0
					SUBD	4,U
					STD		DIV32_16_DENOM,PCR
					LBSR	DIV32_16
					LDD		PERSPECTIVE_CY,PCR
					SUBD	DIV32_16_DENOM,PCR
					STD		2,U

					PULS	A,B,X,Y,U,PC


					; Output vector length=256
CALC_FWD_VECTOR_X	FDB		0	; Output VX
CALC_FWD_VECTOR_Y	FDB		0	; Output VY
CALC_FWD_VECTOR_Z	FDB		0	; Output VZ
CALC_FWD_VECTOR_H	FCB		0	; Input H
CALC_FWD_VECTOR_P	FCB		0	; Input P
CALC_FWD_VECTOR_B	FCB		0	; Input B (Actually irrelevant)

CALC_FWD_VECTOR		PSHS	A,B,X,Y
					LDD		#$FF00
					STD		CALC_FWD_VECTOR_Z,PCR
					LDD		#0
					STD		CALC_FWD_VECTOR_Y,PCR
					STD		CALC_FWD_VECTOR_X,PCR

					LEAX	CALC_FWD_VECTOR_Y,PCR
					LEAY	CALC_FWD_VECTOR_Z,PCR
					LDA		CALC_FWD_VECTOR_P,PCR
					LBSR	TRANS3D_ROTATION

					LEAX	CALC_FWD_VECTOR_Z,PCR
					LEAY	CALC_FWD_VECTOR_X,PCR
					LDA		CALC_FWD_VECTOR_H,PCR
					LBSR	TRANS3D_ROTATION

					PULS	A,B,X,Y,PC
