Details | Last modification | View Log | RSS feed
| Rev | Author | Line No. | Line |
|---|---|---|---|
| 203 | chris | 1 | /* |
| 2 | * PROJECT: NyARToolkit |
||
| 3 | * -------------------------------------------------------------------------------- |
||
| 4 | * This work is based on the original ARToolKit developed by |
||
| 5 | * Hirokazu Kato |
||
| 6 | * Mark Billinghurst |
||
| 7 | * HITLab, University of Washington, Seattle |
||
| 8 | * http://www.hitl.washington.edu/artoolkit/ |
||
| 9 | * |
||
| 10 | * The NyARToolkit is Java version ARToolkit class library. |
||
| 11 | * Copyright (C)2008 R.Iizuka |
||
| 12 | * |
||
| 13 | * This program is free software; you can redistribute it and/or |
||
| 14 | * modify it under the terms of the GNU General Public License |
||
| 15 | * as published by the Free Software Foundation; either version 2 |
||
| 16 | * of the License, or (at your option) any later version. |
||
| 17 | * |
||
| 18 | * This program is distributed in the hope that it will be useful, |
||
| 19 | * but WITHOUT ANY WARRANTY; without even the implied warranty of |
||
| 20 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
||
| 21 | * GNU General Public License for more details. |
||
| 22 | * |
||
| 23 | * You should have received a copy of the GNU General Public License |
||
| 24 | * along with this framework; if not, write to the Free Software |
||
| 25 | * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA |
||
| 26 | * |
||
| 27 | * For further information please contact. |
||
| 28 | * http://nyatla.jp/nyatoolkit/ |
||
| 29 | * <airmail(at)ebony.plala.or.jp> |
||
| 30 | * |
||
| 31 | */ |
||
| 32 | package jp.nyatla.nyartoolkit.core; |
||
| 33 | |||
| 34 | |||
| 35 | import java.io.*; |
||
| 36 | import java.nio.*; |
||
| 37 | |||
| 38 | import jp.nyatla.nyartoolkit.NyARException; |
||
| 39 | import jp.nyatla.util.DoubleValue; |
||
| 40 | |||
| 41 | /*typedef struct { |
||
| 42 | int xsize, ysize; |
||
| 43 | double mat[3][4]; |
||
| 44 | double dist_factor[4]; |
||
| 45 | } ARParam;*/ |
||
| 46 | public class NyARParam{ |
||
| 47 | private static final int SIZE_OF_PARAM_SET=4+4+(3*4*8)+(4*8); |
||
| 48 | private static final int PD_LOOP = 3; |
||
| 49 | protected int xsize, ysize; |
||
| 50 | private double[] array34=new double[3*4];//Double2dArray mat=new Double2dArray(3,4); |
||
| 51 | private double[] dist_factor=new double[4]; |
||
| 52 | public int getX() |
||
| 53 | { |
||
| 54 | return xsize; |
||
| 55 | } |
||
| 56 | public int getY() |
||
| 57 | { |
||
| 58 | return ysize; |
||
| 59 | } |
||
| 60 | public double[] getDistFactor() |
||
| 61 | { |
||
| 62 | return dist_factor; |
||
| 63 | } |
||
| 64 | /** |
||
| 65 | * パラメタを格納した[4x3]配列を返します。 |
||
| 66 | * @return |
||
| 67 | */ |
||
| 68 | public final double[] get34Array() |
||
| 69 | { |
||
| 70 | return array34; |
||
| 71 | } |
||
| 72 | /** |
||
| 73 | * ARToolKit標準ファイルから1個目の設定をロードする。 |
||
| 74 | * @param i_filename |
||
| 75 | * @throws NyARException |
||
| 76 | */ |
||
| 77 | public void loadFromARFile(String i_filename) throws NyARException |
||
| 78 | { |
||
| 79 | try { |
||
| 80 | loadFromARFile(new FileInputStream(i_filename)); |
||
| 81 | } catch (Exception e) { |
||
| 82 | throw new NyARException(e); |
||
| 83 | } |
||
| 84 | } |
||
| 85 | public void loadFromARFile(InputStream i_stream) throws NyARException |
||
| 86 | { |
||
| 87 | try { |
||
| 88 | NyARParam new_inst[] = arParamLoad(i_stream, 1); |
||
| 89 | i_stream.close(); |
||
| 90 | xsize = new_inst[0].xsize; |
||
| 91 | ysize = new_inst[0].ysize; |
||
| 92 | array34 = new_inst[0].array34; |
||
| 93 | dist_factor = new_inst[0].dist_factor; |
||
| 94 | } catch (Exception e) { |
||
| 95 | throw new NyARException(e); |
||
| 96 | } |
||
| 97 | } |
||
| 98 | /*static double dot( double a1, double a2, double a3,double b1, double b2, double b3 )*/ |
||
| 99 | private final static double dot( double a1, double a2, double a3,double b1, double b2, double b3 ) |
||
| 100 | { |
||
| 101 | return( a1 * b1 + a2 * b2 + a3 * b3 ); |
||
| 102 | } |
||
| 103 | /* static double norm( double a, double b, double c )*/ |
||
| 104 | private final static double norm( double a, double b, double c ) |
||
| 105 | { |
||
| 106 | return Math.sqrt( a*a + b*b + c*c ); |
||
| 107 | } |
||
| 108 | /** |
||
| 109 | * int arParamDecompMat( double source[3][4], double cpara[3][4], double trans[3][4] ); |
||
| 110 | * 関数の置き換え |
||
| 111 | * Optimize STEP[754->665] |
||
| 112 | * @param o_cpara |
||
| 113 | * 戻り引数。3x4のマトリクスを指定すること。 |
||
| 114 | * @param o_trans |
||
| 115 | * 戻り引数。3x4のマトリクスを指定すること。 |
||
| 116 | * @return |
||
| 117 | */ |
||
| 118 | public void decompMat(NyARMat o_cpara, NyARMat o_trans) |
||
| 119 | { |
||
| 120 | double[] source=array34; |
||
| 121 | double[] Cpara=new double[3*4];//double Cpara[3][4]; |
||
| 122 | double rem1, rem2, rem3; |
||
| 123 | int i; |
||
| 124 | if(source[2*4+3]>= 0 ){//if( source[2][3] >= 0 ) { |
||
| 125 | //<Optimize> |
||
| 126 | //for(int r = 0; r < 3; r++ ){ |
||
| 127 | // for(int c = 0; c < 4; c++ ){ |
||
| 128 | // Cpara[r][c]=source[r][c];//Cpara[r][c] = source[r][c]; |
||
| 129 | // } |
||
| 130 | //} |
||
| 131 | for(i=0;i<12;i++){ |
||
| 132 | Cpara[i]=source[i];//Cpara[r][c] = source[r][c]; |
||
| 133 | } |
||
| 134 | //</Optimize> |
||
| 135 | }else { |
||
| 136 | //<Optimize> |
||
| 137 | //for(int r = 0; r < 3; r++ ){ |
||
| 138 | // for(int c = 0; c < 4; c++ ){ |
||
| 139 | // Cpara[r][c]=-source[r][c];//Cpara[r][c] = -(source[r][c]); |
||
| 140 | // } |
||
| 141 | //} |
||
| 142 | for(i=0;i<12;i++){ |
||
| 143 | Cpara[i]=source[i];//Cpara[r][c] = source[r][c]; |
||
| 144 | } |
||
| 145 | //</Optimize> |
||
| 146 | } |
||
| 147 | |||
| 148 | double[][] cpara=o_cpara.getArray(); |
||
| 149 | double[][] trans=o_trans.getArray(); |
||
| 150 | for(int r = 0; r < 3; r++ ){ |
||
| 151 | for(int c = 0; c < 4; c++ ){ |
||
| 152 | cpara[r][c]=0.0;//cpara[r][c] = 0.0; |
||
| 153 | } |
||
| 154 | } |
||
| 155 | cpara[2][2]=norm(Cpara[2*4+0],Cpara[2*4+1], Cpara[2*4+2]);//cpara[2][2] = norm( Cpara[2][0], Cpara[2][1], Cpara[2][2] ); |
||
| 156 | trans[2][0]=Cpara[2*4+0] / cpara[2][2];//trans[2][0] = Cpara[2][0] / cpara[2][2]; |
||
| 157 | trans[2][1]=Cpara[2*4+1]/ cpara[2][2];//trans[2][1] = Cpara[2][1] / cpara[2][2]; |
||
| 158 | trans[2][2]=Cpara[2*4+2]/ cpara[2][2];//trans[2][2] = Cpara[2][2] / cpara[2][2]; |
||
| 159 | trans[2][3]=Cpara[2*4+3] / cpara[2][2];//trans[2][3] = Cpara[2][3] / cpara[2][2]; |
||
| 160 | |||
| 161 | cpara[1][2]=dot(trans[2][0], trans[2][1], trans[2][2],Cpara[1*4+0], Cpara[1*4+1], Cpara[1*4+2]);//cpara[1][2] = dot( trans[2][0], trans[2][1], trans[2][2],Cpara[1][0], Cpara[1][1], Cpara[1][2] ); |
||
| 162 | rem1=Cpara[1*4+0]- cpara[1][2] * trans[2][0];//rem1 = Cpara[1][0] - cpara[1][2] * trans[2][0]; |
||
| 163 | rem2=Cpara[1*4+1] - cpara[1][2] * trans[2][1];//rem2 = Cpara[1][1] - cpara[1][2] * trans[2][1]; |
||
| 164 | rem3=Cpara[1*4+2] - cpara[1][2] * trans[2][2];//rem3 = Cpara[1][2] - cpara[1][2] * trans[2][2]; |
||
| 165 | cpara[1][1]=norm(rem1, rem2, rem3 );//cpara[1][1] = norm( rem1, rem2, rem3 ); |
||
| 166 | trans[1][0]= rem1/cpara[1][1];//trans[1][0] = rem1 / cpara[1][1]; |
||
| 167 | trans[1][1]=rem2/cpara[1][1];//trans[1][1] = rem2 / cpara[1][1]; |
||
| 168 | trans[1][2]=rem3 / cpara[1][1];//trans[1][2] = rem3 / cpara[1][1]; |
||
| 169 | |||
| 170 | cpara[0][2]=dot(trans[2][0],trans[2][1], trans[2][2],Cpara[0*4+0], Cpara[0*4+1], Cpara[0*4+2] );//cpara[0][2] = dot( trans[2][0], trans[2][1], trans[2][2],Cpara[0][0], Cpara[0][1], Cpara[0][2] ); |
||
| 171 | cpara[0][1]=dot(trans[1][0],trans[1][1], trans[1][2],Cpara[0*4+0], Cpara[0*4+1], Cpara[0*4+2]);//cpara[0][1] = dot( trans[1][0], trans[1][1], trans[1][2],Cpara[0][0], Cpara[0][1], Cpara[0][2] ); |
||
| 172 | rem1=Cpara[0*4+0]- 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]; |
||
| 173 | rem2 = Cpara[0*4+1] - 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]; |
||
| 174 | rem3 = Cpara[0*4+2] - 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]; |
||
| 175 | cpara[0][0]=norm(rem1, rem2, rem3);//cpara[0][0] = norm( rem1, rem2, rem3 ); |
||
| 176 | trans[0][0]=rem1 / cpara[0][0];//trans[0][0] = rem1 / cpara[0][0]; |
||
| 177 | trans[0][1]= rem2 / cpara[0][0];//trans[0][1] = rem2 / cpara[0][0]; |
||
| 178 | trans[0][2]= rem3 / cpara[0][0];//trans[0][2] = rem3 / cpara[0][0]; |
||
| 179 | |||
| 180 | trans[1][3]=(Cpara[1*4+3] - cpara[1][2]*trans[2][3]) / cpara[1][1];//trans[1][3] = (Cpara[1][3] - cpara[1][2]*trans[2][3]) / cpara[1][1]; |
||
| 181 | trans[0][3]=(Cpara[0*4+3] - 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]; |
||
| 182 | |||
| 183 | for(int r = 0; r < 3; r++ ){ |
||
| 184 | for(int c = 0; c < 3; c++ ){ |
||
| 185 | cpara[r][c]/=cpara[2][2];//cpara[r][c] /= cpara[2][2]; |
||
| 186 | } |
||
| 187 | } |
||
| 188 | } |
||
| 189 | |||
| 190 | |||
| 191 | /*int arParamDisp( ARParam *param );*/ |
||
| 192 | public int paramDisp() |
||
| 193 | { |
||
| 194 | System.out.println("--------------------------------------");//printf("--------------------------------------\n"); |
||
| 195 | System.out.print("SIZE = "+xsize+", "+ysize);//printf("SIZE = %d, %d\n", param->xsize, param->ysize); |
||
| 196 | System.out.println("Distortion factor = "+dist_factor[0]+" "+dist_factor[1]+" "+dist_factor[2]+" "+dist_factor[3]);//printf("Distortion factor = %f %f %f %f\n", param->dist_factor[0],param->dist_factor[1], param->dist_factor[2], param->dist_factor[3] ); |
||
| 197 | for(int j = 0; j < 3; j++ ) {//for(j = 0; j < 3; j++ ) { |
||
| 198 | for(int i = 0; i < 4; i++ ){ |
||
| 199 | System.out.print(array34[j*4+i]+" ");//printf("%7.5f ", param->mat[j][i]); |
||
| 200 | } |
||
| 201 | System.out.println();// printf("\n"); |
||
| 202 | }//} |
||
| 203 | System.out.println("--------------------------------------");//printf("--------------------------------------\n"); |
||
| 204 | return 0; |
||
| 205 | } |
||
| 206 | // /*int arParamDecomp( ARParam *source, ARParam *icpara, double trans[3][4] );*/ |
||
| 207 | // private static int arParamDecomp( NyARParam source, NyARParam icpara, double[][] trans) |
||
| 208 | // { |
||
| 209 | // icpara.xsize = source.xsize;//icpara->xsize = source->xsize; |
||
| 210 | // icpara.ysize = source.ysize;//icpara->ysize = source->ysize; |
||
| 211 | // icpara.dist_factor[0] = source.dist_factor[0];//icpara->dist_factor[0] = source->dist_factor[0]; |
||
| 212 | // icpara.dist_factor[1] = source.dist_factor[1];// icpara->dist_factor[1] = source->dist_factor[1]; |
||
| 213 | // icpara.dist_factor[2] = source.dist_factor[2];//icpara->dist_factor[2] = source->dist_factor[2]; |
||
| 214 | // icpara.dist_factor[3] = source.dist_factor[3];//icpara->dist_factor[3] = source->dist_factor[3]; |
||
| 215 | // return arParamDecompMat(source.mat, icpara.mat, trans ); |
||
| 216 | // } |
||
| 217 | /** |
||
| 218 | * int arParamChangeSize( ARParam *source, int xsize, int ysize, ARParam *newparam ); |
||
| 219 | * 関数の代替関数 |
||
| 220 | * サイズプロパティをi_xsize,i_ysizeに変更します。 |
||
| 221 | * @param xsize |
||
| 222 | * @param ysize |
||
| 223 | * @param newparam |
||
| 224 | * @return |
||
| 225 | * |
||
| 226 | */ |
||
| 227 | public void changeSize(int i_xsize, int i_ysize) |
||
| 228 | { |
||
| 229 | double scale; |
||
| 230 | scale = (double)i_xsize / (double)(xsize);//scale = (double)xsize / (double)(source->xsize); |
||
| 231 | |||
| 232 | for(int i = 0; i < 4; i++ ) { |
||
| 233 | array34[0*4+i]=array34[0*4+i]*scale;//newparam->mat[0][i] = source->mat[0][i] * scale; |
||
| 234 | array34[1*4+i]=array34[1*4+i]*scale;//newparam->mat[1][i] = source->mat[1][i] * scale; |
||
| 235 | array34[2*4+i]=array34[2*4+i];//newparam->mat[2][i] = source->mat[2][i]; |
||
| 236 | } |
||
| 237 | |||
| 238 | dist_factor[0] = dist_factor[0] * scale;//newparam->dist_factor[0] = source->dist_factor[0] * scale; |
||
| 239 | dist_factor[1] = dist_factor[1] * scale;//newparam->dist_factor[1] = source->dist_factor[1] * scale; |
||
| 240 | dist_factor[2] = dist_factor[2] / (scale*scale);//newparam->dist_factor[2] = source->dist_factor[2] / (scale*scale); |
||
| 241 | dist_factor[3] = dist_factor[3];//newparam->dist_factor[3] = source->dist_factor[3]; |
||
| 242 | |||
| 243 | xsize = i_xsize;//newparam->xsize = xsize; |
||
| 244 | ysize = i_ysize;//newparam->ysize = ysize; |
||
| 245 | } |
||
| 246 | /** |
||
| 247 | * int arParamIdeal2Observ( const double dist_factor[4], const double ix, const double iy,double *ox, double *oy ) |
||
| 248 | * 関数の代替関数 |
||
| 249 | * @param ix |
||
| 250 | * @param iy |
||
| 251 | * @param ox |
||
| 252 | * @param oy |
||
| 253 | */ |
||
| 254 | public void ideal2Observ(double ix,double iy,DoubleValue ox, DoubleValue oy) |
||
| 255 | { |
||
| 256 | |||
| 257 | double x, y, d; |
||
| 258 | final double d0,d1,d3; |
||
| 259 | final double df[]=this.dist_factor; |
||
| 260 | d0=df[0]; |
||
| 261 | d1=df[1]; |
||
| 262 | d3=df[3]; |
||
| 263 | x = (ix - d0) * d3; |
||
| 264 | y = (iy - d1) * d3; |
||
| 265 | if( x == 0.0 && y == 0.0 ) { |
||
| 266 | ox.value=d0; |
||
| 267 | oy.value=d1; |
||
| 268 | }else{ |
||
| 269 | d = 1.0 - df[2]/100000000.0 * (x*x+y*y); |
||
| 270 | ox.value=x * d + d0; |
||
| 271 | oy.value=y * d + d1; |
||
| 272 | } |
||
| 273 | } |
||
| 274 | /** |
||
| 275 | * ideal2Observをまとめて実行します。 |
||
| 276 | * @param i_in |
||
| 277 | * double[][2] |
||
| 278 | * @param o_out |
||
| 279 | * double[][2] |
||
| 280 | */ |
||
| 281 | public void ideal2ObservBatch(double[][] i_in,double[][] o_out,int i_size) |
||
| 282 | { |
||
| 283 | |||
| 284 | double x, y, d; |
||
| 285 | final double d0,d1,d3,d2_w; |
||
| 286 | final double df[]=this.dist_factor; |
||
| 287 | d0=df[0]; |
||
| 288 | d1=df[1]; |
||
| 289 | d3=df[3]; |
||
| 290 | d2_w=df[2]/100000000.0; |
||
| 291 | for(int i=0;i<i_size;i++){ |
||
| 292 | x = (i_in[i][0] - d0) * d3; |
||
| 293 | y = (i_in[i][1] - d1) * d3; |
||
| 294 | if( x == 0.0 && y == 0.0 ) { |
||
| 295 | o_out[i][0]=d0; |
||
| 296 | o_out[i][1]=d1; |
||
| 297 | }else{ |
||
| 298 | d = 1.0 - d2_w * (x*x+y*y); |
||
| 299 | o_out[i][0]=x * d + d0; |
||
| 300 | o_out[i][1]=y * d + d1; |
||
| 301 | } |
||
| 302 | } |
||
| 303 | return; |
||
| 304 | } |
||
| 305 | |||
| 306 | |||
| 307 | |||
| 308 | |||
| 309 | /** |
||
| 310 | * int arParamObserv2Ideal( const double dist_factor[4], const double ox, const double oy,double *ix, double *iy ); |
||
| 311 | * |
||
| 312 | * @param ox |
||
| 313 | * @param oy |
||
| 314 | * @param ix |
||
| 315 | * @param iy |
||
| 316 | * @return |
||
| 317 | */ |
||
| 318 | public int observ2Ideal(double ox,double oy,DoubleValue ix,DoubleValue iy) |
||
| 319 | { |
||
| 320 | double z02, z0, p, q, z, px, py,opttmp_1; |
||
| 321 | final double d0,d1,d3; |
||
| 322 | final double df[]=this.dist_factor; |
||
| 323 | d0=df[0]; |
||
| 324 | d1=df[1]; |
||
| 325 | |||
| 326 | px = ox - d0; |
||
| 327 | py = oy - d1; |
||
| 328 | p = df[2]/100000000.0; |
||
| 329 | z02 = px*px+py*py; |
||
| 330 | q = z0 = Math.sqrt(z02);//Optimize//q = z0 = Math.sqrt(px*px+ py*py); |
||
| 331 | |||
| 332 | for(int i = 1; ; i++ ) { |
||
| 333 | if( z0 != 0.0 ) { |
||
| 334 | //Optimize opttmp_1 |
||
| 335 | opttmp_1=p*z02; |
||
| 336 | z = z0 - ((1.0 - opttmp_1)*z0 - q) / (1.0 - 3.0*opttmp_1); |
||
| 337 | px = px*z/z0; |
||
| 338 | py = py*z/z0; |
||
| 339 | }else { |
||
| 340 | px = 0.0; |
||
| 341 | py = 0.0; |
||
| 342 | break; |
||
| 343 | } |
||
| 344 | if( i == PD_LOOP ){ |
||
| 345 | break; |
||
| 346 | } |
||
| 347 | z02 = px*px+ py*py; |
||
| 348 | z0 = Math.sqrt(z02);//Optimize//z0 = Math.sqrt(px*px+ py*py); |
||
| 349 | } |
||
| 350 | d3=df[3]; |
||
| 351 | ix.value=px / d3 + d0; |
||
| 352 | iy.value=py / d3 + d1; |
||
| 353 | return 0; |
||
| 354 | } |
||
| 355 | /** |
||
| 356 | * 指定範囲のobserv2Idealをまとめて実行して、結果をo_idealに格納します。 |
||
| 357 | * @param i_x_coord |
||
| 358 | * @param i_y_coord |
||
| 359 | * @param i_start |
||
| 360 | * coord開始点 |
||
| 361 | * @param i_num |
||
| 362 | * 計算数 |
||
| 363 | * @param o_ideal |
||
| 364 | * 出力バッファ[i_num][2]であること。 |
||
| 365 | */ |
||
| 366 | public void observ2IdealBatch(int[] i_x_coord,int[] i_y_coord,int i_start,int i_num,double[][] o_ideal) |
||
| 367 | { |
||
| 368 | double z02, z0,q, z, px, py,opttmp_1; |
||
| 369 | final double df[]=this.dist_factor; |
||
| 370 | final double d0=df[0]; |
||
| 371 | final double d1=df[1]; |
||
| 372 | final double d3=df[3]; |
||
| 373 | final double p = df[2]/100000000.0; |
||
| 374 | for(int j = 0; j < i_num; j++ ){ |
||
| 375 | |||
| 376 | px = i_x_coord[i_start+j] - d0; |
||
| 377 | py = i_y_coord[i_start+j] - d1; |
||
| 378 | |||
| 379 | z02 = px*px+py*py; |
||
| 380 | q = z0 = Math.sqrt(z02);//Optimize//q = z0 = Math.sqrt(px*px+ py*py); |
||
| 381 | |||
| 382 | for(int i = 1; ; i++ ) { |
||
| 383 | if( z0 != 0.0 ) { |
||
| 384 | //Optimize opttmp_1 |
||
| 385 | opttmp_1=p*z02; |
||
| 386 | z = z0 - ((1.0 - opttmp_1)*z0 - q) / (1.0 - 3.0*opttmp_1); |
||
| 387 | px = px*z/z0; |
||
| 388 | py = py*z/z0; |
||
| 389 | }else { |
||
| 390 | px = 0.0; |
||
| 391 | py = 0.0; |
||
| 392 | break; |
||
| 393 | } |
||
| 394 | if( i == PD_LOOP ){ |
||
| 395 | break; |
||
| 396 | } |
||
| 397 | z02 = px*px+ py*py; |
||
| 398 | z0 = Math.sqrt(z02);//Optimize//z0 = Math.sqrt(px*px+ py*py); |
||
| 399 | } |
||
| 400 | o_ideal[j][0]=px / d3 + d0; |
||
| 401 | o_ideal[j][1]=py / d3 + d1; |
||
| 402 | } |
||
| 403 | } |
||
| 404 | |||
| 405 | |||
| 406 | |||
| 407 | |||
| 408 | /** |
||
| 409 | * int arParamLoad( const char *filename, int num, ARParam *param, ...); |
||
| 410 | * i_streamの入力ストリームからi_num個の設定を読み込み、パラメタを配列にして返します。 |
||
| 411 | * @param filename |
||
| 412 | * @param num |
||
| 413 | * @param param |
||
| 414 | * @return |
||
| 415 | * 設定を格納した配列を返します。 |
||
| 416 | * @throws Exception |
||
| 417 | * i_num個の設定が読み出せない場合、JartkExceptionを発生します。 |
||
| 418 | */ |
||
| 419 | private static NyARParam[] arParamLoad(InputStream i_stream, int i_num) throws NyARException |
||
| 420 | { |
||
| 421 | try{ |
||
| 422 | int read_size=SIZE_OF_PARAM_SET*i_num; |
||
| 423 | byte[] buf=new byte[read_size]; |
||
| 424 | i_stream.read(buf); |
||
| 425 | //返却配列を確保 |
||
| 426 | NyARParam[] result=new NyARParam[i_num]; |
||
| 427 | |||
| 428 | //バッファを加工 |
||
| 429 | ByteBuffer bb = ByteBuffer.wrap(buf); |
||
| 430 | bb.order(ByteOrder.BIG_ENDIAN); |
||
| 431 | |||
| 432 | //固定回数パースして配列に格納 |
||
| 433 | for(int i=0;i<i_num;i++){ |
||
| 434 | NyARParam new_param=new NyARParam();; |
||
| 435 | new_param.xsize=bb.getInt(); |
||
| 436 | new_param.ysize=bb.getInt(); |
||
| 437 | for(int i2=0;i2<3;i2++){ |
||
| 438 | for(int i3=0;i3<4;i3++){ |
||
| 439 | new_param.array34[i2*4+i3]=bb.getDouble(); |
||
| 440 | } |
||
| 441 | } |
||
| 442 | for(int i2=0;i2<4;i2++){ |
||
| 443 | new_param.dist_factor[i2]=bb.getDouble(); |
||
| 444 | } |
||
| 445 | result[i]=new_param; |
||
| 446 | } |
||
| 447 | return result; |
||
| 448 | }catch(Exception e){ |
||
| 449 | throw new NyARException(e); |
||
| 450 | } |
||
| 451 | } |
||
| 452 | public static int arParamSave(String filename,int num, NyARParam param[]) throws Exception |
||
| 453 | { |
||
| 454 | NyARException.trap("未チェックの関数"); |
||
| 455 | byte buf[]=new byte[SIZE_OF_PARAM_SET*param.length]; |
||
| 456 | //バッファをラップ |
||
| 457 | ByteBuffer bb = ByteBuffer.wrap(buf); |
||
| 458 | bb.order(ByteOrder.BIG_ENDIAN); |
||
| 459 | |||
| 460 | //書き込み |
||
| 461 | for(int i=0;i<param.length;i++){ |
||
| 462 | bb.putInt(param[i].xsize); |
||
| 463 | bb.putInt(param[i].ysize); |
||
| 464 | for(int i2=0;i2<3;i2++){ |
||
| 465 | for(int i3=0;i3<4;i3++){ |
||
| 466 | bb.putDouble(param[i].array34[i2*4+i3]); |
||
| 467 | } |
||
| 468 | } |
||
| 469 | for(int i2=0;i2<4;i2++){ |
||
| 470 | bb.putDouble(param[i].dist_factor[i2]); |
||
| 471 | } |
||
| 472 | } |
||
| 473 | //ファイルに保存 |
||
| 474 | FileOutputStream fs=new FileOutputStream(filename); |
||
| 475 | fs.write(buf); |
||
| 476 | fs.close(); |
||
| 477 | |||
| 478 | return 0; |
||
| 479 | } |
||
| 480 | } |