EX <emb3.pdf>
---------------------------------------------------------------------

Rafael Deliano
Steinbergstr.37
82110 Germering
Tel 089/8418317
Rafael_Deliano@t-online.de
V1.0 ( Papier ) : 2.  Dez. 98
V2.0 ( pdf )    : 23. Feb. 01

		      
CORDIC ist die Abkrzung fr  Coordinate Rotation Digital Computer". Es ist 
ein Verfahren zur effizienten Berechnung mathematischer Funktionen. Dabei 
werden nur Addition, Subtraktion und Schiebeoperationen bentigt. Auch 
einfache 8-Bit-Controller haben diese Befehle und man kann deshalb auf ihnen 
CORDIC wirksam implementieren. Ergebnis der Berechnung sind Sinus, Cosinus, 
Arcustangens und ihre inversen Funktionen. Sowie Multiplikation, Division 
und Quadratwurzel. Also so ziemlich alles was was man braucht.

Geschichte                                	
                                                
Der trigonometrische Teil wurde von Jack Volder [1] Ende der 50er Jahre 
entwickelt und von Walther [2] spter um die linearen und hyperbolischen 
Funktionen erweitert.             
Seine Anwendung ist universell. Sie reicht von Taschenrechner von 
Hewlett-Packard aus den frhen 70er Jahren ber Arithmetik-Coprozessoren 	
wie dem 8087 zur Software in den Amsat-Satelliten der Amateurfunker     
Am meisten profitiert man von CORDIC wohl bei der Berechnung Bewegung von 
Krpern im Raum, wie sie fr Werkzeugmaschinen und Roboter typisch ist.                      

Das hier beschriebene Festkomma-CORDIC realisiert zwar trigonometrische 
Funktionen, ist aber kein Ersatz fr eine universelle Floatingpoint-Package. 
Die Dynamik ist durch das Festkommaformat begrenzt und die Konvergenz auf 
bestimmte Wertebereiche beschrnkt. Das Verfahren eignet sich jedoch gut 
fr Anwendung auf Controllern wo Geschwindigkeit und preiswerte Hardware 
wesentlich sind. 
            					
                                                
Bild 3:  Str"-Faktoren                   	
                                                
                                                                                        
Ablauf                                     	
                                        
Als Blackbox fr den Anwender kann man sich den Fleischwolf in Bild 1 
vorstellen. Am Eingabe werden oben drei verschiedene Zahlen fr X, Y, 	
und Z eingeworfen. Dann whlt man mit den beiden Drehschaltern die           	
Betriebsart. Es sind dabei sechs Rechenfunktionen mglich. Das Verfahren           
ist iterativ und reduziert eine der Zahlen, Y oder Z, schrittweise zu Null. 	
Nun mu man also krftig kurbeln um sie klein zu bekommen. Danach kann 		
man unten die drei modifizierten Zahlen entnehmen.                               

Die genaue Wirkung der Berechnung zeigt die Funktionstabelle in Bild 2. 
Xo, Yo, Zo sind jeweils die Eingabewerte, whrend X, Y, Z das Ergebnis sind. 
Die Werte scheinen ziemlich willkrlich verknpft, und zudem geistern noch zwei 
krumme Konstanten herum. Auf den ersten Blick schaut das Ganze deshalb etwas 		
unbrauchbar komplex aus. Man kann aber die Parameter die man nicht bentigt 
durch geeignete Konstanten wie 0 oder 1.0 besetzen und das Ergebnis dadurch 
vereinfachen. Will man z.B. die Funktion b = sin(a), setzt man X=1, Y=0, Z=a. 
Dann berechnet man in der Einstellung ROT-TRIG und das Ergebnis erscheint in Y. 
Es ist allerdings noch mit Str"-Faktor K skaliert, also b =  Y/K. Die Division 
kann durch eine weitere Berechnung mit VECT-LIN ausgefhrt werden. Man wird 
diese Operation in vielen Fllen nicht explizit vornehmen. Sondern versuchen 	
sie in einem anderen Skalierungsfaktor im Anwendungsalgorithmus einzuarbeiten.

Viele der Funktionskombinationen sind auch in Anwendung sinnvoll nutzbar. 
Wenn z.B. einer Multiplikation eine Addition folgt, kann diese Kombination 
unmittelbar durchgefhrt werden. Man kann manche Algorithmen ( auch  FFT ) 
in ein fr CORDIC gnstiges Format zurechtbiegen. Der Wert der Konstanten K
ergibt sich aus einer Reihenbildung bei der die Teilglieder multipliziert 
werden ( Bild 3 ). Die Laufvariable i dieser Reihenbildung entspricht dabei 
der Laufvariable whrend der CORDIC-Berechnung. Der Wert der Konstanten
scheint damit von der Zahl der Iterationen whrend der Berechnung abhngig.
Ihr Wert ist aber bei i=15 schon vllig stabil, whrend die CORDIC-Berechnung 
mehr als 15 Iterationen bentigt. Es handelt sich also praktisch doch um 
Konstanten. In der Software selbst tauchen beide Konstanten nicht auf. Ihr 
Einflu mu im Anwendungsprogramm bercksichtigt werden. Die Funktion sign(x) 
in der Funktionstabelle kann hier nur den Wert +1 oder -1 annehmen und gibt 
an, welches Vorzeichen die Wurzel bernimmt. Alle trigonometrischen Ein- und 
Ausgaben verwenden die Einheit rad.

Implementierung


Fr die Programmierung des Algorithmus mu als erstes die Wortbreite bestimmt 
werden. Die Festlegung der Position des Dezimalpunkts betrifft nicht das 
Programm direkt, sondern nur  die Tabellen und kann  damit abgestimmt auf 
eine bestimmte Anwendung spter erfolgen.

      Bild 2:
      Funktions-
      tabelle



