public GeoPoint Predict(GeoPoint start, double distance)
{
GeoPoint projStart;
GeoPoint target = this.Points.Last();
int segIdx = 0;
int startType = ProjectFrom(start, out projStart, out segIdx);
double currentDistance = 0;
while (segIdx < this.Points.Count - 1)
{
double length = GeoPoint.GetDistance(start, Points[segIdx + 1]);
if (currentDistance + length >= distance)
{
double leftLength = distance - currentDistance;
double ratio = leftLength / length;
double lat = start.Lat + ratio * (Points[segIdx + 1].Lat - start.Lat);
double lng = start.Lng + ratio * (Points[segIdx + 1].Lng - start.Lng);
target = new GeoPoint(lat, lng);
break;
}
else
{
currentDistance += length;
start = Points[segIdx + 1];
segIdx++;
}
}
return(target);
}
/// <summary>
/// Get the distance between the projections on the polyline
/// </summary>
/// <param name="from"></param>
/// <param name="to"></param>
/// <returns></returns>
public double DistOnLine(GeoPoint from, GeoPoint to)
{
GeoPoint fromProject, toProject;
int fromSegIdx, toSegIdx;
int fromType = ProjectFrom(from, out fromProject, out fromSegIdx);
int toType = ProjectFrom(to, out toProject, out toSegIdx);
double distance = 0;
//Debug.Assert(fromType == 0 && toType == 0);
if (fromSegIdx == toSegIdx)
{
distance = GeoPoint.GetDistance(fromProject, toProject);
}
else
{
distance = GeoPoint.GetDistance(fromProject, points[fromSegIdx + 1]);
for (int i = fromSegIdx + 1; i < toSegIdx; i++)
{
distance += GeoPoint.GetDistance(points[i], points[i + 1]);
}
distance += GeoPoint.GetDistance(points[toSegIdx], toProject);
}
//distance+=GeoPoint.GetDistance(fromProject,)
return(distance);
}
public List <GeoPoint> GetParkingPoints(int minInterval = 300)
{
List <GeoPoint> points = new List <GeoPoint>();
double dist = 0, distThreshold = 1;
int start = 0, current = 0;
long minSeconds = 300;
while (current < this.Count - 1)
{
dist += GeoPoint.GetDistance(this[current].point, this[current + 1].point);
if (dist > distThreshold)
{
if (this[current + 1].t - this[start].t >= minSeconds)
{
// Parking point
points.Add(this[start].point);
}
start = current + 1;
dist = 0;
}
++current;
}
if (this[current].t - this[start].t >= minSeconds)
{
points.Add(this[start].point);
}
return(points);
}
/// <summary>
/// Load trajectory from a file
/// </summary>
/// <summary>
/// Remove outliers. <br/>
/// We assume that the first mv is not outlier
/// </summary>
/// <returns></returns>
public Trajectory RemoveOutlier()
{
int trjSize = this.Count;
if (trjSize <= 1)
{
return(this);
}
MotionVector[] mvs = this.ToArray();
double maxSpeed = 60; //60m/s
this.Clear();
this.Add(mvs[0]);
for (int i = 1; i < trjSize; i++)
{
double distance = GeoPoint.GetDistance(mvs[i - 1].point, mvs[i].point);
double inteval = (mvs[i].Time - mvs[i - 1].Time).TotalSeconds;
double speed = distance / inteval;
if (speed <= maxSpeed)
{
this.Add(mvs[i]);
}
else
{
//Debug.Assert(false);
}
}
return(this);
}
public static double DistFrom(GeoPoint start, GeoPoint end, GeoPoint p)
{
GeoPoint result;
ProjectFrom(start, end, p, out result);
double distance = 0;
distance = GeoPoint.GetDistance(p, result);
return(distance);
}
/// <summary>
/// Distance from p to the end of the polyline(by this route)
/// </summary>
/// <param name="p"></param>
/// <param name="type"></param>
/// <returns></returns>
public double EndDistFrom(GeoPoint p, out int type)
{
GeoPoint result;
int segIdx;
type = ProjectFrom(p, out result, out segIdx);
double distance = GeoPoint.GetDistance(p, points[segIdx + 1]);
for (int i = segIdx + 1; i < points.Count - 1; i++)
{
distance += GeoPoint.GetDistance(points[i], points[i + 1]);
}
return(distance);
}
private double getLength(bool isPrecise = false)
{
double tmpLen = 0;
for (int i = 0; i < this.points.Count - 1; i++)
{
if (isPrecise)
{
tmpLen += GeoPoint.GetPreciseDistance(points[i], points[i + 1]);
}
else
{
tmpLen += GeoPoint.GetDistance(points[i], points[i + 1]);
}
}
return(tmpLen);
}
