Do not reject 1.5 format SOLs

[neverball] / share / solid_sim_ode.c

diff --git a/share/solid_sim_ode.c b/share/solid_sim_ode.c
index ea84ae1..7b97f9d 100644 (file)
--- a/share/solid_sim_ode.c
+++ b/share/solid_sim_ode.c
@@ -100,11 +100,55 @@ void free_convex(struct convex *cx)
 #define CAT_OBJECT 0x1
 #define CAT_WORLD  0x2
 
+static void build_node(dBodyID body, dSpaceID parent,
+                       struct s_node *np,
+                       struct s_file *fp)
+{
+    dSpaceID node = dHashSpaceCreate(parent);
+    dGeomID geom;
+    int i;
+
+    for (i = 0; i < np->lc; i++)
+    {
+        struct s_lump *lp = fp->lv + np->l0 + i;
+        struct convex *cx;
+
+        if (lp->fl & L_DETAIL)
+            continue;
+
+        cx = array_add(lumps);
+
+        make_convex(cx, fp, lp);
+
+        geom = dCreateConvex(node,
+                             cx->planes, cx->nplanes,
+                             cx->points, cx->npoints,
+                             cx->polygons);
+
+        dGeomSetCategoryBits(geom, CAT_WORLD);
+        dGeomSetCollideBits (geom, CAT_OBJECT);
+
+        if (body) dGeomSetBody(geom, body);
+    }
+
+    if (np->ni >= 0) build_node(body, node, fp->nv + np->ni, fp);
+    if (np->nj >= 0) build_node(body, node, fp->nv + np->nj, fp);
+}
+
+static void build_branch(dBodyID body, dSpaceID root,
+                         struct s_body *bp,
+                         struct s_file *fp)
+{
+    dSpaceID branch = dHashSpaceCreate(root);
+
+    build_node(body, branch, fp->nv + bp->ni, fp);
+}
+
 void sol_init_sim(struct s_file *fp)
 {
     dGeomID geom;
 
-    int i, j;
+    int i;
 
     dInitODE();
 
@@ -129,34 +173,17 @@ void sol_init_sim(struct s_file *fp)
         else
             bodies[i] = 0;
 
-        for (j = 0; j < bp->lc; j++)
-        {
-            struct s_lump *lp = fp->lv + bp->l0 + j;
-            struct convex *cx;
-
-            if (lp->fl & L_DETAIL)
-                continue;
-
-            cx = array_add(lumps);
-
-            make_convex(cx, fp, lp);
-
-            geom = dCreateConvex(space,
-                                 cx->planes, cx->nplanes,
-                                 cx->points, cx->npoints,
-                                 cx->polygons);
-
-            dGeomSetCategoryBits(geom, CAT_WORLD);
-            dGeomSetCollideBits (geom, CAT_OBJECT);
-
-            if (bodies[i]) dGeomSetBody(geom, bodies[i]);
-        }
+        build_branch(bodies[i], space, bp, fp);
 
         if (bodies[i])
         {
-            float p[3];
+            float p[3], w[3];
+
             sol_body_p(p, fp, bp->pi, 0.0f);
+            sol_body_w(w, fp, bp);
+
             dBodySetPosition(bodies[i], p[0], p[1], p[2]);
+            dBodySetAngularVel(bodies[i], w[0], w[1], w[2]);
         }
     }
 
@@ -239,13 +266,21 @@ static void spin(dContactGeom *contact)
     const dReal *p, *v, *w;
     float r[3], d[3];
 
-    /* Why do I know that the first geom is a sphere? */
+    if ((up = dGeomGetData(contact->g1)))
+    {
 
-    up = dGeomGetData(contact->g1);
+        p = dGeomGetPosition(contact->g1);
+        v = geomvel(contact->g1);
+        w = pointvel(contact->g2, contact->pos);
+    }
+    else
+    {
+        up = dGeomGetData(contact->g2);
 
-    p = dGeomGetPosition(contact->g1);
-    v = geomvel(contact->g1);
-    w = pointvel(contact->g2, contact->pos);
+        p = dGeomGetPosition(contact->g2);
+        v = geomvel(contact->g2);
+        w = pointvel(contact->g1, contact->pos);
+    }
 
     v_sub(r, p, contact->pos);
     v_sub(d, v, w);
@@ -266,6 +301,12 @@ static void collide(void *data, dGeomID o1, dGeomID o2)
     float *b = (float *) data;
     float  d;
 
+    if (dGeomIsSpace(o1) || dGeomIsSpace(o2))
+    {
+        dSpaceCollide2(o1, o2, data, collide);
+        return;
+    }
+
     if ((n = dCollide(o1, o2, 8, &contacts[0].geom, sizeof (dContact))) > 0)
     {
         for (i = 0; i < n; i++)
@@ -311,27 +352,29 @@ static void import_state(struct s_file *fp, float dt)
     for (i = 0; i < fp->bc; i++)
     {
         struct s_body *bp = fp->bv + i;
-        float v[3], p[3];
+        float p[3], v[3], w[3];
 
         if (bodies[i])
         {
             sol_body_p(p, fp, bp->pi, bp->t);
             sol_body_v(v, fp, bp->pi, bp->t, dt);
+            sol_body_w(w, fp, bp);
 
             dBodySetPosition(bodies[i], p[0], p[1], p[2]);
             dBodySetLinearVel(bodies[i], v[0], v[1], v[2]);
+            dBodySetAngularVel(bodies[i], w[0], w[1], w[2]);
         }
     }
 }
 
 static void export_state(struct s_file *fp)
 {
+    const dReal *v;
     int i;
 
     for (i = 0; i < fp->uc; i++)
     {
         struct s_ball *up = fp->uv + i;
-        const dReal *v;
 
         v = dBodyGetPosition(balls[i]);
         v_cpy(up->p, v);
@@ -339,6 +382,17 @@ static void export_state(struct s_file *fp)
         v = dBodyGetLinearVel(balls[i]);
         v_cpy(up->v, v);
     }
+
+    for (i = 0; i < fp->bc; i++)
+    {
+        struct s_body *bp = fp->bv + i;
+
+        if (bodies[i])
+        {
+            v = dBodyGetQuaternion(bodies[i]);
+            q_cpy(bp->e, v);
+        }
+    }
 }
 
 /*---------------------------------------------------------------------------*/