tags/2.2.0/sample/sandbox/jp/nyatla/nyartoolkit/sandbox/x2/NyARFixedFloatFitVecCalculator.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 version ARToolkit class library.\r
  11  * Copyright (C)2008 R.Iizuka\r
  12  *\r
  13  * This program is free software; you can redistribute it and/or\r
  14  * modify it under the terms of the GNU General Public License\r
  15  * as published by the Free Software Foundation; either version 2\r
  16  * of the License, or (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 framework; if not, write to the Free Software\r
  25  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA\r
  26  * \r
  27  * For further information please contact.\r
  28  *      http://nyatla.jp/nyatoolkit/\r
  29  *      <airmail(at)ebony.plala.or.jp>\r
  30  * \r
  31  */\r
  32 package jp.nyatla.nyartoolkit.sandbox.x2;\r
  33 \r
  34 import jp.nyatla.nyartoolkit.*;\r
  35 import jp.nyatla.nyartoolkit.core.param.*;\r
  36 import jp.nyatla.nyartoolkit.core2.types.NyARFixedFloat16Point2d;\r
  37 import jp.nyatla.nyartoolkit.core2.types.NyARFixedFloat16Point3d;\r
  38 import jp.nyatla.nyartoolkit.core.*;\r
  39 \r
  40 class NyARCustomMatrix extends NyARMat\r
  41 {\r
  42         public NyARCustomMatrix(int i_row, int i_clm)\r
  43         {\r
  44                 super(i_row,i_clm);\r
  45                 return;\r
  46         }\r
  47         public void copyFrom(NyARFixedFloat16Mat i_mat) throws NyARException\r
  48         {\r
  49                 long[][] ptr;\r
  50                 int r,c;\r
  51                 // For順変更禁止\r
  52                 for (r = 0; r < this.row; r++){\r
  53                         ptr=i_mat.getArray();\r
  54                         for (c = 0; c < this.clm; c++){\r
  55                                 this.m[c][r]=(double)ptr[c][r]/0x10000;\r
  56                         }\r
  57                 }\r
  58                 return;\r
  59         }\r
  60 }\r
  61 /**\r
  62  * 平行移動量を計算するクラス\r
  63  * \r
  64  * NyARPerspectiveProjectionMatrixに直接アクセスしてる場所があるけど、\r
  65  * この辺の計算はNyARPerspectiveProjectionMatrixクラスの関数にして押し込む予定。\r
  66  *\r
  67  */\r
  68 public class NyARFixedFloatFitVecCalculator\r
  69 {\r
  70         private final NyARFixedFloat16Mat _mat_b = new NyARFixedFloat16Mat(3,8);//3,NUMBER_OF_VERTEX*2\r
  71         private final NyARFixedFloat16Mat _mat_a = new NyARFixedFloat16Mat(8,3);/*NUMBER_OF_VERTEX,3*/\r
  72         private final NyARFixedFloat16Mat _mat_d = new NyARFixedFloat16Mat(3,3);\r
  73         private final NyARCustomMatrix _mat_d2=new NyARCustomMatrix(3,3);\r
  74         private final NyARPerspectiveProjectionMatrix _projection_mat;\r
  75         private final NyARFixedFloatIdeal2Observ _distortionfactor;\r
  76 \r
  77 \r
  78 //      private NyARDoublePoint2d[] _vertex_2d_ref;\r
  79         public NyARFixedFloatFitVecCalculator(final NyARPerspectiveProjectionMatrix i_projection_mat_ref,final NyARCameraDistortionFactor i_distortion_ref)\r
  80         {\r
  81                 // 変換マトリクスdとbの準備(arGetTransMatSubの一部)\r
  82                 final long[][] a_array = this._mat_a.getArray();\r
  83                 final long[][] b_array = this._mat_b.getArray();\r
  84 \r
  85                 //変換用行列のcpara固定値の部分を先に初期化してしまう。\r
  86                 for (int i = 0; i < 4; i++) {\r
  87                         final int x2 = i * 2;\r
  88                         a_array[x2][0] = b_array[0][x2] =(long)(i_projection_mat_ref.m00*NyMath.FIXEDFLOAT16_1);// mat_a->m[j*6+0]=mat_b->m[num*0+j*2] =cpara[0][0];\r
  89                         a_array[x2][1] = b_array[1][x2] =(long)(i_projection_mat_ref.m01*NyMath.FIXEDFLOAT16_1);// mat_a->m[j*6+1]=mat_b->m[num*2+j*2]=cpara[0][1];\r
  90                         a_array[x2 + 1][0] = b_array[0][x2 + 1] =0;// mat_a->m[j*6+3] =mat_b->m[num*0+j*2+1]= 0.0;\r
  91                         a_array[x2 + 1][1] = b_array[1][x2 + 1] =(long)(i_projection_mat_ref.m11*NyMath.FIXEDFLOAT16_1);// mat_a->m[j*6+4] =mat_b->m[num*2+j*2+1]= cpara[1][1];\r
  92                 }\r
  93                 this._projection_mat=i_projection_mat_ref;\r
  94                 this._distortionfactor=new NyARFixedFloatIdeal2Observ(i_distortion_ref);\r
  95                 return;\r
  96         }\r
  97         private final NyARFixedFloat16Point2d[] _fitsquare_vertex=NyARFixedFloat16Point2d.createArray(4);;\r
  98         private NyARFixedFloatTransOffset _offset_square;\r
  99         public void setOffsetSquare(NyARFixedFloatTransOffset i_offset)\r
 100         {\r
 101                 this._offset_square=i_offset;\r
 102                 return;\r
 103         }\r
 104         public NyARFixedFloat16Point2d[] getFitSquare()\r
 105         {\r
 106                 return this._fitsquare_vertex;\r
 107         }\r
 108         public NyARFixedFloatTransOffset getOffsetVertex()\r
 109         {\r
 110                 return this._offset_square;\r
 111         }\r
 112 \r
 113         /**\r
 114          * 適合させる矩形座標を指定します。\r
 115          * @param i_square_vertex\r
 116          * @throws NyARException\r
 117          */\r
 118         public void setFittedSquare(NyARFixedFloat16Point2d[] i_square_vertex) throws NyARException\r
 119         {\r
 120                 final NyARFixedFloat16Point2d[] vertex=_fitsquare_vertex;\r
 121                 for(int i=0;i<4;i++){\r
 122                         this._distortionfactor.ideal2Observ(i_square_vertex[i], vertex[i]);\r
 123                 }               \r
 124                 \r
 125                 final long cpara02=(long)(this._projection_mat.m02*NyMath.FIXEDFLOAT16_1);\r
 126                 final long cpara12=(long)(this._projection_mat.m12*NyMath.FIXEDFLOAT16_1);              \r
 127                 final NyARFixedFloat16Mat mat_d=_mat_d;\r
 128                 final NyARFixedFloat16Mat mat_a=this._mat_a;\r
 129                 final NyARFixedFloat16Mat mat_b=this._mat_b;\r
 130                 final long[][] a_array = mat_a.getArray();\r
 131                 final long[][] b_array = mat_b.getArray();\r
 132                 for (int i = 0; i < 4; i++) {\r
 133                         final int x2 = i * 2;   \r
 134                         a_array[x2][2] = b_array[2][x2] = (long)((cpara02 - vertex[i].x));// mat_a->m[j*6+2]=mat_b->m[num*4+j*2]=cpara[0][2]-pos2d[j][0];\r
 135                         a_array[x2 + 1][2] = b_array[2][x2 + 1] = (long)((cpara12 - vertex[i].y));// mat_a->m[j*6+5]=mat_b->m[num*4+j*2+1]=cpara[1][2]-pos2d[j][1];\r
 136                 }\r
 137                 // mat_d\r
 138                 mat_d.matrixMul(mat_b, mat_a);\r
 139                 this._mat_d2.copyFrom(mat_d);\r
 140                 this._mat_d2.matrixSelfInv();           \r
 141                 return;\r
 142         }\r
 143         private final NyARFixedFloat16Mat _mat_e = new NyARFixedFloat16Mat(3, 1);\r
 144         private final NyARFixedFloat16Mat __calculateTransferVec_mat_c = new NyARFixedFloat16Mat(8, 1);//NUMBER_OF_VERTEX * 2, 1\r
 145         private final NyARFixedFloat16Point3d[] __calculateTransfer_point3d=NyARFixedFloat16Point3d.createArray(4);     \r
 146         \r
 147         /**\r
 148          * 現在のオフセット矩形、適合先矩形と、回転行列から、平行移動量を計算します。\r
 149          * @param i_rotation\r
 150          * @param o_transfer\r
 151          * @throws NyARException\r
 152          */\r
 153         final public void calculateTransfer(NyARFixedFloatRotMatrix i_rotation,NyARFixedFloat16Point3d o_transfer) throws NyARException\r
 154         {\r
 155                 assert(this._offset_square!=null);\r
 156                 final long cpara00=(long)(this._projection_mat.m00*NyMath.FIXEDFLOAT16_1);\r
 157                 final long cpara01=(long)(this._projection_mat.m01*NyMath.FIXEDFLOAT16_1);\r
 158                 final long cpara02=(long)(this._projection_mat.m02*NyMath.FIXEDFLOAT16_1);\r
 159                 final long cpara11=(long)(this._projection_mat.m11*NyMath.FIXEDFLOAT16_1);\r
 160                 final long cpara12=(long)(this._projection_mat.m12*NyMath.FIXEDFLOAT16_1);\r
 161                 \r
 162                 final NyARFixedFloat16Point3d[] point3d=this.__calculateTransfer_point3d;\r
 163                 final NyARFixedFloat16Point3d[] vertex3d=this._offset_square.vertex;            \r
 164                 final NyARFixedFloat16Point2d[] vertex2d=this._fitsquare_vertex;\r
 165                 final NyARFixedFloat16Mat mat_c = this.__calculateTransferVec_mat_c;// 次処理で値をもらうので、初期化の必要は無い。\r
 166         \r
 167                 final long[][] c_array = mat_c.getArray();\r
 168                 \r
 169                 \r
 170                 //（3D座標？）を一括請求\r
 171                 i_rotation.getPoint3dBatch(vertex3d,point3d,4);\r
 172                 for (int i = 0; i < 4; i++) {\r
 173                         final int x2 = i+i;\r
 174                         final NyARFixedFloat16Point3d point3d_ptr=point3d[i];\r
 175                         //透視変換？\r
 176                         c_array[x2][0] =(long)((point3d_ptr.z * vertex2d[i].x - cpara00 * point3d_ptr.x - cpara01 * point3d_ptr.y - cpara02 * point3d_ptr.z)>>16);// mat_c->m[j*2+0] = wz*pos2d[j][0]-cpara[0][0]*wx-cpara[0][1]*wy-cpara[0][2]*wz;\r
 177                         c_array[x2 + 1][0] =(long)((point3d_ptr.z * vertex2d[i].y - cpara11 * point3d_ptr.y - cpara12 * point3d_ptr.z)>>16);// mat_c->m[j*2+1]= wz*pos2d[j][1]-cpara[1][1]*wy-cpara[1][2]*wz;\r
 178                 }\r
 179                 this._mat_e.matrixMul(this._mat_b, mat_c);\r
 180                 final double[][] d2=this._mat_d2.getArray();\r
 181                 final long[][] e2=this._mat_e.getArray();\r
 182                 \r
 183                 \r
 184                 //this._mat_f.matrixMul(this._mat_d, this._mat_e);\r
 185 \r
 186                 // double[] trans=wk_arGetTransMatSub_trans;//double trans[3];\r
 187                 o_transfer.x=(long)(d2[0][0]*e2[0][0]+d2[0][1]*e2[1][0]+d2[0][2]*e2[2][0]);\r
 188                 o_transfer.y=(long)(d2[1][0]*e2[0][0]+d2[1][1]*e2[1][0]+d2[1][2]*e2[2][0]);\r
 189                 o_transfer.z=(long)(d2[2][0]*e2[0][0]+d2[2][1]*e2[1][0]+d2[2][2]*e2[2][0]);\r
 190                 return;\r
 191         }\r
 192         \r
 193         \r
 194         \r
 195 }\r