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  * 並進ベクトル[T]を３次元座標[b]と基点の回転済行列[M]から計算します。\r
  41  * ARToolKit互換の数値を計算します。\r
  42  *\r
  43  */\r
  44 public class NyARTransportVectorSolver_ARToolKit implements INyARTransportVectorSolver\r
  45 {\r
  46         private final NyARMat _mat_at = new NyARMat(3,8);//3,NUMBER_OF_VERTEX*2\r
  47         private final NyARMat _mat_a =  new NyARMat(8,3);//NUMBER_OF_VERTEX,3\r
  48         private final NyARMat _mat_t =  new NyARMat(3,3);//NUMBER_OF_VERTEX,3\r
  49         private final NyARMat _mat_c =  new NyARMat(8,1);//NUMBER_OF_VERTEX * 2, 1\r
  50         private final NyARMat _mat_e =  new NyARMat(3,1);\r
  51         private final NyARMat _mat_f =  new NyARMat(3,1);\r
  52         private double[] _cx=new double[4];\r
  53         private double[] _cy=new double[4];\r
  54         \r
  55         private final NyARPerspectiveProjectionMatrix _projection_mat;\r
  56         public NyARTransportVectorSolver_ARToolKit(final NyARPerspectiveProjectionMatrix i_projection_mat_ref)\r
  57         {\r
  58                 this._projection_mat=i_projection_mat_ref;\r
  59                 //aとb(aの転置行列)の固定部分を設定。\r
  60                 final double[][] mata = this._mat_a.getArray();\r
  61                 final double[][] matat = this._mat_at.getArray();\r
  62 \r
  63                 //変換用行列のcpara部分を先に作成\r
  64                 for (int i = 0; i < 4; i++) {\r
  65                         final int x2 = i * 2;\r
  66                         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
  67                         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
  68                         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
  69                         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
  70                 }\r
  71                 return;\r
  72         }\r
  73         public void set2dVertex(NyARDoublePoint2d[] i_ref_vertex_2d,int i_number_of_vertex) throws NyARException\r
  74         {               \r
  75                 assert(i_number_of_vertex==4);\r
  76                 final double[] cx=this._cx;\r
  77                 final double[] cy=this._cy;\r
  78                 final double cpara02=this._projection_mat.m02;\r
  79                 final double cpara12=this._projection_mat.m12;          \r
  80                 final NyARMat mat_t=this._mat_t;\r
  81                 final double[][] mata = this._mat_a.getArray();\r
  82                 final double[][] matat= this._mat_at.getArray();\r
  83                 for (int i = 0; i < 4; i++){\r
  84                         cx[i]=i_ref_vertex_2d[i].x;\r
  85                         cy[i]=i_ref_vertex_2d[i].y;\r
  86                         final int x2 = i * 2;   \r
  87                         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
  88                         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
  89                 }\r
  90                 //T(3x3行列)の作成\r
  91                 mat_t.matrixMul(this._mat_at, this._mat_a);\r
  92                 mat_t.matrixSelfInv();          \r
  93                 return;         \r
  94         }\r
  95         /**\r
  96          * 画面座標群と3次元座標群から、平行移動量を計算します。\r
  97          * 2d座標系は、直前に実行したset2dVertexのものを使用します。\r
  98          * @param i_vertex_2d\r
  99          * 直前のset2dVertexコールで指定したものと同じものを指定してください。\r
 100          * @param i_vertex3d\r
 101          * 3次元空間の座標群を設定します。頂点の順番は、画面座標群と同じ順序で格納してください。\r
 102          * @param o_transfer\r
 103          * @throws NyARException\r
 104          */\r
 105         public void solveTransportVector(NyARDoublePoint3d[] i_vertex3d,NyARDoublePoint3d o_transfer) throws NyARException\r
 106         {\r
 107                 final double[][] matc = this._mat_c.getArray();\r
 108                 final double cpara00=this._projection_mat.m00;\r
 109                 final double cpara01=this._projection_mat.m01;\r
 110                 final double cpara02=this._projection_mat.m02;\r
 111                 final double cpara11=this._projection_mat.m11;\r
 112                 final double cpara12=this._projection_mat.m12;\r
 113                 final double[] cx=this._cx;\r
 114                 final double[] cy=this._cy;\r
 115                 \r
 116                 //（3D座標？）を一括請求\r
 117                 for (int i = 0; i < 4; i++) {\r
 118                         final int x2 = i+i;\r
 119                         final NyARDoublePoint3d point3d_ptr=i_vertex3d[i];\r
 120                         //透視変換？\r
 121                         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
 122                         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
 123                 }\r
 124                 this._mat_e.matrixMul(this._mat_at,this._mat_c);\r
 125                 this._mat_f.matrixMul(this._mat_t, this._mat_e);\r
 126                 \r
 127                 final double[][] matf = this._mat_f.getArray();\r
 128                 o_transfer.x= matf[0][0];// trans[0] = mat_f->m[0];\r
 129                 o_transfer.y= matf[1][0];\r
 130                 o_transfer.z= matf[2][0];// trans[2] = mat_f->m[2];\r
 131                 return;         \r
 132         }\r
 133 }\r