git.mutantstargoat.com Git - dosdemo/blob - tools/ropesim/src/ropesim.c

   1 #include <stdio.h>
   2 #include <stdlib.h>
   3 #include "ropesim.h"
   4
   5 static void step(struct rsim_world *rsim, struct rsim_rope *rope, float dt);
   6
   7 int rsim_init(struct rsim_world *rsim)
   8 {
   9         rsim->ropes = 0;
  10         cgm_vcons(&rsim->grav, 0, -9.807, 0);
  11         rsim->damping = 0.5;
  12         return 0;
  13 }
  14
  15 void rsim_destroy(struct rsim_world *rsim)
  16 {
  17         while(rsim->ropes) {
  18                 struct rsim_rope *rope = rsim->ropes;
  19                 rsim->ropes = rsim->ropes->next;
  20                 rsim_free_rope(rope);
  21         }
  22 }
  23
  24 int rsim_add_rope(struct rsim_world *rsim, struct rsim_rope *rope)
  25 {
  26         rope->next = rsim->ropes;
  27         rsim->ropes = rope;
  28         return 0;
  29 }
  30
  31 /* actual step function, called by rsim_step in a loop to microstep or once if
  32  * microstepping is disabled
  33  */
  34 static void step(struct rsim_world *rsim, struct rsim_rope *rope, float dt)
  35 {
  36         int i;
  37         float len, fmag;
  38         cgm_vec3 npos, force, dir;
  39         float inv_damp = rsim->damping == 0.0f ? 1.0f : (1.0f - rsim->damping);
  40         struct rsim_mass *mass = rope->masses;
  41         struct rsim_spring *spr = rope->springs;
  42
  43         force.x = rsim->grav.x + rsim->extforce.x;
  44         force.y = rsim->grav.y + rsim->extforce.y;
  45         force.z = rsim->grav.z + rsim->extforce.z;
  46
  47         /* accumulate forces from springs */
  48         for(i=0; i<rope->num_springs; i++) {
  49                 dir = spr->mass[1]->p;
  50                 cgm_vsub(&dir, &spr->mass[0]->p);
  51
  52                 len = cgm_vlength(&dir);
  53                 if(len != 0.0f) {
  54                         float s = 1.0f / len;
  55                         dir.x *= s;
  56                         dir.y *= s;
  57                         dir.z *= s;
  58                 }
  59                 fmag = (len - spr->rest_len) * spr->k;
  60
  61                 cgm_vscale(&dir, fmag);
  62                 cgm_vadd(&spr->mass[0]->f, &dir);
  63                 cgm_vsub(&spr->mass[1]->f, &dir);
  64
  65                 spr++;
  66         }
  67
  68         /* update masses */
  69         for(i=0; i<rope->num_masses; i++) {
  70                 if(mass->fixed) {
  71                         mass++;
  72                         continue;
  73                 }
  74
  75                 /* add constant forces to accumulated mass force */
  76                 cgm_vadd(&mass->f, &force);
  77
  78                 mass->v.x = (mass->v.x - mass->v.x * inv_damp * dt) + mass->f.x * dt;
  79                 mass->v.y = (mass->v.y - mass->v.y * inv_damp * dt) + mass->f.y * dt;
  80                 mass->v.z = (mass->v.z - mass->v.z * inv_damp * dt) + mass->f.z * dt;
  81
  82                 /* zero out the accumulated forces for next iter */
  83                 mass->f.x = mass->f.y = mass->f.z = 0.0f;
  84
  85                 npos = mass->p;
  86                 npos.x = mass->p.x + mass->v.x * dt;
  87                 npos.y = mass->p.y + mass->v.y * dt;
  88                 npos.z = mass->p.z + mass->v.z * dt;
  89
  90                 /* TODO collisions */
  91
  92                 mass->p = npos;
  93                 mass++;
  94         }
  95 }
  96
  97 void rsim_step(struct rsim_world *rsim, float dt)
  98 {
  99         struct rsim_rope *rope = rsim->ropes;
 100
 101         if(rsim->udt > 0.0f) {
 102                 /* microstepping */
 103                 float dt_acc = rsim->udelta_acc;
 104
 105                 while(dt_acc < dt) {
 106                         while(rope) {
 107                                 step(rsim, rope, rsim->udt);
 108                                 rope = rope->next;
 109                         }
 110                         dt_acc += rsim->udt;
 111                 }
 112                 rsim->udelta_acc = dt_acc - dt;
 113         } else {
 114                 while(rope) {
 115                         step(rsim, rope, dt);
 116                         rope = rope->next;
 117                 }
 118         }
 119 }
 120
 121 struct rsim_rope *rsim_alloc_rope(int nmasses, int nsprings)
 122 {
 123         struct rsim_rope *rope;
 124
 125         if(!(rope = malloc(sizeof *rope))) {
 126                 return 0;
 127         }
 128         if(rsim_init_rope(rope, nmasses, nsprings) == -1) {
 129                 free(rope);
 130                 return 0;
 131         }
 132         return rope;
 133 }
 134
 135 void rsim_free_rope(struct rsim_rope *rope)
 136 {
 137         rsim_destroy_rope(rope);
 138         free(rope);
 139 }
 140
 141 int rsim_init_rope(struct rsim_rope *rope, int nmasses, int nsprings)
 142 {
 143         memset(rope, 0, sizeof *rope);
 144
 145         if(!(rope->masses = calloc(nmasses, sizeof *rope->masses))) {
 146                 return -1;
 147         }
 148         if(!(rope->springs = calloc(nsprings, sizeof *rope->springs))) {
 149                 free(rope->masses);
 150                 return -1;
 151         }
 152         rope->num_masses = nmasses;
 153         rope->num_springs = nsprings;
 154         return 0;
 155 }
 156
 157 void rsim_destroy_rope(struct rsim_rope *rope)
 158 {
 159         free(rope->masses);
 160         free(rope->springs);
 161 }
 162
 163 int rsim_freeze_rope_mass(struct rsim_rope *rope, struct rsim_mass *m)
 164 {
 165         if(m->fixed) return -1;
 166
 167         m->fixed = 1;
 168         m->next = rope->fixedlist;
 169         rope->fixedlist = m;
 170         return 0;
 171 }
 172
 173 int rsim_unfreeze_rope_mass(struct rsim_rope *rope, struct rsim_mass *m)
 174 {
 175         struct rsim_mass *it, dummy;
 176
 177         if(!m->fixed) return -1;
 178
 179         dummy.next = rope->fixedlist;
 180         it = &dummy;
 181         while(it->next) {
 182                 if(it->next == m) {
 183                         m->fixed = 0;
 184                         it->next = m->next;
 185                         m->next = 0;
 186                         break;
 187                 }
 188                 it = it->next;
 189         }
 190         rope->fixedlist = dummy.next;
 191
 192         return m->fixed ? -1 : 0;
 193 }