//private void ReadTreeAndSaveList(RRTNode parentNode, KDTree tree)
//{
// try
// {
// tree.insert(RRTNode.ToKey(parentNode.state.Pose.x, parentNode.state.Pose.y), parentNode);
// }
// catch { }
// foreach (RRTNode n in parentNode.children)
// {
// ReadTreeAndSaveList(n, tree);
// }
//}
/// <summary>
/// Recursively go through every child in the tree, and record [Node, distance between the node and the vector point] into the passed dictionary
/// </summary>
/// <param name="parentNode">(RRTNode) parent node </param>
/// <param name="samplePoint">(Vector2) sampled point</param>
/// <param name="dictionary">(Dictionary[RRTNode, Double] to insert the child nodes</param>
private void ReadTreeAndCalculateDistance(RRTNode parentNode, Vector2 samplePoint, Dictionary <RRTNode, Double> dictionary)
{
dictionary.Add(parentNode, parentNode.DistanceTo(samplePoint));
foreach (RRTNode n in parentNode.children)
{
ReadTreeAndCalculateDistance(n, samplePoint, dictionary);
}
}
//private void ReadTreeAndSaveList(RRTNode parentNode, KDTree tree)
//{
// try
// {
// tree.insert(RRTNode.ToKey(parentNode.state.Pose.x, parentNode.state.Pose.y), parentNode);
// }
// catch { }
// foreach (RRTNode n in parentNode.children)
// {
// ReadTreeAndSaveList(n, tree);
// }
//}
/// <summary>
/// Recursively go through every child in the tree, and record [Node, distance between the node and the vector point] into the passed dictionary
/// </summary>
/// <param name="parentNode">(RRTNode) parent node </param>
/// <param name="samplePoint">(Vector2) sampled point</param>
/// <param name="dictionary">(Dictionary[RRTNode, Double] to insert the child nodes</param>
private void ReadTreeAndCalculateDistance(RRTNode parentNode, Vector2 samplePoint, Dictionary<RRTNode, Double> dictionary)
{
dictionary.Add(parentNode, parentNode.DistanceTo(samplePoint));
foreach (RRTNode n in parentNode.children)
{
ReadTreeAndCalculateDistance(n, samplePoint, dictionary);
}
}
