読者です 読者をやめる 読者になる 読者になる

にせねこメモ

はてなダイアリーがUTF-8じゃないので移ってきました。

TrueType命令で三角関数(sin, cos)を計算する

TrueType Instructionで遊ぶシリーズ。
初めてのTrueType命令: Windowsでは見えないフォントをつくる - にせねこメモ
フォントサイズに合わせて回転するフォントを作る(1) - にせねこメモ
PPEM・ポイントサイズを表示するフォント - にせねこメモ


TrueType Instructionを使って回転を計算したい。そうなると当然三角関数が使いたくなる。一方でTrueType Instruction Setには三角関数を計算する命令は入っていない。そのため自前で計算する必要がある。

方針

さて、自前で計算するにはどうすればいいだろうか?方法はいくつかある。
一つはMaclaurin展開(あるいはTaylor展開)によって計算するもの(たぶんこっちのが実装は楽)。
一つはCORDICというアルゴリズム(計算方法について調べてるときに教えてもらった)。
どちらも四則演算のみによって計算できる。


今回はCORDICというアルゴリズムを使ってみる。これは非負整数 i \in \mathbb{N}_{0}について \tan\theta_i = \frac{1}{2^i}となるような \theta_i=\mathrm{atan}\left(\frac{1}{2^i}\right)の値を事前にテーブルとして持っておいて、その値に四則演算をしていくことで、最終的な関数を得るというものらしい*1。テーブルとして持つ iの上限は必要とする精度によって決める。詳しくは次のサイトを参考にされたい。

関数電卓コラム 10/02/24 関数電卓のしくみ(CORDICアルゴルズム)

F26Dot6の精度

さて、TrueType命令で使える小数フォーマットはF26Dot6(以降Q6とも書く)、すなわち整数部が26ビットで小数部が6ビットである固定小数点数である。
Q6の値は整数と同様の扱いであるが、 2^6 = 64だけ下駄がはかされていると考えるとよい。すなわち、Q6を整数として解釈した数値を64で割ると実際の数値が求まる。
例えばQ6フォーマットで表される数値を整数で表現すると、Q6の1.0は整数の64、Q6の2.0は整数の128、Q6の0.75は整数の48、などとなる。

TrueType命令において、足し算(ADD[ ])・引き算(SUB[ ])は整数に対して定義されている。これは小数点の位置を変化させないため固定小数点数に対してもそのまま使える。
一方で掛け算(MUL[ ])や割り算(DIV[ ])はF26Dot6に対して定義されていて、計算結果もF26Dot6である。これは、実際には次のような計算が行われるとのことである。

A * B: (n1 * n2) / 64
A / B: (n1 * 64) / n2

n1, n2は整数として扱われ、基本的に整数同士の掛け算あるいは割り算と同様であるが、固定小数点数なので右端(LSB)から6ビットが小数部であることを考慮して、最終的な計算結果がF26Dot6となるように、適当なところで64で割ったり64を掛けたりして桁を調整している。64で割るのは6ビット分算術右シフトすることと同等で、64掛けるのは6bit分左シフトするのと同等であるが、TrueType命令にビットシフト演算はない。

ところで、A*Bの場合は計算結果を64で割っていて、A/Bの場合はAに64を掛けている。仕様に記載があるかもしれないし、または実装に依るのかもしれないが、32bit整数型で計算していた場合、A*Bの先頭6bitは桁あふれによって、またA/BにおけるAの先頭6ビットのデータについてもデータが失われた状態になりそうな気がする。そう考えると、乗除算で実質に使えるのは26ビット程度(20Dot6、つまり-524288.~524287.程度)になるかもしれない。大きな数値を扱う必要がある場合には問題になる可能性があるかもしれない。


それにしても、フォントの制御点を表すためとはいえ、小数部が6bitというのはかなり心もとない気がする。 2^6 = 0.015625であるから、10進数で0.01の細かさは表現できないことになる。さすがに \sin \cosを計算するのにこれだと心許ない気がするので、さらに6bit分下駄をはかせて20Dot12 (Q12)相当として計算していくことにする。乗除算を計算する際には上で示したようにF26Dot6用で計算されるため、出力される数値の小数点の位置を適宜調整する必要がでてくることに気を付けないといけない。

CORDICアルゴリズムの実装

定数のストア

まず使用する定数類をStorage Areaに保存して、プログラム内から読み出すことを考えてみる。
Storage Areaへの割り付けは次のようにする。定数には6ビット分(=64)だけ下駄をはかせている。

