8bitfiles.net/archives/archives.tar/archives/genie-commodore-file-library/C64-128Toolkit/CORDIC.ARC/CORDIC.S

Text File | 2019-04-13 | 4.4 KB | 296 lines

;CORDIC.S
;WRITTEN BY BILL LUCIER/NOV 92
;FROM AN ARTICLE FROM THE C USER JOURNAL NOV/92
;THIS PROGRAM FINDS THE SINE AND COSINE IN AN INTEGER FORMAT
;THE VALUES IN A CIRCLE RANGE FROM -1 TO 1. THIS PROGRAM USES
;THE RANGE -16384 TO 16384. THE COORDINATES ARE ONLY CALCULATED
;FOR THE 1ST QUADRANT AS THEY ARE THE SAME IN THE OTHERS BUT WITH
;A DIFFERENT SIGN.
;THIS ROUTINE IS ALMOST 3X AS FAST AS THE ROM FUNCTIONS.
;MERLIN USERS SHOULD BE ABLE TO ASSEMBLE THIS BY CHANGING THE ? TO [
X=$8E
Y=$8C
XPOINT=$C8
YPOINT=$CA
.MAC POKE
LDA ?1
STA ?2
.MND
*=4864
STA 193 ;STORE THETA IN 193 & 194 AND MULTIPLY BY 182
STX 194 ;STORE IN $B4 & $B5
CPX #1
BNE LESSTHEN256
CMP #14
BCC QUAD3
POKE #4:QUAD
BNE MULTIPLY
LESSTHEN256 CMP #89
BCS CHECKQUAD2
POKE #1:QUAD
BNE MULTIPLY
CHECKQUAD2 CMP #179
BCS QUAD3
POKE #2:QUAD
BNE MULTIPLY
QUAD3 POKE #3:QUAD
MULTIPLY POKE #0:$B1
LDX #182
STX $B0
STA $B1
STA $B5
STA $B4
LDX #8
MLOOP LSR $B1
ROR $B0
BCC SHIFT
CLC
LDA 193
ADC $B4
STA $B4
LDA 194
ADC $B5
STA $B5
SHIFT ASL 193
ROL 194
DEX
BNE MLOOP
THETA=$B4
LDA QUAD
CMP #2
BEQ IN2QUAD
CMP #3
BEQ IN3QUAD
CMP #4
BEQ IN4QUAD
JMP BEGINCALC
IN2QUAD SEC
LDA #0
SBC THETA
STA THETA
LDA #128
SBC THETA+1
STA THETA+1
JMP BEGINCALC
IN3QUAD SEC
LDA THETA+1
SBC #128
STA THETA+1
JMP BEGINCALC
IN4QUAD LDA THETA
EOR #255
STA THETA
LDA THETA+1
EOR #255
STA THETA+1
BEGINCALC POKE #38:X+1
POKE #221:X
LDA #<THETA
JSR TWOCOMP
LDA #0
STA Y
STA Y+1
STA I
LOOPI LDA THETA+1
BMI COUNTERCW     ;THETA IS NEGATIVE SO ADD ARCTAN TO IT
LDA I
ASL
TAY
CLC
LDA THETA
ADC NEGARCTAN,Y   ;NEGARCTAN IS THE TWO COMP OF ARCTAN SO IN
STA THETA         ;EFFECT WE ARE SUBTRACTING HERE
LDA THETA+1
ADC NEGARCTAN+1,Y
STA THETA+1
JSR ROTATEXY
LDA #$B6          ;SUBTRACT THE ROTATED VALUE OF X FROM Y
JSR TWOCOMP
CLC
LDA Y
ADC $B6
STA Y
LDA Y+1
ADC $B7
STA Y+1
CLC
LDA X
ADC $B8
STA X
LDA X+1
ADC $B9
STA X+1
JMP NEXT
COUNTERCW CLC
LDA I
ASL
TAY
LDA ARCTAN,Y
ADC THETA
STA THETA
LDA ARCTAN+1,Y
ADC THETA+1
STA THETA+1
JSR ROTATEXY
CLC
LDA Y
ADC $B6
STA Y
LDA Y+1
ADC $B7
STA Y+1
LDA #$B8
JSR TWOCOMP
CLC
LDA X
ADC $B8
STA X
LDA X+1
ADC $B9
STA X+1
NEXT INC I
LDA I
CMP #14       ;WE ARE WORKING WITH 16384 AS OUR UNITS AROUND THE
BEQ *+5       ;CIRCLE SO MUST CALCULATE 14 TIMES (2^14=16384)
JMP LOOPI
LDA $8C
LDX $8D
JSR FINALANSWER
STA $8C
STX $8D
LDA $8E
LDX $8F
JSR FINALANSWER
STA $8E
STX $8F
LDA QUAD      ;CHECK WHAT QUADRANT WE WERE ORIGINALLY IN
CMP #1        ;WE MAY HAVE TO NEGATE HERE TO GET THE CORRECT ANSWER
BEQ CHECKY
CMP #4
BEQ CHECKY
LDA #<X
JSR TWOCOMP
CHECKY LDA QUAD
CMP #3
BCC DONE
LDA #<Y
JSR TWOCOMP
DONE LDY #0
LDA $8D
LDX #XPOINT
JSR PUTINBANK1
LDX #XPOINT
LDY #1
LDA $8C
JSR PUTINBANK1
LDX #YPOINT
LDY #0
LDA $8F
JSR PUTINBANK1
LDX #YPOINT
LDY #1
LDA $8E
PUTINBANK1 STX 697
LDX #127
JMP $02AF
ROTATEXY POKE Y:$B8
POKE Y+1:$B9
STA YFLAG
BPL NOTMINUS
LDA #$B8
JSR TWOCOMP
NOTMINUS POKE X:$B6
POKE X+1:$B7
STA XFLAG
BPL NOTMINUS2      ;GET THE TWO COMPLEMENT OF IT. AFTER IT HAS
LDA #$B6           ;BEEN ROTATED WE WILL HAVE TO MAKE IT NEGATIVE AGAIN
JSR TWOCOMP
NOTMINUS2 LDX I
BEQ REVEM
ROTATEM LSR $B9
ROR $B8
LSR $B7
ROR $B6
DEX
BNE ROTATEM
REVEM LDA XFLAG  ;IF THIS WAS NEGATIVE BEFORE MAKE IT NEGATIVE AGAIN
BPL CHECKYFLAG
LDA #$B6
JSR TWOCOMP
CHECKYFLAG LDA YFLAG
BPL NOTNEG
LDA #$B8
BNE TWOCOMP
NOTNEG RTS
TWOCOMP STA $B0   ;THE VALUE POINTED AT IN $B0,$B1 IS
LDY #0            ; EXCLUSIVELY'ORED WITH 65535 AND HAS
STY $B1           ;1 ADDED TO IT TO GET THE TWO'S COMPLEMENT
LDA ($B0),Y
EOR #255
TAX
INY
LDA ($B0),Y
EOR #255
TAY
INX
BNE NOTWRAP
INY
NOTWRAP TYA
LDY #1
STA ($B0),Y
DEY
TXA
STA ($B0),Y
RTS
FINALANSWER STA $FC
STX $FD
POKE #0:NEG
STA $F9
STA $FA
STA $FB
STA $FE
TXA
BPL NOTNEGAT
POKE #$FC:NEG
JSR TWOCOMP
NOTNEGAT POKE #100:$B6     ;HERE THE NUMBER IN $FC & $FD IS * 100.
LDX #8                     ;THEN IT IS DIVIDED BY 16384. IT WOULD
LOOP LSR $B6               ;BE EASIER TO DIVIDE BY 163 BUT THERE
BCC NOADD                  ;WOULD BE A LOSS OF PRECISION
CLC
LDA $FC
ADC $F9
STA $F9
LDA $FD
ADC $FA
STA $FA
LDA $FE
ADC $FB
STA $FB
NOADD ASL $FC
ROL $FD
ROL $FE
DEX
BNE LOOP
LDX #14          ;DIVIDE BY 16384.THAT IS ROTATE RIGHT 14 TIMES
ROTATE LSR $FB
ROR $FA
ROR $F9
DEX
BNE ROTATE
LDA NEG
BEQ POSITIVE
LDA #$F9
JSR TWOCOMP
POSITIVE LDA $F9
LDX $FA
RTS
QUAD .BYT 0
I .BYT 0
NEG .BYT 0
ARCTAN .WOR 8192,4836,2555,1297,651,326,163,81,41,20,10,5,2,1
NEGARCTAN .WOR 57344,60700,62981,64239,64885,65210,65373,65455,65495,65516,65526
.WOR 65531,65534,65535
XFLAG .BYT 0
YFLAG .BYT 0