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.995;
  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         cgm_vec3 npos, f, force;
  38         float inv_damp = rsim->damping == 0.0f ? 1.0f : 1.0f / rsim->damping;
  39         struct rsim_mass *mass = rope->masses;
  40
  41         force.x = rsim->grav.x + rsim->extforce.x;
  42         force.y = rsim->grav.y + rsim->extforce.y;
  43         force.z = rsim->grav.z + rsim->extforce.z;
  44
  45         for(i=0; i<rope->num_masses; i++) {
  46                 if(mass->fixed) {
  47                         mass++;
  48                         continue;
  49                 }
  50
  51                 npos = mass->p;
  52
  53                 npos.x = mass->p.x + mass->v.x * dt;
  54                 npos.y = mass->p.y + mass->v.y * dt;
  55                 npos.z = mass->p.z + mass->v.z * dt;
  56
  57                 f.x = force.x;/* + spring forces */
  58                 f.y = force.y;
  59                 f.z = force.z;
  60
  61                 mass->v.x = ((mass->v.x - mass->v.x * inv_damp) + f.x) * dt;
  62                 mass->v.y = ((mass->v.y - mass->v.y * inv_damp) + f.y) * dt;
  63                 mass->v.z = ((mass->v.z - mass->v.z * inv_damp) + f.z) * dt;
  64
  65                 /* TODO collisions */
  66
  67                 mass->p = npos;
  68                 mass++;
  69         }
  70 }
  71
  72 void rsim_step(struct rsim_world *rsim, float dt)
  73 {
  74         struct rsim_rope *rope = rsim->ropes;
  75
  76         if(rsim->udt > 0.0f) {
  77                 /* microstepping */
  78                 float dt_acc = rsim->udelta_acc;
  79
  80                 while(dt_acc < dt) {
  81                         while(rope) {
  82                                 step(rsim, rope, rsim->udt);
  83                                 rope = rope->next;
  84                         }
  85                         dt_acc += rsim->udt;
  86                 }
  87                 rsim->udelta_acc = dt_acc - dt;
  88         } else {
  89                 while(rope) {
  90                         step(rsim, rope, dt);
  91                         rope = rope->next;
  92                 }
  93         }
  94 }
  95
  96 struct rsim_rope *rsim_alloc_rope(int nmasses, int nsprings)
  97 {
  98         struct rsim_rope *rope;
  99
 100         if(!(rope = malloc(sizeof *rope))) {
 101                 return 0;
 102         }
 103         if(rsim_init_rope(rope, nmasses, nsprings) == -1) {
 104                 free(rope);
 105                 return 0;
 106         }
 107         return rope;
 108 }
 109
 110 void rsim_free_rope(struct rsim_rope *rope)
 111 {
 112         rsim_destroy_rope(rope);
 113         free(rope);
 114 }
 115
 116 int rsim_init_rope(struct rsim_rope *rope, int nmasses, int nsprings)
 117 {
 118         memset(rope, 0, sizeof *rope);
 119
 120         if(!(rope->masses = calloc(nmasses, sizeof *rope->masses))) {
 121                 return -1;
 122         }
 123         if(!(rope->springs = calloc(nsprings, sizeof *rope->springs))) {
 124                 free(rope->masses);
 125                 return -1;
 126         }
 127         rope->num_masses = nmasses;
 128         rope->num_springs = nsprings;
 129         return 0;
 130 }
 131
 132 void rsim_destroy_rope(struct rsim_rope *rope)
 133 {
 134         free(rope->masses);
 135         free(rope->springs);
 136 }
 137
 138 int rsim_freeze_rope_mass(struct rsim_rope *rope, struct rsim_mass *m)
 139 {
 140         if(m->fixed) return -1;
 141
 142         m->fixed = 1;
 143         m->next = rope->fixedlist;
 144         rope->fixedlist = m;
 145         return 0;
 146 }
 147
 148 int rsim_unfreeze_rope_mass(struct rsim_rope *rope, struct rsim_mass *m)
 149 {
 150         struct rsim_mass *it, dummy;
 151
 152         if(!m->fixed) return -1;
 153
 154         dummy.next = rope->fixedlist;
 155         it = &dummy;
 156         while(it->next) {
 157                 if(it->next == m) {
 158                         m->fixed = 0;
 159                         it->next = m->next;
 160                         m->next = 0;
 161                         break;
 162                 }
 163                 it = it->next;
 164         }
 165         rope->fixedlist = dummy.next;
 166
 167         return m->fixed ? -1 : 0;
 168 }