1 /* phys.c -- xspringies physical modeling and numerical solving routines
2 * Copyright (C) 1991,1992 Douglas M. DeCarlo
3 *
4 * This file is part of XSpringies, a mass and spring simulation system for X
5 *
6 * XSpringies is free software; you can redistribute it and/or modify
7 * it under the terms of the GNU General Public License as published by
8 * the Free Software Foundation; either version 1, or (at your option)
9 * any later version.
10 *
11 * XSpringies is distributed in the hope that it will be useful,
12 * but WITHOUT ANY WARRANTY; without even the implied warranty of
13 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14 * GNU General Public License for more details.
15 *
16 * You should have received a copy of the GNU General Public License
17 * along with XSpringies; see the file COPYING. If not, write to
18 * the Free Software Foundation, 675 Mass Ave, Cambridge, MA 02139, USA.
19 *
20 */
21
22 #include "defs.h"
23 #include "obj.h"
24
25 #ifndef M_PI
26 #define M_PI 3.14159265358979323846
27 #endif
28
29 #define DT_MIN 0.0001
30 #define DT_MAX 0.5
31
32 #define MAXCON 1024
33
34 #define CONSTR_KS 1.0
35 #define CONSTR_KD 1.0
36
37 /* Do fudgy bounces */
38 #define BOUNCE_FUDGE 1
39
40 /* Stickiness calibration: STICK_MAG = 1.0, means that a mass = 1.0 with gravity = 1.0 will remain
41 stuck on a wall for all stickiness values > 1.0 */
42 #define STICK_MAG 1.0
43
accumulate_accel()44 void accumulate_accel()
45 {
46 double gval, gmisc;
47 double gx = 0, gy = 0, ogx = 0, ogy = 0;
48 double center_x = draw_wid/2.0, center_y = draw_ht/2.0, center_rad = 1.0;
49 mass *m, *m1, *m2;
50 spring *s;
51 register int i;
52
53 /* ------------------ applied force effects ----------------------- */
54
55 if (mst.center_id >= 0) {
56 if (masses[mst.center_id].status & S_ALIVE) {
57 center_x = masses[mst.center_id].x;
58 center_y = masses[mst.center_id].y;
59 } else {
60 mst.center_id = -1;
61 }
62 }
63
64 /* Do gravity */
65 if (mst.bf_mode[FR_GRAV] > 0) {
66 gval = mst.cur_grav_val[FR_GRAV];
67 gmisc = mst.cur_misc_val[FR_GRAV];
68
69 gx = COORD_DX(gval * sin(gmisc * M_PI / 180.0));
70 gy = COORD_DY(gval * cos(gmisc * M_PI / 180.0));
71 }
72
73 /* Keep center of mass in the middle force */
74 if (mst.bf_mode[FR_CMASS] > 0) {
75 double mixix = 0.0, mixiy = 0.0, mivix = 0.0, miviy = 0.0, msum = 0.0;
76 gval = mst.cur_grav_val[FR_CMASS];
77 gmisc = mst.cur_misc_val[FR_CMASS];
78
79 for (i = 0; i < num_mass; i++) {
80 m = masses + i;
81 if (i != mst.center_id && (m->status & S_ALIVE) && !(m->status & S_FIXED)) {
82 msum += m->mass;
83 mixix += m->mass * m->x;
84 mixiy += m->mass * m->y;
85 mivix += m->mass * m->vx;
86 miviy += m->mass * m->vy;
87 }
88 }
89
90 if (msum) {
91 mixix /= msum;
92 mixiy /= msum;
93 mivix /= msum;
94 miviy /= msum;
95
96 mixix -= center_x;
97 mixiy -= center_y;
98
99 ogx -= (gval * mixix + gmisc * mivix) / msum;
100 ogy -= (gval * mixiy + gmisc * miviy) / msum;
101 }
102 }
103
104 /* Apply Gravity, CM and air drag to all masses */
105 for (i = 0; i < num_mass; i++) {
106 m = masses + i;
107 if ((m->status & S_ALIVE) && !(m->status & S_FIXED)) {
108 /* Do viscous drag */
109 if (i != mst.center_id) {
110 m->ax = gx + ogx - mst.cur_visc * m->vx;
111 m->ay = gy + ogy - mst.cur_visc * m->vy;
112 } else {
113 m->ax = gx - mst.cur_visc * m->vx;
114 m->ay = gy - mst.cur_visc * m->vy;
115 }
116 }
117 }
118
119 /* Do point attraction force */
120 if (mst.bf_mode[FR_PTATTRACT] > 0) {
121 gval = mst.cur_grav_val[FR_PTATTRACT];
122 gmisc = mst.cur_misc_val[FR_PTATTRACT];
123
124 for (i = 0; i < num_mass; i++) {
125 m = masses + i;
126 if ((m->status & S_ALIVE) && !(m->status & S_FIXED)) {
127 double dx, dy, mag, fmag;
128
129 dx = (center_x - m->x);
130 dy = (center_y - m->y);
131 mag = sqrt(dx * dx + dy * dy);
132
133 if (mag < m->radius + center_rad) {
134 dx *= mag / (m->radius + center_rad);
135 dy *= mag / (m->radius + center_rad);
136 mag = m->radius + center_rad;
137 }
138
139 fmag = gval / pow(mag, gmisc);
140
141 m->ax += fmag * dx / mag;
142 m->ay += fmag * dy / mag;
143 }
144 }
145 }
146
147 /* Wall attract/repel force */
148 if (mst.bf_mode[FR_WALL] > 0) {
149 double dax, day, dist;
150
151 gval = -mst.cur_grav_val[FR_WALL];
152 gmisc = mst.cur_misc_val[FR_WALL];
153
154 for (i = 0; i < num_mass; i++) {
155 m = masses + i;
156 if ((m->status & S_ALIVE) && !(m->status & S_FIXED)) {
157 dax = day = 0;
158
159 if (mst.w_left && (dist = m->x - m->radius) >= 0) {
160 if (dist < 1) dist = 1;
161 dist = pow(dist, gmisc);
162 dax -= gval / dist;
163 }
164 if (mst.w_right && (dist = draw_wid - m->radius - m->x) >= 0) {
165 if (dist < 1) dist = 1;
166 dist = pow(dist, gmisc);
167 dax += gval / dist;
168 }
169 if (mst.w_top && (dist = draw_ht - m->radius - m->y) >= 0) {
170 if (dist < 1) dist = 1;
171 dist = pow(dist, gmisc);
172 day += gval / dist;
173 }
174 if (mst.w_bottom && (dist = m->y - m->radius) >= 0) {
175 if (dist < 1) dist = 1;
176 dist = pow(dist, gmisc);
177 day -= gval / dist;
178 }
179
180 m->ax += dax;
181 m->ay += day;
182 }
183 }
184 }
185
186 /* ------------------ spring effects ----------------------- */
187
188 /* Spring compression/damping effects on masses */
189 for (i = 0; i < num_spring; i++) {
190 s = springs + i;
191 if (s->status & S_ALIVE) {
192 double dx, dy, force, forcex, forcey, mag, damp, mass1, mass2;
193
194 m1 = masses + s->m1;
195 m2 = masses + s->m2;
196
197 dx = m1->x - m2->x;
198 dy = m1->y - m2->y;
199
200 if (dx || dy) {
201 mag = sqrt(dx * dx + dy * dy);
202
203 force = s->ks * (s->restlen - mag);
204 if (s->kd) {
205 damp = ((m1->vx - m2->vx) * dx + (m1->vy - m2->vy) * dy) / mag;
206 force -= s->kd * damp;
207 }
208
209 force /= mag;
210 forcex = force * dx;
211 forcey = force * dy;
212
213 mass1 = m1->mass;
214 mass2 = m2->mass;
215
216 if (mass1 != 0) {
217 m1->ax += forcex / mass1;
218 m1->ay += forcey / mass1;
219 }
220 if (mass2 != 0) {
221 m2->ax -= forcex / mass2;
222 m2->ay -= forcey / mass2;
223 }
224 }
225 }
226 }
227 }
228
runge_kutta(h,testloc)229 void runge_kutta(h, testloc)
230 double h;
231 boolean testloc;
232 {
233 mass *m;
234 int i;
235
236 accumulate_accel();
237
238 /* k1 step */
239 for (i = 0; i < num_mass; i++) {
240 m = masses + i;
241
242 if ((m->status & S_ALIVE) && !(m->status & S_FIXED)) {
243 /* Initial storage */
244 m->cur_x = m->x;
245 m->cur_y = m->y;
246 m->cur_vx = m->vx;
247 m->cur_vy = m->vy;
248
249 m->k1x = m->vx * h;
250 m->k1y = m->vy * h;
251 m->k1vx = m->ax * h;
252 m->k1vy = m->ay * h;
253
254 m->x = m->cur_x + m->k1x / 2;
255 m->y = m->cur_y + m->k1y / 2;
256 m->vx = m->cur_vx + m->k1vx / 2;
257 m->vy = m->cur_vy + m->k1vy / 2;
258 }
259 }
260
261 accumulate_accel();
262
263 /* k2 step */
264 for (i = 0; i < num_mass; i++) {
265 m = masses + i;
266 if ((m->status & S_ALIVE) && !(m->status & S_FIXED)) {
267 m->k2x = m->vx * h;
268 m->k2y = m->vy * h;
269 m->k2vx = m->ax * h;
270 m->k2vy = m->ay * h;
271
272 m->x = m->cur_x + m->k2x / 2;
273 m->y = m->cur_y + m->k2y / 2;
274 m->vx = m->cur_vx + m->k2vx / 2;
275 m->vy = m->cur_vy + m->k2vy / 2;
276 }
277 }
278
279 accumulate_accel();
280
281 /* k3 step */
282 for (i = 0; i < num_mass; i++) {
283 m = masses + i;
284 if ((m->status & S_ALIVE) && !(m->status & S_FIXED)) {
285 m->k3x = m->vx * h;
286 m->k3y = m->vy * h;
287 m->k3vx = m->ax * h;
288 m->k3vy = m->ay * h;
289
290 m->x = m->cur_x + m->k3x;
291 m->y = m->cur_y + m->k3y;
292 m->vx = m->cur_vx + m->k3vx;
293 m->vy = m->cur_vy + m->k3vy;
294 }
295 }
296
297 accumulate_accel();
298
299 /* k4 step */
300 for (i = 0; i < num_mass; i++) {
301 m = masses + i;
302 if ((m->status & S_ALIVE) && !(m->status & S_FIXED)) {
303 m->k4x = m->vx * h;
304 m->k4y = m->vy * h;
305 m->k4vx = m->ax * h;
306 m->k4vy = m->ay * h;
307 }
308 }
309
310 /* Find next position */
311 for (i = 0; i < num_mass; i++) {
312 m = masses + i;
313 if ((m->status & S_ALIVE) && !(m->status & S_FIXED)) {
314 if (testloc) {
315 m->test_x = m->cur_x + (m->k1x/2.0 + m->k2x + m->k3x + m->k4x/2.0)/3.0;
316 m->test_y = m->cur_y + (m->k1y/2.0 + m->k2y + m->k3y + m->k4y/2.0)/3.0;
317 m->test_vx = m->cur_vx + (m->k1vx/2.0 + m->k2vx + m->k3vx + m->k4vx/2.0)/3.0;
318 m->test_vy = m->cur_vy + (m->k1vy/2.0 + m->k2vy + m->k3vy + m->k4vy/2.0)/3.0;
319 } else {
320 m->x = m->cur_x + (m->k1x/2.0 + m->k2x + m->k3x + m->k4x/2.0)/3.0;
321 m->y = m->cur_y + (m->k1y/2.0 + m->k2y + m->k3y + m->k4y/2.0)/3.0;
322 m->vx = m->cur_vx + (m->k1vx/2.0 + m->k2vx + m->k3vx + m->k4vx/2.0)/3.0;
323 m->vy = m->cur_vy + (m->k1vy/2.0 + m->k2vy + m->k3vy + m->k4vy/2.0)/3.0;
324 }
325 }
326 }
327 }
328
329 /* ---------------------
330 RKF45 method (upgraded from RK4) thanks to
331 Nate Loofbourrow (loofbour@cis.ohio-state.edu)
332 -------------------- */
333 /* Adapted from Numerical Recipes (2nd ed.) pp. 719-720 (RKF45 method). -njl */
adaptive_runge_kutta()334 void adaptive_runge_kutta()
335 {
336 int i;
337 mass *m;
338 double err, errx, erry, errvx, errvy, maxerr, h;
339
340 restart:
341 if (mst.cur_dt > DT_MAX)
342 mst.cur_dt = DT_MAX;
343 if (mst.cur_dt < DT_MIN)
344 mst.cur_dt = DT_MIN;
345
346 h = mst.cur_dt;
347
348 accumulate_accel();
349
350 /* k1 step */
351 for (i = 0; i < num_mass; i++) {
352 m = masses + i;
353
354 if ((m->status & S_ALIVE) && !(m->status & S_FIXED)) {
355 /* Initial storage */
356 m->cur_x = m->x;
357 m->cur_y = m->y;
358 m->cur_vx = m->vx;
359 m->cur_vy = m->vy;
360
361 m->k1x = m->vx * h;
362 m->k1y = m->vy * h;
363 m->k1vx = m->ax * h;
364 m->k1vy = m->ay * h;
365
366 m->x = m->cur_x + (0.2) * m->k1x;
367 m->y = m->cur_y + (0.2) * m->k1y;
368 m->vx = m->cur_vx + (0.2) * m->k1vx;
369 m->vy = m->cur_vy + (0.2) * m->k1vy;
370 }
371 }
372
373 accumulate_accel();
374
375 /* k2 step */
376 for (i = 0; i < num_mass; i++) {
377 m = masses + i;
378 if ((m->status & S_ALIVE) && !(m->status & S_FIXED)) {
379 m->k2x = m->vx * h;
380 m->k2y = m->vy * h;
381 m->k2vx = m->ax * h;
382 m->k2vy = m->ay * h;
383
384 m->x = m->cur_x + (3.0/40.0) * m->k1x + (9.0/40.0) * m->k2x;
385 m->y = m->cur_y + (3.0/40.0) * m->k1y + (9.0/40.0) * m->k2y;
386 m->vx = m->cur_vx + (3.0/40.0) * m->k1vx + (9.0/40.0) * m->k2vx;
387 m->vy = m->cur_vy + (3.0/40.0) * m->k1vy + (9.0/40.0) * m->k2vy;
388 }
389 }
390
391 accumulate_accel();
392
393 /* k3 step */
394 for (i = 0; i < num_mass; i++) {
395 m = masses + i;
396 if ((m->status & S_ALIVE) && !(m->status & S_FIXED)) {
397 m->k3x = m->vx * h;
398 m->k3y = m->vy * h;
399 m->k3vx = m->ax * h;
400 m->k3vy = m->ay * h;
401
402 m->x = m->cur_x + (0.3)*m->k1x + (-0.9)*m->k2x + (1.2)*m->k3x;
403 m->y = m->cur_y + (0.3)*m->k1y + (-0.9)*m->k2y + (1.2)*m->k3y;
404 m->vx = m->cur_vx + (0.3)*m->k1vx + (-0.9)*m->k2vx + (1.2)*m->k3vx;
405 m->vy = m->cur_vy + (0.3)*m->k1vy + (-0.9)*m->k2vy + (1.2)*m->k3vy;
406 }
407 }
408
409 accumulate_accel();
410
411 /* k4 step */
412 for (i = 0; i < num_mass; i++) {
413 m = masses + i;
414 if ((m->status & S_ALIVE) && !(m->status & S_FIXED)) {
415 m->k4x = m->vx * h;
416 m->k4y = m->vy * h;
417 m->k4vx = m->ax * h;
418 m->k4vy = m->ay * h;
419
420 m->x = m->cur_x + (-11.0/54.0)*m->k1x + (2.5)*m->k2x
421 + (-70.0/27.0)*m->k3x + (35.0/27.0)*m->k4x;
422 m->y = m->cur_y + (-11.0/54.0)*m->k1y + (2.5)*m->k2y
423 + (-70.0/27.0)*m->k3y + (35.0/27.0)*m->k4y;
424 m->vx = m->cur_vx + (-11.0/54.0)*m->k1vx + (2.5)*m->k2vx
425 + (-70.0/27.0)*m->k3vx + (35.0/27.0)*m->k4vx;
426 m->vy = m->cur_vy + (-11.0/54.0)*m->k1vy + (2.5)*m->k2vy
427 + (-70.0/27.0)*m->k3vy + (35.0/27.0)*m->k4vy;
428 }
429 }
430
431 accumulate_accel();
432
433 /* k5 step */
434 for (i = 0; i < num_mass; i++) {
435 m = masses + i;
436 if ((m->status & S_ALIVE) && !(m->status & S_FIXED)) {
437 m->k5x = m->vx * h;
438 m->k5y = m->vy * h;
439 m->k5vx = m->ax * h;
440 m->k5vy = m->ay * h;
441
442 m->x = m->cur_x + (1631.0/55926.0)*m->k1x + (175.0/512.0)*m->k2x
443 + (575.0/13824.0)*m->k3x + (44275.0/110592.0)*m->k4x
444 + (253.0/4096.0)*m->k5x;
445 m->y = m->cur_y + (1631.0/55926.0)*m->k1y + (175.0/512.0)*m->k2y
446 + (575.0/13824.0)*m->k3y + (44275.0/110592.0)*m->k4y
447 + (253.0/4096.0)*m->k5y;
448 m->vx= m->cur_vx + (1631.0/55926.0)*m->k1vx + (175.0/512.0)*m->k2vx
449 + (575.0/13824.0)*m->k3vx + (44275.0/110592.0)*m->k4vx
450 + (253.0/4096.0)*m->k5vx;
451 m->vy= m->cur_vy + (1631.0/55926.0)*m->k1vy + (175.0/512.0)*m->k2vy
452 + (575.0/13824.0)*m->k3vy + (44275.0/110592.0)*m->k4vy
453 + (253.0/4096.0)*m->k5vy;
454 }
455 }
456
457 accumulate_accel();
458
459 /* k6 step */
460 for (i = 0; i < num_mass; i++) {
461 m = masses + i;
462 if ((m->status & S_ALIVE) && !(m->status & S_FIXED)) {
463 m->k6x = m->vx * h;
464 m->k6y = m->vy * h;
465 m->k6vx = m->ax * h;
466 m->k6vy = m->ay * h;
467 }
468 }
469
470 /* Find error */
471 maxerr = 0.00001;
472 for (i = 0; i < num_mass; i++) {
473 m = masses + i;
474 if ((m->status & S_ALIVE) && !(m->status & S_FIXED)) {
475 errx = ((37.0/378.0)-(2825.0/27648.0))*m->k1x +
476 ((250.0/621.0)-(18575.0/48384.0))*m->k3x +
477 ((125.0/594.0)-(13525.0/55296.0))*m->k4x +
478 (-277.0/14336.0)*m->k5x + ((512.0/1771.0)-(0.25))*m->k6x;
479 erry = ((37.0/378.0)-(2825.0/27648.0))*m->k1y +
480 ((250.0/621.0)-(18575.0/48384.0))*m->k3y +
481 ((125.0/594.0)-(13525.0/55296.0))*m->k4y +
482 (-277.0/14336.0)*m->k5y + ((512.0/1771.0)-(0.25))*m->k6y;
483 errvx = ((37.0/378.0)-(2825.0/27648.0))*m->k1vx +
484 ((250.0/621.0)-(18575.0/48384.0))*m->k3vx +
485 ((125.0/594.0)-(13525.0/55296.0))*m->k4vx +
486 (-277.0/14336.0)*m->k5vx + ((512.0/1771.0)-(0.25))*m->k6vx;
487 errvy = ((37.0/378.0)-(2825.0/27648.0))*m->k1vy +
488 ((250.0/621.0)-(18575.0/48384.0))*m->k3vy +
489 ((125.0/594.0)-(13525.0/55296.0))*m->k4vy +
490 (-277.0/14336.0)*m->k5vy + ((512.0/1771.0)-(0.25))*m->k6vy;
491 err = fabs(errx) + fabs(erry) + fabs(errvx) + fabs(errvy);
492
493 if (err > maxerr) {
494 maxerr = err;
495 }
496 }
497 }
498
499 /* Fudgy scale factor -- user controlled */
500 maxerr /= mst.cur_prec;
501
502 if (maxerr < 1.0) {
503 mst.cur_dt *= 0.9 * exp(-log(maxerr)/8.0);
504 } else {
505 if (mst.cur_dt > DT_MIN) {
506 for (i = 0; i < num_mass; i++) {
507 m = masses + i;
508 if ((m->status & S_ALIVE) && !(m->status & S_FIXED)) {
509 m->x = m->old_x;
510 m->y = m->old_y;
511 m->vx = m->old_vx;
512 m->vy = m->old_vy;
513 }
514 }
515
516 mst.cur_dt *= 0.9 * exp(-log(maxerr)/4.0);
517
518 goto restart;
519 }
520 }
521
522 /* Find next position */
523 for (i = 0; i < num_mass; i++) {
524 m = masses + i;
525 if ((m->status & S_ALIVE) && !(m->status & S_FIXED)) {
526 m->x=m->cur_x + (37.0/378.0)*m->k1x + (250.0/621.0)*m->k3x
527 + (125.0/594.0)*m->k4x + (512.0/1771.0)*m->k6x;
528 m->y=m->cur_y + (37.0/378.0)*m->k1y + (250.0/621.0)*m->k3y
529 + (125.0/594.0)*m->k4y + (512.0/1771.0)*m->k6y;
530 m->vx=m->cur_vx + (37.0/378.0)*m->k1vx
531 + (250.0/621.0)*m->k3vx + (125.0/594.0)*m->k4vx
532 + (512.0/1771.0)*m->k6vx;
533 m->vy=m->cur_vy + (37.0/378.0)*m->k1vy
534 + (250.0/621.0)*m->k3vy + (125.0/594.0)*m->k4vy
535 + (512.0/1771.0)*m->k6vy;
536 }
537 }
538 }
539
animate_obj()540 boolean animate_obj()
541 {
542 mass *m;
543 spring *s;
544 int i;
545 double stick_mag;
546 static int num_since = 0;
547 static double time_elapsed = 0.0;
548
549 /* Save initial values */
550 for (i = 0; i < num_mass; i++) {
551 m = masses + i;
552 if ((m->status & S_ALIVE) && !(m->status & S_FIXED)) {
553 m->old_x = m->x;
554 m->old_y = m->y;
555 m->old_vx = m->vx;
556 m->old_vy = m->vy;
557 }
558 }
559
560 if (mst.adaptive_step) {
561 boolean any_spring = FALSE;
562
563 for (i = 0; i < num_spring; i++) {
564 s = springs + i;
565 if (s->status & S_ALIVE) {
566 any_spring = TRUE;
567 break;
568 }
569 }
570
571 /* If no springs, then use dt=DEF_TSTEP */
572 if (any_spring) {
573 /* check mst.stiff */
574 adaptive_runge_kutta();
575 } else {
576 runge_kutta(mst.cur_dt = DEF_TSTEP, FALSE);
577 }
578 } else {
579 runge_kutta(mst.cur_dt, FALSE);
580 }
581
582 stick_mag = STICK_MAG * mst.cur_dt * mst.cur_stick;
583
584 /* Crappy wall code */
585 for (i = 0; i < num_mass; i++) {
586 m = masses + i;
587
588 if ((m->status & S_ALIVE) && !(m->status & S_FIXED)) {
589 /* Delete "exploded" objects */
590 if (m->ax - m->ax != 0.0 || m->ay - m->ay != 0.0 || m->x - m->x != 0.0 || m->y - m->y != 0.0) {
591 delete_mass(i);
592 continue;
593 }
594
595 /* Check if stuck to a wall */
596 if (m->old_vx == 0.0 && m->old_vy == 0.0) {
597 /* Left or right wall */
598 if ((mst.w_left && ABS(m->old_x - m->radius) < 0.5) || (mst.w_right && ABS(m->old_x - draw_wid + m->radius) < 0.5)) {
599 if (ABS(m->vx) < stick_mag / m->mass) {
600 m->vx = m->vy = 0;
601 m->x = m->old_x;
602 m->y = m->old_y;
603
604 continue;
605 }
606 } else if ((mst.w_bottom && ABS(m->old_y - m->radius) < 0.5) || (mst.w_top && ABS(m->old_y - draw_ht + m->radius) < 0.5)) {
607 /* Top or bottom wall */
608 if (ABS(m->vy) < stick_mag / m->mass) {
609 m->vx = m->vy = 0;
610 m->x = m->old_x;
611 m->y = m->old_y;
612
613 continue;
614 }
615 }
616 }
617
618 /* Bounce off left or right wall */
619 if (mst.w_left && m->x < m->radius && m->old_x >= m->radius) {
620 m->x = m->radius;
621
622 if (m->vx < 0) {
623 m->vx = -m->vx * m->elastic;
624 m->vy *= m->elastic;
625
626 /* Get stuck if not going fast enough */
627 if (m->vx > 0) {
628 m->vx -= STICK_MAG * mst.cur_stick / m->mass;
629
630 if (m->vx < 0) {
631 m->vx = m->vy = 0;
632 }
633 }
634 }
635 } else if (mst.w_right && m->x > draw_wid - m->radius && m->old_x <= draw_wid - m->radius) {
636 m->x = draw_wid - m->radius;
637
638 if (m->vx > 0) {
639 m->vx = -m->vx * m->elastic;
640 m->vy *= m->elastic;
641
642 /* Get stuck if not going fast enough */
643 if (m->vx < 0) {
644 m->vx += STICK_MAG * mst.cur_stick / m->mass;
645
646 if (m->vx > 0) {
647 m->vx = m->vy = 0;
648 }
649 }
650 }
651 }
652 /* Stick to top or bottom wall */
653 if (mst.w_bottom && m->y < m->radius && m->old_y >= m->radius) {
654 m->y = m->radius;
655
656 if (m->vy < 0) {
657 m->vy = -m->vy * m->elastic;
658 m->vx *= m->elastic;
659
660 /* Get stuck if not going fast enough */
661 if (m->vy > 0) {
662 m->vy -= STICK_MAG * mst.cur_stick / m->mass;
663
664 if (m->vy < 0) {
665 m->vx = m->vy = 0;
666 }
667 }
668 }
669 } else if (mst.w_top && m->y > (draw_ht - m->radius) && m->old_y <= (draw_ht - m->radius)) {
670 m->y = draw_ht - m->radius;
671
672 if (m->vy > 0) {
673 m->vy = -m->vy * m->elastic;
674 m->vx *= m->elastic;
675
676 /* Get stuck if not going fast enough */
677 if (m->vy < 0) {
678 m->vy += STICK_MAG * mst.cur_stick / m->mass;
679
680 if (m->vy > 0) {
681 m->vx = m->vy = 0;
682 }
683 }
684 }
685 }
686 }
687 }
688
689 /* Oblique impact */
690 /* added by Martin Lukanowicz march 93 luky@ihssv.wsr.ac.at */
691 /* - changed a little by dmd */
692 if (mst.collide > 0) {
693 for (i = 0; i < num_mass; i++) {
694 mass *m1, *m2;
695 register int j;
696
697 m1 = masses + i;
698
699 if (m1->status & S_ALIVE) {
700 double m1radius, m2radius, ratio, ratio2;
701 double dx, dy, dxq, dyq, sumxyq, m1x, m1y, mag;
702 double m1vx, m1vy;
703 double m2vx, m2vy;
704
705 m1radius = (m1->status & S_FIXED) ? NAIL_SIZE : m1->radius;
706 m1x = m1->x;
707 m1y = m1->y;
708
709 for (j = i+1; j < num_mass; j++) {
710 m2 = masses + j;
711
712 if (m2->status & S_ALIVE) {
713 m2radius = (m2->status & S_FIXED) ?
714 NAIL_SIZE : m2->radius;
715
716 dx = m2->x - m1x;
717 dy = m2->y - m1y;
718 dxq = dx*dx;
719 dyq = dy*dy;
720 sumxyq = dxq+dyq;
721 mag = sqrt(sumxyq);
722
723 if (mag < m1radius + m2radius) {
724 m1vx = m1->vx;
725 m1vy = m1->vy;
726 m2vx = m2->vx;
727 m2vy = m2->vy;
728
729 if ((m1vx-m2vx)*(dx)>0 || (m1vy-m2vy)*(dy)>0) {
730 if (dx == 0) dx = 1e-10;
731
732 if (!(m1->status & S_FIXED)) {
733 if (m2->status & S_FIXED)
734 ratio = 1+(m1->elastic+m2->elastic)/2;
735 else
736 ratio = (1+(m1->elastic+m2->elastic)/2)/
737 (1+m1->mass/m2->mass);
738
739 m1->vx = (m1vx - (m1vx - m2vx)*ratio)*(dxq/sumxyq) +
740 m1vx*(dyq/sumxyq) -
741 (m1vy - m2vy)*ratio*(dx*dy/sumxyq);
742 m1->vy = (m1->vx - m1vx)*(dy/dx) + m1vy;
743 }
744
745
746 if (!(m2->status & S_FIXED)) {
747 if (m1->status & S_FIXED)
748 ratio = 1+(m1->elastic+m2->elastic)/2;
749 else
750 ratio = (1+(m1->elastic+m2->elastic)/2)/
751 (1+m2->mass/m1->mass);
752
753 m2->vx = (m2vx - (m2vx - m1vx)*ratio)*(dxq/sumxyq) +
754 m2vx*(dyq/sumxyq) -
755 (m2vy - m1vy)*ratio*(dx*dy/sumxyq);
756 m2->vy = (m2->vx - m2vx)*(dy/dx) + m2vy;
757 }
758 }
759 }
760 }
761 }
762 }
763 }
764 }
765
766
767 time_elapsed += mst.cur_dt;
768
769 if (time_elapsed > 0.05) {
770 time_elapsed -= 0.05;
771 num_since = 0;
772 return TRUE;
773 }
774
775 num_since++;
776
777 if (num_since > 8) {
778 num_since = 0;
779 return TRUE;
780 }
781 return FALSE;
782 }
783