A Taste of ODE in OCaml

As explained in the manual of ODE a typical simulation will proceed like this:

  1. Create a dynamics world.
  2. Create bodies in the dynamics world.
  3. Set the state (position etc) of all bodies.
  4. Create joints in the dynamics world.
  5. Attach the joints to the bodies.
  6. Set the parameters of all joints.
  7. Create a collision world and collision geometry objects, as necessary.
  8. Create a joint group to hold the contact joints.
  9. Loop:
    1. Apply forces to the bodies as necessary.
    2. Adjust the joint parameters as necessary.
    3. Call collision detection.
    4. Create a contact joint for every collision point, and put it in the contact joint group.
    5. Take a simulation step.
    6. Remove all joints in the contact joint group.
  10. Destroy the dynamics and collision worlds.

Here is how it looks like in OCaml with the ODE bindings with the most simple possible example:

open Ode.LowLevel

let () =
  dInitODE ();
  let wrl = dWorldCreate () in
  dWorldSetGravity wrl 0. 0. (-0.9);
  let space = dHashSpaceCreate None in
  let plane = dCreatePlane (Some space) 0. 0. 1. 0. in
  let cgrp = dJointGroupCreate () in

  let (lx,ly,lz) = (1.,1.,1.) in
  let b = dBodyCreate wrl in
  dBodySetPosition b 0. 0. 1.;
  let m = dMassCreate () in
  dMassSetBox m 2.4 lx ly lz;
  dMassAdjust m 1.0;
  dBodySetMass b m;
  let g = dCreateBox (Some space) lx ly lz in
  dGeomSetBody g (Some b);

  let near ga gb =
    let surf_params = { surf_param_zero with
      sp_mode = [`dContactBounce];
      sp_mu = dInfinity;
      sp_bounce = 0.7;
      sp_bounce_vel = 0.1;
    } in
    let cnt_arr = dCollide ga gb 5 in
    ArrayLabels.iter cnt_arr ~f:(fun cnt_geom ->
      let cnt = {
        c_surface = surf_params;
        c_geom = cnt_geom;
        c_fdir1 = { x=0.; y=0.; z=0.; w=0. }
      } in
      let j = dJointCreateContact wrl (Some cgrp) cnt in
      dJointAttach j (dGeomGetBody ga)
                     (dGeomGetBody gb);
    );
  in

  Sys.catch_break true;
  try while true do
    dSpaceCollide space near;
    let p = dGeomGetPosition g in
    Printf.printf " (%6.3f %6.3f %6.3f)\n%!" p.x p.y p.z;
    dWorldStep wrl 0.1;
    dJointGroupEmpty cgrp;
    Unix.sleep 1;
  done
  with Sys.Break ->
    dBodyDestroy b;
    dGeomDestroy g;
    dGeomDestroy plane;
    dSpaceDestroy space;
    dWorldDestroy wrl;
    dCloseODE ();
;;