/// <summary>
/// Calculate the part of the path the car is running
/// Returns -1 case is out of th epath
/// </summary>
/// <param name="c"></param>
/// <returns></returns>
public float PartOfPath(Car c)
{
bool isBetweenTheLine = Calc.DoesPointLineNormalizedIntersectionIsBetweenTheLine(c.Position, P1, P2);
if (!isBetweenTheLine)
{
return(-1.00f);
}
//
PointF intersectionPoint = Calc.PointLineNormalizedIntersection(c.Position, P1, P2);
float dist = Calc.Magnitude(c.Position, intersectionPoint);
if (dist > ErrorMargin)
{
return(-1.00f);
}
//
return(Calc.Magnitude(P1, c.Position));
}
public PointF GetNearestPathPoint(PointF pos)
{
bool isBetweenLineSegment = Calc.DoesPointLineNormalizedIntersectionIsBetweenTheLine(pos, P1, P2);
if (isBetweenLineSegment)
{
return(Calc.PointLineNormalizedIntersection(pos, P1, P2));
}
else
{
float distCP1 = Calc.Modulus(Calc.Magnitude(pos, P1));
float distCP2 = Calc.Modulus(Calc.Magnitude(pos, P2));
if (distCP1 < distCP2)
{
return(P1);
}
else
{
return(P2);
}
}
}
/// <summary>
/// Calculate intersection with another path
/// </summary>
/// <param name="p"></param>
/// <returns></returns>
public IntersectionData IntersectsWith(Path p)
{
// Get the path lines
Line l1 = this.GetLine();
Line l2 = p.GetLine();
// Calc Intersections
IntersectionData id = new IntersectionData();
id.Intersects = Calc.DoLinesIntersect(l1, l2);
id.Parallel = Calc.AreTwoLinesParallel(l1, l2);
if (id.Intersects)
{
id.IntersectionPoint = Calc.IntersectionBetweenTwoLines(l1, l2);
if (!Calc.DoesPointLineNormalizedIntersectionIsBetweenTheLine(id.IntersectionPoint, l1.P1, l1.P2) ||
!Calc.DoesPointLineNormalizedIntersectionIsBetweenTheLine(id.IntersectionPoint, l2.P1, l2.P2))
{
/////////////////// TODO <-----------------------------------------------
id.Intersects = false;
}
}
return(id);
}