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