index value 備考
0 0x2D000  64\theta_0 = \mathrm{atan}\left(\frac{1}{2^0}\right)
1 0x1A90A  64\theta_1 = \mathrm{atan}\left(\frac{1}{2^1}\right)
2 0xE094  64\theta_2 = \mathrm{atan}\left(\frac{1}{2^2}\right)
3 0x7200  64\theta_3 = \mathrm{atan}\left(\frac{1}{2^3}\right)
4 0x3938  64\theta_4 = \mathrm{atan}\left(\frac{1}{2^4}\right)
5 0x1CA3  64\theta_5 = \mathrm{atan}\left(\frac{1}{2^5}\right)
6 0x0E52  64\theta_6 = \mathrm{atan}\left(\frac{1}{2^6}\right)
7 0x0729  64\theta_7 = \mathrm{atan}\left(\frac{1}{2^7}\right)
8 0x0394  64\theta_8 = \mathrm{atan}\left(\frac{1}{2^8}\right)
9 0x01CA  64\theta_9 = \mathrm{atan}\left(\frac{1}{2^9}\right)
10 0x00E5  64\theta_{10} = \mathrm{atan}\left(\frac{1}{2^{10}}\right)
11 0x0072  64\theta_{11} = \mathrm{atan}\left(\frac{1}{2^{11}}\right)
12 0x0039  64\theta_{12} = \mathrm{atan}\left(\frac{1}{2^{12}}\right)
13 0x1A59  r_{12}

定数なので'prep'でstoreしてしまおう。

ここで、PUSHWでは1ワード(16 bit)が符号拡張されるため、すなわち1ワードの最上位ビットが符号ビットとなるため、0x8000以上の数値はそのままPUSHWを使ってスタックに積むことはできない。例えば、

(上14ビット)*2^14 + (下14ビット)

みたいに値を分割してPUSHして後で合成してやらないといけない。

'prep'
PUSHB_1 /* store dest */
0
PUSHW_4
0x1000  /* 0x2D000 % 2^14 */
0x000B  /* 0x2D000 / 2^14 */
0x4000  /* 0x100.0 */
0x1000  /* 0x40.0 */
MUL
MUL
ADD
WS

PUSHB_1 /* store dest */
1
PUSHW_4
0x290A  /* 0x1A90A % 2^14 */
0x0006  /* 0x1A90A / 2^14 */
0x4000  /* 0x100.0 */
0x1000  /* 0x40.0 */
MUL
MUL
ADD
WS


PUSHB_1 /* store dest */
2
PUSHW_4
0x2094  /* 0xE094 % 2^14 */
0x0003  /* 0xE094 / 2^14 */
0x4000  /* 0x100.0 */
0x1000  /* 0x40.0 */
MUL
MUL
ADD
WS

PUSHB_1 /* store dest */
3
PUSHW_1
0x7200
WS

PUSHB_1 /* store dest */
4
PUSHW_1
0x3938
WS

PUSHB_1 /* store dest */
5
PUSHW_1
0x1CA3
WS

PUSHB_1 /* store dest */
6
PUSHW_1
0x0E52
WS

PUSHB_1 /* store dest */
7
PUSHW_1
0x0729
WS

PUSHB_1 /* store dest */
8
PUSHW_1
0x0394
WS

PUSHB_1 /* store dest */
9
PUSHW_1
0x01CA
WS

PUSHB_1 /* store dest */
10
PUSHW_1
0x00E5
WS

PUSHB_1 /* store dest */
11
PUSHW_1
0x0072
WS

PUSHB_1 /* store dest */
12
PUSHW_1
0x0039
WS

PUSHB_1 /* store dest */
13
PUSHW_1
0x1A59
WS

計算の実装

さて、CORDICアルゴリズムで実際に計算をする部分の実装に入る。
入力は角度{\theta[\mbox{°}]}(F26Dot6)、出力は 64\cdot\sin\theta, 64\cdot\cos\theta(F26Dot6)とする。
入力となる角度{\theta[\mbox{°}]} 0 \le \theta \le 90の範囲にあるものとする。

Pythonで書くとこんな感じになる。

import math

# constants 定数格納部分
theta = [math.atan(1.0 / (2**x)) * 180 / math.pi for x in range(13)]
rsq = [2.0]
for i in range(1,13):
    rsq.append(rsq[-1] + rsq[-1]/(2.0**(2*i)))
r = list(map(math.sqrt, rsq))

# calculate sin, cos 計算部分
deg = 30 #input angle 入力の角度 [0,90]
d = theta[0] # current temporary angle
x = 1.0
y = 1.0
for i in range(1,13):
    lastx = x
    lasty = y
    if d > deg:
        d = d - theta[i]
        x = lastx + lasty/(2**i)
        y = lasty - lastx/(2**i)
    else:
        d = d + theta[i]
        x = lastx - lasty/(2**i)
        y = lasty + lastx/(2**i)

mycos = x/r[12] #cos計算結果
mysin = y/r[12] #sin計算結果

#print("mycos:", mycos, ", mysin:", mysin)
#print("  cos:", math.cos(deg/180*math.pi), ",   sin:", math.sin(deg/180*math.pi))

定数格納はしたので、計算部分を実装する。

最初に入力 \thetaを与えて、それに対応する \sin\theta, \cos\thetaを計算するような次のプログラムをつくった。