Als Wortbreite werden manchmal 16 oder 24 Bit gewhlt, doch ergeben sich 
hier Probleme durch die geringe Auflsung und die Gefahr des berlaufs. 
32 bis 64 Bit haben als Nachteil hohen Speicherverbrauch der Tabellen und 
die lngere Rechenzeit. Hier wurde 32 Bit gewhlt, weil diese Wortbreite 
in Hochsprachen blich ist. ( Listing CORDIC ) Da Vorzeichen bentigt wird, 
kommt als Zahlenformat 2er-Komplement zur Anwendung. Die Geschwindigkeit 
hngt von der Zahl der Iterationen ab. Diese ist proportional zur Wortbreite. 
32 Bit z.B. bentigen maximal 32+1 Iterationen. In der praktischen Realisierung
wird man 15 Iterationen mindestens durchfhren. Die braucht es ja schon damit 
die Konstante K korrekt vorgegeben wurde. Dann wird man jedoch nach jedem 
Schritt prfen, ob der Parameter Y oder Z, der eliminiert werden soll, schon 
Null geworden ist. Wenn ja, bricht man ab. Meist erfolgt die Konvergenz 
nmlich schon etwas frher. Manchmal bleibt allerdings ein geringer Rest 
stehen der nicht mehr weiter eliminiert werden kann.          

Die Iterationen werden in FORTH ber eine uere DO...LOOP-Schleife mit 
eventuell verfrhtem Abbruch durch LEAVE realisiert. In Bild 4 ist diese 
Schleife am Beispiel Rotation dargestellt. Im Vektor-Modus wrde nicht Z 
sondern Y auf Abbruch geprft. Den Kern der Schleife zeigt Bild 5. Die 
Berechnung wurde in die Flle A und B unterteilt, was in FORTH einer  IF...   
ELSE...THEN-Verzweigung entspricht. Der Unterschied zwischen Programmteil 
A und B besteht dabei nur in der Vertauschung von Addition und Subtraktion. 
In der Literatur findet man hier deshalb oft eine programmierbare Multiplikation 
mit +1 oder -1. Als arithmetische Gleichung effizient, allerdings in Software 
umstndlich.     
                                      	
Bild 4: Hauptschleife der Rotation			                                                                                        

Ob Block A oder B auszufhren ist, hngt von der Betriebsart ab. Im einen Fall 
wird geprft, ob die Vorzeichen identisch sind. Im anderen, ob Z negativ ist. 
Wie man in den A und B-Blcken sieht, braucht man an arithmetischen Funktionen 
also Addition und Subtraktion. Sowie Shiftfunktionen. Etwas anderes ist die 
Multiplikation mit 2^-I nmlich nicht. I = -1 bedeuted um ein Bit nach links
schieben". I = 0 bedeuted  berhauptnicht schieben". I = 1 bis N bedeuted
um I-Bit nach rechts schieben". Bei 2er-Komplementzahlen mu man in letzterem 
Fall links jeweils eine Kopie des Vorzeichenbits einziehen (Bild 5).

In Hochsprache fhrt man diese Shifts am einfachsten Bit fr Bit in einer 
Schleife aus. Da  i  abhngig von der Wortbreite Werte von typisch 32 
erreichen kann, ist das jedoch langsam. Diese Teilfunktion sollte deshalb in
Assembler optimiert programmiert werden. Man zerlegt sie in zwei Schritte: 
Umspeichern von Bytes und Schieben von Bits. Auf einer 8 Bit CPU teilt man 
dazu i durch 8. Der Integeranteil des Ergebnisses kann mit 32 Bit Wortbreite 
bei 0 - 3 liegen. Das ist die Zahl der 8-fach-Shifts die man durch Umspeichern 
von Bytes ausfhren kann. Auch hier mu der ursprngliche Wert des obersten 
Bits beachted werden: ist es 0 wird ein 00- Byte eingezogen.

Bild 5: Shifts                                             				
                                                                                	
Ist es 1 ein FF-Byte. Der Rest der Division von 0 - 7 mu anschlieend 
konventionell durch bitweises Schieben bewegt werden.
Die Arctan- und Artanh-Werte werden in einer Tabelle abgelegt. Deren Umfang 
hlt sich in Grenzen. Bei 32 Bit sind etwas mehr als 128 Byte fllig, da 
jede Zahl 4 Byte belegt und fr jedes i ein Tabellenwert vorhanden sein 
mu. Fr 64 Bit sind es dann allerdings schon 0,5k Byte. Die Werte der 
Laufvariable I unterscheiden sich bei alle drei Varianten. Das gilt 
besonders fr die hyperbolische Funktion wo der Ablauf stellenweise 
Schluckauf hat. Bei 4, 13, 40, 121 mu die Iteration wiederholt werden. 
In Hochsprache empfiehlt sich eine Tabelle. Da es sich jedoch nur um 
4 Werte handelt, kann man die Ausnahmen in Assembler auch durch Vergleich
aussortieren. Den Shift der Konstanten 1.0 in Lineare Funktionen" ist im 
Beispielprogramm noch nicht optimiert. Entweder man legt auch hier eine 
Tabelle an. Oder man benutzt ein eigenes Register, das mit 1.0 initalisiert 
wird. Dann wird der Wert jeweils um ein ein Bit links oder rechts geschoben 
und zwischengespeichert. Letzere Methode verbraucht weniger Speicher als 
eine Tabelle und ist nur unwesentlich langsamer.         
                                      	
