1 #include "Gravity.h"
2
3 #include <cmath>
4 #include <iostream>
5 #include <sys/types.h>
6
7 #include "CoordStack.h"
8 #include "Misc.h"
9 #include "Simulation.h"
10 #include "SimulationData.h"
11
12
Gravity()13 Gravity::Gravity()
14 {
15 // Allocate full size Gravmaps
16 unsigned int size = (XRES / CELL) * (YRES / CELL);
17 th_ogravmap = new float[size];
18 th_gravmap = new float[size];
19 th_gravy = new float[size];
20 th_gravx = new float[size];
21 th_gravp = new float[size];
22 gravmap = new float[size];
23 gravy = new float[size];
24 gravx = new float[size];
25 gravp = new float[size];
26 gravmask = new unsigned[size];
27 }
28
~Gravity()29 Gravity::~Gravity()
30 {
31 stop_grav_async();
32 #ifdef GRAVFFT
33 grav_fft_cleanup();
34 #endif
35
36 delete[] th_ogravmap;
37 delete[] th_gravmap;
38 delete[] th_gravy;
39 delete[] th_gravx;
40 delete[] th_gravp;
41 delete[] gravmap;
42 delete[] gravy;
43 delete[] gravx;
44 delete[] gravp;
45 delete[] gravmask;
46 }
47
Clear()48 void Gravity::Clear()
49 {
50 int size = (XRES / CELL) * (YRES / CELL);
51 std::fill(gravy, gravy + size, 0.0f);
52 std::fill(gravx, gravx + size, 0.0f);
53 std::fill(gravp, gravp + size, 0.0f);
54 std::fill(gravmap, gravmap + size, 0.0f);
55 std::fill(gravmask, gravmask + size, 0xFFFFFFFF);
56
57 ignoreNextResult = true;
58 }
59
60 #ifdef GRAVFFT
grav_fft_init()61 void Gravity::grav_fft_init()
62 {
63 int xblock2 = XRES/CELL*2;
64 int yblock2 = YRES/CELL*2;
65 int fft_tsize = (xblock2/2+1)*yblock2;
66 float distance, scaleFactor;
67 fftwf_plan plan_ptgravx, plan_ptgravy;
68 if (grav_fft_status) return;
69
70 //use fftw malloc function to ensure arrays are aligned, to get better performance
71 th_ptgravx = reinterpret_cast<float*>(fftwf_malloc(xblock2 * yblock2 * sizeof(float)));
72 th_ptgravy = reinterpret_cast<float*>(fftwf_malloc(xblock2 * yblock2 * sizeof(float)));
73 th_ptgravxt = reinterpret_cast<fftwf_complex*>(fftwf_malloc(fft_tsize * sizeof(fftwf_complex)));
74 th_ptgravyt = reinterpret_cast<fftwf_complex*>(fftwf_malloc(fft_tsize * sizeof(fftwf_complex)));
75 th_gravmapbig = reinterpret_cast<float*>(fftwf_malloc(xblock2 * yblock2 * sizeof(float)));
76 th_gravmapbigt = reinterpret_cast<fftwf_complex*>(fftwf_malloc(fft_tsize * sizeof(fftwf_complex)));
77 th_gravxbig = reinterpret_cast<float*>(fftwf_malloc(xblock2 * yblock2 * sizeof(float)));
78 th_gravybig = reinterpret_cast<float*>(fftwf_malloc(xblock2 * yblock2 * sizeof(float)));
79 th_gravxbigt = reinterpret_cast<fftwf_complex*>(fftwf_malloc(fft_tsize * sizeof(fftwf_complex)));
80 th_gravybigt = reinterpret_cast<fftwf_complex*>(fftwf_malloc(fft_tsize * sizeof(fftwf_complex)));
81
82 //select best algorithm, could use FFTW_PATIENT or FFTW_EXHAUSTIVE but that increases the time taken to plan, and I don't see much increase in execution speed
83 plan_ptgravx = fftwf_plan_dft_r2c_2d(yblock2, xblock2, th_ptgravx, th_ptgravxt, FFTW_MEASURE);
84 plan_ptgravy = fftwf_plan_dft_r2c_2d(yblock2, xblock2, th_ptgravy, th_ptgravyt, FFTW_MEASURE);
85 plan_gravmap = fftwf_plan_dft_r2c_2d(yblock2, xblock2, th_gravmapbig, th_gravmapbigt, FFTW_MEASURE);
86 plan_gravx_inverse = fftwf_plan_dft_c2r_2d(yblock2, xblock2, th_gravxbigt, th_gravxbig, FFTW_MEASURE);
87 plan_gravy_inverse = fftwf_plan_dft_c2r_2d(yblock2, xblock2, th_gravybigt, th_gravybig, FFTW_MEASURE);
88
89 //(XRES/CELL)*(YRES/CELL)*4 is size of data array, scaling needed because FFTW calculates an unnormalized DFT
90 scaleFactor = -M_GRAV/((XRES/CELL)*(YRES/CELL)*4);
91 //calculate velocity map caused by a point mass
92 for (int y = 0; y < yblock2; y++)
93 {
94 for (int x = 0; x < xblock2; x++)
95 {
96 if (x == XRES / CELL && y == YRES / CELL)
97 continue;
98 distance = sqrtf(pow(x-(XRES/CELL), 2.0f) + pow(y-(YRES/CELL), 2.0f));
99 th_ptgravx[y * xblock2 + x] = scaleFactor * (x - (XRES / CELL)) / pow(distance, 3);
100 th_ptgravy[y * xblock2 + x] = scaleFactor * (y - (YRES / CELL)) / pow(distance, 3);
101 }
102 }
103 th_ptgravx[yblock2 * xblock2 / 2 + xblock2 / 2] = 0.0f;
104 th_ptgravy[yblock2 * xblock2 / 2 + xblock2 / 2] = 0.0f;
105
106 //transform point mass velocity maps
107 fftwf_execute(plan_ptgravx);
108 fftwf_execute(plan_ptgravy);
109 fftwf_destroy_plan(plan_ptgravx);
110 fftwf_destroy_plan(plan_ptgravy);
111 fftwf_free(th_ptgravx);
112 fftwf_free(th_ptgravy);
113
114 //clear padded gravmap
115 memset(th_gravmapbig, 0, xblock2 * yblock2 * sizeof(float));
116
117 grav_fft_status = true;
118 }
119
grav_fft_cleanup()120 void Gravity::grav_fft_cleanup()
121 {
122 if (!grav_fft_status) return;
123 fftwf_free(th_ptgravxt);
124 fftwf_free(th_ptgravyt);
125 fftwf_free(th_gravmapbig);
126 fftwf_free(th_gravmapbigt);
127 fftwf_free(th_gravxbig);
128 fftwf_free(th_gravybig);
129 fftwf_free(th_gravxbigt);
130 fftwf_free(th_gravybigt);
131 fftwf_destroy_plan(plan_gravmap);
132 fftwf_destroy_plan(plan_gravx_inverse);
133 fftwf_destroy_plan(plan_gravy_inverse);
134 grav_fft_status = false;
135 }
136 #endif
137
gravity_update_async()138 void Gravity::gravity_update_async()
139 {
140 int result;
141 if (!enabled)
142 return;
143
144 bool signal_grav = false;
145
146 {
147 std::unique_lock<std::mutex> l(gravmutex, std::defer_lock);
148 if (l.try_lock())
149 {
150 result = grav_ready;
151 if (result) //Did the gravity thread finish?
152 {
153 if (th_gravchanged && !ignoreNextResult)
154 {
155 #if !defined(GRAVFFT) && defined(GRAV_DIFF)
156 memcpy(gravy, th_gravy, (XRES/CELL)*(YRES/CELL)*sizeof(float));
157 memcpy(gravx, th_gravx, (XRES/CELL)*(YRES/CELL)*sizeof(float));
158 memcpy(gravp, th_gravp, (XRES/CELL)*(YRES/CELL)*sizeof(float));
159 #else
160 // Copy thread gravity maps into this one
161 std::swap(gravy, th_gravy);
162 std::swap(gravx, th_gravx);
163 std::swap(gravp, th_gravp);
164 #endif
165 }
166 ignoreNextResult = false;
167
168 std::swap(gravmap, th_gravmap);
169
170 grav_ready = 0; //Tell the other thread that we're ready for it to continue
171 signal_grav = true;
172 }
173 }
174 }
175
176 if (signal_grav)
177 {
178 gravcv.notify_one();
179 }
180 unsigned int size = (XRES / CELL) * (YRES / CELL);
181 membwand(gravy, gravmask, size * sizeof(float), size * sizeof(unsigned));
182 membwand(gravx, gravmask, size * sizeof(float), size * sizeof(unsigned));
183 std::fill(&gravmap[0], &gravmap[size], 0.0f);
184 }
185
update_grav_async()186 void Gravity::update_grav_async()
187 {
188 int done = 0;
189 int thread_done = 0;
190 unsigned int size = (XRES / CELL) * (YRES / CELL);
191 std::fill(&th_ogravmap[0], &th_ogravmap[size], 0.0f);
192 std::fill(&th_gravmap[0], &th_gravmap[size], 0.0f);
193 std::fill(&th_gravy[0], &th_gravy[size], 0.0f);
194 std::fill(&th_gravx[0], &th_gravx[size], 0.0f);
195 std::fill(&th_gravp[0], &th_gravp[size], 0.0f);
196
197 #ifdef GRAVFFT
198 if (!grav_fft_status)
199 grav_fft_init();
200 #endif
201
202 std::unique_lock<std::mutex> l(gravmutex);
203 while (!thread_done)
204 {
205 if (!done)
206 {
207 // run gravity update
208 update_grav();
209 done = 1;
210 grav_ready = 1;
211 thread_done = gravthread_done;
212 }
213 else
214 {
215 // wait for main thread
216 gravcv.wait(l);
217 done = grav_ready;
218 thread_done = gravthread_done;
219 }
220 }
221 }
222
start_grav_async()223 void Gravity::start_grav_async()
224 {
225 if (enabled) //If it's already enabled, restart it
226 stop_grav_async();
227
228 gravthread_done = 0;
229 grav_ready = 0;
230 gravthread = std::thread([this]() { update_grav_async(); }); //Start asynchronous gravity simulation
231 enabled = true;
232
233 unsigned int size = (XRES / CELL) * (YRES / CELL);
234 std::fill(&gravy[0], &gravy[size], 0.0f);
235 std::fill(&gravx[0], &gravx[size], 0.0f);
236 std::fill(&gravp[0], &gravp[size], 0.0f);
237 std::fill(&gravmap[0], &gravmap[size], 0.0f);
238 }
239
stop_grav_async()240 void Gravity::stop_grav_async()
241 {
242 if (enabled)
243 {
244 {
245 std::lock_guard<std::mutex> g(gravmutex);
246 gravthread_done = 1;
247 }
248 gravcv.notify_one();
249 gravthread.join();
250 enabled = false;
251 }
252 // Clear the grav velocities
253 unsigned int size = (XRES / CELL) * (YRES / CELL);
254 std::fill(&gravy[0], &gravy[size], 0.0f);
255 std::fill(&gravx[0], &gravx[size], 0.0f);
256 std::fill(&gravp[0], &gravp[size], 0.0f);
257 std::fill(&gravmap[0], &gravmap[size], 0.0f);
258 }
259
260 #ifdef GRAVFFT
update_grav()261 void Gravity::update_grav()
262 {
263 int xblock2 = XRES/CELL*2, yblock2 = YRES/CELL*2;
264 int fft_tsize = (xblock2/2+1)*yblock2;
265 float mr, mc, pr, pc, gr, gc;
266 if (memcmp(th_ogravmap, th_gravmap, sizeof(float)*(XRES/CELL)*(YRES/CELL)) != 0)
267 {
268 th_gravchanged = 1;
269
270 membwand(th_gravmap, gravmask, (XRES/CELL)*(YRES/CELL)*sizeof(float), (XRES/CELL)*(YRES/CELL)*sizeof(unsigned));
271 //copy gravmap into padded gravmap array
272 for (int y = 0; y < YRES / CELL; y++)
273 {
274 for (int x = 0; x < XRES / CELL; x++)
275 {
276 th_gravmapbig[(y+YRES/CELL)*xblock2+XRES/CELL+x] = th_gravmap[y*(XRES/CELL)+x];
277 }
278 }
279 //transform gravmap
280 fftwf_execute(plan_gravmap);
281 //do convolution (multiply the complex numbers)
282 for (int i = 0; i < fft_tsize; i++)
283 {
284 mr = th_gravmapbigt[i][0];
285 mc = th_gravmapbigt[i][1];
286 pr = th_ptgravxt[i][0];
287 pc = th_ptgravxt[i][1];
288 gr = mr*pr-mc*pc;
289 gc = mr*pc+mc*pr;
290 th_gravxbigt[i][0] = gr;
291 th_gravxbigt[i][1] = gc;
292 pr = th_ptgravyt[i][0];
293 pc = th_ptgravyt[i][1];
294 gr = mr*pr-mc*pc;
295 gc = mr*pc+mc*pr;
296 th_gravybigt[i][0] = gr;
297 th_gravybigt[i][1] = gc;
298 }
299 //inverse transform, and copy from padded arrays into normal velocity maps
300 fftwf_execute(plan_gravx_inverse);
301 fftwf_execute(plan_gravy_inverse);
302 for (int y = 0; y < YRES / CELL; y++)
303 {
304 for (int x = 0; x < XRES / CELL; x++)
305 {
306 th_gravx[y*(XRES/CELL)+x] = th_gravxbig[y*xblock2+x];
307 th_gravy[y*(XRES/CELL)+x] = th_gravybig[y*xblock2+x];
308 th_gravp[y*(XRES/CELL)+x] = sqrtf(pow(th_gravxbig[y*xblock2+x],2)+pow(th_gravybig[y*xblock2+x],2));
309 }
310 }
311 }
312 else
313 {
314 th_gravchanged = 0;
315 }
316
317 // Copy th_ogravmap into th_gravmap (doesn't matter what th_ogravmap is afterwards)
318 std::swap(th_gravmap, th_ogravmap);
319 }
320
321 #else
322 // gravity without fast Fourier transforms
323
update_grav(void)324 void Gravity::update_grav(void)
325 {
326 int x, y, i, j, changed = 0;
327 float val, distance;
328 th_gravchanged = 0;
329 #ifndef GRAV_DIFF
330 //Find any changed cells
331 for (i=0; i<YRES/CELL; i++)
332 {
333 if(changed)
334 break;
335 for (j=0; j<XRES/CELL; j++)
336 {
337 if(th_ogravmap[i*(XRES/CELL)+j]!=th_gravmap[i*(XRES/CELL)+j]){
338 changed = 1;
339 break;
340 }
341 }
342 }
343 if(!changed)
344 goto fin;
345 memset(th_gravy, 0, (XRES/CELL)*(YRES/CELL)*sizeof(float));
346 memset(th_gravx, 0, (XRES/CELL)*(YRES/CELL)*sizeof(float));
347 #endif
348 th_gravchanged = 1;
349 membwand(th_gravmap, gravmask, (XRES/CELL)*(YRES/CELL)*sizeof(float), (XRES/CELL)*(YRES/CELL)*sizeof(unsigned));
350 for (i = 0; i < YRES / CELL; i++) {
351 for (j = 0; j < XRES / CELL; j++) {
352 #ifdef GRAV_DIFF
353 if (th_ogravmap[i*(XRES/CELL)+j] != th_gravmap[i*(XRES/CELL)+j])
354 {
355 #else
356 if (th_gravmap[i*(XRES/CELL)+j] > 0.0001f || th_gravmap[i*(XRES/CELL)+j]<-0.0001f) //Only calculate with populated or changed cells.
357 {
358 #endif
359 for (y = 0; y < YRES / CELL; y++) {
360 for (x = 0; x < XRES / CELL; x++) {
361 if (x == j && y == i)//Ensure it doesn't calculate with itself
362 continue;
363 distance = sqrt(pow(j - x, 2.0f) + pow(i - y, 2.0f));
364 #ifdef GRAV_DIFF
365 val = th_gravmap[i*(XRES/CELL)+j] - th_ogravmap[i*(XRES/CELL)+j];
366 #else
367 val = th_gravmap[i*(XRES/CELL)+j];
368 #endif
369 th_gravx[y*(XRES/CELL)+x] += M_GRAV * val * (j - x) / pow(distance, 3.0f);
370 th_gravy[y*(XRES/CELL)+x] += M_GRAV * val * (i - y) / pow(distance, 3.0f);
371 th_gravp[y*(XRES/CELL)+x] += M_GRAV * val / pow(distance, 2.0f);
372 }
373 }
374 }
375 }
376 }
377 fin:
378 memcpy(th_ogravmap, th_gravmap, (XRES/CELL)*(YRES/CELL)*sizeof(float));
379 }
380 #endif
381
382
383
384 bool Gravity::grav_mask_r(int x, int y, char checkmap[YRES/CELL][XRES/CELL], char shape[YRES/CELL][XRES/CELL])
385 {
386 int x1, x2;
387 bool ret = false;
388 try
389 {
390 CoordStack cs;
391 cs.push(x, y);
392 do
393 {
394 cs.pop(x, y);
395 x1 = x2 = x;
396 while (x1 >= 0)
397 {
398 if (x1 == 0)
399 {
400 ret = true;
401 break;
402 }
403 else if (checkmap[y][x1-1] || bmap[y][x1-1] == WL_GRAV)
404 break;
405 x1--;
406 }
407 while (x2 <= XRES/CELL-1)
408 {
409 if (x2 == XRES/CELL-1)
410 {
411 ret = true;
412 break;
413 }
414 else if (checkmap[y][x2+1] || bmap[y][x2+1] == WL_GRAV)
415 break;
416 x2++;
417 }
418 for (x = x1; x <= x2; x++)
419 {
420 shape[y][x] = 1;
421 checkmap[y][x] = 1;
422 }
423 if (y == 0)
424 {
425 for (x = x1; x <= x2; x++)
426 if (bmap[y][x] != WL_GRAV)
427 ret = true;
428 }
429 else if (y >= 1)
430 {
431 for (x = x1; x <= x2; x++)
432 if (!checkmap[y-1][x] && bmap[y-1][x] != WL_GRAV)
433 {
434 if (y-1 == 0)
435 ret = true;
436 cs.push(x, y-1);
437 }
438 }
439 if (y < YRES/CELL-1)
440 for (x=x1; x<=x2; x++)
441 if (!checkmap[y+1][x] && bmap[y+1][x] != WL_GRAV)
442 {
443 if (y+1 == YRES/CELL-1)
444 ret = true;
445 cs.push(x, y+1);
446 }
447 } while (cs.getSize()>0);
448 }
449 catch (std::exception& e)
450 {
451 std::cerr << e.what() << std::endl;
452 ret = false;
453 }
454 return ret;
455 }
456 void Gravity::mask_free(mask_el *c_mask_el)
457 {
458 if (c_mask_el == nullptr)
459 return;
460 delete[] c_mask_el->next;
461 delete[] c_mask_el->shape;
462 delete[] c_mask_el;
463 }
464
465 void Gravity::gravity_mask()
466 {
467 char checkmap[YRES/CELL][XRES/CELL];
468 unsigned maskvalue;
469 mask_el *t_mask_el = nullptr;
470 mask_el *c_mask_el = nullptr;
471 if (!gravmask)
472 return;
473 memset(checkmap, 0, sizeof(checkmap));
474 for (int x = 0; x < XRES / CELL; x++)
475 {
476 for(int y = 0; y < YRES / CELL; y++)
477 {
478 if (bmap[y][x] != WL_GRAV && checkmap[y][x] == 0)
479 {
480 // Create a new shape
481 if (t_mask_el == nullptr)
482 {
483 t_mask_el = new mask_el[sizeof(mask_el)];
484 t_mask_el->shape = new char[(XRES / CELL) * (YRES / CELL)];
485 std::fill(&t_mask_el->shape[0], &t_mask_el->shape[(XRES / CELL) * (YRES / CELL)], 0);
486 t_mask_el->shapeout = 0;
487 t_mask_el->next = nullptr;
488 c_mask_el = t_mask_el;
489 }
490 else
491 {
492 c_mask_el->next = new mask_el[sizeof(mask_el)];
493 c_mask_el = c_mask_el->next;
494 c_mask_el->shape = new char[(XRES / CELL) * (YRES / CELL)];
495 std::fill(&c_mask_el->shape[0], &c_mask_el->shape[(XRES / CELL) * (YRES / CELL)], 0);
496 c_mask_el->shapeout = 0;
497 c_mask_el->next = nullptr;
498 }
499 // Fill the shape
500 if (grav_mask_r(x, y, checkmap, reinterpret_cast<char(*)[XRES/CELL]>(c_mask_el->shape)))
501 c_mask_el->shapeout = 1;
502 }
503 }
504 }
505 c_mask_el = t_mask_el;
506 std::fill(&gravmask[0], &gravmask[(XRES / CELL) * (YRES / CELL)], 0);
507 while (c_mask_el != nullptr)
508 {
509 char *cshape = c_mask_el->shape;
510 for (int x = 0; x < XRES / CELL; x++)
511 {
512 for (int y = 0; y < YRES / CELL; y++)
513 {
514 if (cshape[y * (XRES / CELL) + x])
515 {
516 if (c_mask_el->shapeout)
517 maskvalue = 0xFFFFFFFF;
518 else
519 maskvalue = 0x00000000;
520 gravmask[y * (XRES / CELL) + x] = maskvalue;
521 }
522 }
523 }
524 c_mask_el = c_mask_el->next;
525 }
526 mask_free(t_mask_el);
527 }
528