atan2

C 言語などにある、角度を求める関数 atan2(y, x) の Z80 での実装例です。サイズ/精度/速度の違うものをいくつか実装しました。

atan2 の詳しい解説については Wikipedia を参照してください。




仕様

atan2(y, x) はベクトル (x, y) の(x 軸正方向から左回りの)角度を $ -\pi~+\pi $(-180°~+180°)の範囲で返しますが、Z80 で浮動小数点数を扱うのは大変なので、角度に $ 128/\pi $ を掛けて、8bit 値の -128~+127 を返すことにします。

なお、x, y が両方 0 の場合は返す値は不定です。

x, y については符号付き 8~16bit を想定します。少ない方が楽なのですが、8bit だと足りないことも多そうです。

例えば PC-8001 のセミグラフィック画面は 160x100 ドットしかありませんが、それでも座標の x 差分は最大で 159 になり、符号付き 8bit(-128~+127)の範囲に収まりません。




共通アルゴリズム

角度 $ 0~\pi/4 $(0°~45°)については $ \arctan(y/x) $、その他の角度は対称形なのを利用して、$ x $ と $ y $ を入れ替えたり符号を変えることで求めることができます。

たとえば $ x \leqq 0, y \gt 0, -x \leqq y $ の場合は、角度は $ \pi/2~3\pi/4 $(90°~135°)の範囲になり、$ \pi/2+\arctan(-x/y) $ で求められます。

したがって、$ x \geqq 0, y \geqq 0, x \geqq y $ の条件下で $ \arctan(y/x) $ を求める方法が重要になります。




アルゴリズム 1

サイズ最優先の簡易版で、精度は高くなく最大で±3 の誤差が出ます。

arctan

この区間では直線に近いので単純に直線近似することにして、$ 32y/x $ を返します。

まずは C 言語で書いてみます(符号付きではなく符号無し 8bit 値を返していますが、同じことです)。

typedef unsigned char u8;

u8 Atan2Sub2(int y, int x) {
    return y * 32 / x;
}

u8 Atan2Sub(int y, int x) {
    if (x >= y) {
        return Atan2Sub2(y, x);
    } else {
        return 0x40 - Atan2Sub2(x, y);
    }
}

u8 Atan2(int y, int x) {
    u8 baseAngle;
    int t;

    if (x >= 0) {
        if (y >= 0) {
            if (y == 0) return 0;
            baseAngle = 0;
        } else {
            baseAngle = 0xc0;
            t = x;
            x = -y;
            y = t;
        }
    } else {
        if (y >= 0) {
            baseAngle = 0x40;
            t = -x;
            x = y;
            y = t;
        } else {
            baseAngle = 0x80;
            x = -x;
            y = -y;
        }
    }
    return Atan2Sub(y, x) + baseAngle;
}

関数 Atan2() では、まず x, y の符号によって、90°単位で場合分けします(0 除算で落ちないよう、x, y が両方 0 の場合は 0 を返しています)。

そして関数 Atan2Sub() では、x, y の大小によって 45°を境に場合分けします。

これらの処理によって、関数 Atan2Sub2() は $ x \geqq 0, y \geqq 0, x \geqq y $ の条件下で $ \arctan(y/x) $ の近似値を返すことになります。

この処理を Z80 に置き換えたのが atan2_simple.z80s です。ただしコードサイズを 1Byte でも減らすために工夫しているので、 わかりにくそうなところを解説しておきます。

下記のコードは HL=-HL です。

	XOR A
	SUB L
	LD L,A
	SBC A,A
	SUB H
	LD H,A

下記の部分の DJNZ はループではなく、B レジスタを分岐フラグとして利用しています(1 以外で分岐)。

	LD C,2
	DJNZ _ATAN2_L3
_ATAN2_L2:
	DJNZ _ATAN2_SUB
_ATAN2_L3:
	INC C

C レジスタは C 言語版の baseAngle に相当しますが、除算の処理で 6bit 左に移動することを考慮してあります。

下記のコードは A=(C<<6)+HL*32/DE です。ここに来た時には C フラグがクリアされていることに注意してください。

	LD A,C
	LD B,6
