From 83ed4375534f2191f72dd3a103dd98332ffab067 Mon Sep 17 00:00:00 2001 From: jespergravgaard Date: Fri, 28 Jun 2019 17:05:54 +0200 Subject: [PATCH] Working on atan2() --- src/main/fragment/vwsz1=vwsz2_ror_vbuxx.asm | 14 +++ src/main/fragment/vwsz1=vwsz2_ror_vbuyy.asm | 14 +++ .../vwuz1=vwuz1_plus_pwuc1_derefidx_vbuxx.asm | 7 ++ .../vwuz1=vwuz1_plus_pwuc1_derefidx_vbuyy.asm | 7 ++ src/main/kc/stdlib/atan2.kc | 87 +++++++++++++++++++ src/main/kc/stdlib/print.kc | 2 + src/test/kc/cordic-atan2-16.kc | 41 +++++++++ src/test/kc/cordic-atan2.kc | 45 +--------- 8 files changed, 175 insertions(+), 42 deletions(-) create mode 100644 src/main/fragment/vwsz1=vwsz2_ror_vbuxx.asm create mode 100644 src/main/fragment/vwsz1=vwsz2_ror_vbuyy.asm create mode 100644 src/main/fragment/vwuz1=vwuz1_plus_pwuc1_derefidx_vbuxx.asm create mode 100644 src/main/fragment/vwuz1=vwuz1_plus_pwuc1_derefidx_vbuyy.asm create mode 100644 src/main/kc/stdlib/atan2.kc create mode 100644 src/test/kc/cordic-atan2-16.kc diff --git a/src/main/fragment/vwsz1=vwsz2_ror_vbuxx.asm b/src/main/fragment/vwsz1=vwsz2_ror_vbuxx.asm new file mode 100644 index 000000000..1fb978dd8 --- /dev/null +++ b/src/main/fragment/vwsz1=vwsz2_ror_vbuxx.asm @@ -0,0 +1,14 @@ +lda {z2} +sta {z1} +lda {z2}+1 +sta {z1}+1 +cpx #0 +beq !e+ +!: +lda {z1}+1 +cmp #$80 +ror {z1}+1 +ror {z1} +dex +bne !- +!e: \ No newline at end of file diff --git a/src/main/fragment/vwsz1=vwsz2_ror_vbuyy.asm b/src/main/fragment/vwsz1=vwsz2_ror_vbuyy.asm new file mode 100644 index 000000000..50ab21e92 --- /dev/null +++ b/src/main/fragment/vwsz1=vwsz2_ror_vbuyy.asm @@ -0,0 +1,14 @@ +lda {z2} +sta {z1} +lda {z2}+1 +sta {z1}+1 +cpy #0 +beq !e+ +!: +lda {z1}+1 +cmp #$80 +ror {z1}+1 +ror {z1} +dey +bne !- +!e: \ No newline at end of file diff --git a/src/main/fragment/vwuz1=vwuz1_plus_pwuc1_derefidx_vbuxx.asm b/src/main/fragment/vwuz1=vwuz1_plus_pwuc1_derefidx_vbuxx.asm new file mode 100644 index 000000000..8d51130a2 --- /dev/null +++ b/src/main/fragment/vwuz1=vwuz1_plus_pwuc1_derefidx_vbuxx.asm @@ -0,0 +1,7 @@ +clc +lda {z1} +adc {c1},x +sta {z1} +lda {z1}+1 +adc {c1}+1,x +sta {z1}+1 diff --git a/src/main/fragment/vwuz1=vwuz1_plus_pwuc1_derefidx_vbuyy.asm b/src/main/fragment/vwuz1=vwuz1_plus_pwuc1_derefidx_vbuyy.asm new file mode 100644 index 000000000..a05e7d623 --- /dev/null +++ b/src/main/fragment/vwuz1=vwuz1_plus_pwuc1_derefidx_vbuyy.asm @@ -0,0 +1,7 @@ +clc +lda {z1} +adc {c1},y +sta {z1} +lda {z1}+1 +adc {c1}+1,y +sta {z1}+1 diff --git a/src/main/kc/stdlib/atan2.kc b/src/main/kc/stdlib/atan2.kc new file mode 100644 index 000000000..39d504f86 --- /dev/null +++ b/src/main/kc/stdlib/atan2.kc @@ -0,0 +1,87 @@ +// Find atan2(x, y) using the CORDIC method +// See http://bsvi.ru/uploads/CORDIC--_10EBA/cordic.pdf + +// The number of iterations performed during 16-bit CORDIC atan2 calculation +const byte CORDIC_ITERATIONS_16 = 15; + +// Angles representing ATAN(0.5), ATAN(0.25), ATAN(0.125), ... +word* CORDIC_ATAN2_ANGLES_16 = 0x1000; + +// Populate cordic angles table +kickasm(pc CORDIC_ATAN2_ANGLES_16, uses CORDIC_ITERATIONS_16) {{ + .for (var i=0; i0)?y:-y; + signed word xi = (x>0)?x:-x; + word angle = 0; + for( byte i: 0..CORDIC_ITERATIONS_16) { + if(yi==0) { + // We found the correct angle! + break; + } + signed word xd = xi>>i; + signed word yd = yi>>i; + if(yi>0) { + xi += yd; + yi -= xd; + angle += CORDIC_ATAN2_ANGLES_16[i]; + } else { + xi -= yd; + yi += xd; + angle -= CORDIC_ATAN2_ANGLES_16[i]; + } + } + angle /=2; + if(x<0) angle = 0x8000-angle; + if(y<0) angle = -angle; + // Iterations complete - return estimated angle + return angle; +} + +// The number of iterations performed during 8-bit CORDIC atan2 calculation +const byte CORDIC_ITERATIONS_8 = 8; + +// Angles representing ATAN(0.5), ATAN(0.25), ATAN(0.125), ... +byte* CORDIC_ATAN2_ANGLES_8 = 0x1100; + +// Populate cordic angles table +kickasm(pc CORDIC_ATAN2_ANGLES_8, uses CORDIC_ITERATIONS_8) {{ + .fill CORDIC_ITERATIONS_8, 2*256*atan(1/pow(2,i))/PI/2 +}} + +// Find the atan2(x, y) - which is the angle of the line from (0,0) to (x,y) +// Finding the angle requires a binary search using CORDIC_ITERATIONS +// Returns the angle in hex-degrees (0=0, 0x80=PI, 0x100=2*PI) +byte atan2_8(signed byte x, signed byte y) { + signed byte yi = (y>0)?y:-y; + signed byte xi = (x>0)?x:-x; + byte angle = 0; + for( byte i: 0..CORDIC_ITERATIONS_8) { + if(yi==0) { + // We found the correct angle! + break; + } + signed byte xd = xi>>i; + signed byte yd = yi>>i; + if(yi>0) { + xi += yd; + yi -= xd; + angle += CORDIC_ATAN2_ANGLES_8[i]; + } else { + xi -= yd; + yi += xd; + angle -= CORDIC_ATAN2_ANGLES_8[i]; + } + } + angle = angle/2; + if(x<0) angle = 128-angle; + if(y<0) angle = -angle; + // Iterations complete - return estimated angle + return angle; +} \ No newline at end of file diff --git a/src/main/kc/stdlib/print.kc b/src/main/kc/stdlib/print.kc index a4f50a019..bca128ade 100644 --- a/src/main/kc/stdlib/print.kc +++ b/src/main/kc/stdlib/print.kc @@ -49,6 +49,8 @@ void print_sword(signed word w) { if(w<0) { print_char('-'); w = -w; + } else { + print_char(' '); } print_word((word)w); } diff --git a/src/test/kc/cordic-atan2-16.kc b/src/test/kc/cordic-atan2-16.kc new file mode 100644 index 000000000..c34c7ea0b --- /dev/null +++ b/src/test/kc/cordic-atan2-16.kc @@ -0,0 +1,41 @@ +// Find atan2(x, y) using the CORDIC method +// See http://bsvi.ru/uploads/CORDIC--_10EBA/cordic.pdf + +import "font-hex" +import "atan2" +import "c64" + +const byte* CHARSET = 0x2000; +const byte* SCREEN = 0x2800; +const byte* SCREEN_REF = 0x2c00; + +kickasm(pc SCREEN_REF) {{ + .for(var y=-12;y<=12;y++) + .for(var x=-19;x<=20;x++) + .byte round(256*atan2(y, x)/PI/2) +}} + + +void main() { + init_font_hex(CHARSET); + *D018 = toD018(SCREEN, CHARSET); + + byte* screen = SCREEN; + byte* screen_ref = SCREEN_REF; + for(signed byte y: -12..12) { + for(signed byte x: -19..20) { + //byte angle_b = atan2_8(x, y); + signed word xw = (signed word)(word){ (byte)x, 0 }; + signed word yw = (signed word)(word){ (byte)y, 0 }; + word angle_w = atan2_16(xw, yw); + //*screen++ = (>angle_w)-angle_b; + //*screen++ = >angle_w; + *screen++ = (>(angle_w+0x0080)) - *screen_ref++; + //*screen++ = angle_b - *screen_ref++; + //*screen++ = *screen_ref++; + } + } + byte* col00 = COLS+12*40+19; + while(true) (*col00)++; +} + diff --git a/src/test/kc/cordic-atan2.kc b/src/test/kc/cordic-atan2.kc index b45eeda66..5d238e703 100644 --- a/src/test/kc/cordic-atan2.kc +++ b/src/test/kc/cordic-atan2.kc @@ -3,6 +3,8 @@ import "font-hex" import "c64" +import "atan2" + const byte* CHARSET = 0x2000; const byte* SCREEN = 0x2800; @@ -14,7 +16,7 @@ void main() { byte* screen = SCREEN; for(signed byte y: -12..12) { for(signed byte x: -19..20) { - byte angle = atan2(x, y); + byte angle = atan2_8(x, y); *screen++ = angle; } } @@ -24,44 +26,3 @@ void main() { } -// The number of iterations performed during CORDIC atan2 calculation -const byte CORDIC_ITERATIONS = 8; - -// Angles representing ATAN(0.5), ATAN(0.25), ATAN(0.125), ... -byte* CORDIC_ATAN2_ANGLES = 0x1000; - -// Populate cordic angles table -kickasm(pc CORDIC_ATAN2_ANGLES) {{ - .fill CORDIC_ITERATIONS, 2*256*atan(1/pow(2,i))/PI/2 -}} - -// Find the atan2(x, y) - which is the angle of the line from (0,0) to (x,y) -// Finding the angle requires a binary search using CORDIC_ITERATIONS -// Returns the angle in hex-degrees (0=0, 128=PI, 256=2*PI) -byte atan2(signed byte x, signed byte y) { - signed byte yi = (y>0)?y:-y; - signed byte xi = (x>0)?x:-x; - byte angle = 0; - for( byte i: 0..CORDIC_ITERATIONS) { - if(yi==0) { - // We found the correct angle! - break; - } - signed byte xd = xi>>i; - signed byte yd = yi>>i; - if(yi>0) { - xi += yd; - yi -= xd; - angle += CORDIC_ATAN2_ANGLES[i]; - } else { - xi -= yd; - yi += xd; - angle -= CORDIC_ATAN2_ANGLES[i]; - } - } - angle /=2; - if(x<0) angle = 128-angle; - if(y<0) angle = -angle; - // Iterations complete - return estimated angle - return angle; -} \ No newline at end of file