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(NyARPerspectiveProjectionMatrix i_matrix)
: base(i_matrix)
{
this._angle = new NyARDoublePoint3d();
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_projection_mat_ref
* 射影変換オブジェクトの参照値。
*/
public NyARRotMatrixOptimize_O2(NyARPerspectiveProjectionMatrix i_projection_mat_ref)
{
this._projection_mat_ref = i_projection_mat_ref;
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);
}
/**
* コンストラクタです。
* 射影変換オブジェクトの参照値を設定して、インスタンスを生成します。
* @param i_projection_mat_ref
* 射影変換オブジェクトの参照値。
*/
public NyARPartialDifferentiationOptimize(NyARPerspectiveProjectionMatrix i_projection_mat_ref)
{
this._projection_mat_ref = i_projection_mat_ref;
return;
}
/**
* この関数は、回転行列を最適化します。
* 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);
}
