public bool GapInPath(IPath path, out Vector2 ipt, out LineSegment gap)
{
foreach (IPathSegment ps in path)
{
foreach (LineSegment ls in ThreateningGaps)
{
LineSegment s = new LineSegment(ps.Start, ps.End);
if (ls.Intersect(s, out ipt))
{
gap = ls;
return true;
}
}
}
ipt = default(Vector2);
gap = default(LineSegment);
return false;
}
public void UpdatePath(IPath path, LineSegment gap)
{
if (path == null || path.Count == 0 || pose == null)
return;
double dist = 1.5;
PointOnPath lookAhead = path.AdvancePoint(path.StartPoint, ref dist);
Vector2 mean = new Vector2((gap.P0.X + gap.P1.X) / 2, (gap.P0.Y + gap.P1.Y) / 2);
double theta = Math.Atan2(gap.P0.Y - gap.P1.Y, gap.P0.X - gap.P1.X) + Math.PI / 2;
Vector2 goal1 = mean + 0.25 * (new Vector2(Math.Cos(theta), Math.Sin(theta)));
Vector2 goal2 = mean - 0.25 * (new Vector2(Math.Cos(theta), Math.Sin(theta)));
Vector2 goalPointGlobal = ((goal1.X - lookAhead.pt.X) * (goal1.X - lookAhead.pt.X) + (goal1.Y - lookAhead.pt.Y) * (goal1.Y - lookAhead.pt.Y)
< (goal2.X - lookAhead.pt.X) * (goal2.X - lookAhead.pt.X) + (goal2.Y - lookAhead.pt.Y) * (goal2.Y - lookAhead.pt.Y)) ? goal1 : goal2;
double xg = (goalPointGlobal.X - pose.x) * Math.Cos(pose.yaw) + (goalPointGlobal.Y - pose.y) * Math.Sin(pose.yaw);
double yg = -(goalPointGlobal.X - pose.x) * Math.Sin(pose.yaw) + (goalPointGlobal.Y - pose.y) * Math.Cos(pose.yaw);
goalPoint = new Vector2(xg, yg);
}
/// <summary>
/// Finds gaps between polygon objects indiscriminately.
/// </summary>
/// <param name="polys">List of polygons to find gaps between</param>
/// <returns>List of LineSegment objects representing gaps found</returns>
private List<LineSegment> FindAllGaps(List<Polygon> polys, RobotPose pose)
{
List<LineSegment> gaps = new List<LineSegment>();
for (int i = 0; i < polys.Count; i++)
{
Polygon poly1 = polys[i];
for (int j = i + 1; j < polys.Count; j++)
{
Polygon poly2 = polys[j];
LineSegment gap = poly1.ShortestLineToOther(poly2);
if (gap.Length <= ignoreabovethis)
{
Vector2 startGlobal = new Vector2(Math.Cos(pose.yaw) * gap.P0.X - Math.Sin(pose.yaw) * gap.P0.Y + pose.x,
Math.Sin(pose.yaw) * gap.P0.X + Math.Cos(pose.yaw) * gap.P0.Y + pose.y);
Vector2 endGlobal = new Vector2(Math.Cos(pose.yaw) * gap.P1.X - Math.Sin(pose.yaw) * gap.P1.Y + pose.x,
Math.Sin(pose.yaw) * gap.P1.X + Math.Cos(pose.yaw) * gap.P1.Y + pose.y);
LineSegment gapGlobal = new LineSegment(startGlobal, endGlobal);
gaps.Add(gapGlobal);
}
}
}
return gaps;
}
public void UpdatePath(IPath path, LineSegment gap)
{
if (path == null || path.Count == 0) return;
/*double dist = 0.45;
PointOnPath lookAhead = path.AdvancePoint(path.StartPoint, ref dist);
Vector2 goalpointGlobal = lookAhead.pt;
double xg = (goalpointGlobal.X - currentPoint.x) * Math.Cos(currentPoint.yaw) + (goalpointGlobal.Y - currentPoint.y) * Math.Sin(currentPoint.yaw);
double yg = -(goalpointGlobal.X - currentPoint.x) * Math.Sin(currentPoint.yaw) + (goalpointGlobal.Y - currentPoint.y) * Math.Cos(currentPoint.yaw);
goalpoint = new Vector2(xg, yg);*/
double dist = 1.5;
PointOnPath lookAhead = path.AdvancePoint(path.StartPoint, ref dist);
Vector2 mean = new Vector2((gap.P0.X + gap.P1.X)/2, (gap.P0.Y + gap.P1.Y)/2);
double theta = Math.Atan2(gap.P0.Y - gap.P1.Y, gap.P0.X - gap.P1.X);
Vector2 goal1 = mean + 0.25 * (new Vector2(Math.Cos(theta + Math.PI / 2), Math.Sin(theta + Math.PI / 2)));
Vector2 goal2 = mean - 0.25 * (new Vector2(Math.Cos(theta + Math.PI / 2), Math.Sin(theta + Math.PI / 2)));
Vector2 goalpointGlobal = (goal1.DistanceTo(lookAhead.pt) < goal2.DistanceTo(lookAhead.pt)) ? goal1 : goal2;
double xg = (goalpointGlobal.X - currentPoint.x) * Math.Cos(currentPoint.yaw) + (goalpointGlobal.Y - currentPoint.y) * Math.Sin(currentPoint.yaw);
double yg = -(goalpointGlobal.X - currentPoint.x) * Math.Sin(currentPoint.yaw) + (goalpointGlobal.Y - currentPoint.y) * Math.Cos(currentPoint.yaw);
goalpoint = new Vector2(xg, yg);
/*double dist = 1.5;
PointOnPath lookAhead = path.AdvancePoint(path.StartPoint, ref dist);
double goal1Y = (gap.P0.Y + gap.P1.Y) / 2 + (gap.P0.X - gap.P1.X) / (gap.Length) * 0.25;
double goal1X = (gap.P0.X + gap.P1.X) / 2 + (gap.P0.Y - gap.P1.Y) / (gap.Length) * 0.25;
Vector2 goal1 = new Vector2(goal1X, goal1Y);
double goal2Y = (gap.P0.Y + gap.P1.Y) / 2 + (gap.P0.X - gap.P1.X) / (gap.Length) * -0.25;
double goal2X = (gap.P0.X + gap.P1.X) / 2 + (gap.P0.Y - gap.P1.Y) / (gap.Length) * -0.25;
Vector2 goal2 = new Vector2(goal2X, goal2Y);
goalpoint = (goal1.DistanceTo(lookAhead.pt) < goal2.DistanceTo(lookAhead.pt)) ? goal1 : goal2;*/
}
/// <summary>
/// Check obstacle avoidance by intersecting the line segment between two nodes
/// </summary>
private bool CheckObstacles(SimpleTreeNode<Vector2> nodeOne, SimpleTreeNode<Vector2> nodeTwo)
{
bool isSafe = true;
LineSegment ls = new LineSegment(nodeOne.Value, nodeTwo.Value);
foreach (Polygon poly in bloatedObstacles)
{
if (poly.DoesIntersect(ls))
{
isSafe = false;
break;
}
}
return isSafe;
}
public bool Intersect(LineSegment l, out Vector2 pts, out Vector2 K)
{
return(l.Intersect(this, out pts, out K));
}
private bool DoSegmentsCollide(IPathSegment seg1, IPathSegment seg2, out double collideDist1, out double collideDist2)
{
bool doesIntersect = false;
Vector2 intersectPoint;
Vector2[] p = new Vector2[4];
collideDist1 = Double.MaxValue;
collideDist2 = Double.MaxValue;
LineSegment ls1 = new LineSegment(seg1.Start, seg1.End);
LineSegment ls2 = new LineSegment(seg2.Start, seg2.End);
doesIntersect = ls1.Intersect(ls2, out intersectPoint);
p[0] = ls1.ClosestPoint(seg2.Start);
p[1] = ls1.ClosestPoint(seg2.End);
p[2] = ls2.ClosestPoint(seg1.Start);
p[3] = ls2.ClosestPoint(seg1.End);
if (doesIntersect)
{
collideDist1 = seg1.Start.DistanceTo(intersectPoint);
collideDist2 = seg2.Start.DistanceTo(intersectPoint);
}
if ((p[0] - seg2.Start).Length < collideThreshold)
{
if (collideDist1 > seg1.Start.DistanceTo(p[0]))
{
collideDist1 = seg1.Start.DistanceTo(p[0]);
collideDist2 = 0;
doesIntersect = true;
}
}
if ((p[1] - seg2.End).Length < collideThreshold)
{
if (collideDist1 > seg1.Start.DistanceTo(p[1]))
{
collideDist1 = seg1.Start.DistanceTo(p[1]);
collideDist2 = seg2.Length;
doesIntersect = true;
}
}
if ((p[2] - seg1.Start).Length < collideThreshold)
{
collideDist1 = 0;
collideDist2 = seg2.Start.DistanceTo(p[2]);
}
if ((p[3] - seg1.End).Length < collideThreshold)
{
if (collideDist1 > seg1.Length)
{
collideDist1 = seg1.Length;
collideDist2 = seg2.Start.DistanceTo(p[3]);
}
}
return doesIntersect;
}
public bool Intersect(LineSegment l, out Vector2 pt)
{
Vector2 K;
return Intersect(l, out pt, out K);
}
public bool Equals(LineSegment other)
{
return P0.Equals(other.P0) && P1.Equals(other.P1);
}
public bool Intersect(LineSegment l, out Vector2 pt, out Vector2 K)
{
Vector2 P = P1 - P0;
Vector2 S = l.P1 - l.P0;
Matrix2 A = new Matrix2(P.X, -S.X, P.Y, -S.Y);
if (Math.Abs(A.Determinant()) < 1e-10) {
pt = default(Vector2);
K = default(Vector2);
return false;
}
K = A.Inverse()*(l.P0 - P0);
if (K.X >= 0 && K.X <= 1 && K.Y >= 0 && K.Y <= 1) {
pt = P0 + P*K.X;
return true;
}
else {
pt = default(Vector2);
return false;
}
}
/// <summary>
///
/// </summary>
/// <param name="goalPoint"></param>
/// <param name="obstacles"></param>
/// <param name="goal"></param>
/// <param name="foundPath"></param>
/// <param name="samplePoint"></param>
/// <param name="closestNode"></param>
/// <returns>true if the node was added to the tree (i.e. didnt hit crap)</returns>
private RRTNode ExtendNode(ref Vector2 goalPoint, List<Polygon> obstacles, ref RRTNode goal, ref bool foundPath, ref Vector2 samplePoint, RRTNode closestNode, Random rand)
{
//3) generate a control input that drives towards the sample point also biased with our initial control inputs
//3a) -Biasing Details:
// Select Velocity: Normal Distribution with mean = closest node velocity and sigma = SigmaVelocity
// Select Turn Rate:
// Apply the following heuristic: mean = (atan2(yf-yi,xf-xi) - thetaInit)/(delT)
// sigma = SigmaTurnRate
// velocity distribution
MathNet.Numerics.Distributions.NormalDistribution vDist = new MathNet.Numerics.Distributions.NormalDistribution(closestNode.State.Command.velocity, vSigma);
// turn-rate biased
double mixingSample = rand.NextDouble();
double wMean = 0;
if (mixingSample > mixingProportion)
{
double angleToClosestNode = Math.Atan2((samplePoint.Y - closestNode.Point.Y), (samplePoint.X - closestNode.Point.X));
//wMean = -kPwSample * angleToClosestNode;
wMean = ((angleToClosestNode - closestNode.State.Pose.yaw)) * 180.0 / Math.PI / timeStep;
if (wMean > MAX_TURN - 20)
wMean = MAX_TURN - 20;
if (wMean < MIN_TURN + 20)
wMean = MIN_TURN + 20;
}
else
wMean = 0;
MathNet.Numerics.Distributions.NormalDistribution wDist = new MathNet.Numerics.Distributions.NormalDistribution(wMean, wSigma);
double velSampled = vDist.NextDouble();
double wSampled = wDist.NextDouble();
while (velSampled > MAX_VEL || velSampled < MIN_VEL)
velSampled = vDist.NextDouble();
while (wSampled > MAX_TURN || wSampled < MIN_TURN)
wSampled = wDist.NextDouble();
// 4) Predict a node
RRTNode predictedNode = CalculateNextNode(closestNode, velSampled, wSampled, obstacles);
if (predictedNode != null)
{
closestNode.AddChild(predictedNode);
//5) Check if the new node added is within some tolerance of the goal node. If so, mark node as goal and you're done! Else, Goto 1.
//Polygon goalPolygon = Polygon.VehiclePolygonWithRadius(0.5, goalPoint);
Circle c = new Circle(.5, goalPoint);
LineSegment nodeToParent = new LineSegment(predictedNode.Point, predictedNode.Parent.Point);
Vector2[] pts = new Vector2[2];
if (c.Intersect(nodeToParent, out pts))
{
foundPath = true;
goal = predictedNode;
}
//if (predictedNode.DistanceTo(goalPoint) < goalTolerance)
//{
// foundPath = true;
// goal = predictedNode;
//}
return predictedNode;
}
return null;
}
private Boolean IsClear(Vector2 start, Vector2 end, List<Polygon> obstacles)
{
Vector2[] temp;
LineSegment line = new LineSegment(start, end);
foreach (Polygon p in obstacles)
{
if (p.Intersect(line, out temp))
return false;
}
return true;
}
