//    $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_points()
        {
            int status = (int)f_err.F_ERR_NONE;

            // 点列(変換前と変換後).
            DPNT_T_PTR taWork = DPNT_T_PTR.Zero;
            DPNT_T_PTR taMaster = DPNT_T_PTR.Zero;
            DPNT_T_PTR taConv = 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_PTR daErr = DOUBLE_PTR.Zero;

            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);
                taConv = DPNT_T_PTR.alloc(num);
                daErr = DOUBLE_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_points(num, taWork, taMaster, 3, f_pfit_calc_mode.F_PFIT_RBST_SCALE,
                                               taConv, daErr);
                Assert.IsTrue(status == (int)f_err.F_ERR_NONE, "エラーが発生しました。({0})", (f_err)status);

                // 結果を出力する.
                ConsoleOut.WriteFunctionName(":\n");
                for( int i = 0; i < num ; i++)
                {
                    Console.WriteLine(" taConv[{3}].x = {0}, taConv[{3}].y = {1}, daErr[{3}] = {2}", taConv[i].x, taConv[i].y, daErr[i], i);
                }
                Console.Write(" ...");
                ConsoleOut.IsTrue(DblEqual(daErr[0], 0) && DblEqual(daErr[1], 0) && DblEqual(daErr[2], 0) && DblEqual(daErr[3], 0));
            }
            finally
            {
                mat_rotate.Dispose();
                mat_scale.Dispose();
                mat.Dispose();
                taWork.Dispose();
                taMaster.Dispose();
                taConv.Dispose();
                daErr.Dispose();
            }
        }
    }
}

'    $Revision: 1.1 $

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

Public Partial Class FIE
    <FvPluginExecute> _
    Public Sub fnFIE_pfit_points()
        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 taConv 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 daErr As DOUBLE_PTR = DOUBLE_PTR.Zero

        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)
            taConv = DPNT_T_PTR.alloc(num)
            daErr = DOUBLE_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_points(num, taWork, taMaster, 3, f_pfit_calc_mode.F_PFIT_RBST_SCALE, taConv, _
                daErr)
            Assert.IsTrue(status = CInt(f_err.F_ERR_NONE), "エラーが発生しました。({0})", CType(status, f_err))

            ' 結果を出力する.
            ConsoleOut.WriteFunctionName(":" & vbLf)
            For i As Integer = 0 To num - 1
                Console.WriteLine(" taConv[{3}].x = {0}, taConv[{3}].y = {1}, daErr[{3}] = {2}", taConv(i).x, taConv(i).y, daErr(i), i)
            Next
            Console.Write(" ...")
            ConsoleOut.IsTrue(DblEqual(daErr(0), 0) AndAlso DblEqual(daErr(1), 0) AndAlso DblEqual(daErr(2), 0) AndAlso DblEqual(daErr(3), 0))
        Finally
            mat_rotate.Dispose()
            mat_scale.Dispose()
            mat.Dispose()
            taWork.Dispose()
            taMaster.Dispose()
            taConv.Dispose()
            daErr.Dispose()
        End Try
    End Sub
End Class