├── mygif.gif
├── ResultPic.JPG
├── obj
└── Debug
│ ├── InformedRRTStar.csproj.CoreCompileInputs.cache
│ ├── InformedRRTStar.exe
│ ├── InformedRRTStar.pdb
│ ├── InformedRRTStar.csprojAssemblyReference.cache
│ ├── DesignTimeResolveAssemblyReferencesInput.cache
│ └── InformedRRTStar.csproj.FileListAbsolute.txt
├── bin
└── Debug
│ ├── InformedRRTStar.exe
│ ├── InformedRRTStar.pdb
│ └── InformedRRTStar.exe.config
├── App.config
├── Node.cs
├── Program.cs
├── README.md
├── Properties
└── AssemblyInfo.cs
├── InformedRRTStar.csproj
└── InformedRRTStar.cs
/mygif.gif:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/sahand995/Informed-RRT-Star/HEAD/mygif.gif
--------------------------------------------------------------------------------
/ResultPic.JPG:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/sahand995/Informed-RRT-Star/HEAD/ResultPic.JPG
--------------------------------------------------------------------------------
/obj/Debug/InformedRRTStar.csproj.CoreCompileInputs.cache:
--------------------------------------------------------------------------------
1 | 2ef986a354e25a860e0a9df52b01f0ce95cece82
2 |
--------------------------------------------------------------------------------
/bin/Debug/InformedRRTStar.exe:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/sahand995/Informed-RRT-Star/HEAD/bin/Debug/InformedRRTStar.exe
--------------------------------------------------------------------------------
/bin/Debug/InformedRRTStar.pdb:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/sahand995/Informed-RRT-Star/HEAD/bin/Debug/InformedRRTStar.pdb
--------------------------------------------------------------------------------
/obj/Debug/InformedRRTStar.exe:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/sahand995/Informed-RRT-Star/HEAD/obj/Debug/InformedRRTStar.exe
--------------------------------------------------------------------------------
/obj/Debug/InformedRRTStar.pdb:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/sahand995/Informed-RRT-Star/HEAD/obj/Debug/InformedRRTStar.pdb
--------------------------------------------------------------------------------
/obj/Debug/InformedRRTStar.csprojAssemblyReference.cache:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/sahand995/Informed-RRT-Star/HEAD/obj/Debug/InformedRRTStar.csprojAssemblyReference.cache
--------------------------------------------------------------------------------
/obj/Debug/DesignTimeResolveAssemblyReferencesInput.cache:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/sahand995/Informed-RRT-Star/HEAD/obj/Debug/DesignTimeResolveAssemblyReferencesInput.cache
--------------------------------------------------------------------------------
/App.config:
--------------------------------------------------------------------------------
1 |
2 |
3 |
4 |
5 |
6 |
7 |
--------------------------------------------------------------------------------
/bin/Debug/InformedRRTStar.exe.config:
--------------------------------------------------------------------------------
1 |
2 |
3 |
4 |
5 |
6 |
7 |
--------------------------------------------------------------------------------
/Node.cs:
--------------------------------------------------------------------------------
1 | using System;
2 | using System.Collections.Generic;
3 | using System.Linq;
4 | using System.Text;
5 | using System.Threading.Tasks;
6 |
7 | namespace InformedRRTStar
8 | {
9 | public class Node
10 | {
11 | public double x;
12 | public double y;
13 | public double cost;
14 | public int parent;
15 |
16 | public Node(double x, double y)
17 | {
18 | this.x = x;
19 | this.y = y;
20 | cost = 0.0;
21 | parent = -1;
22 | }
23 |
24 | public Node DeepCopy()
25 | {
26 | Node temp = (Node)this.MemberwiseClone();
27 | temp.x = x;
28 | temp.y = y;
29 | temp.cost = cost;
30 | temp.parent = parent;
31 | return temp;
32 | }
33 | }
34 | }
35 |
--------------------------------------------------------------------------------
/obj/Debug/InformedRRTStar.csproj.FileListAbsolute.txt:
--------------------------------------------------------------------------------
1 | C:\Users\Asus\Desktop\RRT\Informed_RRT_Star_CSharp\InformedRRTStar\InformedRRTStar\bin\Debug\InformedRRTStar.exe.config
2 | C:\Users\Asus\Desktop\RRT\Informed_RRT_Star_CSharp\InformedRRTStar\InformedRRTStar\bin\Debug\InformedRRTStar.exe
3 | C:\Users\Asus\Desktop\RRT\Informed_RRT_Star_CSharp\InformedRRTStar\InformedRRTStar\bin\Debug\InformedRRTStar.pdb
4 | C:\Users\Asus\Desktop\RRT\Informed_RRT_Star_CSharp\InformedRRTStar\InformedRRTStar\obj\Debug\InformedRRTStar.csprojAssemblyReference.cache
5 | C:\Users\Asus\Desktop\RRT\Informed_RRT_Star_CSharp\InformedRRTStar\InformedRRTStar\obj\Debug\InformedRRTStar.csproj.CoreCompileInputs.cache
6 | C:\Users\Asus\Desktop\RRT\Informed_RRT_Star_CSharp\InformedRRTStar\InformedRRTStar\obj\Debug\InformedRRTStar.exe
7 | C:\Users\Asus\Desktop\RRT\Informed_RRT_Star_CSharp\InformedRRTStar\InformedRRTStar\obj\Debug\InformedRRTStar.pdb
8 |
--------------------------------------------------------------------------------
/Program.cs:
--------------------------------------------------------------------------------
1 | using System;
2 | using System.Collections.Generic;
3 | using System.Linq;
4 | using System.Text;
5 | using System.Threading.Tasks;
6 |
7 | namespace InformedRRTStar
8 | {
9 | class Program
10 | {
11 | static void Main(string[] args)
12 | {
13 | double[,] obstacleList = new double[,]
14 | {
15 | { 5, 5, 0.5 },
16 | { 9, 6, 1 },
17 | { 7, 5, 1 },
18 | { 1, 5, 1 },
19 | { 3, 6, 1 },
20 | { 7, 9, 1 }
21 | };
22 |
23 |
24 | InformedRRTStar rrt = new InformedRRTStar(new double[] { 0, 0 }, new double[] { 5, 10 },
25 | obstacleList, new double[] { -2, 15 });
26 |
27 | List path = rrt.informed_rrt_star_search();
28 | //Console.WriteLine("Path: {0}", string.Join(", ", path));
29 |
30 | Console.Write("Path: ");
31 | for (int i = path.Count - 1; i >= 0; i--)
32 | {
33 | Console.Write($"({path[i].x}, {path[i].y}), ");
34 | }
35 | Console.WriteLine();
36 |
37 | Console.WriteLine("Distance between start to end: {0}", InformedRRTStar.get_path_len(path));
38 | Console.WriteLine("Done!!");
39 | Console.ReadKey();
40 |
41 | }
42 | }
43 | }
44 |
--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
1 | # Informed RRT*
2 | I have developed the Informed Rapidly-exploring Random Tree-Star (Informed-RRT*) algorithm with C# Programming.
3 |
4 | The method aims to achieve the shortest path between two given points by generating a tree from the [startpoint] to the [endpoint] without colliding with the obstacles.
5 |
6 | The informed version of the RRT* algorithm performs the same as the RRT* until a solution is found. However, this algorithm only generates new random nodes in an area where the previously found path may be improved.
7 |
8 |
9 | ### Assumption:
10 |
11 | * Iteration: 200
12 | * startpoint: (0, 0)
13 | * endpoint: (5, 10)
14 | * obstacles (x, y, radius): { { 5, 5, 0.5 },
15 | { 9, 6, 1 },
16 | { 7, 5, 1 },
17 | { 1, 5, 1 },
18 | { 3, 6, 1 },
19 | { 7, 9, 1 } }
20 |
21 |
22 |
23 | ### Result:
24 |
25 | * Position of all Nodes and their Parents
26 | * List of paths
27 | * Distance between [startpoint] to [endpoint]
28 |
29 |
30 | 
31 |
32 |
33 |
34 | 
35 |
36 |
37 |
38 |
39 |
40 |
--------------------------------------------------------------------------------
/Properties/AssemblyInfo.cs:
--------------------------------------------------------------------------------
1 | using System.Reflection;
2 | using System.Runtime.CompilerServices;
3 | using System.Runtime.InteropServices;
4 |
5 | // General Information about an assembly is controlled through the following
6 | // set of attributes. Change these attribute values to modify the information
7 | // associated with an assembly.
8 | [assembly: AssemblyTitle("InformedRRTStar")]
9 | [assembly: AssemblyDescription("")]
10 | [assembly: AssemblyConfiguration("")]
11 | [assembly: AssemblyCompany("Microsoft")]
12 | [assembly: AssemblyProduct("InformedRRTStar")]
13 | [assembly: AssemblyCopyright("Copyright © Microsoft 2020")]
14 | [assembly: AssemblyTrademark("")]
15 | [assembly: AssemblyCulture("")]
16 |
17 | // Setting ComVisible to false makes the types in this assembly not visible
18 | // to COM components. If you need to access a type in this assembly from
19 | // COM, set the ComVisible attribute to true on that type.
20 | [assembly: ComVisible(false)]
21 |
22 | // The following GUID is for the ID of the typelib if this project is exposed to COM
23 | [assembly: Guid("1bc22d5b-64b4-45b9-953a-be8b46642f3e")]
24 |
25 | // Version information for an assembly consists of the following four values:
26 | //
27 | // Major Version
28 | // Minor Version
29 | // Build Number
30 | // Revision
31 | //
32 | // You can specify all the values or you can default the Build and Revision Numbers
33 | // by using the '*' as shown below:
34 | // [assembly: AssemblyVersion("1.0.*")]
35 | [assembly: AssemblyVersion("1.0.0.0")]
36 | [assembly: AssemblyFileVersion("1.0.0.0")]
37 |
--------------------------------------------------------------------------------
/InformedRRTStar.csproj:
--------------------------------------------------------------------------------
1 |
2 |
3 |
4 |
5 | Debug
6 | AnyCPU
7 | {1BC22D5B-64B4-45B9-953A-BE8B46642F3E}
8 | Exe
9 | InformedRRTStar
10 | InformedRRTStar
11 | v4.6.1
12 | 512
13 | true
14 |
15 |
16 | AnyCPU
17 | true
18 | full
19 | false
20 | bin\Debug\
21 | DEBUG;TRACE
22 | prompt
23 | 4
24 |
25 |
26 | AnyCPU
27 | pdbonly
28 | true
29 | bin\Release\
30 | TRACE
31 | prompt
32 | 4
33 |
34 |
35 |
36 |
37 |
38 |
39 |
40 |
41 |
42 |
43 |
44 |
45 |
46 |
47 |
48 |
49 |
50 |
51 |
52 |
53 |
54 |
--------------------------------------------------------------------------------
/InformedRRTStar.cs:
--------------------------------------------------------------------------------
1 | using System;
2 | using System.Collections.Generic;
3 | using System.IO;
4 | using System.Linq;
5 | using System.Text;
6 | using System.Threading.Tasks;
7 |
8 | namespace InformedRRTStar
9 | {
10 | public class InformedRRTStar
11 | {
12 | public Node start;
13 | public Node goal;
14 | public double minRand;
15 | public double maxRand;
16 | public double expandDis;
17 | public int goalSampleRate;
18 | public int maxIter;
19 | public double[,] obstacleList;
20 | public List node_List;
21 | Random random = new Random();
22 |
23 | public InformedRRTStar(double[] start_pos, double[] goal_pos, double[,] obstacleList, double[] randArea,
24 | double expandDis = 0.5, int goalSampleRate = 10, int maxIter = 200)
25 | {
26 | start = new Node(start_pos[0], start_pos[1]);
27 | goal = new Node(goal_pos[0], goal_pos[1]);
28 | minRand = randArea[0];
29 | maxRand = randArea[1];
30 | this.expandDis = expandDis;
31 | this.goalSampleRate = goalSampleRate;
32 | this.maxIter = maxIter;
33 | this.obstacleList = obstacleList;
34 | node_List = new List();
35 | }
36 |
37 | public List informed_rrt_star_search()
38 | {
39 | node_List.Add(start);
40 | double cBest = double.PositiveInfinity;
41 | //var solutionSet = new HashSet();
42 | List path = new List();
43 | double cMin = Math.Sqrt(Math.Pow(start.x - goal.x, 2) + Math.Pow(start.y - goal.y, 2));
44 |
45 | double[,] xCenter = { { (start.x + goal.x) / 2.0 },
46 | { (start.y + goal.y) / 2.0 },
47 | { 0 } };
48 |
49 | double[,] a1 = { { (goal.x - start.x) / cMin },
50 | { (goal.y - start.y) / cMin },
51 | { 0 } };
52 |
53 | double etheta = Math.Atan2(a1[1, 0] , a1[0, 0]);
54 |
55 | double[,] id1_t = new double[1, 3] { {1, 0, 0} };
56 |
57 | double[,] M = new double[3, 3]; //np.dot(a1, idt_1)
58 | for (int i = 0; i < 3; i++)
59 | {
60 | for (int j = 0; j < 3; j++)
61 | {
62 | M[i, j] = 0;
63 | for (int k = 0; k < 1; k++)
64 | {
65 | M[i, j] += a1[i, k] * id1_t[k, j];
66 | }
67 | }
68 | }
69 |
70 | //U, S, Vh
71 | //C
72 |
73 | double[,] C = { { M[0,0], -1 * M[1,0], 0 },
74 | { M[1,0], M[0,0], 0 },
75 | { 0, 0, 1 } };
76 |
77 | for (int i = 0; i < maxIter; i++)
78 | {
79 | double[] rnd = informed_sample(cBest, cMin, xCenter, C);
80 | int nind = get_nearest_list_index(node_List, rnd);
81 | Node nearestNode = node_List[nind];
82 | double theta = Math.Atan2(rnd[1] - nearestNode.y, rnd[0] - nearestNode.x);
83 | Node newNode = get_new_node(theta, nind, nearestNode);
84 | double d = line_cost(nearestNode, newNode);
85 | bool noCollision = check_collision(nearestNode, theta, d);
86 |
87 | if (noCollision)
88 | {
89 | List nearInds = find_near_nodes(newNode);
90 | newNode = choose_parent(newNode, nearInds);
91 | node_List.Add(newNode);
92 | Console.WriteLine($"{node_List.Count - 1} - ({newNode.x}, {newNode.y}), {newNode.parent}");
93 | rewire(newNode, nearInds);
94 |
95 | if (is_near_goal(newNode))
96 | {
97 | if (check_segment_collision(newNode.x, newNode.y, goal.x, goal.y))
98 | {
99 | //solutionSet.Add(newNode);
100 | int lastIndex = node_List.Count - 1;
101 | List tempPath = get_final_course(lastIndex);
102 | double tempPathLen = get_path_len(tempPath);
103 |
104 | if (tempPathLen < cBest)
105 | {
106 | path = tempPath;
107 | cBest = tempPathLen;
108 | }
109 | }
110 | }
111 | }
112 | }
113 |
114 | return path;
115 | }
116 |
117 | #region [- choose_parent(Node newNode, List nearInds) -]
118 | public Node choose_parent(Node newNode, List nearInds)
119 | {
120 | if (nearInds.Count == 0)
121 | {
122 | return newNode;
123 | }
124 |
125 | List dList = new List();
126 |
127 | for (int i = 0; i < nearInds.Count; i++)
128 | {
129 | double dx = newNode.x - node_List[nearInds[i]].x;
130 | double dy = newNode.y - node_List[nearInds[i]].y;
131 | double d = Math.Sqrt(Math.Pow(dx, 2) + Math.Pow(dy, 2));
132 | double theta = Math.Atan2(dy, dx);
133 |
134 | if (check_collision(node_List[nearInds[i]], theta, d))
135 | {
136 | dList.Add(node_List[nearInds[i]].cost + d);
137 | }
138 | else
139 | {
140 | dList.Add(double.PositiveInfinity);
141 | }
142 | }
143 |
144 | double minCost = dList.Min();
145 | int minInd = nearInds[dList.IndexOf(minCost)];
146 |
147 | if (minCost == double.PositiveInfinity)
148 | {
149 | return newNode;
150 | }
151 |
152 | newNode.cost = minCost;
153 | newNode.parent = minInd;
154 |
155 | return newNode;
156 | }
157 | #endregion
158 |
159 | #region [- find_near_nodes(Node newNode) -]
160 | public List find_near_nodes(Node newNode)
161 | {
162 | int nnode = node_List.Count;
163 | double r = 50 * Math.Sqrt(Math.Log(nnode) / nnode);
164 |
165 | List dlist = new List();
166 | List nearinds = new List();
167 |
168 | foreach (var item in node_List)
169 | {
170 | dlist.Add(Math.Pow(item.x - newNode.x, 2) + Math.Pow(item.y - newNode.y, 2));
171 | }
172 |
173 | for (int i = 0; i < dlist.Count; i++)
174 | {
175 | if (dlist[i] <= Math.Pow(r, 2))
176 | {
177 | nearinds.Add(i);
178 | }
179 | }
180 |
181 | return nearinds;
182 | }
183 | #endregion
184 |
185 | #region [- informed_sample(double cMax, double cMin, double[,] xCenter, double[,] C) -]
186 | public double[] informed_sample(double cMax, double cMin, double[,] xCenter, double[,] C)
187 | {
188 | if (cMax < double.PositiveInfinity)
189 | {
190 | double[] r = {cMax / 2.0,
191 | Math.Sqrt(Math.Pow(cMax,2) - Math.Pow(cMin,2)) / 2.0,
192 | Math.Sqrt(Math.Pow(cMax,2) - Math.Pow(cMin,2)) / 2.0};
193 |
194 | double[,] L = new double[3, 3];
195 | for (int i = 0; i < 3; i++)
196 | {
197 | for (int j = 0; j < 3; j++)
198 | {
199 | if (i == 0 && j == 0)
200 | {
201 | L[i, j] = r[0];
202 | }
203 | else if (i == 1 && j == 1)
204 | {
205 | L[i, j] = r[1];
206 | }
207 | else if (i == 2 && j == 2)
208 | {
209 | L[i, j] = r[2];
210 | }
211 | else
212 | {
213 | L[i, j] = 0;
214 | }
215 | }
216 | }
217 |
218 | double[,] xBall = sample_unit_ball();
219 |
220 | double[,] multiply_C_L = new double[3, 3];
221 | for (int i = 0; i < 3; i++)
222 | {
223 | for (int j = 0; j < 3; j++)
224 | {
225 | multiply_C_L[i, j] = 0;
226 | for (int k = 0; k < 3; k++)
227 | {
228 | multiply_C_L[i, j] += C[i, k] * L[k, j];
229 | }
230 | }
231 | }
232 |
233 | double[,] multiply_multiply_C_L_xBall = new double[3, 1];
234 | for (int i = 0; i < 3; i++)
235 | {
236 | for (int j = 0; j < 1; j++)
237 | {
238 | multiply_multiply_C_L_xBall[i, j] = 0;
239 | for (int k = 0; k < 3; k++)
240 | {
241 | multiply_multiply_C_L_xBall[i, j] += multiply_C_L[i, k] * xBall[k, j];
242 | }
243 | }
244 | }
245 |
246 | double[,] rnd = new double[3, 1];
247 | for (int i = 0; i < 3; i++)
248 | {
249 | for (int j = 0; j < 1; j++)
250 | {
251 | rnd[i, j] = multiply_multiply_C_L_xBall[i, j] + xCenter[i, j];
252 | }
253 | }
254 |
255 | return new double[2] { rnd[0, 0], rnd[1, 0] };
256 | }
257 |
258 | else
259 | {
260 | return sample_free_space();
261 | }
262 | }
263 | #endregion
264 |
265 | #region [- sample_unit_ball() -]
266 | public static double[,] sample_unit_ball()
267 | {
268 | Random random = new Random();
269 |
270 | double a = random.NextDouble();
271 | double b = random.NextDouble();
272 |
273 | if (b < a)
274 | {
275 | double temp = a;
276 | a = b;
277 | b = temp;
278 | }
279 |
280 | double[] sample = { b * Math.Cos(2 * Math.PI * (a / b)), b * Math.Sin(2 * Math.PI * (a / b)) };
281 |
282 | return new double[3, 1] { { sample[0] },
283 | { sample[1] },
284 | { 0 } };
285 | }
286 | #endregion
287 |
288 | #region [- sample_free_space() -]
289 | public double[] sample_free_space()
290 | {
291 | double[] rnd;
292 |
293 | if (random.Next(0, 100) > goalSampleRate)
294 | {
295 | rnd = new double[2] { random.NextDouble() * (maxRand - minRand) + minRand,
296 | random.NextDouble() * (maxRand - minRand) + minRand };
297 | }
298 | else
299 | {
300 | rnd = new double[2] { goal.x, goal.y };
301 | }
302 |
303 | return rnd;
304 | }
305 | #endregion
306 |
307 | #region [- get_path_len(List path) -]
308 | public static double get_path_len(List path)
309 | {
310 | double pathLen = 0;
311 | for (int i = 1; i < path.Count; i++)
312 | {
313 | double node1_x = path[i].x;
314 | double node1_y = path[i].y;
315 | double node2_x = path[i - 1].x;
316 | double node2_y = path[i - 1].y;
317 |
318 | pathLen += Math.Sqrt(Math.Pow(node1_x - node2_x, 2) + Math.Pow(node1_y - node2_y, 2));
319 | }
320 |
321 | return pathLen;
322 | }
323 | #endregion
324 |
325 | #region [- line_cost(Node node1, Node node2) -]
326 | public static double line_cost(Node node1, Node node2)
327 | {
328 | return Math.Sqrt(Math.Pow(node1.x - node2.x, 2) + Math.Pow(node1.y - node2.y, 2));
329 | }
330 | #endregion
331 |
332 | #region [- get_nearest_list_index(Node[] nodes, double[] rnd) -]
333 | public static int get_nearest_list_index(List nodes, double[] rnd)
334 | {
335 | List dList = new List();
336 |
337 | foreach (var item in nodes)
338 | {
339 | dList.Add(Math.Pow(item.x - rnd[0], 2) + Math.Pow(item.y - rnd[1], 2));
340 | }
341 |
342 | int minIndex = dList.IndexOf(dList.Min());
343 | return minIndex;
344 | }
345 | #endregion
346 |
347 | #region [- get_new_node(double theta, int nind, Node nearestnode) -]
348 | public Node get_new_node(double theta, int nind, Node nearestnode)
349 | {
350 | Node newNode = nearestnode.DeepCopy();
351 | newNode.x += expandDis * Math.Cos(theta);
352 | newNode.y += expandDis * Math.Sin(theta);
353 | newNode.cost += expandDis;
354 | newNode.parent = nind;
355 | return newNode;
356 | }
357 | #endregion
358 |
359 | #region [- is_near_goal(Node node) -]
360 | public bool is_near_goal(Node node)
361 | {
362 | double d = line_cost(node, goal);
363 |
364 | if (d < expandDis)
365 | {
366 | return true;
367 | }
368 |
369 | return false;
370 | }
371 | #endregion
372 |
373 | #region [- rewire(Node newNode, List nearInds) -]
374 | public void rewire(Node newNode, List nearInds)
375 | {
376 | int n_node = node_List.Count;
377 |
378 | for (int i = 0; i < nearInds.Count; i++)
379 | {
380 | Node nearNode = node_List[nearInds[i]];
381 |
382 | double d = Math.Sqrt(Math.Pow(nearNode.x - newNode.x, 2) + Math.Pow(nearNode.y - newNode.y, 2));
383 | double scost = newNode.cost + d;
384 |
385 | if (nearNode.cost > scost)
386 | {
387 | double theta = Math.Atan2(newNode.y - nearNode.y, newNode.x - nearNode.x);
388 |
389 | if (check_collision(nearNode, theta, d))
390 | {
391 | nearNode.parent = n_node - 1;
392 | nearNode.cost = scost;
393 | }
394 | }
395 | }
396 | }
397 | #endregion
398 |
399 | #region [- distance_squared_point_to_segment(double[] v, double[] w, double[] p) -]
400 | public static double distance_squared_point_to_segment(double[] v, double[] w, double[] p)
401 | {
402 |
403 | double[] p_v = new double[2] { p[0] - v[0], p[1] - v[1] }; //p-v
404 |
405 | if (v[0] == w[0] && v[1] == w[1])
406 | {
407 | return Math.Pow(p_v[0], 2) + Math.Pow(p_v[1], 2); //(p-v).dot(p-v)
408 | }
409 | else
410 | {
411 | double[] w_v = new double[2] { w[0] - v[0], w[1] - v[1] }; //w-v
412 | double l2 = Math.Pow(w_v[0], 2) + Math.Pow(w_v[1], 2); //(w-v).dot(w-v)
413 |
414 | double temp = ((p_v[0] * w_v[0]) + (p_v[1] * w_v[1])) / l2; //(p-v).dot(w-v) / l2
415 |
416 | double t = Math.Max(0, Math.Min(1, temp)); // 0 < t < 1
417 |
418 | double[] projection = new double[2] { (w_v[0] * t) + v[0], (w_v[1] * t) + v[1] };
419 |
420 | double[] p_projection = new double[2] { p[0] - projection[0], p[1] - projection[1] }; //p_projection
421 |
422 | return Math.Pow(p_projection[0], 2) + Math.Pow(p_projection[1], 2);
423 | }
424 | }
425 | #endregion
426 |
427 | #region [- check_segment_collision(double x1, double y1, double x2, double y2) -]
428 | public bool check_segment_collision(double x1, double y1, double x2, double y2)
429 | {
430 | for (int i = 0; i < obstacleList.Length / 3; i++)
431 | {
432 | double dd = distance_squared_point_to_segment(new double[] { x1, y1 }, new double[] { x2, y2 },
433 | new double[] { obstacleList[i, 0], obstacleList[i, 1] });
434 |
435 | if (dd <= Math.Pow(obstacleList[i, 2], 2))
436 | {
437 | return false;
438 | }
439 | }
440 |
441 | return true;
442 | }
443 | #endregion
444 |
445 | #region [- check_collision(Node nearNode, double theta, double d) -]
446 | public bool check_collision(Node nearNode, double theta, double d)
447 | {
448 | Node tmpNode = nearNode.DeepCopy();
449 | double endx = tmpNode.x + Math.Cos(theta) * d;
450 | double endy = tmpNode.y + Math.Sin(theta) * d;
451 |
452 | return check_segment_collision(tmpNode.x, tmpNode.y, endx, endy);
453 | }
454 | #endregion
455 |
456 | #region [- get_final_course(int lastIndex) -]
457 | public List get_final_course(int lastIndex)
458 | {
459 | List path = new List();
460 | path.Add(goal);
461 |
462 | while (node_List[lastIndex].parent != -1)
463 | {
464 | Node node = node_List[lastIndex];
465 | path.Add(node);
466 | lastIndex = node.parent;
467 | }
468 |
469 | path.Add(start);
470 | return path;
471 | }
472 | #endregion
473 |
474 |
475 |
476 | //public void draw_graph(double xCenter = 0,
477 | // double cBest = 0, double cMin = 0, double etheta = 0, double rnd = 0)
478 | //{
479 |
480 | //}
481 |
482 | //public static void plot_ellipse(double xCenter, double cBest, double cMin, double etheta)
483 | //{
484 |
485 | //}
486 |
487 | }
488 | }
489 |
--------------------------------------------------------------------------------