private SimWaypoint CreateXYZ(int x, int y)
{
int ix = IndexX(x);
int iy = IndexY(y);
if (saved[ix, iy] == null)
{
saved[ix, iy] = SimWaypointImpl.CreateGlobal(x, y, SimZ);
}
SimWaypoint wp = saved[ix, iy];
AddNode(wp);
return(wp);
}
/// <summary>
///
/// </summary>
/// <param name="v3"></param>
/// <returns></returns>
public SimWaypoint CreateClosestWaypoint(Vector3d v3)
{
double Dist;
SimWaypoint Closest = ClosestNode(v3.X, v3.Y, v3.Z, out Dist, false);
SimWaypoint V3Waypoint = SimWaypointImpl.CreateGlobal(v3);
if (Closest != V3Waypoint)
{
IList <SimWaypoint> more = ClosestNodes(V3Waypoint, Dist, Dist * 2, false);
AddNode(V3Waypoint);
Intern2Arc(Closest, V3Waypoint, 1f);
foreach (SimWaypoint P in more)
{
Intern2Arc(P, V3Waypoint, 1f);
}
}
return(V3Waypoint);
}
public virtual SimWaypoint GetPointAt(double p, SimPathStore PathStore)
{
double len = Length;
if (p <= 0.0f)
{
return(StartNode);
}
if (p >= len)
{
return(EndNode);
}
Vector3d dir = EndNode.GlobalPosition - StartNode.GlobalPosition;
double X = (dir.X / len) * p;
double Y = (dir.Y / len) * p;
double Z = (dir.Z / len) * p;
return(SimWaypointImpl.CreateGlobal(new Vector3d(X, Y, Z)));
}
/// <summary>
///
/// </summary>
/// <param name="v3"></param>
/// <param name="radius"></param>
/// <param name="numPoints"></param>
/// <param name="Weight"></param>
/// <returns></returns>
public SimWaypoint CreateClosestWaypointBox(Vector3d v3, double radius, int numPoints, double Weight)
{
SimWaypoint node = SimWaypointImpl.CreateGlobal(v3);
double radiansStep = Math.PI * 2 / numPoints;
SimWaypoint Last = node;
Dictionary <SimWaypoint, List <SimWaypoint> > newWaypoints = new Dictionary <SimWaypoint, List <SimWaypoint> >();
for (int Step = 0; Step < numPoints; Step++)
{
double ThisAngle = Step * radiansStep;
Vector3d vectNew = (new Vector3d((double)Math.Cos(ThisAngle), (double)Math.Sin(ThisAngle), 0) * radius) + v3;
SimWaypoint nodeNew = SimWaypointImpl.CreateGlobal(v3);
List <SimWaypoint> closeNodes = new List <SimWaypoint>();
newWaypoints[nodeNew] = closeNodes;
double Dist;
closeNodes.Add(node);
closeNodes.Add(Last);
closeNodes.Add(ClosestNode(vectNew.X, vectNew.Y, vectNew.Z, out Dist, false));
Last = nodeNew;
}
foreach (SimWaypoint P in newWaypoints.Keys)
{
AddNode(P);
foreach (SimWaypoint V in newWaypoints[P])
{
if (V == null)
{
continue;
}
AddNode(V);
if (P != V)
{
Intern2Arc(P, V, Weight);
}
}
}
return(node);
}
/// <summary>
/// Creates a node, adds to the graph and returns its reference.
/// </summary>
/// <param name="x">X coordinate.</param>
/// <param name="y">Y coordinate.</param>
/// <param name="z">Z coordinate.</param>
/// <returns>The reference of the new node / null if the node is already in the graph.</returns>
public SimWaypoint AddNode(double x, double y, double z)
{
SimWaypoint NewNode = SimWaypointImpl.CreateGlobal(x, y, z);
return(AddNode(NewNode) ? NewNode : null);
}