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); }