tags/2.0.0/src/jp/nyatla/nyartoolkit/core/param/NyARPerspectiveProjectionMatrix.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.core.param;\r
  33 \r
  34 import jp.nyatla.nyartoolkit.core.*;\r
  35 import jp.nyatla.nyartoolkit.core.types.matrix.NyARDoubleMatrix34;\r
  36 \r
  37 /**\r
  38  * 透視変換行列を格納します。\r
  39  * http://www.hitl.washington.edu/artoolkit/Papers/ART02-Tutorial.pdf\r
  40  * 7ページを見るといいよ。\r
  41  *\r
  42  */\r
  43 final public class NyARPerspectiveProjectionMatrix extends NyARDoubleMatrix34\r
  44 {\r
  45         /*\r
  46          * static double dot( double a1, double a2, double a3,double b1, double b2,double b3 )\r
  47          */\r
  48         private final static double dot(double a1, double a2, double a3, double b1,double b2, double b3)\r
  49         {\r
  50                 return (a1 * b1 + a2 * b2 + a3 * b3);\r
  51         }\r
  52 \r
  53         /* static double norm( double a, double b, double c ) */\r
  54         private final static double norm(double a, double b, double c)\r
  55         {\r
  56                 return Math.sqrt(a * a + b * b + c * c);\r
  57         }\r
  58 \r
  59         /**\r
  60          * int arParamDecompMat( double source[3][4], double cpara[3][4], double trans[3][4] ); 関数の置き換え Optimize STEP[754->665]\r
  61          * \r
  62          * @param o_cpara\r
  63          *            戻り引数。3x4のマトリクスを指定すること。\r
  64          * @param o_trans\r
  65          *            戻り引数。3x4のマトリクスを指定すること。\r
  66          * @return\r
  67          */\r
  68         public void decompMat(NyARMat o_cpara, NyARMat o_trans)\r
  69         {\r
  70                 double rem1, rem2, rem3;\r
  71                 double c00,c01,c02,c03,c10,c11,c12,c13,c20,c21,c22,c23;\r
  72                 if (this.m23>= 0) {// if( source[2][3] >= 0 ) {\r
  73                         // <Optimize>\r
  74                         // for(int r = 0; r < 3; r++ ){\r
  75                         // for(int c = 0; c < 4; c++ ){\r
  76                         // Cpara[r][c]=source[r][c];//Cpara[r][c] = source[r][c];\r
  77                         // }\r
  78                         // }\r
  79                         c00=this.m00;\r
  80                         c01=this.m01;\r
  81                         c02=this.m02;\r
  82                         c03=this.m03;\r
  83                         c10=this.m10;\r
  84                         c11=this.m11;\r
  85                         c12=this.m12;\r
  86                         c13=this.m13;\r
  87                         c20=this.m20;\r
  88                         c21=this.m21;\r
  89                         c22=this.m22;\r
  90                         c23=this.m23;\r
  91                 } else {\r
  92                         // <Optimize>\r
  93                         // for(int r = 0; r < 3; r++ ){\r
  94                         // for(int c = 0; c < 4; c++ ){\r
  95                         // Cpara[r][c]=-source[r][c];//Cpara[r][c] = -(source[r][c]);\r
  96                         // }\r
  97                         // }\r
  98                         c00=-this.m00;\r
  99                         c01=-this.m01;\r
 100                         c02=-this.m02;\r
 101                         c03=-this.m03;\r
 102                         c10=-this.m10;\r
 103                         c11=-this.m11;\r
 104                         c12=-this.m12;\r
 105                         c13=-this.m13;\r
 106                         c20=-this.m20;\r
 107                         c21=-this.m21;\r
 108                         c22=-this.m22;\r
 109                         c23=-this.m23;\r
 110                 }\r
 111 \r
 112                 double[][] cpara = o_cpara.getArray();\r
 113                 double[][] trans = o_trans.getArray();\r
 114                 for (int r = 0; r < 3; r++) {\r
 115                         for (int c = 0; c < 4; c++) {\r
 116                                 cpara[r][c] = 0.0;// cpara[r][c] = 0.0;\r
 117                         }\r
 118                 }\r
 119                 cpara[2][2] = norm(c20, c21, c22);// cpara[2][2] =norm( Cpara[2][0],Cpara[2][1],Cpara[2][2]);\r
 120                 trans[2][0] = c20 / cpara[2][2];// trans[2][0] = Cpara[2][0] /cpara[2][2];\r
 121                 trans[2][1] = c21 / cpara[2][2];// trans[2][1] = Cpara[2][1] / cpara[2][2];\r
 122                 trans[2][2] = c22 / cpara[2][2];// trans[2][2] =Cpara[2][2] /cpara[2][2];\r
 123                 trans[2][3] = c23 / cpara[2][2];// trans[2][3] =Cpara[2][3] /cpara[2][2];\r
 124 \r
 125                 cpara[1][2] = dot(trans[2][0], trans[2][1], trans[2][2], c10, c11, c12);// cpara[1][2]=dot(trans[2][0],trans[2][1],trans[2][2],Cpara[1][0],Cpara[1][1],Cpara[1][2]);\r
 126                 rem1 = c10 - cpara[1][2] * trans[2][0];// rem1 =Cpara[1][0] -cpara[1][2] *trans[2][0];\r
 127                 rem2 = c11 - cpara[1][2] * trans[2][1];// rem2 =Cpara[1][1] -cpara[1][2] *trans[2][1];\r
 128                 rem3 = c12 - cpara[1][2] * trans[2][2];// rem3 =Cpara[1][2] -cpara[1][2] *trans[2][2];\r
 129                 cpara[1][1] = norm(rem1, rem2, rem3);// cpara[1][1] = norm( rem1,// rem2, rem3 );\r
 130                 trans[1][0] = rem1 / cpara[1][1];// trans[1][0] = rem1 / cpara[1][1];\r
 131                 trans[1][1] = rem2 / cpara[1][1];// trans[1][1] = rem2 / cpara[1][1];\r
 132                 trans[1][2] = rem3 / cpara[1][1];// trans[1][2] = rem3 / cpara[1][1];\r
 133 \r
 134                 cpara[0][2] = dot(trans[2][0], trans[2][1], trans[2][2], c00, c01, c02);// cpara[0][2] =dot(trans[2][0], trans[2][1],trans[2][2],Cpara[0][0],Cpara[0][1],Cpara[0][2]);\r
 135                 cpara[0][1] = dot(trans[1][0], trans[1][1], trans[1][2], c00, c01, c02);// cpara[0][1]=dot(trans[1][0],trans[1][1],trans[1][2],Cpara[0][0],Cpara[0][1],Cpara[0][2]);\r
 136                 rem1 = c00 - cpara[0][1] * trans[1][0] - cpara[0][2]* trans[2][0];// rem1 = Cpara[0][0] - cpara[0][1]*trans[1][0]- cpara[0][2]*trans[2][0];\r
 137                 rem2 = c01 - cpara[0][1] * trans[1][1] - cpara[0][2]* trans[2][1];// rem2 = Cpara[0][1] - cpara[0][1]*trans[1][1]- cpara[0][2]*trans[2][1];\r
 138                 rem3 = c02 - cpara[0][1] * trans[1][2] - cpara[0][2]* trans[2][2];// rem3 = Cpara[0][2] - cpara[0][1]*trans[1][2] - cpara[0][2]*trans[2][2];\r
 139                 cpara[0][0] = norm(rem1, rem2, rem3);// cpara[0][0] = norm( rem1,rem2, rem3 );\r
 140                 trans[0][0] = rem1 / cpara[0][0];// trans[0][0] = rem1 / cpara[0][0];\r
 141                 trans[0][1] = rem2 / cpara[0][0];// trans[0][1] = rem2 / cpara[0][0];\r
 142                 trans[0][2] = rem3 / cpara[0][0];// trans[0][2] = rem3 / cpara[0][0];\r
 143 \r
 144                 trans[1][3] = (c13 - cpara[1][2] * trans[2][3])/ cpara[1][1];// trans[1][3] = (Cpara[1][3] -cpara[1][2]*trans[2][3]) / cpara[1][1];\r
 145                 trans[0][3] = (c03 - cpara[0][1] * trans[1][3] - cpara[0][2]* trans[2][3])/ cpara[0][0];// trans[0][3] = (Cpara[0][3] -cpara[0][1]*trans[1][3]-cpara[0][2]*trans[2][3]) / cpara[0][0];\r
 146 \r
 147                 for (int r = 0; r < 3; r++) {\r
 148                         for (int c = 0; c < 3; c++) {\r
 149                                 cpara[r][c] /= cpara[2][2];// cpara[r][c] /= cpara[2][2];\r
 150                         }\r
 151                 }\r
 152                 return;\r
 153         }\r
 154         /**\r
 155          * int arParamChangeSize( ARParam *source, int xsize, int ysize, ARParam *newparam );\r
 156          * Matrixのスケールを変換します。\r
 157          * @param i_scale\r
 158          * \r
 159          */\r
 160         public void changeScale(double i_scale)\r
 161         {\r
 162                 this.m00=this.m00*i_scale;\r
 163                 this.m10=this.m10*i_scale;\r
 164                 this.m01=this.m01*i_scale;\r
 165                 this.m11=this.m11*i_scale;\r
 166                 this.m02=this.m02*i_scale;\r
 167                 this.m12=this.m12*i_scale;\r
 168                 this.m03=this.m03*i_scale;\r
 169                 this.m13=this.m13*i_scale;\r
 170                 //for (int i = 0; i < 4; i++) {\r
 171                 //      array34[0 * 4 + i] = array34[0 * 4 + i] * scale;// newparam->mat[0][i]=source->mat[0][i]* scale;\r
 172                 //      array34[1 * 4 + i] = array34[1 * 4 + i] * scale;// newparam->mat[1][i]=source->mat[1][i]* scale;\r
 173                 //      array34[2 * 4 + i] = array34[2 * 4 + i];// newparam->mat[2][i] = source->mat[2][i];\r
 174                 //}\r
 175                 return;\r
 176         }\r
 177 \r
 178         \r
 179         \r
 180         \r
 181 }\r