nyarmat.java

java 版的 ARToolkit

/* 
 * PROJECT: NyARToolkit
 * --------------------------------------------------------------------------------
 * This work is based on the original ARToolKit developed by
 *   Hirokazu Kato
 *   Mark Billinghurst
 *   HITLab, University of Washington, Seattle
 * http://www.hitl.washington.edu/artoolkit/
 *
 * The NyARToolkit is Java version ARToolkit class library.
 * Copyright (C)2008 R.Iizuka
 *
 * This program is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License
 * as published by the Free Software Foundation; either version 2
 * of the License, or (at your option) any later version.
 * 
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 * 
 * You should have received a copy of the GNU General Public License
 * along with this framework; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 * 
 * For further information please contact.
 *	http://nyatla.jp/nyatoolkit/
 *	<airmail(at)ebony.plala.or.jp>
 * 
 */
package jp.nyatla.nyartoolkit.core;

import jp.nyatla.nyartoolkit.NyARException;



/**
 * ARMat構造体に対応するクラス typedef struct { double *m; int row; int clm; }ARMat;
 * 
 */
public class NyARMat
{
	/**
	 * 配列サイズと行列サイズは必ずしも一致しないことに注意 返された配列のサイズを行列の大きさとして使わないこと！
	 * 
	 */
	protected double[][] m;

	protected int clm, row;

	/**
	 * デフォルトコンストラクタは機能しません。
	 * 
	 * @throws NyARException
	 */
	protected NyARMat() throws NyARException
	{
		throw new NyARException();
	}

	public NyARMat(int i_row, int i_clm)
	{
		m = new double[i_row][i_clm];
		clm = i_clm;
		row = i_row;
	}

	/**
	 * i_row x i_clmサイズの行列を格納できるように行列サイズを変更します。 実行後、行列の各値は不定になります。
	 * 
	 * @param i_row
	 * @param i_clm
	 */
	public void realloc(int i_row, int i_clm)
	{
		if (i_row <= this.m.length && i_clm <= this.m[0].length) {
			// 十分な配列があれば何もしない。
		} else {
			// 不十分なら取り直す。
			m = new double[i_row][i_clm];
		}
		this.clm = i_clm;
		this.row = i_row;
	}

	public int getClm()
	{
		return clm;
	}

	public int getRow()
	{
		return row;
	}

	/**
	 * 行列をゼロクリアする。
	 */
	public void zeroClear()
	{
		int i, i2;
		// For順変更OK
		for (i = row - 1; i >= 0; i--) {
			for (i2 = clm - 1; i2 >= 0; i2--) {
				m[i][i2] = 0.0;
			}
		}
	}

	/**
	 * i_copy_fromの内容を自分自身にコピーします。 高さ・幅は同一で無いと失敗します。
	 * 
	 * @param i_copy_from
	 */
	public void copyFrom(NyARMat i_copy_from) throws NyARException
	{
		// サイズ確認
		if (this.row != i_copy_from.row || this.clm != i_copy_from.clm) {
			throw new NyARException();
		}
		// 値コピー
		for (int r = this.row - 1; r >= 0; r--) {
			for (int c = this.clm - 1; c >= 0; c--) {
				this.m[r][c] = i_copy_from.m[r][c];
			}
		}
	}

	public double[][] getArray()
	{
		return m;
	}

	// public void getRowVec(int i_row,NyARVec o_vec)
	// {
	// o_vec.set(this.m[i_row],this.clm);
	// }
	/**
	 * aとbの積を自分自身に格納する。arMatrixMul()の代替品
	 * 
	 * @param a
	 * @param b
	 * @throws NyARException
	 */
	public void matrixMul(NyARMat a, NyARMat b) throws NyARException
	{
		if (a.clm != b.row || this.row != a.row || this.clm != b.clm) {
			throw new NyARException();
		}
		double w;
		int r, c, i;
		double[][] am = a.m, bm = b.m, dm = this.m;
		// For順変更禁止
		for (r = 0; r < this.row; r++) {
			for (c = 0; c < this.clm; c++) {
				w = 0.0;// dest.setARELEM0(r, c,0.0);
				for (i = 0; i < a.clm; i++) {
					w += am[r][i] * bm[i][c];// ARELEM0(dest, r, c) +=ARELEM0(a, r, i) * ARELEM0(b,i, c);
				}
				dm[r][c] = w;
			}
		}
	}

