;*======================================================================*
;*                TITLE:       TURTLE.GAS                               *
;*                Function:    Turtle Geometry Rotation and Translation *
;*                                                                      *
;*                Project #:              RAPIER                        *
;*                Programmer:             Rob Zdybel                    *
;*                                                                      *
;*            COPYRIGHT 1993 Atari Computer Corporation                 *
;*          UNATHORIZED REPRODUCTION, ADAPTATION, DISTRIBUTION,         *
;*          PERFORMANCE OR DISPLAY OF THIS COMPUTER PROGRAM OR          *
;*        THE ASSOCIATED AUDIOVISUAL WORK IS STRICTLY PROHIBITED.       *
;*                            ALL RIGHTS RESERVED.                      *
;*                                                                      *
;*======================================================================*
;
;	DATA ORGANIZATION
;

;
;   	        +-------------------+	 +---------------+
;   	        |  Xrite  |  Yrite  |<---|  PTR to ARG1  | INPUT1
;   	        +-------------------+	 +---------------+
;   	        |  Zrite  |  Xdown  |
;   	        +-------------------+	Turtle Record
;   	        |  Ydown  |  Zdown  |
;   	        +-------------------+
;   	        |  Xhead  |  Yhead  |
;   	        +-------------------+
;   	        |  Zhead  |  Xposn  |
;   	        +-------------------+
;   	        |  Yposn  |  Zposn  |
;   	        +-------------------+
;                        ...         
;   	        +-------------------+
;   	        |  Pitch (Brads)    |   INPUT2 (16-bits)
;   	        +-------------------+
;   	        |  Yaw (Brads)      |   INPUT3 (16-bits)
;   	        +-------------------+
;   	        |  Roll (Brads)     |   INPUT4 (16-bits)
;   	        +-------------------+
;   	        |  Velocity         |   INPUT5 (16-bits)
;   	        +-------------------+
;
		XDEF	turtpak
;
;	COMMON EQUATES
;

DEBUG_ON	EQU	0

PTRSIZE		EQU	(1*4)		; 1 LONG
INPUT6		EQU	G_ENDRAM-PTRSIZE
INPUT5		EQU	INPUT6-PTRSIZE
INPUT4		EQU	INPUT5-PTRSIZE
INPUT3		EQU	INPUT4-PTRSIZE
INPUT2		EQU	INPUT3-PTRSIZE
INPUT1		EQU	INPUT2-PTRSIZE

	NOLIST

	include "jaguar.inc"
	include "blit.inc"
	include "gpu.inc"
	include "macros.inc"

        .gpu

	LIST

ALIGN PHRASE
	ORG     G_RAM

;
;	GLOBAL REGISTER EQUATES
;
temp		REGEQU	r0
temp2		REGEQU	r1
ptr		REGEQU	r2

pitch		REGEQU	r3
yaw		REGEQU	r4
roll		REGEQU	r5
velocity	REGEQU	r6

half		REGEQU	r7

xrite		REGEQU	r10
yrite		REGEQU	r11
zrite		REGEQU	r12
xdown		REGEQU	r13
ydown		REGEQU	r14
zdown		REGEQU	r15
xhead		REGEQU	r16
yhead		REGEQU	r17
zhead		REGEQU	r18
xposn		REGEQU	r19
yposn		REGEQU	r20
zposn		REGEQU	r21

sin		REGEQU	r22
cos		REGEQU	r23
rpi2		REGEQU	r24
rsintbl		REGEQU	r25
rcos		REGEQU	r26
rsin		REGEQU	r27
return		REGEQU	r28


;
;	LOCAL MACROS
;

;	NOTE: Dedicated Subroutine calls expect the proper
;	      registers to be pre-set. No Nesting!

MACRO	JSR_SIN
	move	PC,return
	jump	(rsin)
	addq	#6,return	; Determine Return Address
ENDM

MACRO	JSR_COS
	move	PC,return
	jump	(rcos)
	addq	#6,return	; Determine Return Address
ENDM

MACRO	SIN_RTS
	jump	(return)
ENDM

;
;	CODE SEGMENT
;

;
;	System Initialization
;
	DEBUG_INIT
