6 static void step(struct rsim_world *rsim, struct rsim_rope *rope, float dt);
8 int rsim_init(struct rsim_world *rsim)
11 cgm_vcons(&rsim->grav, 0, -9.807, 0);
16 void rsim_destroy(struct rsim_world *rsim)
19 struct rsim_rope *rope = rsim->ropes;
20 rsim->ropes = rsim->ropes->next;
25 int rsim_add_rope(struct rsim_world *rsim, struct rsim_rope *rope)
27 rope->next = rsim->ropes;
32 static inline struct rsim_spring *getspring(struct rsim_rope *rope, int aidx, int bidx)
34 struct rsim_spring *spr = rope->springs + aidx * rope->num_masses + bidx;
35 return *(uint32_t*)&spr->rest_len == 0xffffffff ? 0 : spr;
38 /* actual step function, called by rsim_step in a loop to microstep or once if
39 * microstepping is disabled
41 static void step(struct rsim_world *rsim, struct rsim_rope *rope, float dt)
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;
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))) {
57 dir = rope->masses[i].p;
58 cgm_vsub(&dir, &rope->masses[j].p);
60 len = cgm_vlength(&dir);
65 fmag = (len - spr->rest_len) * spr->k;
67 cgm_vscale(&dir, fmag / rope->masses[j].m);
68 cgm_vadd(&rope->masses[j].f, &dir);
74 for(i=0; i<rope->num_masses; i++) {
80 /* add constant forces to accumulated mass force */
81 cgm_vadd(&mass->f, &rsim->grav);
83 faccel = rsim->extforce;
84 cgm_vscale(&faccel, 1.0f / mass->m);
85 cgm_vadd(&mass->f, &rsim->extforce);
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;
91 /* zero out the accumulated forces for next iter */
92 mass->f.x = mass->f.y = mass->f.z = 0.0f;
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;
106 void rsim_step(struct rsim_world *rsim, float dt)
108 struct rsim_rope *rope = rsim->ropes;
110 if(rsim->udt > 0.0f) {
112 float dt_acc = rsim->udelta_acc;
116 step(rsim, rope, rsim->udt);
121 rsim->udelta_acc = dt_acc - dt;
124 step(rsim, rope, dt);
130 struct rsim_rope *rsim_alloc_rope(int nmasses)
132 struct rsim_rope *rope;
134 if(!(rope = malloc(sizeof *rope))) {
137 if(rsim_init_rope(rope, nmasses) == -1) {
144 void rsim_free_rope(struct rsim_rope *rope)
146 rsim_destroy_rope(rope);
150 int rsim_init_rope(struct rsim_rope *rope, int nmasses)
152 memset(rope, 0, sizeof *rope);
154 if(!(rope->masses = calloc(nmasses, sizeof *rope->masses))) {
157 rope->num_masses = nmasses;
159 if(!(rope->springs = malloc(nmasses * nmasses * sizeof *rope->springs))) {
163 memset(rope->springs, 0xff, nmasses * nmasses * sizeof *rope->springs);
167 void rsim_destroy_rope(struct rsim_rope *rope)
173 int rsim_set_rope_spring(struct rsim_rope *rope, int ma, int mb, float k, float rlen)
176 struct rsim_spring *spr;
178 if(ma == mb || ma < 0 || ma >= rope->num_masses || mb < 0 || mb >= rope->num_masses) {
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);
188 spr = rope->springs + ma * rope->num_masses + mb;
190 spr->rest_len = rlen;
194 int rsim_have_spring(struct rsim_rope *rope, int ma, int mb)
196 return getspring(rope, ma, mb) ? 1 : 0;
199 int rsim_freeze_rope_mass(struct rsim_rope *rope, struct rsim_mass *m)
201 if(m->fixed) return -1;
207 int rsim_unfreeze_rope_mass(struct rsim_rope *rope, struct rsim_mass *m)
209 if(!m->fixed) return -1;