PUSHW_1 1920 /* theta=30.0 */ /* input angle */



             /* ...|theta| */
PUSHW_1 4096 /* ...|theta|64.0| */
MUL          /* ...|64t| */ /* 64t = 64*theta, 更に6ビット下駄を履かす */

PUSHW_2      /* ...|64t|64*1.0|64*1.0| */ 
 4096 
 4096 
PUSHB_1 0    /* ...|64t|64*1.0|64*1.0|0| */
DUP          /* ...|64t|64*1.0|64*1.0|0|0| */
RS           /* ...|64t|64*1.0|64*1.0|0|64t0| *//*t0=theta_0=StorageArea[0]*/
PUSHB_1 64   /* ...|64t|64*1.0|64*1.0|0|64t0|1.0| */
SWAP         /* ...|64t|64*1.0|64*1.0|0|1.0|64t0| */
             /* ...|64t|64x=64*1.0|64y=64*1.0|i=0|2.0^i=1.0|ti=64t0| */
             /* i: counter,   2.0^i: 冪関数やビットシフト演算の代替*/
             /* ti: current angle */
/* L0: */
/* i = i+1, 2.0^i = 2.0^(i+1) に更新 */  
ROLL         /* ...|64t|64x|64y|2.0^i|64ti|i| */ 
PUSHB_1 1    /* ...|64t|64x|64y|2.0^i|64ti|i|1| */
ADD          /* ...|64t|64x|64y|2.0^i|64ti|i+1| */
ROLL         /* ...|64t|64x|64y|64ti|i+1|2.0^i| */
PUSHB_1 128  /* ...|64t|64x|64y|64ti|i+1|2.0^i|2.0| */
MUL          /* ...|64t|64x|64y|64ti|i+1|2.0^(i+1)| */
ROLL         /* ...|64t|64x|64y|i+1|2.0^(i+1)|64ti| */

/* if i <= 12  */
PUSHB_1 3
CINDEX       /* ...|64t|64x|64y|i|2.0^i|64ti|i| */
PUSHB_1 12   /* ...|64t|64x|64y|i|2.0^i|64ti|i|12| */
LTEQ         /* ...|64t|64x|64y|i|2.0^i|64ti|i<=12| */
IF           /* ...|64t|64x|64y|i|2.0^i|64ti| */ /* if (i <= 12) */
  
  PUSHB_1 5
  MINDEX     /* ...|64t|64y|i|2.0^i|64ti|64x| */
  PUSHB_1 5
  MINDEX     /* ...|64t|i|2.0^i|64ti|64x|64y| */
  DUP        /* ...|64t|i|2.0^i|64ti|64x|64y|64y| */
  PUSHB_1 5
  CINDEX     /* ...|64t|i|2.0^i|64ti|64x|64y|64y|2.0^i| */
  DIV        /* ...|64t|i|2.0^i|64ti|64x|64y|64y/2.0^i| */
  PUSHB_1 3
  CINDEX     /* ...|64t|i|2.0^i|64ti|64x|64y|64y/2.0^i|64x| */
  PUSHB_1 6
  CINDEX     /* ...|64t|i|2.0^i|64ti|64x|64y|64y/2.0^i|64x|2.0^i| */
  DIV        /* ...|64t|i|2.0^i|64ti|64x|64y|64y/2.0^i|64x/2.0^i| */

  ROLL       /* ...|64t|i|2.0^i|64ti|64x|64y/2.0^i|64x/2.0^i|64y| */
  SWAP       /* ...|64t|i|2.0^i|64ti|64x|64y/2.0^i|64y|64x/2.0^i| */

  /* if (ti<t) { */
  PUSHB_1 5
  CINDEX     /* ...|64t|i|2.0^i|64ti|64x|64y/2.0^i|64y|64x/2.0^i|64ti| */
  PUSHB_1 9
  CINDEX     /* ...|64t|i|2.0^i|64ti|64x|64y/2.0^i|64y|64x/2.0^i|64ti|64t| */
  LT         /* ...|64t|i|2.0^i|64ti|64x|64y/2.0^i|64y|64x/2.0^i|64ti<64t| */
  IF         /* ...|64t|i|2.0^i|64ti|64x|64y/2.0^i|64y|64x/2.0^i| */ 
    /* if ti<t */

    /* x -= y/(2^i) , y+= x/(2^i)*/
    ADD      /* ...|64t|i|2.0^i|64ti|64x|64y/2.0^i|64y+64x/2.0^i| */
    ROLL     /* ...|64t|i|2.0^i|64ti|64y/2.0^i|64y+64x/2.0^i|64x| */
    ROLL     /* ...|64t|i|2.0^i|64ti|64y+64x/2.0^i|64x|64y/2.0^i| */
    SUB      /* ...|64t|i|2.0^i|64ti|64y+64x/2.0^i|64x-64y/2.0^i| */
    SWAP     /* ...|64t|i|2.0^i|64ti|64x-64y/2.0^i|64y+64x/2.0^i| */
    PUSHB_1 5
    MINDEX   /* ...|64t|2.0^i|64ti|64x-64y/2.0^i|64y+64x/2.0^i|i| */
    PUSHB_1 5
    MINDEX   /* ...|64t|64ti|64x-64y/2.0^i|64y+64x/2.0^i|i|2.0^i| */
    PUSHB_1 5
    MINDEX   /* ...|64t|64x-64y/2.0^i|64y+64x/2.0^i|i|2.0^i|64ti| */
    
    /* ti = ti+thetai */
    PUSHB_1 3
    CINDEX   /* ...|64t|64x-64y/2.0^i|64y+64x/2.0^i|i|2.0^i|64ti|i| */
    RS       /* ...|64t|64x-64y/2.0^i|64y+64x/2.0^i|i|2.0^i|64ti|theta_i| */
    ADD      /* ...|64t|64x-64y/2.0^i|64y+64x/2.0^i|i|2.0^i|64ti+theta_i| */ 

  ELSE /* if it>= t */

    /* x += y/(2^i) , y-= x/(2^i)*/
    SUB      /* ...|64t|i|2.0^i|64ti|64x|64y/2.0^i|64y-64x/2.0^i| */
    ROLL     /* ...|64t|i|2.0^i|64ti|64y/2.0^i|64y-64x/2.0^i|64x| */
    ROLL     /* ...|64t|i|2.0^i|64ti|64y-64x/2.0^i|64x|64y/2.0^i| */
    ADD      /* ...|64t|i|2.0^i|64ti|64y-64x/2.0^i|64x+64y/2.0^i| */
    SWAP     /* ...|64t|i|2.0^i|64ti|64x+64y/2.0^i|64y-64x/2.0^i| */
    PUSHB_1 5
    MINDEX   /* ...|64t|2.0^i|64ti|64x+64y/2.0^i|64y-64x/2.0^i|i| */
    PUSHB_1 5
    MINDEX   /* ...|64t|64ti|64x+64y/2.0^i|64y-64x/2.0^i|i|2.0^i| */
    PUSHB_1 5
    MINDEX   /* ...|64t|64x+64y/2.0^i|64y-64x/2.0^i|i|2.0^i|64ti| */

    /* ti = ti-theta_i */
    PUSHB_1 3
    CINDEX   /* ...|64t|64x+64y/2.0^i|64y-64x/2.0^i|i|2.0^i|64ti|i| */
    RS       /* ...|64t|64x+64y/2.0^i|64y-64x/2.0^i|i|2.0^i|64ti|theta_i| */
    SUB      /* ...|64t|64x+64y/2.0^i|64y-64x/2.0^i|i|2.0^i|64ti-theta_i| */ 

  EIF
  PUSHW_1 -87
  JMPR       /*goto L0*/
