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