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, faccel, 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         /* accumulate forces from springs */
  44         for(i=0; i<rope->num_springs; i++) {
  45                 dir = spr->mass[1]->p;
  46                 cgm_vsub(&dir, &spr->mass[0]->p);
  47
  48                 len = cgm_vlength(&dir);
  49                 if(len != 0.0f) {
  50                         float s = 1.0f / len;
  51                         dir.x *= s;
  52                         dir.y *= s;
  53                         dir.z *= s;
  54                 }
  55                 fmag = (len - spr->rest_len) * spr->k;
  56
  57                 spr->mass[0]->f.x += dir.x * fmag / spr->mass[0]->m;
  58                 spr->mass[0]->f.y += dir.y * fmag / spr->mass[0]->m;
  59                 spr->mass[0]->f.z += dir.z * fmag / spr->mass[0]->m;
  60
  61                 spr->mass[1]->f.x -= dir.x * fmag / spr->mass[1]->m;
  62                 spr->mass[1]->f.y -= dir.y * fmag / spr->mass[1]->m;
  63                 spr->mass[1]->f.z -= dir.z * fmag / spr->mass[1]->m;
  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, &rsim->grav);
  77
  78                 faccel = rsim->extforce;
  79                 cgm_vscale(&faccel, 1.0f / mass->m);
  80                 cgm_vadd(&mass->f, &rsim->extforce);
  81
  82                 mass->v.x = (mass->v.x - mass->v.x * inv_damp * dt) + mass->f.x * dt;
  83                 mass->v.y = (mass->v.y - mass->v.y * inv_damp * dt) + mass->f.y * dt;
  84                 mass->v.z = (mass->v.z - mass->v.z * inv_damp * dt) + mass->f.z * dt;
  85
  86                 /* zero out the accumulated forces for next iter */
  87                 mass->f.x = mass->f.y = mass->f.z = 0.0f;
  88
  89                 npos = mass->p;
  90                 npos.x = mass->p.x + mass->v.x * dt;
  91                 npos.y = mass->p.y + mass->v.y * dt;
  92                 npos.z = mass->p.z + mass->v.z * dt;
  93
  94                 /* TODO collisions */
  95
  96                 mass->p = npos;
  97                 mass++;
  98         }
  99 }
 100
 101 void rsim_step(struct rsim_world *rsim, float dt)
 102 {
 103         struct rsim_rope *rope = rsim->ropes;
 104
 105         if(rsim->udt > 0.0f) {
 106                 /* microstepping */
 107                 float dt_acc = rsim->udelta_acc;
 108
 109                 while(dt_acc < dt) {
 110                         while(rope) {
 111                                 step(rsim, rope, rsim->udt);
 112                                 rope = rope->next;
 113                         }
 114                         dt_acc += rsim->udt;
 115                 }
 116                 rsim->udelta_acc = dt_acc - dt;
 117         } else {
 118                 while(rope) {
 119                         step(rsim, rope, dt);
 120                         rope = rope->next;
 121                 }
 122         }
 123 }
 124
 125 struct rsim_rope *rsim_alloc_rope(int nmasses, int nsprings)
 126 {
 127         struct rsim_rope *rope;
 128
 129         if(!(rope = malloc(sizeof *rope))) {
 130                 return 0;
 131         }
 132         if(rsim_init_rope(rope, nmasses, nsprings) == -1) {
 133                 free(rope);
 134                 return 0;
 135         }
 136         return rope;
 137 }
 138
 139 void rsim_free_rope(struct rsim_rope *rope)
 140 {
 141         rsim_destroy_rope(rope);
 142         free(rope);
 143 }
 144
 145 int rsim_init_rope(struct rsim_rope *rope, int nmasses, int nsprings)
 146 {
 147         memset(rope, 0, sizeof *rope);
 148
 149         if(!(rope->masses = calloc(nmasses, sizeof *rope->masses))) {
 150                 return -1;
 151         }
 152         if(!(rope->springs = calloc(nsprings, sizeof *rope->springs))) {
 153                 free(rope->masses);
 154                 return -1;
 155         }
 156         rope->num_masses = nmasses;
 157         rope->num_springs = nsprings;
 158         return 0;
 159 }
 160
 161 void rsim_destroy_rope(struct rsim_rope *rope)
 162 {
 163         free(rope->masses);
 164         free(rope->springs);
 165 }
 166
 167 int rsim_freeze_rope_mass(struct rsim_rope *rope, struct rsim_mass *m)
 168 {
 169         if(m->fixed) return -1;
 170
 171         m->fixed = 1;
 172         m->next = rope->fixedlist;
 173         rope->fixedlist = m;
 174         return 0;
 175 }
 176
 177 int rsim_unfreeze_rope_mass(struct rsim_rope *rope, struct rsim_mass *m)
 178 {
 179         struct rsim_mass *it, dummy;
 180
 181         if(!m->fixed) return -1;
 182
 183         dummy.next = rope->fixedlist;
 184         it = &dummy;
 185         while(it->next) {
 186                 if(it->next == m) {
 187                         m->fixed = 0;
 188                         it->next = m->next;
 189                         m->next = 0;
 190                         break;
 191                 }
 192                 it = it->next;
 193         }
 194         rope->fixedlist = dummy.next;
 195
 196         return m->fixed ? -1 : 0;
 197 }