nyarmat.java

java 版的 ARToolkit

				m1[j] = sum * work;// *(m++) = sum * work;
				// {//*(m++) = sum * work;
				// m.set(sum * work);
				// m.incPtr();}
			}
		}
		for (; i < row; i++) {
			NyARException.trap("未チェックのパス");
			ev_array[i] = 0.0;// ev->v[i] = 0.0;
			for (int j = 0; j < clm; j++) {
				m[i][j] = 0.0;
				// m.set(0.0);//*(m++) = 0.0;
				// m.incPtr();
			}
		}
	}

	private NyARMat wk_PCA_PCA_u = null;

	/**
	 * static int PCA( ARMat *input, ARMat *output, ARVec *ev )
	 * 
	 * @param output
	 * @param o_ev
	 * @throws NyARException
	 */
	private void PCA_PCA(NyARMat o_output, NyARVec o_ev) throws NyARException
	{

		int l_row, l_clm, min;
		double[] ev_array = o_ev.getArray();

		l_row = this.row;// row = input->row;
		l_clm = this.clm;// clm = input->clm;
		min = (l_clm < l_row) ? l_clm : l_row;
		if (l_row < 2 || l_clm < 2) {
			throw new NyARException();
		}
		if (o_output.clm != this.clm) {// if( output->clm != input->clm ){
			throw new NyARException();
		}
		if (o_output.row != min) {// if( output->row != min ){
			throw new NyARException();
		}
		if (o_ev.getClm() != min) {// if( ev->clm != min ){
			throw new NyARException();
		}

		NyARMat u;// u =new NyARMat( min, min );
		if (this.wk_PCA_PCA_u == null) {
			u = new NyARMat(min, min);
			this.wk_PCA_PCA_u = u;
		} else {
			u = this.wk_PCA_PCA_u;
			u.realloc(min, min);
		}

		if (l_row < l_clm) {
			NyARException.trap("未チェックのパス");
			PCA_x_by_xt(this, u);// if(x_by_xt( input, u ) < 0 ) {
		} else {
			PCA_xt_by_x(this, u);// if(xt_by_x( input, u ) < 0 ) {
		}
		u.PCA_QRM(o_ev);

		double[][] m1, m2;
		if (l_row < l_clm) {
			NyARException.trap("未チェックのパス");
			PCA_EV_create(this, u, o_output, o_ev);
		} else {
			m1 = u.m;// m1 = u->m;
			m2 = o_output.m;// m2 = output->m;
			int i;
			for (i = 0; i < min; i++) {
				if (ev_array[i] < PCA_VZERO) {// if( ev->v[i] < VZERO ){
					break;
				}
				for (int j = 0; j < min; j++) {
					m2[i][j] = m1[i][j];// *(m2++) = *(m1++);
				}
			}
			for (; i < min; i++) {// for( ; i < min; i++){
				// コードを見た限りあってそうだからコメントアウト(2008/03/26)NyARException.trap("未チェックのパス");
				ev_array[i] = 0.0;// ev->v[i] = 0.0;
				for (int j = 0; j < min; j++) {
					m2[i][j] = 0.0;// *(m2++) = 0.0;
				}
			}
		}
	}

	private NyARMat wk_work_matrixPCA = null;

	/**
	 * int arMatrixPCA( ARMat *input, ARMat *evec, ARVec *ev, ARVec *mean );
	 * 関数の置き換え。input引数がthisになる。 Optimize:2008.04.19
	 * 
	 * @param o_evec
	 * @param o_ev
	 * 
	 * @param mean
	 * @throws NyARException
	 */
	public void matrixPCA(NyARMat o_evec, NyARVec o_ev, NyARVec mean)throws NyARException
	{
		double srow, sum;
		int l_row, l_clm;
		int check;

		l_row = this.row;// row = input->row;
		l_clm = this.clm;// clm = input->clm;
		check = (l_row < l_clm) ? l_row : l_clm;
		if (l_row < 2 || l_clm < 2) {
			throw new NyARException();
		}
		if (o_evec.clm != l_clm || o_evec.row != check) {// if( evec->clm !=
															// input->clm ||
															// evec->row !=
															// check ){
			throw new NyARException();
		}
		if (o_ev.getClm() != check) {// if( ev->clm != check ){
			throw new NyARException();
		}
		if (mean.getClm() != l_clm) {// if( mean->clm != input->clm ){
			throw new NyARException();
		}

		// 自分の内容をワークにコピー(高速化の為に、1度作ったインスタンスは使いまわす)
		NyARMat work;
		if (this.wk_work_matrixPCA == null) {
			work = this.matrixAllocDup();
			this.wk_work_matrixPCA = work;
		} else {
			work = this.wk_work_matrixPCA;
			work.matrixDup(this);// arMatrixAllocDup( input );work =
									// arMatrixAllocDup( input );
		}

		srow = Math.sqrt((double) l_row);
		work.PCA_EX(mean);

		PCA_CENTER(work, mean);

		int i, j;
		// For順変更OK
		for (i = 0; i < l_row; i++) {
			for (j = 0; j < l_clm; j++) {
				work.m[i][j] /= srow;// work->m[i] /= srow;
			}
		}

		work.PCA_PCA(o_evec, o_ev);

		sum = 0.0;
		double[] ev_array = o_ev.getArray();
		int ev_clm = o_ev.getClm();
		// For順変更禁止
		for (i = 0; i < ev_clm; i++) {// for(int i = 0; i < ev->clm; i++ ){
			sum += ev_array[i];// sum += ev->v[i];
		}
		// For順変更禁止
		for (i = 0; i < ev_clm; i++) {// for(int i = 0; i < ev->clm; i++ ){
			ev_array[i] /= sum;// ev->v[i] /= sum;
		}
	}

	/* int arMatrixPCA2( ARMat *input, ARMat *evec, ARVec *ev ); */
	public static void arMatrixPCA2(NyARMat input, NyARMat evec, NyARVec ev) throws NyARException
	{
		NyARException.trap("未チェックのパス");
		NyARMat work;
		// double srow; // unreferenced
		double sum;
		int row, clm;
		int check;

		row = input.row;// row = input->row;
		clm = input.clm;// clm = input->clm;
		check = (row < clm) ? row : clm;
		if (row < 2 || clm < 2) {
			throw new NyARException();
		}
		if (evec.getClm() != input.clm || evec.row != check) {// if( evec->clm!= input->clm|| evec->row!= check ){
			throw new NyARException();
		}
		if (ev.getClm() != check) {// if( ev->clm != check ){
			throw new NyARException();
		}

		NyARException.trap("未チェックのパス");
		work = input.matrixAllocDup();

		NyARException.trap("未チェックパス");
		work.PCA_PCA(evec, ev);// rval = PCA( work, evec, ev );
		sum = 0.0;
		double[] ev_array = ev.getArray();
		for (int i = 0; i < ev.getClm(); i++) {// for( i = 0; i < ev->clm; i++
												// ){
			NyARException.trap("未チェックパス");
			sum += ev_array[i];// sum += ev->v[i];
		}
		for (int i = 0; i < ev.getClm(); i++) {// for(int i = 0; i < ev->clm;i++ ){
			NyARException.trap("未チェックパス");
			ev_array[i] /= sum;// ev->v[i] /= sum;
		}
		return;
	}

	public static NyARMat matrixAllocMul(NyARMat a, NyARMat b) throws NyARException
	{
		NyARException.trap("未チェックのパス");
		NyARMat dest = new NyARMat(a.row, b.clm);
		NyARException.trap("未チェックのパス");
		dest.matrixMul(a, b);
		return dest;
	}

	/* static double mdet(double *ap, int dimen, int rowa) */
	private static double Det_mdet(double[][] ap, int dimen, int rowa) throws NyARException
	{
		NyARException.trap("動作未チェック/配列化未チェック");
		double det = 1.0;
		double work;
		int is_ = 0;
		int mmax;

		for (int k = 0; k < dimen - 1; k++) {
			mmax = k;
			for (int i = k + 1; i < dimen; i++) {
				// if (Math.abs(arMatrixDet_MATRIX_get(ap, i, k, rowa)) >
				// Math.abs(arMatrixDet_MATRIX_get(ap, mmax, k, rowa))){
				if (Math.abs(ap[i][k]) > Math.abs(ap[mmax][k])) {
					mmax = i;
				}
			}
			if (mmax != k) {
				for (int j = k; j < dimen; j++) {
					work = ap[k][j];// work = MATRIX(ap, k, j, rowa);
					ap[k][j] = ap[mmax][j];// MATRIX(ap, k, j, rowa) =MATRIX(ap, mmax, j, rowa);
					ap[mmax][j] = work;// MATRIX(ap, mmax, j, rowa) = work;
				}
				is_++;
			}
			for (int i = k + 1; i < dimen; i++) {
				work = ap[i][k] / ap[k][k];// work = arMatrixDet_MATRIX_get(ap,i, k, rowa) /arMatrixDet_MATRIX_get(ap, k, k,rowa);
				for (int j = k + 1; j < dimen; j++) {
					// MATRIX(ap, i, j, rowa) -= work * MATRIX(ap, k, j, rowa);
					ap[i][j] -= work * ap[k][j];
				}
			}
		}
		for (int i = 0; i < dimen; i++) {
			det = ap[i][i];// det *= MATRIX(ap, i, i, rowa);
		}
		for (int i = 0; i < is_; i++) {
			det *= -1.0;
		}
		return det;
	}

	/* double arMatrixDet(ARMat *m); */
	public static double arMatrixDet(NyARMat m) throws NyARException
	{
		NyARException.trap("動作未チェック/配列化未チェック");
		if (m.row != m.clm) {
			return 0.0;
		}
		return Det_mdet(m.getArray(), m.row, m.clm);// return mdet(m->m, m->row,m->row);
	}

	private final NyARVec wk_vecTridiagonalize_vec = new NyARVec(0);

	private final NyARVec wk_vecTridiagonalize_vec2 = new NyARVec(0);

	/**
	 * arVecTridiagonalize関数の代替品 a,d,e間で演算をしてる。何をどうしているかはさっぱりさっぱり
	 * 
	 * @param a
	 * @param d
	 * @param e
	 * @param i_e_start
	 *            演算開始列(よくわからないけどarVecTridiagonalizeの呼び出し元でなんかしてる)
	 * @return
	 * @throws NyARException
	 */
	private void vecTridiagonalize(NyARVec d, NyARVec e, int i_e_start)throws NyARException
	{
		NyARVec vec = wk_vecTridiagonalize_vec;
		// double[][] a_array=a.getArray();
		double s, t, p, q;
		int dim;

		if (this.clm != this.row) {// if(a.getClm()!=a.getRow()){
			throw new NyARException();
		}
		if (this.clm != d.getClm()) {// if(a.getClm() != d.clm){
			throw new NyARException();
		}
		if (this.clm != e.getClm()) {// if(a.getClm() != e.clm){
			throw new NyARException();
		}
		dim = this.getClm();

		double[] d_vec, e_vec;
		d_vec = d.getArray();
		e_vec = e.getArray();
		double[] a_vec_k;

		for (int k = 0; k < dim - 2; k++) {

			a_vec_k = this.m[k];
			vec.setNewArray(a_vec_k, clm);// vec=this.getRowVec(k);//double[]
											// vec_array=vec.getArray();
			NyARException.trap("未チェックパス");
			d_vec[k] = a_vec_k[k];// d.v[k]=vec.v[k];//d.set(k,v.get(k));
									// //d->v[k] = v[k];

			// wv1.clm = dim-k-1;
			// wv1.v = &(v[k+1]);
			NyARException.trap("未チェックパス");
			e_vec[k + i_e_start] = vec.vecHousehold(k + 1);// e.v[k+i_e_start]=vec.vecHousehold(k+1);//e->v[k]= arVecHousehold(&wv1);
			if (e_vec[k + i_e_start] == 0.0) {// if(e.v[k+i_e_start]== 0.0){//if(e.v[k+i_e_start]== 0.0){
				continue;
			}

			for (int i = k + 1; i < dim; i++) {
				s = 0.0;
				for (int j = k + 1; j < i; j++) {
					NyARException.trap("未チェックのパス");
					s += this.m[j][i] * a_vec_k[j];// s += a_array[j][i] *vec.v[j];//s +=a.get(j*dim+i) *v.get(j);//s +=a->m[j*dim+i] * v[j];
				}
				for (int j = i; j < dim; j++) {
					NyARException.trap("未チェックのパス");
					s += this.m[i][j] * a_vec_k[j];// s += a_array[i][j] *vec.v[j];//s +=a.get(i*dim+j) *v.get(j);//s +=a->m[i*dim+j] * v[j];
				}
				NyARException.trap("未チェックのパス");
				d_vec[i] = s;// d.v[i]=s;//d->v[i] = s;
			}

			// wv1.clm = wv2.clm = dim-k-1;
			// wv1.v = &(v[k+1]);
			// wv2.v = &(d->v[k+1]);
			a_vec_k = this.m[k];
			vec.setNewArray(a_vec_k, clm);// vec=this.getRowVec(k);
			// vec_array=vec.getArray();
			NyARException.trap("未チェックパス");
			t = vec.vecInnerproduct(d, k + 1) / 2;
			for (int i = dim - 1; i > k; i--) {
				NyARException.trap("未チェックパス");
				p = a_vec_k[i];// p = v.get(i);//p = v[i];
				d_vec[i] -= t * p;
				q = d_vec[i];// d.v[i]-=t*p;q=d.v[i];//q = d->v[i] -= t*p；
				for (int j = i; j < dim; j++) {
					NyARException.trap("未チェックパス");
					this.m[i][j] -= p * (d_vec[j] + q * a_vec_k[j]);// a.m[i][j]-=p*(d.v[j] +q*vec.v[j]);//a->m[i*dim+j] -=p*(d->v[j]) + q*v[j];
				}
			}
		}

		if (dim >= 2) {
			d_vec[dim - 2] = this.m[dim - 2][dim - 2];// d.v[dim-2]=a.m[dim-2][dim-2];//d->v[dim-2]=a->m[(dim-2)*dim+(dim-2)];
			e_vec[dim - 2 + i_e_start] = this.m[dim - 2][dim - 1];// e.v[dim-2+i_e_start]=a.m[dim-2][dim-1];//e->v[dim-2] = a->m[(dim-2)*dim+(dim-1)];
		}

		if (dim >= 1) {
			d_vec[dim - 1] = this.m[dim - 1][dim - 1];// d.v[dim-1]=a_array[dim-1][dim-1];//d->v[dim-1] =a->m[(dim-1)*dim+(dim-1)];
		}
		NyARVec vec2 = this.wk_vecTridiagonalize_vec2;
		for (int k = dim - 1; k >= 0; k--) {
			a_vec_k = this.m[k];
			vec.setNewArray(a_vec_k, clm);// vec=this.getRowVec(k);//v =a.getPointer(k*dim);//v = &(a->m[k*dim]);
			if (k < dim - 2) {
				for (int i = k + 1; i < dim; i++) {
					// wv1.clm = wv2.clm = dim-k-1;
					// wv1.v = &(v[k+1]);
					// wv2.v = &(a->m[i*dim+k+1]);
					vec2.setNewArray(this.m[i], clm);// vec2=this.getRowVec(i);

					t = vec.vecInnerproduct(vec2, k + 1);
					for (int j = k + 1; j < dim; j++) {
						NyARException.trap("未チェックパス");
						this.m[i][j] -= t * a_vec_k[j];// a_array[i][j]-=t*vec.v[j];//a.subValue(i*dim+j,t*v.get(j));//a->m[i*dim+j]-= t * v[j];
					}
				}
			}
			for (int i = 0; i < dim; i++) {
				a_vec_k[i] = 0.0;// v.set(i,0.0);//v[i] = 0.0;
			}
			a_vec_k[k] = 1;// v.set(k,1);//v[k] = 1;
		}
		return;
	}
}