home *** CD-ROM | disk | FTP | other *** search
/ Usenet 1994 October / usenetsourcesnewsgroupsinfomagicoctober1994disk2.iso / x / volume14 / xpsringies / part03 / phys.c < prev    next >
Encoding:
C/C++ Source or Header  |  1991-08-27  |  13.0 KB  |  553 lines
  1. /* phys.c -- xspringies physical modeling and numerical solving routines
  2.  * Copyright (C) 1991  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.  
  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.         m1->ax += forcex / mass1;
  217.         m1->ay += forcey / mass1;
  218.         m2->ax -= forcex / mass2;
  219.         m2->ay -= forcey / mass2;
  220.         }
  221.     }
  222.     }
  223. }
  224.  
  225. void runge_kutta(h, testloc)
  226. double h;
  227. boolean testloc;
  228. {
  229.     mass *m;
  230.     int i;
  231.  
  232.     accumulate_accel();
  233.  
  234.     /* k1 step */
  235.     for (i = 0; i < num_mass; i++) {
  236.     m = masses + i;
  237.  
  238.     if ((m->status & S_ALIVE) && !(m->status & S_FIXED)) {
  239.         /* Initial storage */
  240.         m->cur_x = m->x;
  241.         m->cur_y = m->y;
  242.         m->cur_vx = m->vx;
  243.         m->cur_vy = m->vy;
  244.  
  245.         m->k1x = m->vx * h;
  246.         m->k1y = m->vy * h;
  247.         m->k1vx = m->ax * h;
  248.         m->k1vy = m->ay * h;
  249.         
  250.         m->x = m->cur_x + m->k1x / 2;
  251.         m->y = m->cur_y + m->k1y / 2;
  252.         m->vx = m->cur_vx + m->k1vx / 2;
  253.         m->vy = m->cur_vy + m->k1vy / 2;
  254.     }
  255.     }
  256.  
  257.     accumulate_accel();
  258.  
  259.     /* k2 step */
  260.     for (i = 0; i < num_mass; i++) {
  261.     m = masses + i;
  262.     if ((m->status & S_ALIVE) && !(m->status & S_FIXED)) {
  263.         m->k2x = m->vx * h;
  264.         m->k2y = m->vy * h;
  265.         m->k2vx = m->ax * h;
  266.         m->k2vy = m->ay * h;
  267.         
  268.         m->x = m->cur_x + m->k2x / 2;
  269.         m->y = m->cur_y + m->k2y / 2;
  270.         m->vx = m->cur_vx + m->k2vx / 2;
  271.         m->vy = m->cur_vy + m->k2vy / 2;
  272.     }
  273.     }
  274.  
  275.     accumulate_accel();
  276.  
  277.     /* k3 step */
  278.     for (i = 0; i < num_mass; i++) {
  279.     m = masses + i;
  280.     if ((m->status & S_ALIVE) && !(m->status & S_FIXED)) {
  281.         m->k3x = m->vx * h;
  282.         m->k3y = m->vy * h;
  283.         m->k3vx = m->ax * h;
  284.         m->k3vy = m->ay * h;
  285.         
  286.         m->x = m->cur_x + m->k3x;
  287.         m->y = m->cur_y + m->k3y;
  288.         m->vx = m->cur_vx + m->k3vx;
  289.         m->vy = m->cur_vy + m->k3vy;
  290.     }
  291.     }
  292.  
  293.     accumulate_accel();
  294.  
  295.     /* k4 step */
  296.     for (i = 0; i < num_mass; i++) {
  297.     m = masses + i;
  298.     if ((m->status & S_ALIVE) && !(m->status & S_FIXED)) {
  299.         m->k4x = m->vx * h;
  300.         m->k4y = m->vy * h;
  301.         m->k4vx = m->ax * h;
  302.         m->k4vy = m->ay * h;
  303.     }
  304.     }
  305.  
  306.     /* Find next position */
  307.     for (i = 0; i < num_mass; i++) {
  308.     m = masses + i;
  309.     if ((m->status & S_ALIVE) && !(m->status & S_FIXED)) {
  310.         if (testloc) {
  311.         m->test_x = m->cur_x + (m->k1x/2.0 + m->k2x + m->k3x + m->k4x/2.0)/3.0;
  312.         m->test_y = m->cur_y + (m->k1y/2.0 + m->k2y + m->k3y + m->k4y/2.0)/3.0;
  313.         m->test_vx = m->cur_vx + (m->k1vx/2.0 + m->k2vx + m->k3vx + m->k4vx/2.0)/3.0;
  314.         m->test_vy = m->cur_vy + (m->k1vy/2.0 + m->k2vy + m->k3vy + m->k4vy/2.0)/3.0;
  315.         } else {
  316.         m->x = m->cur_x + (m->k1x/2.0 + m->k2x + m->k3x + m->k4x/2.0)/3.0;
  317.         m->y = m->cur_y + (m->k1y/2.0 + m->k2y + m->k3y + m->k4y/2.0)/3.0;
  318.         m->vx = m->cur_vx + (m->k1vx/2.0 + m->k2vx + m->k3vx + m->k4vx/2.0)/3.0;
  319.         m->vy = m->cur_vy + (m->k1vy/2.0 + m->k2vy + m->k3vy + m->k4vy/2.0)/3.0;
  320.         }
  321.     }
  322.     }
  323. }
  324.  
  325. void adaptive_runge_kutta()
  326. {
  327.     int i;
  328.     mass *m;
  329.     double err, maxerr;
  330.  
  331.   restart:
  332.     if (mst.cur_dt > DT_MAX)
  333.       mst.cur_dt = DT_MAX;
  334.     if (mst.cur_dt < DT_MIN)
  335.       mst.cur_dt = DT_MIN;
  336.  
  337.     runge_kutta(mst.cur_dt/2.0, FALSE);
  338.     runge_kutta(mst.cur_dt/2.0, TRUE);
  339.  
  340.     for (i = 0; i < num_mass; i++) {
  341.     m = masses + i;
  342.     if ((m->status & S_ALIVE) && !(m->status & S_FIXED)) {
  343.         m->x = m->old_x;
  344.         m->y = m->old_y;
  345.         m->vx = m->old_vx;
  346.         m->vy = m->old_vy;
  347.     }
  348.     }
  349.     runge_kutta(mst.cur_dt, FALSE);
  350.  
  351.     /* Find error */
  352.     maxerr = 0.00001;
  353.     for (i = 0; i < num_mass; i++) {
  354.     m = masses + i;
  355.     if ((m->status & S_ALIVE) && !(m->status & S_FIXED)) {
  356.         err = fabs(m->x - m->test_x) + fabs(m->y - m->test_y) +
  357.           fabs(m->vx - m->test_vx) + fabs(m->vy - m->test_vy);
  358.  
  359.         if (err > maxerr) {
  360.         maxerr = err;
  361.         }
  362.     }
  363.     }
  364.  
  365.     /* Fudgy scale factor -- user controlled */
  366.     maxerr /= mst.cur_prec;
  367.  
  368.     if (maxerr < 1.0) {
  369.     mst.cur_dt *= 0.9 * exp(-log(maxerr)/8.0);
  370.     } else {
  371.     if (mst.cur_dt > DT_MIN) {
  372.         for (i = 0; i < num_mass; i++) {
  373.         m = masses + i;
  374.         if ((m->status & S_ALIVE) && !(m->status & S_FIXED)) {
  375.             m->x = m->old_x;
  376.             m->y = m->old_y;
  377.             m->vx = m->old_vx;
  378.             m->vy = m->old_vy;
  379.         }
  380.         }
  381.  
  382.         mst.cur_dt *= 0.9 * exp(-log(maxerr)/4.0);
  383.         
  384.         goto restart;
  385.     }
  386.     }
  387. }
  388.  
  389. boolean animate_obj()
  390. {
  391.     mass *m;
  392.     spring *s;
  393.     int i;
  394.     double stick_mag;
  395.     static int num_since = 0;
  396.     static double time_elapsed = 0.0;
  397.  
  398.     /* Save initial values */
  399.     for (i = 0; i < num_mass; i++) {
  400.     m = masses + i;
  401.     if ((m->status & S_ALIVE) && !(m->status & S_FIXED)) {
  402.         m->old_x = m->x;
  403.         m->old_y = m->y;
  404.         m->old_vx = m->vx;
  405.         m->old_vy = m->vy;
  406.     }
  407.     }
  408.  
  409.     if (mst.adaptive_step) {
  410.     boolean any_spring = FALSE;
  411.  
  412.     for (i = 0; i < num_spring; i++) {
  413.         s = springs + i;
  414.         if (s->status & S_ALIVE) {
  415.         any_spring = TRUE;
  416.         break;
  417.         }
  418.     }
  419.  
  420.     /* If no springs, then use dt=DEF_TSTEP */
  421.     if (any_spring) {
  422.         adaptive_runge_kutta();
  423.     } else {
  424.         runge_kutta(mst.cur_dt = DEF_TSTEP, FALSE);
  425.     }
  426.     } else {
  427.     runge_kutta(mst.cur_dt, FALSE);
  428.     }
  429.  
  430.     stick_mag = STICK_MAG * mst.cur_dt * mst.cur_stick;
  431.  
  432.     /* Crappy wall code */
  433.     for (i = 0; i < num_mass; i++) {
  434.     m = masses + i;
  435.  
  436.     if ((m->status & S_ALIVE) && !(m->status & S_FIXED)) {
  437.         /* Delete "exploded" objects */
  438.         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) {
  439.         delete_mass(i);
  440.         continue;
  441.         }
  442.  
  443.         /* Check if stuck to a wall */
  444.         if (m->old_vx == 0.0 && m->old_vy == 0.0) {
  445.         /* Left or right wall */
  446.         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)) {
  447.             if (ABS(m->vx) < stick_mag / m->mass) {
  448.             m->vx = m->vy = 0;
  449.             m->x = m->old_x;
  450.             m->y = m->old_y;
  451.  
  452.             continue;
  453.             }
  454.         } 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)) {
  455.             /* Top or bottom wall */
  456.             if (ABS(m->vy) < stick_mag / m->mass) {
  457.             m->vx = m->vy = 0;
  458.             m->x = m->old_x;
  459.             m->y = m->old_y;
  460.  
  461.             continue;
  462.             }            
  463.         }
  464.         }
  465.  
  466.         /* Bounce off left or right wall */
  467.         if (mst.w_left && m->x < m->radius && m->old_x >= m->radius) {
  468.         m->x = m->radius;
  469.  
  470.         if (m->vx < 0) {
  471.             m->vx = -m->vx * m->elastic;
  472.             m->vy *= m->elastic;
  473.             
  474.             /* Get stuck if not going fast enough */
  475.             if (m->vx > 0) {
  476.             m->vx -= STICK_MAG * mst.cur_stick / m->mass;
  477.             
  478.             if (m->vx < 0) {
  479.                 m->vx = m->vy = 0;
  480.             }
  481.             }
  482.         }
  483.         } else if (mst.w_right && m->x > draw_wid - m->radius && m->old_x <= draw_wid - m->radius) {
  484.         m->x = draw_wid - m->radius;
  485.         
  486.         if (m->vx > 0) {
  487.             m->vx = -m->vx * m->elastic;
  488.             m->vy *= m->elastic;
  489.             
  490.             /* Get stuck if not going fast enough */
  491.             if (m->vx < 0) {
  492.             m->vx += STICK_MAG * mst.cur_stick / m->mass;
  493.             
  494.             if (m->vx > 0) {
  495.                 m->vx = m->vy = 0;
  496.             }
  497.             }
  498.         }
  499.         }
  500.         /* Stick to top or bottom wall */
  501.         if (mst.w_bottom && m->y < m->radius && m->old_y >= m->radius) {
  502.         m->y = m->radius;
  503.         
  504.         if (m->vy < 0) {
  505.             m->vy = -m->vy * m->elastic;
  506.             m->vx *= m->elastic;
  507.             
  508.             /* Get stuck if not going fast enough */
  509.             if (m->vy > 0) {
  510.             m->vy -= STICK_MAG * mst.cur_stick / m->mass;
  511.             
  512.             if (m->vy < 0) {
  513.                 m->vx = m->vy = 0;
  514.             }
  515.             }
  516.         }
  517.         } else if (mst.w_top && m->y > (draw_ht - m->radius) && m->old_y <= (draw_ht - m->radius)) {
  518.         m->y = draw_ht - m->radius;
  519.  
  520.         if (m->vy > 0) {
  521.             m->vy = -m->vy * m->elastic;
  522.             m->vx *= m->elastic;
  523.             
  524.             /* Get stuck if not going fast enough */
  525.             if (m->vy < 0) {
  526.             m->vy += STICK_MAG * mst.cur_stick / m->mass;
  527.             
  528.             if (m->vy > 0) {
  529.                 m->vx = m->vy = 0;
  530.             }
  531.             }
  532.         }
  533.         }
  534.     }
  535.     }
  536.  
  537.     time_elapsed += mst.cur_dt;
  538.  
  539.     if (time_elapsed > 0.05) {
  540.         time_elapsed -= 0.05;
  541.         num_since = 0;
  542.         return TRUE;
  543.     }
  544.  
  545.     num_since++;
  546.  
  547.     if (num_since > 8) {
  548.         num_since = 0;
  549.         return TRUE;
  550.     }
  551.     return FALSE;
  552. }
  553.