Konvergenz & berlauf                      	
                                      	
                                      	
Bei Festkommaformat ist man nicht auf Integerzahlen beschrnkt, es sind auch 
Zahlen kleiner Null darstellbar. Der Dezimalpunkt ist zwar  fest", aber er 
ist nicht rechts von Bit 0 einbetoniert, sondern seine Position kann und 	
vom Anwender passend fr die Anwendung skaliert werden. Legt man ihn zu weit 
nach rechts verliert man an Auflsung. Legt man ihn zu weit nach links kann 
man keine groen Zahlen darstellen. Insbesondere besteht das fr Festkomma 
typische Problem des berlaufs, wenn eine zu groe Zahlen den darstellbaren 
Zahlenbereich whrend der Berechnung berschreited. Man mu deshalb anhand 
der Ausgangswerte prfen, ob das Ergebnis berhaupt darstellbar ist. Bei 
groen Ausgangswerten kann auch whrend der Berechnung ein berlauf eintreten, 	
selbst wenn der Endwert darstellbar gewesen wre. Das gilt besonders fr       	
die beiden linearen Funktionen. Ein Sicherheitsfaktor von 2 - 3 ist meist
ausreichend.
Zustzlich gibt es  das fr CORDIC spezifische Problem, da die Reihe nicht 
konvergiert. D.h. der Parameter Y oder Z reduziert sich nicht zu Null. Auch 
das erfordert eine Beschrnkung des Eingangswertebereichs wie in Bild 7 
dargestellt. Whrend die Konvergenzgrenzen weitgehend unabhngig von der 
gewhlten Implementierung sind, sind die berlaufgrenzen durchwegs von ihr 
abhngig. Im vorliegenden Beispiel wurde als Wortbreite 32 Bit gewhlt und 
der Wert 1,0 auf 333000000d skaliert. Damit kann man Werte bis etwa 6,45 
darstellen. Hierauf beziehen sich die Angaben in Tabelle 1 fr berlauf 
und Auflsung. Die Grenzen werden als Betrag angegeben knnen zwischen den 
negativen und positiven Werten  liegen.


Programm

Das Listing zeigt ein einfaches Programm in nanoFORTH. Die Befehle fr den 
Anwender sind ROT-TRIG bis VECT-HYP und befinden sich am Ende des Listings. 
Die Daten werden 32 Bit breit auf dem Stack bergeben. Die Befehle 0.0, 0.5, 
1.0 erzeugen dafr geeignet skalierte Zahlen zum Testen. VECT-  und ROT-  
fhren die Hauptschleife aus  Bild 4 aus. STOP? prft ob ein vorzeitiger 
Abbruch mglich ist. CORE verzweigt auf die Kerne in LIN-CORE, TRIG-CORE, 
HYP-CORE.             


Bild 6:
Hauptschleife


  Bild 7:                                                         
  Funktionen.                                                     
  Eingerahmt jeweils der Konvergenzbereich, soweit er beschrnkt ist                                                  

                    
Diese berechnen die Funktion aus Bild 5. Die Laufvariable i wird dabei 
jeweils aus der Tabelle J entnommen. Die Schiebefunktion wird anhand ihr     
von (SHIFT) ausgefhrt. (ARCTN) und  (ARCTNH) greifen auf die jeweils        
entsprechende Tabelle zu. Zum Testen ist es sinnvoll sich bei jeder 
Iteration den Wert von J, X, Y und Z auf dem Bildschirm auszudrucken. 
Dafr ist der Befehl PRINT vorgesehen.                

Diese simple Version in Hochsprache braucht pro CORDIC-Berechnung etwa 
90msec auf einem 2,5MHz-Mitsubishi-6502. Die auf Geschwindigkeit optimierte 
Assemblerversion (ASMCORD.F65) fr die es als Prototyp diente ist um den 
Faktor 10 schneller. Beide verbrauchen jeweils  etwa 2kByte Speicher.
                                              
                                                                    
Tabelle 1:


Erlaubte Eingangswerte fr X,Y,Z fr das gewhlte Format

ROT-TRIG
Fr Konvergenz mu Z kleiner 1,74 sein. Damit deckt man zumindest einen 
PI/2-Bereich voll ab. X und Y haben keine Auswirkung auf Konvergenz, knnen 
aber zu berlauf fhren. Sie sollten hier beide deutlich kleiner 2,77 sein 
oder geeignet kombiniert werden. 
Auflsung: 5-6 Stellen.

ROT-LIN
Fr Konvergenz mu Z kleiner 4,0 sein. X und Y haben keine Auswirkung auf 
Konvergenz, knnen aber zu berlauf fhren.
Auflsung: 7-8 Stellen.

ROT-HYP
Fr Konvergenz mu Z kleiner 1,13 sein. X und Y haben keine Auswirkung auf 
Konvergenz, knnen aber zu berlauf fhren. Sie sollten hier deutlich kleiner
2,50 sein, oder geeignet kombiniert werden.
Auflsung: 5-6 Stellen.


VECT-TRIG
Scheint keine Konvergenzprobleme zu haben. X und Y knnen aber zu berlauf 
fhren. Sie sollten gemeinsam deutlich kleiner 2,8 bleiben. Auch wenn sie
geeignet kombiniert werden, kann keiner der beiden Werte grer als 3,9 sein. 
Auflsung: 6-7 Stellen.


 VECT-LIN
Fr Konvergenz sollte Y/X kleiner 4,0 sein. Kombinationen von kleinem Zhler 
und groem Nenner fhren whrend der Berechnung zu berlauf. Die Grenze liegt 
ungefhr bei 4/5, 2/4, 0,75/3,5. Ab Y/3 gibt es kaum noch Probleme. 
Auflsung: 7-8 Stellen.

 VECT-HYP
Fr Konvergenz mu Y/X kleiner
0,81 sein.
Auflsung: 6-7 Stellen.              		

