bbiwarg-original/bbiwarg/Utility/CoordinateConverter.cs

using System;

namespace BBIWARG.Utility
{
    /// <summary>
    /// Converts between 2d and 3d coordinates.
    /// </summary>
    public class CoordinateConverter
    {
        /// <summary>
        /// the horizontal field of view angle
        /// </summary>
        private float fieldOfViewHorizontal;

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

        /// <summary>
        /// size of the image the coordinates are from
        /// </summary>
        private ImageSize imageSize;

        /// <summary>
        /// horizontal ratio relative to depth
        /// </summary>
        private float relativeRatioHorizontal;

        /// <summary>
        /// vertical ratio relative to depth
        /// </summary>
        private float relativeRatioVertical;

        /// <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.fieldOfViewHorizontal = hfov;
            this.fieldOfViewVertical = vfov;

            relativeRatioHorizontal = (float)Math.Tan(hfov / 2);
            relativeRatioVertical = (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 coordinate 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 * relativeRatioHorizontal;
            float tanY = deltaY * relativeRatioVertical;

            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 line segment parallel to the camera at the given depth to the length of the line segment in 2d.
        /// </summary>
        /// <param name="length">3d length of the line segment</param>
        /// <param name="depth">depth of the line segment</param>
        /// <returns>2d length of the line segment</returns>
        public float convertLength3Dto2D(float length, float depth)
        {
            float fullLengthX = (float)(2 * Math.Cos(fieldOfViewHorizontal / 2) * depth);
            float fullLengthY = (float)(2 * Math.Cos(fieldOfViewVertical / 2) * depth);
            float fullLengthDiagonal = (float)Math.Sqrt(Math.Pow(fullLengthX, 2) + Math.Pow(fullLengthY, 2));

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