private bool LineCrosses(Location line0, Location line1, Location point)
{
float LineMag = line0.GetDistanceTo(line1); // Magnitude( LineEnd, LineStart );
float U =
(((point.X - line0.X) * (line1.X - line0.X)) +
((point.Y - line0.Y) * (line1.Y - line0.Y)) +
((point.Z - line0.Z) * (line1.Z - line0.Z))) /
(LineMag * LineMag);
if (U < 0.0f || U > 1.0f)
{
return(false);
}
float InterX = line0.X + U * (line1.X - line0.X);
float InterY = line0.Y + U * (line1.Y - line0.Y);
float InterZ = line0.Z + U * (line1.Z - line0.Z);
float Distance = point.GetDistanceTo(new Location(InterX, InterY, InterZ));
if (Distance < 0.5f)
{
return(true);
}
return(false);
}
public Path CreatePath(Location fromLoc, Location toLoc, float howClose, ILocationHeuristics locationHeuristics)
{
logger.WriteLine("Creating Path from " + fromLoc.ToString() + " tot " + toLoc.ToString());
var sw = new Stopwatch();
sw.Start();
fromLoc = GetBestLocations(fromLoc);
toLoc = GetBestLocations(toLoc);
Spot from = FindClosestSpot("fromLoc", fromLoc, MinStepLength);
Spot to = FindClosestSpot("toLoc", toLoc, MinStepLength);
if (from == null)
{
from = AddAndConnectSpot(new Spot(fromLoc));
}
if (to == null)
{
to = AddAndConnectSpot(new Spot(toLoc));
}
Path rawPath = CreatePath(from, to, howClose, locationHeuristics);
if (rawPath != null && paint != null)
{
Location prev = null;
for (int i = 0; i < rawPath.Count(); i++)
{
Location l = rawPath.Get(i);
paint.AddBigMarker(l.X, l.Y, l.Z);
if (prev != null)
{
paint.PaintPath(l.X, l.Y, l.Z, prev.X, prev.Y, prev.Z);
}
prev = l;
}
}
logger.Debug(string.Format("CreatePath took {0} seconds.", sw.ElapsedMilliseconds / 1000));
if (rawPath == null)
{
return(null);
}
else
{
Location last = rawPath.GetLast();
if (last.GetDistanceTo(toLoc) > 1.0)
{
rawPath.AddLast(toLoc);
}
}
LastPath = rawPath;
return(rawPath);
}
public Path CreatePath(Location fromLoc, Location toLoc,
float howClose,
ILocationHeuristics locationHeuristics)
{
Stopwatch t = Stopwatch.StartNew();
Spot from = FindClosestSpot(fromLoc, MinStepLength);
Spot to = FindClosestSpot(toLoc, MinStepLength);
if (from == null)
{
from = AddAndConnectSpot(new Spot(fromLoc));
}
if (to == null)
{
to = AddAndConnectSpot(new Spot(toLoc));
}
Path rawPath = CreatePath(from, to, to.location, howClose, true, locationHeuristics);
if (rawPath != null && paint != null)
{
Location prev = null;
for (int i = 0; i < rawPath.Count(); i++)
{
Location l = rawPath.Get(i);
paint.AddBigMarker(l.X, l.Y, l.Z);
if (prev != null)
{
paint.PaintPath(l.X, l.Y, l.Z + 3, prev.X, prev.Y, prev.Z + 3);
}
prev = l;
}
}
t.Stop();
Log("CreatePath took " + t.Elapsed);
if (rawPath == null)
{
return(null);
}
else
{
Location last = rawPath.GetLast();
if (last.GetDistanceTo(toLoc) > 1.0)
{
rawPath.AddLast(toLoc);
}
}
return(rawPath);
}