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