#include <stdio.h>
#include <stdlib.h>
+#include <stdint.h>
#include "ropesim.h"
static void step(struct rsim_world *rsim, struct rsim_rope *rope, float dt);
{
rsim->ropes = 0;
cgm_vcons(&rsim->grav, 0, -9.807, 0);
- rsim->damping = 0.995;
+ rsim->damping = 0.5;
return 0;
}
return 0;
}
+static inline struct rsim_spring *getspring(struct rsim_rope *rope, int aidx, int bidx)
+{
+ struct rsim_spring *spr = rope->springs + aidx * rope->num_masses + bidx;
+ return *(uint32_t*)&spr->rest_len == 0xffffffff ? 0 : spr;
+}
+
/* actual step function, called by rsim_step in a loop to microstep or once if
* microstepping is disabled
*/
static void step(struct rsim_world *rsim, struct rsim_rope *rope, float dt)
{
- int i;
- cgm_vec3 npos, f, force;
- float inv_damp = rsim->damping == 0.0f ? 1.0f : 1.0f / rsim->damping;
- struct rsim_mass *mass = rope->masses;
+ int i, j;
+ float len, fmag;
+ cgm_vec3 npos, faccel, dir;
+ float inv_damp = rsim->damping == 0.0f ? 1.0f : (1.0f - rsim->damping);
+ struct rsim_mass *mass;
+ struct rsim_spring *spr;
+
+ /* for each mass, add spring forces to every other mass it's connected to */
+ for(i=0; i<rope->num_masses; i++) {
+ for(j=0; j<rope->num_masses; j++) {
+ if(i == j || !(spr = getspring(rope, i, j))) {
+ continue;
+ }
- force.x = rsim->grav.x + rsim->extforce.x;
- force.y = rsim->grav.y + rsim->extforce.y;
- force.z = rsim->grav.z + rsim->extforce.z;
+ dir = rope->masses[i].p;
+ cgm_vsub(&dir, &rope->masses[j].p);
+
+ len = cgm_vlength(&dir);
+ if(len != 0.0f) {
+ float s = 1.0f / len;
+ cgm_vscale(&dir, s);
+ }
+ fmag = (len - spr->rest_len) * spr->k;
+
+ cgm_vscale(&dir, fmag / rope->masses[j].m);
+ cgm_vadd(&rope->masses[j].f, &dir);
+ }
+ }
+ /* update masses */
+ mass = rope->masses;
for(i=0; i<rope->num_masses; i++) {
if(mass->fixed) {
mass++;
continue;
}
- npos = mass->p;
+ /* add constant forces to accumulated mass force */
+ cgm_vadd(&mass->f, &rsim->grav);
+
+ faccel = rsim->extforce;
+ cgm_vscale(&faccel, 1.0f / mass->m);
+ cgm_vadd(&mass->f, &rsim->extforce);
+
+ mass->v.x = (mass->v.x - mass->v.x * inv_damp * dt) + mass->f.x * dt;
+ mass->v.y = (mass->v.y - mass->v.y * inv_damp * dt) + mass->f.y * dt;
+ mass->v.z = (mass->v.z - mass->v.z * inv_damp * dt) + mass->f.z * dt;
+ /* zero out the accumulated forces for next iter */
+ mass->f.x = mass->f.y = mass->f.z = 0.0f;
+
+ npos = mass->p;
npos.x = mass->p.x + mass->v.x * dt;
npos.y = mass->p.y + mass->v.y * dt;
npos.z = mass->p.z + mass->v.z * dt;
- f.x = force.x;/* + spring forces */
- f.y = force.y;
- f.z = force.z;
-
- mass->v.x = ((mass->v.x - mass->v.x * inv_damp) + f.x) * dt;
- mass->v.y = ((mass->v.y - mass->v.y * inv_damp) + f.y) * dt;
- mass->v.z = ((mass->v.z - mass->v.z * inv_damp) + f.z) * dt;
-
/* TODO collisions */
mass->p = npos;
}
}
-struct rsim_rope *rsim_alloc_rope(int nmasses, int nsprings)
+struct rsim_rope *rsim_alloc_rope(int nmasses)
{
struct rsim_rope *rope;
if(!(rope = malloc(sizeof *rope))) {
return 0;
}
- if(rsim_init_rope(rope, nmasses, nsprings) == -1) {
+ if(rsim_init_rope(rope, nmasses) == -1) {
free(rope);
return 0;
}
free(rope);
}
-int rsim_init_rope(struct rsim_rope *rope, int nmasses, int nsprings)
+int rsim_init_rope(struct rsim_rope *rope, int nmasses)
{
memset(rope, 0, sizeof *rope);
if(!(rope->masses = calloc(nmasses, sizeof *rope->masses))) {
return -1;
}
- if(!(rope->springs = calloc(nsprings, sizeof *rope->springs))) {
+ rope->num_masses = nmasses;
+
+ if(!(rope->springs = malloc(nmasses * nmasses * sizeof *rope->springs))) {
free(rope->masses);
return -1;
}
- rope->num_masses = nmasses;
- rope->num_springs = nsprings;
+ memset(rope->springs, 0xff, nmasses * nmasses * sizeof *rope->springs);
return 0;
}
free(rope->springs);
}
+int rsim_set_rope_spring(struct rsim_rope *rope, int ma, int mb, float k, float rlen)
+{
+ cgm_vec3 dir;
+ struct rsim_spring *spr;
+
+ if(ma == mb || ma < 0 || ma >= rope->num_masses || mb < 0 || mb >= rope->num_masses) {
+ return -1;
+ }
+
+ if(rlen == RSIM_RLEN_DEFAULT) {
+ dir = rope->masses[ma].p;
+ cgm_vsub(&dir, &rope->masses[mb].p);
+ rlen = cgm_vlength(&dir);
+ }
+
+ spr = rope->springs + ma * rope->num_masses + mb;
+ spr->k = fabs(k);
+ spr->rest_len = rlen;
+ return 0;
+}
+
+int rsim_have_spring(struct rsim_rope *rope, int ma, int mb)
+{
+ return getspring(rope, ma, mb) ? 1 : 0;
+}
+
int rsim_freeze_rope_mass(struct rsim_rope *rope, struct rsim_mass *m)
{
if(m->fixed) return -1;
m->fixed = 1;
- m->next = rope->fixedlist;
- rope->fixedlist = m;
return 0;
}
int rsim_unfreeze_rope_mass(struct rsim_rope *rope, struct rsim_mass *m)
{
- struct rsim_mass *it, dummy;
-
if(!m->fixed) return -1;
- dummy.next = rope->fixedlist;
- it = &dummy;
- while(it->next) {
- if(it->next == m) {
- m->fixed = 0;
- it->next = m->next;
- m->next = 0;
- break;
- }
- it = it->next;
- }
- rope->fixedlist = dummy.next;
-
- return m->fixed ? -1 : 0;
+ m->fixed = 0;
+ return 0;
}
projects / dosdemo / blobdiff