public void FindPath( FindPathContext context ) { if( tiledNavMesh != null ) { if( navMeshQuery == null ) navMeshQuery = new NavMeshQuery( tiledNavMesh, PathfindingMaxNodes ); try { var extents = ToSharpNav( context.PolygonPickExtents, true ); var startPt = navMeshQuery.FindNearestPoly( ToSharpNav( context.Start ), extents ); var endPt = navMeshQuery.FindNearestPoly( ToSharpNav( context.End ), extents ); var filter = new NavQueryFilter(); var path = new SharpNav.Pathfinding.Path(); var found = navMeshQuery.FindPath( ref startPt, ref endPt, filter, path ); Vector3[] pathResult = null; if( found ) { //find a smooth path over the mesh surface int npolys = path.Count; SharpNav.Geometry.Vector3 iterPos = new SharpNav.Geometry.Vector3(); SharpNav.Geometry.Vector3 targetPos = new SharpNav.Geometry.Vector3(); navMeshQuery.ClosestPointOnPoly( startPt.Polygon, startPt.Position, ref iterPos ); navMeshQuery.ClosestPointOnPoly( path[ npolys - 1 ], endPt.Position, ref targetPos ); var smoothPath = new List<SharpNav.Geometry.Vector3>( context.MaxSmoothPath ); smoothPath.Add( iterPos ); //for( int n = 0; n < path.Count; n++ ) //{ // Vector3 closest = Vector3.Zero; // if( navMeshQuery.ClosestPointOnPoly( path[ n ], startPt.Position, ref closest ) ) // smoothPath.Add( closest ); //} float stepSize = (float)context.StepSize; float slop = (float)context.Slop; while( npolys > 0 && smoothPath.Count < smoothPath.Capacity ) { //find location to steer towards SharpNav.Geometry.Vector3 steerPos = new SharpNav.Geometry.Vector3(); StraightPathFlags steerPosFlag = 0; NavPolyId steerPosRef = NavPolyId.Null; if( !GetSteerTarget( navMeshQuery, iterPos, targetPos, slop, path, ref steerPos, ref steerPosFlag, ref steerPosRef ) ) break; bool endOfPath = ( steerPosFlag & StraightPathFlags.End ) != 0 ? true : false; bool offMeshConnection = ( steerPosFlag & StraightPathFlags.OffMeshConnection ) != 0 ? true : false; //find movement delta SharpNav.Geometry.Vector3 delta = steerPos - iterPos; float len = (float)Math.Sqrt( SharpNav.Geometry.Vector3.Dot( delta, delta ) ); //if steer target is at end of path or off-mesh link //don't move past location if( ( endOfPath || offMeshConnection ) && len < stepSize ) len = 1; else len = stepSize / len; SharpNav.Geometry.Vector3 moveTgt = new SharpNav.Geometry.Vector3(); VMad( ref moveTgt, iterPos, delta, len ); //move SharpNav.Geometry.Vector3 result = new SharpNav.Geometry.Vector3(); List<NavPolyId> visited = new List<NavPolyId>( 16 ); NavPoint startPoint = new NavPoint( path[ 0 ], iterPos ); navMeshQuery.MoveAlongSurface( ref startPoint, ref moveTgt, out result, visited ); path.FixupCorridor( visited ); npolys = path.Count; float h = 0; navMeshQuery.GetPolyHeight( path[ 0 ], result, ref h ); result.Y = h; iterPos = result; //handle end of path when close enough if( endOfPath && InRange( iterPos, steerPos, slop, 1.0f ) ) { //reached end of path iterPos = targetPos; if( smoothPath.Count < smoothPath.Capacity ) smoothPath.Add( iterPos ); break; } //store results if( smoothPath.Count < smoothPath.Capacity ) smoothPath.Add( iterPos ); } pathResult = new Vector3[ smoothPath.Count ]; for( int n = 0; n < pathResult.Length; n++ ) pathResult[ n ] = ToEngine( smoothPath[ n ] ); //if( pathCount > 0 ) //{ // path = new Vec3[ pathCount ]; // //!!!!double support // for( int n = 0; n < pathCount; n++ ) // path[ n ] = ToEngineVec3( pathPointer[ n ] ); //} //else // path = new Vec3[] { context.Start }; } context.Path = pathResult; } catch( Exception e ) { context.Error = e.Message; } } else context.Error = "No NavMesh"; context.Finished = true; }
public void GeneratePathfinding() { /* 生成路径寻找 * 1. 根据polymesh * */ Random rand = new Random(); NavQueryFilter filter = new NavQueryFilter(); //1. buildData = new NavMeshBuilder(polyMesh, polyMeshDetail, new SharpNav.Pathfinding.OffMeshConnection[0], settings); tiledNavMesh = new TiledNavMesh(buildData); OutMesh(); navMeshQuery = new NavMeshQuery(tiledNavMesh, 2048); //Find random start and end points on the poly mesh /*int startRef; * navMeshQuery.FindRandomPoint(out startRef, out startPos);*/ SVector3 c = new SVector3(21.4f, 0, -122f); SVector3 e = new SVector3(5, 5, 5); navMeshQuery.FindNearestPoly(ref c, ref e, out startPt); //navMeshQuery.FindRandomPointAroundCircle(ref startPt, 1000, out endPt); //endPt = new NavPoint(new NavPolyId(20), new SVector3(-10, -2, 10)); c = new SVector3(150.7f, 0, -59f); e = new SVector3(5, 5, 5); navMeshQuery.FindNearestPoly(ref c, ref e, out endPt); //calculate the overall path, which contains an array of polygon references int MAX_POLYS = 256; path = new SharpNav.Pathfinding.Path(); navMeshQuery.FindPath(ref startPt, ref endPt, filter, path); //find a smooth path over the mesh surface int npolys = path.Count; SVector3 iterPos = new SVector3(); SVector3 targetPos = new SVector3(); navMeshQuery.ClosestPointOnPoly(startPt.Polygon, startPt.Position, ref iterPos); navMeshQuery.ClosestPointOnPoly(path[npolys - 1], endPt.Position, ref targetPos); //smoothPath = new List<SVector3>(2048); //smoothPath.Add(iterPos); //float STEP_SIZE = 5f; //float SLOP = 0.01f; //while (npolys > 0 && smoothPath.Count < smoothPath.Capacity) //{ // //find location to steer towards // SVector3 steerPos = new SVector3(); // StraightPathFlags steerPosFlag = 0; // NavPolyId steerPosRef = NavPolyId.Null; // if (!GetSteerTarget(navMeshQuery, iterPos, targetPos, SLOP, path, ref steerPos, ref steerPosFlag, ref steerPosRef)) // break; // bool endOfPath = (steerPosFlag & StraightPathFlags.End) != 0 ? true : false; // bool offMeshConnection = (steerPosFlag & StraightPathFlags.OffMeshConnection) != 0 ? true : false; // //find movement delta // SVector3 delta = steerPos - iterPos; // float len = (float)Math.Sqrt(SVector3.Dot(delta, delta)); // //if steer target is at end of path or off-mesh link // //don't move past location // if ((endOfPath || offMeshConnection) && len < STEP_SIZE) // len = 1; // else // len = STEP_SIZE / len; // SVector3 moveTgt = new SVector3(); // VMad(ref moveTgt, iterPos, delta, len); // //move // SVector3 result = new SVector3(); // List<NavPolyId> visited = new List<NavPolyId>(16); // NavPoint startPoint = new NavPoint(path[0], iterPos); // navMeshQuery.MoveAlongSurface(ref startPoint, ref moveTgt, out result, visited); // path.FixupCorridor(visited); // npolys = path.Count; // float h = 0; // navMeshQuery.GetPolyHeight(path[0], result, ref h); // result.Y = h; // iterPos = result; // //handle end of path when close enough // if (endOfPath && InRange(iterPos, steerPos, SLOP, 1.0f)) // { // //reached end of path // iterPos = targetPos; // if (smoothPath.Count < smoothPath.Capacity) // { // smoothPath.Add(iterPos); // } // break; // } // //store results // if (smoothPath.Count < smoothPath.Capacity) // { // smoothPath.Add(iterPos); // } //} StraightPath corners = new StraightPath(); FindCorners(startPt.Position, endPt.Position, corners, navMeshQuery); }
private void GeneratePathfinding() { if (!hasGenerated) { return; } NavQueryFilter filter = new NavQueryFilter(); buildData = new NavMeshBuilder(polyMesh, polyMeshDetail, new SharpNav.Pathfinding.OffMeshConnection[0], settings); tiledNavMesh = new TiledNavMesh(buildData); for (int i = 0; i < tiledNavMesh.Tiles.Count; ++i) { for (int j = 0; j < tiledNavMesh.Tiles[i].Verts.Length; ++j) { if (j < tiledNavMesh.Tiles[i].Verts.Length - 1) { Debug.DrawLine(ExportNavMeshToObj.ToUnityVector(tiledNavMesh.Tiles[i].Verts[j]), ExportNavMeshToObj.ToUnityVector(tiledNavMesh.Tiles[i].Verts[j + 1]), Color.blue, 99); } } } navMeshQuery = new NavMeshQuery(tiledNavMesh, 2048); //Find random start and end points on the poly mesh /*int startRef; * navMeshQuery.FindRandomPoint(out startRef, out startPos);*/ //SVector3 c = new SVector3(10, 0, 0); //SVector3 e = new SVector3(5, 5, 5); //navMeshQuery.FindNearestPoly(ref c, ref e, out startPt); //navMeshQuery.FindRandomPointAroundCircle(ref startPt, 1000, out endPt); startPt = navMeshQuery.FindRandomPoint(); endPt = navMeshQuery.FindRandomPoint(); //calculate the overall path, which contains an array of polygon references int MAX_POLYS = 256; path = new Path(); navMeshQuery.FindPath(ref startPt, ref endPt, filter, path); //find a smooth path over the mesh surface int npolys = path.Count; SVector3 iterPos = new SVector3(); SVector3 targetPos = new SVector3(); navMeshQuery.ClosestPointOnPoly(startPt.Polygon, startPt.Position, ref iterPos); navMeshQuery.ClosestPointOnPoly(path[npolys - 1], endPt.Position, ref targetPos); smoothPath = new List <SVector3>(2048); smoothPath.Add(iterPos); float STEP_SIZE = 0.5f; float SLOP = 0.01f; while (npolys > 0 && smoothPath.Count < smoothPath.Capacity) { //find location to steer towards SVector3 steerPos = new SVector3(); StraightPathFlags steerPosFlag = 0; NavPolyId steerPosRef = NavPolyId.Null; if (!GetSteerTarget(navMeshQuery, iterPos, targetPos, SLOP, path, ref steerPos, ref steerPosFlag, ref steerPosRef)) { break; } bool endOfPath = (steerPosFlag & StraightPathFlags.End) != 0 ? true : false; bool offMeshConnection = (steerPosFlag & StraightPathFlags.OffMeshConnection) != 0 ? true : false; //find movement delta SVector3 delta = steerPos - iterPos; float len = (float)Math.Sqrt(SVector3.Dot(delta, delta)); //if steer target is at end of path or off-mesh link //don't move past location if ((endOfPath || offMeshConnection) && len < STEP_SIZE) { len = 1; } else { len = STEP_SIZE / len; } SVector3 moveTgt = new SVector3(); VMad(ref moveTgt, iterPos, delta, len); //move SVector3 result = new SVector3(); List <NavPolyId> visited = new List <NavPolyId>(16); NavPoint startPoint = new NavPoint(path[0], iterPos); navMeshQuery.MoveAlongSurface(ref startPoint, ref moveTgt, out result, visited); path.FixupCorridor(visited); npolys = path.Count; float h = 0; navMeshQuery.GetPolyHeight(path[0], result, ref h); result.Y = h; iterPos = result; //handle end of path when close enough if (endOfPath && InRange(iterPos, steerPos, SLOP, 1.0f)) { //reached end of path iterPos = targetPos; if (smoothPath.Count < smoothPath.Capacity) { smoothPath.Add(iterPos); } break; } //store results if (smoothPath.Count < smoothPath.Capacity) { smoothPath.Add(iterPos); } } for (int i = 0; i < smoothPath.Count; i++) { if (i < smoothPath.Count - 1) { Debug.DrawLine(ExportNavMeshToObj.ToUnityVector(smoothPath[i]), ExportNavMeshToObj.ToUnityVector(smoothPath[i + 1]), Color.red, 99); } } }