; trigonometric subroutines - sine/cosine in one, arctan in another

.text

; get_sine returns sine and cosine of d0 in d1 and d2, prescaled by 16384 - to allow certainty of no overflow in 16 bits

old_get_sine::	; ALL values except d1 and d2 are returned in tact.

	movem.l	d0/a0,-(a7)
	movea.l	#sine16,a0
	and.l	#$ff,d0		; to restrict to table area !
	asl.l	#2,d0
	move.l	(a0,d0),d1	; sin in d1
	neg.l	d0		;
	add.l	#256*4,d0		; get pointer to cosine value ( cos(x)= sin(x+90) etc)
	move.l	(a0,d0),d2	; cos in d2
	move.l	(a7)+,d0

	btst	#8,d0
	beq.b	no_sine_swap
	exg	d1,d2
	neg.l	d2

no_sine_swap:
	btst	#9,d0		; is it in second quadrant ?
	beq.b	pos_both	; positive sine + cosine values
	neg.l	d1
	neg.l	d2

pos_both:
	move.l	(a7)+,a0	; purely so that a0 is protected
	rts			; DONE IT!



get_sine::			; cycle count :
	movem.l	d0/a0,-(a7)	; 24

	movea.l	#sine_table,a0	; 12
	and.w	#$3ff,d0	;  8 limit to 360 degrees
	asl.w	d0		;  8
	move.w	(a0,d0.w),d1	; 14
	ext.l	d1		;  4
	lea	$200(a0),a0	;  8
	move.w	(a0,d0.w),d2	; 14
	ext.l	d2		;  4

	movem.l	(a7)+,d0/a0	; 28
	rts			; 16 total = 140

trim_to_16bit:

	moveq	#0,d0		; shift counter
	movem.l	d3/d4,-(sp)	; retain original

	tst.l	d4
	bpl.b	.nn
	neg.l	d4		; make absolute
.nn:	tst.l	d3
	bpl.b	.nn2
	neg.l	d3
.nn2:	cmp.l	d3,d4		; keep the biggest in d4
	bge.b	.nxg
	exg	d3,d4
.nxg:	asl.l	#1,d4		; use double it, so that sign will always be made clear (just in case)
	swap	d4		; only check upper word
	tst.w	d4
	beq.b	.ok		; drop straight out if ok
.lp:	addq.b	#1,d0		; increment shift counter
	lsr.w	d4		; shift down - note using lsr not asr, because the push up above can make it -ve !!
	bne	.lp		; and loop round if still not clear
.ok:	movem.l	(sp)+,d3/d4	; recover original
	asr.l	d0,d4		; apply shift to both sides
	asr.l	d0,d3
	rts	


; get_atan returns an angle in the internal format (where Pi radians (= 180 degrees) = $200) given d3,d4 x,z directional offsets
; leaves d3,d4 unchanged and everything except d0 (angle returned)

get_atan::
	movem.l	d2-d4/a0,-(sp)

	bsr	trim_to_16bit	; because it is necessary for the divisor to be 16 bit unsigned
	moveq.l	#0,d0
	moveq.l	#0,d2		; sign thang
	tst.l	d3		; check sign of x
	bne.b	.nz		; skip special zero cases

	tst.l	d4		; test for direction
	bpl.b	.nn2		; no further changes if 0 or +ve
	move.w	#$200,d0
	bra.b	.nn2		; done

.nz:	bpl.b	.nn1
	not.w	d0
	neg.l	d3		; make d3 absolute
	not.w	d2		; modify sign flag
.nn1:	asl.w	d0		; push high bit up
	tst.l	d4		; check sign of z
	bne.b	.nz2		; skip more special zero cases

	bset	#7,d0		; make bit 8 high (will be)
	asl.w	d0		; push up
	clr.b	d0		; zap out
	bra.b	.nn2		; use

.nz2:	bpl.b	.nc		; change next highest bit(s) if z -ve
	not.b	d0		; perform change on all previously set bits
	neg.l	d4		; make d4 absolute
	not.w	d2		; modify sign flag
.nc:	asl.w	d0		; push up into final places
	cmp.l	d3,d4		; test for basic direction (x aligned or z aligned)
	bge.b	.nc2		; if z bigger or equal then don't touch next bit
	not.w	d2
	exg	d3,d4		; make d4 bigger than d3 so division would give 0-1
.nc2:	asl.l	#8,d3		; scale up to give 0-256
	asl.l	d3		; shift up to allow extra rounding facility
	divu	d4,d3		; perform division for lookup
	addq.w	#1,d3		; perform rounding adjustment (word only - from division)
	asr.w	d3		; round off one bit
	movea.l	#Atan,a0
	move.b	(a0,d3),d0	; get actual segment arctan into byte
	tst.b	d2		; check if needs negating
	beq.b	.nn2
	neg.b	d0
.nn2:	and.w	#$3ff,d0	; keep to 10 bits
	movem.l	(sp)+,d2-d4/a0
	rts


.end

english version for re-write

test x
if 0 then :
	test z
	if 0 then :
		return 0
	else if +ve :
		return 0
	else if -ve :
		return $200
	endif
else if +ve then :
	test z
	if 0 then :
		return $100
	if +ve then :
		test |x|>|z|
		if true return 0 + index
		if not  return 0 + (-index)
	if -ve then :
		test |x|>|z|
		if true return $100 + (-index)
		if not  return $100 + index
	endif
else if -ve then :
	test z
	if 0 then :
		return $300
	if +ve then :
		test |x|>|z|
		if true return $300 + (-index)
		if not  return $300 + index
	if -ve then :
		test |x|>|z|
		if true return $200 + index
		if not  return $200 + (-index)
	endif
endif


