bbiwarg-original/bbiwarg/Utility/CoordinateConverter.cs

using System;
using BBIWARG.Input.InputProviding;

namespace BBIWARG.Utility
{
    /// <summary>
    /// Converts between 2d and 3d coordinates.
    /// </summary>
    public class CoordinateConverter
    {
        private IInputProvider inputProvider;


        /// <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(IInputProvider inputProvider)
        {
            this.inputProvider = inputProvider;
        }

        /// <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 / inputProvider.ImageWidth) - 0.5f);
            float deltaY = -2 * ((y / inputProvider.ImageHeight) - 0.5f);

            float tanX = deltaX * (float)Math.Tan(inputProvider.FieldOfViewHorizontal / 2);
            float tanY = deltaY * (float)Math.Tan(inputProvider.FieldOfViewVertical / 2);

            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(inputProvider.FieldOfViewHorizontal / 2) * depth);
            float fullLengthY = (float)(2 * Math.Cos(inputProvider.FieldOfViewVertical / 2) * depth);
            float fullLengthDiagonal = (float)Math.Sqrt(Math.Pow(fullLengthX, 2) + Math.Pow(fullLengthY, 2));

            float ratio = length / fullLengthDiagonal;
            float imageSizeDiagonalLength = (float)Math.Sqrt(inputProvider.ImageWidth*inputProvider.ImageWidth + inputProvider.ImageHeight*inputProvider.ImageHeight);
            return ratio * imageSizeDiagonalLength;
        }
    }
}