started on the rope sim

diff --git a/tools/ropesim/src/cmesh.c b/tools/ropesim/src/cmesh.c
index 64ed2d0..3c5e928 100644 (file)
--- 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];
 
        /* 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;
        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);
 
        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; i<CMESH_NUM_ATTR; i++) {
 
        for(i=0; i<CMESH_NUM_ATTR; i++) {

-               cm->vattr[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;
 }
 
        return 0;
 }
 

@@ -144,7 +144,7 @@ void cmesh_destroy(struct cmesh *cm)
 
        cmesh_clear_submeshes(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);
        }
        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 (file)
--- a/tools/ropesim/src/main.c
+++ b/tools/ropesim/src/main.c
@@ -3,6 +3,7 @@
 #include <GL/glut.h>
 #include "cmesh.h"
 #include "cgmath/cgmath.h"
 #include <GL/glut.h>
 #include "cmesh.h"
 #include "cgmath/cgmath.h"

+#include "ropesim.h"

 
 int init(void);
 void cleanup(void);
 
 int init(void);
 void cleanup(void);

@@ -34,6 +35,8 @@ cgm_vec3 ganchor[4];
 
 cgm_vec3 dbgvec[4];
 
 
 cgm_vec3 dbgvec[4];
 

+struct rsim_world rsim;
+

 int main(int argc, char **argv)
 {
        glutInit(&argc, argv);
 int main(int argc, char **argv)
 {
        glutInit(&argc, argv);

@@ -61,13 +64,19 @@ int main(int argc, char **argv)
        return 0;
 }
 
        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 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);
 
        glEnable(GL_CULL_FACE);
        glEnable(GL_DEPTH_TEST);

@@ -96,11 +105,41 @@ int init(void)
                cmesh_remove_submesh(scn, idx);
        }
 
                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;
        /* 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; j<ROPE_MASSES; j++) {
+                       rope->masses[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;
        }
 
        return 0;

@@ -112,6 +151,8 @@ void cleanup(void)
        cmesh_free(mesh_gout);
        cmesh_free(mesh_gin);
        cmesh_free(scn);
        cmesh_free(mesh_gout);
        cmesh_free(mesh_gin);
        cmesh_free(scn);

+
+       rsim_destroy(&rsim);

 }
 
 void update(long tmsec, float dt)
 }
 
 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;
        int i;
        cgm_vec3 apt0, apt1;
        float theta, phi, brot;

+       struct rsim_rope *rope;

 
        /*
        cgm_mrotation_quat(ginner_xform, &grot);
 
        /*
        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);
 
        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;
        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)
 }
 
 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;
        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;
 
        update(tmsec, (float)(tmsec - prev_tmsec) / 1000.0f);
        prev_tmsec = tmsec;

@@ -195,6 +242,21 @@ void display(void)
        }
        glEnd();
 
        }
        glEnd();
 

+       glPushAttrib(GL_ENABLE_BIT);
+       glDisable(GL_LIGHTING);
+       glLineWidth(2);
+
+       rope = rsim.ropes;
+       while(rope) {
+               glBegin(GL_LINE_STRIP);
+               for(i=0; i<rope->num_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)
        glutSwapBuffers();
 }
 void idle(void)

diff --git a/tools/ropesim/src/ropesim.c b/tools/ropesim/src/ropesim.c
new file mode 100644 (file)
index 0000000..2c2dcd8
--- /dev/null
+++ b/tools/ropesim/src/ropesim.c
@@ -0,0 +1,168 @@
+#include <stdio.h>
+#include <stdlib.h>
+#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; i<rope->num_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 (file)
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_ */