5

Mehr zu CORDIC

Durch geschickte Anwendung bekannter Formeln kann man noch weitere Funktionen 
einfach berechnen:

Die besonders ntzliche Umrechnung von polaren auf kartesische Koordinaten 
wird von CORDIC-Funktionen direkt unter sttzt:


[1] Volder J.E. The CORDIC Trigonometric Computing Technique"
     IRE-EC 1959 S.330-334               
[2] Walther J.S. A unified algorithm for  elementary functions"
     Spring Joint Computer Conference (SJCC) 1971 S.379-385
                                              
CORDIC ist ein Nachdruck aus ELRAD 3/1995  Zahlenmhle" und wurde mit Hilfe von
Unterlagen erstellt die Bernd Beuster und Klaus Kohl zur Verfgung stellten.


***********************************************************

Listings fuer Heft 3, "Embedded"

***********************************************************
CORDIC

<| \ CORDIC

\ include:
U/MOD U/ U* D+ D- 2OVER DNEGATE SDD. D2/ D2* D@ D!

DECIMAL

2   ZVARIABLE M
4   ZVARIABLE X               \ X      X-HW-LB  Order in Memory
                              \ X 1+   X-HW-HB
                              \ X 2+   X-LW-LB
                              \ X 3 +  X-LW-HB
4   ZVARIABLE Y
4   ZVARIABLE Z
35  CONSTANT  ITS             \ Iterations

HEX

: STOP?                       \ ( Addr  ---  )
D@ OR LNOT I FFF0 AND LAND IF LEAVE THEN ;

