//    $Revision: 1.1 $

using System;
using System.Collections.Generic;
using System.Text;
using fvalgcli;

namespace TC
{
    public partial class FIE
    {
        [FvPluginExecute]
        public void fnFIE_pfit_param()
        {
            int status = (int)f_err.F_ERR_NONE;

            // 点列(変換前と変換後).
            DPNT_T_PTR taWork = DPNT_T_PTR.Zero;
            DPNT_T_PTR taMaster = DPNT_T_PTR.Zero;
            int num = 4;    // 点列数.

            // 変換行列.
            FMATRIX_PTR mat_rotate = FMATRIX_PTR.Zero;
            FMATRIX_PTR mat_scale = FMATRIX_PTR.Zero;
            FMATRIX_PTR mat = FMATRIX_PTR.Zero;

            // 変換行列生成用パラメータ(回転).
            double rad = Math.PI / 2;
            double cx = 75;
            double cy = 75;

            // 変換行列生成用パラメータ(スケール).
            double sx = 2.0;
            double sy = 2.0;

            // 変換パラメータ [out].
            double dpXcOrg = 0;
            double dpYcOrg = 0;
            double dpXc = 0;
            double dpYc = 0;
            double dpTheta = 0;
            double dpScale = 0;

            const double ans_Scale = 0.5;

            try
            {
                // 行列の領域確保.
                mat_rotate = FMATRIX_PTR.alloc(3, 3);
                mat_scale = FMATRIX_PTR.alloc(3, 3);
                mat = FMATRIX_PTR.alloc(3, 3);

                // 点列の領域を確保.
                taWork = DPNT_T_PTR.alloc(num);
                taMaster = DPNT_T_PTR.alloc(num);

                // 回転及びスケールの変換行列を求める.
                api.fnFIE_geotrans_calc_rotate_matrix(mat_rotate, rad, cx, cy);
                api.fnFIE_geotrans_calc_scale_matrix(mat_scale, sx, sy);

                // 行列を掛け,1つの行列にまとめる.
                api.fnFIE_mat_mul_aa(mat_scale, mat_rotate, mat);

                // Workの座標を指定する.
                taWork[0] = DPNT_T.init(50, 50);
                taWork[1] = DPNT_T.init(100, 50);
                taWork[2] = DPNT_T.init(50, 100);
                taWork[3] = DPNT_T.init(100, 100);

                // 点列をアフィン変換して新座標系での点群とする.
                api.fnFIE_geotrans_affine_npoints(taWork, taMaster, num, mat);

                // 点群マッチングによる座標系パラメータ(原点座標，傾きおよびスケール)の算出.
                status = api.fnFIE_pfit_param(num, taWork, taMaster, 3, f_pfit_calc_mode.F_PFIT_RBST_SCALE,
                                              ref dpXcOrg, ref dpYcOrg, ref dpXc, ref dpYc, ref dpTheta, ref dpScale);
                Assert.IsTrue(status == (int)f_err.F_ERR_NONE, "エラーが発生しました。({0})", (f_err)status);

                // 結果を出力する.
                ConsoleOut.WriteFunctionName(":\n");
                Console.WriteLine(" dpXcOrg = {0}\n dpYcOrg = {1}\n dpXc = {2}\n dpYc = {3}\n dpTheta = {4}\n dpScale = {5}", 
                                  dpXcOrg, dpYcOrg, dpXc, dpYc, dpTheta, dpScale);
                Console.Write(" ...");
                ConsoleOut.IsTrue(ans_Scale == dpScale);
            }
            finally
            {
                mat_rotate.Dispose();
                mat_scale.Dispose();
                mat.Dispose();
                taWork.Dispose();
                taMaster.Dispose();
            }
        }
    }
}

'    $Revision: 1.1 $

Imports System.Collections.Generic
Imports System.Text
Imports fvalgcli

Public Partial Class FIE
    <FvPluginExecute> _
    Public Sub fnFIE_pfit_param()
        Dim status As Integer = CInt(f_err.F_ERR_NONE)

        ' 点列(変換前と変換後).
        Dim taWork As DPNT_T_PTR = DPNT_T_PTR.Zero
        Dim taMaster As DPNT_T_PTR = DPNT_T_PTR.Zero
        Dim num As Integer = 4
        ' 点列数.
        ' 変換行列.
        Dim mat_rotate As FMATRIX_PTR = FMATRIX_PTR.Zero
        Dim mat_scale As FMATRIX_PTR = FMATRIX_PTR.Zero
        Dim mat As FMATRIX_PTR = FMATRIX_PTR.Zero

        ' 変換行列生成用パラメータ(回転).
        Dim rad As Double = Math.PI / 2
        Dim cx As Double = 75
        Dim cy As Double = 75

        ' 変換行列生成用パラメータ(スケール).
        Dim sx As Double = 2.0
        Dim sy As Double = 2.0

        ' 変換パラメータ [out].
        Dim dpXcOrg As Double = 0
        Dim dpYcOrg As Double = 0
        Dim dpXc As Double = 0
        Dim dpYc As Double = 0
        Dim dpTheta As Double = 0
        Dim dpScale As Double = 0

        Const  ans_Scale As Double = 0.5

        Try
            ' 行列の領域確保.
            mat_rotate = FMATRIX_PTR.alloc(3, 3)
            mat_scale = FMATRIX_PTR.alloc(3, 3)
            mat = FMATRIX_PTR.alloc(3, 3)

            ' 点列の領域を確保.
            taWork = DPNT_T_PTR.alloc(num)
            taMaster = DPNT_T_PTR.alloc(num)

            ' 回転及びスケールの変換行列を求める.
            api.fnFIE_geotrans_calc_rotate_matrix(mat_rotate, rad, cx, cy)
            api.fnFIE_geotrans_calc_scale_matrix(mat_scale, sx, sy)

            ' 行列を掛け,1つの行列にまとめる.
            api.fnFIE_mat_mul_aa(mat_scale, mat_rotate, mat)

            ' Workの座標を指定する.
            taWork(0) = DPNT_T.init(50, 50)
            taWork(1) = DPNT_T.init(100, 50)
            taWork(2) = DPNT_T.init(50, 100)
            taWork(3) = DPNT_T.init(100, 100)

            ' 点列をアフィン変換して新座標系での点群とする.
            api.fnFIE_geotrans_affine_npoints(taWork, taMaster, num, mat)

            ' 点群マッチングによる座標系パラメータ(原点座標，傾きおよびスケール)の算出.
            status = api.fnFIE_pfit_param(num, taWork, taMaster, 3, f_pfit_calc_mode.F_PFIT_RBST_SCALE, dpXcOrg, _
                dpYcOrg, dpXc, dpYc, dpTheta, dpScale)
            Assert.IsTrue(status = CInt(f_err.F_ERR_NONE), "エラーが発生しました。({0})", CType(status, f_err))

            ' 結果を出力する.
            ConsoleOut.WriteFunctionName(":" & vbLf)
            Console.WriteLine(" dpXcOrg = {0}" & vbLf & " dpYcOrg = {1}" & vbLf & " dpXc = {2}" & vbLf & " dpYc = {3}" & vbLf & " dpTheta = {4}" & vbLf & " dpScale = {5}", dpXcOrg, dpYcOrg, dpXc, dpYc, dpTheta, _
                dpScale)
            Console.Write(" ...")
            ConsoleOut.IsTrue(ans_Scale = dpScale)
        Finally
            mat_rotate.Dispose()
            mat_scale.Dispose()
            mat.Dispose()
            taWork.Dispose()
            taMaster.Dispose()
        End Try
    End Sub
End Class