_ATAN2_SUB2_L2:
	SBC HL,DE
	JR NC,_ATAN2_SUB2_L3
	ADD HL,DE
_ATAN2_SUB2_L3:
	CCF
	ADC A,A
	ADD HL,HL
	DJNZ _ATAN2_SUB2_L2

前後しますが、下記のコードは A=(A&0xc0)+(0x40-(A&0x3f)) に相当します。ちょっと難しいかもしれません。

	XOR 3FH
	INC A

サイズは 60Byte、平均 533clk くらいです。除算は重いので、まぁこんなもんでしょう。




折れ線近似で精度を上げて、最大誤差を±1 まで減らしたものです。

アルゴリズムは「マイコン系 プログラムのテクニック アルゴリズム」中の「6bit程度の精度のarctan」 (リンク先の少し下)を参考にしました。

$ y=0 $ なら 0、$ x=y $ なら 32 を返します。$ x=2y $ のとき $ \arctan(y/x)=\arctan(1/2)\fallingdotseq 0.4636 $ で、$ 128/\pi $ を掛けた値は約 18.9 なので 19 を返すことにし、

  • $ x \geqq 2y $ : $ 38y/x $
  • $ x \lt 2y $ : $ 26y/x+6 $

と分割して近似します。

C 言語版の関数 Atan2Sub2() を上記の式に置き換えます。ただし除算に四捨五入処理を加えてあります (分母と分子を 2 倍し、分子に x を加算)。

u8 Atan2Sub2(int y, int x) {
    y *= 2;
    if (x >= y) {
        return (u8)((38 * y + x) / (x * 2));
    } else {
        return (u8)((26 * y + x) / (x * 2) + 6);
    }
}

Z80 版は、

を作りました。まずサイズ重視版の補足説明をします。

下記の部分で、命令の途中にジャンプしています。

	LD C,0C0H
	DJNZ _ATAN2_L2
	DB 01H,80H	; LD BC,xx80H
_ATAN2_L2:
	EX DE,HL

B が 0 のときは「C=0xc0、DE と HL を交換」、B が 1 のときは「C=0x80」になります。分岐を 1 回で済ませて、サイズを節約しています。

下記のコードは A=0x40-A です。

	CPL
	ADD A,41H

下記のコードは除算ですが、アルゴリズム 1 と違って CCF を実行せずに、最後に CPL でまとめて反転しています(ただし 0 のままにしておきたい上位ビットも反転するので、C レジスタの値で調整しています)。

	XOR A
_ATAN2_SUB2_L2:
	SBC HL,DE
	JR NC,_ATAN2_SUB2_L3
	ADD HL,DE
_ATAN2_SUB2_L3:
	ADC A,A
	ADD HL,HL
	DJNZ _ATAN2_SUB2_L2
	DEC A
	RRA
	CPL
	ADD A,C

除算処理の都合上、x, y の許容範囲は -2048~+2048 です。

次に速度重視版の補足説明をします。

速度を稼ぐため、AF' レジスタも使用しているので注意してください。AF' レジスタを破壊したくない場合は、

	EX AF,AF'
	CALL _ATAN2_SUB
	LD C,A
	EX AF,AF'
	ADD A,C

の部分を、

	PUSH AF
	CALL _ATAN2_SUB
	POP BC
	ADD A,B

とするといいでしょう(9clk 増加)。

除算には非回復法を使用し、さらにループ展開しています (非回復法のアルゴリズムについては省略しますが、 わかりやすい解説サイトは見つかりませんでした)。

除算処理の都合上、x, y の許容範囲は -1024~+1024 です。




速度最優先版で、最大誤差は±1 です。

アルゴリズムは 「8-bit atan2」を参考にしました。

まず正の数 $ x, y $ について、 $$ \log(y/x)=\log y-\log x より、\\ y/x=\exp(\log y-\log x) $$ が成り立ちます($ \log $ は自然対数)。したがって、 $$ \arctan(y/x)=\arctan\{\exp(\log y-\log x)\} $$ なので、$ \log x $ と $ \arctan(\exp x) $ をテーブル化してしまえば、重たい除算をしなくて済みます。

問題は $ x $ や $ y $ が 0 の場合ですが、まず $ y=0 $ の場合は $ \arctan(y/x)=0 $ で、これは先に例外処理します。

そして $ x \geqq y $ の条件下では $ x=0 $ なら $ y=0 $ なので、こちらは考えなくて済みます。

C 言語版は省略します。Z80 版は atan2_table.z80s です。

エントリは _ATAN_8_ATAN_16 の 2 つがあります。_ATAN_8 の x, y は符号付き 8bit です。

_ATAN_16 の x, y は符号付き 16bit で、符号付き 8bit に収める処理(この処理は結構重たいです)をしてから _ATAN_8 へ移ります。下記のコードで、符号付き 8bit に収まっているかどうかの判定をしています。

	LD A,L
	ADD A,A
	SBC A,A
	CP H
	JR Z,_ATAN2_16_L3

2 つのテーブルのサイズは 128Byte ずつで、精度を確保するためにこれくらい必要でした。速度を稼ぐために、

	ALIGN 100H

というアセンブラ疑似命令で 256Byte 境界に置いています。

前半は $ \log x $ テーブルです。具体的には整数 $ x=1~128 $ について、$ 127\log_{128} x $ を四捨五入した値です($ \log 1=0 $、$ \log_{n}n=1 $ に注目)。

後半は $ \arctan(\exp x) $ テーブルです。具体的には整数 $ x=-127~0 $ について、 $$ \frac{128}{\pi}\arctan\left\{ \exp\left(\frac{\log_{e}128}{127}x\right)\right\} $$ を四捨五入した値です($ \exp -\infty=0 $、$ \exp 0=1 $ に注目)。

サイズと速度に関しては以下の通りです。

  • _ATAN2_8:320Byte、平均 174clk くらい
  • _ATAN2_16:360Byte、最大で 697clk くらい

_ATAN2_8_ATAN2_16 の前処理部分を削除した場合です。

_ATAN2_16 は x, y の絶対値が大きいほど遅くなるので注意してください。




各アルゴリズムの比較です。


アルゴリズム Byte 平均clk 備考
1(サイズ最優先) 60 533 誤差やや大
2(サイズ重視) 104 745 x, y は -2048~+2048
2(速度重視) 164 547 x, y は -1024~+1024
3(速度最優先) 320 174 x, y は符号付き 8bit
360 697 x, y は符号付き 16bit(ワーストケース)

用途によりますが、アルゴリズム 1 で事足りることが多いかと思います。精度や速度が求められる場合に、 他のアルゴリズムの採用を検討してください。




  • atan2_z80_240319.zip(487,430 Bytes)
    作成した Z80 ソースの他、Z80 アセンブラ、Z80 エミュレータなどが入っています。
    詳しくはアーカイブ内の README.TXT を読んでください。


さて、ゲームで atan2 を使うのはどんなときかと言うと、 たとえば「狙った方向に弾を撃つ」というのがあります。C++ で書くと、

struct Vec2 {
	float x, y;
};

void Normalize(Vec2& v) {
	float angle = atan2(v.y, v.x);
	v.x = cos(angle);
	v.y = sin(angle);
}

こんな感じで弾の速度を一定にできます。cos, sin は sin テーブルを用意しておけば素早く求められますね (ただし弾の速度を変えたい場合、 乗算するかテーブルを複数用意する必要があります)。

では、Z80 ではなく今どきの CPU だとどうでしょうか。

void Normalize(Vec2& v) {
	float d = sqrt(v.x * v.x + v.y * v.y);
	if (d == 0.0f) {
		v.x = 1.0f;
		return;
	}
	v.x /= d;
	v.y /= d;
}

float で正確に求めるなら、この方が速そうです。Z80 で精度を落として計算する場合、この計算式だと工夫の余地が少なく、atan2 を使う方が都合が良いのです。

そんなわけで、今どきの CPU ではこの用途ではなく角度が必要なときに atan2 を使用すると思いますが、出番は少ないかもしれません。


inserted by FC2 system