EIF
             /* ...|64t|64x|64y|i|2.0^i|64ti| */
POP          /* ...|64t|64x|64y|i|2.0^i| */
POP          /* ...|64t|64x|64y|i| */
POP          /* ...|64t|64x|64y| */
PUSHB_1 13   /* ...|64t|64x|64y|13| */
RS           /* ...|64t|64x|64y|64r_12| */ /* 64r_12=StorageArea[13] */
DUP          /* ...|64t|64x|64y|64r_12|64r_12| */
ROLL         /* ...|64t|64x|64r_12|64r_12|64y| */
PUSHW_1 4096 /* ...|64t|64x|64r_12|64r_12|64y|64.0| */
MUL          /* ...|64t|64x|64r_12|64r_12|64*64y| */
SWAP         /* ...|64t|64x|64r_12|64*64y|64r_12| */
DIV          /* ...|64t|64x|64r_12|64sin| */ /* 64sin = 64*y/r_12 */
ROLL         /* ...|64t|64r_12|64sin|64x| */
PUSHW_1 4096 /* ...|64t|64r_12|64sin|64x|64.0| */
MUL          /* ...|64t|64r_12|64sin|64*64x| */
ROLL         /* ...|64t|64sin|64*64x|64r_12| */
DIV          /* ...|64t|64sin|64cos| */ /* 64cos = 64*x/r_12 */
ROLL         /* ...|64sin|64cos|64t| */
POP          /* ...|64sin|64cos| */

やっぱStorage Area使わない方がいいよね

Storage Area使うと、他の関数や命令で使う値を潰してしまうかもしれないので、あまりよろしくない。という訳で、Storage Area使わない版もつくった。

theta_iの定数には、順番に、かつ1回しかアクセスしないため、スタックの下の方に定数を先に使う方が上になるように積んでおいて(theta_12, theta_11, ..., theta_1, theta_0)、MINDEXで若い順にスタックトップに取り出して使うようにした。
MINDEXはスタックトップから数えるので、ループでも、毎回同じ番号を指定することで定数に順番にアクセスできる。

PUSHW_1 1920 /* 30 deg */ /*入力として与える角度*/


