src/jp/nyatla/nyartoolkit/core/transmat/rotmatrix/NyARRotMatrix.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.rotmatrix;\r
  32 \r
  33 import jp.nyatla.nyartoolkit.NyARException;\r
  34 import jp.nyatla.nyartoolkit.core.param.NyARPerspectiveProjectionMatrix;\r
  35 import jp.nyatla.nyartoolkit.core.transmat.NyARTransMatResult;\r
  36 import jp.nyatla.nyartoolkit.core.types.*;\r
  37 import jp.nyatla.nyartoolkit.core.types.matrix.NyARDoubleMatrix33;\r
  38 /**\r
  39  * 回転行列計算用の、3x3行列\r
  40  *\r
  41  */\r
  42 public class NyARRotMatrix extends NyARDoubleMatrix33\r
  43 {\r
  44         /**\r
  45          * インスタンスを準備します。\r
  46          * \r
  47          * @param i_param\r
  48          */\r
  49         public NyARRotMatrix(NyARPerspectiveProjectionMatrix i_matrix) throws NyARException\r
  50         {\r
  51                 this.__initRot_vec1=new NyARRotVector(i_matrix);\r
  52                 this.__initRot_vec2=new NyARRotVector(i_matrix);\r
  53                 return;\r
  54         }\r
  55         final private NyARRotVector __initRot_vec1;\r
  56         final private NyARRotVector __initRot_vec2;\r
  57         /**\r
  58          * NyARTransMatResultの内容からNyARRotMatrixを復元します。\r
  59          * @param i_prev_result\r
  60          */\r
  61         public final void initRotByPrevResult(NyARTransMatResult i_prev_result)\r
  62         {\r
  63 \r
  64                 this.m00=i_prev_result.m00;\r
  65                 this.m01=i_prev_result.m01;\r
  66                 this.m02=i_prev_result.m02;\r
  67 \r
  68                 this.m10=i_prev_result.m10;\r
  69                 this.m11=i_prev_result.m11;\r
  70                 this.m12=i_prev_result.m12;\r
  71 \r
  72                 this.m20=i_prev_result.m20;\r
  73                 this.m21=i_prev_result.m21;\r
  74                 this.m22=i_prev_result.m22;\r
  75                 return;\r
  76         }       \r
  77         /**\r
  78          * \r
  79          * @param i_linear\r
  80          * @param i_sqvertex\r
  81          * @throws NyARException\r
  82          */\r
  83         public void initRotBySquare(final NyARLinear[] i_linear,final NyARDoublePoint2d[] i_sqvertex) throws NyARException\r
  84         {\r
  85                 final NyARRotVector vec1=this.__initRot_vec1;\r
  86                 final NyARRotVector vec2=this.__initRot_vec2;\r
  87 \r
  88                 //向かい合った辺から、２本のベクトルを計算\r
  89                 \r
  90                 //軸１\r
  91                 vec1.exteriorProductFromLinear(i_linear[0], i_linear[2]);\r
  92                 vec1.checkVectorByVertex(i_sqvertex[0], i_sqvertex[1]);\r
  93 \r
  94                 //軸２\r
  95                 vec2.exteriorProductFromLinear(i_linear[1], i_linear[3]);\r
  96                 vec2.checkVectorByVertex(i_sqvertex[3], i_sqvertex[0]);\r
  97 \r
  98                 //回転の最適化？\r
  99                 NyARRotVector.checkRotation(vec1,vec2);\r
 100 \r
 101                 this.m00 =vec1.v1;\r
 102                 this.m10 =vec1.v2;\r
 103                 this.m20 =vec1.v3;\r
 104                 this.m01 =vec2.v1;\r
 105                 this.m11 =vec2.v2;\r
 106                 this.m21 =vec2.v3;\r
 107                 \r
 108                 //最後の軸を計算\r
 109                 final double w02 = vec1.v2 * vec2.v3 - vec1.v3 * vec2.v2;\r
 110                 final double w12 = vec1.v3 * vec2.v1 - vec1.v1 * vec2.v3;\r
 111                 final double w22 = vec1.v1 * vec2.v2 - vec1.v2 * vec2.v1;\r
 112                 final double w = Math.sqrt(w02 * w02 + w12 * w12 + w22 * w22);\r
 113                 this.m02 = w02/w;\r
 114                 this.m12 = w12/w;\r
 115                 this.m22 = w22/w;\r
 116                 return;\r
 117         }\r
 118         /**\r
 119          * i_in_pointを変換行列で座標変換する。\r
 120          * @param i_in_point\r
 121          * @param i_out_point\r
 122          */\r
 123         public final void getPoint3d(final NyARDoublePoint3d i_in_point,final NyARDoublePoint3d i_out_point)\r
 124         {\r
 125                 final double x=i_in_point.x;\r
 126                 final double y=i_in_point.y;\r
 127                 final double z=i_in_point.z;\r
 128                 i_out_point.x=this.m00 * x + this.m01 * y + this.m02 * z;\r
 129                 i_out_point.y=this.m10 * x + this.m11 * y + this.m12 * z;\r
 130                 i_out_point.z=this.m20 * x + this.m21 * y + this.m22 * z;\r
 131                 return;\r
 132         }\r
 133         /**\r
 134          * 複数の頂点を一括して変換する\r
 135          * @param i_in_point\r
 136          * @param i_out_point\r
 137          * @param i_number_of_vertex\r
 138          */\r
 139         public final void getPoint3dBatch(final NyARDoublePoint3d[] i_in_point,NyARDoublePoint3d[] i_out_point,int i_number_of_vertex)\r
 140         {\r
 141                 for(int i=i_number_of_vertex-1;i>=0;i--){\r
 142                         final NyARDoublePoint3d out_ptr=i_out_point[i];\r
 143                         final NyARDoublePoint3d in_ptr=i_in_point[i];\r
 144                         final double x=in_ptr.x;\r
 145                         final double y=in_ptr.y;\r
 146                         final double z=in_ptr.z;\r
 147                         out_ptr.x=this.m00 * x + this.m01 * y + this.m02 * z;\r
 148                         out_ptr.y=this.m10 * x + this.m11 * y + this.m12 * z;\r
 149                         out_ptr.z=this.m20 * x + this.m21 * y + this.m22 * z;\r
 150                 }\r
 151                 return;\r
 152         }\r
 153 }\r