static bool InRange( SharpNav.Geometry.Vector3 v1, SharpNav.Geometry.Vector3 v2, float r, float h )
{
float dx = v2.X - v1.X;
float dy = v2.Y - v1.Y;
float dz = v2.Z - v1.Z;
return ( dx * dx + dz * dz ) < ( r * r ) && Math.Abs( dy ) < h;
}
static bool GetSteerTarget( NavMeshQuery navMeshQuery, SharpNav.Geometry.Vector3 startPos, SharpNav.Geometry.Vector3 endPos, float minTargetDist, SharpNav.Pathfinding.Path path, ref SharpNav.Geometry.Vector3 steerPos, ref StraightPathFlags steerPosFlag, ref NavPolyId steerPosRef )
{
StraightPath steerPath = new StraightPath();
navMeshQuery.FindStraightPath( startPos, endPos, path, steerPath, 0 );
int nsteerPath = steerPath.Count;
if( nsteerPath == 0 )
return false;
//find vertex far enough to steer to
int ns = 0;
while( ns < nsteerPath )
{
if( ( steerPath[ ns ].Flags & StraightPathFlags.OffMeshConnection ) != 0 ||
!InRange( steerPath[ ns ].Point.Position, startPos, minTargetDist, 1000.0f ) )
break;
ns++;
}
//failed to find good point to steer to
if( ns >= nsteerPath )
return false;
steerPos = steerPath[ ns ].Point.Position;
steerPos.Y = startPos.Y;
steerPosFlag = steerPath[ ns ].Flags;
if( steerPosFlag == StraightPathFlags.None && ns == ( nsteerPath - 1 ) )
steerPosFlag = StraightPathFlags.End; // otherwise seeks path infinitely!!!
steerPosRef = steerPath[ ns ].Point.Polygon;
return true;
}
private void GenerateCrowd()
{
if (!hasGenerated || navMeshQuery == null)
{
return;
}
System.Random rand = new System.Random();
crowd = new Crowd(MAX_AGENTS, 0.6f, ref tiledNavMesh);
SVector3 c = new SVector3(10, 0, 0);
SVector3 e = new SVector3(5, 5, 5);
AgentParams ap = new AgentParams();
ap.Radius = 0.6f;
ap.Height = 2.0f;
ap.MaxAcceleration = 8.0f;
ap.MaxSpeed = 3.5f;
ap.CollisionQueryRange = ap.Radius * 12.0f;
ap.PathOptimizationRange = ap.Radius * 30.0f;
ap.UpdateFlags = new UpdateFlags();
//initialize starting positions for each active agent
for (int i = 0; i < numActiveAgents; i++)
{
//Get the polygon that the starting point is in
NavPoint startPt;
navMeshQuery.FindNearestPoly(ref c, ref e, out startPt);
//Pick a new random point that is within a certain radius of the current point
NavPoint newPt;
navMeshQuery.FindRandomPointAroundCircle(ref startPt, 1000, out newPt);
c = newPt.Position;
//Save this random point as the starting position
trails[i].Trail = new SVector3[AGENT_MAX_TRAIL];
trails[i].Trail[0] = newPt.Position;
trails[i].HTrail = 0;
//add this agent to the crowd
int idx = crowd.AddAgent(newPt.Position, ap);
var targetPt = navMeshQuery.FindNearestPoly(new SVector3()
{
X = 0, Y = 0, Z = 0
}, new SVector3 {
X = 1, Y = 1, Z = 1
});
//Give this agent a target point
//NavPoint targetPt;
//navMeshQuery.FindRandomPointAroundCircle(ref newPt, 1000, out targetPt);
crowd.GetAgent(idx).RequestMoveTarget(targetPt.Polygon, targetPt.Position);
trails[i].Trail[AGENT_MAX_TRAIL - 1] = targetPt.Position;
}
}
public void FindCorners(SVector3 pos, SVector3 target, StraightPath corners, NavMeshQuery navquery)
{
const float MinTargetDist = 0.01f;
navquery.FindStraightPath(pos, target, path, corners, 0);
//prune points in the beginning of the path which are too close
while (corners.Count > 0)
{
if (((corners[0].Flags & StraightPathFlags.OffMeshConnection) != 0) ||
Vector3Extensions.Distance2D(corners[0].Point.Position, pos) > MinTargetDist)
{
break;
}
corners.RemoveAt(0);
}
//prune points after an off-mesh connection
for (int i = 0; i < corners.Count; i++)
{
if ((corners[i].Flags & StraightPathFlags.OffMeshConnection) != 0)
{
corners.RemoveRange(i + 1, corners.Count - i);
break;
}
}
}
public static NavMesh GetNavMesh()
{
var startPoint = new SharpNav.Geometry.Vector3(0f, -4.72f, 0f);
//prepare the geometry from your mesh data
var tris = TriangleEnumerable.FromIndexedVector3(
new SharpNav.Geometry.Vector3[]
{
new SharpNav.Geometry.Vector3(-100, 0, -100) + startPoint,
new SharpNav.Geometry.Vector3(0, 0, 200) + startPoint,
new SharpNav.Geometry.Vector3(100, 0, -100) + startPoint,
//new SharpNav.Geometry.Vector3(10, 0, -10) + startPoint
},
new int[]
{
0,
1,
2,
//0,
//2,
//3
},
0, 1, 0, 1);
//use the default generation settings
var settings = NavMeshGenerationSettings.Default;
settings.AgentHeight = 1.75f;
settings.AgentRadius = 0.4f;
settings.VertsPerPoly = 3;
//generate the mesh
var navMesh = NavMesh.Generate(tris, NavMeshGenerationSettings.Default);
return(navMesh);
}
private bool InRange(SVector3 v1, SVector3 v2, float r, float h)
{
float dx = v2.X - v1.X;
float dy = v2.Y - v1.Y;
float dz = v2.Z - v1.Z;
return((dx * dx + dz * dz) < (r * r) && Math.Abs(dy) < h);
}
static void GenerateNavMesh(string fileName)
{
NavMeshConfigurationFile file = new NavMeshConfigurationFile();
file.GenerationSettings.AgentRadius = 0.31f;
file.ExportPath = fileName + ".snj";
NavMeshConfigurationFile.MeshSettings loadMesh = new NavMeshConfigurationFile.MeshSettings()
{
Path = fileName + ".obj", Position = new float[3], Scale = 1.0f
};
file.InputMeshes.Add(loadMesh);
List <string> meshes = new List <string>();
List <ObjModel> models = new List <ObjModel>();
foreach (var mesh in file.InputMeshes)
{
//Log.("Path: " + mesh.Path, 2);
//Log.Debug("Scale: " + mesh.Scale, 2);
//Log.Debug("Position: " + mesh.Position.ToString(), 2);
meshes.Add(mesh.Path);
SharpNav.Geometry.Vector3 position = new SharpNav.Geometry.Vector3(mesh.Position[0], mesh.Position[1], mesh.Position[2]);
if (File.Exists(mesh.Path))
{
ObjModel obj = new ObjModel(mesh.Path);
float scale = mesh.Scale;
//TODO SCALE THE OBJ FILE
models.Add(obj);
}
else
{
//Log.Error("Mesh file does not exist.");
return;
}
}
var tris = Enumerable.Empty <SharpNav.Geometry.Triangle3>();
foreach (var model in models)
{
tris = tris.Concat(model.GetTriangles());
}
TiledNavMesh navmesh = NavMesh.Generate(tris, file.GenerationSettings);
new NavMeshJsonSerializer().Serialize(file.ExportPath, navmesh);
}
public override void AddNavTriangles(List <Triangle3> triangles)
{
if (dungeon == null)
{
Debug.LogWarning("LayoutFloorTriangleProvider: Dungeon is not assigned");
return;
}
var model = dungeon.ActiveModel as GridDungeonModel;
if (model == null)
{
Debug.LogWarning("LayoutFloorTriangleProvider: Dungeon model is invalid. Rebuild the dungeon");
return;
}
var config = model.Config;
var verts = new SVector3[4];
for (int i = 0; i < verts.Length; i++)
{
verts[i] = new SVector3();
}
foreach (var cell in model.Cells)
{
//if (cell.CellType == CellType.Unknown) continue;
var bounds = cell.Bounds;
var location = MathUtils.GridToWorld(config.GridCellSize, bounds.Location);
var size = MathUtils.GridToWorld(config.GridCellSize, bounds.Size);
verts[0].Set(location.x, location.y, location.z);
verts[1].Set(location.x + size.x, location.y, location.z);
verts[2].Set(location.x + size.x, location.y, location.z + size.z);
verts[3].Set(location.x, location.y, location.z + size.z);
triangles.Add(new Triangle3(
verts[0],
verts[1],
verts[2]));
triangles.Add(new Triangle3(
verts[2],
verts[3],
verts[0]));
}
}
private bool GetSteerTarget(NavMeshQuery navMeshQuery, SVector3 startPos, SVector3 endPos, float minTargetDist, PolyId[] path, int pathSize,
ref SVector3 steerPos, ref int steerPosFlag, ref PolyId steerPosRef)
{
int MAX_STEER_POINTS = 3;
SVector3[] steerPath = new SVector3[MAX_STEER_POINTS];
int[] steerPathFlags = new int[MAX_STEER_POINTS];
PolyId[] steerPathPolys = new PolyId[MAX_STEER_POINTS];
int nsteerPath = 0;
navMeshQuery.FindStraightPath(startPos, endPos, path, pathSize,
steerPath, steerPathFlags, steerPathPolys, ref nsteerPath, MAX_STEER_POINTS, 0);
if (nsteerPath == 0)
{
return(false);
}
//find vertex far enough to steer to
int ns = 0;
while (ns < nsteerPath)
{
if ((steerPathFlags[ns] & PathfindingCommon.STRAIGHTPATH_OFFMESH_CONNECTION) != 0 ||
!InRange(steerPath[ns], startPos, minTargetDist, 1000.0f))
{
break;
}
ns++;
}
//failed to find good point to steer to
if (ns >= nsteerPath)
{
return(false);
}
steerPos = steerPath[ns];
steerPos.Y = startPos.Y;
steerPosFlag = steerPathFlags[ns];
steerPosRef = steerPathPolys[ns];
return(true);
}
private void DrawPathfinding()
{
if (path == null)
{
return;
}
GL.PushMatrix();
Color4 color = Color4.Cyan;
GL.Begin(PrimitiveType.Triangles);
for (int i = 0; i < path.Count; i++)
{
if (i == 0)
{
color = Color4.Cyan;
}
else if (i == path.Count - 1)
{
color = Color4.PaleVioletRed;
}
else
{
color = Color4.LightYellow;
}
GL.Color4(color);
PolyId polyRef = path[i];
MeshTile tile;
Poly poly;
tiledNavMesh.TryGetTileAndPolyByRefUnsafe(polyRef, out tile, out poly);
for (int j = 2; j < poly.VertCount; j++)
{
int vertIndex0 = poly.Verts[0];
int vertIndex1 = poly.Verts[j - 1];
int vertIndex2 = poly.Verts[j];
var v = tile.Verts[vertIndex0];
GL.Vertex3(v.X, v.Y, v.Z);
v = tile.Verts[vertIndex1];
GL.Vertex3(v.X, v.Y, v.Z);
v = tile.Verts[vertIndex2];
GL.Vertex3(v.X, v.Y, v.Z);
}
}
GL.End();
GL.DepthMask(false);
//neighbor edges
GL.LineWidth(1.5f);
GL.Begin(PrimitiveType.Lines);
for (int i = 0; i < path.Count; i++)
{
if (i == 0)
{
color = Color4.Blue;
}
else if (i == path.Count - 1)
{
color = Color4.Red;
}
else
{
color = Color4.Yellow;
}
GL.Color4(color);
PolyId polyRef = path[i];
MeshTile tile;
Poly poly;
tiledNavMesh.TryGetTileAndPolyByRefUnsafe(polyRef, out tile, out poly);
for (int j = 0; j < poly.VertCount; j++)
{
int vertIndex0 = poly.Verts[j];
int vertIndex1 = poly.Verts[(j + 1) % poly.VertCount];
var v = tile.Verts[vertIndex0];
GL.Vertex3(v.X, v.Y, v.Z);
v = tile.Verts[vertIndex1];
GL.Vertex3(v.X, v.Y, v.Z);
}
}
GL.End();
//steering path
GL.Color4(Color4.Black);
GL.Begin(PrimitiveType.Lines);
for (int i = 0; i < smoothPath.Count - 1; i++)
{
SVector3 v0 = smoothPath[i];
GL.Vertex3(v0.X, v0.Y, v0.Z);
SVector3 v1 = smoothPath[i + 1];
GL.Vertex3(v1.X, v1.Y, v1.Z);
}
GL.End();
GL.DepthMask(true);
GL.PopMatrix();
}
private void DrawCrowd()
{
if (crowd == null)
{
return;
}
GL.PushMatrix();
//The black line represents the actual path that the agent takes
/*GL.Color4(Color4.Black);
* GL.Begin(PrimitiveType.Lines);
* for (int i = 0; i < numActiveAgents; i++)
* {
* for (int j = 0; j < numIterations - 1; j++)
* {
* SVector3 v0 = trails[i].Trail[j];
* GL.Vertex3(v0.X, v0.Y, v0.Z);
*
* SVector3 v1 = trails[i].Trail[j + 1];
* GL.Vertex3(v1.X, v1.Y, v1.Z);
* }
* }
* GL.End();
*
* //The yellow line represents the ideal path from the start to the target
* GL.Color4(Color4.Yellow);
* GL.LineWidth(1.5f);
* GL.Begin(PrimitiveType.Lines);
* for (int i = 0; i < numActiveAgents; i++)
* {
* SVector3 v0 = trails[i].Trail[0];
* GL.Vertex3(v0.X, v0.Y, v0.Z);
*
* SVector3 v1 = trails[i].Trail[AGENT_MAX_TRAIL - 1];
* GL.Vertex3(v1.X, v1.Y, v1.Z);
* }
* GL.End();
*
* //The cyan point represents the agent's starting location
* GL.PointSize(100.0f);
* GL.Color4(Color4.Cyan);
* GL.Begin(PrimitiveType.Points);
* for (int i = 0; i < numActiveAgents; i++)
* {
* SVector3 v0 = trails[i].Trail[0];
* GL.Vertex3(v0.X, v0.Y, v0.Z);
* }
* GL.End();
*
* //The red point represent's the agent's target location
* GL.Color4(Color4.PaleVioletRed);
* GL.Begin(PrimitiveType.Points);
* for (int i = 0; i < numActiveAgents; i++)
* {
* SVector3 v0 = trails[i].Trail[AGENT_MAX_TRAIL - 1];
* GL.Vertex3(v0.X, v0.Y, v0.Z);
* }
* GL.End();*/
//GL.DepthMask(true);
GL.Color4(Color4.PaleVioletRed);
GL.PointSize(10);
GL.Begin(PrimitiveType.Points);
GL.Color4(Color4.Blue);
for (int i = 0; i < numActiveAgents; i++)
{
SVector3 p = crowd.GetAgent(i).TargetPosition;
GL.Vertex3(p.X, p.Y, p.Z);
}
GL.End();
if (agentCylinder != null)
{
for (int i = 0; i < numActiveAgents; i++)
{
SVector3 p = crowd.GetAgent(i).Position;
agentCylinder.Draw(new Vector3(p.X, p.Y, p.Z));
}
}
GL.PopMatrix();
}
private bool GetSteerTarget(NavMeshQuery navMeshQuery, SVector3 startPos, SVector3 endPos, float minTargetDist, int[] path, int pathSize,
ref SVector3 steerPos, ref int steerPosFlag, ref int steerPosRef)
{
int MAX_STEER_POINTS = 3;
SVector3[] steerPath = new SVector3[MAX_STEER_POINTS];
int[] steerPathFlags = new int[MAX_STEER_POINTS];
int[] steerPathPolys = new int[MAX_STEER_POINTS];
int nsteerPath = 0;
navMeshQuery.FindStraightPath(startPos, endPos, path, pathSize,
steerPath, steerPathFlags, steerPathPolys, ref nsteerPath, MAX_STEER_POINTS, 0);
if (nsteerPath == 0)
return false;
//find vertex far enough to steer to
int ns = 0;
while (ns < nsteerPath)
{
if ((steerPathFlags[ns] & PathfindingCommon.STRAIGHTPATH_OFFMESH_CONNECTION) != 0 ||
!InRange(steerPath[ns], startPos, minTargetDist, 1000.0f))
break;
ns++;
}
//failed to find good point to steer to
if (ns >= nsteerPath)
return false;
steerPos = steerPath[ns];
steerPos.Y = startPos.Y;
steerPosFlag = steerPathFlags[ns];
steerPosRef = steerPathPolys[ns];
return true;
}
public bool BuildNavMesh( out string error )
{
DestroyNavMesh();
if( !EnabledInHierarchy )
{
error = "Is not enabled.";
return false;
}
//get geometry data
var collector = GetAllGeometriesForNavigationMesh();
Vector3[] vertices = collector.resultVertices;
int[] indices = collector.resultIndices;
int vertexCount = collector.resultVertexCount;
int indexCount = collector.resultIndexCount;
if( vertexCount == 0 )
{
error = "No vertices were gathered from collision objects.";
return false;
}
//get settings
var settings = new NavMeshGenerationSettings();
settings.CellSize = (float)CellSize;
settings.CellHeight = (float)CellHeight;
settings.MaxClimb = (float)AgentMaxClimb;
settings.AgentHeight = (float)AgentHeight;
settings.AgentRadius = (float)AgentRadius;
settings.MinRegionSize = MinRegionSize;
settings.MergedRegionSize = MergedRegionSize;
settings.MaxEdgeLength = MaxEdgeLength;
settings.MaxEdgeError = (float)MaxEdgeError;
settings.VertsPerPoly = MaxVerticesPerPolygon;
settings.SampleDistance = DetailSampleDistance;
settings.MaxSampleError = DetailMaxSampleError;
settings.BuildBoundingVolumeTree = true;
TiledNavMesh newTiledNavMesh;
try
{
//level.SetBoundingBoxOffset(new SVector3(settings.CellSize * 0.5f, settings.CellHeight * 0.5f, settings.CellSize * 0.5f));
var bounds = Bounds.Cleared;
bounds.Add( vertices );
var heightfield = new Heightfield( ToSharpNav( bounds ), settings );
var vertices2 = new SharpNav.Geometry.Vector3[ indexCount ];
for( int index = 0; index < indexCount; index++ )
vertices2[ index ] = ToSharpNav( vertices[ indices[ index ] ] );
//Area[] areas = AreaGenerator.From( vertices2, Area.Default )
// .MarkBelowSlope( (float)AgentMaxSlope.Value.InRadians(), Area.Null )
// .ToArray();
//Area[] areas = AreaGenerator.From(triEnumerable, Area.Default)
// .MarkAboveHeight(areaSettings.MaxLevelHeight, Area.Null)
// .MarkBelowHeight(areaSettings.MinLevelHeight, Area.Null)
// .MarkBelowSlope(areaSettings.MaxTriSlope, Area.Null)
// .ToArray();
//heightfield.RasterizeTrianglesWithAreas( vertices2, areas );
heightfield.RasterizeTriangles( vertices2, Area.Default );
heightfield.FilterLedgeSpans( settings.VoxelAgentHeight, settings.VoxelMaxClimb );
heightfield.FilterLowHangingWalkableObstacles( settings.VoxelMaxClimb );
heightfield.FilterWalkableLowHeightSpans( settings.VoxelAgentHeight );
var compactHeightfield = new CompactHeightfield( heightfield, settings );
compactHeightfield.Erode( settings.VoxelAgentRadius );
compactHeightfield.BuildDistanceField();
compactHeightfield.BuildRegions( 0, settings.MinRegionSize, settings.MergedRegionSize );
//!!!!
System.Random r = new System.Random();
var regionColors = new ColorByte[ compactHeightfield.MaxRegions ];
regionColors[ 0 ] = new ColorByte( 0, 0, 0 );
for( int i = 1; i < regionColors.Length; i++ )
regionColors[ i ] = new ColorByte( (byte)r.Next( 0, 255 ), (byte)r.Next( 0, 255 ), (byte)r.Next( 0, 255 ), (byte)255 );
var contourSet = compactHeightfield.BuildContourSet( settings );
var polyMesh = new PolyMesh( contourSet, settings );
var polyMeshDetail = new PolyMeshDetail( polyMesh, compactHeightfield, settings );
var buildData = new NavMeshBuilder( polyMesh, polyMeshDetail, new OffMeshConnection[ 0 ], settings );
newTiledNavMesh = new TiledNavMesh( buildData );
//!!!!
////Pathfinding with multiple units
//GenerateCrowd();
}
catch( Exception e )
{
DestroyNavMesh();
error = e.Message;
return false;
}
int dataLength;
byte[] data;
using( var memoryStream = new MemoryStream() )
{
var serializer = new NavMeshBinarySerializer();
serializer.Serialize( memoryStream, newTiledNavMesh );
dataLength = (int)memoryStream.Length;
data = memoryStream.GetBuffer();
}
//generate nav mesh data
var writer = new ArrayDataWriter();
writer.Write( navMeshDataVersion );
writer.Write( dataLength );
writer.Write( data, 0, dataLength );
//set NavMeshData and init
var newNavMeshData = new byte[ writer.BitLength / 8 ];
Buffer.BlockCopy( writer.Data, 0, newNavMeshData, 0, newNavMeshData.Length );
NavMeshData = newNavMeshData;
error = "";
return true;
}
private void GeneratePathfinding()
{
if (!hasGenerated)
return;
Random rand = new Random();
NavQueryFilter filter = new NavQueryFilter();
buildData = new NavMeshBuilder(polyMesh, polyMeshDetail, new SharpNav.Pathfinding.OffMeshConnection[0], settings);
tiledNavMesh = new TiledNavMesh(buildData);
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);
//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);
}
}
}
/// <summary>
/// Scaled vector addition
/// </summary>
/// <param name="dest">Result</param>
/// <param name="v1">Vector 1</param>
/// <param name="v2">Vector 2</param>
/// <param name="s">Scalar</param>
private void VMad(ref SVector3 dest, SVector3 v1, SVector3 v2, float s)
{
dest.X = v1.X + v2.X * s;
dest.Y = v1.Y + v2.Y * s;
dest.Z = v1.Z + v2.Z * s;
}
private bool InRange(SVector3 v1, SVector3 v2, float r, float h)
{
float dx = v2.X - v1.X;
float dy = v2.Y - v1.Y;
float dz = v2.Z - v1.Z;
return (dx * dx + dz * dz) < (r * r) && Math.Abs(dy) < h;
}
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;
}
//只找Corner
public StraightPath Pathfinding(SVector3 startVector, SVector3 endVector)
{
Random rand = new Random();
NavQueryFilter filter = new NavQueryFilter();
//buildData = new NavMeshBuilder(polyMesh, polyMeshDetail, new SharpNav.Pathfinding.OffMeshConnection[0], settings);
//tiledNavMesh = new TiledNavMesh(buildData);
//navMeshQuery = new NavMeshQuery(tiledNavMesh, 2048);
SVector3 extents = new SVector3(5, 5, 5);
navMeshQuery.FindNearestPoly(ref startVector, ref extents, out startPt);
navMeshQuery.FindNearestPoly(ref endVector, ref extents, out endPt);
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 = 1f;
//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(startVector, endVector, corners, navMeshQuery);
return(corners);
//return smoothPath;
}
//为了使接口适应而附加的
public List <Vector2> SmothPathfinding(Vector2 start, Vector2 target)
{
Random rand = new Random();
SVector3 startVector = new SVector3(start.x_, 0, start.y_);
SVector3 endVector = new SVector3(target.x_, 0, target.y_);
NavQueryFilter filter = new NavQueryFilter();
//buildData = new NavMeshBuilder(polyMesh, polyMeshDetail, new SharpNav.Pathfinding.OffMeshConnection[0], settings);
//tiledNavMesh = new TiledNavMesh(buildData);
//navMeshQuery = new NavMeshQuery(tiledNavMesh, 2048);
SVector3 extents = new SVector3(5, 5, 5);
navMeshQuery.FindNearestPoly(ref startVector, ref extents, out startPt);
navMeshQuery.FindNearestPoly(ref endVector, ref extents, out endPt);
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 = 2f;
float SLOP = 0.1f;
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);
}
}
List <Vector2> _path = new List <Vector2>();
for (int i = 0; i < smoothPath.Count; i++)
{
_path.Add(new Vector2(smoothPath[i].X, smoothPath[i].Z));
}
return(_path);
}
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);
}
}
public void DrawCrowd()
{
if (Program._agents.Count == 0)
{
return;
}
GL.PushMatrix();
//The black line represents the actual path that the agent takes
/*GL.Color4(Color4.Black);
* GL.Begin(BeginMode.Lines);
* for (int i = 0; i < numActiveAgents; i++)
* {
* for (int j = 0; j < numIterations - 1; j++)
* {
* SVector3 v0 = trails[i].Trail[j];
* GL.Vertex3(v0.X, v0.Y, v0.Z);
*
* SVector3 v1 = trails[i].Trail[j + 1];
* GL.Vertex3(v1.X, v1.Y, v1.Z);
* }
* }
* GL.End();
*
* //The yellow line represents the ideal path from the start to the target
* GL.Color4(Color4.Yellow);
* GL.LineWidth(1.5f);
* GL.Begin(BeginMode.Lines);
* for (int i = 0; i < numActiveAgents; i++)
* {
* SVector3 v0 = trails[i].Trail[0];
* GL.Vertex3(v0.X, v0.Y, v0.Z);
*
* SVector3 v1 = trails[i].Trail[AGENT_MAX_TRAIL - 1];
* GL.Vertex3(v1.X, v1.Y, v1.Z);
* }
* GL.End();
*
* //The cyan point represents the agent's starting location
* GL.PointSize(100.0f);
* GL.Color4(Color4.Cyan);
* GL.Begin(BeginMode.Points);
* for (int i = 0; i < numActiveAgents; i++)
* {
* SVector3 v0 = trails[i].Trail[0];
* GL.Vertex3(v0.X, v0.Y, v0.Z);
* }
* GL.End();
*
* //The red point represent's the agent's target location
* GL.Color4(Color4.PaleVioletRed);
* GL.Begin(BeginMode.Points);
* for (int i = 0; i < numActiveAgents; i++)
* {
* SVector3 v0 = trails[i].Trail[AGENT_MAX_TRAIL - 1];
* GL.Vertex3(v0.X, v0.Y, v0.Z);
* }
* GL.End();*/
//GL.DepthMask(true);
GL.Color4(Color4.PaleVioletRed);
GL.PointSize(10);
GL.LineWidth(0.8f);
//for (int i = 0; i < Program._agents.Count; i++)
//{
// GL.Begin(BeginMode.Lines);
// GL.Color4(Color4.Green);
// //GL.Color4(new Color4((float)(Simulate.MathHelper.random.NextDouble()), (float)(Simulate.MathHelper.random.NextDouble()), (float)(Simulate.MathHelper.random.NextDouble()),0.5f));
// for (int j = 0; j < Program._agents[i].navPoints.Count - 1; j++)
// {
// SharpNav.Geometry.Vector3 p = new SharpNav.Geometry.Vector3(Program._agents[i].navPoints[j].x_, 1f, Program._agents[i].navPoints[j].y_);
// GL.Vertex3(p.X, p.Y, p.Z);
// }
// GL.End();
//}
for (int i = 0; i < Program._agents.Count; i++)
{
//song 画导航点
//GL.Begin(BeginMode.Points);
//GL.Color4(Color4.Green);
////GL.Color4(new Color4((float)(Simulate.MathHelper.random.NextDouble()), (float)(Simulate.MathHelper.random.NextDouble()), (float)(Simulate.MathHelper.random.NextDouble()),0.5f));
//for (int j = 0; j < Program._agents[i].navPoints.Count - 1; j++)
//{
// SharpNav.Geometry.Vector3 p = new SharpNav.Geometry.Vector3(Program._agents[i].navPoints[j].x_, 1f, Program._agents[i].navPoints[j].y_);
// GL.Vertex3(p.X, p.Y, p.Z);
//}
//GL.End();
////画导航线 song
//GL.Begin(BeginMode.Lines);
//GL.Color4(Color4.Green);
////GL.Color4(new Color4((float)(Simulate.MathHelper.random.NextDouble()), (float)(Simulate.MathHelper.random.NextDouble()), (float)(Simulate.MathHelper.random.NextDouble()),0.5f));
//for (int j = 1; j < Program._agents[i].navPoints.Count - 1; j++)
//{
// SharpNav.Geometry.Vector3 p = new SharpNav.Geometry.Vector3(Program._agents[i].navPoints[j].x_, 1f, Program._agents[i].navPoints[j].y_);
// GL.Vertex3(p.X, p.Y, p.Z);
// p = new SharpNav.Geometry.Vector3(Program._agents[i].navPoints[j - 1].x_, 1f, Program._agents[i].navPoints[j - 1].y_);
// GL.Vertex3(p.X, p.Y, p.Z);
//}
//GL.End();
//画当前目标线
//try
//{
// GL.Begin(BeginMode.Lines);
// GL.Color4(Program._agents[i].color);
// if (Program._agents[i].navPoints.Count > 1)
// {
// SharpNav.Geometry.Vector3 p = new SharpNav.Geometry.Vector3(Program._agents[i].positionNow.x_, 1.01f, Program._agents[i].positionNow.y_);
// GL.Vertex3(p.X, p.Y, p.Z);
// p = new SharpNav.Geometry.Vector3(Program._agents[i].navPoints[0].x_, 1.01f, Program._agents[i].navPoints[0].y_);
// GL.Vertex3(p.X, p.Y, p.Z);
// }
// GL.End();
//}
//catch
//{
//}
}
GL.Color4(Color4.Green);
if (agentCylinder != null)
{
for (int i = 0; i < Program._agents.Count; i++)
{
try
{
SharpNav.Geometry.Vector3 p = new SharpNav.Geometry.Vector3(Program._agents[i].positionNow.x_, 0, Program._agents[i].positionNow.y_);
agentCylinder.Draw(new OpenTK.Vector3(p.X, p.Y, p.Z), Program._agents[i].color);
//#if outfile
// FileHelper.Write(Program._agents[i].positionNow.x_ + " " + Program._agents[i].positionNow.y_ + " ");
//#endif
}
catch (Exception e)
{
Console.WriteLine("多线程对_agents.count访问出错 - 绘制");
Console.WriteLine(e);
}
}
//#if outfile
// FileHelper.NewLine();
//#endif
//#if outfile
// //输出agent信息文件
// for (int i = 0; i < Program._agents.Count; i++)
// {
// //FileHelper.Write(_agents[i].positionNow.x_ + " " + _agents[i].positionNow.y_ + " " + _agents[i].navPoints.Count + " ");
// FileHelper.Write(Program._agents[i].positionNow.x_ + " " + Program._agents[i].positionNow.y_ + " ");
// }
// FileHelper.NewLine();
//#endif
}
GL.PopMatrix();
}
static void VMad( ref SharpNav.Geometry.Vector3 dest, SharpNav.Geometry.Vector3 v1, SharpNav.Geometry.Vector3 v2, float s )
{
dest.X = v1.X + v2.X * s;
dest.Y = v1.Y + v2.Y * s;
dest.Z = v1.Z + v2.Z * s;
}
UnityEngine.Vector3 ConvertVector3(SharpNav.Geometry.Vector3 vector3)
{
return(new UnityEngine.Vector3(vector3.X, vector3.Y, vector3.Z));
}
private void GenerateCrowd()
{
if (!hasGenerated || navMeshQuery == null)
return;
Random rand = new Random();
crowd = new Crowd(MAX_AGENTS, 0.6f, ref tiledNavMesh);
SVector3 c = new SVector3(10, 0, 0);
SVector3 e = new SVector3(5, 5, 5);
AgentParams ap = new AgentParams();
ap.Radius = 0.6f;
ap.Height = 2.0f;
ap.MaxAcceleration = 8.0f;
ap.MaxSpeed = 3.5f;
ap.CollisionQueryRange = ap.Radius * 12.0f;
ap.PathOptimizationRange = ap.Radius * 30.0f;
ap.UpdateFlags = new UpdateFlags();
//initialize starting positions for each active agent
for (int i = 0; i < numActiveAgents; i++)
{
//Get the polygon that the starting point is in
NavPoint startPt;
navMeshQuery.FindNearestPoly(ref c, ref e, out startPt);
//Pick a new random point that is within a certain radius of the current point
NavPoint newPt;
navMeshQuery.FindRandomPointAroundCircle(ref startPt, 1000, out newPt);
c = newPt.Position;
//Save this random point as the starting position
trails[i].Trail = new SVector3[AGENT_MAX_TRAIL];
trails[i].Trail[0] = newPt.Position;
trails[i].HTrail = 0;
//add this agent to the crowd
int idx = crowd.AddAgent(newPt.Position, ap);
//Give this agent a target point
NavPoint targetPt;
navMeshQuery.FindRandomPointAroundCircle(ref newPt, 1000, out targetPt);
crowd.GetAgent(idx).RequestMoveTarget(targetPt.Polygon, targetPt.Position);
trails[i].Trail[AGENT_MAX_TRAIL - 1] = targetPt.Position;
}
}
private void GeneratePathfinding()
{
if (!hasGenerated)
{
return;
}
Random rand = new Random();
buildData = new NavMeshBuilder(polyMesh, polyMeshDetail, new SharpNav.Pathfinding.OffMeshConnection[0], settings);
tiledNavMesh = new TiledNavMesh(buildData);
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(startPt, 1000, out endPt);
//calculate the overall path, which contains an array of polygon references
int MAX_POLYS = 256;
path = new List <PolyId>(MAX_POLYS);
navMeshQuery.FindPath(ref startPt, ref endPt, path);
//find a smooth path over the mesh surface
int npolys = path.Count;
PolyId[] polys = path.ToArray();
SVector3 iterPos = new SVector3();
SVector3 targetPos = new SVector3();
navMeshQuery.ClosestPointOnPoly(startPt.Polygon, startPt.Position, ref iterPos);
navMeshQuery.ClosestPointOnPoly(polys[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();
int steerPosFlag = 0;
PolyId steerPosRef = PolyId.Null;
if (!GetSteerTarget(navMeshQuery, iterPos, targetPos, SLOP, polys, npolys, ref steerPos, ref steerPosFlag, ref steerPosRef))
{
break;
}
bool endOfPath = (steerPosFlag & PathfindingCommon.STRAIGHTPATH_END) != 0 ? true : false;
bool offMeshConnection = (steerPosFlag & PathfindingCommon.STRAIGHTPATH_OFFMESH_CONNECTION) != 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 <PolyId> visited = new List <PolyId>(16);
navMeshQuery.MoveAlongSurface(new NavPoint(polys[0], iterPos), moveTgt, ref result, visited);
npolys = FixupCorridor(polys, npolys, MAX_POLYS, visited);
float h = 0;
navMeshQuery.GetPolyHeight(polys[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);
}
}
}
private bool GetSteerTarget(NavMeshQuery navMeshQuery, SVector3 startPos, SVector3 endPos, float minTargetDist, SharpNav.Pathfinding.Path path,
ref SVector3 steerPos, ref StraightPathFlags steerPosFlag, ref NavPolyId steerPosRef)
{
StraightPath steerPath = new StraightPath();
navMeshQuery.FindStraightPath(startPos, endPos, path, steerPath, 0);
int nsteerPath = steerPath.Count;
if (nsteerPath == 0)
return false;
//find vertex far enough to steer to
int ns = 0;
while (ns < nsteerPath)
{
if ((steerPath[ns].Flags & StraightPathFlags.OffMeshConnection) != 0 ||
!InRange(steerPath[ns].Point.Position, startPos, minTargetDist, 1000.0f))
break;
ns++;
}
//failed to find good point to steer to
if (ns >= nsteerPath)
return false;
steerPos = steerPath[ns].Point.Position;
steerPos.Y = startPos.Y;
steerPosFlag = steerPath[ns].Flags;
if (steerPosFlag == StraightPathFlags.None && ns == (nsteerPath - 1))
steerPosFlag = StraightPathFlags.End; // otherwise seeks path infinitely!!!
steerPosRef = steerPath[ns].Point.Polygon;
return true;
}
static Vector3F ToEngine( SharpNav.Geometry.Vector3 v )
{
return new Vector3F( v.X, -v.Z, v.Y );
}
/// <summary>
/// Scaled vector addition
/// </summary>
/// <param name="dest">Result</param>
/// <param name="v1">Vector 1</param>
/// <param name="v2">Vector 2</param>
/// <param name="s">Scalar</param>
private void VMad(ref SVector3 dest, SVector3 v1, SVector3 v2, float s)
{
dest.X = v1.X + v2.X * s;
dest.Y = v1.Y + v2.Y * s;
dest.Z = v1.Z + v2.Z * s;
}
public Microsoft.Xna.Framework.Vector2[] GetPath(Microsoft.Xna.Framework.Vector2 start, Microsoft.Xna.Framework.Vector2 end)
{
NavPoint startPt = navMeshQuery.FindNearestPoly(new SVector3(-start.X, 0, start.Y), new SharpNav.Geometry.Vector3(2f, 2f, 2f));
NavPoint endPt = navMeshQuery.FindNearestPoly(new SVector3(-end.X, 0, end.Y), new SharpNav.Geometry.Vector3(2f, 2f, 2f));
Path path = new Path();
navMeshQuery.FindPath(ref startPt, ref endPt, new NavQueryFilter(), path);
List <SVector3> smoothPath;
//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);
}
}
return(smoothPath.Select(x => new Microsoft.Xna.Framework.Vector2(-x.X, x.Z)).ToArray());
}