/* stack: ...|theta| */
/* PUSH constants */
NPUSHW 10
 0x0039
 0x0072
 0x00E5
 0x01CA
 0x0394
 0x0729
 0x0E52
 0x1CA3
 0x3938
 0x7200
PUSHW_4
 0x2094  /* 0xE094 % 2^14 */
 0x0003  /* 0xE094 / 2^14 */
 0x4000  /* 0x100.0 */
 0x1000  /* 0x40.0 */
MUL
MUL
ADD
PUSHW_4
 0x290A  /* 0x1A90A % 2^14 */
 0x0006  /* 0x1A90A / 2^14 */
 0x4000  /* 0x100.0 */
 0x1000  /* 0x40.0 */
MUL
MUL
ADD
PUSHW_4
 0x1000  /* 0x2D000 % 2^14 */
 0x000B  /* 0x2D000 / 2^14 */
 0x4000  /* 0x100.0 */
 0x1000  /* 0x40.0 */
MUL
MUL
ADD
             /* ...|theta|theta_12|theta_11|...|theta_0| */


PUSHB_1 14
MINDEX       /* ...|64theta_12|...|64theta_0|theta| */
PUSHW_1 4096 /* ...|64theta_12|...|64theta_0|theta|64.0| */
MUL          /* ...|64theta_12|...|64theta_0|64t| */ /* 64t=64*theta */
             /* ...|64theta_i|64t| */

PUSHW_2      /* ...|64theta_0|64t|64*1.0|64*1.0| */
 4096
 4096 
PUSHB_1 0
PUSHB_1 64   /* ...|64theta_0|64t|64*1.0|64*1.0|i=0|1.0| */
PUSHB_1 6  
MINDEX       /* ...|64theta_1|64t|64*1.0|64*1.0|i=0|1.0|64theta_0| */ 
   /* ...|64theta_i|64t|64x=64*1.0|64y=64*1.0|i=0|2.0^i=1.0|64ti=64theta_0| */

/*L0:*/ 
/* i = i+1, 2.0^i = 2.0^(i+1) に更新 */  
             /* ...|64t|64x|64y|i|2.0^i|64ti| */ 
ROLL         /* ...|64t|64x|64y|2.0^i|64ti|i| */ /* i++ */
PUSHB_1 1    /* ...|64t|64x|64y|2.0^i|64ti|i|1| */
ADD          /* ...|64t|64x|64y|2.0^i|64ti|i+1| */
ROLL         /* ...|64t|64x|64y|64ti|i+1|2.0^i| */
PUSHB_1 128  /* ...|64t|64x|64y|64ti|i+1|2.0^i|2.0| */
MUL          /* ...|64t|64x|64y|64ti|i+1|2.0^(i+1)| */
ROLL         /* ...|64t|64x|64y|i+1|2.0^(i+1)|64ti| */