turtpak:

	moveq	#1,half
	movei	#sine,rsin
	shlq	#13,half		; Half of One ($2000)
	movei	#cosine,rcos
	moveq	#4,rpi2
	movei	#sintbl,rsintbl
	shlq	#4,rpi2			; RPI2 = Pi/2 ($40 in Brads)

	movei	#INPUT1,ptr
	load	(ptr),temp		; TEMP = 68000 ptr to active Turtle
	addq	#4,ptr
	load	(ptr),pitch		; PITCH = Amount to Pitch the Turtle
	addq	#4,ptr
	load	(ptr),yaw		; YAW = Amount to Yaw the Turtle
	addq	#4,ptr
	load	(ptr),roll		; ROLL = Amount to Roll the Turtle
	addq	#4,ptr
	load	(ptr),velocity		; VELOCITY = Amount to Move the Turtle
	shlq	#16,velocity
	sharq	#16,velocity		; Sign-extend Velocity

	loadw	(temp),xrite		; Fetch Turtle into registers
	addq	#2,temp
	loadw	(temp),yrite
	addq	#2,temp
	shlq	#16,xrite
	shlq	#16,yrite
	sharq	#16,xrite		; And sign-extend to 32-bits
	sharq	#16,yrite
	loadw	(temp),zrite
	addq	#2,temp
	loadw	(temp),xdown
	addq	#2,temp
	shlq	#16,zrite
	shlq	#16,xdown
	sharq	#16,zrite
	sharq	#16,xdown
	loadw	(temp),ydown
	addq	#2,temp
	loadw	(temp),zdown
	addq	#2,temp
	shlq	#16,ydown
	shlq	#16,zdown
	sharq	#16,ydown
	sharq	#16,zdown
	loadw	(temp),xhead
	addq	#2,temp
	loadw	(temp),yhead
	addq	#2,temp
	shlq	#16,xhead
	shlq	#16,yhead
	sharq	#16,xhead
	sharq	#16,yhead
	loadw	(temp),zhead
	addq	#2,temp
	loadw	(temp),xposn
	addq	#2,temp
	shlq	#16,zhead
	shlq	#16,xposn
	sharq	#16,zhead
	sharq	#16,xposn
	loadw	(temp),yposn
	addq	#2,temp
	loadw	(temp),zposn
	shlq	#16,yposn
	shlq	#16,zposn
	sharq	#16,yposn
	sharq	#16,zposn

;
;	PITCH (Rotation about the X-Axis)
;
	cmpq	#0,pitch
	jr	NE,ipitch
	nop
	movei	#iyaw,temp
	jump	(temp)
	nop
ipitch:					; IF (Pitch <> 0) THEN Pitch the Turtle
	move	pitch,temp
	JSR_SIN
	move	temp,sin		; SIN = SINE(Pitch)
	move	pitch,temp
	JSR_COS
	move	temp,cos		; COS = COSINE(Pitch)

	move	sin,temp2
	imult	xdown,temp		; TEMP = xdown*cos(a)
	imult	xhead,temp2
	sub	temp2,temp		; TEMP = xdown*cos(a) - xhead*sin(a)
	move	sin,temp2
	add	half,temp
	sharq	#14,temp
	imult	xdown,temp2		; TEMP2 = xdown*sin(a)
	move	temp,xdown		; XDOWN = xdown*cos(a) - xhead*sin(a)
	move	cos,temp
	imult	xhead,temp
	add	temp2,temp		; TEMP = xhead*cos(a) + xdown*sin(a)
	add	half,temp
 	sharq	#14,temp
	move	temp,xhead		; XHEAD = xhead*cos(a) + xdown*sin(a)

	move	cos,temp
	move	sin,temp2
	imult	ydown,temp		; TEMP = ydown*cos(a)
	imult	yhead,temp2
	sub	temp2,temp		; TEMP = ydown*cos(a) - yhead*sin(a)
	move	sin,temp2
	add	half,temp
	sharq	#14,temp
	imult	ydown,temp2		; TEMP2 = ydown*sin(a)
	move	temp,ydown		; YDOWN = ydown*cos(a) - yhead*sin(a)
	move	cos,temp
	imult	yhead,temp
	add	temp2,temp		; TEMP = yhead*cos(a) + ydown*sin(a)
	add	half,temp
 	sharq	#14,temp
	move	temp,yhead		; YHEAD = yhead*cos(a) + ydown*sin(a)

	move	cos,temp
	move	sin,temp2
	imult	zdown,temp		; TEMP = zdown*cos(a)
	imult	zhead,temp2
	sub	temp2,temp		; TEMP = zdown*cos(a) - zhead*sin(a)
	move	sin,temp2
	add	half,temp
	sharq	#14,temp
	imult	zdown,temp2		; TEMP2 = zdown*sin(a)
	move	temp,zdown		; ZDOWN = zdown*cos(a) - zhead*sin(a)
	move	cos,temp
	imult	zhead,temp
	add	temp2,temp		; TEMP = zhead*cos(a) + zdown*sin(a)
	add	half,temp
 	sharq	#14,temp
	move	temp,zhead		; ZHEAD = zhead*cos(a) + zdown*sin(a)

