提交Unity 联机Pro

JNFrame2/Assets/JNGame/Map/2DPathFinding/NavMesh/Geometry/GeometryUtil.cs

@@ -0,0 +1,242 @@
+using JNGame.Math;
+
+namespace JNGame.PathFinding
+{
+    public static class GeometryUtil
+    {
+        /// <summary>
+        /// Projects a point to a line segment. 
+        /// <para>This implementation is thread-safe.</para>
+        /// </summary>
+        /// <param name="nearest">输出：点point在线段的投影点</param>
+        /// <param name="start">线段起点</param>
+        /// <param name="end">线段终点</param>
+        /// <param name="point">被投影的原始点</param>
+        /// <returns>投影点与原始点的距离平方</returns>
+        public static LFloat NearestSegmentPointSquareDistance(ref LVector3 nearest, in LVector3 start, in LVector3 end, in LVector3 point)
+        {
+            nearest = start;
+            // 线段AB
+            var abX = end.x - start.x;
+            var abY = end.y - start.y;
+            var abZ = end.z - start.z;
+            // 线段AB的长度平方
+            var abLen2 = abX * abX + abY * abY + abZ * abZ;
+            if (abLen2 > 0)
+            { // Avoid NaN due to the indeterminate form 0/0
+                // 投影点在线段上的比例位置
+                var t = ((point.x - start.x) * abX + (point.y - start.y) * abY + (point.z - start.z) * abZ) / abLen2;
+                var s = LMath.Clamp01(t);
+                nearest.x += abX * s;
+                nearest.y += abY * s;
+                nearest.z += abZ * s;
+            }
+            return LVector3.Distance2(nearest, point);
+        }
+
+
+        /// <summary>
+        /// Find the closest point on the triangle, given a measure point.
+        /// This is the optimized algorithm taken from the book "Real-Time Collision Detection".
+        /// <para>This implementation is NOT thread-safe.</para>
+        /// </summary>
+        /// <param name="vertexA"></param>
+        /// <param name="vertexB"></param>
+        /// <param name="vertexC"></param>
+        /// <param name="p"></param>
+        /// <param name="_out"></param>
+        /// <returns></returns>
+        public static LFloat GetClosestPointOnTriangle(in LVector3 vertexA, in LVector3 vertexB, in LVector3 vertexC, in LVector3 p, ref LVector3 _out)
+        {
+            // Check if P in vertex region outside A
+            var ab = vertexB - vertexA;
+            var ac = vertexC - vertexA;
+            var ap = p - vertexA;
+            var d1 = LVector3.Dot(ab, ap);
+            var d2 = LVector3.Dot(ac, ap);
+            if (d1 <= 0 && d2 <= 0)
+            {
+                _out = vertexA;
+                return LVector3.Distance2(p, vertexA);
+            }
+
+            // Check if P in vertex region outside B
+            var bp = p - vertexB;
+            var d3 = LVector3.Dot(ab, bp);
+            var d4 = LVector3.Dot(ac, bp);
+            if (d3 >= 0 && d4 <= d3)
+            {
+                _out = vertexB;
+                return LVector3.Distance2(p, vertexB);
+            }
+
+            // Check if P in edge region of AB, if so return projection of P onto AB
+            var vc = d1 * d4 - d3 * d2;
+            if (vc <= 0 && d1 >= 0 && d3 <= 0)
+            {
+                var v = d1 / (d1 - d3);
+                _out = vertexA + ab * v;    // barycentric coordinates (1-v,v,0)
+                return LVector3.Distance2(p, _out);
+            }
+
+            // Check if P in vertex region outside C
+            var cp = p - vertexC;
+            var d5 = LVector3.Dot(ab, cp);
+            var d6 = LVector3.Dot(ac, cp);
+            if (d6 >= 0 && d5 <= d6)
+            {
+                _out = vertexC;
+                return LVector3.Distance2(p, vertexC);
+            }
+
+            // Check if P in edge region of AC, if so return projection of P onto AC
+            var vb = d5 * d2 - d1 * d6;
+            if (vb <= 0 && d2 >= 0 && d6 <= 0)
+            {
+                var w = d2 / (d2 - d6);
+                _out = vertexA + ac * w; // barycentric coordinates (1-w,0,w)
+                return LVector3.Distance2(_out, p);
+            }
+
+            // Check if P in edge region of BC, if so return projection of P onto BC
+            var va = d3 * d6 - d5 * d4;
+            if (va <= 0 && (d4 - d3) >= 0 && (d5 - d6) >= 0)
+            {
+                var w = (d4 - d3) / ((d4 - d3) + (d5 - d6));
+                _out = vertexB + (vertexC - vertexB) * w; // barycentric coordinates (0,1-w,w)
+                return LVector3.Distance2(_out, p);
+            }
+
+            // P inside face region. Compute Q through its barycentric coordinates (u,v,w)
+            var denom = 1 / (va + vb + vc);
+            {
+                LFloat v = vb * denom;
+                LFloat w = vc * denom;
+                _out = vertexA + ab * v + ac * w;
+            }
+            return LVector3.Distance2(_out, p);
+        }
+
+
+        public static bool IntersectRayTriangle(Ray ray, in LVector3 vertexA, in LVector3 vertexB, in LVector3 vertexC, out LVector3 intersection)
+        {
+            intersection = LVector3.Zero;
+            // AB边向量
+            LVector3 edgeAB = vertexB - vertexA;
+            // AC边向量
+            LVector3 edgeAC = vertexC - vertexA;
+
+            LVector3 pvec = LVector3.Cross(ray.direction, edgeAC);
+            LFloat det = LVector3.Dot(edgeAB, pvec);
+            if (LMath.Abs(det) <= LFloat.EPSILON)
+            {
+                var p = new Plane(vertexA, vertexB, vertexC);
+                if (p.TestPoint(ray.origin) == EnumPlaneSide.OnPlane && IsPointInTriangle(ray.origin, vertexA, vertexB, vertexC))
+                {
+                    intersection = ray.origin;
+                    return true;
+                }
+
+                return false;
+            }
+
+            det = 1 / det;
+
+            LVector3 tvec = ray.origin - vertexA;
+            LFloat u = LVector3.Dot(tvec, pvec) * det;
+            if (u < 0 || u > 1)
+            {
+                return false;
+            }
+
+            LVector3 qvec = LVector3.Cross(tvec, edgeAB);
+            LFloat v = LVector3.Dot(ray.direction, qvec) * det;
+            if (v < 0 || u + v > 1)
+            {
+                return false;
+            }
+
+            LFloat t = LVector3.Dot(edgeAC, qvec) * det;
+            if (t < 0)
+            {
+                return false;
+            }
+
+            if (LMath.Abs(t) <= LFloat.EPSILON)
+            {
+                intersection = ray.origin;
+            }
+            else
+            {
+                intersection = ray.GetEndPoint(t);
+            }
+
+            return true;
+        }
+
+        /// <summary>
+        /// 检测点point是否在三角形ABC内（重心法）
+        /// </summary>
+        /// <param name="point"></param>
+        /// <param name="vertexA"></param>
+        /// <param name="vertexB"></param>
+        /// <param name="vertexC"></param>
+        /// <returns></returns>
+        public static bool IsPointInTriangle(in LVector3 point, in LVector3 vertexA, in LVector3 vertexB, in LVector3 vertexC)
+        {
+            var vPA = vertexA - point;
+            var vPB = vertexB - point;
+            var vPC = vertexC - point;
+
+            var ab = LVector3.Dot(vPA, vPB);
+            var ac = LVector3.Dot(vPA, vPC);
+            var bc = LVector3.Dot(vPB, vPC);
+            var cc = LVector3.Dot(vPC, vPC);
+
+            if (bc * ac - cc * ab < 0)
+                return false;
+            var bb = LVector3.Dot(vPB, vPB);
+            if (ab * bc - ac * bb < 0)
+                return false;
+            return true;
+        }
+
+        /// <summary>
+        /// 计算点P是否在有向直线AB的左侧，返回值>0s说明在左侧
+        /// <para>Computes the signed distance from a line connecting the
+        /// specified points to a specified point.</para>
+        /// </summary>
+        /// <param name="linePointA">直线上一点A</param>
+        /// <param name="linePointB">直线上一点B</param>
+        /// <param name="p">查询点P</param>
+        /// <returns>Positive when the point p lies to the left of the line ab.</returns>
+        public static LFloat IsLineLeft(in LVector2 linePointA, in LVector2 linePointB, in LVector2 p)
+        {
+            return LMath.Cross(linePointA - p, linePointB - linePointA);
+        }
+
+        /// <summary>
+        /// 返回点P到直线AB的距离平方
+        /// </summary>
+        /// <param name="linePointA">直线上一点A</param>
+        /// <param name="linePointB">直线上一点B</param>
+        /// <param name="pointP">点P</param>
+        /// <returns></returns>
+        public static LFloat SqrDistPointLineSegment(in LVector2 linePointA, in LVector2 linePointB, in LVector2 pointP)
+        {
+            LFloat r = LMath.Dot(pointP - linePointA, linePointB - linePointA) / (linePointB - linePointA).SqrMagnitude;
+
+            if (r < LFloat.Zero)
+            {
+                return (pointP - linePointA).SqrMagnitude;
+            }
+
+            if (r > LFloat.One)
+            {
+                return (pointP - linePointB).SqrMagnitude;
+            }
+
+            return (pointP - (linePointA + r * (linePointB - linePointA))).SqrMagnitude;
+        }
+    }
+}