/* if i <= 12  */
PUSHB_1 3
CINDEX       /* ...|64t|64x|64y|i|2.0^i|64ti|i| */
PUSHB_1 12   /* ...|64t|64x|64y|i|2.0^i|64ti|i|12| */
LTEQ         /* ...|64t|64x|64y|i|2.0^i|64ti|i<=12| */
IF           /* ...|64t|64x|64y|i|2.0^i|64ti| */ /* if (i <= 12) */

  PUSHB_1 5
  MINDEX     /* ...|64t|64y|i|2.0^i|64ti|64x| */
  PUSHB_1 5
  MINDEX     /* ...|64t|i|2.0^i|64ti|64x|64y| */
  DUP        /* ...|64t|i|2.0^i|64ti|64x|64y|64y| */
  PUSHB_1 5
  CINDEX     /* ...|64t|i|2.0^i|64ti|64x|64y|64y|2.0^i| */
  DIV        /* ...|64t|i|2.0^i|64ti|64x|64y|64y/2.0^i| */
  PUSHB_1 3
  CINDEX     /* ...|64t|i|2.0^i|64ti|64x|64y|64y/2.0^i|64x| */
  PUSHB_1 6
  CINDEX     /* ...|64t|i|2.0^i|64ti|64x|64y|64y/2.0^i|64x|2.0^i| */
  DIV        /* ...|64t|i|2.0^i|64ti|64x|64y|64y/2.0^i|64x/2.0^i| */

  ROLL       /* ...|64t|i|2.0^i|64ti|64x|64y/2.0^i|64x/2.0^i|64y| */
  SWAP       /* ...|64t|i|2.0^i|64ti|64x|64y/2.0^i|64y|64x/2.0^i| */

  /* if (ti<t) { */
  PUSHB_1 5
  CINDEX     /* ...|64t|i|2.0^i|64ti|64x|64y/2.0^i|64y|64x/2.0^i|64ti| */
  PUSHB_1 9
  CINDEX     /* ...|64t|i|2.0^i|64ti|64x|64y/2.0^i|64y|64x/2.0^i|64ti|64t| */
  LT         /* ...|64t|i|2.0^i|64ti|64x|64y/2.0^i|64y|64x/2.0^i|64ti<64t| */
  IF         /* ...|64t|i|2.0^i|64ti|64x|64y/2.0^i|64y|64x/2.0^i| */ 
    /* if ti<t */

    /* x -= y/(2^i) , y+= x/(2^i)*/
    ADD      /* ...|64t|i|2.0^i|64ti|64x|64y/2.0^i|64y+64x/2.0^i| */
    ROLL     /* ...|64t|i|2.0^i|64ti|64y/2.0^i|64y+64x/2.0^i|64x| */
    ROLL     /* ...|64t|i|2.0^i|64ti|64y+64x/2.0^i|64x|64y/2.0^i| */
    SUB      /* ...|64t|i|2.0^i|64ti|64y+64x/2.0^i|64x-64y/2.0^i| */
    SWAP     /* ...|64t|i|2.0^i|64ti|64x-64y/2.0^i|64y+64x/2.0^i| */
    PUSHB_1 5
    MINDEX   /* ...|64t|2.0^i|64ti|64x-64y/2.0^i|64y+64x/2.0^i|i| */
    PUSHB_1 5
    MINDEX   /* ...|64t|64ti|64x-64y/2.0^i|64y+64x/2.0^i|i|2.0^i| */
    PUSHB_1 5
    MINDEX   /* ...|64t|64x-64y/2.0^i|64y+64x/2.0^i|i|2.0^i|64ti| */

    /* ti = ti+theta_i */
    PUSHB_1 7
    MINDEX   /* ...|64t|64x-64y/2.0^i|64y+64x/2.0^i|i|2.0^i|64ti|theta_i| */
    ADD      /* ...|64t|64x-64y/2.0^i|64y+64x/2.0^i|i|2.0^i|64ti+theta_i| */ 

  ELSE /* if it>= t */

    /* x += y/(2^i) , y-= x/(2^i)*/
    SUB      /* ...|64t|i|2.0^i|64ti|64x|64y/2.0^i|64y-64x/2.0^i| */
    ROLL     /* ...|64t|i|2.0^i|64ti|64y/2.0^i|64y-64x/2.0^i|64x| */
    ROLL     /* ...|64t|i|2.0^i|64ti|64y-64x/2.0^i|64x|64y/2.0^i| */
    ADD      /* ...|64t|i|2.0^i|64ti|64y-64x/2.0^i|64x+64y/2.0^i| */
    SWAP     /* ...|64t|i|2.0^i|64ti|64x+64y/2.0^i|64y-64x/2.0^i| */
    PUSHB_1 5
    MINDEX   /* ...|64t|2.0^i|64ti|64x+64y/2.0^i|64y-64x/2.0^i|i| */
    PUSHB_1 5
    MINDEX   /* ...|64t|64ti|64x+64y/2.0^i|64y-64x/2.0^i|i|2.0^i| */
    PUSHB_1 5
    MINDEX   /* ...|64t|64x+64y/2.0^i|64y-64x/2.0^i|i|2.0^i|64ti| */

    /* ti = ti-theta_i */
    PUSHB_1 7
    MINDEX   /* ...|64t|64x+64y/2.0^i|64y-64x/2.0^i|i|2.0^i|64ti|theta_i| */
    SUB      /* ...|64t|64x+64y/2.0^i|64y-64x/2.0^i|i|2.0^i|64ti-theta_i| */ 

  EIF
  PUSHW_1 -85
  JMPR  /*goto L0*/
EIF
             /* ...|64t|64x|64y|i|2.0^i|64ti| */
POP          /* ...|64t|64x|64y|i|2.0^i| */
POP          /* ...|64t|64x|64y|i| */
POP          /* ...|64t|64x|64y| */
PUSHW_1      /* ...|64t|64x|64y|64r_12| */ /* 64*r12 */
 0x1A59 
DUP          /* ...|64t|64x|64y|64r_12|64r_12| */
ROLL         /* ...|64t|64x|64r_12|64r_12|64y| */
PUSHW_1 4096 /* 64.0 */
MUL          /* ...|64t|64x|64r_12|64r_12|64*64y| */
SWAP         /* ...|64t|64x|64r_12|64*64y|64r_12| */
DIV          /* ...|64t|64x|64r_12|64sin| */ /* 64sin = 64*y/r_12 */
ROLL         /* ...|64t|64r_12|64sin|64x| */
PUSHW_1 4096 /* 64.0 */
MUL          /* ...|64t|64r_12|64sin|64*64x| */
ROLL         /* ...|64t|64sin|64*64x|64r_12| */
DIV          /* ...|64t|64sin|64cos| */ /* 64cos = 64*x/r_12 */
ROLL         /* ...|64sin|64cos|64t| */
POP          /* ...|64sin|64cos| */

計算の定義域の拡大