;
;	YAW (Rotation about the Y-Axis)
;
iyaw:
	cmpq	#0,yaw
	jr	NE,10$
	nop
	movei	#iroll,temp
	jump	(temp)
	nop
10$:					; IF (Yaw <> 0) THEN Yaw the Turtle
	move	yaw,temp
	JSR_SIN
	move	temp,sin		; SIN = SINE(Yaw)
	move	yaw,temp
	JSR_COS
	move	temp,cos		; COS = COSINE(Yaw)

	move	sin,temp2
	imult	xhead,temp		; TEMP = xhead*cos(a)
	imult	xrite,temp2
	sub	temp2,temp		; TEMP = xhead*cos(a) - xrite*sin(a)
	move	sin,temp2
	add	half,temp
	sharq	#14,temp
	imult	xhead,temp2		; TEMP2 = xhead*sin(a)
	move	temp,xhead		; XHEAD = xhead*cos(a) - xrite*sin(a)
	move	cos,temp
	imult	xrite,temp
	add	temp2,temp		; TEMP = xrite*cos(a) + xhead*sin(a)
	add	half,temp
 	sharq	#14,temp
	move	temp,xrite		; XRITE = xrite*cos(a) + xhead*sin(a)

	move	cos,temp
	move	sin,temp2
	imult	yhead,temp		; TEMP = yhead*cos(a)
	imult	yrite,temp2
	sub	temp2,temp		; TEMP = yhead*cos(a) - yrite*sin(a)
	move	sin,temp2
	add	half,temp
	sharq	#14,temp
	imult	yhead,temp2		; TEMP2 = yhead*sin(a)
	move	temp,yhead		; YHEAD = yhead*cos(a) - yrite*sin(a)
	move	cos,temp
	imult	yrite,temp
	add	temp2,temp		; TEMP = yrite*cos(a) + yhead*sin(a)
	add	half,temp
 	sharq	#14,temp
	move	temp,yrite		; YRITE = yrite*cos(a) + yhead*sin(a)

	move	cos,temp
	move	sin,temp2
	imult	zhead,temp		; TEMP = zhead*cos(a)
	imult	zrite,temp2
	sub	temp2,temp		; TEMP = zhead*cos(a) - zrite*sin(a)
	move	sin,temp2
	add	half,temp
	sharq	#14,temp
	imult	zhead,temp2		; TEMP2 = zhead*sin(a)
	move	temp,zhead		; ZHEAD = zhead*cos(a) - zrite*sin(a)
	move	cos,temp
	imult	zrite,temp
	add	temp2,temp		; TEMP = zrite*cos(a) + zhead*sin(a)
	add	half,temp
 	sharq	#14,temp
	move	temp,zrite		; ZRITE = zrite*cos(a) + zhead*sin(a)

;
;	ROLL (Rotation about the Z-Axis)
;
iroll:
	cmpq	#0,roll
	jr	NE,10$
	nop
	movei	#imove,temp
	jump	(temp)
	nop