JNFrame2/Assets/JNGame/Map/2DPathFinding/NavMesh/Geometry/GeometryUtil.cs.meta

@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: 36c93c611b97d4495b67967418da8216
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

JNFrame2/Assets/JNGame/Map/2DPathFinding/NavMesh/Geometry/Plane.cs

@@ -0,0 +1,158 @@
+using JNGame.Math;
+
+namespace JNGame.PathFinding
+{
+    /// <summary>
+    /// 平面的数据结构
+    /// </summary>
+    public class Plane
+    {
+        /// <summary>
+        /// 平面法向量，单位向量，法向量方向为面的正面
+        /// </summary>
+        public LVector3 normal = LVector3.Zero;
+        /// <summary>
+        /// 平面到原点的距离
+        /// </summary>
+        public LFloat distFromOrigin = LFloat.Zero;
+
+        /// <summary>
+        /// 默认构造
+        /// </summary>
+        public Plane() { }
+
+        /// <summary>
+        /// 拷贝构造
+        /// </summary>
+        /// <param name="plane"></param>
+        public Plane(Plane plane)
+        {
+            this.normal = plane.normal;
+            this.distFromOrigin = plane.distFromOrigin;
+        }
+
+        /// <summary>
+        /// 方向向量和平面到原点的距离构成平面
+        /// </summary>
+        /// <param name="normal">平面的方向向量</param>
+        /// <param name="distance">平面到原点的距离</param>
+        public Plane(in LVector3 normal, LFloat distance)
+        {
+            this.normal = normal.Normalized;
+            this.distFromOrigin = distance;
+        }
+
+        /// <summary>
+        /// 方向向量和平面内一点构成平面
+        /// </summary>
+        /// <param name="normal">平面的方向向量</param>
+        /// <param name="point">平面内一点，也可视为原点到平面内一点的向量</param>
+        public Plane(in LVector3 normal, in LVector3 point)
+        {
+            this.normal = normal.Normalized;
+            this.distFromOrigin = -LVector3.Dot(this.normal, point);
+        }
+
+        /// <summary>
+        /// 三个点构成一个平面
+        /// </summary>
+        /// <param name="point1"></param>
+        /// <param name="point2"></param>
+        /// <param name="point3"></param>
+        public Plane(in LVector3 point1, in LVector3 point2, in LVector3 point3)
+        {
+            Set(point1, point2, point3);
+        }
+
+        /// <summary>
+        /// 根据三个点，设置平面信息
+        /// </summary>
+        /// <param name="point1"></param>
+        /// <param name="point2"></param>
+        /// <param name="point3"></param>
+        public void Set(in LVector3 point1, in LVector3 point2, in LVector3 point3)
+        {
+            LVector3 vector12 = point2 - point1; // 点1指向点2的向量
+            LVector3 vector23 = point3 - point2; // 点2指向点3的向量
+            normal = LVector3.Cross(vector12, vector23).Normalized;
+            // 规定原点在面的背面，因此距离为负
+            distFromOrigin = -LVector3.Dot(point1, normal);
+        }
+
+        /// <summary>
+        /// 根据点和法线，设置平面信息
+        /// </summary>
+        /// <param name="point"></param>
+        /// <param name="normal"></param>
+        public void Set(in LVector3 point, in LVector3 normal)
+        {
+            this.normal = normal.Normalized;
+            distFromOrigin = -LVector3.Dot(point, normal);
+        }
+
+        /// <summary>
+        /// 根据点和法线，设置平面信息
+        /// </summary>
+        /// <param name="pointX"></param>
+        /// <param name="pointY"></param>
+        /// <param name="pointZ"></param>
+        /// <param name="norX"></param>
+        /// <param name="norY"></param>
+        /// <param name="norZ"></param>
+        public void Set(LFloat pointX, LFloat pointY, LFloat pointZ, LFloat norX, LFloat norY, LFloat norZ)
+        {
+            this.normal = new LVector3(norX, norY, norZ).Normalized;
+            distFromOrigin = -(pointX * norX + pointY * norY + pointZ * norZ);
+        }
+
+        /// <summary>
+        /// 计算平面到点的距离
+        /// </summary>
+        /// <param name="point"></param>
+        /// <returns></returns>
+        public LFloat Distance(in LVector3 point)
+        {
+            // 点在normal方向的投影距离 + 面到原点的距离。根据符号，可判断点在面的哪一侧
+            return LVector3.Dot(normal, point) + distFromOrigin;
+        }
+
+        /// <summary>
+        /// 测试点位于平面的哪一面
+        /// </summary>
+        /// <param name="point"></param>
+        /// <returns></returns>
+        public EnumPlaneSide TestPoint(in LVector3 point)
+        {
+            LFloat dist = Distance(in point);
+
+            if (dist == 0)
+                return EnumPlaneSide.OnPlane;
+            else if (dist < 0)
+                return EnumPlaneSide.Back;
+            else
+                return EnumPlaneSide.Front;
+        }
+
+        /// <summary>
+        /// 是否位于正面
+        /// </summary>
+        /// <param name="direction"></param>
+        /// <returns></returns>
+        public bool IsFrontFacing(in LVector3 direction)
+        {
+            LFloat dot = LVector3.Dot(normal, direction);
+            return dot <= 0;
+        }
+
+        public void Reset()
+        {
+            normal = LVector3.Zero;
+            distFromOrigin = LFloat.Zero;
+        }
+
+        public override string ToString()
+        {
+            return normal.ToString() + ", " + distFromOrigin;
+        }
+    }
+}