さて、0~90度の範囲だけしか計算できないというのはよろしくないので、これ以外の範囲にも対応できるようにする。


最初に、\theta 0\le \theta < 360の範囲に入るようにする。
これは、三角関数 2\piの周期関数であることから、自然数nについて
 \sin(\theta) = \sin(\theta-2n\pi)
 \cos(\theta) = \cos(\theta-2n\pi)
が成り立つことによる。つまり、\theta

  • 360以上 → 360未満になるまで360を引く
  • 0未満 → 0以上になるまで360を足す
         /* stack: ...|theta(F26Dot6)| */

/* L1: */
DUP             /* ...|theta|theta| */ 
PUSHW_1 23040   /* ...|theta|theta|360.0| */
GTEQ            /* ...|theta|theta>=360.0| */
IF              /* ...|theta| */ /* if (theta >= 360.0) */
  PUSHW_1 23040 /* ...|theta|360.0| */
  SUB           /* ...|theta-360.0| */
  PUSHW_1 -13   /* ...|theta-360.0|-13| */
  JMPR          /* ...|theta-360.0| */ /* goto L1 */
EIF

/* L2: */
DUP             /* ...|theta|theta| */ 
PUSHB_1 0       /* ...|theta|theta|0.0| */
LT              /* ...|theta|theta<0.0| */
IF              /* ...|theta| */ /* if (theta < 0.0) */
  PUSHW_1 23040 /* ...|theta|360.0| */
  ADD           /* ...|theta+360.0| */
  PUSHW_1 -12   /* ...|theta+360.0|-12| */
  JMPR          /* ...|theta+360.0| */ /* goto L2 */
EIF

PUSHB_2 64 64 /* ...|theta|sign_sin=1.0|sign_cos=1.0| */
ROLL /* ...|sign_sin|sign_cos|theta| */

最後でsinとcosに対応する符号(1.0で初期化)をスタックに用意した。これは次で利用する。


次に、 180 < \theta < 360の場合に対応する。
 \sin\theta = -\sin(2\pi-\theta)
 \cos\theta = \cos(2\pi-\theta)
となることより、 180 < \theta < 360の場合は、

  •  \theta := 360 - \theta
  • sinの符号を反転
DUP /* ...|sign_sin|sign_cos|theta|theta| */
PUSHW_1 11520 /* ...|sign_sin|sign_cos|theta|theta|180.0| */
GT  /* ...|sign_sin|sign_cos|theta|theta>180.0| */
IF  /* ...|sign_sin|sign_cos|theta| */ /* if (theta>180.0) */
  PUSHW_1 23040 /* ...|sign_sin|sign_cos|theta|360.0| */
  SWAP  /* ...|sign_sin|sign_cos|360.0|theta| */
  SUB   /* ...|sign_sin|sign_cos|360.0-theta| */
  ROLL  /* ...|sign_cos|360.0-theta|sign_sin| */
  NEG   /* ...|sign_cos|360.0-theta|-sign_sin| */
  ROLL  /* ...|360.0-theta|-sign_sin|sign_cos| */
  ROLL  /* ...|-sign_sin|sign_cos|360.0-theta| */
EIF

次に、 90 < \theta \le 180のときに対応する。
 \sin\theta = \sin(\pi - \theta)
 \cos\theta = -\cos(\pi - \theta)
となることより、 90 < \theta \le 180となるときは、

  •  \theta := 180 - \theta
  • cosの符号を反転
DUP /* ...|sign_sin|sign_cos|theta|theta| */
PUSHW_1 5760 /* ...|sign_sin|sign_cos|theta|theta|90.0| */
GT  /* ...|sign_sin|sign_cos|theta|theta>90.0| */
IF  /* ...|sign_sin|sign_cos|theta| */ /* if (theta>90.0) */
  PUSHW_1 11520 /* ...|sign_sin|sign_cos|theta|180.0| */
  SWAP  /* ...|sign_sin|sign_cos|180.0|theta| */
  SUB   /* ...|sign_sin|sign_cos|180.0-theta| */
  SWAP  /* ...|sign_sin|180.0-theta|sign_cos| */
  NEG   /* ...|sign_sin|180.0-theta|-sign_cos| */
  SWAP  /* ...|sign_sin|-sign_cos|180.0-theta| */
EIF


0度と90度の時は場合分けして値を出すようにしてみた。まあそんなことしなくても結構な精度で計算してくれるのだけれど。
/* CORDIC algorithm comes here */ の部分は、実際には上で作ったようなCORDICアルゴリズムによる角度計算のプログラムを入れる。

DUP         /* ...|sign_sin|sign_cos|theta|theta| */
PUSHB_1 0   /* ...|sign_sin|sign_cos|theta|theta|0.0| */
EQ          /* ...|sign_sin|sign_cos|theta|theta==0.0| */
IF          /* ...|sign_sin|sign_cos|theta| */ /* if (theta == 0.0) */
  POP       /* ...|sign_sin|sign_cos| */
  PUSHW_2   /* ...|sign_sin|sign_cos|0.0|64.0| */ 
   0
   4096 
            /* ...|sign_sin|sign_cos|64sin0 = 0|64cos0 = 64*1.0| */
