; File:   trig32.s
; Author: Mikael Kalms <mikael@kalms.org>
; Date:   7 December 1999
; Title:  Trigonometric function library (CPU based)
;
; Description:
;   Contains sin/cos/tan and their arcus equivalents

; fix32	sin32(angle a);
; fix32	cos32(angle a);
; fix32,fix32 sincos32(angle a);
; fix32	tan32(angle a);
; angle	asin32(fix32 x);
; angle	acos32(fix32 x);
; angle	atan32(fix32 x);
; angle	atan232(fix32 x, fix32 y);

	section	code,code

; d0	angle
; --
; d0	sin(angle)

sin32
	movem.l	d2/a0,-(sp)
	lea	sintabl,a0
	move.w	d0,d2
	lsr.w	#6,d2
	and.w	#$3f,d0
	move.l	(a0,d2.w*4),d1
	move.l	4(a0,d2.w*4),d2
	sub.l	d1,d2
	muls.w	d2,d0
	asr.l	#6,d0
	add.l	d1,d0
	movem.l	(sp)+,d2/a0
	rts

; d0	angle
; --
; d0	cos(angle)

cos32
	add.w	#$4000,d0
	bra.s	sin32

; d0	angle
; --
; d0	sin(angle)
; d1	cos(angle)

sincos32
	movem.l	d2-d3/a0,-(sp)
	lea	sintabl,a0
	move.w	d0,d3
	lsr.w	#6,d3
	and.w	#$3f,d0
	move.l	(a0,d3.w*4),d1
	move.l	4(a0,d3.w*4),d2
	sub.l	d1,d2
	muls.w	d0,d2
	asr.l	#6,d2
	add.l	d1,d2
	lea	costabl,a0
	move.l	(a0,d3.w*4),d1
	move.l	4(a0,d3.w*4),d3
	sub.l	d1,d3
	muls.w	d0,d3
	asr.l	#6,d3
	add.l	d3,d1
	move.l	d2,d0
	movem.l	(sp)+,d2-d3/a0
	rts

; d0	angle
; --
; d0	tan(angle)

tan32
	movem.l	d2/a0,-(sp)
	lea	tantabl,a0
	move.w	d0,d2
	lsr.w	#6,d2
	and.w	#$3f,d0
	move.l	(a0,d2.w*4),d1
	move.l	4(a0,d2.w*4),d2
	sub.l	d1,d2
	muls.w	d2,d0
	asr.l	#6,d0
	add.l	d1,d0
	movem.l	(sp)+,d2/a0
	rts

; d0	sin(angle)
; --
; d0	angle (-0.25 <= x <= 0.25)

asin32
	cmp.l	#$10000,d0
	bge.s	.up
	cmp.l	#-$10000,d0
	ble.s	.down
	movem.l	d2/a0,-(sp)
	lea	asintabl,a0
	move.l	d0,d2
	asr.l	#8,d2
	and.l	#$ff,d0
	move.l	(a0,d2.w*4),d1
	move.l	4(a0,d2.w*4),d2
	sub.l	d1,d2
	muls.w	d2,d0
	asr.l	#8,d0
	add.l	d1,d0
	movem.l	(sp)+,d2/a0
	rts
.up	move.l	#$4000,d0
	rts
.down	move.l	#$4000,d0
	rts

; d0	cos(angle)
; --
; d0	angle (0 <= x <= 0.5)

acos32
	cmp.l	#$10000,d0
	bge.s	.up
	cmp.l	#-$10000,d0
	ble.s	.down
	movem.l	d2/a0,-(sp)
	lea	acostabl,a0
	move.l	d0,d2
	asr.l	#8,d2
	and.l	#$ff,d0
	move.l	(a0,d2.w*4),d1
	move.l	4(a0,d2.w*4),d2
	sub.l	d1,d2
	muls.w	d2,d0
	asr.l	#8,d0
	add.l	d1,d0
	movem.l	(sp)+,d2/a0
	rts
.up	move.l	#$8000,d0
	rts
.down	moveq	#0,d0
	rts

; d0	tan(angle) == dy/dx
; --
; d0	angle (-0.25 <= x <= 0.25)

atan32
	movem.l	d2/a0,-(sp)
	move.l	d0,-(sp)
	bpl.s	.pos
	neg.l	d0