10$:					; IF (Roll <> 0) THEN Roll the Turtle
	move	roll,temp
	JSR_SIN
	move	temp,sin		; SIN = SINE(Roll)
	move	roll,temp
	JSR_COS
	move	temp,cos		; COS = COSINE(Roll)

	move	sin,temp2
	imult	xrite,temp		; TEMP = xrite*cos(a)
	imult	xdown,temp2
	sub	temp2,temp		; TEMP = xrite*cos(a) - xdown*sin(a)
	move	sin,temp2
	add	half,temp
	sharq	#14,temp
	imult	xrite,temp2		; TEMP2 = xrite*sin(a)
	move	temp,xrite		; XRITE = xrite*cos(a) - xdown*sin(a)
	move	cos,temp
	imult	xdown,temp
	add	temp2,temp		; TEMP = xdown*cos(a) + xrite*sin(a)
	add	half,temp
 	sharq	#14,temp
	move	temp,xdown		; XDOWN = xdown*cos(a) + xrite*sin(a)

	move	cos,temp
	move	sin,temp2
	imult	yrite,temp		; TEMP = yrite*cos(a)
	imult	ydown,temp2
	sub	temp2,temp		; TEMP = yrite*cos(a) - ydown*sin(a)
	move	sin,temp2
	add	half,temp
	sharq	#14,temp
	imult	yrite,temp2		; TEMP2 = yrite*sin(a)
	move	temp,yrite		; YRITE = yrite*cos(a) - ydown*sin(a)
	move	cos,temp
	imult	ydown,temp
	add	temp2,temp		; TEMP = ydown*cos(a) + yrite*sin(a)
	add	half,temp
 	sharq	#14,temp
	move	temp,ydown		; YDOWN = ydown*cos(a) + yrite*sin(a)

	move	cos,temp
	move	sin,temp2
	imult	zrite,temp		; TEMP = zrite*cos(a)
	imult	zdown,temp2
	sub	temp2,temp		; TEMP = zrite*cos(a) - zdown*sin(a)
	move	sin,temp2
	add	half,temp
	sharq	#14,temp
	imult	zrite,temp2		; TEMP2 = zrite*sin(a)
	move	temp,zrite		; ZRITE = zrite*cos(a) - zdown*sin(a)
	move	cos,temp
	imult	zdown,temp
	add	temp2,temp		; TEMP = zdown*cos(a) + zrite*sin(a)
	add	half,temp
 	sharq	#14,temp
	move	temp,zdown		; ZDOWN = zdown*cos(a) + zrite*sin(a)

;
;	MOVE (Translation along the HEAD vector)
;
imove:
	cmpq	#0,velocity
	move	velocity,temp
	jr	EQ,idone
	move	velocity,temp2		; IF (Velocity <> 0) THEN Move the Turtle
	imult	xhead,temp
	imult	yhead,temp2
	add	half,temp
	add	half,temp2
	sharq	#14,temp
	sharq	#14,temp2
	add	temp,xposn		; Xpos = Xpos + Vel*Xhead
	add	temp2,yposn		; Ypos = Ypos + Vel*Yhead
	imult	zhead,velocity
	add	half,velocity
	sharq	#14,velocity
	add	velocity,zposn		; Zpos = Zpos + Vel*Zhead

;
;	Write Final Rotated and Translated Turtle Record
;
idone:
	movei	#INPUT1,ptr
	load	(ptr),temp		; TEMP = 68000 ptr to active Turtle
	storew	xrite,(temp)
	addq	#2,temp
	storew	yrite,(temp)		; Write back turtle
	addq	#2,temp
	storew	zrite,(temp)
	addq	#2,temp
	storew	xdown,(temp)
	addq	#2,temp
	storew	ydown,(temp)
	addq	#2,temp
	storew	zdown,(temp)
	addq	#2,temp
	storew	xhead,(temp)
	addq	#2,temp
	storew	yhead,(temp)
	addq	#2,temp
	storew	zhead,(temp)
	addq	#2,temp
	storew	xposn,(temp)
	addq	#2,temp
	cmpq	#0,yposn		; don't let it go below zero
	jr	MI,sky			; DLF
	nop
	moveq	#0,yposn
sky:
	storew	yposn,(temp)
	addq	#2,temp
	storew	zposn,(temp)

;
;	Prepare for GPU RTS
;
gpuend:
	movei	#G_CTRL,temp2		; GPU control/status register
	load	(temp2),temp
	bclr	0,temp			; clear GPUGO bit
	store	temp,(temp2)		; stop the GPU

endloop:
	jr	endloop			; infinite loop
	nop


;
;	SUBROUTINE AREA
;

;
;	COSINE   16-bit table lookup Cosine
;
;	GIVEN:
;		TEMP = Angle (Binary Radians 0..255)
;		RPI2 = PI/2 ($40 in Brads)
;		RSINTBL = Ptr to Sine Table
;
;	RETURNS:
;		TEMP = Cos(Angle)*$4000
;
;	REGISTER USAGE:
;		ptr, temp, temp2
;
cosine:
	add	rpi2,temp	; Cos(a) = Sin(a + pi/2)
;
;	*CAUTION* Fall Thru to SINE Subroutine *CAUTION*
;