ELSE
  DUP       /* ...|sign_sin|sign_cos|theta|theta| */
  PUSHW_1   /* ...|sign_sin|sign_cos|theta|theta|90.0| */
   5760
  EQ        /* ...|sign_sin|sign_cos|theta|theta==90.0| */
  IF        /* ...|sign_sin|sign_cos|theta| */ /* if (theta==90.0) */
    POP     /* ...|sign_sin|sign_cos| */
    PUSHW_2 /* ...|sign_sin|sign_cos|64.0|0.0| */
     4096 
     0
            /* ...|sign_sin|sign_cos|sin90 = 64.0|cos90 = 0.0| */
  ELSE /* theta != 0.0 and theta != 90.0 */
            /* ...|sign_sin|sign_cos|theta| */

    /* CORDIC algorithm comes here */

            /* ...|sign_sin|sign_cos|64sin|64cos| */
  EIF
EIF         /* ...|sign_sin|sign_cos|64sin|64cos| */

ROLL        /* ...|sign_sin|64sin|64cos|sign_cos| */
MUL         /* ...|sign_sin|64sin|sign_cos*64cos| */
ROLL        /* ...|64sin|sign_cos*64cos|sign_sin| */
ROLL        /* ...|sign_cos*64cos|sign_sin|64sin| */
MUL         /* ...|sign_cos*64cos|sign_sin*64sin| */
SWAP        /* ...|sign_sin*64sin|sign_cos*64cos| */

関数化

さて、これらの計算を関数としてまとめる。関数0とした。

initial stack: ...|angle (deg) (F26Dot6)|
  final stack: ...|64*sin|64*cos|
PUSHB_1
 0
FDEF
DUP
PUSHW_1
 23040
GTEQ
IF
PUSHW_1
 23040
SUB
PUSHW_1
 -13
JMPR
EIF
DUP
PUSHB_1
 0
LT
IF
PUSHW_1
 23040
ADD
PUSHW_1
 -12
JMPR
EIF
PUSHB_2
 64
 64
ROLL
DUP
PUSHW_1
 11520
GT
IF
PUSHW_1
 23040
SWAP
SUB
ROLL
NEG
ROLL
ROLL
EIF
DUP
PUSHW_1
 5760
GT
IF
PUSHW_1
 11520
SWAP
SUB
SWAP
NEG
SWAP
EIF
DUP
PUSHB_1
 0
EQ
IF
POP
PUSHW_2
 0
 4096
ELSE
DUP
PUSHW_1
 5760
EQ
IF
POP
PUSHW_2
 4096
 0
ELSE
NPUSHW
 10
 57
 114
 229
 458
 916
 1833
 3666
 7331
 14648
 29184
PUSHW_4
 8340
 3
 16384
 4096
MUL
MUL
ADD
PUSHW_4
 10506
 6
 16384
 4096
MUL
MUL
ADD
PUSHW_4
 4096
 11
 16384
 4096
MUL
MUL
ADD
PUSHB_1
 14
MINDEX
PUSHW_1
 4096
MUL
PUSHW_2
 4096
 4096
PUSHB_1
 0
PUSHB_1
 64
PUSHB_1
 6
MINDEX
ROLL
PUSHB_1
 1
ADD
ROLL
PUSHB_1
 128
MUL
ROLL
PUSHB_1
 3
CINDEX
PUSHB_1
 12
LTEQ
IF
PUSHB_1
 5
MINDEX
PUSHB_1
 5
MINDEX
DUP
PUSHB_1
 5
CINDEX
DIV
PUSHB_1
 3
CINDEX
PUSHB_1
 6
CINDEX
DIV
ROLL
SWAP
PUSHB_1
 5
CINDEX
PUSHB_1
 9
CINDEX
LT
IF
ADD
ROLL
ROLL
SUB
SWAP
PUSHB_1
 5
MINDEX
PUSHB_1
 5
MINDEX
PUSHB_1
 5
MINDEX
PUSHB_1
 7
MINDEX
ADD
ELSE
SUB
ROLL
ROLL
ADD
SWAP
PUSHB_1
 5
MINDEX
PUSHB_1
 5
MINDEX
PUSHB_1
 5
MINDEX
PUSHB_1
 7
MINDEX
SUB
EIF
PUSHW_1
 -85
JMPR
EIF
POP
POP
POP
PUSHW_1
 6745
DUP
ROLL
PUSHW_1
 4096
MUL
SWAP
DIV
ROLL
PUSHW_1
 4096
MUL
ROLL
DIV
ROLL
POP
EIF
EIF
ROLL
MUL
ROLL
ROLL
MUL
SWAP
ENDF

*1:こんな風にMathJaxで数学記号が出せるのほんと楽しい。