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