home *** CD-ROM | disk | FTP | other *** search
/ Fresh Fish 8 / FreshFishVol8-CD2.bin / bbs / misc / aspringies-1.0.lha / ASpringies / src / phys.c < prev    next >
Encoding:
C/C++ Source or Header  |  1994-08-24  |  13.3 KB  |  554 lines
  1. /* phys.c -- ASpringies physical modeling and numerical solving routines
  2.  * Copyright (C) 1991  Douglas M. DeCarlo
  3.  *
  4.  * Modifications for the Amiga port Copyright (C) 1994  Torsten Klein
  5.  *
  6.  * This file is part of ASpringies, a mass and spring simulation system for the Amiga
  7.  *
  8.  * ASpringies is free software; you can redistribute it and/or modify
  9.  * it under the terms of the GNU General Public License as published by
  10.  * the Free Software Foundation; either version 1, or (at your option)
  11.  * any later version.
  12.  *
  13.  * ASpringies is distributed in the hope that it will be useful,
  14.  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  15.  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  16.  * GNU General Public License for more details.
  17.  *
  18.  * You should have received a copy of the GNU General Public License
  19.  * along with ASpringies; see the file COPYING.  If not, write to
  20.  * the Free Software Foundation, 675 Mass Ave, Cambridge, MA 02139, USA.
  21.  *
  22.  * $Id: phys.c,v 1.4 1994/06/26 20:48:57 Torsten_Klein Exp $
  23.  */
  24.  
  25. #include "defs.h"
  26. #include "obj.h"
  27.  
  28. #ifndef M_PI
  29. #define M_PI    3.14159265358979323846
  30. #endif
  31.  
  32. #define DT_MIN        0.0001
  33. #define DT_MAX        0.5
  34.  
  35. #define MAXCON        1024
  36.  
  37. #define CONSTR_KS    1.0
  38. #define CONSTR_KD    1.0
  39.  
  40. /* Do fudgy bounces */
  41. #define BOUNCE_FUDGE    1
  42.  
  43. /* Stickiness calibration:  STICK_MAG = 1.0, means that a mass = 1.0 with gravity = 1.0 will remain
  44.    stuck on a wall for all stickiness values > 1.0 */
  45. #define STICK_MAG    1.0
  46.  
  47. void accumulate_accel(void)
  48. {
  49.     double gval, gmisc;
  50.     double gx = 0, gy = 0, ogx = 0, ogy = 0;
  51.     double center_x = draw_wid/2.0, center_y = draw_ht/2.0, center_rad = 1.0;
  52.     register mass *m;
  53.     register spring *s;
  54.     register int i;
  55.  
  56.     /* ------------------ applied force effects ----------------------- */
  57.  
  58.     if (mst.center_id >= 0) {
  59.     if (masses[mst.center_id].status & S_ALIVE) {
  60.         center_x = masses[mst.center_id].x;
  61.         center_y = masses[mst.center_id].y;
  62.     } else {
  63.         mst.center_id = -1;
  64.     }
  65.     }
  66.  
  67.     /* Do gravity */
  68.     if (mst.bf_mode[FR_GRAV] > 0) {
  69.     gval = mst.cur_grav_val[FR_GRAV];
  70.     gmisc = mst.cur_misc_val[FR_GRAV] * M_PI / 180;
  71.  
  72.     gx = COORD_DX(gval * sin(gmisc));
  73.     gy = COORD_DY(gval * cos(gmisc));
  74.     }
  75.  
  76.     /* Keep center of mass in the middle force */
  77.     if (mst.bf_mode[FR_CMASS] > 0) {
  78.     double mixix = 0.0, mixiy = 0.0, mivix = 0.0, miviy = 0.0, msum = 0.0;
  79.     gval = mst.cur_grav_val[FR_CMASS];
  80.     gmisc = mst.cur_misc_val[FR_CMASS];
  81.  
  82.     for (i = 0, m=masses; i < num_mass; i++, m++) {
  83.         if (i != mst.center_id && (m->status & S_ALIVE) && !(m->status & S_FIXED)) {
  84.         msum += m->mass;
  85.         mixix += m->mass * m->x;
  86.         mixiy += m->mass * m->y;
  87.         mivix += m->mass * m->vx;
  88.         miviy += m->mass * m->vy;
  89.         }
  90.     }
  91.     
  92.     if (msum) {
  93.         mixix /= msum;
  94.         mixiy /= msum;
  95.         mivix /= msum;
  96.         miviy /= msum;
  97.     
  98.         mixix -= center_x;
  99.         mixiy -= center_y;
  100.  
  101.         ogx -= (gval * mixix + gmisc * mivix) / msum;
  102.         ogy -= (gval * mixiy + gmisc * miviy) / msum;
  103.     }
  104.     }
  105.  
  106.     /* Apply Gravity, CM and air drag to all masses */
  107.     m = masses;
  108.     for (i = 0; i < num_mass; i++, m++) {
  109.     if ((m->status & S_ALIVE) && !(m->status & S_FIXED)) {
  110.         /* Do viscous drag */
  111.         if (i != mst.center_id) {
  112.         m->ax = gx + ogx - mst.cur_visc * m->vx;
  113.         m->ay = gy + ogy - mst.cur_visc * m->vy;
  114.         } else {
  115.         m->ax = gx - mst.cur_visc * m->vx;
  116.         m->ay = gy - mst.cur_visc * m->vy;
  117.         }
  118.     }
  119.     }
  120.  
  121.     /* Do point attraction force */
  122.     if (mst.bf_mode[FR_PTATTRACT] > 0) {
  123.     gval = mst.cur_grav_val[FR_PTATTRACT];
  124.     gmisc = mst.cur_misc_val[FR_PTATTRACT];
  125.     
  126.     for (i = 0, m = masses; i < num_mass; i++, m++) {
  127.         if ((m->status & S_ALIVE) && !(m->status & S_FIXED)) {
  128.         double dx, dy, mag, fmag, rubi;
  129.         
  130.         dx = (center_x - m->x);
  131.         dy = (center_y - m->y);
  132.         mag = sqrt(dx * dx + dy * dy);
  133.         
  134.         if (mag < m->radius + center_rad) {
  135.             rubi = mag / (m->radius + center_rad);
  136.             dx *= rubi;
  137.             dy *= rubi;
  138.             mag = m->radius + center_rad;
  139.         }
  140.  
  141. if (gmisc!=0.0)        fmag = gval / pow(mag, gmisc);
  142. else                    fmag = gval;
  143.         m->ax += fmag * dx / mag;
  144.         m->ay += fmag * dy / mag;
  145.         }
  146.     }
  147.     }
  148.  
  149.     /* Wall attract/repel force */
  150.     if (mst.bf_mode[FR_WALL] > 0) {
  151.     double dax, day, dist;
  152.     
  153.     gval = -mst.cur_grav_val[FR_WALL];
  154.     gmisc = mst.cur_misc_val[FR_WALL];
  155.     
  156.     m = masses;
  157.     for (i = 0; i < num_mass; i++, m++) {
  158.         if ((m->status & S_ALIVE) && !(m->status & S_FIXED)) {
  159.         dax = day = 0;
  160.         
  161.         if (mst.w_left && (dist = m->x - m->radius) >= 0) {
  162.             if (dist < 1)  dist = 1;
  163. if (gmisc!=1.0)
  164.             dist = pow(dist, gmisc);
  165.             dax -= gval / dist;
  166.         }
  167.         if (mst.w_right && (dist = draw_wid - m->radius - m->x) >= 0) {
  168.             if (dist < 1)  dist = 1;
  169. if (gmisc!=1.0)
  170.             dist = pow(dist, gmisc);
  171.             dax += gval / dist;
  172.         }
  173.         if (mst.w_top && (dist = draw_ht - m->radius - m->y) >= 0) {
  174.             if (dist < 1)  dist = 1;
  175. if (gmisc!=1.0)
  176.             dist = pow(dist, gmisc);
  177.             day += gval / dist;
  178.         }
  179.         if (mst.w_bottom && (dist = m->y - m->radius) >= 0) {
  180.             if (dist < 1)  dist = 1;
  181. if (gmisc!=1.0)
  182.             dist = pow(dist, gmisc);
  183.             day -= gval / dist;
  184.         }        
  185.         m->ax += dax;
  186.         m->ay += day;
  187.         }
  188.     }
  189.     }
  190.  
  191.     /* ------------------ spring effects ----------------------- */
  192.  
  193.     /* Spring compression/damping effects on masses */
  194.     s = springs;
  195.     for (i = 0; i < num_spring; i++, s++) {
  196.     if (s->status & S_ALIVE) {
  197.         register mass *m1, *m2;
  198.         double dx, dy, force, forcex, forcey, mag, damp;
  199.     
  200.         m1 = masses + s->m1;
  201.         m2 = masses + s->m2;
  202.     
  203.         dx = m1->x - m2->x;
  204.         dy = m1->y - m2->y;
  205.     
  206.         if (dx || dy) {
  207.         mag = sqrt(dx * dx + dy * dy);
  208.         
  209.         force = s->ks * (s->restlen - mag);
  210.         if (s->kd) {
  211.             damp = ((m1->vx - m2->vx) * dx + (m1->vy - m2->vy) * dy) / mag;
  212.             force -= s->kd * damp;
  213.         }
  214.         
  215.         force /= mag;
  216.         forcex = force * dx;
  217.         forcey = force * dy;
  218.         
  219.         m1->ax += forcex / m1->mass;
  220.         m1->ay += forcey / m1->mass;
  221.         m2->ax -= forcex / m2->mass;
  222.         m2->ay -= forcey / m2->mass;
  223.         }
  224.     }
  225.     }
  226. }
  227.  
  228. void runge_kutta(double h, boolean testloc)
  229. {
  230.     register mass *m;
  231.     register int i;
  232.  
  233.     accumulate_accel();
  234.  
  235.     /* k1 step */
  236.     m = masses;
  237.     for (i = 0; i < num_mass; i++, m++) {
  238.  
  239.     if ((m->status & S_ALIVE) && !(m->status & S_FIXED)) {
  240.         /* Initial storage */
  241.         m->cur_x = m->x;
  242.         m->cur_y = m->y;
  243.         m->cur_vx = m->vx;
  244.         m->cur_vy = m->vy;
  245.  
  246.         m->k1x = m->vx * h;
  247.         m->k1y = m->vy * h;
  248.         m->k1vx = m->ax * h;
  249.         m->k1vy = m->ay * h;
  250.     
  251.         m->x = m->cur_x + m->k1x / 2;
  252.         m->y = m->cur_y + m->k1y / 2;
  253.         m->vx = m->cur_vx + m->k1vx / 2;
  254.         m->vy = m->cur_vy + m->k1vy / 2;
  255.     }
  256.     }
  257.  
  258.     accumulate_accel();
  259.  
  260.     /* k2 step */
  261.     m = masses;
  262.     for (i = 0; i < num_mass; i++, m++) {
  263.     if ((m->status & S_ALIVE) && !(m->status & S_FIXED)) {
  264.         m->k2x = m->vx * h;
  265.         m->k2y = m->vy * h;
  266.         m->k2vx = m->ax * h;
  267.         m->k2vy = m->ay * h;
  268.     
  269.         m->x = m->cur_x + m->k2x / 2;
  270.         m->y = m->cur_y + m->k2y / 2;
  271.         m->vx = m->cur_vx + m->k2vx / 2;
  272.         m->vy = m->cur_vy + m->k2vy / 2;
  273.     }
  274.     }
  275.  
  276.     accumulate_accel();
  277.  
  278.     /* k3 step */
  279.     m = masses;
  280.     for (i = 0; i < num_mass; i++, m++) {
  281.     if ((m->status & S_ALIVE) && !(m->status & S_FIXED)) {
  282.         m->k3x = m->vx * h;
  283.         m->k3y = m->vy * h;
  284.         m->k3vx = m->ax * h;
  285.         m->k3vy = m->ay * h;
  286.     
  287.         m->x = m->cur_x + m->k3x;
  288.         m->y = m->cur_y + m->k3y;
  289.         m->vx = m->cur_vx + m->k3vx;
  290.         m->vy = m->cur_vy + m->k3vy;
  291.     }
  292.     }
  293.  
  294.     accumulate_accel();
  295.  
  296.     /* k4 step */
  297.     m = masses;
  298.     for (i = 0; i < num_mass; i++, m++) {
  299.     if ((m->status & S_ALIVE) && !(m->status & S_FIXED)) {
  300.         m->k4x = m->vx * h;
  301.         m->k4y = m->vy * h;
  302.         m->k4vx = m->ax * h;
  303.         m->k4vy = m->ay * h;
  304.     }
  305.     }
  306.  
  307.     /* Find next position */
  308.     m = masses;
  309.     for (i = 0; i < num_mass; i++, m++) {
  310.     if ((m->status & S_ALIVE) && !(m->status & S_FIXED)) {
  311.         if (testloc) {
  312.           m->test_x = m->cur_x + (m->k1x/2.0 + m->k2x + m->k3x + m->k4x/2.0)/3.0;
  313.           m->test_y = m->cur_y + (m->k1y/2.0 + m->k2y + m->k3y + m->k4y/2.0)/3.0;
  314.           m->test_vx = m->cur_vx + (m->k1vx/2.0 + m->k2vx + m->k3vx + m->k4vx/2.0)/3.0;
  315.           m->test_vy = m->cur_vy + (m->k1vy/2.0 + m->k2vy + m->k3vy + m->k4vy/2.0)/3.0;
  316.         } else {
  317.           m->x = m->cur_x + (m->k1x/2.0 + m->k2x + m->k3x + m->k4x/2.0)/3.0;
  318.           m->y = m->cur_y + (m->k1y/2.0 + m->k2y + m->k3y + m->k4y/2.0)/3.0;
  319.           m->vx = m->cur_vx + (m->k1vx/2.0 + m->k2vx + m->k3vx + m->k4vx/2.0)/3.0;
  320.           m->vy = m->cur_vy + (m->k1vy/2.0 + m->k2vy + m->k3vy + m->k4vy/2.0)/3.0;
  321.         }
  322.     }
  323.     }
  324. }
  325.  
  326. void adaptive_runge_kutta(void)
  327. {
  328.     register int i;
  329.     register mass *m;
  330.     double err, maxerr;
  331.  
  332.   restart:
  333.     if (mst.cur_dt > DT_MAX)
  334.       mst.cur_dt = DT_MAX;
  335.     if (mst.cur_dt < DT_MIN)
  336.       mst.cur_dt = DT_MIN;
  337.  
  338.     runge_kutta(mst.cur_dt/2.0, FALSE);
  339.     runge_kutta(mst.cur_dt/2.0, TRUE);
  340.  
  341.     m = masses;
  342.     for (i = 0; i < num_mass; i++, m++) {
  343.     if ((m->status & S_ALIVE) && !(m->status & S_FIXED)) {
  344.         m->x = m->old_x;
  345.         m->y = m->old_y;
  346.         m->vx = m->old_vx;
  347.         m->vy = m->old_vy;
  348.     }
  349.     }
  350.     runge_kutta(mst.cur_dt, FALSE);
  351.  
  352.     /* Find error */
  353.     maxerr = 0.00001;
  354.     m = masses;
  355.     for (i = 0; i < num_mass; i++, m++) {
  356.     if ((m->status & S_ALIVE) && !(m->status & S_FIXED)) {
  357.         err = fabs(m->x - m->test_x) + fabs(m->y - m->test_y) +
  358.           fabs(m->vx - m->test_vx) + fabs(m->vy - m->test_vy);
  359.  
  360.         if (err > maxerr) {
  361.         maxerr = err;
  362.         }
  363.     }
  364.     }
  365.  
  366.     /* Fudgy scale factor -- user controlled */
  367.     maxerr /= mst.cur_prec;
  368.  
  369.     if (maxerr < 1.0) {
  370.     mst.cur_dt *= 0.9 * exp(-log(maxerr)/8.0);
  371.     } else {
  372.     if (mst.cur_dt > DT_MIN) {
  373.         for (i = 0; i < num_mass; i++) {
  374.         m = masses + i;
  375.         if ((m->status & S_ALIVE) && !(m->status & S_FIXED)) {
  376.             m->x = m->old_x;
  377.             m->y = m->old_y;
  378.             m->vx = m->old_vx;
  379.             m->vy = m->old_vy;
  380.         }
  381.         }
  382.  
  383.         mst.cur_dt *= 0.9 * exp(-log(maxerr)/4.0);
  384.     
  385.         goto restart;
  386.     }
  387.     }
  388. }
  389.  
  390. boolean animate_obj(void)
  391. {
  392.     register  mass *m;
  393.     register  spring *s;
  394.     register  int i;
  395.     double stick_mag;
  396.     static int num_since = 0;
  397.     static double time_elapsed = 0.0;
  398.  
  399.     /* Save initial values */
  400.     m = masses;
  401.     for (i = 0; i < num_mass; i++, m++) {
  402.     if ((m->status & S_ALIVE) && !(m->status & S_FIXED)) {
  403.         m->old_x = m->x;
  404.         m->old_y = m->y;
  405.         m->old_vx = m->vx;
  406.         m->old_vy = m->vy;
  407.     }
  408.     }
  409.  
  410.     if (mst.adaptive_step) {
  411.     boolean any_spring = FALSE;
  412.     
  413.     s = springs;
  414.     for (i = 0; i < num_spring; i++, s++) {
  415.         if (s->status & S_ALIVE) {
  416.         any_spring = TRUE;
  417.         break;
  418.         }
  419.     }
  420.  
  421.     /* If no springs, then use dt=DEF_TSTEP */
  422.     if (any_spring) {
  423.         adaptive_runge_kutta();
  424.     } else {
  425.         runge_kutta(mst.cur_dt = DEF_TSTEP, FALSE);
  426.     }
  427.     } else {
  428.     runge_kutta(mst.cur_dt, FALSE);
  429.     }
  430.  
  431.     stick_mag = STICK_MAG * mst.cur_dt * mst.cur_stick;
  432.  
  433.     /* Crappy wall code */
  434.     m = masses;
  435.     for (i = 0; i < num_mass; i++, m++) {
  436.  
  437.     if ((m->status & S_ALIVE) && !(m->status & S_FIXED)) {
  438.         /* Delete "exploded" objects */
  439.         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) {
  440.         delete_mass(i);
  441.         continue;
  442.         }
  443.  
  444.         /* Check if stuck to a wall */
  445.         if (m->old_vx == 0.0 && m->old_vy == 0.0) {
  446.         /* Left or right wall */
  447.         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)) {
  448.             if (ABS(m->vx) < stick_mag / m->mass) {
  449.             m->vx = m->vy = 0;
  450.             m->x = m->old_x;
  451.             m->y = m->old_y;
  452.  
  453.             continue;
  454.             }
  455.         } 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)) {
  456.             /* Top or bottom wall */
  457.             if (ABS(m->vy) < stick_mag / m->mass) {
  458.             m->vx = m->vy = 0;
  459.             m->x = m->old_x;
  460.             m->y = m->old_y;
  461.  
  462.             continue;
  463.             }        
  464.         }
  465.         }
  466.  
  467.         /* Bounce off left or right wall */
  468.         if (mst.w_left && m->x < m->radius && m->old_x >= m->radius) {
  469.         m->x = m->radius;
  470.  
  471.         if (m->vx < 0) {
  472.             m->vx = -m->vx * m->elastic;
  473.             m->vy *= m->elastic;
  474.         
  475.             /* Get stuck if not going fast enough */
  476.             if (m->vx > 0) {
  477.             m->vx -= STICK_MAG * mst.cur_stick / m->mass;
  478.             
  479.             if (m->vx < 0) {
  480.                 m->vx = m->vy = 0;
  481.             }
  482.             }
  483.         }
  484.         } else if (mst.w_right && m->x > draw_wid - m->radius && m->old_x <= draw_wid - m->radius) {
  485.         m->x = draw_wid - m->radius;
  486.         
  487.         if (m->vx > 0) {
  488.             m->vx = -m->vx * m->elastic;
  489.             m->vy *= m->elastic;
  490.         
  491.             /* Get stuck if not going fast enough */
  492.             if (m->vx < 0) {
  493.             m->vx += STICK_MAG * mst.cur_stick / m->mass;
  494.             
  495.             if (m->vx > 0) {
  496.                 m->vx = m->vy = 0;
  497.             }
  498.             }
  499.         }
  500.         }
  501.         /* Stick to top or bottom wall */
  502.         if (mst.w_bottom && m->y < m->radius && m->old_y >= m->radius) {
  503.         m->y = m->radius;
  504.         
  505.         if (m->vy < 0) {
  506.             m->vy = -m->vy * m->elastic;
  507.             m->vx *= m->elastic;
  508.         
  509.             /* Get stuck if not going fast enough */
  510.             if (m->vy > 0) {
  511.             m->vy -= STICK_MAG * mst.cur_stick / m->mass;
  512.             
  513.             if (m->vy < 0) {
  514.                 m->vx = m->vy = 0;
  515.             }
  516.             }
  517.         }
  518.         } else if (mst.w_top && m->y > (draw_ht - m->radius) && m->old_y <= (draw_ht - m->radius)) {
  519.         m->y = draw_ht - m->radius;
  520.  
  521.         if (m->vy > 0) {
  522.             m->vy = -m->vy * m->elastic;
  523.             m->vx *= m->elastic;
  524.         
  525.             /* Get stuck if not going fast enough */
  526.             if (m->vy < 0) {
  527.             m->vy += STICK_MAG * mst.cur_stick / m->mass;
  528.             
  529.             if (m->vy > 0) {
  530.                 m->vx = m->vy = 0;
  531.             }
  532.             }
  533.         }
  534.         }
  535.     }
  536.     }
  537.  
  538.     time_elapsed += mst.cur_dt;
  539.  
  540.     if (time_elapsed > 0.05) {
  541.         time_elapsed -= 0.05;
  542.         num_since = 0;
  543.         return TRUE;
  544.     }
  545.  
  546.     num_since++;
  547.  
  548.     if (num_since > 8) {
  549.         num_since = 0;
  550.         return TRUE;
  551.     }
  552.     return FALSE;
  553. }
  554.