lib/src/jp/nyatla/nyartoolkit/core/transmat/solver/NyARTransportVectorSolver_ARToolKit.java

   1 /* \r
   2  * PROJECT: NyARToolkit\r
   3  * --------------------------------------------------------------------------------\r
   4  * This work is based on the original ARToolKit developed by\r
   5  *   Hirokazu Kato\r
   6  *   Mark Billinghurst\r
   7  *   HITLab, University of Washington, Seattle\r
   8  * http://www.hitl.washington.edu/artoolkit/\r
   9  *\r
  10  * The NyARToolkit is Java edition ARToolKit class library.\r
  11  * Copyright (C)2008-2009 Ryo Iizuka\r
  12  *\r
  13  * This program is free software: you can redistribute it and/or modify\r
  14  * it under the terms of the GNU General Public License as published by\r
  15  * the Free Software Foundation, either version 3 of the License, or\r
  16  * (at your option) any later version.\r
  17  * \r
  18  * This program is distributed in the hope that it will be useful,\r
  19  * but WITHOUT ANY WARRANTY; without even the implied warranty of\r
  20  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\r
  21  * GNU General Public License for more details.\r
  22  *\r
  23  * You should have received a copy of the GNU General Public License\r
  24  * along with this program.  If not, see <http://www.gnu.org/licenses/>.\r
  25  * \r
  26  * For further information please contact.\r
  27  *      http://nyatla.jp/nyatoolkit/\r
  28  *      <airmail(at)ebony.plala.or.jp> or <nyatla(at)nyatla.jp>\r
  29  * \r
  30  */\r
  31 package jp.nyatla.nyartoolkit.core.transmat.solver;\r
  32 \r
  33 import jp.nyatla.nyartoolkit.NyARException;\r
  34 import jp.nyatla.nyartoolkit.core.NyARMat;\r
  35 import jp.nyatla.nyartoolkit.core.param.NyARPerspectiveProjectionMatrix;\r
  36 import jp.nyatla.nyartoolkit.core.types.NyARDoublePoint2d;\r
  37 import jp.nyatla.nyartoolkit.core.types.NyARDoublePoint3d;\r
  38 \r
  39 /**\r
  40  * このクラスは、ARToolKitと同じ計算手順で並進ベクトルを求めます。\r
  41  */\r
  42 public class NyARTransportVectorSolver_ARToolKit implements INyARTransportVectorSolver\r
  43 {\r
  44         private final NyARMat _mat_at = new NyARMat(3,8);//3,NUMBER_OF_VERTEX*2\r
  45         private final NyARMat _mat_a =  new NyARMat(8,3);//NUMBER_OF_VERTEX,3\r
  46         private final NyARMat _mat_t =  new NyARMat(3,3);//NUMBER_OF_VERTEX,3\r
  47         private final NyARMat _mat_c =  new NyARMat(8,1);//NUMBER_OF_VERTEX * 2, 1\r
  48         private final NyARMat _mat_e =  new NyARMat(3,1);\r
  49         private final NyARMat _mat_f =  new NyARMat(3,1);\r
  50         private double[] _cx=new double[4];\r
  51         private double[] _cy=new double[4];\r
  52         \r
  53         private final NyARPerspectiveProjectionMatrix _projection_mat;\r
  54         /**\r
  55          * コンストラクタです。\r
  56          * 射影変換オブジェクトの参照値を指定して、インスタンスを生成します。\r
  57          * @param i_projection_mat_ref\r
  58          * 射影変換オブジェクトの参照値です。\r
  59          */\r
  60         public NyARTransportVectorSolver_ARToolKit(NyARPerspectiveProjectionMatrix i_projection_mat_ref)\r
  61         {\r
  62                 this._projection_mat=i_projection_mat_ref;\r
  63                 //aとb(aの転置行列)の固定部分を設定。\r
  64                 final double[][] mata = this._mat_a.getArray();\r
  65                 final double[][] matat = this._mat_at.getArray();\r
  66 \r
  67                 //変換用行列のcpara部分を先に作成\r
  68                 for (int i = 0; i < 4; i++) {\r
  69                         final int x2 = i * 2;\r
  70                         mata[x2][0] = matat[0][x2] = i_projection_mat_ref.m00;// mat_a->m[j*6+0]=mat_b->m[num*0+j*2] =cpara[0][0];\r
  71                         mata[x2][1] = matat[1][x2] = i_projection_mat_ref.m01;// mat_a->m[j*6+1]=mat_b->m[num*2+j*2]=cpara[0][1];\r
  72                         mata[x2 + 1][0] = matat[0][x2 + 1] = 0.0;// mat_a->m[j*6+3] =mat_b->m[num*0+j*2+1]= 0.0;\r
  73                         mata[x2 + 1][1] = matat[1][x2 + 1] = i_projection_mat_ref.m11;// mat_a->m[j*6+4] =mat_b->m[num*2+j*2+1]= cpara[1][1];\r
  74                 }\r
  75                 return;\r
  76         }\r
  77         /**\r
  78          * この関数は、射影変換後の２次元頂点座標をセットします。\r
  79          * i_number_of_vertexは４である必要があります。\r
  80          */\r
  81         public void set2dVertex(NyARDoublePoint2d[] i_ref_vertex_2d,int i_number_of_vertex) throws NyARException\r
  82         {               \r
  83                 assert(i_number_of_vertex==4);\r
  84                 final double[] cx=this._cx;\r
  85                 final double[] cy=this._cy;\r
  86                 final double cpara02=this._projection_mat.m02;\r
  87                 final double cpara12=this._projection_mat.m12;          \r
  88                 final NyARMat mat_t=this._mat_t;\r
  89                 final double[][] mata = this._mat_a.getArray();\r
  90                 final double[][] matat= this._mat_at.getArray();\r
  91                 for (int i = 0; i < 4; i++){\r
  92                         cx[i]=i_ref_vertex_2d[i].x;\r
  93                         cy[i]=i_ref_vertex_2d[i].y;\r
  94                         final int x2 = i * 2;   \r
  95                         mata[x2][2] = matat[2][x2] = cpara02 - i_ref_vertex_2d[i].x;// mat_a->m[j*6+2]=mat_b->m[num*4+j*2]=cpara[0][2]-pos2d[j][0];\r
  96                         mata[x2 + 1][2] = matat[2][x2 + 1] = cpara12 - i_ref_vertex_2d[i].y;// mat_a->m[j*6+5]=mat_b->m[num*4+j*2+1]=cpara[1][2]-pos2d[j][1];\r
  97                 }\r
  98                 //T(3x3行列)の作成\r
  99                 mat_t.matrixMul(this._mat_at, this._mat_a);\r
 100                 mat_t.matrixSelfInv();          \r
 101                 return;         \r
 102         }\r
 103         /**\r
 104          * 画面座標群と3次元座標群から、平行移動量を計算します。\r
 105          * 2d座標系は、直前に実行した{@link #set2dVertex}のものを使用します。\r
 106          */\r
 107         public void solveTransportVector(NyARDoublePoint3d[] i_vertex3d,NyARDoublePoint3d o_transfer) throws NyARException\r
 108         {\r
 109                 final double[][] matc = this._mat_c.getArray();\r
 110                 final double cpara00=this._projection_mat.m00;\r
 111                 final double cpara01=this._projection_mat.m01;\r
 112                 final double cpara02=this._projection_mat.m02;\r
 113                 final double cpara11=this._projection_mat.m11;\r
 114                 final double cpara12=this._projection_mat.m12;\r
 115                 final double[] cx=this._cx;\r
 116                 final double[] cy=this._cy;\r
 117                 \r
 118                 //（3D座標？）を一括請求\r
 119                 for (int i = 0; i < 4; i++) {\r
 120                         final int x2 = i+i;\r
 121                         final NyARDoublePoint3d point3d_ptr=i_vertex3d[i];\r
 122                         //透視変換？\r
 123                         matc[x2][0] = point3d_ptr.z * cx[i] - cpara00 * point3d_ptr.x - cpara01 * point3d_ptr.y - cpara02 * point3d_ptr.z;// mat_c->m[j*2+0] = wz*pos2d[j][0]-cpara[0][0]*wx-cpara[0][1]*wy-cpara[0][2]*wz;\r
 124                         matc[x2 + 1][0] = point3d_ptr.z * cy[i] - cpara11 * point3d_ptr.y - cpara12 * point3d_ptr.z;// mat_c->m[j*2+1]= wz*pos2d[j][1]-cpara[1][1]*wy-cpara[1][2]*wz;\r
 125                 }\r
 126                 this._mat_e.matrixMul(this._mat_at,this._mat_c);\r
 127                 this._mat_f.matrixMul(this._mat_t, this._mat_e);\r
 128                 \r
 129                 final double[][] matf = this._mat_f.getArray();\r
 130                 o_transfer.x= matf[0][0];// trans[0] = mat_f->m[0];\r
 131                 o_transfer.y= matf[1][0];\r
 132                 o_transfer.z= matf[2][0];// trans[2] = mat_f->m[2];\r
 133                 return;         \r
 134         }\r
 135 }\r