public bool Equals(IPathSegment other)
{
if (other is BezierPathSegment)
{
BezierPathSegment bp = (BezierPathSegment)other;
// check that all the point match up
return(bp.cb.P0 == cb.P0 && bp.cb.P1 == cb.P1 && bp.cb.P2 == cb.P2 && bp.cb.P3 == cb.P3);
}
else
{
return(false);
}
}
private Boolean IsClear(BezierPathSegment seg, double minLength, List<Polygon> obstacles)
{
foreach (Polygon p in obstacles)
if (p.Intersect(seg))
{
if (seg.Length < minLength)
{
return false;
}
else
{
CubicBezier[] newBeziers = seg.cb.Subdivide(0.5);
BezierPathSegment newSegA = new BezierPathSegment(newBeziers[0], seg.EndSpeed, seg.StopLine);
BezierPathSegment newSegB = new BezierPathSegment(newBeziers[1], seg.EndSpeed, seg.StopLine);
return IsClear(newSegA, minLength, obstacles) && IsClear(newSegB, minLength, obstacles);
}
}
return true;
}
private Boolean IsBezSegmentClear(BezierPathSegment seg, List<Polygon> obstacles, double advanceDistance)
{
PointOnPath p = seg.StartPoint;
double refDist = 0;
while (refDist == 0)
{
PointOnPath p2;
refDist = advanceDistance;
p2 = seg.AdvancePoint(p, ref refDist);
foreach (Polygon poly in obstacles)
{
if (poly.ConvexDoesIntersect(p.pt, p2.pt)) return false;
}
p = p2;
}
return true;
}