JNFrame2/Assets/JNGame/Map/2DPathFinding/NavMesh/Geometry/Plane.cs.meta

@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: 555aac364a42943c4993c540358381df
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

JNFrame2/Assets/JNGame/Map/2DPathFinding/NavMesh/Geometry/Ray.cs

@@ -0,0 +1,50 @@
+using JNGame.Math;
+
+namespace JNGame.PathFinding
+{
+    /// <summary>
+    /// 射线数据结构
+    /// </summary>
+    public class Ray
+    {
+        /// <summary>
+        /// 射线起点
+        /// </summary>
+        public LVector3 origin = LVector3.Zero;
+        /// <summary>
+        /// 射线方向，单位向量
+        /// </summary>
+        public LVector3 direction = LVector3.Zero;
+
+        public Ray() { }
+
+        public Ray(Ray ray)
+        {
+            this.origin = ray.origin;
+            this.direction = ray.direction;
+        }
+
+        public Ray(in LVector3 origin, in LVector3 direction)
+        {
+            this.origin = origin;
+            this.direction = direction.Normalized;
+        }
+
+        public LVector3 GetEndPoint(LFloat distance)
+        {
+            return origin + direction * distance;
+        }
+
+        public void Reset()
+        {
+            origin = LVector3.Zero;
+            direction = LVector3.Zero;
+        }
+
+        /** {@inheritDoc} */
+        public override string ToString()
+        {
+            return "ray [" + origin + ":" + direction + "]";
+        }
+    }
+}

JNFrame2/Assets/JNGame/Map/2DPathFinding/NavMesh/Geometry/Ray.cs.meta

@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: 07d36ae82dda2434da808db57664e965
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: