Balls

#define dDOUBLE
#include <ode/ode.h>
#include <drawstuff/drawstuff.h>

#include <vector>

using namespace std;

uint32_t colors[] = { // -std=c++11
  0xcccccccc, 0xcc9933cc, 0x33cc99cc, 0x9933cccc,
  0x3399cccc, 0x99cc33cc, 0xcc3399cc, 0x999999cc,
  0x666666cc, 0x996633cc, 0x339966cc, 0x663399cc,
  0x336699cc, 0x669933cc, 0x993366cc, 0x333333cc
};

inline
void vec4fromU32(dVector4 &c, uint32_t col)
{
  for(int j = 0; j < 4; ++j)
    c[j] = (dReal)*((unsigned char *)&col + (3 - j)) / 255.0; // little endian
}

inline
void vec4fromV4(dVector4 &dst, dVector4 &src)
{
  for(int i = 0; i < 4; ++i) dst[i] = src[i];
}

typedef struct {
  dWorldID world;
  dSpaceID space;
  dGeomID ground;
  dJointGroupID contactgroup;
} Gws;

typedef struct {
  dBodyID body;
  dGeomID geom;
  dVector4 col;
} Obg;

static Gws gws;
static vector<Obg *> obgs;
const dReal mass = 1.0, radius = 0.2;
const dReal tDelta = 0.002;

void createWorld(Gws *gws)
{
  dInitODE2(0);
  gws->world = dWorldCreate();
  dWorldSetGravity(gws->world, 0, 0, -9.8);
  gws->space = dHashSpaceCreate(0);
  dSpaceSetCleanup(gws->space, 1);
  gws->ground = dCreatePlane(gws->space, 0, 0, 1, 0);
  gws->contactgroup = dJointGroupCreate(0);
}

void destroyWorld(Gws *gws)
{
  dSpaceDestroy(gws->space);
  dWorldDestroy(gws->world);
  dCloseODE();
}

void destroyObg(Obg *obg)
{
  dGeomDestroy(obg->geom);
  dBodyDestroy(obg->body);
}

void clearObgs()
{
  for(vector<Obg *>::iterator it = obgs.begin(); it != obgs.end(); ++it){
    destroyObg(*it);
  }
  obgs.clear();
}

Obg *mkSphere(Gws *gws, dReal m, dReal r, dVector4 &col, dVector3 &pos)
{
  dMass mass;
  dMassSetZero(&mass);
  dMassSetSphereTotal(&mass, m, r);
  Obg *obg = new Obg;
  vec4fromV4(obg->col, col);
  obg->body = dBodyCreate(gws->world);
  dBodySetMass(obg->body, &mass);
  obg->geom = dCreateSphere(gws->space, r);
  dGeomSetBody(obg->geom, obg->body);
  dBodySetPosition(obg->body, pos[0], pos[1], pos[2]);
  return obg;
}

void nearCallback(void *dat, dGeomID o1, dGeomID o2)
{
  // if(!(gws.ground == o1 || gws.ground == o2)) return;
  const int num = 40;
  dContact contacts[num];
  int n = dCollide(o1, o2, num, &contacts[0].geom, sizeof(dContact));
  for(int i = 0; i < n; ++i){
    dContact cntct = contacts[i];
    cntct.surface.mu = dInfinity;
    cntct.surface.mode = dContactBounce;
    cntct.surface.bounce = 0.95;
    cntct.surface.bounce_vel = 0.0;
    dJointID c = dJointCreateContact(gws.world, gws.contactgroup, &cntct);
    dJointAttach(c, dGeomGetBody(cntct.geom.g1), dGeomGetBody(cntct.geom.g2));
  }
}

void simLoop(int pause)
{
  if(pause != 1){
    dSpaceCollide(gws.space, 0, nearCallback);
    dWorldStep(gws.world, tDelta);
    dJointGroupEmpty(gws.contactgroup);
  }
  for(vector<Obg *>::iterator it = obgs.begin(); it != obgs.end(); ++it){
    dVector4 &c = (*it)->col;
    dsSetColorAlpha(c[0], c[1], c[2], c[3]);
    dGeomID geom = (*it)->geom;
    dBodyID body = dGeomGetBody(geom);
    int cls = dGeomGetClass(geom);
    switch(cls){
    case dSphereClass: {
      const dReal *pos = dBodyGetPosition(body);
      const dReal *rot = dBodyGetRotation(body);
      const dReal radius = dGeomSphereGetRadius(geom);
      dsDrawSphereD(pos, rot, radius);
    } break;
    default: {
      printf("unknown class: %d\n", cls);
    }
    }
  }
}

void startCallback()
{
  static float xyz[3] = {4.0, 3.0, 5.0};
  static float hpr[3] = {-150.0, -30.0, 3.0};
  dsSetViewpoint(xyz, hpr);
}

int main(int ac, char **av)
{
  createWorld(&gws);
  for(int i = 0; i < 16; ++i){
    dVector4 c;
    vec4fromU32(c, colors[i]);
    dVector3 p{i % 4 - 1.5, i / 4 - 1.5, 2.0, 1.0};
    obgs.push_back(mkSphere(&gws, mass, radius, c, p));
  }
  dVector4 c{1.0, 1.0, 0.0, 0.8};
  dVector3 p{0.0, 0.0, 10.0, 1.0};
  obgs.push_back(mkSphere(&gws, 0.1, 1.0, c, p));

  dsFunctions fn;
  fn.version = DS_VERSION;
  fn.start = startCallback;
  fn.step = simLoop;
  fn.command = NULL;
  fn.stop = NULL;
  fn.path_to_textures = "./resources";
  dsSimulationLoop(ac, av, 640, 480, &fn);

  clearObgs();
  destroyWorld(&gws);
  return 0;
}