;
;	SINE   16-bit table lookup Sine
;
;	GIVEN:
;		TEMP = Angle (Binary Radians 0..255)
;		RPI2 = PI/2 ($40 in Brads)
;		RSINTBL = Ptr to Sine Table
;
;	RETURNS:
;		TEMP = Sin(Angle)*$4000
;
;	REGISTER USAGE:
;		ptr, temp, temp2
;
sine:
	shlq	#24,temp	; Strip any extraneous bits
	move	rsintbl,ptr
	jr	MI,69$
	shrq	#24,temp	; IF (Angle < Pi) THEN
	cmp	temp,rpi2
	jr	PL,49$		; IF (Angle > Pi/2) THEN Reduce it
	move	rpi2,temp2
	shlq	#1,temp2	; TEMP2 = Pi ($80 in Brads)
	sub	temp,temp2	; Sin(pi/2 + a) = Sin(pi/2 -a)
	move	temp2,temp
49$:	shlq	#2,temp
	add	temp,ptr
	SIN_RTS
	load	(ptr),temp	; TEMP = Table Lookup Result
69$:				; ELSE (Angle >= Pi)
	bclr	#7,temp		; TEMP = TEMP - $80
	cmp	temp,rpi2
	jr	PL,99$		; IF (Angle > Pi/2) THEN Reduce it
	move	rpi2,temp2
	shlq	#1,temp2	; TEMP2 = Pi ($80 in Brads)
	sub	temp,temp2	; Sin(pi/2 + a) = Sin(pi/2 -a)
	move	temp2,temp
99$:	shlq	#2,temp
	add	temp,ptr
	load	(ptr),temp	; TEMP = Table Lookup
	SIN_RTS
	neg	temp		; Sin(a) = -1*Sin(a - pi)


;
;	CONSTANT DATA
;

	ALIGN LONG
sintbl: 	; Lookup Table for Trigonometry
;	dc.l	$0000,$0192,$0324,$04b5,$0646,$07d6,$0964,$0af1		; True BRAD Values
;	dc.l	$0c7c,$0e06,$0f8d,$1112,$1294,$1413,$1590,$1709		; True BRAD Values
;	dc.l	$187e,$19ef,$1b5d,$1cc6,$1e2b,$1f8c,$20e7,$223d		; True BRAD Values

;	dc.l	$0000,$0195,$0329,$04b0,$0647,$07d4,$0962,$0af0		; Better integer approximations for sin/cos matching
;	dc.l	$0c7d,$0e05,$0f8d,$1110,$1292,$1414,$1590,$1709
;	dc.l	$187e,$19f0,$1b5d,$1cc6,$1e2c,$1f8b,$20e7,$223e
;	dc.l	$238e,$24da,$2620,$2760,$289a,$29ce,$2afb,$2c21
;	dc.l	$2d41,$2e5a,$2f6c,$3076,$3179,$3274,$3368,$3453
;	dc.l	$3537,$3612,$36e5,$37b0,$3871,$392b,$39db,$3a82
;	dc.l	$3b21,$3bb6,$3c42,$3cc5,$3d3f,$3daf,$3e15,$3e72
;	dc.l	$3ec5,$3f0f,$3f4f,$3f85,$3fb1,$3fd4,$3fec,$3ffb
;	dc.l	$4000

;	Swapped for GASM BUG **KLUDGE**

	dc.l	$00000000,$01950000,$03290000,$04b00000,$06470000,$07d40000,$09620000,$0af00000
	dc.l	$0c7d0000,$0e050000,$0f8d0000,$11100000,$12920000,$14140000,$15900000,$17090000
	dc.l	$187e0000,$19f00000,$1b5d0000,$1cc60000,$1e2c0000,$1f8b0000,$20e70000,$223e0000
	dc.l	$238e0000,$24da0000,$26200000,$27600000,$289a0000,$29ce0000,$2afb0000,$2c210000
	dc.l	$2d410000,$2e5a0000,$2f6c0000,$30760000,$31790000,$32740000,$33680000,$34530000
	dc.l	$35370000,$36120000,$36e50000,$37b00000,$38710000,$392b0000,$39db0000,$3a820000
	dc.l	$3b210000,$3bb60000,$3c420000,$3cc50000,$3d3f0000,$3daf0000,$3e150000,$3e720000
	dc.l	$3ec50000,$3f0f0000,$3f4f0000,$3f850000,$3fb10000,$3fd40000,$3fec0000,$3ffb0000
	dc.l	$40000000


;*======================================================================*
;*                                 EOF                                  *
;*======================================================================*
ALIGN PHRASE