.pos	lea	atantabl,a0
	cmp.l	#$10000,d0
	bgt.s	.more45
	move.w	d0,d2
	lsr.l	#8,d2
	and.w	#$ff,d0
	move.l	(a0,d2.w*4),d1
	move.l	4(a0,d2.w*4),d2
	sub.l	d1,d2
	muls.w	d2,d0
	asr.l	#8,d0
	add.l	d1,d0
	tst.l	(sp)+
	bpl.s	.pos2
	neg.l	d0
.pos2	movem.l	(sp)+,d2/a0
	rts
.more45
	move.l	#$40000000,d2
	divs.l	d0,d2
	lsl.l	#2,d2
	move.w	d2,d0
	lsr.l	#8,d2
	and.w	#$ff,d0
	move.l	(a0,d2.w*4),d1
	move.l	4(a0,d2.w*4),d2
	sub.l	d1,d2
	muls.w	d2,d0
	asr.l	#8,d0
	add.l	d1,d0
	sub.l	#$10000/4,d0
	neg.l	d0
	tst.l	(sp)+
	bpl.s	.pos3
	neg.l	d0
.pos3	movem.l	(sp)+,d2/a0
	rts

; d0	x
; d1	y
; --
; d0	angle (0 <= x < 1)

atan232
	movem.l	d2-d4/a0,-(sp)
	tst.l	d1
	beq	.yz
	tst.l	d0
	beq	.xz

	move.l	d0,d2
	bpl.s	.ok1
	neg.l	d2
.ok1	move.l	d1,d3
	bpl.s	.ok2
	neg.l	d3
.ok2	lea	atantabl,a0
	cmp.l	d2,d3
	bgt.s	.ok3

	bfffo	d3{0:32},d4
	cmp.w	#16,d4
	bls.s	.nadjust1
	moveq	#16,d4
.nadjust1
	lsl.l	d4,d3
	sub.w	#16,d4
	neg.w	d4
	lsr.l	d4,d2

	divu.l	d2,d3
	move.w	d3,d4
	lsr.l	#8,d3
	and.w	#$ff,d4
	move.l	(a0,d3.w*4),d2
	move.l	4(a0,d3.w*4),d3
	sub.l	d2,d3
	muls.w	d4,d3
	asr.l	#8,d3
	add.l	d3,d2
	bra.s	.ok7
.ok3	exg	d2,d3

	bfffo	d3{0:32},d4
	cmp.w	#16,d4
	bls.s	.nadjust2
	moveq	#16,d4
.nadjust2
	lsl.l	d4,d3
	sub.w	#16,d4
	neg.w	d4
	lsr.l	d4,d2

	divu.l	d2,d3
	move.w	d3,d4
	lsr.l	#8,d3
	and.w	#$ff,d4
	move.l	(a0,d3.w*4),d2
	move.l	4(a0,d3.w*4),d3
	sub.l	d2,d3
	muls.w	d4,d3
	asr.l	#8,d3
	add.l	d3,d2
	sub.l	#$10000/4,d2
	neg.l	d2
.ok7
	tst.l	d1
	bmi.s	.ok4
	tst.l	d0
	bpl.s	.ok5
	sub.l	#$10000/2,d2
	neg.l	d2
	bra.s	.ok5
.ok4	sub.l	#$10000,d2
	neg.l	d2
	tst.l	d0
	bpl.s	.ok5
	sub.l	#$10000+$10000/2,d2
	neg.l	d2
.ok5	move.l	d2,d0
	andi.l	#$ffff,d0
	movem.l	(sp)+,d2-d4/a0
	rts
.yz	tst.l	d0
	bpl.s	.yz2
	move.l	#$10000/2,d2
	bra.s	.ok5
.yz2	moveq	#0,d2
	bra.s	.ok5
.xz	tst.l	d1
	bpl.s	.xz2
	move.l	#$10000*3/4,d2
	bra.s	.ok5
.xz2	move.l	#$10000/4,d2
	bra.s	.ok5

	section	data,data

sintabl	incbin	ainc:dat/trig/sincos1024l65536.bin
costabl EQU	sintabl+256*4
tantabl	incbin	ainc:dat/trig/tan1024l65536.bin

asintabl EQU	*+256*4
	incbin	ainc:dat/trig/asin512l65536.bin
acostabl EQU	*+256*4
	incbin	ainc:dat/trig/acos512l65536.bin
atantabl incbin	ainc:dat/trig/atan256l65536.bin