From c4e738daa32d1949f2d5c6fc62237760fb8aac68 Mon Sep 17 00:00:00 2001 From: John Tsiombikas Date: Wed, 1 Jan 2020 08:08:58 +0200 Subject: [PATCH] started on the rope sim --- tools/ropesim/src/cmesh.c | 10 +-- tools/ropesim/src/main.c | 64 ++++++++++++++++- tools/ropesim/src/ropesim.c | 168 +++++++++++++++++++++++++++++++++++++++++++ tools/ropesim/src/ropesim.h | 58 +++++++++++++++ 4 files changed, 294 insertions(+), 6 deletions(-) create mode 100644 tools/ropesim/src/ropesim.c create mode 100644 tools/ropesim/src/ropesim.h diff --git a/tools/ropesim/src/cmesh.c b/tools/ropesim/src/cmesh.c index 64ed2d0..3c5e928 100644 --- a/tools/ropesim/src/cmesh.c +++ b/tools/ropesim/src/cmesh.c @@ -38,7 +38,7 @@ struct cmesh { /* current value for each attribute for the immediate mode interface */ cgm_vec4 cur_val[CMESH_NUM_ATTR]; - unsigned int buffer_objects[CMESH_NUM_ATTR + 1]; + unsigned int vbuf[CMESH_NUM_ATTR + 1]; struct cmesh_vattrib vattr[CMESH_NUM_ATTR]; unsigned int *idata; @@ -121,13 +121,13 @@ int cmesh_init(struct cmesh *cm) memset(cm, 0, sizeof *cm); cgm_wcons(cm->cur_val + CMESH_ATTR_COLOR, 1, 1, 1, 1); - glGenBuffers(CMESH_NUM_ATTR + 1, cm->buffer_objects); + glGenBuffers(CMESH_NUM_ATTR + 1, cm->vbuf); for(i=0; ivattr[i].vbo = cm->buffer_objects[i]; + cm->vattr[i].vbo = cm->vbuf[i]; } - cm->ibo = cm->buffer_objects[CMESH_NUM_ATTR]; + cm->ibo = cm->vbuf[CMESH_NUM_ATTR]; return 0; } @@ -144,7 +144,7 @@ void cmesh_destroy(struct cmesh *cm) cmesh_clear_submeshes(cm); - glDeleteBuffers(CMESH_NUM_ATTR + 1, cm->buffer_objects); + glDeleteBuffers(CMESH_NUM_ATTR + 1, cm->vbuf); if(cm->wire_ibo) { glDeleteBuffers(1, &cm->wire_ibo); } diff --git a/tools/ropesim/src/main.c b/tools/ropesim/src/main.c index 90bccdf..2ccd695 100644 --- a/tools/ropesim/src/main.c +++ b/tools/ropesim/src/main.c @@ -3,6 +3,7 @@ #include #include "cmesh.h" #include "cgmath/cgmath.h" +#include "ropesim.h" int init(void); void cleanup(void); @@ -34,6 +35,8 @@ cgm_vec3 ganchor[4]; cgm_vec3 dbgvec[4]; +struct rsim_world rsim; + int main(int argc, char **argv) { glutInit(&argc, argv); @@ -61,13 +64,19 @@ int main(int argc, char **argv) return 0; } +#define ROPE_MASSES 4 +#define ROPE_SPRINGS (ROPE_MASSES - 1) +#define ROPE_LEN 1.0f +#define ROPE_MASSES_MASS 0.1f +#define ROPE_K 0.5f int init(void) { static const char *meshnames[] = {"suzanne", "gimbal_outer", "gimbal_inner"}; static struct cmesh **meshes[] = {&mesh_suz, &mesh_gout, &mesh_gin}; static const float amb[] = {0.05, 0.05, 0.08, 1}; - int i; + int i, j; + struct rsim_rope *rope, *ropes_tail; glEnable(GL_CULL_FACE); glEnable(GL_DEPTH_TEST); @@ -96,11 +105,41 @@ int init(void) cmesh_remove_submesh(scn, idx); } + rsim_init(&rsim); + ropes_tail = 0; + /* anchor points on the inner gimbal */ for(i=0; i<4; i++) { ganchor[i].x = (float)(((i & 1) << 1) - 1) * 1.5f; ganchor[i].y = (float)((i & 2) - 1) * 1.5f; ganchor[i].z = 0; + + /* create a rope hanging from the anchor point */ + if(!(rope = rsim_alloc_rope(ROPE_MASSES, ROPE_SPRINGS))) { + fprintf(stderr, "failed to allocate rope\n"); + return -1; + } + for(j=0; jmasses[j].p = ganchor[i]; + rope->masses[j].p.y = ganchor[i].y - j * ROPE_LEN / ROPE_SPRINGS; + rope->masses[j].m = 0.1f; + + if(j < ROPE_SPRINGS) { + rope->springs[j].rest_len = ROPE_LEN / ROPE_SPRINGS; + rope->springs[j].k = ROPE_K; + rope->springs[j].mass[0] = rope->masses + j; + rope->springs[j].mass[1] = rope->masses + j + 1; + } + } + rsim_freeze_rope_mass(rope, rope->masses); /* freeze first mass */ + + if(!ropes_tail) { + rsim.ropes = ropes_tail = rope; + } else { + ropes_tail->next = rope; + ropes_tail = rope; + } + rope->next = 0; } return 0; @@ -112,6 +151,8 @@ void cleanup(void) cmesh_free(mesh_gout); cmesh_free(mesh_gin); cmesh_free(scn); + + rsim_destroy(&rsim); } void update(long tmsec, float dt) @@ -119,6 +160,7 @@ void update(long tmsec, float dt) int i; cgm_vec3 apt0, apt1; float theta, phi, brot; + struct rsim_rope *rope; /* cgm_mrotation_quat(ginner_xform, &grot); @@ -131,12 +173,16 @@ void update(long tmsec, float dt) cgm_mrotation_euler(ginner_xform, phi, theta, 0, CGM_EULER_XYZ); cgm_mrotation_euler(gouter_xform, phi, 0, 0, CGM_EULER_XYZ); + rope = rsim.ropes; for(i=0; i<4; i++) { apt0 = ganchor[i]; cgm_vmul_m4v3(&apt0, ginner_xform); dbgvec[i] = apt0; + rope->masses[0].p = apt0; } + + rsim_step(&rsim, dt); } void display(void) @@ -154,6 +200,7 @@ void display(void) int i, count; long tmsec = glutGet(GLUT_ELAPSED_TIME) - start_msec; static long prev_tmsec; + struct rsim_rope *rope; update(tmsec, (float)(tmsec - prev_tmsec) / 1000.0f); prev_tmsec = tmsec; @@ -195,6 +242,21 @@ void display(void) } glEnd(); + glPushAttrib(GL_ENABLE_BIT); + glDisable(GL_LIGHTING); + glLineWidth(2); + + rope = rsim.ropes; + while(rope) { + glBegin(GL_LINE_STRIP); + for(i=0; inum_masses; i++) { + glVertex3f(rope->masses[i].p.x, rope->masses[i].p.y, rope->masses[i].p.z); + } + glEnd(); + rope = rope->next; + } + glPopAttrib(); + glutSwapBuffers(); } void idle(void) diff --git a/tools/ropesim/src/ropesim.c b/tools/ropesim/src/ropesim.c new file mode 100644 index 0000000..2c2dcd8 --- /dev/null +++ b/tools/ropesim/src/ropesim.c @@ -0,0 +1,168 @@ +#include +#include +#include "ropesim.h" + +static void step(struct rsim_world *rsim, struct rsim_rope *rope, float dt); + +int rsim_init(struct rsim_world *rsim) +{ + rsim->ropes = 0; + cgm_vcons(&rsim->grav, 0, -9.807, 0); + rsim->damping = 0.995; + return 0; +} + +void rsim_destroy(struct rsim_world *rsim) +{ + while(rsim->ropes) { + struct rsim_rope *rope = rsim->ropes; + rsim->ropes = rsim->ropes->next; + rsim_free_rope(rope); + } +} + +int rsim_add_rope(struct rsim_world *rsim, struct rsim_rope *rope) +{ + rope->next = rsim->ropes; + rsim->ropes = rope; + return 0; +} + +/* 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; + + force.x = rsim->grav.x + rsim->extforce.x; + force.y = rsim->grav.y + rsim->extforce.y; + force.z = rsim->grav.z + rsim->extforce.z; + + for(i=0; inum_masses; i++) { + if(mass->fixed) { + mass++; + continue; + } + + 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; + mass++; + } +} + +void rsim_step(struct rsim_world *rsim, float dt) +{ + struct rsim_rope *rope = rsim->ropes; + + if(rsim->udt > 0.0f) { + /* microstepping */ + float dt_acc = rsim->udelta_acc; + + while(dt_acc < dt) { + while(rope) { + step(rsim, rope, rsim->udt); + rope = rope->next; + } + dt_acc += rsim->udt; + } + rsim->udelta_acc = dt_acc - dt; + } else { + while(rope) { + step(rsim, rope, dt); + rope = rope->next; + } + } +} + +struct rsim_rope *rsim_alloc_rope(int nmasses, int nsprings) +{ + struct rsim_rope *rope; + + if(!(rope = malloc(sizeof *rope))) { + return 0; + } + if(rsim_init_rope(rope, nmasses, nsprings) == -1) { + free(rope); + return 0; + } + return rope; +} + +void rsim_free_rope(struct rsim_rope *rope) +{ + rsim_destroy_rope(rope); + free(rope); +} + +int rsim_init_rope(struct rsim_rope *rope, int nmasses, int nsprings) +{ + memset(rope, 0, sizeof *rope); + + if(!(rope->masses = calloc(nmasses, sizeof *rope->masses))) { + return -1; + } + if(!(rope->springs = calloc(nsprings, sizeof *rope->springs))) { + free(rope->masses); + return -1; + } + rope->num_masses = nmasses; + rope->num_springs = nsprings; + return 0; +} + +void rsim_destroy_rope(struct rsim_rope *rope) +{ + free(rope->masses); + free(rope->springs); +} + +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; +} diff --git a/tools/ropesim/src/ropesim.h b/tools/ropesim/src/ropesim.h new file mode 100644 index 0000000..f75e71b --- /dev/null +++ b/tools/ropesim/src/ropesim.h @@ -0,0 +1,58 @@ +#ifndef ROPESIM_H_ +#define ROPESIM_H_ + +#include "cgmath/cgmath.h" + +struct rsim_mass { + cgm_vec3 p; + cgm_vec3 v; + float m; + int fixed; + + /* used for stringing fixed masses together */ + struct rsim_mass *next; +}; + +struct rsim_spring { + float rest_len, k; + struct rsim_mass *mass[2]; +}; + +struct rsim_rope { + struct rsim_mass *masses; + int num_masses; + struct rsim_spring *springs; + int num_springs; + + /* elements of the masses array which are fixed */ + struct rsim_mass *fixedlist; + + struct rsim_rope *next; +}; + +struct rsim_world { + struct rsim_rope *ropes; /* list of ropes to simulate */ + + cgm_vec3 grav, extforce; + float damping; + + float udt; /* microstepping delta (valid if > 0) */ + float udelta_acc; /* dt leftover accumulator for microstepping */ +}; + +int rsim_init(struct rsim_world *rsim); +void rsim_destroy(struct rsim_world *rsim); + +int rsim_add_rope(struct rsim_world *rsim, struct rsim_rope *rope); + +void rsim_step(struct rsim_world *rsim, float dt); + +struct rsim_rope *rsim_alloc_rope(int nmasses, int nsprings); +void rsim_free_rope(struct rsim_rope *rope); +int rsim_init_rope(struct rsim_rope *rope, int nmasses, int nsprings); +void rsim_destroy_rope(struct rsim_rope *rope); + +int rsim_freeze_rope_mass(struct rsim_rope *rope, struct rsim_mass *m); +int rsim_unfreeze_rope_mass(struct rsim_rope *rope, struct rsim_mass *m); + +#endif /* ROPESIM_H_ */ -- 1.7.10.4