bbiwarg-original/bbiwarg/Utility/CoordinateConverter.cs

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

namespace bbiwarg.Utility
{
    class CoordinateConverter
    {
        private ImageSize imageSize;
        private float hfov;
        private float vfov;
        private float hRatio;
        private float vRatio;

        public CoordinateConverter(ImageSize imageSize, float hfov, float vfov)
        {
            this.imageSize = imageSize;
            this.hfov = hfov;
            this.vfov = vfov;

            hRatio = (float)Math.Tan(hfov / 2);
            vRatio = (float)Math.Tan(vfov / 2);
        }

        public Vector3D convert(Vector2D p, float depth) {
            return convert(p.X, p.Y, depth);
        }

        public Vector3D convert(float x, float y, float depth)
        {
            float deltaX = 2 * ((x / imageSize.Width) - 0.5f);
            float deltaY = -2 * ((y / imageSize.Height) - 0.5f);

            float tanX = deltaX * hRatio;
            float tanY = deltaY * vRatio;

            float a = depth * depth;
            float b = (float)(Math.Pow(tanX, 2) + Math.Pow(tanY, 2));

            float z3 = (float)(Math.Sqrt(0.5f*a + Math.Sqrt(0.25f*Math.Pow(a,2)-b)));
            float x3 = tanX * z3;
            float y3 = tanY * z3;

            return new Vector3D(x3, y3, z3);
        }
    }
}