	private int[] wk_nos_matrixSelfInv = new int[50];

	// private final static double matrixSelfInv_epsl=1.0e-10;
	/**
	 * i_targetを逆行列に変換する。arMatrixSelfInv()と、arMatrixSelfInv_minv()関数を合成してあります。
	 * OPTIMIZE STEP[485->422]
	 * 
	 * @param i_target
	 *            逆行列にする行列
	 * @return 逆行列があればTRUE/無ければFALSE
	 * 
	 * @throws NyARException
	 */
	public boolean matrixSelfInv() throws NyARException
	{
		double[][] ap = this.m;
		int dimen = this.row;
		int dimen_1 = dimen - 1;
		double[] ap_n, ap_ip, ap_i;// wap;
		int j, ip, nwork;
		int[] nos = wk_nos_matrixSelfInv;// この関数で初期化される。
		// double epsl;
		double p, pbuf, work;

		/* check size */
		switch (dimen) {
		case 0:
			throw new NyARException();
		case 1:
			ap[0][0] = 1.0 / ap[0][0];// *ap = 1.0 / (*ap);
			return true;/* 1 dimension */
		}

		for (int n = 0; n < dimen; n++) {
			nos[n] = n;
		}

		/*
		 * nyatla memo ipが定まらないで計算が行われる場合があるので挿入。 ループ内で0初期化していいかが判らない。
		 */
		ip = 0;
		// For順変更禁止
		for (int n = 0; n < dimen; n++) {
			ap_n = ap[n];// wcp = ap + n * rowa;
			p = 0.0;
			for (int i = n; i < dimen; i++) {// for(i = n, wap = wcp, p =
												// 0.0; i < dimen ; i++, wap +=
												// rowa)
				if (p < (pbuf = Math.abs(ap[i][0]))) {
					p = pbuf;
					ip = i;
				}
			}
			// if (p <= matrixSelfInv_epsl){
			if (p == 0.0) {
				return false;
				// throw new NyARException();
			}

			nwork = nos[ip];
			nos[ip] = nos[n];
			nos[n] = nwork;

			ap_ip = ap[ip];
			for (j = 0; j < dimen; j++) {// for(j = 0, wap = ap + ip * rowa,
											// wbp = wcp; j < dimen ; j++) {
				work = ap_ip[j]; // work = *wap;
				ap_ip[j] = ap_n[j];
				ap_n[j] = work;
			}

			work = ap_n[0];
			for (j = 0; j < dimen_1; j++) {// for(j = 1, wap = wcp, work =
											// *wcp; j < dimen ; j++, wap++)
				ap_n[j] = ap_n[j + 1] / work;// *wap = *(wap + 1) / work;
			}
			ap_n[j] = 1.0 / work;// *wap = 1.0 / work;
			for (int i = 0; i < dimen; i++) {
				if (i != n) {
					ap_i = ap[i];// wap = ap + i * rowa;

					work = ap_i[0];
					for (j = 0; j < dimen_1; j++) {// for(j = 1, wbp = wcp,work = *wap;j < dimen ;j++, wap++, wbp++)
						ap_i[j] = ap_i[j + 1] - work * ap_n[j];// wap = *(wap +1) - work *(*wbp);
					}
					ap_i[j] = -work * ap_n[j];// *wap = -work * (*wbp);
				}
			}
		}

		for (int n = 0; n < dimen; n++) {
			for (j = n; j < dimen; j++) {
				if (nos[j] == n) {
					break;
				}
			}
			nos[j] = nos[n];
			for (int i = 0; i < dimen; i++) {// for(i = 0, wap = ap + j, wbp
												// = ap + n; i < dimen ;i++, wap
												// += rowa, wbp += rowa) {
				ap_i = ap[i];
				work = ap_i[j];// work = *wap;
				ap_i[j] = ap_i[n];// *wap = *wbp;
				ap_i[n] = work;// *wbp = work;
			}
		}
		return true;
	}

