/// <summary>
/// The main entry point for the application.
/// </summary>
static void Main(string[] args)
{
using (Pathfinding game = new Pathfinding())
{
game.Run();
}
}
/** Deserializes common info additively
* Common info is what is shared between the editor serialization and the runtime serializer.
* This is mostly everything except the graph inspectors which serialize some extra data in the editor
*/
public void DeserializeGraphsPartAdditive(Pathfinding.Serialization.AstarSerializer sr)
{
if (graphs == null) graphs = new NavGraph[0];
if (userConnections == null) userConnections = new UserConnection[0];
List<NavGraph> gr = new List<NavGraph>(graphs);
gr.AddRange (sr.DeserializeGraphs ());
graphs = gr.ToArray();
List<UserConnection> conns = new List<UserConnection>(userConnections);
conns.AddRange (sr.DeserializeUserConnections());
userConnections = conns.ToArray ();
sr.DeserializeNodes();
sr.DeserializeExtraInfo();
sr.PostDeserialization();
for (int i=0;i<graphs.Length;i++) {
for (int j=i+1;j<graphs.Length;j++) {
if (graphs[i].guid == graphs[j].guid) {
Debug.LogWarning ("Guid Conflict when importing graphs additively. Imported graph will get a new Guid.\nThis message is (relatively) harmless.");
graphs[i].guid = Pathfinding.Util.Guid.NewGuid ();
break;
}
}
}
}
/** Deserializes common info.
* Common info is what is shared between the editor serialization and the runtime serializer.
* This is mostly everything except the graph inspectors which serialize some extra data in the editor
*/
public void DeserializeGraphsPart (Pathfinding.Serialization.AstarSerializer sr) {
ClearGraphs ();
graphs = sr.DeserializeGraphs ();
if ( graphs != null ) for ( int i = 0; i<graphs.Length;i++ ) if ( graphs[i] != null ) graphs[i].graphIndex = (uint)i;
userConnections = sr.DeserializeUserConnections();
//sr.DeserializeNodes();
sr.DeserializeExtraInfo();
//Assign correct graph indices.
for (int i=0;i<graphs.Length;i++) {
if (graphs[i] == null) continue;
graphs[i].GetNodes (delegate (GraphNode node) {
node.GraphIndex = (uint)i;
return true;
});
}
sr.PostDeserialization();
}
public float DistanceTo(Pathfinding.IPoint pPoint)
{
if (pPoint is TileNode) {
TileNode otherNode = pPoint as TileNode;
return this.worldPosition.DistanceTo(otherNode.worldPosition);
}
else {
throw new NotImplementedException();
}
}
/** Deserializes common info.
* Common info is what is shared between the editor serialization and the runtime serializer.
* This is mostly everything except the graph inspectors which serialize some extra data in the editor
*/
public void DeserializeGraphsPart (Pathfinding.Serialization.AstarSerializer sr) {
ClearGraphs ();
graphs = sr.DeserializeGraphs ();
if ( graphs != null ) for ( int i = 0; i<graphs.Length;i++ ) if ( graphs[i] != null ) graphs[i].graphIndex = (uint)i;
userConnections = sr.DeserializeUserConnections();
//sr.DeserializeNodes();
sr.DeserializeExtraInfo();
sr.PostDeserialization();
}
/** Main serializer function.
* Serializes all graphs to a byte array
* A similar function exists in the AstarEditor.cs script to save additional info */
public byte[] SerializeGraphs (Pathfinding.Serialization.SerializeSettings settings, out uint checksum) {
AstarPath.active.BlockUntilPathQueueBlocked();
Pathfinding.Serialization.AstarSerializer sr = new Pathfinding.Serialization.AstarSerializer(this, settings);
sr.OpenSerialize();
SerializeGraphsPart (sr);
byte[] bytes = sr.CloseSerialize();
checksum = sr.GetChecksum ();
#if ASTARDEBUG
Debug.Log ("Got a whole bunch of data, "+bytes.Length+" bytes");
#endif
return bytes;
}
public void SaveCacheData (Pathfinding.Serialization.SerializeSettings settings) {
data_cachedStartup = SerializeGraphs (settings);
cacheStartup = true;
}
/** Deserializes common info additively
* Common info is what is shared between the editor serialization and the runtime serializer.
* This is mostly everything except the graph inspectors which serialize some extra data in the editor
*/
public void DeserializeGraphsPartAdditive(Pathfinding.Serialize.AstarSerializer sr)
{
if (graphs == null) graphs = new NavGraph[0];
if (userConnections == null) userConnections = new UserConnection[0];
List<NavGraph> gr = new List<NavGraph>(graphs);
gr.AddRange (sr.DeserializeGraphs ());
graphs = gr.ToArray();
List<UserConnection> conns = new List<UserConnection>(userConnections);
conns.AddRange (sr.DeserializeUserConnections());
userConnections = conns.ToArray ();
sr.DeserializeNodes();
sr.DeserializeExtraInfo();
sr.PostDeserialization();
}
/** Deserializes common info.
* Common info is what is shared between the editor serialization and the runtime serializer.
* This is mostly everything except the graph inspectors which serialize some extra data in the editor
*/
public void DeserializeGraphsPart (Pathfinding.Serialization.AstarSerializer sr) {
ClearGraphs ();
graphs = sr.DeserializeGraphs ();
sr.DeserializeExtraInfo();
//Assign correct graph indices.
for (int i=0;i<graphs.Length;i++) {
if (graphs[i] == null) continue;
graphs[i].GetNodes (node => {
node.GraphIndex = (uint)i;
return true;
});
}
sr.PostDeserialization();
}
/** Main serializer function.
* Serializes all graphs to a byte array
* A similar function exists in the AstarPathEditor.cs script to save additional info */
public byte[] SerializeGraphs (Pathfinding.Serialization.SerializeSettings settings, out uint checksum) {
AstarPath.active.BlockUntilPathQueueBlocked();
var sr = new Pathfinding.Serialization.AstarSerializer(this, settings);
sr.OpenSerialize();
SerializeGraphsPart (sr);
byte[] bytes = sr.CloseSerialize();
checksum = sr.GetChecksum ();
return bytes;
}
/** Adds obstacles for a graph */
public void AddGraphObstacles (Pathfinding.RVO.Simulator sim, NavGraph graph) {
if (obstacles.Count > 0 && lastSim != null && lastSim != sim) {
Debug.LogError("Simulator has changed but some old obstacles are still added for the previous simulator. Deleting previous obstacles.");
RemoveObstacles();
}
//Remember which simulator these obstacles were added to
lastSim = sim;
var ng = graph as INavmesh;
if (ng == null) return;
//Assume less than 20 vertices per node (actually assumes 3, but I will change that some day)
var uses = new int[20];
ng.GetNodes(delegate(GraphNode _node) {
var node = _node as TriangleMeshNode;
uses[0] = uses[1] = uses[2] = 0;
if (node != null) {
//Find out which edges are shared with other nodes
for (int j = 0; j < node.connections.Length; j++) {
var other = node.connections[j] as TriangleMeshNode;
// Not necessarily a TriangleMeshNode
if (other != null) {
int a = node.SharedEdge(other);
if (a != -1) uses[a] = 1;
}
}
//Loop through all edges on the node
for (int j = 0; j < 3; j++) {
//The edge is not shared with any other node
//I.e it is an exterior edge on the mesh
if (uses[j] == 0) {
//The two vertices of the edge
var v1 = (Vector3)node.GetVertex(j);
var v2 = (Vector3)node.GetVertex((j+1) % node.GetVertexCount());
//I think node vertices always should be clockwise, but it's good to be certain
/*if (!Polygon.IsClockwise (v1,v2,(Vector3)node.GetVertex((j+2) % node.GetVertexCount()))) {
* Vector3 tmp = v2;
* v2 = v1;
* v1 = tmp;
* }*/
#if ASTARDEBUG
Debug.DrawLine(v1, v2, Color.red);
Debug.DrawRay(v1, Vector3.up*wallHeight, Color.red);
#endif
//Find out the height of the wall/obstacle we are about to add
float height = System.Math.Abs(v1.y-v2.y);
height = System.Math.Max(height, 5);
//Add the edge as a line obstacle
obstacles.Add(sim.AddObstacle(v1, v2, wallHeight));
}
}
}
return true;
});
}
void DrawIsometricField(GridGraph graph)
{
var isometricGUIContent = new GUIContent ("Isometric Angle", "For an isometric 2D game, you can use this parameter to scale the graph correctly.\nIt can also be used to create a hexagon grid.");
var isometricOptions = new [] {new GUIContent ("None (0°)"), new GUIContent ("Isometric (≈54.74°)"), new GUIContent("Custom")};
var isometricValues = new [] {0f, standardIsometric};
var isometricOption = 2;
for (int i = 0; i < isometricValues.Length; i++) {
if (Mathf.Approximately (graph.isometricAngle, isometricValues[i])) {
isometricOption = i;
}
}
var prevIsometricOption = isometricOption;
isometricOption = EditorGUILayout.IntPopup (isometricGUIContent, isometricOption, isometricOptions, new [] {0, 1, 2});
if (prevIsometricOption != isometricOption) {
// Change to something that will not match the predefined values above
graph.isometricAngle = 45;
}
if (isometricOption < 2) {
graph.isometricAngle = isometricValues[isometricOption];
} else {
// Custom
graph.isometricAngle = EditorGUILayout.FloatField (isometricGUIContent, graph.isometricAngle);
}
}
/** Serializes common info to the serializer.
* Common info is what is shared between the editor serialization and the runtime serializer.
* This is mostly everything except the graph inspectors which serialize some extra data in the editor
*/
public void SerializeGraphsPart (Pathfinding.Serialization.AstarSerializer sr) {
sr.SerializeGraphs(graphs);
sr.SerializeExtraInfo();
}
internal void CalculateVelocity ( Pathfinding.RVO.Simulator.WorkerContext context ) {
if ( locked ) {
newVelocity = Vector2.zero;
return;
}
if ( context.vos.Length < neighbours.Count ) {
context.vos = new VO[Mathf.Max(context.vos.Length*2, neighbours.Count)];
}
Vector2 position2D = new Vector2(position.x,position.z);
var vos = context.vos;
var voCount = 0;
Vector2 optimalVelocity = new Vector2(velocity.x, velocity.z);
float inverseAgentTimeHorizon = 1.0f/agentTimeHorizon;
for ( int o = 0; o < neighbours.Count; o++ ) {
Agent other = neighbours[o];
if ( other == this ) continue;
float maxY = System.Math.Min (position.y+height,other.position.y+other.height);
float minY = System.Math.Max (position.y,other.position.y);
//The agents cannot collide since they
//are on different y-levels
if (maxY - minY < 0) {
continue;
}
Vector2 otherOptimalVelocity = new Vector2(other.Velocity.x, other.velocity.z);
float totalRadius = radius + other.radius;
// Describes a circle on the border of the VO
//float boundingRadius = totalRadius * inverseAgentTimeHorizon;
Vector2 voBoundingOrigin = new Vector2(other.position.x,other.position.z) - position2D;
//float boundingDist = voBoundingOrigin.magnitude;
Vector2 relativeVelocity = optimalVelocity - otherOptimalVelocity;
{
//voBoundingOrigin *= inverseAgentTimeHorizon;
//boundingDist *= inverseAgentTimeHorizon;
// Common case, no collision
vos[voCount] = new VO( voBoundingOrigin, (optimalVelocity + otherOptimalVelocity)*0.5f, totalRadius, relativeVelocity, inverseAgentTimeHorizon);
voCount++;
}
#if OLD
//Debug.Log (boundingDist + " " + total
if ( boundingDist < totalRadius ) {
// Collision
// We want the collision to end this frame, so ignore the inverseAgentTimeHorizon and
// instead use the frame's delta time instead. The closest point will not always be
// on the circle, but the velocity will have to be really
// high for it not to be, so just assume the velocity is low
// (if not, the agents will jump through each other)
voBoundingOrigin *= (1.0f/simulator.DeltaTime);
boundingDist *= (1.0f/simulator.DeltaTime);
Vector2 d = relativeVelocity - voBoundingOrigin;
float dm = d.magnitude;
// Normalize
d /= dm;
float resMagn = (totalRadius*(1.0f/simulator.DeltaTime) - dm);
maxRelVelMagn = System.Math.Max ( resMagn, maxRelVelMagn );
Vector2 closestOnCircleDelta = d*resMagn;//d*(boundingDist-dm);
Vector2 closestOnCircleVector = d;
halfPlanes.Add ( new HalfPlane ( closestOnCircleDelta*0.5f + optimalVelocity, closestOnCircleVector ) );
if (debug) halfPlanes[halfPlanes.Count-1].Draw ( Color.blue );
} else {
voBoundingOrigin *= inverseAgentTimeHorizon;
boundingDist *= inverseAgentTimeHorizon;
// Common case, no collision
VO vo = new VO( voBoundingOrigin + (optimalVelocity + otherOptimalVelocity)*0.5f, voBoundingOrigin.normalized, totalRadius*inverseAgentTimeHorizon);
maxRelVelMagn = System.Math.Max ( maxRelVelMagn, relativeVelocity.magnitude );
// Optimized: float factor = Vector3.Dot ( optimalVelocity, voBoundingOrigin ) < (boundingDist^2);
if (debug) DrawVO ( voBoundingOrigin, boundingRadius, Vector2.zero);
if ( debug ) {
//Debug.DrawLine ( To3D(position), To3D ( relativeVelocity ), Color.blue );
DrawCross ( relativeVelocity, Color.white, 0.5f);
}
//DrawCross ( voBoundingOrigin );
float alpha = Mathf.Atan2 ( -voBoundingOrigin.y, -voBoundingOrigin.x );
float delta = Mathf.Abs(Mathf.Acos (boundingRadius/(voBoundingOrigin).magnitude));
float actualDot = Vector2.Dot ( -voBoundingOrigin.normalized, (relativeVelocity-voBoundingOrigin).normalized );
float dot = Mathf.Abs ( Mathf.Acos ( Mathf.Clamp01(actualDot) ) );
bool closestIsCircle = dot < delta;
/*DrawCircle ( circleCenter, radius, alpha-delta, alpha+delta, Color.black );
Vector2 p1 = circleCenter + new Vector2 ( Mathf.Cos (alpha-delta), Mathf.Sin (alpha-delta) ) * boundingRadius;
Vector2 p2 = circleCenter + new Vector2 ( Mathf.Cos (alpha+delta), Mathf.Sin (alpha+delta) ) * boundingRadius;*/
if ( debug ) Debug.Log (" <= 1 && " + dot + " < " + delta + " ( " +actualDot + ")");
if ( closestIsCircle ) {
// The cutoff circle is the closest
// part of the VO
// Find the closest point on the VO
Vector2 closestOnCircleVector = (relativeVelocity - voBoundingOrigin);
float magn = closestOnCircleVector.magnitude;
closestOnCircleVector = closestOnCircleVector/magn;//(closestOnCircleVector/magn)*boundingRadius*(1-(magn/boundingRadius));
Vector2 closestOnCircle = closestOnCircleVector*boundingRadius + voBoundingOrigin;
//if ( magn > boundingRadius ) closestOnCircleVector = -closestOnCircleVector;
if ( debug ) DrawCross ( closestOnCircle, 1);
halfPlanes.Add ( new HalfPlane ( (closestOnCircle - relativeVelocity)*0.5f + optimalVelocity, closestOnCircleVector.normalized ) );
if (debug) halfPlanes[halfPlanes.Count-1].Draw ( closestIsCircle ? Color.blue : Color.red );
} else {
// One of the legs of the VO is the closest part
// find the closest point on the VO
bool left = Polygon.Left ( Vector2.zero, voBoundingOrigin, relativeVelocity );
float gamma = alpha + (left ? delta : -delta);
// Point on circle
Vector2 line = new Vector2 ( Mathf.Cos (gamma), Mathf.Sin (gamma) ) * boundingRadius;
// Vector tangent to circle which is the correct line tangent
Vector2 dir = new Vector2(line.y,-line.x).normalized;
// Point in space
line += voBoundingOrigin;
Vector2 onLine = line + dir * Vector2.Dot ( dir, relativeVelocity - line);
if (debug) {
DrawCross ( onLine, Color.red, 1 );
Debug.DrawRay ( To3D(line), To3D(dir*10), Color.red );
Debug.Log (line + " " + dir + " " + gamma + " " + relativeVelocity);
}
if ( !left ) dir = -dir;
halfPlanes.Add ( new HalfPlane ( (onLine - relativeVelocity)*0.5f + optimalVelocity, (line-voBoundingOrigin).normalized ) );
if (debug) halfPlanes[halfPlanes.Count-1].Draw ( Color.blue );
}
}
#endif
}
if ( this.DebugDraw ) {
/*for ( int x = 0; x < simulator.tex.width; x++ ) {
for ( int y = 0; y < simulator.tex.height; y++ ) {
Vector2 p = new Vector2 (x*simulator.textureSize / simulator.tex.width, y*simulator.textureSize / simulator.tex.height);
Vector2 dir = Vector2.zero;
float weight = 0;
for ( int i = 0; i < voCount; i++ ) {
float w = 0;
dir += vos[i].Sample (p-position, out w);
if ( w > weight ) weight = w;
}
Vector2 d2 = (desiredVelocity - (p-position));
dir += d2*DesiredVelocityScale;
if ( d2.magnitude * DesiredVelocityWeight > weight ) weight = d2.magnitude * DesiredVelocityWeight;
if ( weight > 0 ) dir /= weight;
Vector2 d3 = simulator.SampleDensity (p+position);
Debug.DrawRay ( To3D(p), To3D(d3*1f), Color.blue );
simulator.Plot (p, Rainbow(weight*simulator.colorScale));
}
}*/
}
//if ( debug ) {
float best = float.PositiveInfinity;
Vector2 result = Vector2.zero;
float cutoff = new Vector2(velocity.x,velocity.z).magnitude*simulator.qualityCutoff;
//for ( int i = 0; i < 10; i++ ) {
{
result = Trace ( vos, voCount, new Vector2(desiredVelocity.x, desiredVelocity.z), cutoff, out best );
}
// Can be uncommented for higher quality local avoidance
/*for ( int i = 0; i < 3; i++ ) {
Vector2 p = desiredVelocity + new Vector2(Mathf.Cos(Mathf.PI*2*(i/3.0f)), Mathf.Sin(Mathf.PI*2*(i/3.0f)));
float score;
Vector2 res = Trace ( vos, voCount, p, velocity.magnitude*simulator.qualityCutoff, out score );
if ( score < best ) {
//if ( score < best*0.9f ) Debug.Log ("Better " + score + " < " + best);
result = res;
best = score;
}
}*/
{
Vector2 p = Vector2.zero;
float score;
Vector2 res = Trace ( vos, voCount, p, cutoff, out score );
if ( score < best ) {
//if ( score < best*0.9f ) Debug.Log ("Better " + score + " < " + best);
result = res;
best = score;
}
}
//}
if (DebugDraw) DrawCross (result+position2D);
newVelocity = To3D(Vector2.ClampMagnitude (result, maxSpeed));
}
public virtual float DistanceTo(Pathfinding.IPoint pPoint)
{
if (pPoint is TileNode) {
TileNode otherNode = pPoint as TileNode;
return localPoint.EuclidianDistanceTo(otherNode.localPoint);
}
else {
throw new NotImplementedException();
}
}
/** Deserializes common info.
* Common info is what is shared between the editor serialization and the runtime serializer.
* This is mostly everything except the graph inspectors which serialize some extra data in the editor
*/
public void DeserializeGraphsPart(Pathfinding.Serialize.AstarSerializer sr)
{
graphs = sr.DeserializeGraphs ();
userConnections = sr.DeserializeUserConnections();
sr.DeserializeNodes();
sr.DeserializeExtraInfo();
sr.PostDeserialization();
}
/** Deserializes common info additively
* Common info is what is shared between the editor serialization and the runtime serializer.
* This is mostly everything except the graph inspectors which serialize some extra data in the editor
*/
public void DeserializeGraphsPartAdditive (Pathfinding.Serialization.AstarSerializer sr) {
if (graphs == null) graphs = new NavGraph[0];
var gr = new List<NavGraph>(graphs);
// Set an offset so that the deserializer will load
// the graphs with the correct graph indexes
sr.SetGraphIndexOffset (gr.Count);
gr.AddRange (sr.DeserializeGraphs ());
graphs = gr.ToArray();
//Assign correct graph indices. Issue #21
for (int i=0;i<graphs.Length;i++) {
if (graphs[i] == null) continue;
graphs[i].GetNodes (node => {
node.GraphIndex = (uint)i;
return true;
});
}
sr.DeserializeExtraInfo();
sr.PostDeserialization();
for (int i=0;i<graphs.Length;i++) {
for (int j=i+1;j<graphs.Length;j++) {
if (graphs[i] != null && graphs[j] != null && graphs[i].guid == graphs[j].guid) {
Debug.LogWarning ("Guid Conflict when importing graphs additively. Imported graph will get a new Guid.\nThis message is (relatively) harmless.");
graphs[i].guid = Pathfinding.Util.Guid.NewGuid ();
break;
}
}
}
}
/** Main serializer function.
* Serializes all graphs to a byte array
* A similar function exists in the AstarEditor.cs script to save additional info */
public byte[] SerializeGraphs(Pathfinding.Serialize.SerializeSettings settings, out uint checksum)
{
Pathfinding.Serialize.AstarSerializer sr = new Pathfinding.Serialize.AstarSerializer(this, settings);
sr.OpenSerialize();
SerializeGraphsPart (sr);
byte[] bytes = sr.CloseSerialize();
checksum = sr.GetChecksum ();
return bytes;
}
public Vector3 IntPointToV3 (Pathfinding.ClipperLib.IntPoint p) {
Int3 ip = new Int3((int)p.X,0,(int)p.Y);
return (Vector3)ip;
}
/** Main serializer function. */
public byte[] SerializeGraphs (Pathfinding.Serialization.SerializeSettings settings) {
uint checksum;
return SerializeGraphs (settings, out checksum);
}
internal void CalculateVelocity(Pathfinding.RVO.Simulator.WorkerContext context)
{
if (locked)
{
newVelocity = Vector2.zero;
return;
}
if (context.vos.Length < neighbours.Count + simulator.obstacles.Count)
{
context.vos = new VO[Mathf.Max(context.vos.Length * 2, neighbours.Count + simulator.obstacles.Count)];
}
Vector2 position2D = new Vector2(position.x, position.z);
var vos = context.vos;
var voCount = 0;
Vector2 optimalVelocity = new Vector2(velocity.x, velocity.z);
float inverseAgentTimeHorizon = 1.0f / agentTimeHorizon;
float wallThickness = simulator.WallThickness;
float wallWeight = simulator.algorithm == Simulator.SamplingAlgorithm.GradientDecent ? 1 : WallWeight;
for (int i = 0; i < simulator.obstacles.Count; i++)
{
var obstacle = simulator.obstacles[i];
var vertex = obstacle;
do
{
if (vertex.ignore || position.y > vertex.position.y + vertex.height || position.y + height < vertex.position.y || (vertex.layer & collidesWith) == 0)
{
vertex = vertex.next;
Debug.Log("continue");
continue;
}
float cross = VO.Det(new Vector2(vertex.position.x, vertex.position.z), vertex.dir, position2D);// vertex.dir.x * ( vertex.position.z - position.z ) - vertex.dir.y * ( vertex.position.x - position.x );
// Signed distance from the line (not segment), lines are infinite
// Usually divided by vertex.dir.magnitude, but that is known to be 1
float signedDist = cross;
float dotFactor = Vector2.Dot(vertex.dir, position2D - new Vector2(vertex.position.x, vertex.position.z));
// It is closest to the segment
// if the dotFactor is <= 0 or >= length of the segment
// WallThickness*0.1 is added as a margin to avoid false positives when moving along the edges of square obstacles
bool closestIsEndpoints = dotFactor <= wallThickness * 0.05f || dotFactor >= (new Vector2(vertex.position.x, vertex.position.z) - new Vector2(vertex.next.position.x, vertex.next.position.z)).magnitude - wallThickness * 0.05f;
if (Mathf.Abs(signedDist) < neighbourDist)
{
if (signedDist <= 0 && !closestIsEndpoints && signedDist > -wallThickness)
{
// Inside the wall on the "wrong" side
vos[voCount] = new VO(position2D, new Vector2(vertex.position.x, vertex.position.z) - position2D, vertex.dir, wallWeight * 2);
voCount++;
}
else if (signedDist > 0)
{
//Debug.DrawLine (position, (vertex.position+vertex.next.position)*0.5f, Color.yellow);
Vector2 p1 = new Vector2(vertex.position.x, vertex.position.z) - position2D;
Vector2 p2 = new Vector2(vertex.next.position.x, vertex.next.position.z) - position2D;
Vector2 tang1 = (p1).normalized;
Vector2 tang2 = (p2).normalized;
vos[voCount] = new VO(position2D, p1, p2, tang1, tang2, wallWeight);
voCount++;
}
}
vertex = vertex.next;
} while (vertex != obstacle);
}
for (int o = 0; o < neighbours.Count; o++)
{
Agent other = neighbours[o];
if (other == this) continue;
float maxY = System.Math.Min(position.y + height, other.position.y + other.height);
float minY = System.Math.Max(position.y, other.position.y);
//The agents cannot collide since they
//are on different y-levels
if (maxY - minY < 0)
{
continue;
}
Vector2 otherOptimalVelocity = new Vector2(other.Velocity.x, other.velocity.z);
float totalRadius = radius + other.radius;
// Describes a circle on the border of the VO
//float boundingRadius = totalRadius * inverseAgentTimeHorizon;
Vector2 voBoundingOrigin = new Vector2(other.position.x, other.position.z) - position2D;
//float boundingDist = voBoundingOrigin.magnitude;
Vector2 relativeVelocity = optimalVelocity - otherOptimalVelocity;
{
//voBoundingOrigin *= inverseAgentTimeHorizon;
//boundingDist *= inverseAgentTimeHorizon;
// Common case, no collision
Vector2 voCenter;
if (other.locked)
{
voCenter = otherOptimalVelocity;
}
else
{
voCenter = (optimalVelocity + otherOptimalVelocity) * 0.5f;
}
vos[voCount] = new VO(voBoundingOrigin, voCenter, totalRadius, relativeVelocity, inverseAgentTimeHorizon, 1);
voCount++;
if (DebugDraw) DrawVO(position2D + voBoundingOrigin * inverseAgentTimeHorizon + voCenter, totalRadius * inverseAgentTimeHorizon, position2D + voCenter);
}
}
Vector2 result = Vector2.zero;
if (simulator.algorithm == Simulator.SamplingAlgorithm.GradientDecent)
{
if (this.DebugDraw)
{
const int PlotWidth = 40;
const float WorldPlotWidth = 15;
for (int x = 0; x < PlotWidth; x++)
{
for (int y = 0; y < PlotWidth; y++)
{
Vector2 p = new Vector2(x * WorldPlotWidth / PlotWidth, y * WorldPlotWidth / PlotWidth);
Vector2 dir = Vector2.zero;
float weight = 0;
for (int i = 0; i < voCount; i++)
{
float w = 0;
dir += vos[i].Sample(p - position2D, out w);
if (w > weight) weight = w;
}
Vector2 d2 = (new Vector2(desiredVelocity.x, desiredVelocity.z) - (p - position2D));
dir += d2 * DesiredVelocityScale;
if (d2.magnitude * DesiredVelocityWeight > weight) weight = d2.magnitude * DesiredVelocityWeight;
if (weight > 0) dir /= weight;
//Vector2 d3 = simulator.SampleDensity (p+position2D);
Debug.DrawRay(To3D(p), To3D(d2 * 0.00f), Color.blue);
//simulator.Plot (p, Rainbow(weight*simulator.colorScale));
float sc = 0;
Vector2 p0 = p - Vector2.one * WorldPlotWidth * 0.5f;
Vector2 p1 = Trace(vos, voCount, p0, 0.01f, out sc);
if ((p0 - p1).sqrMagnitude < Sqr(WorldPlotWidth / PlotWidth) * 2.6f)
{
Debug.DrawRay(To3D(p1 + position2D), Vector3.up * 1, Color.red);
}
}
}
}
//if ( debug ) {
float best = float.PositiveInfinity;
float cutoff = new Vector2(velocity.x, velocity.z).magnitude * simulator.qualityCutoff;
//for ( int i = 0; i < 10; i++ ) {
{
result = Trace(vos, voCount, new Vector2(desiredVelocity.x, desiredVelocity.z), cutoff, out best);
if (DebugDraw) DrawCross(result + position2D, Color.yellow, 0.5f);
}
// Can be uncommented for higher quality local avoidance
/*for ( int i = 0; i < 3; i++ ) {
Vector2 p = desiredVelocity + new Vector2(Mathf.Cos(Mathf.PI*2*(i/3.0f)), Mathf.Sin(Mathf.PI*2*(i/3.0f)));
float score;
Vector2 res = Trace ( vos, voCount, p, velocity.magnitude*simulator.qualityCutoff, out score );
if ( score < best ) {
//if ( score < best*0.9f ) Debug.Log ("Better " + score + " < " + best);
result = res;
best = score;
}
}*/
{
Vector2 p = this.Velocity;
float score;
Vector2 res = Trace(vos, voCount, p, cutoff, out score);
if (score < best)
{
//if ( score < best*0.9f ) Debug.Log ("Better " + score + " < " + best);
result = res;
best = score;
}
if (DebugDraw) DrawCross(res + position2D, Color.magenta, 0.5f);
}
}
else
{
// Adaptive sampling
Vector2[] samplePos = context.samplePos;
float[] sampleSize = context.sampleSize;
int samplePosCount = 0;
Vector2 desired2D = new Vector2(desiredVelocity.x, desiredVelocity.z);
float sampleScale = Mathf.Max(radius, Mathf.Max(desired2D.magnitude, Velocity.magnitude));
samplePos[samplePosCount] = desired2D;
sampleSize[samplePosCount] = sampleScale * 0.3f;
samplePosCount++;
const float GridScale = 0.3f;
// Initial 9 samples
samplePos[samplePosCount] = optimalVelocity;
sampleSize[samplePosCount] = sampleScale * GridScale;
samplePosCount++;
{
Vector2 fw = optimalVelocity * 0.5f;
Vector2 rw = new Vector2(fw.y, -fw.x);
const int Steps = 8;
for (int i = 0; i < Steps; i++)
{
samplePos[samplePosCount] = rw * Mathf.Sin(i * Mathf.PI * 2 / Steps) + fw * (1 + Mathf.Cos(i * Mathf.PI * 2 / Steps));
sampleSize[samplePosCount] = (1.0f - (Mathf.Abs(i - Steps * 0.5f) / Steps)) * sampleScale * 0.5f;
samplePosCount++;
}
const float InnerScale = 0.6f;
fw *= InnerScale;
rw *= InnerScale;
const int Steps2 = 6;
for (int i = 0; i < Steps2; i++)
{
samplePos[samplePosCount] = rw * Mathf.Cos((i + 0.5f) * Mathf.PI * 2 / Steps2) + fw * ((1.0f / InnerScale) + Mathf.Sin((i + 0.5f) * Mathf.PI * 2 / Steps2));
sampleSize[samplePosCount] = sampleScale * 0.3f;
samplePosCount++;
}
const float TargetScale = 0.2f;
const int Steps3 = 6;
for (int i = 0; i < Steps3; i++)
{
samplePos[samplePosCount] = optimalVelocity + new Vector2(sampleScale * TargetScale * Mathf.Cos((i + 0.5f) * Mathf.PI * 2 / Steps3), sampleScale * TargetScale * Mathf.Sin((i + 0.5f) * Mathf.PI * 2 / Steps3));
sampleSize[samplePosCount] = sampleScale * TargetScale * 2;
samplePosCount++;
}
}
samplePos[samplePosCount] = optimalVelocity * 0.5f;
sampleSize[samplePosCount] = sampleScale * 0.4f;
samplePosCount++;
const int KeepCount = Simulator.WorkerContext.KeepCount;
Vector2[] bestPos = context.bestPos;
float[] bestSizes = context.bestSizes;
float[] bestScores = context.bestScores;
for (int i = 0; i < KeepCount; i++)
{
bestScores[i] = float.PositiveInfinity;
}
bestScores[KeepCount] = float.NegativeInfinity;
Vector2 bestEver = optimalVelocity;
float bestEverScore = float.PositiveInfinity;
for (int sub = 0; sub < 3; sub++)
{
for (int i = 0; i < samplePosCount; i++)
{
float score = 0;
for (int vo = 0; vo < voCount; vo++)
{
score = System.Math.Max(score, vos[vo].ScalarSample(samplePos[i]));
}
// Note that velocity is a vector and speed is a scalar, not the same thing
float bonusForDesiredVelocity = (samplePos[i] - desired2D).magnitude;
// This didn't work out as well as I though
// Code left here because I might reenable it later
//float bonusForDesiredSpeed = Mathf.Abs (samplePos[i].magnitude - desired2D.magnitude);
float biasedScore = score + bonusForDesiredVelocity * DesiredVelocityWeight;// + bonusForDesiredSpeed*0;
score += bonusForDesiredVelocity * 0.001f;
if (DebugDraw)
{
DrawCross(position2D + samplePos[i], Rainbow(Mathf.Log(score + 1) * 5), sampleSize[i] * 0.5f);
}
if (biasedScore < bestScores[0])
{
for (int j = 0; j < KeepCount; j++)
{
if (biasedScore >= bestScores[j + 1])
{
bestScores[j] = biasedScore;
bestSizes[j] = sampleSize[i];
bestPos[j] = samplePos[i];
break;
}
}
}
if (score < bestEverScore)
{
bestEver = samplePos[i];
bestEverScore = score;
if (score == 0)
{
sub = 100;
break;
}
}
}
samplePosCount = 0;
for (int i = 0; i < KeepCount; i++)
{
Vector2 p = bestPos[i];
float s = bestSizes[i];
bestScores[i] = float.PositiveInfinity;
const float Half = 0.6f;
float offset = s * Half * 0.5f;
samplePos[samplePosCount + 0] = (p + new Vector2(+offset, +offset));
samplePos[samplePosCount + 1] = (p + new Vector2(-offset, +offset));
samplePos[samplePosCount + 2] = (p + new Vector2(-offset, -offset));
samplePos[samplePosCount + 3] = (p + new Vector2(+offset, -offset));
s *= s * Half;
sampleSize[samplePosCount + 0] = (s);
sampleSize[samplePosCount + 1] = (s);
sampleSize[samplePosCount + 2] = (s);
sampleSize[samplePosCount + 3] = (s);
samplePosCount += 4;
}
}
result = bestEver;
}
if (DebugDraw) DrawCross(result + position2D);
newVelocity = To3D(Vector2.ClampMagnitude(result, maxSpeed));
if (newVelocity != Vector3.zero)
{
float angle = Vector3.Angle(DesiredVelocity, newVelocity);
//:限制新速度方向
//如果计算出来的新速度是与设计速度的夹角大于angleMax
//则新速度旋转到最大夹角
float angleMax = 45;
if (angle > angleMax)
{
Vector2 firstObject = new Vector2(DesiredVelocity.x, DesiredVelocity.z);
Vector2 secondObject = new Vector2(newVelocity.x, newVelocity.z);
float det = firstObject.x * secondObject.y - firstObject.y * secondObject.x;
if (det > 0)//param2 is unclockwise
{
Vector2 verticalObject = secondObject.Rotate(-(angle - angleMax));
newVelocity.x = verticalObject.x;
newVelocity.z = verticalObject.y;
if (newVelocity.magnitude < minSpeed)
{
newVelocity = newVelocity.normalized * minSpeed;
}
}
else if (det < 0)
{
Vector2 verticalObject = secondObject.Rotate((angle - angleMax));
newVelocity.x = verticalObject.x;
newVelocity.z = verticalObject.y;
if (newVelocity.magnitude < minSpeed)
{
newVelocity = newVelocity.normalized * minSpeed;
}
}
else
{
newVelocity = Vector3.zero;
}
}
if (printVelocity)
{
Debug.Log("vel " + velocity + "newVelocity " + newVelocity);
}
}
}
/** Serializes common info to the serializer.
* Common info is what is shared between the editor serialization and the runtime serializer.
* This is mostly everything except the graph inspectors which serialize some extra data in the editor
*/
public void SerializeGraphsPart (Pathfinding.Serialization.AstarSerializer sr) {
sr.SerializeGraphs(graphs);
sr.SerializeUserConnections (userConnections);
sr.SerializeNodes();
sr.SerializeExtraInfo();
}
/** Main serializer function.
* Serializes all graphs to a byte array
* A similar function exists in the AstarEditor.cs script to save additional info */
public byte[] SerializeGraphs(Pathfinding.Serialize.SerializeSettings settings, out uint checksum)
{
Pathfinding.Serialize.AstarSerializer sr = new Pathfinding.Serialize.AstarSerializer(this, settings);
sr.OpenSerialize();
SerializeGraphsPart (sr);
byte[] bytes = sr.CloseSerialize();
checksum = sr.GetChecksum ();
#if DEBUG
Debug.Log ("Got a whole bunch of data, "+bytes.Length+" bytes");
#endif
return bytes;
}
/** Deserializes common info additively
* Common info is what is shared between the editor serialization and the runtime serializer.
* This is mostly everything except the graph inspectors which serialize some extra data in the editor
*/
public void DeserializeGraphsPartAdditive (Pathfinding.Serialization.AstarSerializer sr) {
if (graphs == null) graphs = new NavGraph[0];
if (userConnections == null) userConnections = new UserConnection[0];
List<NavGraph> gr = new List<NavGraph>(graphs);
gr.AddRange (sr.DeserializeGraphs ());
graphs = gr.ToArray();
if ( graphs != null ) for ( int i = 0; i<graphs.Length;i++ ) if ( graphs[i] != null ) graphs[i].graphIndex = (uint)i;
List<UserConnection> conns = new List<UserConnection>(userConnections);
conns.AddRange (sr.DeserializeUserConnections());
userConnections = conns.ToArray ();
sr.DeserializeNodes();
//Assign correct graph indices. Issue #21
for (int i=0;i<graphs.Length;i++) {
if (graphs[i] == null) continue;
graphs[i].GetNodes (delegate (GraphNode node) {
//GraphNode[] nodes = graphs[i].nodes;
node.GraphIndex = (uint)i;
return true;
});
}
sr.DeserializeExtraInfo();
sr.PostDeserialization();
for (int i=0;i<graphs.Length;i++) {
for (int j=i+1;j<graphs.Length;j++) {
if (graphs[i] != null && graphs[j] != null && graphs[i].guid == graphs[j].guid) {
Debug.LogWarning ("Guid Conflict when importing graphs additively. Imported graph will get a new Guid.\nThis message is (relatively) harmless.");
graphs[i].guid = Pathfinding.Util.Guid.NewGuid ();
break;
}
}
}
}
public static void SetTagField (GUIContent label, ref Pathfinding.TagMask value) {
GUILayout.BeginHorizontal ();
EditorGUIUtility.LookLikeControls();
EditorGUILayout.PrefixLabel (label,EditorStyles.layerMaskField);
string text = "";
if (value.tagsChange == 0) text = "Nothing";
else if (value.tagsChange == ~0) text = "Everything";
else {
text = System.Convert.ToString (value.tagsChange,2);
}
string[] tagNames = AstarPath.FindTagNames ();
if (GUILayout.Button (text,EditorStyles.layerMaskField,GUILayout.ExpandWidth (true))) {
GenericMenu menu = new GenericMenu ();
menu.AddItem (new GUIContent ("Everything"),value.tagsChange == ~0, value.SetValues, new Pathfinding.TagMask (~0,value.tagsSet));
menu.AddItem (new GUIContent ("Nothing"),value.tagsChange == 0, value.SetValues, new Pathfinding.TagMask (0,value.tagsSet));
for (int i=0;i<tagNames.Length;i++) {
bool on = (value.tagsChange >> i & 0x1) != 0;
Pathfinding.TagMask result = new Pathfinding.TagMask (on ? value.tagsChange & ~(1 << i) : value.tagsChange | 1<<i,value.tagsSet);
menu.AddItem (new GUIContent (tagNames[i]),on,value.SetValues, result);
}
menu.AddItem (new GUIContent ("Edit Tags..."),false,AstarPathEditor.EditTags);
menu.ShowAsContext ();
Event.current.Use ();
}
#if UNITY_LE_4_3
EditorGUIUtility.LookLikeInspector();
#endif
GUILayout.EndHorizontal ();
}
public static void TagsMaskField (GUIContent changeLabel, GUIContent setLabel,ref Pathfinding.TagMask value) {
GUILayout.BeginHorizontal ();
EditorGUIUtility.LookLikeControls();
EditorGUILayout.PrefixLabel (changeLabel,EditorStyles.layerMaskField);
string text = "";
if (value.tagsChange == 0) text = "Nothing";
else if (value.tagsChange == ~0) text = "Everything";
else {
text = System.Convert.ToString (value.tagsChange,2);
}
if (GUILayout.Button (text,EditorStyles.layerMaskField,GUILayout.ExpandWidth (true))) {
GenericMenu menu = new GenericMenu ();
menu.AddItem (new GUIContent ("Everything"),value.tagsChange == ~0, value.SetValues, new Pathfinding.TagMask (~0,value.tagsSet));
menu.AddItem (new GUIContent ("Nothing"),value.tagsChange == 0, value.SetValues, new Pathfinding.TagMask (0,value.tagsSet));
for (int i=0;i<32;i++) {
bool on = (value.tagsChange >> i & 0x1) != 0;
Pathfinding.TagMask result = new Pathfinding.TagMask (on ? value.tagsChange & ~(1 << i) : value.tagsChange | 1<<i,value.tagsSet);
menu.AddItem (new GUIContent (""+i),on,value.SetValues, result);
}
menu.ShowAsContext ();
Event.current.Use ();
}
#if UNITY_LE_4_3
EditorGUIUtility.LookLikeInspector();
#endif
GUILayout.EndHorizontal ();
GUILayout.BeginHorizontal ();
EditorGUIUtility.LookLikeControls();
EditorGUILayout.PrefixLabel (setLabel,EditorStyles.layerMaskField);
text = "";
if (value.tagsSet == 0) text = "Nothing";
else if (value.tagsSet == ~0) text = "Everything";
else {
text = System.Convert.ToString (value.tagsSet,2);
}
if (GUILayout.Button (text,EditorStyles.layerMaskField,GUILayout.ExpandWidth (true))) {
GenericMenu menu = new GenericMenu ();
if (value.tagsChange != 0) menu.AddItem (new GUIContent ("Everything"),value.tagsSet == ~0, value.SetValues, new Pathfinding.TagMask (value.tagsChange,~0));
else menu.AddDisabledItem (new GUIContent ("Everything"));
menu.AddItem (new GUIContent ("Nothing"),value.tagsSet == 0, value.SetValues, new Pathfinding.TagMask (value.tagsChange,0));
for (int i=0;i<32;i++) {
bool enabled = (value.tagsChange >> i & 0x1) != 0;
bool on = (value.tagsSet >> i & 0x1) != 0;
Pathfinding.TagMask result = new Pathfinding.TagMask (value.tagsChange, on ? value.tagsSet & ~(1 << i) : value.tagsSet | 1<<i);
if (enabled) menu.AddItem (new GUIContent (""+i),on,value.SetValues, result);
else menu.AddDisabledItem (new GUIContent (""+i));
}
menu.ShowAsContext ();
Event.current.Use ();
}
#if UNITY_LE_4_3
EditorGUIUtility.LookLikeInspector();
#endif
GUILayout.EndHorizontal ();
}
protected override void Initialize()
{
this.keyboard = new KeyboardManager();
this.grid = new Grid(ROW, COLUMN);
matrizNodes = new Node[ROW, COLUMN];
General.ROW = ROW;
General.COLUMN = COLUMN;
this.lRaffle = new List<Node>();
this.lPathfinding = new List<Node>();
this.pathfinding = new General();
base.Initialize();
}
