nyarmat.java

java 版的 ARToolkit

		NyARMat result = new NyARMat(dim, dim);
		NyARException.trap("未チェックのパス");
		NyARMat.matrixUnit(result);
		return result;
	}

	/**
	 * arMatrixDispの代替品
	 * 
	 * @param m
	 * @return
	 */
	public int matrixDisp() throws NyARException
	{
		NyARException.trap("未チェックのパス");
		System.out.println(" === matrix (" + row + "," + clm + ") ===");// printf(" ===matrix (%d,%d) ===\n", m->row, m->clm);
		for (int r = 0; r < row; r++) {// for(int r = 0; r < m->row; r++) {
			System.out.print(" |");// printf(" |");
			for (int c = 0; c < clm; c++) {// for(int c = 0; c < m->clm; c++) {
				System.out.print(" " + m[r][c]);// printf(" %10g", ARELEM0(m, r, c));
			}
			System.out.println(" |");// printf(" |\n");
		}
		System.out.println(" ======================");// printf(" ======================\n");
		return 0;
	}

	private static final double PCA_EPS = 1e-6; // #define EPS 1e-6

	private static final int PCA_MAX_ITER = 100; // #define MAX_ITER 100

	private static final double PCA_VZERO = 1e-16; // #define VZERO 1e-16

	/**
	 * static int EX( ARMat *input, ARVec *mean )の代替関数 Optimize:STEP:[144->110]
	 * 
	 * @param input
	 * @param mean
	 * @return
	 * @throws NyARException
	 */
	private void PCA_EX(NyARVec mean) throws NyARException
	{
		int lrow, lclm;
		int i, i2;
		lrow = this.row;
		lclm = this.clm;
		double[][] lm = this.m;

		if (lrow <= 0 || lclm <= 0) {
			throw new NyARException();
		}
		if (mean.getClm() != lclm) {
			throw new NyARException();
		}
		// double[] mean_array=mean.getArray();
		// mean.zeroClear();
		final double[] mean_array = mean.getArray();
		double w;
		// For順変更禁止
		for (i2 = 0; i2 < lclm; i2++) {
			w = 0.0;
			for (i = 0; i < lrow; i++) {
				// *(v++) += *(m++);
				w += lm[i][i2];
			}
			mean_array[i2] = w / lrow;// mean->v[i] /= row;
		}
	}

	/**
	 * static int CENTER( ARMat *inout, ARVec *mean )の代替関数
	 * 
	 * @param inout
	 * @param mean
	 * @return
	 */
	private static void PCA_CENTER(NyARMat inout, NyARVec mean)throws NyARException
	{
		double[] v;
		int row, clm;

		row = inout.getRow();
		clm = inout.getClm();
		if (mean.getClm() != clm) {
			throw new NyARException();
		}
		double[][] im = inout.m;
		double[] im_i;
		double w0, w1;
		v = mean.getArray();
		// 特にパフォーマンスが劣化するclm=1と2ときだけ、別パスで処理します。
		switch (clm) {
		case 1:
			w0 = v[0];
			for (int i = 0; i < row; i++) {
				im[i][0] -= w0;
			}
			break;
		case 2:
			w0 = v[0];
			w1 = v[1];
			for (int i = 0; i < row; i++) {
				im_i = im[i];
				im_i[0] -= w0;
				im_i[1] -= w1;
			}
			break;
		default:
			for (int i = 0; i < row; i++) {
				im_i = im[i];
				for (int j = 0; j < clm; j++) {
					// *(m++) -= *(v++);
					im_i[j] -= v[j];
				}
			}
			break;
		}
		return;
	}

	/**
	 * int x_by_xt( ARMat *input, ARMat *output )の代替関数
	 * 
	 * @param input
	 * @param output
	 * @throws NyARException
	 */
	private static void PCA_x_by_xt(NyARMat input, NyARMat output) throws NyARException
	{
		NyARException.trap("動作未チェック/配列化未チェック");
		int row, clm;
		// double[][] out;
		double[] in1, in2;

		NyARException.trap("未チェックのパス");
		row = input.row;
		clm = input.clm;
		NyARException.trap("未チェックのパス");
		if (output.row != row || output.clm != row) {
			throw new NyARException();
		}

		// out = output.getArray();
		for (int i = 0; i < row; i++) {
			for (int j = 0; j < row; j++) {
				if (j < i) {
					NyARException.trap("未チェックのパス");
					output.m[i][j] = output.m[j][i];// *out =
													// output->m[j*row+i];
				} else {
					NyARException.trap("未チェックのパス");
					in1 = input.m[i];// input.getRowArray(i);//in1 = &(input->m[clm*i]);
					in2 = input.m[j];// input.getRowArray(j);//in2 = &(input->m[clm*j]);
					output.m[i][j] = 0;// *out = 0.0;
					for (int k = 0; k < clm; k++) {
						output.m[i][j] += (in1[k] * in2[k]);// *out += *(in1++)
															// * *(in2++);
					}
				}
				// out.incPtr();
			}
		}
	}

	/**
	 * static int xt_by_x( ARMat *input, ARMat *output )の代替関数
	 * Optimize:2008.04.19
	 * 
	 * @param input
	 * @param i_output
	 * @throws NyARException
	 */
	private static void PCA_xt_by_x(NyARMat input, NyARMat i_output) throws NyARException
	{
		double[] in_;
		int row, clm;

		row = input.row;
		clm = input.clm;
		if (i_output.row != clm || i_output.clm != clm) {
			throw new NyARException();
		}

		int k, j;
		double[][] out_m = i_output.m;
		double w;
		for (int i = 0; i < clm; i++) {
			for (j = 0; j < clm; j++) {
				if (j < i) {
					out_m[i][j] = out_m[j][i];// *out = output->m[j*clm+i];
				} else {
					w = 0.0;// *out = 0.0;
					for (k = 0; k < row; k++) {
						in_ = input.m[k];// in=input.getRowArray(k);
						w += (in_[i] * in_[j]);// *out += *in1 * *in2;
					}
					out_m[i][j] = w;
				}
			}
		}
	}

	private final NyARVec wk_PCA_QRM_ev = new NyARVec(1);

	/**
	 * static int QRM( ARMat *a, ARVec *dv )の代替関数
	 * 
	 * @param a
	 * @param dv
	 * @throws NyARException
	 */
	private void PCA_QRM(NyARVec dv) throws NyARException
	{
		double w, t, s, x, y, c;
		int dim, iter;
		double[] dv_array = dv.getArray();

		dim = this.row;
		if (dim != this.clm || dim < 2) {
			throw new NyARException();
		}
		if (dv.getClm() != dim) {
			throw new NyARException();
		}

		NyARVec ev = this.wk_PCA_QRM_ev;
		ev.realloc(dim);
		double[] ev_array = ev.getArray();
		if (ev == null) {
			throw new NyARException();
		}
		final double[][] L_m = this.m;
		this.vecTridiagonalize(dv, ev, 1);

		ev_array[0] = 0.0;// ev->v[0] = 0.0;
		for (int h = dim - 1; h > 0; h--) {
			int j = h;
			while (j > 0&& Math.abs(ev_array[j]) > PCA_EPS* (Math.abs(dv_array[j - 1]) + Math.abs(dv_array[j]))) {// while(j>0 && fabs(ev->v[j]) >EPS*(fabs(dv->v[j-1])+fabs(dv->v[j])))
				// j--;
				j--;
			}
			if (j == h) {
				continue;
			}
			iter = 0;
			do {
				iter++;
				if (iter > PCA_MAX_ITER) {
					break;
				}
				w = (dv_array[h - 1] - dv_array[h]) / 2;// w = (dv->v[h-1] -dv->v[h]) / 2;//ここ？
				t = ev_array[h] * ev_array[h];// t = ev->v[h] * ev->v[h];
				s = Math.sqrt(w * w + t);
				if (w < 0) {
					s = -s;
				}
				x = dv_array[j] - dv_array[h] + t / (w + s);// x = dv->v[j] -dv->v[h] +t/(w+s);
				y = ev_array[j + 1];// y = ev->v[j+1];
				for (int k = j; k < h; k++) {
					if (Math.abs(x) >= Math.abs(y)) {
						if (Math.abs(x) > PCA_VZERO) {
							t = -y / x;
							c = 1 / Math.sqrt(t * t + 1);
							s = t * c;
						} else {
							c = 1.0;
							s = 0.0;
						}
					} else {
						t = -x / y;
						s = 1.0 / Math.sqrt(t * t + 1);
						c = t * s;
					}
					w = dv_array[k] - dv_array[k + 1];// w = dv->v[k] -dv->v[k+1];
					t = (w * s + 2 * c * ev_array[k + 1]) * s;// t = (w * s +2 * c *ev->v[k+1]) *s;
					dv_array[k] -= t;// dv->v[k] -= t;
					dv_array[k + 1] += t;// dv->v[k+1] += t;
					if (k > j) {
						NyARException.trap("未チェックパス");
						{
							ev_array[k] = c * ev_array[k] - s * y;// ev->v[k]= c *ev->v[k]- s * y;
						}
					}
					ev_array[k + 1] += s * (c * w - 2 * s * ev_array[k + 1]);// ev->v[k+1]+= s * (c* w- 2* s *ev->v[k+1]);

					for (int i = 0; i < dim; i++) {
						x = L_m[k][i];// x = a->m[k*dim+i];
						y = L_m[k + 1][i];// y = a->m[(k+1)*dim+i];
						L_m[k][i] = c * x - s * y;// a->m[k*dim+i] = c * x - s* y;
						L_m[k + 1][i] = s * x + c * y;// a->m[(k+1)*dim+i] = s* x + c * y;
					}
					if (k < h - 1) {
						NyARException.trap("未チェックパス");
						{
							x = ev_array[k + 1];// x = ev->v[k+1];
							y = -s * ev_array[k + 2];// y = -s * ev->v[k+2];
							ev_array[k + 2] *= c;// ev->v[k+2] *= c;
						}
					}
				}
			} while (Math.abs(ev_array[h]) > PCA_EPS
					* (Math.abs(dv_array[h - 1]) + Math.abs(dv_array[h])));
		}
		for (int k = 0; k < dim - 1; k++) {
			int h = k;
			t = dv_array[h];// t = dv->v[h];
			for (int i = k + 1; i < dim; i++) {
				if (dv_array[i] > t) {// if( dv->v[i] > t ) {
					h = i;
					t = dv_array[h];// t = dv->v[h];
				}
			}
			dv_array[h] = dv_array[k];// dv->v[h] = dv->v[k];
			dv_array[k] = t;// dv->v[k] = t;
			this.flipRow(h, k);
		}
	}

	/**
	 * i_row_1番目の行と、i_row_2番目の行を入れ替える。
	 * 
	 * @param i_row_1
	 * @param i_row_2
	 */
	private void flipRow(int i_row_1, int i_row_2)
	{
		int i;
		double w;
		double[] r1 = this.m[i_row_1], r2 = this.m[i_row_2];
		// For順変更OK
		for (i = clm - 1; i >= 0; i--) {
			w = r1[i];
			r1[i] = r2[i];
			r2[i] = w;
		}
	}

	/**
	 * static int EV_create( ARMat *input, ARMat *u, ARMat *output, ARVec *ev
	 * )の代替関数
	 * 
	 * @param input
	 * @param u
	 * @param output
	 * @param ev
	 * @throws NyARException
	 */
	private static void PCA_EV_create(NyARMat input, NyARMat u, NyARMat output,NyARVec ev) throws NyARException
	{
		NyARException.trap("未チェックのパス");
		int row, clm;
		row = input.row;// row = input->row;
		clm = input.clm;// clm = input->clm;
		if (row <= 0 || clm <= 0) {
			throw new NyARException();
		}
		if (u.row != row || u.clm != row) {// if( u->row != row || u->clm !=
											// row ){
			throw new NyARException();
		}
		if (output.row != row || output.clm != clm) {// if( output->row !=
														// row || output->clm !=
														// clm ){
			throw new NyARException();
		}
		if (ev.getClm() != row) {// if( ev->clm != row ){
			throw new NyARException();
		}
		double[][] m, in_;
		double[] m1, ev_array;
		double sum, work;

		NyARException.trap("未チェックのパス");
		m = output.m;// m = output->m;
		in_ = input.m;
		int i;
		ev_array = ev.getArray();
		for (i = 0; i < row; i++) {
			NyARException.trap("未チェックのパス");
			if (ev_array[i] < PCA_VZERO) {// if( ev->v[i] < VZERO ){
				break;
			}
			NyARException.trap("未チェックのパス");
			work = 1 / Math.sqrt(Math.abs(ev_array[i]));// work = 1 /
														// sqrt(fabs(ev->v[i]));
			for (int j = 0; j < clm; j++) {
				sum = 0.0;
				m1 = u.m[i];// m1 = &(u->m[i*row]);
				// m2=input.getPointer(j);//m2 = &(input->m[j]);
				for (int k = 0; k < row; k++) {
					sum += m1[k] + in_[k][j];// sum += *m1 * *m2;
					// m1.incPtr(); //m1++;
					// m2.addPtr(clm);//m2 += clm;
				}