33279be8174056bef151075dc9cd6429813eb3de
[dosdemo] / 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 }