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
{
    /// <summary>
    /// Converts between 2d and 3d coordinates.
    /// </summary>
    public class CoordinateConverter
    {
        /// <summary>
        /// size of the image the coordinates are from
        /// </summary>
        private ImageSize imageSize;

        /// <summary>
        /// the horizontal field of view angle
        /// </summary>
        private float hfov;

        /// <summary>
        /// the vertical field of view angle
        /// </summary>
        private float vfov;

        /// <summary>
        /// tangens of hfov / 2
        /// </summary>
        private float hRatio;

        /// <summary>
        /// tanges of vfov / 2
        /// </summary>
        private float vRatio;


        /// <summary>
        /// Constructs a CoordinateConverter.
        /// </summary>
        /// <param name="imageSize">size of the image the coordinates are from</param>
        /// <param name="hfov">horizontal field of view angle</param>
        /// <param name="vfov">vertical field of view angle</param>
        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);
        }

        /// <summary>
        /// Converts a point with a depth to a 3d position.
        /// </summary>
        /// <param name="p">the point</param>
        /// <param name="depth">the depth</param>
        /// <returns>the 3d position</returns>
        public Vector3D convertCoordinate2Dto3D(Vector2D p, float depth)
        {
            return convertCoordinate2Dto3D(p.X, p.Y, depth);
        }

        /// <summary>
        /// Converts a position with a depth to a 3d position.
        /// </summary>
        /// <param name="x">x coordinate of the position</param>
        /// <param name="y">y cooridnate of the position</param>
        /// <param name="depth">the depth</param>
        /// <returns>the 3d position</returns>
        public Vector3D convertCoordinate2Dto3D(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);
        }

        /// <summary>
        /// Converts the length of a 3d linesegment parallel to the camera at the given depth to the length of the linesegment in 2d.
        /// </summary>
        /// <param name="length">3d length of the linesegment</param>
        /// <param name="depth">depth of the linesegment</param>
        /// <returns>2d length of the linesegment</returns>
        public float convertLength3Dto2D(float length, float depth)
        {
            float fullLengthX = (float)(2 * Math.Cos(hfov / 2) * depth);
            float fullLengthY = (float)(2 * Math.Cos(vfov / 2) * depth);
            float fullLengthDiagonal = (float)Math.Sqrt(Math.Pow(fullLengthX, 2) + Math.Pow(fullLengthY, 2));

            float ratio = length / fullLengthDiagonal;
            return ratio * imageSize.DiagonalLength;
        }
    }
}