/*
* PROJECT: NyARToolkit(Extension)
* --------------------------------------------------------------------------------
* The NyARToolkit is Java edition ARToolKit class library.
* Copyright (C)2008-2009 Ryo 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 3 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 program. If not, see <http://www.gnu.org/licenses/>.
*
* For further information please contact.
* http://nyatla.jp/nyatoolkit/
* <airmail(at)ebony.plala.or.jp> or <nyatla(at)nyatla.jp>
*
*/
package jp.nyatla.nyartoolkit.core.utils;
/**
* このクラスは、連立方程式を解く関数を実装します。
*/
public class NyARSystemOfLinearEquationsProcessor
{
/**
* i_reftとi_rightの整合性を確認します。
* @param i_left
* @param i_right
* @return
*/
private static boolean isValid2dArray(double[][] i_left,double[] i_right)
{
final int sm=i_left.length;
final int sn=i_left[0].length;
if(i_left.length!=sm){
return false;
}
if(i_right.length!=sm){
return false;
}
for(int i=1;i<sm;i++){
if(i_left[i].length!=sn){
return false;
}
}
return true;
}
/**
* この関数は、方程式の係数配列にガウスの消去法を実行します。
* [i_left_src]=[i_right_src]の式にガウスの消去法を実行して、i_leftの要素を1になるように基本変形します。
* i_mとi_nが等しくない場合は、最終行の要素が1になるまで基本変形を実行して、終了します。
* <p>
* 方程式の係数は、次のように入力します。
* <pre>
* <table>
* <tr><td>i_left[0][0]*X+</td><td>i_left[0][1]*Y+</td><td>i_left[0][2]*Z</td><td>=</td><td>i_right[0]</td></tr>
* <tr><td>i_left[1][0]*X+</td><td>i_left[1][1]*Y+</td><td>i_left[1][2]*Z</td><td>=</td><td>i_right[1]</td></tr>
* <tr><td>i_left[2][0]*X+</td><td>i_left[2][1]*Y+</td><td>i_left[2][2]*Z</td><td>=</td><td>i_right[2]</td></tr>
* </table>
* </pre>
* 関数を実行すると、配列は次のようになります。
* <pre>
* <table>
* <tr><td>1</td><td>0</td><td>0</td><td>=</td><td>A</td></tr>
* <tr><td>0</td><td>1</td><td>0</td><td>=</td><td>B</td></tr>
* <tr><td>0</td><td>0</td><td>1</td><td>=</td><td>C</td></tr>
* </table>
* </pre>
* </p>
* @param i_left
* 連立方程式の左辺値を指定します。[i_m][i_n]の配列を指定してください。
* @param i_right
* 連立方程式の右辺値を指定します。[i_m]の配列を指定してください。
* @param i_n
* 連立方程式の係数の数を指定します。
* @param i_m
* 連立方程式の数を指定します。
* @return
* 最終行まで基本変形ができてばtrueを返します。
*/
public static boolean doGaussianElimination(double[][] i_left,double[] i_right,int i_n,int i_m)
{
//整合性を確認する.
assert isValid2dArray(i_left,i_right);
//1行目以降
for(int solve_row=0;solve_row<i_m;solve_row++)
{
{//ピボット操作
int pivod=solve_row;
double pivod_value=Math.abs(i_left[pivod][pivod]);
for(int i=solve_row+1;i<i_m;i++){
final double pivod_2=Math.abs(i_left[i][pivod]);
if(pivod_value<Math.abs(pivod_2)){
pivod=i;
pivod_value=pivod_2;
}
}
if(solve_row!=pivod){
//行の入れ替え(Cの時はポインタテーブル使って！)
final double[] t=i_left[solve_row];
i_left[solve_row]=i_left[pivod];
i_left[pivod]=t;
final double t2=i_right[solve_row];
i_right[solve_row]=i_right[pivod];
i_right[pivod]=t2;
}
}
final double[] dest_l_n=i_left[solve_row];
final double dest_l_nn=i_left[solve_row][solve_row];
if(dest_l_nn==0.0){
//選択後の対角要素が0になってしまったら失敗する。
return false;
}
//消去計算(0 - solve_row-1項までの消去)
for(int i=0;i<solve_row;i++){
double s=dest_l_n[i];
for(int i2=0;i2<i_n;i2++)
{
final double p=i_left[i][i2]*s;
dest_l_n[i2]=dest_l_n[i2]-p;
}
final double k=i_right[i]*s;
i_right[solve_row]=i_right[solve_row]-k;
}
//消去法の実行(割り算)
final double d=dest_l_n[solve_row];
for(int i2=0;i2<solve_row;i2++){
dest_l_n[i2]=0;
}
if(d!=1.0){
dest_l_n[solve_row]=1.0;
for(int i=solve_row+1;i<i_n;i++){
dest_l_n[i]/=d;
}
i_right[solve_row]/=d;
}
}
return true;
}
/**
* この関数は、i_leftとi_rightで表現された連立方程式を解いて、i_left,i_right内容を更新します。
* i_right[n]の内容が、i_left[x][n]番目の係数の解になります。
* 配列の表現については、{@link #doGaussianElimination}を参照してください。
* @param i_left
* 連立方程式の左辺値を指定します。[i_m][i_n]の配列を指定してください。
* @param i_right
* 連立方程式の右辺値を指定します。[i_m]の配列を指定してください。
* @param i_number_of_system
* 方程式の数を指定します。
* @return
* 方程式が解ければtrueを返します。
*/
public static boolean solve(double[][] i_left,double[] i_right,int i_number_of_system)
{
return doGaussianElimination(i_left,i_right,i_number_of_system,i_number_of_system);
}
}