public NyARTransportVectorSolver(NyARPerspectiveProjectionMatrix i_projection_mat_ref, int i_max_vertex) { this._projection_mat = i_projection_mat_ref; this._cx = new double[i_max_vertex]; this._cy = new double[i_max_vertex]; return; }
/** * インスタンスを準備します。 * * @param i_param */ public NyARRotMatrix_ARToolKit(NyARPerspectiveProjectionMatrix i_matrix):base(i_matrix) { this.__initRot_vec1 = new NyARRotVector(i_matrix); this.__initRot_vec2 = new NyARRotVector(i_matrix); this._angle = new NyARDoublePoint3d(); return; }
/** * この関数は、コンストラクタから呼び出してください。 * @param i_distfactor * 歪みの逆矯正に使うオブジェクト。 * @param i_projmat * @ */ private void initInstance(INyARCameraDistortionFactor i_distfactor, NyARPerspectiveProjectionMatrix i_projmat) { this._transsolver = new NyARTransportVectorSolver(i_projmat, 4); //互換性が重要な時は、NyARRotMatrix_ARToolKitを使うこと。 //理屈はNyARRotMatrix_NyARToolKitもNyARRotMatrix_ARToolKitも同じだけど、少しだけ値がずれる。 this._rotmatrix = new NyARRotMatrix(i_projmat); this._mat_optimize = new NyARPartialDifferentiationOptimize(i_projmat); this._ref_dist_factor = i_distfactor; this._ref_projection_mat = i_projmat; return; }
public NyARTransMat(NyARParam i_param) { NyARCameraDistortionFactor dist = i_param.getDistortionFactor(); NyARPerspectiveProjectionMatrix pmat = i_param.getPerspectiveProjectionMatrix(); this._transsolver = new NyARTransportVectorSolver(pmat, 4); //互換性が重要な時は、NyARRotMatrix_ARToolKitを使うこと。 //理屈はNyARRotMatrix_NyARToolKitもNyARRotMatrix_ARToolKitも同じだけど、少しだけ値がずれる。 this._rotmatrix = new NyARRotMatrix(pmat); this._mat_optimize = new NyARPartialDifferentiationOptimize(pmat); this._ref_dist_factor = dist; this._projection_mat_ref = pmat; }
public NyARTransportVectorSolver_ARToolKit(NyARPerspectiveProjectionMatrix i_projection_mat_ref) { this._projection_mat=i_projection_mat_ref; //aとb(aの転置行列)の固定部分を設定。 double[][] mata = this._mat_a.getArray(); double[][] matat = this._mat_at.getArray(); //変換用行列のcpara部分を先に作成 for (int i = 0; i < 4; i++) { int x2 = i * 2; mata[x2][0] = matat[0][x2] = i_projection_mat_ref.m00;// mat_a->m[j*6+0]=mat_b->m[num*0+j*2] =cpara[0][0]; mata[x2][1] = matat[1][x2] = i_projection_mat_ref.m01;// mat_a->m[j*6+1]=mat_b->m[num*2+j*2]=cpara[0][1]; mata[x2 + 1][0] = matat[0][x2 + 1] = 0.0;// mat_a->m[j*6+3] =mat_b->m[num*0+j*2+1]= 0.0; mata[x2 + 1][1] = matat[1][x2 + 1] = i_projection_mat_ref.m11;// mat_a->m[j*6+4] =mat_b->m[num*2+j*2+1]= cpara[1][1]; } return; }
public NyARRotVector(NyARPerspectiveProjectionMatrix i_cmat) { NyARMat mat_a = new NyARMat(3, 3); double[][] a_array = mat_a.getArray(); a_array[0][0] = i_cmat.m00; a_array[0][1] = i_cmat.m01; a_array[0][2] = i_cmat.m02; a_array[1][0] = i_cmat.m10; a_array[1][1] = i_cmat.m11; a_array[1][2] = i_cmat.m12; a_array[2][0] = i_cmat.m20; a_array[2][1] = i_cmat.m21; a_array[2][2] = i_cmat.m22; mat_a.matrixSelfInv(); this._projection_mat_ref = i_cmat; this._inv_cpara_array_ref = mat_a.getArray(); //GCない言語のときは、ここで配列の所有権委譲してね! }
/** * この関数は、ARToolKitスタイルのProjectionMatrixから、 CameraFrustamを計算します。 * @param i_promat * @param i_size * スクリーンサイズを指定します。 * @param i_scale * {@link #toCameraFrustumRH(NyARParam i_arparam,double i_scale,double i_near,double i_far,double[] o_gl_projection)}を参照。 * @param i_near * {@link #toCameraFrustumRH(NyARParam i_arparam,double i_scale,double i_near,double i_far,double[] o_gl_projection)}を参照。 * @param i_far * {@link #toCameraFrustumRH(NyARParam i_arparam,double i_scale,double i_near,double i_far,double[] o_gl_projection)}を参照。 * @param o_gl_projection * {@link #toCameraFrustumRH(NyARParam i_arparam,double i_scale,double i_near,double i_far,double[] o_gl_projection)}を参照。 */ public static void toCameraFrustumRH(NyARPerspectiveProjectionMatrix i_promat,NyARIntSize i_size,double i_scale,double i_near,double i_far,ref Matrix4x4 o_mat) { NyARDoubleMatrix44 m=new NyARDoubleMatrix44(); i_promat.makeCameraFrustumRH(i_size.w,i_size.h,i_near*i_scale,i_far*i_scale,m); o_mat.m00=(float)m.m00; o_mat.m01=(float)m.m01; o_mat.m02=(float)m.m02; o_mat.m03=(float)m.m03; o_mat.m10=(float)m.m10; o_mat.m11=(float)m.m11; o_mat.m12=(float)m.m12; o_mat.m13=(float)m.m13; o_mat.m20=(float)m.m20; o_mat.m21=(float)m.m21; o_mat.m22=(float)m.m22; o_mat.m23=(float)m.m23; o_mat.m30=(float)m.m30; o_mat.m31=(float)m.m31; o_mat.m32=(float)m.m32; o_mat.m33=(float)m.m33; return; }
/** * コンストラクタ。 * @param i_screen * スクリーン(入力画像)のサイズを指定します。 * @param i_near * {@link #NyARReality(NyARParam i_param,double i_near,double i_far,int i_max_known_target,int i_max_unknown_target)}を参照 * @param i_far * {@link #NyARReality(NyARParam i_param,double i_near,double i_far,int i_max_known_target,int i_max_unknown_target)}を参照 * @param i_prjmat * ARToolKit形式の射影変換パラメータを指定します。 * @param i_dist_factor * カメラ歪み矯正オブジェクトを指定します。歪み矯正が不要な時は、nullを指定します。 * @param i_max_known_target * {@link #NyARReality(NyARParam i_param,double i_near,double i_far,int i_max_known_target,int i_max_unknown_target)}を参照 * @param i_max_unknown_target * {@link #NyARReality(NyARParam i_param,double i_near,double i_far,int i_max_known_target,int i_max_unknown_target)}を参照 * @throws NyARException */ public NyARReality(NyARIntSize i_screen,double i_near,double i_far,NyARPerspectiveProjectionMatrix i_prjmat,INyARCameraDistortionFactor i_dist_factor,int i_max_known_target,int i_max_unknown_target) { this.MAX_LIMIT_KNOWN=i_max_known_target; this.MAX_LIMIT_UNKNOWN=i_max_unknown_target; this.initInstance(i_screen,i_near,i_far,i_prjmat,i_dist_factor); }
/** * コンストラクタです。 * 射影変換オブジェクトの参照値を設定して、インスタンスを作成します。 * @param i_cmat * 射影変換オブジェクト。この値はインスタンスの生存中は変更しないでください。 * @throws NyARException */ public NyARRotVectorV2(NyARPerspectiveProjectionMatrix i_cmat):base() { this._inv_cpara.inverse(i_cmat); this._projection_mat_ref = i_cmat; }
/** * インスタンスを準備します。 * * @param i_param */ public NyARRotMatrix_ARToolKit_O2(NyARPerspectiveProjectionMatrix i_matrix) : base(i_matrix) { return; }
/** * ストリームから読み出したデータでインスタンスを初期化します。 * @param i_stream * @throws NyARException */ public ParamLoader(StreamReader i_stream) { try { //読み出し ByteBufferedInputStream bis=new ByteBufferedInputStream(i_stream,512); int s=bis.readToBuffer(512); bis.order(ByteBufferedInputStream.ENDIAN_BIG); //読み出したサイズでバージョンを決定 int[] version_table={136,144,152,176}; int version=-1; for(int i=0;i<version_table.Length;i++){ if(s%version_table[i]==0){ version=i+1; break; } } //一致しなければ無し if(version==-1){ throw new NyARException(); } //size this.size=new NyARIntSize(); this.size.setValue(bis.getInt(),bis.getInt()); //projection matrix this.pmat=new NyARPerspectiveProjectionMatrix(); double[] pjv=new double[16]; for(int i=0;i<12;i++){ pjv[i]=bis.getDouble(); } pjv[12]=pjv[13]=pjv[14]=0; pjv[15]=1; this.pmat.setValue(pjv); //dist factor double[] df; switch(version) { case 1://Version1 df=new double[NyARCameraDistortionFactorV2.NUM_OF_FACTOR]; this.dist_factor=new NyARCameraDistortionFactorV2(); break; case 4://Version4 df=new double[NyARCameraDistortionFactorV4.NUM_OF_FACTOR]; this.dist_factor=new NyARCameraDistortionFactorV4(); break; default: throw new NyARException(); } for(int i=0;i<df.Length;i++){ df[i]=bis.getDouble(); } this.dist_factor.setValue(df); } catch (Exception e) { throw new NyARException(e); } }
/** * intrinsic_matrixとdistortion_coeffsからインスタンスを初期化する。 * @param i_w * カメラパラメータ生成時の画面サイズ * @param i_h * カメラパラメータ生成時の画面サイズ * @param i_intrinsic_matrix 3x3 matrix このパラメータは、OpenCVのcvCalibrateCamera2関数が出力するintrinsic_matrixの値と合致します。 * @param i_distortion_coeffs 4x1 vector このパラメータは、OpenCVのcvCalibrateCamera2関数が出力するdistortion_coeffsの値と合致します。 */ public ParamLoader(int i_w, int i_h, double[] i_intrinsic_matrix, double[] i_distortion_coeffs) { this.size = new NyARIntSize(i_w, i_h); //dist factor NyARCameraDistortionFactorV4 v4dist = new NyARCameraDistortionFactorV4(); v4dist.setValue(this.size, i_intrinsic_matrix, i_distortion_coeffs); double s = v4dist.getS(); this.dist_factor = v4dist; //projection matrix this.pmat = new NyARPerspectiveProjectionMatrix(); NyARDoubleMatrix33 r = new NyARDoubleMatrix33(); r.setValue(i_intrinsic_matrix); r.m00 /= s; r.m01 /= s; r.m10 /= s; r.m11 /= s; this.pmat.setValue(r, new NyARDoublePoint3d()); }
/** * コンストラクタです。 * 参照する射影変換オブジェクトを指定して、インスタンスを生成します。 * @param i_matrix * 参照する射影変換オブジェクト * @ */ public NyARRotMatrix_ARToolKit(NyARPerspectiveProjectionMatrix i_matrix) : base(i_matrix) { this._angle = new NyARDoublePoint3d(); return; }
/* カメラのプロジェクションMatrix(RH)を返します。 * このMatrixはMicrosoft.DirectX.Direct3d.Device.Transform.Projectionに設定できます。 */ public static void toCameraFrustumRH(NyARPerspectiveProjectionMatrix i_promat, NyARIntSize i_size, double i_scale, double i_near, double i_far, ref Matrix o_d3d_projection) { NyARDoubleMatrix44 m = new NyARDoubleMatrix44(); i_promat.makeCameraFrustumRH(i_size.w, i_size.h, i_near * i_scale, i_far * i_scale, m); NyARD3dUtil.mat44ToD3dMatrixT(m, ref o_d3d_projection); return; }
/** * 透視投影行列と視錐体パラメータを元に、インスタンスを作成します。 * この関数は、樽型歪み矯正を外部で行うときに使います。 * @param i_prjmat * ARToolKitスタイルのカメラパラメータです。通常は{@link NyARParam#getPerspectiveProjectionMatrix()}から得られた値を使います。 * @param i_screen_size * スクリーン(入力画像)のサイズです。通常は{@link NyARParam#getScreenSize()}から得られた値を使います。 * @param i_near * 視錐体のnear-pointをmm単位で指定します。 * default値は{@link #FRASTRAM_ARTK_NEAR}です。 * @param i_far * 視錐体のfar-pointをmm単位で指定します。 * default値は{@link #FRASTRAM_ARTK_FAR}です。 * @param i_max_known_target * @param i_max_unknown_target * @throws NyARException */ public NyARRealityD3d(NyARPerspectiveProjectionMatrix i_prjmat, NyARIntSize i_screen_size, double i_near, double i_far, int i_max_known_target, int i_max_unknown_target) : base(i_screen_size, i_near, i_far, i_prjmat, null, i_max_known_target, i_max_unknown_target) { }
/** * コンストラクタです。 * 射影変換オブジェクトの参照値を設定して、インスタンスを生成します。 * @param i_projection_mat_ref * 射影変換オブジェクトの参照値。 */ public NyARRotMatrixOptimize_O2(NyARPerspectiveProjectionMatrix i_projection_mat_ref) { this._projection_mat_ref = i_projection_mat_ref; return; }
/** * コンストラクタから呼び出す共通な初期化部分です。 * @param i_dist_factor * @param i_prjmat * @throws NyARException */ protected void initInstance(NyARIntSize i_screen,double i_near,double i_far,NyARPerspectiveProjectionMatrix i_prjmat,INyARCameraDistortionFactor i_dist_factor) { int number_of_reality_target=this.MAX_LIMIT_KNOWN+this.MAX_LIMIT_UNKNOWN; //演算インスタンス this._transmat=new NyARTransMat(i_dist_factor,i_prjmat); //データインスタンス this._pool=new NyARRealityTargetPool(number_of_reality_target,i_prjmat); this.target=new NyARRealityTargetList(number_of_reality_target); //Trackerの特性値 this._tracker=new NyARTracker((this.MAX_LIMIT_KNOWN+this.MAX_LIMIT_UNKNOWN)*2,1,this.MAX_LIMIT_KNOWN*2); //フラスタムの計算とスクリーンサイズの保存 this._ref_prjmat=i_prjmat; this._frustum=new NyARFrustum(i_prjmat,i_screen.w,i_screen.h, i_near, i_far); //初期化 this._number_of_dead=this._number_of_unknown=this._number_of_known=0; return; }
/** * コンストラクタです。 * 参照する射影変換オブジェクトを指定して、インスタンスを生成します。 * @param i_matrix * 参照する射影変換オブジェクト * @ */ public NyARRotMatrix_ARToolKit_O2(NyARPerspectiveProjectionMatrix i_matrix):base(i_matrix) { return; }
/** * コンストラクタです。 * 射影変換オブジェクトの参照値を設定して、インスタンスを生成します。 * @param i_projection_mat_ref * 射影変換オブジェクトの参照値。 */ public NyARPartialDifferentiationOptimize(NyARPerspectiveProjectionMatrix i_projection_mat_ref) { this._projection_mat_ref = i_projection_mat_ref; return; }
/** * コンストラクタです。 * 参照する射影変換オブジェクトを指定して、インスタンスを生成します。 * @param i_matrix * 参照する射影変換オブジェクト * @ */ public NyARRotMatrix(NyARPerspectiveProjectionMatrix i_matrix) { this.__initRot_vec1 = new NyARRotVectorV2(i_matrix); this.__initRot_vec2 = new NyARRotVectorV2(i_matrix); return; }
/** * コンストラクタです。 * 座標計算に必要なオブジェクトの参照値を元に、インスタンスを生成します。 * @param i_ref_distfactor * 樽型歪み矯正オブジェクトの参照値です。歪み矯正が不要な時は、nullを指定します。 * @param i_ref_projmat * 射影変換オブジェクトの参照値です。 * @ */ public NyARTransMat(INyARCameraDistortionFactor i_ref_distfactor, NyARPerspectiveProjectionMatrix i_ref_projmat) { initInstance(i_ref_distfactor, i_ref_projmat); return; }
/** * * @param i_width * 取得するビットマップの幅 * @param i_height * 取得するビットマップの解像度 * @param i_resolution * resolution of reading pixel per point. ---- 取得時の解像度。高解像度のときは1を指定してください。低解像度のときは2以上を指定します。 */ TransformedBitmapPickup(NyARPerspectiveProjectionMatrix i_ref_cparam, int i_width, int i_height, int i_resolution) : base(i_width, i_height, i_resolution, 0) { this._ref_perspective = i_ref_cparam; }
/** * 標準パラメータでインスタンスを初期化します。 * @throws NyARException */ public ParamLoader() { double[] df={318.5,263.5,26.2,1.0127565206658486}; double[] pj={ 700.9514702992245,0,316.5,0, 0,726.0941816535367,241.5,0.0, 0.0,0.0,1.0,0.0, 0.0,0.0,0.0,1.0}; this.size=new NyARIntSize(640,480); this.pmat=new NyARPerspectiveProjectionMatrix(); this.pmat.setValue(pj); this.dist_factor=new NyARCameraDistortionFactorV2(); this.dist_factor.setValue(df); }
/** * この関数は、ARToolKitスタイルの射影変換行列から視錐台を作成してセットします。 * @param i_artk_perspective_mat * ARToolKitスタイルの射影変換行列 * @param i_width * スクリーンサイズです。 * @param i_height * スクリーンサイズです。 * @param i_near * nearポイントをmm単位で指定します。 * @param i_far * farポイントをmm単位で指定します。 */ public void setValue(NyARPerspectiveProjectionMatrix i_artk_perspective_mat, int i_width, int i_height, double i_near, double i_far) { i_artk_perspective_mat.makeCameraFrustumRH(i_width, i_height, i_near, i_far, this._frustum_rh); this._inv_frustum_rh.inverse(this._frustum_rh); this._screen_size.setValue(i_width, i_height); }
public NyARParam(NyARIntSize i_screen_size,NyARPerspectiveProjectionMatrix i_projection_mat,INyARCameraDistortionFactor i_dist_factor) { this._screen_size=new NyARIntSize(i_screen_size); this._dist=i_dist_factor; this._projection_matrix=i_projection_mat; }
/** * コンストラクタです。 * ARToolkitの射影変換行列から、インスタンスを作ります。 * @param i_perspective_mat * @param i_width * スクリーンサイズです。 * @param i_height * スクリーンサイズです。 * @param i_near * 近平面までの距離です。単位はmm * @param i_far * 遠平面までの距離です。単位はmm */ public NyARFrustum(NyARPerspectiveProjectionMatrix i_perspective_mat, int i_width, int i_height, double i_near, double i_far) { this.setValue(i_perspective_mat, i_width, i_height, i_near, i_far); }
/** * この関数は、回転行列を最適化します。 * ARToolKitのarGetRotに相当します。 */ public double modifyMatrix(NyARRotMatrix_ARToolKit io_rot, NyARDoublePoint3d trans, NyARDoublePoint3d[] i_vertex3d, NyARDoublePoint2d[] i_vertex2d) { double factor; double a2, b2, c2; double h, x, y; double err, minerr = 0; int t1, t2, t3; int best_idx = 0; factor = 10.0 * Math.PI / 180.0; double rot0, rot1, rot2; double combo00, combo01, combo02, combo03, combo10, combo11, combo12, combo13, combo20, combo21, combo22, combo23; double combo02_2, combo02_5, combo02_8, combo02_11; double combo22_2, combo22_5, combo22_8, combo22_11; double combo12_2, combo12_5, combo12_8, combo12_11; // vertex展開 double VX00, VX01, VX02, VX10, VX11, VX12, VX20, VX21, VX22, VX30, VX31, VX32; VX00 = i_vertex3d[0].x; VX01 = i_vertex3d[0].y; VX02 = i_vertex3d[0].z; VX10 = i_vertex3d[1].x; VX11 = i_vertex3d[1].y; VX12 = i_vertex3d[1].z; VX20 = i_vertex3d[2].x; VX21 = i_vertex3d[2].y; VX22 = i_vertex3d[2].z; VX30 = i_vertex3d[3].x; VX31 = i_vertex3d[3].y; VX32 = i_vertex3d[3].z; double P2D00, P2D01, P2D10, P2D11, P2D20, P2D21, P2D30, P2D31; P2D00 = i_vertex2d[0].x; P2D01 = i_vertex2d[0].y; P2D10 = i_vertex2d[1].x; P2D11 = i_vertex2d[1].y; P2D20 = i_vertex2d[2].x; P2D21 = i_vertex2d[2].y; P2D30 = i_vertex2d[3].x; P2D31 = i_vertex2d[3].y; NyARPerspectiveProjectionMatrix prjmat = this._projection_mat_ref; double CP0 = prjmat.m00, CP1 = prjmat.m01, CP2 = prjmat.m02, CP4 = prjmat.m10, CP5 = prjmat.m11, CP6 = prjmat.m12, CP8 = prjmat.m20, CP9 = prjmat.m21, CP10 = prjmat.m22; combo03 = CP0 * trans.x + CP1 * trans.y + CP2 * trans.z + prjmat.m03; combo13 = CP4 * trans.x + CP5 * trans.y + CP6 * trans.z + prjmat.m13; combo23 = CP8 * trans.x + CP9 * trans.y + CP10 * trans.z + prjmat.m23; double CACA, SASA, SACA, CA, SA; double CACACB, SACACB, SASACB, CASB, SASB; double SACASC, SACACBSC, SACACBCC, SACACC; double[][] double1D = this.__modifyMatrix_double1D; double[] a_factor = double1D[1]; double[] sinb = double1D[2]; double[] cosb = double1D[3]; double[] b_factor = double1D[4]; double[] sinc = double1D[5]; double[] cosc = double1D[6]; double[] c_factor = double1D[7]; double w, w2; double wsin, wcos; NyARDoublePoint3d angle = io_rot.getAngle(); a2 = angle.x; b2 = angle.y; c2 = angle.z; // comboの3行目を先に計算 for (int i = 0; i < 10; i++) { minerr = 1000000000.0; // sin-cosテーブルを計算(これが外に出せるとは…。) for (int j = 0; j < 3; j++) { w2 = factor * (j - 1); w = a2 + w2; a_factor[j] = w; w = b2 + w2; b_factor[j] = w; sinb[j] = Math.Sin(w); cosb[j] = Math.Cos(w); w = c2 + w2; c_factor[j] = w; sinc[j] = Math.Sin(w); cosc[j] = Math.Cos(w); } // for (t1 = 0; t1 < 3; t1++) { SA = Math.Sin(a_factor[t1]); CA = Math.Cos(a_factor[t1]); // Optimize CACA = CA * CA; SASA = SA * SA; SACA = SA * CA; for (t2 = 0; t2 < 3; t2++) { wsin = sinb[t2]; wcos = cosb[t2]; CACACB = CACA * wcos; SACACB = SACA * wcos; SASACB = SASA * wcos; CASB = CA * wsin; SASB = SA * wsin; // comboの計算1 combo02 = CP0 * CASB + CP1 * SASB + CP2 * wcos; combo12 = CP4 * CASB + CP5 * SASB + CP6 * wcos; combo22 = CP8 * CASB + CP9 * SASB + CP10 * wcos; combo02_2 = combo02 * VX02 + combo03; combo02_5 = combo02 * VX12 + combo03; combo02_8 = combo02 * VX22 + combo03; combo02_11 = combo02 * VX32 + combo03; combo12_2 = combo12 * VX02 + combo13; combo12_5 = combo12 * VX12 + combo13; combo12_8 = combo12 * VX22 + combo13; combo12_11 = combo12 * VX32 + combo13; combo22_2 = combo22 * VX02 + combo23; combo22_5 = combo22 * VX12 + combo23; combo22_8 = combo22 * VX22 + combo23; combo22_11 = combo22 * VX32 + combo23; for (t3 = 0; t3 < 3; t3++) { wsin = sinc[t3]; wcos = cosc[t3]; SACASC = SACA * wsin; SACACC = SACA * wcos; SACACBSC = SACACB * wsin; SACACBCC = SACACB * wcos; rot0 = CACACB * wcos + SASA * wcos + SACACBSC - SACASC; rot1 = SACACBCC - SACACC + SASACB * wsin + CACA * wsin; rot2 = -CASB * wcos - SASB * wsin; combo00 = CP0 * rot0 + CP1 * rot1 + CP2 * rot2; combo10 = CP4 * rot0 + CP5 * rot1 + CP6 * rot2; combo20 = CP8 * rot0 + CP9 * rot1 + CP10 * rot2; rot0 = -CACACB * wsin - SASA * wsin + SACACBCC - SACACC; rot1 = -SACACBSC + SACASC + SASACB * wcos + CACA * wcos; rot2 = CASB * wsin - SASB * wcos; combo01 = CP0 * rot0 + CP1 * rot1 + CP2 * rot2; combo11 = CP4 * rot0 + CP5 * rot1 + CP6 * rot2; combo21 = CP8 * rot0 + CP9 * rot1 + CP10 * rot2; // err = 0.0; h = combo20 * VX00 + combo21 * VX01 + combo22_2; x = P2D00 - (combo00 * VX00 + combo01 * VX01 + combo02_2) / h; y = P2D01 - (combo10 * VX00 + combo11 * VX01 + combo12_2) / h; err += x * x + y * y; h = combo20 * VX10 + combo21 * VX11 + combo22_5; x = P2D10 - (combo00 * VX10 + combo01 * VX11 + combo02_5) / h; y = P2D11 - (combo10 * VX10 + combo11 * VX11 + combo12_5) / h; err += x * x + y * y; h = combo20 * VX20 + combo21 * VX21 + combo22_8; x = P2D20 - (combo00 * VX20 + combo01 * VX21 + combo02_8) / h; y = P2D21 - (combo10 * VX20 + combo11 * VX21 + combo12_8) / h; err += x * x + y * y; h = combo20 * VX30 + combo21 * VX31 + combo22_11; x = P2D30 - (combo00 * VX30 + combo01 * VX31 + combo02_11) / h; y = P2D31 - (combo10 * VX30 + combo11 * VX31 + combo12_11) / h; err += x * x + y * y; if (err < minerr) { minerr = err; a2 = a_factor[t1]; b2 = b_factor[t2]; c2 = c_factor[t3]; best_idx = t1 + t2 * 3 + t3 * 9; } } } } if (best_idx == (1 + 3 + 9)) { factor *= 0.5; } } io_rot.setAngle(a2, b2, c2); /* printf("factor = %10.5f\n", factor*180.0/MD_PI); */ return(minerr / 4); }