src/jp/nyatla/nyartoolkit/core/transmat/rotmatrix/NyARRotMatrix.java

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