ATAN2 in 6502

Some code rolling off the heavy industries math line today.

I've implemented a straighforward version of a CORDIC ATAN2 routine.
I've taken a few shortcuts. The #'s are normalized by shifting left 4 (x16) and I've limited the iterations to 8.

The accuracy is around 1 binary radian. (Binary Radians or BRADS : 0 - 255)

The slowest part is the two subroutine calls to ASRN in the inner loop. The slowness is a reflection of the fact that the 6502 doesn't have a barrel shifter, and doesn't have an arithmetic shift right instruction ( and well, chokes on 16 bits a tad)

I still enjoyed creating this routine but it's not the fastest. I've timed it and it takes about 2100 cycles or around 27 scanlines. It could be optimized. The main speedup would be to use shifting tables I think.

I've got another ATAN2 routine planned but it only spits out I think 4 times less accuracy but should be a lot faster.

This routine could also be sped up at the expense of accuracy by using fewer iterations. However, I am going to try out this other idea before I get back to this CORDIC version.

	processor  6502

	seg.u   zpage
	ORG $0080


theta   ds 2
x	   ds 2
y	   ds 2
rt	  ds 2
i	   ds 1
ox	  ds 1
oy	  ds 1

HALF equ 32768
MAXL equ 8

	seg rom
	org $f000

Start	
	
	lda #<$190
	sta x
	lda #>$190
	sta x+1

	lda #<0
	sta y
	lda #>0
	sta y+1
	
	jsr atan2
	
	jmp Start
	
	
atan2
	lda x+1
	sta ox
	lda y+1
	sta oy
	
  ;x * 16
	asl x
	rol x+1
	asl x
	rol x+1
	asl x
	rol x+1
	asl x
	rol x+1

  ;y * 16
	asl y
	rol y+1
	asl y
	rol y+1
	asl y
	rol y+1
	asl y
	rol y+1
	
	lda #0
	sta theta
	sta theta+1
	
	lda y+1
	bpl .ypositive

  ; y = -y
	lda #0
	sec
	sbc y
	sta y
	lda #0
	sbc y+1
	sta y+1

  ; x = -x
	lda #0
	sec
	sbc x
	sta x
	lda #0
	sbc x+1
	sta x+1
	
.ypositive	

	lda x+1
	bpl .xpositive
	
  ; ytemp = y
	ldx y
	ldy y+1
	
  ; y = -x
	lda #0
	sec
	sbc x
	sta y
	lda #0
	sbc x+1
	sta y+1
	
  ; x = ytemp
	stx y
	sty y+1

.xpositive

  ;------------------
  ; 

	ldx #0
	
.cordicLoop	
	stx i
	
	lda y+1
	bpl .rotateNegative
	
  ;rotate positive
	
	lda x
	sta rt
	lda x+1
	sta rt+1
	ldx i
	beq .noshiftxp
	jsr asrn
.noshiftxp

	ldx y
	ldy y+1
	
	txa
	clc
	adc rt
	sta y
	tya
	adc rt+1
	sta y+1
	
	stx rt
	sty rt+1
	
	ldx i
	beq .noshiftyp
	jsr asrn
.noshiftyp

	lda x
	sec
	sbc rt
	sta x
	lda x+1
	sbc rt+1
	sta x+1

	ldx i
	lda theta
	sec
	sbc atantabL,x
	sta theta
	lda theta+1
	sbc atantabH,x
	sta theta+1

	inx
	cpx #MAXL
	bcc .cordicLoop
	bcs .done
	
.rotateNegative

	lda x
	sta rt
	lda x+1
	sta rt+1
	ldx i
	beq .noshiftxn 
	jsr asrn
.noshiftxn
	ldx y
	ldy y+1
	
	txa
	sec
	sbc rt
	sta y
	tya
	sbc rt+1
	sta y+1
	
	stx rt
	sty rt+1
	ldx i
	beq .noshiftyn   
	jsr asrn
.noshiftyn	
	lda x
	clc
	adc rt
	sta x
	lda x+1
	adc rt+1
	sta x+1

	ldx i
	lda theta
	clc
	adc atantabL,x
	sta theta
	lda theta+1
	adc atantabH,x
	sta theta+1
	
	inx
	cpx #MAXL
	bcs .done
	jmp .cordicLoop

.done	
  ; if original y was negative
	lda oy
	bpl .oyplus
	lda theta+1
	clc
	adc #$80
	sta theta+1
.oyplus	
	
  ; if original x was negative
	lda ox
	bpl .oxplus
	lda theta+1
	clc
	adc #$40
	sta theta+1
.oxplus

	rts

; arithmetic shift right xtl and xth by X
asrn
	lda rt+1
.shiftr
	cmp #$80
	ror
	ror rt
	dex
	bne .shiftr
	sta rt+1
	rts
	
	
; arithmetic shift left xtl and xth by X
asln
	lda rt+1
.shiftl
	asl rt
	rol
	dex
	bne .shiftl
	sta rt+1
	rts
	  
 
atantabL
	.byte <8192, <4836, <2555, <1297, <651, <326, <163, <81, <41, <20, <10, <5, <3, <1, <1
atantabH	
	.byte >8192, >4836, >2555, >1297, >651, >326, >163, >81, >41, >20, >10, >5, >3, >1, >1
	
	
 ORG $FFF8
 .word 0	  ; for supercharger
 .word 0	  ; nmi vector
 .word Start  ; start vector
 .word Start  ; brk vector