File: | home/HaikuArchives/ArtPaint/artpaint/application/IntelligentPathFinder.cpp |
Warning: | line 283, column 31 The result of the left shift is undefined because the left operand is negative |
Press '?' to see keyboard shortcuts
Keyboard shortcuts:
1 | /* | ||||||||||||||||
2 | * Copyright 2003, Heikki Suhonen | ||||||||||||||||
3 | * Distributed under the terms of the MIT License. | ||||||||||||||||
4 | * | ||||||||||||||||
5 | * Authors: | ||||||||||||||||
6 | * Heikki Suhonen <heikki.suhonen@gmail.com> | ||||||||||||||||
7 | * | ||||||||||||||||
8 | */ | ||||||||||||||||
9 | #include <Bitmap.h> | ||||||||||||||||
10 | #include <stdio.h> | ||||||||||||||||
11 | #include <stdlib.h> | ||||||||||||||||
12 | #include <StopWatch.h> | ||||||||||||||||
13 | |||||||||||||||||
14 | |||||||||||||||||
15 | #include "IntelligentPathFinder.h" | ||||||||||||||||
16 | |||||||||||||||||
17 | |||||||||||||||||
18 | IntelligentPathFinder::IntelligentPathFinder(BBitmap *bm) | ||||||||||||||||
19 | { | ||||||||||||||||
20 | original_bitmap = bm; | ||||||||||||||||
21 | bitmap_bits = (uint32*)bm->Bits(); | ||||||||||||||||
22 | bitmap_bpr = bm->BytesPerRow()/4; | ||||||||||||||||
23 | right_border = (int32)bm->Bounds().right + 1; | ||||||||||||||||
24 | bottom_border = (int32)bm->Bounds().bottom + 1; | ||||||||||||||||
25 | |||||||||||||||||
26 | local_cost_map = new uint8*[right_border]; | ||||||||||||||||
27 | for (int32 i=0;i<right_border;i++) { | ||||||||||||||||
28 | local_cost_map[i] = new uint8[bottom_border]; | ||||||||||||||||
29 | } | ||||||||||||||||
30 | for (int32 y=0;y<bottom_border;y++) { | ||||||||||||||||
31 | for (int32 x=0;x<right_border;x++) { | ||||||||||||||||
32 | local_cost_map[x][y] = 0x00; | ||||||||||||||||
33 | } | ||||||||||||||||
34 | } | ||||||||||||||||
35 | |||||||||||||||||
36 | total_cost_map = new uint16*[right_border]; | ||||||||||||||||
37 | path_pointers_map = new uint32*[right_border]; | ||||||||||||||||
38 | for (int32 i=0;i<right_border;i++) { | ||||||||||||||||
39 | total_cost_map[i] = new uint16[bottom_border]; | ||||||||||||||||
40 | path_pointers_map[i] = new uint32[bottom_border]; | ||||||||||||||||
41 | for (int32 y=0;y<bottom_border;y++) { | ||||||||||||||||
42 | total_cost_map[i][y] = 0x0000; | ||||||||||||||||
43 | path_pointers_map[i][y]= 0x00000000; | ||||||||||||||||
44 | } | ||||||||||||||||
45 | } | ||||||||||||||||
46 | |||||||||||||||||
47 | expanded_bits = new uint8*[(right_border>>3)+1]; | ||||||||||||||||
48 | for (int32 i=0;i<(right_border>>3)+1;i++) { | ||||||||||||||||
49 | #pragma GCC diagnostic push | ||||||||||||||||
50 | #pragma GCC diagnostic ignored "-Walloc-size-larger-than=" | ||||||||||||||||
51 | expanded_bits[i] = new uint8[bottom_border]; | ||||||||||||||||
52 | #pragma GCC diagnostic pop | ||||||||||||||||
53 | for (int32 y=0;y<bottom_border;y++) { | ||||||||||||||||
54 | expanded_bits[i][y] = 0x00; | ||||||||||||||||
55 | } | ||||||||||||||||
56 | } | ||||||||||||||||
57 | |||||||||||||||||
58 | calculation_continuing = TRUE1; | ||||||||||||||||
59 | seed_point_x = -1; | ||||||||||||||||
60 | seed_point_y = -1; | ||||||||||||||||
61 | |||||||||||||||||
62 | dp_thread = spawn_thread(dp_thread_entry,"dp_thread",B_NORMAL_PRIORITY10,this); | ||||||||||||||||
63 | resume_thread(dp_thread); | ||||||||||||||||
64 | |||||||||||||||||
65 | } | ||||||||||||||||
66 | |||||||||||||||||
67 | |||||||||||||||||
68 | IntelligentPathFinder::~IntelligentPathFinder() | ||||||||||||||||
69 | { | ||||||||||||||||
70 | calculation_continuing = FALSE0; | ||||||||||||||||
71 | int32 return_value; | ||||||||||||||||
72 | wait_for_thread(dp_thread,&return_value); | ||||||||||||||||
73 | |||||||||||||||||
74 | for (int32 x=0;x<right_border;x++) { | ||||||||||||||||
75 | delete[] local_cost_map[x]; | ||||||||||||||||
76 | local_cost_map[x] = NULL__null; | ||||||||||||||||
77 | } | ||||||||||||||||
78 | for (int32 i=0;i<right_border;i++) { | ||||||||||||||||
79 | delete[] total_cost_map[i]; | ||||||||||||||||
80 | total_cost_map[i] = NULL__null; | ||||||||||||||||
81 | |||||||||||||||||
82 | delete[] path_pointers_map[i]; | ||||||||||||||||
83 | path_pointers_map[i] = NULL__null; | ||||||||||||||||
84 | } | ||||||||||||||||
85 | delete[] local_cost_map; | ||||||||||||||||
86 | delete[] total_cost_map; | ||||||||||||||||
87 | delete[] path_pointers_map; | ||||||||||||||||
88 | |||||||||||||||||
89 | for (int32 i=0;i<(right_border>>3)+1;i++) { | ||||||||||||||||
90 | delete[] expanded_bits[i]; | ||||||||||||||||
91 | expanded_bits[i] = NULL__null; | ||||||||||||||||
92 | } | ||||||||||||||||
93 | delete[] expanded_bits; | ||||||||||||||||
94 | |||||||||||||||||
95 | } | ||||||||||||||||
96 | |||||||||||||||||
97 | |||||||||||||||||
98 | void IntelligentPathFinder::SetSeedPoint(int32 x,int32 y) | ||||||||||||||||
99 | { | ||||||||||||||||
100 | seed_point_x = x; | ||||||||||||||||
101 | seed_point_y = y; | ||||||||||||||||
102 | seed_point_changed = TRUE1; | ||||||||||||||||
103 | } | ||||||||||||||||
104 | |||||||||||||||||
105 | BPoint* IntelligentPathFinder::ReturnPath(int32 x, int32 y,int32 *num_points) | ||||||||||||||||
106 | { | ||||||||||||||||
107 | x = max_c(0,min_c(right_border-1,x))((0)>(((right_border-1)>(x)?(x):(right_border-1)))?(0): (((right_border-1)>(x)?(x):(right_border-1)))); | ||||||||||||||||
108 | y = max_c(0,min_c(bottom_border-1,y))((0)>(((bottom_border-1)>(y)?(y):(bottom_border-1)))?(0 ):(((bottom_border-1)>(y)?(y):(bottom_border-1)))); | ||||||||||||||||
109 | int32 point_array_length = 128; | ||||||||||||||||
110 | BPoint *point_array = new BPoint[point_array_length]; | ||||||||||||||||
111 | |||||||||||||||||
112 | int32 next_x = x; | ||||||||||||||||
113 | int32 next_y = y; | ||||||||||||||||
114 | int32 point_count = 0; | ||||||||||||||||
115 | |||||||||||||||||
116 | while (((next_x != seed_point_x) || (next_y != seed_point_y)) && ((next_x >= 0) && (next_y >= 0))) { | ||||||||||||||||
117 | if (point_count == point_array_length) { | ||||||||||||||||
118 | point_array_length *= 2; | ||||||||||||||||
119 | BPoint *new_array = new BPoint[point_array_length]; | ||||||||||||||||
120 | for (int32 i=0;i<point_count;i++) { | ||||||||||||||||
121 | new_array[i] = point_array[i]; | ||||||||||||||||
122 | } | ||||||||||||||||
123 | delete[] point_array; | ||||||||||||||||
124 | point_array = new_array; | ||||||||||||||||
125 | } | ||||||||||||||||
126 | BPoint point = ReturnNextPointInPath(next_x,next_y); | ||||||||||||||||
127 | next_x = (int32)point.x; | ||||||||||||||||
128 | next_y = (int32)point.y; | ||||||||||||||||
129 | point_array[point_count++] = point; | ||||||||||||||||
130 | } | ||||||||||||||||
131 | |||||||||||||||||
132 | *num_points = point_count; | ||||||||||||||||
133 | |||||||||||||||||
134 | if (point_count > 1) | ||||||||||||||||
135 | return point_array; | ||||||||||||||||
136 | else { | ||||||||||||||||
137 | delete[] point_array; | ||||||||||||||||
138 | return NULL__null; | ||||||||||||||||
139 | } | ||||||||||||||||
140 | } | ||||||||||||||||
141 | |||||||||||||||||
142 | |||||||||||||||||
143 | uint8 IntelligentPathFinder::LocalCost(int32 x, int32 y, int32 dx, int32 dy) | ||||||||||||||||
144 | { | ||||||||||||||||
145 | if (local_cost_map[x][y] != 0x00) { | ||||||||||||||||
146 | if (abs(dx)+abs(dy)>1) | ||||||||||||||||
147 | return local_cost_map[x][y]; | ||||||||||||||||
148 | else | ||||||||||||||||
149 | return (uint8)(local_cost_map[x][y]/sqrt(2.0)); | ||||||||||||||||
150 | } | ||||||||||||||||
151 | else { | ||||||||||||||||
152 | // Here we must calculate the local cost between x,y and all of its neighbours. | ||||||||||||||||
153 | union { | ||||||||||||||||
154 | uint8 bytes[4]; | ||||||||||||||||
155 | uint32 word; | ||||||||||||||||
156 | } lt,t,rt,l,r,lb,b,rb,c; | ||||||||||||||||
157 | int32 new_x,new_y; | ||||||||||||||||
158 | new_x = max_c(min_c(x-1,right_border-1),0)((((x-1)>(right_border-1)?(right_border-1):(x-1)))>(0)? (((x-1)>(right_border-1)?(right_border-1):(x-1))):(0)); | ||||||||||||||||
159 | new_y = max_c(min_c(y-1,bottom_border-1),0)((((y-1)>(bottom_border-1)?(bottom_border-1):(y-1)))>(0 )?(((y-1)>(bottom_border-1)?(bottom_border-1):(y-1))):(0)); | ||||||||||||||||
160 | lt.word = *(bitmap_bits + new_x + new_y*bitmap_bpr); | ||||||||||||||||
161 | |||||||||||||||||
162 | new_x = max_c(min_c(x,right_border-1),0)((((x)>(right_border-1)?(right_border-1):(x)))>(0)?(((x )>(right_border-1)?(right_border-1):(x))):(0)); | ||||||||||||||||
163 | new_y = max_c(min_c(y-1,bottom_border-1),0)((((y-1)>(bottom_border-1)?(bottom_border-1):(y-1)))>(0 )?(((y-1)>(bottom_border-1)?(bottom_border-1):(y-1))):(0)); | ||||||||||||||||
164 | t.word = *(bitmap_bits + new_x + new_y*bitmap_bpr); | ||||||||||||||||
165 | |||||||||||||||||
166 | new_x = max_c(min_c(x+1,right_border-1),0)((((x+1)>(right_border-1)?(right_border-1):(x+1)))>(0)? (((x+1)>(right_border-1)?(right_border-1):(x+1))):(0)); | ||||||||||||||||
167 | new_y = max_c(min_c(y-1,bottom_border-1),0)((((y-1)>(bottom_border-1)?(bottom_border-1):(y-1)))>(0 )?(((y-1)>(bottom_border-1)?(bottom_border-1):(y-1))):(0)); | ||||||||||||||||
168 | rt.word = *(bitmap_bits + new_x + new_y*bitmap_bpr); | ||||||||||||||||
169 | |||||||||||||||||
170 | new_x = max_c(min_c(x-1,right_border-1),0)((((x-1)>(right_border-1)?(right_border-1):(x-1)))>(0)? (((x-1)>(right_border-1)?(right_border-1):(x-1))):(0)); | ||||||||||||||||
171 | new_y = max_c(min_c(y,bottom_border-1),0)((((y)>(bottom_border-1)?(bottom_border-1):(y)))>(0)?(( (y)>(bottom_border-1)?(bottom_border-1):(y))):(0)); | ||||||||||||||||
172 | l.word = *(bitmap_bits + new_x + new_y*bitmap_bpr); | ||||||||||||||||
173 | |||||||||||||||||
174 | new_x = max_c(min_c(x+1,right_border-1),0)((((x+1)>(right_border-1)?(right_border-1):(x+1)))>(0)? (((x+1)>(right_border-1)?(right_border-1):(x+1))):(0)); | ||||||||||||||||
175 | new_y = max_c(min_c(y,bottom_border-1),0)((((y)>(bottom_border-1)?(bottom_border-1):(y)))>(0)?(( (y)>(bottom_border-1)?(bottom_border-1):(y))):(0)); | ||||||||||||||||
176 | r.word = *(bitmap_bits + new_x + new_y*bitmap_bpr); | ||||||||||||||||
177 | |||||||||||||||||
178 | new_x = max_c(min_c(x-1,right_border-1),0)((((x-1)>(right_border-1)?(right_border-1):(x-1)))>(0)? (((x-1)>(right_border-1)?(right_border-1):(x-1))):(0)); | ||||||||||||||||
179 | new_y = max_c(min_c(y+1,bottom_border-1),0)((((y+1)>(bottom_border-1)?(bottom_border-1):(y+1)))>(0 )?(((y+1)>(bottom_border-1)?(bottom_border-1):(y+1))):(0)); | ||||||||||||||||
180 | lb.word = *(bitmap_bits + new_x + new_y*bitmap_bpr); | ||||||||||||||||
181 | |||||||||||||||||
182 | new_x = max_c(min_c(x,right_border-1),0)((((x)>(right_border-1)?(right_border-1):(x)))>(0)?(((x )>(right_border-1)?(right_border-1):(x))):(0)); | ||||||||||||||||
183 | new_y = max_c(min_c(y+1,bottom_border-1),0)((((y+1)>(bottom_border-1)?(bottom_border-1):(y+1)))>(0 )?(((y+1)>(bottom_border-1)?(bottom_border-1):(y+1))):(0)); | ||||||||||||||||
184 | b.word = *(bitmap_bits + new_x + new_y*bitmap_bpr); | ||||||||||||||||
185 | |||||||||||||||||
186 | new_x = max_c(min_c(x+1,right_border-1),0)((((x+1)>(right_border-1)?(right_border-1):(x+1)))>(0)? (((x+1)>(right_border-1)?(right_border-1):(x+1))):(0)); | ||||||||||||||||
187 | new_y = max_c(min_c(y+1,bottom_border-1),0)((((y+1)>(bottom_border-1)?(bottom_border-1):(y+1)))>(0 )?(((y+1)>(bottom_border-1)?(bottom_border-1):(y+1))):(0)); | ||||||||||||||||
188 | rb.word = *(bitmap_bits + new_x + new_y*bitmap_bpr); | ||||||||||||||||
189 | |||||||||||||||||
190 | c.word = *(bitmap_bits + x + y*bitmap_bpr); | ||||||||||||||||
191 | |||||||||||||||||
192 | // Here we can calculate the costs. Costs are calculated | ||||||||||||||||
193 | // for each RGB(A)-element and then summed. | ||||||||||||||||
194 | // For now lets just use the inverse of gradient magnitude as the cost. | ||||||||||||||||
195 | int32 gradient_magnitude; | ||||||||||||||||
196 | int32 red_magn,green_magn,blue_magn; | ||||||||||||||||
197 | red_magn = abs(lb.bytes[2] - lt.bytes[2]) + | ||||||||||||||||
198 | abs(2*b.bytes[2] - 2*t.bytes[2]) + | ||||||||||||||||
199 | abs(rb.bytes[2] - rt.bytes[2]); | ||||||||||||||||
200 | |||||||||||||||||
201 | green_magn = abs(lb.bytes[1] - lt.bytes[1]) + | ||||||||||||||||
202 | abs(2*b.bytes[1] - 2*t.bytes[1]) + | ||||||||||||||||
203 | abs(rb.bytes[1] - rt.bytes[1]); | ||||||||||||||||
204 | |||||||||||||||||
205 | blue_magn = abs(lb.bytes[0] - lt.bytes[0]) + | ||||||||||||||||
206 | abs(2*b.bytes[0] - 2*t.bytes[0]) + | ||||||||||||||||
207 | abs(rb.bytes[0] - rt.bytes[0]); | ||||||||||||||||
208 | |||||||||||||||||
209 | gradient_magnitude = red_magn+blue_magn+green_magn; | ||||||||||||||||
210 | |||||||||||||||||
211 | red_magn = abs(rt.bytes[2] - lt.bytes[2]) + | ||||||||||||||||
212 | abs(2*r.bytes[2] - 2*l.bytes[2]) + | ||||||||||||||||
213 | abs(rb.bytes[2] - lb.bytes[2]); | ||||||||||||||||
214 | |||||||||||||||||
215 | green_magn = abs(rt.bytes[1] - lt.bytes[1]) + | ||||||||||||||||
216 | abs(2*r.bytes[1] - 2*l.bytes[1]) + | ||||||||||||||||
217 | abs(rb.bytes[1] - lb.bytes[1]); | ||||||||||||||||
218 | |||||||||||||||||
219 | blue_magn = abs(rt.bytes[0] - lt.bytes[0]) + | ||||||||||||||||
220 | abs(2*r.bytes[0] - 2*l.bytes[0]) + | ||||||||||||||||
221 | abs(rb.bytes[0] - lb.bytes[0]); | ||||||||||||||||
222 | |||||||||||||||||
223 | gradient_magnitude = max_c(gradient_magnitude,red_magn+blue_magn+green_magn)((gradient_magnitude)>(red_magn+blue_magn+green_magn)?(gradient_magnitude ):(red_magn+blue_magn+green_magn)); | ||||||||||||||||
224 | |||||||||||||||||
225 | uint8 red_zero,green_zero,blue_zero; | ||||||||||||||||
226 | |||||||||||||||||
227 | red_zero = (abs(8*c.bytes[2] - (lt.bytes[2] + t.bytes[2] | ||||||||||||||||
228 | + rt.bytes[2] + l.bytes[2] + r.bytes[2] | ||||||||||||||||
229 | + lb.bytes[2] + b.bytes[2] + rb.bytes[2])) < 2 ? 32 : 0); | ||||||||||||||||
230 | |||||||||||||||||
231 | green_zero = (abs(8*c.bytes[1] - (lt.bytes[1] + t.bytes[1] | ||||||||||||||||
232 | + rt.bytes[1] + l.bytes[1] + r.bytes[1] | ||||||||||||||||
233 | + lb.bytes[1] + b.bytes[1] + rb.bytes[1])) < 2 ? 32 : 0); | ||||||||||||||||
234 | |||||||||||||||||
235 | blue_zero = (abs(8*c.bytes[0] - (lt.bytes[0] + t.bytes[0] | ||||||||||||||||
236 | + rt.bytes[0] + l.bytes[0] + r.bytes[0] | ||||||||||||||||
237 | + lb.bytes[0] + b.bytes[0] + rb.bytes[0])) < 2 ? 32 : 0); | ||||||||||||||||
238 | |||||||||||||||||
239 | |||||||||||||||||
240 | gradient_magnitude = (int32)(32 - (gradient_magnitude / 3000.0)*32); | ||||||||||||||||
241 | uint8 value = red_zero + green_zero + blue_zero + gradient_magnitude; | ||||||||||||||||
242 | |||||||||||||||||
243 | local_cost_map[x][y] = value; | ||||||||||||||||
244 | |||||||||||||||||
245 | if (abs(dx)+abs(dy)>1) | ||||||||||||||||
246 | return value; | ||||||||||||||||
247 | else | ||||||||||||||||
248 | return (uint8)(value / sqrt(2.0)); | ||||||||||||||||
249 | } | ||||||||||||||||
250 | } | ||||||||||||||||
251 | |||||||||||||||||
252 | |||||||||||||||||
253 | uint16 IntelligentPathFinder::ReturnTotalCost(int32 x, int32 y) | ||||||||||||||||
254 | { | ||||||||||||||||
255 | return total_cost_map[x][y]; | ||||||||||||||||
256 | } | ||||||||||||||||
257 | |||||||||||||||||
258 | void IntelligentPathFinder::SetTotalCost(int32 x, int32 y, uint16 cost) | ||||||||||||||||
259 | { | ||||||||||||||||
260 | total_cost_map[x][y] = cost; | ||||||||||||||||
261 | } | ||||||||||||||||
262 | |||||||||||||||||
263 | BPoint IntelligentPathFinder::ReturnNextPointInPath(int32 x, int32 y) | ||||||||||||||||
264 | { | ||||||||||||||||
265 | if (ReturnTotalCost(x,y) > 0) { | ||||||||||||||||
266 | int32 value = path_pointers_map[x][y]; | ||||||||||||||||
267 | BPoint point; | ||||||||||||||||
268 | point.x = (value >> 16) & 0xFFFF; | ||||||||||||||||
269 | point.y = value & 0xFFFF; | ||||||||||||||||
270 | |||||||||||||||||
271 | return point; | ||||||||||||||||
272 | } | ||||||||||||||||
273 | else { | ||||||||||||||||
274 | BPoint point(-1,-1); | ||||||||||||||||
275 | return point; | ||||||||||||||||
276 | } | ||||||||||||||||
277 | |||||||||||||||||
278 | } | ||||||||||||||||
279 | |||||||||||||||||
280 | |||||||||||||||||
281 | void IntelligentPathFinder::SetNextPointInPath(int32 x, int32 y, int32 nx, int32 ny) | ||||||||||||||||
282 | { | ||||||||||||||||
283 | path_pointers_map[x][y] = (nx<<16) | ny; | ||||||||||||||||
| |||||||||||||||||
284 | } | ||||||||||||||||
285 | |||||||||||||||||
286 | void IntelligentPathFinder::ResetTotalCostsAndPaths() | ||||||||||||||||
287 | { | ||||||||||||||||
288 | for (int32 i=0;i<right_border;i++) { | ||||||||||||||||
289 | for (int32 y=0;y<bottom_border;y++) { | ||||||||||||||||
290 | total_cost_map[i][y] = 0x0000; | ||||||||||||||||
291 | path_pointers_map[i][y]= 0x00000000; | ||||||||||||||||
292 | } | ||||||||||||||||
293 | } | ||||||||||||||||
294 | } | ||||||||||||||||
295 | |||||||||||||||||
296 | |||||||||||||||||
297 | bool IntelligentPathFinder::IsExpanded(int32 x, int32 y) | ||||||||||||||||
298 | { | ||||||||||||||||
299 | return (expanded_bits[x>>3][y] >> ((7 - x) & 0x7)) & 0x01; | ||||||||||||||||
300 | } | ||||||||||||||||
301 | |||||||||||||||||
302 | void IntelligentPathFinder::SetExpanded(int32 x, int32 y) | ||||||||||||||||
303 | { | ||||||||||||||||
304 | expanded_bits[x>>3][y] |= 0x01 << ((7 - x) & 0x7); | ||||||||||||||||
305 | } | ||||||||||||||||
306 | |||||||||||||||||
307 | void IntelligentPathFinder::ResetExpanded() | ||||||||||||||||
308 | { | ||||||||||||||||
309 | for (int32 i=0;i<(right_border>>3)+1;i++) { | ||||||||||||||||
310 | for (int32 y=0;y<bottom_border;y++) { | ||||||||||||||||
311 | expanded_bits[i][y] = 0x00; | ||||||||||||||||
312 | } | ||||||||||||||||
313 | } | ||||||||||||||||
314 | } | ||||||||||||||||
315 | |||||||||||||||||
316 | |||||||||||||||||
317 | |||||||||||||||||
318 | int32 IntelligentPathFinder::dp_thread_entry(void *data) | ||||||||||||||||
319 | { | ||||||||||||||||
320 | IntelligentPathFinder *this_pointer = (IntelligentPathFinder*)data; | ||||||||||||||||
321 | return this_pointer->dp_thread_function(); | ||||||||||||||||
| |||||||||||||||||
322 | } | ||||||||||||||||
323 | |||||||||||||||||
324 | |||||||||||||||||
325 | int32 IntelligentPathFinder::dp_thread_function() | ||||||||||||||||
326 | { | ||||||||||||||||
327 | // Wait until the seed-point is set. | ||||||||||||||||
328 | while ((seed_point_x == -1) && (seed_point_y == -1) && calculation_continuing) { | ||||||||||||||||
329 | snooze(50 * 1000); | ||||||||||||||||
330 | } | ||||||||||||||||
331 | seed_point_changed = FALSE0; | ||||||||||||||||
332 | |||||||||||||||||
333 | while (calculation_continuing) { | ||||||||||||||||
334 | active_point_list = new OrderedPointList(); | ||||||||||||||||
335 | active_point_list->InsertPoint(seed_point_x,seed_point_y,0); | ||||||||||||||||
336 | SetTotalCost(seed_point_x,seed_point_y,0); | ||||||||||||||||
337 | while (!active_point_list->IsEmpty() && !seed_point_changed
| ||||||||||||||||
338 | int32 x,y; | ||||||||||||||||
339 | uint16 cost; | ||||||||||||||||
340 | active_point_list->RemoveLowestCostPoint(&x,&y,&cost); | ||||||||||||||||
341 | SetExpanded(x,y); | ||||||||||||||||
342 | |||||||||||||||||
343 | for (int32 dy=-1;dy<=1;dy++) { | ||||||||||||||||
344 | for (int32 dx=-1;dx<=1;dx++) { | ||||||||||||||||
345 | if ((dx
| ||||||||||||||||
346 | if ((x+dx >= 0) && (x+dx<right_border) && | ||||||||||||||||
347 | (y+dy >= 0) && (y+dy<bottom_border)) { | ||||||||||||||||
348 | if (IsExpanded(x+dx,y+dy) == FALSE0) { | ||||||||||||||||
349 | uint16 temp_cost = ReturnTotalCost(x,y) + LocalCost(x,y,dx,dy); | ||||||||||||||||
350 | uint16 previous_cost = ReturnTotalCost(x+dx,y+dy); | ||||||||||||||||
351 | if ((previous_cost > 0) && (temp_cost < previous_cost)) | ||||||||||||||||
352 | active_point_list->RemovePoint(x+dx,y+dy,previous_cost); | ||||||||||||||||
353 | if ((previous_cost
| ||||||||||||||||
354 | SetTotalCost(x+dx,y+dy,temp_cost); | ||||||||||||||||
355 | SetNextPointInPath(x+dx,y+dy,x,y); | ||||||||||||||||
356 | active_point_list->InsertPoint(x+dx,y+dy,temp_cost); | ||||||||||||||||
357 | } | ||||||||||||||||
358 | } | ||||||||||||||||
359 | } | ||||||||||||||||
360 | } | ||||||||||||||||
361 | } | ||||||||||||||||
362 | } | ||||||||||||||||
363 | } | ||||||||||||||||
364 | while (!seed_point_changed && calculation_continuing) | ||||||||||||||||
365 | snooze(100 * 1000); // Calculated all costs. | ||||||||||||||||
366 | |||||||||||||||||
367 | if (calculation_continuing) { | ||||||||||||||||
368 | delete active_point_list; | ||||||||||||||||
369 | ResetExpanded(); | ||||||||||||||||
370 | ResetTotalCostsAndPaths(); | ||||||||||||||||
371 | seed_point_changed = FALSE0; | ||||||||||||||||
372 | } | ||||||||||||||||
373 | |||||||||||||||||
374 | } | ||||||||||||||||
375 | |||||||||||||||||
376 | return B_OK((int)0); | ||||||||||||||||
377 | } | ||||||||||||||||
378 | |||||||||||||||||
379 | |||||||||||||||||
380 | int32 IntelligentPathFinder::lc_thread_entry(void *data) | ||||||||||||||||
381 | { | ||||||||||||||||
382 | return B_OK((int)0); | ||||||||||||||||
383 | } | ||||||||||||||||
384 | |||||||||||||||||
385 | |||||||||||||||||
386 | int32 IntelligentPathFinder::lc_thread_function() | ||||||||||||||||
387 | { | ||||||||||||||||
388 | return B_OK((int)0); | ||||||||||||||||
389 | } | ||||||||||||||||
390 | |||||||||||||||||
391 | |||||||||||||||||
392 | void IntelligentPathFinder::PrintCostMap() | ||||||||||||||||
393 | { | ||||||||||||||||
394 | for (int32 y=0;y<bottom_border;y++) { | ||||||||||||||||
395 | for (int32 x=0;x<right_border;x++) { | ||||||||||||||||
396 | printf("%d ",ReturnTotalCost(x,y)); | ||||||||||||||||
397 | } | ||||||||||||||||
398 | printf("\n"); | ||||||||||||||||
399 | } | ||||||||||||||||
400 | } | ||||||||||||||||
401 | |||||||||||||||||
402 | // ------------------------- | ||||||||||||||||
403 | |||||||||||||||||
404 | |||||||||||||||||
405 | OrderedPointList::OrderedPointList() | ||||||||||||||||
406 | { | ||||||||||||||||
407 | point_list_head = NULL__null; | ||||||||||||||||
408 | cost_limits = new point*[(int32)pow(2,16)]; | ||||||||||||||||
409 | for (int32 i=0;i<pow(2,16);i++) | ||||||||||||||||
410 | cost_limits[i] = NULL__null; | ||||||||||||||||
411 | |||||||||||||||||
412 | highest_cost = 0; | ||||||||||||||||
413 | } | ||||||||||||||||
414 | |||||||||||||||||
415 | |||||||||||||||||
416 | OrderedPointList::~OrderedPointList() | ||||||||||||||||
417 | { | ||||||||||||||||
418 | point *spare = point_list_head; | ||||||||||||||||
419 | while (spare != NULL__null) { | ||||||||||||||||
420 | point *helper = spare->next_point; | ||||||||||||||||
421 | delete spare; | ||||||||||||||||
422 | spare = helper; | ||||||||||||||||
423 | } | ||||||||||||||||
424 | |||||||||||||||||
425 | for (int32 i=0;i<pow(2,16);i++) { | ||||||||||||||||
426 | cost_limits[i] = NULL__null; | ||||||||||||||||
427 | } | ||||||||||||||||
428 | delete[] cost_limits; | ||||||||||||||||
429 | } | ||||||||||||||||
430 | |||||||||||||||||
431 | |||||||||||||||||
432 | void OrderedPointList::RemoveLowestCostPoint(int32 *x, int32 *y, uint16 *cost) | ||||||||||||||||
433 | { | ||||||||||||||||
434 | if (point_list_head
| ||||||||||||||||
435 | point *spare = point_list_head; | ||||||||||||||||
436 | point_list_head = spare->next_point; | ||||||||||||||||
437 | *x = spare->x; | ||||||||||||||||
438 | *y = spare->y; | ||||||||||||||||
439 | *cost = spare->cost; | ||||||||||||||||
440 | if (point_list_head != NULL__null) { | ||||||||||||||||
441 | point_list_head->prev_point = NULL__null; | ||||||||||||||||
442 | if (point_list_head->cost == *cost) { | ||||||||||||||||
443 | cost_limits[*cost] = point_list_head; | ||||||||||||||||
444 | } | ||||||||||||||||
445 | else { | ||||||||||||||||
446 | cost_limits[*cost] = NULL__null; | ||||||||||||||||
447 | } | ||||||||||||||||
448 | } | ||||||||||||||||
449 | else { | ||||||||||||||||
450 | highest_cost = 0; | ||||||||||||||||
451 | cost_limits[*cost] = NULL__null; | ||||||||||||||||
452 | } | ||||||||||||||||
453 | delete spare; | ||||||||||||||||
454 | } | ||||||||||||||||
455 | else { | ||||||||||||||||
456 | *x = -1; | ||||||||||||||||
457 | *y = -1; | ||||||||||||||||
458 | *cost = 0; | ||||||||||||||||
459 | } | ||||||||||||||||
460 | } | ||||||||||||||||
461 | |||||||||||||||||
462 | void OrderedPointList::RemovePoint(int32 x, int32 y, uint16 cost) | ||||||||||||||||
463 | { | ||||||||||||||||
464 | point *spare = cost_limits[cost]; | ||||||||||||||||
465 | while ((spare != NULL__null) && ((spare->x != x) && (spare->y != y) && (spare->cost == cost))) { | ||||||||||||||||
466 | spare = spare->next_point; | ||||||||||||||||
467 | } | ||||||||||||||||
468 | if ((spare != NULL__null) && (spare->cost != cost)) | ||||||||||||||||
469 | spare = NULL__null; | ||||||||||||||||
470 | |||||||||||||||||
471 | if (spare != NULL__null) { | ||||||||||||||||
472 | if (spare->prev_point != NULL__null) { | ||||||||||||||||
473 | spare->prev_point->next_point = spare->next_point; | ||||||||||||||||
474 | } | ||||||||||||||||
475 | if (spare->next_point != NULL__null) { | ||||||||||||||||
476 | spare->next_point->prev_point = spare->prev_point; | ||||||||||||||||
477 | } | ||||||||||||||||
478 | if (spare == cost_limits[cost]) { | ||||||||||||||||
479 | if ((spare->next_point != NULL__null) && (spare->next_point->cost == cost)) | ||||||||||||||||
480 | cost_limits[cost] = spare->next_point; | ||||||||||||||||
481 | else { | ||||||||||||||||
482 | cost_limits[cost] = NULL__null; | ||||||||||||||||
483 | if (highest_cost == cost) { | ||||||||||||||||
484 | while ((highest_cost > 0) && (cost_limits[highest_cost] == NULL__null)) | ||||||||||||||||
485 | highest_cost--; | ||||||||||||||||
486 | } | ||||||||||||||||
487 | } | ||||||||||||||||
488 | } | ||||||||||||||||
489 | if (point_list_head == spare) { | ||||||||||||||||
490 | point_list_head = spare->next_point; | ||||||||||||||||
491 | } | ||||||||||||||||
492 | delete spare; | ||||||||||||||||
493 | } | ||||||||||||||||
494 | } | ||||||||||||||||
495 | |||||||||||||||||
496 | |||||||||||||||||
497 | void OrderedPointList::InsertPoint(int32 x, int32 y,uint16 cost) | ||||||||||||||||
498 | { | ||||||||||||||||
499 | point *spare = new point; | ||||||||||||||||
500 | spare->x = x; | ||||||||||||||||
501 | spare->y = y; | ||||||||||||||||
502 | spare->cost = cost; | ||||||||||||||||
503 | spare->next_point = NULL__null; | ||||||||||||||||
504 | spare->prev_point = NULL__null; | ||||||||||||||||
505 | |||||||||||||||||
506 | if (cost_limits[cost] != NULL__null) { | ||||||||||||||||
507 | spare->next_point = cost_limits[cost]; | ||||||||||||||||
508 | spare->prev_point = cost_limits[cost]->prev_point; | ||||||||||||||||
509 | if (spare->next_point != NULL__null) | ||||||||||||||||
510 | spare->next_point->prev_point = spare; | ||||||||||||||||
511 | if (spare->prev_point != NULL__null) | ||||||||||||||||
512 | spare->prev_point->next_point = spare; | ||||||||||||||||
513 | cost_limits[cost] = spare; | ||||||||||||||||
514 | } | ||||||||||||||||
515 | else { | ||||||||||||||||
516 | if (cost < highest_cost) { | ||||||||||||||||
517 | int32 next_cost = cost+1; | ||||||||||||||||
518 | while (cost_limits[next_cost] == NULL__null) | ||||||||||||||||
519 | next_cost++; | ||||||||||||||||
520 | |||||||||||||||||
521 | |||||||||||||||||
522 | spare->next_point = cost_limits[next_cost]; | ||||||||||||||||
523 | spare->prev_point = cost_limits[next_cost]->prev_point; | ||||||||||||||||
524 | if (spare->next_point != NULL__null) | ||||||||||||||||
525 | spare->next_point->prev_point = spare; | ||||||||||||||||
526 | if (spare->prev_point != NULL__null) | ||||||||||||||||
527 | spare->prev_point->next_point = spare; | ||||||||||||||||
528 | cost_limits[cost] = spare; | ||||||||||||||||
529 | } | ||||||||||||||||
530 | else { | ||||||||||||||||
531 | int32 prev_cost = cost - 1; | ||||||||||||||||
532 | while ((prev_cost >= 0) && (cost_limits[prev_cost] == NULL__null)) | ||||||||||||||||
533 | prev_cost--; | ||||||||||||||||
534 | |||||||||||||||||
535 | if (prev_cost >= 0) { | ||||||||||||||||
536 | point *helper = cost_limits[prev_cost]; | ||||||||||||||||
537 | |||||||||||||||||
538 | if (helper != NULL__null) { | ||||||||||||||||
539 | while ((helper->next_point != NULL__null) && (helper->cost < cost)) | ||||||||||||||||
540 | helper = helper->next_point; | ||||||||||||||||
541 | |||||||||||||||||
542 | spare->next_point = helper; | ||||||||||||||||
543 | spare->prev_point = helper->prev_point; | ||||||||||||||||
544 | if (spare->next_point != NULL__null) | ||||||||||||||||
545 | spare->next_point->prev_point = spare; | ||||||||||||||||
546 | if (spare->prev_point != NULL__null) | ||||||||||||||||
547 | spare->prev_point->next_point = spare; | ||||||||||||||||
548 | cost_limits[cost] = spare; | ||||||||||||||||
549 | } | ||||||||||||||||
550 | else { | ||||||||||||||||
551 | cost_limits[cost] = spare; | ||||||||||||||||
552 | } | ||||||||||||||||
553 | } | ||||||||||||||||
554 | else { | ||||||||||||||||
555 | cost_limits[cost] = spare; | ||||||||||||||||
556 | } | ||||||||||||||||
557 | } | ||||||||||||||||
558 | } | ||||||||||||||||
559 | |||||||||||||||||
560 | if (spare->prev_point == NULL__null) | ||||||||||||||||
561 | point_list_head = spare; | ||||||||||||||||
562 | |||||||||||||||||
563 | highest_cost = max_c(highest_cost,cost)((highest_cost)>(cost)?(highest_cost):(cost)); | ||||||||||||||||
564 | } | ||||||||||||||||
565 | |||||||||||||||||
566 | |||||||||||||||||
567 | int32 OrderedPointList::ContainsPoint(int32 x, int32 y,uint16 min_cost) | ||||||||||||||||
568 | { | ||||||||||||||||
569 | if (min_cost > highest_cost) | ||||||||||||||||
570 | return -1; | ||||||||||||||||
571 | while (cost_limits[min_cost] == NULL__null) | ||||||||||||||||
572 | min_cost++; | ||||||||||||||||
573 | |||||||||||||||||
574 | point *spare = cost_limits[min_cost]; | ||||||||||||||||
575 | |||||||||||||||||
576 | while (spare != NULL__null) { | ||||||||||||||||
577 | if ((spare->x == x) && (spare->y == y)) | ||||||||||||||||
578 | return spare->cost; | ||||||||||||||||
579 | |||||||||||||||||
580 | spare = spare->next_point; | ||||||||||||||||
581 | } | ||||||||||||||||
582 | |||||||||||||||||
583 | return -1; | ||||||||||||||||
584 | } |
1 | /* |
2 | * Copyright 2003, Heikki Suhonen |
3 | * Distributed under the terms of the MIT License. |
4 | * |
5 | * Authors: |
6 | * Heikki Suhonen <heikki.suhonen@gmail.com> |
7 | * |
8 | */ |
9 | #ifndef INTELLIGENT_PATH_FINDER_H |
10 | #define INTELLIGENT_PATH_FINDER_H |
11 | |
12 | #include <OS.h> |
13 | #include <SupportDefs.h> |
14 | |
15 | /* |
16 | This class calculates shortest paths between seed-point and other points. |
17 | This is primarily intended to be used with intelligent scissors but it could |
18 | be used for other purposes too. |
19 | */ |
20 | class OrderedPointList { |
21 | struct point { |
22 | point *next_point; |
23 | point *prev_point; |
24 | int32 x; |
25 | int32 y; |
26 | uint16 cost; |
27 | }; |
28 | |
29 | uint16 highest_cost; |
30 | point *point_list_head; |
31 | point **cost_limits; |
32 | |
33 | public: |
34 | OrderedPointList(); |
35 | ~OrderedPointList(); |
36 | |
37 | void RemoveLowestCostPoint(int32 *x,int32 *y,uint16 *cost); |
38 | void RemovePoint(int32 x,int32 y,uint16 cost); |
39 | void InsertPoint(int32 x, int32 y,uint16 cost); |
40 | int32 ContainsPoint(int32 x, int32 y,uint16 min_cost); |
41 | bool IsEmpty() { return point_list_head == NULL__null; } |
42 | }; |
43 | |
44 | |
45 | class IntelligentPathFinder { |
46 | BBitmap *original_bitmap; |
47 | uint32 *bitmap_bits; |
48 | int32 bitmap_bpr; |
49 | int32 right_border; |
50 | int32 bottom_border; |
51 | |
52 | // Entry that is 0 in local cost map means that the value for that particular pixel |
53 | // has not yet been calculated. |
54 | uint8 **local_cost_map; |
55 | uint8 LocalCost(int32 x, int32 y,int32 dx, int32 dy); |
56 | |
57 | |
58 | // The total costs and the paths are stored here. |
59 | uint16 **total_cost_map; |
60 | uint32 **path_pointers_map; // High 16 bits is the x coordinate while the rest is y coordinate |
61 | uint16 ReturnTotalCost(int32 x,int32 y); |
62 | void SetTotalCost(int32 x,int32 y,uint16 cost); |
63 | BPoint ReturnNextPointInPath(int32 x,int32 y); |
64 | void SetNextPointInPath(int32 x,int32 y,int32 nx,int32 ny); |
65 | void ResetTotalCostsAndPaths(); |
66 | |
67 | // The expanded pixels are stored here. |
68 | uint8 **expanded_bits; // 1 means that the pixel is expanded, 0 means it is not. |
69 | bool IsExpanded(int32 x, int32 y); |
70 | void SetExpanded(int32 x, int32 y); |
71 | void ResetExpanded(); |
72 | |
73 | |
74 | |
75 | // Here is the list that holds the active points. |
76 | OrderedPointList *active_point_list; |
77 | |
78 | |
79 | int32 seed_point_x; |
80 | int32 seed_point_y; |
81 | |
82 | |
83 | thread_id dp_thread; |
84 | |
85 | static int32 dp_thread_entry(void*); |
86 | int32 dp_thread_function(); |
87 | |
88 | static int32 lc_thread_entry(void*); |
89 | int32 lc_thread_function(); |
90 | |
91 | |
92 | // The next variables are used in controlling the two threads. |
93 | bool calculation_continuing; |
94 | bool seed_point_changed; |
95 | |
96 | void PrintCostMap(); |
97 | |
98 | public: |
99 | IntelligentPathFinder(BBitmap*); |
100 | ~IntelligentPathFinder(); |
101 | |
102 | void SetSeedPoint(int32 x, int32 y); |
103 | BPoint* ReturnPath(int32 x, int32 y,int32 *num_points); |
104 | }; |
105 | |
106 | #endif |