JisolGame/JNFrame2/Assets/JNGame/Map/DotRecast/DotRecastRoot.cs

using System.Collections.Generic;
using DotRecast.Core.Numerics;
using DotRecast.Detour;
using DotRecast.Detour.Crowd;
using DotRecast.Recast.Geom;
using DotRecast.Recast.Toolset;
using DotRecast.Recast.Toolset.Builder;
using JNGame.Map.DotRecast.Util;
using JNGame.Math;
using Newtonsoft.Json;
using UnityEngine;

namespace JNGame.Map.DotRecast
{
    public class DotRecastRoot
    {
        
        //导航数据
        public DtNavMesh NavMesh;
        
        //导航查询
        public DtNavMeshQuery Query { private set; get; }
        
        //智能体避障
        public DtCrowd Crowd { private set; get; }
        private Dictionary<ulong, DtCrowdAgent> agents;

        public DotRecastRoot(MeshData data)
        {
            agents = new Dictionary<ulong, DtCrowdAgent>();
            //初始化导航数据
            InitNavMesh(data);
            //初始化避障数据
            InitCrowd(data);
        }
        
        //更新
        public void Update(LFloat dt)
        {
            Crowd.Update(dt,null);
        }
        
        public DtCrowdAgentParams GetAgentParams()
        {
            int updateFlags = DtCrowdAgentUpdateFlags.DT_CROWD_ANTICIPATE_TURNS |
                              DtCrowdAgentUpdateFlags.DT_CROWD_OPTIMIZE_VIS |
                              DtCrowdAgentUpdateFlags.DT_CROWD_OPTIMIZE_TOPO |
                              DtCrowdAgentUpdateFlags.DT_CROWD_OBSTACLE_AVOIDANCE;
            DtCrowdAgentParams ap = new DtCrowdAgentParams();
            ap.radius = (LFloat)0.6f;
            ap.height = (LFloat)2f;
            ap.maxAcceleration = (LFloat)8.0f;
            ap.maxSpeed = (LFloat)6f;
            ap.collisionQueryRange = ap.radius * (LFloat)12f;
            ap.pathOptimizationRange = ap.radius * (LFloat)30f;
            ap.updateFlags = updateFlags;
            ap.obstacleAvoidanceType = 0;
            ap.separationWeight = (LFloat)2f;
            return ap;
        }

        //初始化导航
        private void InitNavMesh(MeshData data)
        {
            var vertices = new LFloat[data.vertices.Length * 3];
            for (int i = 0; i < data.vertices.Length; ++i)
            {
                vertices[i * 3 + 0] = data.vertices[i].x;
                vertices[i * 3 + 1] = data.vertices[i].y;
                vertices[i * 3 + 2] = data.vertices[i].z;
            }
            
            var bmin = RcVecUtils.Create(vertices);
            var bmax = RcVecUtils.Create(vertices);
            for (int i = 1; i < vertices.Length / 3; i++)
            {
                bmin = RcVecUtils.Min(bmin, vertices, i * 3);
                bmax = RcVecUtils.Max(bmax, vertices, i * 3);
            }

            var geom = new DefaultInputGeomProvider(vertices,data.triangles);
            var builder = new TileNavMeshBuilder();
            var settings = new RcNavMeshBuildSettings();
            
            settings.cellSize = (LFloat)1f;
            var result = builder.Build(geom, settings);
            
            NavMesh = result.NavMesh;
            Query = new DtNavMeshQuery(NavMesh);
        }

        //初始化避障
        private void InitCrowd(MeshData data)
        {
            DtCrowdConfig config = new DtCrowdConfig((LFloat)0.6f);
            Crowd = new DtCrowd(config, NavMesh);
            DtObstacleAvoidanceParams option = new DtObstacleAvoidanceParams();
            option.velBias = (LFloat)0.5f;
            option.adaptiveDivs = 5;
            option.adaptiveRings = 2;
            option.adaptiveDepth = 1;
            Crowd.SetObstacleAvoidanceParams(0, option);
        }
        
        //添加避障
        public void AddAgent(ulong id,RcVec3f start)
        {
            DelAgent(id);
            agents.Add(id,Crowd.AddAgent(start, GetAgentParams()));
        }
        public void AddAgent(ulong id,RcVec3f start,DtCrowdAgentParams agentParams)
        {
            DelAgent(id);
            agents.Add(id,Crowd.AddAgent(start, agentParams));
        }
        
        //添加避障
        public void DelAgent(ulong id)
        {
            if (agents.ContainsKey(id))
            {
                Crowd.RemoveAgent(agents[id]);
                agents.Remove(id);
            }
        }
        
        //移动避障
        public void MoveAgent(ulong id,RcVec3f move)
        {

            DtCrowdAgent agent = agents[id];
            Query.FindNearestPoly(move, Crowd.GetQueryExtents(), Crowd.GetFilter(0), out var nearestRef, out var nearestPt, out var _);
            Crowd.RequestMoveTarget(agent, nearestRef, nearestPt);
            
        }
        
        //向量移动避障
        public void VectorMoveAgent(ulong id,RcVec3f vector)
        {
            DtCrowdAgent agent = agents[id];
            Crowd.RequestMoveVelocity(agent,vector);
        }
        
        //获取避障
        public DtCrowdAgent GetAgent(ulong id)
        {
            return agents[id];
        }
        
        
        /// <summary>
        /// 查询导航
        /// </summary>
        /// <param name="pos"></param>
        /// <returns></returns>
        public List<DtStraightPath> FindPath(LVector3 start,LVector3 end)
        {
            var start1 = new RcVec3f(start.x,start.y,start.z);
            var end1 = new RcVec3f(end.x,end.y,end.z);
            var half = new RcVec3f(1000, 1000, 1000);
            var filter = new DtQueryDefaultFilter();
            Query.FindNearestPoly(start1, half, filter, out var startRef, out var startPt, out var _);
            Query.FindNearestPoly(end1, half, filter, out var endRef, out var endPt, out var _);
            var path = new List<long>();
            DtStatus status = Query.FindPath(startRef,endRef,startPt, endPt, filter,ref path, DtFindPathOption.AnyAngle);
            var paths = new List<DtStraightPath>();
            Query.FindStraightPath(startPt, endPt, path, path.Count, ref paths, 256, 0);
            return paths;
        }
        
        /// <summary>
        /// 返回最近点
        /// </summary>
        /// <param name="pos"></param>
        /// <returns></returns>
        public LVector3 GetNearbyPoints(LVector3 pos)
        {
            DtStatus status = Query.FindNearestPoly(
                new RcVec3f(pos.x, pos.y, pos.z),
                new RcVec3f(1000, 1000, 1000),
                new DtQueryDefaultFilter(),
                out var refs,
                out var nearestPt,
                out var _);
            return new LVector3((LFloat)nearestPt.X, (LFloat)nearestPt.Y, (LFloat)nearestPt.Z);
        }
        
    }

    
    
}