バグ検出ドリル（3）それでもプログラマーは数学を知っておくべきだ：山浦恒央の“くみこみ”な話（103）（2/3 ページ）

[山浦恒央 東海大学 大学院 組込み技術研究科 非常勤講師（工学博士），MONOist]

4．今回の問題

　新人エンジニアA君は、先輩からリスト1-1に示す3次元座標の回転アルゴリズムを実装するよう依頼された。A君は、数学が得意でなかったが、数式通りに実装すれば問題ないと考えた。実装は、C++で作成した（リスト1-2）。実行結果（リスト1-3）を見ると、リスト1-1に示す予想結果が合致しないことが分かった。上記から、実行結果が異なる理由を推察せよ。

　仕様：3次元の物体をZ軸に60度、X軸に180度、Y軸に90度回転する組み合わせ行列を作成し、その式から頂点A座標（200, 0, -30）、B座標（0, 50, -150）、C座標（40, 20, -100）の三角形を回転し、コンソールに出力する。

　組み合わせ行列アルゴリズムは、以下に示す。

θx：X軸回りの回転角度[rad]、θy：Y軸回りの回転角度[rad]、θz：Z軸回りの回転角度[rad]

　実行結果は、下記となる。

座標A（30.00, -173.21, -100.00）

座標B（150.00, -25.00, 43.30）

座標C（100.00, -44.64, -2.68）

リスト1-1　座標回転プログラムのアルゴリズム

#include <stdio.h>
#include <math.h>
#define PI 3.14159265359
//3*3の行列クラス
class Matrix33 {
private:
	int i,j;	//ループ変数
	double index[3][3];	//行列の要素
public:
	//3行3列の行列の値を0で初期化する
	Matrix33(){
		for (i = 0; i < 3; i++) {
			for (j = 0; j < 3; j++) {
				index[i][j] = 0.0;
			}
		}
	}
	~Matrix33(){}
	//指定した要素に値を代入
	void SetVal(int x, int y, double value){
		index[x][y] = value;
	}
	//指定した要素の値を出力
	double GetIndex(int i, int j) {
		return index[i][j];
	}
	//X軸、Y軸、Z軸の回転行列を算出する
	Matrix33 Rotate(double xrad, double yrad, double zrad){
		Matrix33 res_zx;
		Matrix33 res_zxy;
		Matrix33 mx, my, mz;
		//X軸回りの回転行列
		mx.SetVal(0, 0, 1.0);
		mx.SetVal(1, 1, cos(xrad));
		mx.SetVal(1, 2, -1.0 * sin(xrad));
		mx.SetVal(2, 1, sin(xrad));
		mx.SetVal(2, 2, cos(xrad));
		//Y軸回りの回転行列
		my.SetVal(0, 0, cos(yrad));
		my.SetVal(0, 2, sin(yrad));
		my.SetVal(1, 1, 1);
		my.SetVal(2, 0, -1.0 * sin(yrad));
		my.SetVal(2, 2, cos(yrad));
		//Z軸回りの回転行列
		mz.SetVal(0, 0, cos(zrad));
		mz.SetVal(0, 1, -1.0 * sin(zrad));
		mz.SetVal(1, 0, sin(zrad));
		mz.SetVal(1, 1, cos(zrad));
		mz.SetVal(2, 2, 1);
		res_zx = Multiply33(mz, mx);
		res_zxy = Multiply33(res_zx,my);
		return res_zxy;
	}
	//3行3列の行列の乗算をする
	Matrix33 Multiply33(Matrix33 m1, Matrix33 m2){
		Matrix33 res;
		for (int i = 0; i < 3; i++) {
			for (int j = 0; j < 3; j++) {
				for (int k = 0; k < 3; k++) {
					res.index[i][j] += (m1.index[i][k] * m2.index[k][j]);
				}
			}
		}
		return res;	
	}
};
//3*3の行列クラス
class Matrix31{
private:
	int i,j;
	double index[3];
public:
	//3行1列の行列を初期化
	Matrix31(){
		for (i = 0; i < 3; i++) {
			index[i] = 0;
		}
	}
	~Matrix31(){}
	//3行1列の行列に値を代入
	void SetIndex(double val1, double val2, double val3){
		index[0] = val1;
		index[1] = val2;
		index[2] = val3;
	}
	//3行3列の行列×3行1列の行列
	Matrix31 Multiply31by33(Matrix33 m1, Matrix31 m2){
		Matrix31 res;
		
		for (i = 0; i < 3; i++) {
			for (j = 0; j < 3; j++) {
				res.index[i] += (m1.GetIndex(i,j) * m2.index[j]);
			}
		}
		return res;
	}
	//3行1列の行列の値をコンソールに出力
	void PrintConsole(Matrix31 m){
		for (i = 0; i < 3; i++) {
			printf("%.2lf ",m.index[i]);
		}
		printf("\n");
	}
};
int main(void)
{
	Matrix33 RotateMatrix;	//3*3の行列クラスの宣言
	Matrix31 A, B, C;		//3*1の行列クラスの宣言
	double xdeg = 180.0;		//X軸回りの角度
	double ydeg = 90.0;		//Y軸回りの角度
	double zdeg = 60.0;		//Z軸回りの角度
	//回転行列の作成
	RotateMatrix = RotateMatrix.Rotate(xdeg * PI / 180.0 , ydeg * PI / 180.0, zdeg * PI / 180.0);
	A.SetIndex(200.0, 0.0, -30.0);	//座標Aの作成
	B.SetIndex(0.0, 50.0, -150.0);	//座標Bの作成
	C.SetIndex(40.0, 20.0, -100.0);	//座標Cの作成
	A = A.Multiply31by33(RotateMatrix, A);	//座標Aの回転
	B = B.Multiply31by33(RotateMatrix, B);	//座標Bの回転
	C = C.Multiply31by33(RotateMatrix, C);	//座標Cの回転
	
	printf("実行結果\n");
	printf("座標A\n");
	A.PrintConsole(A);	//座標Aの値を出力
	printf("座標B\n");
	B.PrintConsole(B);	//座標Bの値を出力
	printf("座標C\n");
	C.PrintConsole(C);	//座標Cの値を出力
	return 0;
}

リスト1-2　座標回転プログラム

実行結果
座標A
-15.00 -25.98 200.00
座標B
-31.70 -154.90 -0.00
座標C
-32.68 -96.60 40.0

リスト1-3　実行結果