﻿using UnityEngine;

namespace JLGames.RocketDriver.Actions.Utils
{
    public static class GeometryUtil
    {
        /// <summary>
        /// Get point to line distance
        /// 点到直线距离
        /// </summary>
        /// <param name="point">点坐标</param>
        /// <param name="linePoint1">直线上一个点的坐标</param>
        /// <param name="linePoint2">直线上另一个点的坐标</param>
        /// <returns></returns>
        public static float DisPoint2Line(Vector3 point, Vector3 linePoint1, Vector3 linePoint2)
        {
            var vec1 = point - linePoint1;
            var vec2 = linePoint2 - linePoint1;
            var vecProj = Vector3.Project(vec1, vec2);
            var dis = Mathf.Sqrt(Mathf.Pow(Vector3.Magnitude(vec1), 2) - Mathf.Pow(Vector3.Magnitude(vecProj), 2));
            return dis;
        }

        /// <summary>
        /// Get Distance from point to plane.
        /// 点到平面的距离
        /// </summary>
        /// <param name="point"></param>
        /// <param name="surfacePoint1"></param>
        /// <param name="surfacePoint2"></param>
        /// <param name="surfacePoint3"></param>
        /// <returns></returns>
        public static float DisPoint2Surface(Vector3 point, Vector3 surfacePoint1, Vector3 surfacePoint2,
            Vector3 surfacePoint3)
        {
            //空间直线一般式方程 Ax + By + Cz + D = 0;
            //假定 A = 1 ，推演B C D用A来表示，约去A，可得方程
            var bNumerator = (surfacePoint1.x - surfacePoint2.x) * (surfacePoint2.z - surfacePoint3.z) -
                               (surfacePoint2.x - surfacePoint3.x) * (surfacePoint1.z - surfacePoint2.z);
            var bDenominator = (surfacePoint2.y - surfacePoint3.y) * (surfacePoint1.z - surfacePoint2.z) -
                                 (surfacePoint1.y - surfacePoint2.y) * (surfacePoint2.z - surfacePoint3.z);
            var B = bNumerator / bDenominator;
            var C = (B * (surfacePoint1.y - surfacePoint2.y) + (surfacePoint1.x - surfacePoint2.x)) /
                      (surfacePoint2.z - surfacePoint1.z);
            var D = -surfacePoint1.x - B * surfacePoint1.y - C * surfacePoint1.z;

            return DisPoint2Surface(point, 1f, B, C, D);
        }

        /// <summary>
        /// Get Distance from point to plane.
        /// 点到平面的距离
        /// </summary>
        /// <param name="point"></param>
        /// <param name="factorA"></param>
        /// <param name="factorB"></param>
        /// <param name="factorC"></param>
        /// <param name="factorD"></param>
        /// <returns></returns>
        public static float DisPoint2Surface(Vector3 point, float factorA, float factorB, float factorC, float factorD)
        {
            //点到平面的距离公式 d = |Ax + By + Cz + D|/sqrt(A2 + B2 + C2);
            var numerator = Mathf.Abs(factorA * point.x + factorB * point.y + factorC * point.z + factorD);
            var denominator = Mathf.Sqrt(Mathf.Pow(factorA, 2) + Mathf.Pow(factorB, 2) + Mathf.Pow(factorC, 2));
            var dis = numerator / denominator;
            return dis;
        }

        /// <summary>
        /// Get Distance from point to plane. Use Unity Plane class.
        /// 点到平面的距离, 调用U3D Plane类处理
        /// </summary>
        /// <param name="point"></param>
        /// <param name="surfacePoint1"></param>
        /// <param name="surfacePoint2"></param>
        /// <param name="surfacePoint3"></param>
        /// <returns></returns>
        public static float DisPoint2Surface2(Vector3 point, Vector3 surfacePoint1, Vector3 surfacePoint2,
            Vector3 surfacePoint3)
        {
            var plane = new Plane(surfacePoint1, surfacePoint2, surfacePoint3);
            return DisPoint2Surface2(point, plane);
        }

        /// <summary>
        /// Get Distance from point to plane. Use Unity Plane class.
        /// 点到平面的距离, 调用U3D Plane类处理
        /// </summary>
        /// <param name="point"></param>
        /// <param name="plane"></param>
        /// <returns></returns>
        public static float DisPoint2Surface2(Vector3 point, Plane plane)
        {
            return plane.GetDistanceToPoint(point);
        }

        /// <summary>
        /// Calculate the angle between two planes
        /// 计算两个平面夹角
        /// </summary>
        /// <param name="surface1Point1"></param>
        /// <param name="surface1Point2"></param>
        /// <param name="surface1Point3"></param>
        /// <param name="surface2Point1"></param>
        /// <param name="surface2Point2"></param>
        /// <param name="surface2Point3"></param>
        /// <returns></returns>
        public static float SurfaceAngle(Vector3 surface1Point1, Vector3 surface1Point2, Vector3 surface1Point3,
            Vector3 surface2Point1, Vector3 surface2Point2, Vector3 surface2Point3)
        {
            var plane1 = new Plane(surface1Point1, surface1Point2, surface1Point3);
            var plane2 = new Plane(surface2Point1, surface2Point2, surface2Point3);
            return SurfaceAngle(plane1, plane2);
        }

        /// <summary>
        /// Calculate the angle between two planes
        /// 计算两个平面夹角
        /// </summary>
        /// <param name="plane1"></param>
        /// <param name="plane2"></param>
        /// <returns></returns>
        public static float SurfaceAngle(Plane plane1, Plane plane2)
        {
            return Vector3.Angle(plane1.normal, plane2.normal);
        }
    }
}