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

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