tags/2.5.0/sample/sandbox/jp/nyatla/nyartoolkit/sandbox/x2/NyARFixedFloatRotMatrix.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.NyARException;\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.NyARFixedFloat24Matrix33;\r
  38 import jp.nyatla.nyartoolkit.core.param.*;\r
  39 import jp.nyatla.nyartoolkit.core.types.NyARFixedFloat16Point2d;\r
  40 import jp.nyatla.nyartoolkit.core.types.NyARFixedFloat16Point3d;\r
  41 \r
  42 \r
  43 \r
  44 /**\r
  45  * NyARRotMatrix_NyARToolKitのFixedFloat版です。\r
  46  * \r
  47  */\r
  48 public class NyARFixedFloatRotMatrix extends NyARFixedFloat24Matrix33\r
  49 {\r
  50         private static int DIV0CANCEL=1;\r
  51         /**\r
  52          * インスタンスを準備します。\r
  53          * \r
  54          * @param i_param\r
  55          */\r
  56         public NyARFixedFloatRotMatrix(NyARPerspectiveProjectionMatrix i_matrix) throws NyARException\r
  57         {\r
  58                 this.__initRot_vec1 = new NyARFixedFloatRotVector(i_matrix);\r
  59                 this.__initRot_vec2 = new NyARFixedFloatRotVector(i_matrix);\r
  60                 return;\r
  61         }\r
  62 \r
  63         final private NyARFixedFloatRotVector __initRot_vec1;\r
  64         final private NyARFixedFloatRotVector __initRot_vec2;\r
  65         final private NyARFixedFloat16Point3d _angle=new NyARFixedFloat16Point3d();\r
  66 \r
  67         public final void initRotByPrevResult(NyARTransMatResult i_prev_result)\r
  68         {\r
  69                 this.m00 =(long)(i_prev_result.m00*0x1000000);\r
  70                 this.m01 =(long)(i_prev_result.m01*0x1000000);\r
  71                 this.m02 =(long)(i_prev_result.m02*0x1000000);\r
  72 \r
  73                 this.m10 =(long)(i_prev_result.m10*0x1000000);\r
  74                 this.m11 =(long)(i_prev_result.m11*0x1000000);\r
  75                 this.m12 =(long)(i_prev_result.m12*0x1000000);\r
  76 \r
  77                 this.m20 =(long)(i_prev_result.m20*0x1000000);\r
  78                 this.m21 =(long)(i_prev_result.m21*0x1000000);\r
  79                 this.m22 =(long)(i_prev_result.m22*0x1000000);\r
  80                 return;\r
  81         }\r
  82 \r
  83         public final void initRotBySquare(final NyARLinear[] i_linear, final NyARFixedFloat16Point2d[] i_sqvertex) throws NyARException\r
  84         {\r
  85                 final NyARFixedFloatRotVector vec1 = this.__initRot_vec1;\r
  86                 final NyARFixedFloatRotVector 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                 NyARFixedFloatRotVector.checkRotation(vec1, vec2);\r
 100         \r
 101 \r
 102                 this.m00 = vec1.v1<<8;\r
 103                 this.m10 = vec1.v2<<8;\r
 104                 this.m20 = vec1.v3<<8;\r
 105                 this.m01 = vec2.v1<<8;\r
 106                 this.m11 = vec2.v2<<8;\r
 107                 this.m21 = vec2.v3<<8;\r
 108                 \r
 109 \r
 110 \r
 111                 // 最後の軸を計算\r
 112                 final long w02 = (vec1.v2 * vec2.v3 - vec1.v3 * vec2.v2)>>16;//S16\r
 113                 final long w12 = (vec1.v3 * vec2.v1 - vec1.v1 * vec2.v3)>>16;//S16\r
 114                 final long w22 = (vec1.v1 * vec2.v2 - vec1.v2 * vec2.v1)>>16;//S16\r
 115                 final long w = NyMath.sqrtFixdFloat((w02 * w02 + w12 * w12 + w22 * w22)>>16,16);//S16\r
 116                 this.m02 = (w02<<24) / w;\r
 117                 this.m12 = (w12<<24) / w;\r
 118                 this.m22 = (w22<<24) / w;\r
 119         //Matrixからangleをロード\r
 120         this.updateAngleFromMatrix();\r
 121                 \r
 122                 return;\r
 123         }\r
 124     public NyARFixedFloat16Point3d refAngle()\r
 125     {\r
 126         return this._angle;\r
 127     }\r
 128         /**\r
 129          * int arGetAngle( double rot[3][3], double *wa, double *wb, double *wc ) Optimize:2008.04.20:STEP[481→433] 3x3変換行列から、回転角を復元して返します。\r
 130          * \r
 131          * @param o_angle\r
 132          * @return\r
 133          */\r
 134         private final void updateAngleFromMatrix()\r
 135         {\r
 136                 int a, b, c;\r
 137                 long sina, cosa, sinb, cosb, sinc, cosc;\r
 138 \r
 139                 if (this.m22 > NyMath.FIXEDFLOAT24_1) {// <Optimize/>if( rot[2][2] > 1.0 ) {\r
 140                         this.m22 = NyMath.FIXEDFLOAT24_1;// <Optimize/>rot[2][2] = 1.0;\r
 141                 } else if (this.m22 < -NyMath.FIXEDFLOAT24_1) {// <Optimize/>}else if( rot[2][2] < -1.0 ) {\r
 142                         this.m22 = -NyMath.FIXEDFLOAT24_1;// <Optimize/>rot[2][2] = -1.0;\r
 143                 }\r
 144                 cosb = this.m22;// <Optimize/>cosb = rot[2][2];\r
 145                 b=NyMath.acosFixedFloat16((int)cosb);\r
 146                 sinb = (long)NyMath.sinFixedFloat24(b);\r
 147                 final long rot02 = this.m02;\r
 148                 final long rot12 = this.m12;\r
 149                 if (sinb!=0) {\r
 150                         cosa = (rot02<<24) / sinb;// <Optimize/>cosa = rot[0][2] / sinb;\r
 151                         sina = (rot12<<24) / sinb;// <Optimize/>sina = rot[1][2] / sinb;\r
 152                         if (cosa > NyMath.FIXEDFLOAT24_1) {\r
 153                                 /* printf("cos(alph) = %f\n", cosa); */\r
 154                                 cosa = NyMath.FIXEDFLOAT24_1;\r
 155                                 sina = 0;\r
 156                         }\r
 157                         if (cosa < -NyMath.FIXEDFLOAT24_1) {\r
 158                                 /* printf("cos(alph) = %f\n", cosa); */\r
 159                                 cosa = -NyMath.FIXEDFLOAT24_1;\r
 160                                 sina = 0;\r
 161                         }\r
 162                         if (sina > NyMath.FIXEDFLOAT24_1) {\r
 163                                 /* printf("sin(alph) = %f\n", sina); */\r
 164                                 sina = NyMath.FIXEDFLOAT24_1;\r
 165                                 cosa = 0;\r
 166                         }\r
 167                         if (sina < -NyMath.FIXEDFLOAT24_1) {\r
 168                                 /* printf("sin(alph) = %f\n", sina); */\r
 169                                 sina = -NyMath.FIXEDFLOAT24_1;\r
 170                                 cosa = 0;\r
 171                         }\r
 172                         a = NyMath.acosFixedFloat16((int)cosa);\r
 173                         if (sina < 0) {\r
 174                                 a = -a;\r
 175                         }\r
 176                         // <Optimize>\r
 177                         // sinc = (rot[2][1]*rot[0][2]-rot[2][0]*rot[1][2])/(rot[0][2]*rot[0][2]+rot[1][2]*rot[1][2]);\r
 178                         // cosc = -(rot[0][2]*rot[2][0]+rot[1][2]*rot[2][1])/(rot[0][2]*rot[0][2]+rot[1][2]*rot[1][2]);\r
 179                         final long tmp = DIV0CANCEL+((rot02 * rot02 + rot12 * rot12)>>24);\r
 180                         sinc = (this.m21 * rot02 - this.m20 * rot12) / tmp;\r
 181                         cosc = -(rot02 * this.m20 + rot12 * this.m21) / tmp;\r
 182                         // </Optimize>\r
 183 \r
 184                         if (cosc > NyMath.FIXEDFLOAT24_1){\r
 185                                 /* printf("cos(r) = %f\n", cosc); */\r
 186                                 cosc = NyMath.FIXEDFLOAT24_1;\r
 187                                 sinc = 0;\r
 188                         }\r
 189                         if (cosc < -NyMath.FIXEDFLOAT24_1) {\r
 190                                 /* printf("cos(r) = %f\n", cosc); */\r
 191                                 cosc = -NyMath.FIXEDFLOAT24_1;\r
 192                                 sinc = 0;\r
 193                         }\r
 194                         if (sinc > NyMath.FIXEDFLOAT24_1) {\r
 195                                 /* printf("sin(r) = %f\n", sinc); */\r
 196                                 sinc = NyMath.FIXEDFLOAT24_1;\r
 197                                 cosc = 0;\r
 198                         }\r
 199                         if (sinc < -NyMath.FIXEDFLOAT24_1) {\r
 200                                 /* printf("sin(r) = %f\n", sinc); */\r
 201                                 sinc = -NyMath.FIXEDFLOAT24_1;\r
 202                                 cosc = 0;\r
 203                         }\r
 204                         c = (int)NyMath.acosFixedFloat16((int)cosc);\r
 205                         if (sinc < 0) {\r
 206                                 c = -c;\r
 207                         }\r
 208                 } else {\r
 209                         a = b = 0;\r
 210                         cosa = cosb = NyMath.FIXEDFLOAT24_1;\r
 211                         sina = sinb = 0;\r
 212                         cosc = this.m00;// cosc = rot[0];// <Optimize/>cosc = rot[0][0];\r
 213                         sinc = this.m01;// sinc = rot[1];// <Optimize/>sinc = rot[1][0];\r
 214                         if (cosc > NyMath.FIXEDFLOAT24_1) {\r
 215                                 /* printf("cos(r) = %f\n", cosc); */\r
 216                                 cosc = NyMath.FIXEDFLOAT24_1;\r
 217                                 sinc = 0;\r
 218                         }\r
 219                         if (cosc < -NyMath.FIXEDFLOAT24_1) {\r
 220                                 /* printf("cos(r) = %f\n", cosc); */\r
 221                                 cosc = -NyMath.FIXEDFLOAT24_1;\r
 222                                 sinc = 0;\r
 223                         }\r
 224                         if (sinc > NyMath.FIXEDFLOAT24_1) {\r
 225                                 /* printf("sin(r) = %f\n", sinc); */\r
 226                                 sinc = NyMath.FIXEDFLOAT24_1;\r
 227                                 cosc = 0;\r
 228                         }\r
 229                         if (sinc < -NyMath.FIXEDFLOAT24_1) {\r
 230                                 /* printf("sin(r) = %f\n", sinc); */\r
 231                                 sinc = -NyMath.FIXEDFLOAT24_1;\r
 232                                 cosc = 0;\r
 233                         }\r
 234                         c = NyMath.acosFixedFloat16((int)cosc);\r
 235                         if (sinc < 0) {\r
 236                                 c = -c;\r
 237                         }\r
 238                 }\r
 239                 this._angle.x = (long)a;// wa.value=a;//*wa = a;\r
 240                 this._angle.y = (long)b;// wb.value=b;//*wb = b;\r
 241                 this._angle.z = (long)c;// wc.value=c;//*wc = c;\r
 242                 return;\r
 243         }\r
 244         public final void setAngle(int i_x, int i_y, int i_z)\r
 245         {\r
 246                 /*\r
 247                  * |cos(a) -sin(a) 0| |cos(b) 0 sin(b)| |cos(a-c) sin(a-c) 0| rot = |sin(a) cos(a) 0| |0 1 0 | |-sin(a-c) cos(a-c) 0| |0 0 1| |-sin(b) 0 cos(b)| |0 0 1|\r
 248                  */\r
 249 \r
 250                 long Sa, Sb, Ca, Cb, Sac, Cac, CaCb, SaCb;\r
 251                 Sa = NyMath.sinFixedFloat24(i_x);\r
 252                 Ca = NyMath.cosFixedFloat24(i_x);\r
 253                 Sb = NyMath.sinFixedFloat24(i_y);\r
 254                 Cb = NyMath.cosFixedFloat24(i_y);\r
 255                 Sac = NyMath.sinFixedFloat24(i_x - i_z);\r
 256                 Cac = NyMath.cosFixedFloat24(i_x - i_z);\r
 257                 CaCb =(Ca * Cb)>>24;\r
 258                 SaCb =(Sa * Cb)>>24;\r
 259 \r
 260                 this.m00 =(CaCb * Cac + Sa * Sac)>>24;\r
 261                 this.m01 =(CaCb * Sac - Sa * Cac)>>24;\r
 262                 this.m02 =(Ca * Sb)>>24;\r
 263                 this.m10 =(SaCb * Cac - Ca * Sac)>>24;\r
 264                 this.m11 =(SaCb * Sac + Ca * Cac)>>24;\r
 265                 this.m12 =(Sa * Sb)>>24;\r
 266                 this.m20 =(-Sb * Cac)>>24;\r
 267                 this.m21 =(-Sb * Sac)>>24;\r
 268                 this.m22 =Cb;\r
 269                 //angleを逆計算せずに直接代入\r
 270                 this._angle.x=i_x;\r
 271                 this._angle.y=i_y;\r
 272                 this._angle.z=i_z;              \r
 273                 return;\r
 274         }\r
 275         /**\r
 276          * i_in_pointを変換行列で座標変換する。\r
 277          * \r
 278          * @param i_in_point\r
 279          * @param i_out_point\r
 280          */\r
 281         public final void getPoint3d(final NyARFixedFloat16Point3d i_in_point, final NyARFixedFloat16Point3d i_out_point)\r
 282         {\r
 283                 i_out_point.x = (this.m00 * i_in_point.x + this.m01 * i_in_point.y + this.m02 * i_in_point.z)>>24;\r
 284                 i_out_point.y = (this.m10 * i_in_point.x + this.m11 * i_in_point.y + this.m12 * i_in_point.z)>>24;\r
 285                 i_out_point.z = (this.m20 * i_in_point.x + this.m21 * i_in_point.y + this.m22 * i_in_point.z)>>24;\r
 286                 return;\r
 287         }\r
 288 \r
 289         /**\r
 290          * 複数の頂点を一括して変換する\r
 291          * \r
 292          * @param i_in_point\r
 293          * @param i_out_point\r
 294          * @param i_number_of_vertex\r
 295          */\r
 296         public final void getPoint3dBatch(final NyARFixedFloat16Point3d[] i_in_point, NyARFixedFloat16Point3d[] i_out_point, int i_number_of_vertex)\r
 297         {\r
 298                 for (int i = i_number_of_vertex - 1; i >= 0; i--) {\r
 299                         final NyARFixedFloat16Point3d out_ptr = i_out_point[i];\r
 300                         final NyARFixedFloat16Point3d in_ptr = i_in_point[i];\r
 301                         out_ptr.x =(this.m00 * in_ptr.x + this.m01 * in_ptr.y + this.m02 * in_ptr.z)>>24;\r
 302                         out_ptr.y =(this.m10 * in_ptr.x + this.m11 * in_ptr.y + this.m12 * in_ptr.z)>>24;\r
 303                         out_ptr.z =(this.m20 * in_ptr.x + this.m21 * in_ptr.y + this.m22 * in_ptr.z)>>24;\r
 304                 }\r
 305                 return;\r
 306         }\r
 307 }\r