using Intel.RealSense;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using Vector = Intel.RealSense.Math.Vector;
namespace WindowsFormsApp1
{
    public static class ExtrinsicsExtensions
    {
        public static Vector Transform(this Extrinsics extrin, Vector from_point)
        {
            Vector to_point;
            // 使用外部參數執行坐標轉換
            to_point.x = extrin.rotation[0] * from_point.x + extrin.rotation[3] * from_point.y + extrin.rotation[6] * from_point.z + extrin.translation[0];
            to_point.y = extrin.rotation[1] * from_point.x + extrin.rotation[4] * from_point.y + extrin.rotation[7] * from_point.z + extrin.translation[1];
            to_point.z = extrin.rotation[2] * from_point.x + extrin.rotation[5] * from_point.y + extrin.rotation[8] * from_point.z + extrin.translation[2];
            return to_point;
        }
    }
}