: (SHFT)                      \ ( D1 #  --- D2 )
DUP                           \ # = 0 : no Shift
IF 1 DO D2/ LOOP
ELSE DROP
THEN ;

: PRINT                       \ ( J  ---  )
CR  CH.  X D@ SDD. SPACE Y D@ SDD. SPACE Z D@ SDD. ;

\ ----------------------------------------------------------
\ Trigonometric

DECIMAL

TABLE ARCTN-TAB     \ Scaled to 1,0 = 333E+6 (dez)
                       3990 , 48948 , (   0 ) 2355 , 57374 , (  +1 )
1244 , 51111 , (  +2 )  631 , 56997 , (  +3 )  317 , 10552 , (  +4 )
 158 , 48177 , (  +5 )   79 , 25358 , (  +6 )   39 , 45606 , (  +7 )
  19 , 55591 , (  +8 )    9 , 60566 , (  +9 )    4 , 63051 , ( +10 )
   2 , 31526 , ( +11 )    1 , 15763 , ( +12 )    0 , 40649 , ( +13 )
   0 , 20325 , ( +14 )    0 , 10162 , ( +15 )    0 ,  5081 , ( +16 )
   0 ,  2541 , ( +17 )    0 ,  1270 , ( +18 )    0 ,   635 , ( +19 )
   0 ,   318 , ( +20 )    0 ,   159 , ( +21 )    0 ,    79 , ( +22 )
   0 ,    40 , ( +23 )    0 ,    20 , ( +24 )    0 ,    10 , ( +25 )
   0 ,     5 , ( +26 )    0 ,     2 , ( +27 )    0 ,     1 , ( +28 )
   0 ,     0 , ( +29 )    0 ,     0 , ( +30 )    0 ,     0 , ( +31 )
   0 ,     0 , ( +32 )    0 ,     0 , ( +33 )    0 ,     0 , ( +34 )
   0 ,     0 , ( +35 )

TABLE J
00 C, 01 C, 02 C, 03 C, 04 C, 05 C, 06 C, 07 C, 08 C, 09 C,
10 C, 11 C, 12 C, 13 C, 14 C, 15 C, 16 C, 17 C, 18 C, 19 C,
20 C, 21 C, 22 C, 23 C, 24 C, 25 C, 26 C, 27 C, 28 C, 29 C,
30 C, 31 C, 32 C, 33 C, 34 C, 35 C, 36 C, 37 C, 38 C, 39 C,

: (SHIFT)                     \ ( D1 --- D2 )
I J + C@ (SHFT) ;

: (ARCTN)                     \ (  --- D1 )
I J + C@ 1<SHIFT 1<SHIFT ARCTN-TAB + D@ ;

: TRIG-CORE                   \ ( F1 ---  )
I J + C@ PRINT \ include during test
IF X D@ Y D@ (SHIFT) D+
   Y D@ X D@ (SHIFT) D- Y D!
                        X D!
   Z D@      (ARCTN) D+ Z D!
ELSE
   X D@ Y D@ (SHIFT) D-
   Y D@ X D@ (SHIFT) D+ Y D!
                        X D!
   Z D@      (ARCTN) D- Z D!
THEN ;

\ ----------------------------------------------------------
\ Linear

TABLE J
HEX  80 C, DECIMAL
00 C, 01 C, 02 C, 03 C, 04 C, 05 C, 06 C, 07 C, 08 C, 09 C,
10 C, 11 C, 12 C, 13 C, 14 C, 15 C, 16 C, 17 C, 18 C, 19 C,
20 C, 21 C, 22 C, 23 C, 24 C, 25 C, 26 C, 27 C, 28 C, 29 C,
30 C, 31 C, 32 C, 33 C, 34 C, 35 C, 36 C, 37 C, 38 C, 39 C,

HEX

: (SHIFT)                     \ ( D1  --- D2 )
I J + C@
DUP 80 AND                    \ # = -1 : D2*
IF DROP D2*
ELSE (SHFT) THEN ;

DECIMAL

: LIN-CORE                   \ ( F1 ---  )
I J + C@ PRINT  \ include during test
IF
   Y D@ X D@ (SHIFT) D- Y D!
   Z D@ 11584 5081 (SHIFT) D+ Z D!
ELSE
   Y D@ X D@ (SHIFT) D+ Y D!
   Z D@ 11584 5081 (SHIFT) D- Z D!
THEN ;

\ ----------------------------------------------------------
\ Hyperbolic

DECIMAL

TABLE ARCTNH-TAB     \ Scaled to 1,0 = 333E+6 (dez)
2791 , 07970 , ( + 1 ) 1297 , 52274 , ( + 2 ) 638 , 31884 , ( + 3 )
 317 , 64751 , ( + 4 )  158 , 54951 , ( + 5 )  79 , 26204 , ( + 6 )
  39 , 45711 , ( + 7 )   19 , 55604 , ( + 8 )   9 , 60567 , ( + 9 )
   4 , 63051 , ( +10 )    2 , 31526 , ( +11 )   1 , 15763 , ( +12 )
   0 , 40649 , ( +13 )    0 , 20325 , ( +14 )   0 , 10162 , ( +15 )
   0 ,  5081 , ( +16 )    0 ,  2541 , ( +17 )   0 ,  1270 , ( +18 )
   0 ,   635 , ( +19 )    0 ,   318 , ( +20 )   0 ,   159 , ( +21 )
   0 ,    79 , ( +22 )    0 ,    40 , ( +23 )   0 ,    20 , ( +24 )
   0 ,    10 , ( +25 )    0 ,     5 , ( +26 )   0 ,     2 , ( +27 )
   0 ,     1 , ( +28 )    0 ,     0 , ( +29 )   0 ,     0 , ( +30 )
   0 ,     0 , ( +31 )    0 ,     0 , ( +32 )   0 ,     0 , ( +33 )
   0 ,     0 , ( +34 )    0 ,     0 , ( +35 )

TABLE J
01 C, 02 C, 03 C, 04 C, 04 C, 05 C, 06 C, 07 C, 08 C, 09 C,
10 C, 11 C, 12 C, 13 C, 13 C, 14 C, 15 C, 16 C, 17 C, 18 C,
19 C, 20 C, 21 C, 22 C, 23 C, 24 C, 25 C, 26 C, 27 C, 28 C,
29 C, 30 C, 31 C, 32 C, 33 C, 34 C, 35 C, 36 C, 37 C, 38 C, 39 C,

: (SHIFT)                     \ ( UN1  --- UN2 )
I J + C@ (SHFT) ;

: (ARCTNH)                    \ (  --- D1 )
I J + C@ 1- 1<SHIFT 1<SHIFT ARCTNH-TAB + D@ ;

: HYP-CORE                   \ ( F1 ---  )
I J + C@ PRINT  \ include during test
IF X D@ Y D@ (SHIFT) D-
   Y D@ X D@ (SHIFT) D- Y D!
                        X D!
   Z D@     (ARCTNH) D+ Z D!
ELSE
   X D@ Y D@ (SHIFT) D+
   Y D@ X D@ (SHIFT) D+ Y D!
                        X D!
   Z D@     (ARCTNH) D- Z D!
THEN ;

\ ----------------------------------------------------------

HEX

: CORE                        \ ( F1  ---  F1 )
M @ DUP
IF 8000 AND IF HYP-CORE ELSE TRIG-CORE THEN
ELSE DROP LIN-CORE
THEN ;

: VECT-                       \ ( X Y Z C1 --- X Y Z )
M ! Z D! Y D! X D!
ITS 0 DO
  Y STOP?
  X @ Y @ XOR 8000 AND LNOT CORE
LOOP
\ X D@ Y D@ Z D@  \ exclude during test
;

: ROT-                        \ ( X Y Z C1 --- X Y Z )
M ! Z D! Y D! X D!
ITS 0 DO
  Z STOP?
  Z @ 8000 AND CORE
LOOP
\ X D@ Y D@ Z D@   \ exclude during test
;

HEX

\  X, Y, Z, are 32-Bit signed, scaled to 1.0 = 333000000 (dez)

: ROT-TRIG    0001 ROT- ;     \ ( X Y Z --- X Y Z )
: ROT-LIN     0000 ROT- ;     \ ( X Y Z --- X Y Z )
: ROT-HYP     FFFF ROT- ;     \ ( X Y Z --- X Y Z )
: VECT-TRIG   0001 VECT- ;    \ ( X Y Z --- X Y Z )
: VECT-LIN    0000 VECT- ;    \ ( X Y Z --- X Y Z )
: VECT-HYP    FFFF VECT- ;    \ ( X Y Z --- X Y Z )

DECIMAL

: 0.0          0     0 ;   \ Test only
: 0.5      38560  2540 ;   \ Test only
: 1.0      11584  5081 ;   \ Test only

|>
<|

\ dummy-run

: X
CR ." 1.0 0.5 0.0 ROT-TRIG"  1.0 0.5 0.0 ROT-TRIG
CR ." 1.0 0.5 0.0 ROT-LIN"   1.0 0.5 0.0 ROT-LIN
CR ." 1.0 0.5 0.0 ROT-HYP"   1.0 0.5 0.0 ROT-HYP
CR ." 1.0 0.5 0.0 VECT-TRIG" 1.0 0.5 0.0 VECT-TRIG
CR ." 1.0 0.5 0.0 VECT-LIN"  1.0 0.5 0.0 VECT-LIN
CR ." 1.0 0.5 0.0 VECT-HYP"  1.0 0.5 0.0 VECT-HYP ;

\ more numbers for testing

DECIMAL

: - DNEGATE ;

: 0.0          0     0 ;
: 0.005    26600    25 ;
: 0.01     53200    50 ;
: 0.05      3856   254 ;
: 0.1       7712   508 ;
: 0.25     19280  1270 ;
: 0.5      38560  2540 ;
: 0.75     57840  3810 ;
: 1.0      11584  5081 ;
: 2.5      61728 12702 ;
: 5.0      57920 25405 ;
: PI/4     48948  3990 ;
: PI/2     32361  7981 ;
: PI       64722 15962 ;
: 2*PI     63907 31925 ;

|>

<| \ ASMCORD.F65   Assembler 6502

\ include:
\ D+ D- DNEGATE 2OVER SDD.

HEX

4     ZVARIABLE X
4     ZVARIABLE Y      \  4 +
4     ZVARIABLE Z      \  8 +
4     ZVARIABLE AKKU   \  C +
4     ZVARIABLE CONST  \ 10 +

3     ZVARIABLE A-VECT
3     ZVARIABLE B-VECT

1     ZVARIABLE 04/13/40
1     ZVARIABLE YSAVE
1     ZVARIABLE PRINT&

: R@  DUP @ SWAP 2+ @ ;       \ ( Addr --- D1 )
: R!  SWAP OVER 2+ ! ! ;      \ ( D1 Addr --- )

: PRINT                       \ (  ---  )  I = Y
[ PRINT& STY, XSAVE LDX, ]
CR PRINT& C@ CH. SPACE X R@ SDD. SPACE Y R@ SDD. SPACE Z R@ SDD.
[ PRINT& LDY, ] ;

:CODE LOAD-AKKU               \ AKKU indirect via X-Offset
                              \ X = 0 , 4 , 8 , C , 10
       X         ,X LDA,
       AKKU         STA,
       X 1+      ,X LDA,
       AKKU 1+      STA,
       X 2+      ,X LDA,
       AKKU 2+      STA,
       X 3 +     ,X LDA,
       AKKU 3 +     STA,
                    RTS,
CODE;

:CODE LOAD-SHIFT              \ shift CONST one time left or right
                              \ according to Y
                              \ and copy it to AKKU
                    TYA,
2 $                 BPL,      \ MSB=0 : branch
       CONST        ASL,      \ Y = FF: shift 1x left
       CONST 1+     ROL,
       CONST 2+     ROL,
       CONST 3 +    ROL,
1 $                 JMP,
2 $:   CONST 3 +    LDA,      \ Get Sign into Carry
       A.           ASL,
       CONST 3 +    ROR,      \ Shift 1x right
       CONST 2+     ROR,
       CONST 1+     ROR,
       CONST        ROR,
1 $:   CONST X - #. LDX,      \ Move CONST to AKKU
     ' LOAD-AKKU    JSR,
                    RTS,
CODE;

DECIMAL

TABLE ATAN      \ Scaled to 1,0 = 333E+6 (dez)
                       3990 , 48948 , (   0 ) 2355 , 57374 , (  +1 )
1244 , 51111 , (  +2 )  631 , 56997 , (  +3 )  317 , 10552 , (  +4 )
 158 , 48177 , (  +5 )   79 , 25358 , (  +6 )   39 , 45606 , (  +7 )
  19 , 55591 , (  +8 )    9 , 60566 , (  +9 )    4 , 63051 , ( +10 )
   2 , 31526 , ( +11 )    1 , 15763 , ( +12 )    0 , 40649 , ( +13 )
   0 , 20325 , ( +14 )    0 , 10162 , ( +15 )    0 ,  5081 , ( +16 )
   0 ,  2541 , ( +17 )    0 ,  1270 , ( +18 )    0 ,   635 , ( +19 )
   0 ,   318 , ( +20 )    0 ,   159 , ( +21 )    0 ,    79 , ( +22 )
   0 ,    40 , ( +23 )    0 ,    20 , ( +24 )    0 ,    10 , ( +25 )
   0 ,     5 , ( +26 )    0 ,     2 , ( +27 )    0 ,     1 , ( +28 )
   0 ,     0 , ( +29 )    0 ,     0 , ( +30 )    0 ,     0 , ( +31 )
   0 ,     0 , ( +32 )    0 ,     0 , ( +33 )    0 ,     0 , ( +34 )
   0 ,     0 , ( +35 )

HEX

:CODE LOAD-ATAN               \ AKKU = ATAN-Tab via Y*4
                    TYA,
                 A. ASL,      \ X = Y * 4
                 A. ASL,
                    TAX,
       ATAN      ,X LDA,      \ HW LB
       AKKU 2+      STA,
       ATAN 1+   ,X LDA,      \    HB
       AKKU 3 +     STA,
       ATAN 2+   ,X LDA,      \ LW LB
       AKKU         STA,
       ATAN 3 +  ,X LDA,      \    HB
       AKKU 1+      STA,
                    RTS,
CODE;

DECIMAL

TABLE ATANH     \ Scaled to 1,0 = 333E+6 (dez)
   0 ,     0 , (   0 Dummy )
2791 , 07970 , ( + 1 ) 1297 , 52274 , ( + 2 ) 638 , 31884 , ( + 3 )
 317 , 64751 , ( + 4 )  158 , 54951 , ( + 5 )  79 , 26204 , ( + 6 )
  39 , 45711 , ( + 7 )   19 , 55604 , ( + 8 )   9 , 60567 , ( + 9 )
   4 , 63051 , ( +10 )    2 , 31526 , ( +11 )   1 , 15763 , ( +12 )
   0 , 40649 , ( +13 )    0 , 20325 , ( +14 )   0 , 10162 , ( +15 )
   0 ,  5081 , ( +16 )    0 ,  2541 , ( +17 )   0 ,  1270 , ( +18 )
   0 ,   635 , ( +19 )    0 ,   318 , ( +20 )   0 ,   159 , ( +21 )
   0 ,    79 , ( +22 )    0 ,    40 , ( +23 )   0 ,    20 , ( +24 )
   0 ,    10 , ( +25 )    0 ,     5 , ( +26 )   0 ,     2 , ( +27 )
   0 ,     1 , ( +28 )    0 ,     0 , ( +29 )   0 ,     0 , ( +30 )
   0 ,     0 , ( +31 )    0 ,     0 , ( +32 )   0 ,     0 , ( +33 )
   0 ,     0 , ( +34 )    0 ,     0 , ( +35 )

HEX

:CODE LOAD-ATANH              \ AKKU = ATANH-Tab via Y*4
                    TYA,
                 A. ASL,      \ X = Y * 4
                 A. ASL,
                    TAX,
       ATANH     ,X LDA,      \ HW LB
       AKKU 2+      STA,
       ATANH 1+  ,X LDA,      \    HB
       AKKU 3 +     STA,
       ATANH 2+  ,X LDA,      \ LW LB
       AKKU         STA,
       ATANH 3 + ,X LDA,      \    HB
       AKKU 1+      STA,
                    RTS,
CODE;

:CODE ADD-AKKU                \ Reg+1 = Reg0 + Akku
                              \ Akku  = Reg0
                              \ X = Pointer to Reg
                              \ X = 0 , 4 , 8 , C , 10
       YSAVE        STY,
                    CLC,
       X         ,X LDA,
                    TAY,
       AKKU         ADC,
       X         ,X STA,
       AKKU         STY,
       X 1+      ,X LDA,
                    TAY,
       AKKU 1+      ADC,
       X 1+      ,X STA,
       AKKU 1+      STY,
       X 2+      ,X LDA,
                    TAY,
       AKKU 2+      ADC,
       X 2+      ,X STA,
       AKKU 2+      STY,
       X 3 +     ,X LDA,
                    TAY,
       AKKU 3 +     ADC,
       X 3 +     ,X STA,
       AKKU 3 +     STY,
       YSAVE        LDY,
                    RTS,
CODE;

:CODE SUB-AKKU                \ Reg+1 = Reg0 - Akku
                              \ Akku = Reg0
                              \ X = Pointer to Reg
                              \ X = 0 , 4 , 8 , C , 10
       YSAVE        STY,
                    SEC,
       X         ,X LDA,
                    TAY,
       AKKU         SBC,
       X         ,X STA,
       AKKU         STY,
       X 1+      ,X LDA,
                    TAY,
       AKKU 1+      SBC,
       X 1+      ,X STA,
       AKKU 1+      STY,
       X 2+      ,X LDA,
                    TAY,
       AKKU 2+      SBC,
       X 2+      ,X STA,
       AKKU 2+      STY,
       X 3 +     ,X LDA,
                    TAY,
       AKKU 3 +     SBC,
       X 3 +     ,X STA,
       AKKU 3 +     STY,
       YSAVE        LDY,
                    RTS,
CODE;

:CODE SHIFT                   \ Akku = shifted Akku
                              \ Y = count and direction
                    TYA,
9 $                 BEQ,      \ Y = 0 : Mop
3 $                 BPL,      \ MSB = 0
       AKKU         ASL,      \ If Y = FF shift 1x left
       AKKU 1+      ROL,
       AKKU 2+      ROL,
       AKKU 3 +     ROL,
9 $:                RTS,
3 $:  A. LSR, A. LSR, A. LSR,
                    PHA,
       00        #. LDA,
       AKKU 3 +     LDX,
4 $                 BPL,
       FF        #. LDA,
4 $:                TAX,      \ X = FF or 00 according to Sign
                    PLA,
5 $                 BEQ,      \ no Byteshifts
       01        #. CMP,
6 $                 BNE,
       AKKU 1+  LDA, AKKU     STA,
       AKKU 2+  LDA, AKKU 1+  STA,
       AKKU 3 + LDA, AKKU 2+  STA,
                     AKKU 3 + STX,
5 $                 JMP,
6 $:   02        #. CMP,
7 $                 BNE,
       AKKU 2+  LDA, AKKU     STA,
       AKKU 3 + LDA, AKKU 1+  STA,
                     AKKU 2 + STX,
                     AKKU 3 + STX,
5 $                 JMP,
7 $:   AKKU 3 + LDA, AKKU     STA,
                     AKKU 1+  STX,
                     AKKU 2+  STX,
                     AKKU 3 + STX,
5 $:                TYA,      \ Now Bitshifts
       07        #. AND,
2 $                 BEQ,      \ No Bitshifts
                    TAX,
1 $:   AKKU 3 +     LDA,      \ Get Sign into Carry
       A.           ASL,
       AKKU 3 +     ROR,      \ 1 - 7 ArithmeticShiftsRight
       AKKU 2+      ROR,
       AKKU 1+      ROR,
       AKKU         ROR,
                    DEX,
1 $                 BNE,
2 $:                RTS,
CODE;

:CODE A-TRIG                  \ Init Y
       00        #. LDY,
                    RTS,
CODE;

:CODE A-LIN                   \ Init Y
       FF        #. LDY,
                    RTS,
CODE;

:CODE A-HYP                   \ Init Y
       01        #. LDY,
       04/13/40     STY,
                    RTS,
CODE;

:CODE B-TRIG
1 $                 BPL,
      Y X - #. LDX, ' LOAD-AKKU JSR,
                    '     SHIFT JSR,
      X X - #. LDX, '  ADD-AKKU JSR,
                    '     SHIFT JSR,
      Y X - #. LDX, '  SUB-AKKU JSR,
                    ' LOAD-ATAN JSR,
      Z X - #. LDX, '  ADD-AKKU JSR,
2 $                 JMP,
1 $:  Y X - #. LDX, ' LOAD-AKKU JSR,
                    '     SHIFT JSR,
      X X - #. LDX, '  SUB-AKKU JSR,
                    '     SHIFT JSR,
      Y X - #. LDX, '  ADD-AKKU JSR,
                    ' LOAD-ATAN JSR,
      Z X - #. LDX, '  SUB-AKKU JSR,
2 $:                INY,
                    RTS,
CODE;

:CODE B-LIN
1 $                 BPL,
      X X - #. LDX, '  LOAD-AKKU JSR,
                    '      SHIFT JSR,
      Y X - #. LDX, '   SUB-AKKU JSR,
                    ' LOAD-SHIFT JSR,
      Z X - #. LDX, '   ADD-AKKU JSR,
2 $                 JMP,
1 $:  X X - #. LDX, '  LOAD-AKKU JSR,
                    '      SHIFT JSR,
      Y X - #. LDX, '   ADD-AKKU JSR,
                    ' LOAD-SHIFT JSR,
      Z X - #. LDX, '   SUB-AKKU JSR,
2 $:                INY,
                    RTS,
CODE;

:CODE B-HYP
1 $                 BPL,
      Y X - #. LDX, '  LOAD-AKKU JSR,
                    '      SHIFT JSR,
      X X - #. LDX, '   SUB-AKKU JSR,
                    '      SHIFT JSR,
      Y X - #. LDX, '   SUB-AKKU JSR,
                    ' LOAD-ATANH JSR,
      Z X - #. LDX, '   ADD-AKKU JSR,
2 $                 JMP,
1 $:  Y X - #. LDX, '  LOAD-AKKU JSR,
                    '      SHIFT JSR,
      X X - #. LDX, '   ADD-AKKU JSR,
                    '      SHIFT JSR,
      Y X - #. LDX, '   ADD-AKKU JSR,
                    ' LOAD-ATANH JSR,
      Z X - #. LDX, '   SUB-AKKU JSR,
2 $:   04        #. CPY,      \ 04
4 $                 BEQ,
3 $                 BCC,
       0D        #. CPY,      \ 13
4 $                 BEQ,
\ 3 $                 BCC,
\        28        #. CPY,      \ 40
\ 2 $                 BEQ,
3 $:                INY,
                    RTS,
4 $:   04/13/40     INC,      \ Invert Bit0
       04/13/40     LDA,
              01 #. AND,
3 $                 BNE,      \ Y unchanged if Bit0 is 0
                    RTS,
CODE;

:CODE VEKTOR
       XSAVE        STX,
       A-VECT       JSR,      \ Init Y,
2 $:
       ' PRINT      JSR,

       X 3 +        LDA,      \ Check flags of Y and X
       Y 3 +        EOR,
              80 #. EOR,      \ Invert MSB
       B-VECT       JSR,      \ Calculate and inc Y,
       20        #. CPY,      \ runs to 31
1 $                 BEQ,
       11        #. CPY,      \ -1 = FF is already incremented
2 $                 BCC,      \ Branch if Y, < 17
             04  #. LDX,
             00  #. LDA,
3 $:   Y 1-     ,X  ORA,      \ Check if Y = 0
                    DEX,
3 $                 BNE,
                    TAX,
2 $                 BNE,
1 $:
       ' PRINT      JSR,

       XSAVE        LDX,
                    RTS,
CODE;

:CODE ROTATE
       XSAVE        STX,
       A-VECT       JSR,      \ Init Y
2 $:
       ' PRINT      JSR,

       Z 3 +        LDA,
       B-VECT       JSR,      \ Calculate and inc Y
       20        #. CPY,      \ runs to 31
1 $                 BEQ,
       11        #. CPY,      \ -1 = FF is already incremented
2 $                 BCC,      \ Branch if Y < 17
             04  #. LDX,
             00  #. LDA,
3 $:   Z 1-     ,X  ORA,      \ Check if Z = 0
                    DEX,
3 $                 BNE,
                    TAX,
2 $                 BNE,
1 $:
       ' PRINT      JSR,

       XSAVE        LDX,
                    RTS,
CODE;

DECIMAL

: TRIG                                \ (  ---  )
[ ' A-TRIG CODE-LIT ] A-VECT 1+ !
[ ' B-TRIG CODE-LIT ] B-VECT 1+ ! ;

: LIN                                 \ (  ---  )
11584 CONST !  5081 CONST 2+ !
[ ' A-LIN CODE-LIT ] A-VECT 1+ !
[ ' B-LIN CODE-LIT ] B-VECT 1+ ! ;

: HYP                                 \ (  ---  )
[ ' A-HYP CODE-LIT ] A-VECT 1+ !
[ ' B-HYP CODE-LIT ] B-VECT 1+ ! ;

HEX

: INIT                        \ (  ---  )
4C A-VECT C!  4C B-VECT C! ;

: REG!                        \ ( X Y Z ---  )
INIT   Z R! Y R! X R! ;

: REG@                        \ (  ---  X Y Z )
\ X R@ Y R@ Z R@
;

: ROT-TRIG    REG!  TRIG  ROTATE  REG@ ;  \ ( X Y Z  --- X Y Z )
: ROT-LIN     REG!  LIN   ROTATE  REG@ ;
: ROT-HYP     REG!  HYP   ROTATE  REG@ ;
: VECT-TRIG   REG!  TRIG  VEKTOR  REG@ ;
: VECT-LIN    REG!  LIN   VEKTOR  REG@ ;
: VECT-HYP    REG!  HYP   VEKTOR  REG@ ;

DECIMAL

: 0.0          0     0 ;  \ Test only
: 0.5      38560  2540 ;
: 1.0      11584  5081 ;

|>

***********************************************************
Ende