	/**
	 * sourceの転置行列をdestに得る。arMatrixTrans()の代替品
	 * 
	 * @param dest
	 * @param source
	 * @return
	 */
	public static void matrixTrans(NyARMat dest, NyARMat source) throws NyARException
	{
		if (dest.row != source.clm || dest.clm != source.row) {
			throw new NyARException();
		}
		NyARException.trap("未チェックのパス");
		// For順変更禁止
		for (int r = 0; r < dest.row; r++) {
			for (int c = 0; c < dest.clm; c++) {
				dest.m[r][c] = source.m[c][r];
			}
		}
	}

	/**
	 * unitを単位行列に初期化する。arMatrixUnitの代替品
	 * 
	 * @param unit
	 */
	public static void matrixUnit(NyARMat unit) throws NyARException
	{
		if (unit.row != unit.clm) {
			throw new NyARException();
		}
		NyARException.trap("未チェックのパス");
		// For順変更禁止
		for (int r = 0; r < unit.getRow(); r++) {
			for (int c = 0; c < unit.getClm(); c++) {
				if (r == c) {
					unit.m[r][c] = 1.0;
				} else {
					unit.m[r][c] = 0.0;
				}
			}
		}
	}

	/**
	 * sourceの内容を自身に複製する。 Optimized 2008.04.19
	 * 
	 * @param i_source
	 * @return
	 */
	public void matrixDup(NyARMat i_source) throws NyARException
	{
		// 自身の配列サイズを相手のそれより大きいことを保障する。
		this.realloc(i_source.row, i_source.clm);
		// 内容を転写
		int r, c;
		double[][] src_m, dest_m;
		src_m = i_source.m;
		dest_m = this.m;
		// コピーはFor順を変えてもOK
		for (r = this.row - 1; r >= 0; r--) {
			for (c = this.clm - 1; c >= 0; c--) {
				dest_m[r][c] = src_m[r][c];
			}
		}
	}

	public NyARMat matrixAllocDup() throws NyARException
	{
		NyARMat result = new NyARMat(this.row, this.clm);
		// コピー
		int r, c;
		double[][] dest_m, src_m;
		dest_m = result.m;
		src_m = this.m;
		// コピーはFor順を変えてもOK
		for (r = this.row - 1; r >= 0; r--) {
			for (c = this.clm - 1; c >= 0; c--) {
				dest_m[r][c] = src_m[r][c];
			}
		}
		return result;
	}

	/**
	 * arMatrixInv関数の代替品です。 destにsourceの逆行列を返します。
	 * 
	 * @param dest
	 * @param source
	 * @throws NyARException
	 */
	public static void matrixInv(NyARMat dest, NyARMat source)
			throws NyARException
	{
		NyARException.trap("未チェックのパス");
		dest.matrixDup(source);

		NyARException.trap("未チェックのパス");
		dest.matrixSelfInv();
	}

	public NyARMat matrixAllocInv() throws NyARException
	{
		NyARException.trap("未チェックのパス");
		NyARMat result = matrixAllocDup();

		NyARException.trap("未チェックのパス");
		result.matrixSelfInv();
		return result;
	}

	/**
	 * dim x dim の単位行列を作る。
	 * 
	 * @param dim
	 * @return
	 * @throws NyARException
	 */
	public static NyARMat matrixAllocUnit(int dim) throws NyARException
	{
		NyARException.trap("未チェックのパス");