/*!
  \file
  \brief 自由落下のサンプル

  球体を自由落下させたときの位置を出力する。「ロボットシミュレーション」(森北出版)のサンプルを参照。

  \author Satofumi KAMIMURA

  $Id: freeFall.cpp 1113 2009-07-06 21:58:15Z satofumi $
*/

#include <ode/ode.h>
#include <iostream>

using namespace std;


namespace
{
  dWorldID world_;
  dSpaceID space_;
  dGeomID ground_;
  dJointGroupID contact_group_;


  typedef struct {
    dBodyID body;
    dGeomID geometry;
    dReal length;
    dReal radius;
    dReal mass;
  } Object;

  Object ball_;


  void nearCallback(void* data, dGeomID o1, dGeomID o2)
  {
    static_cast<void>(data);

    enum { N = 10 };            // 接触点数の最大値
    dContact contact[N];

    bool is_ground = (ground_ == o1) || (ground_ == o2);
    int n = dCollide(o1, o2, N, &contact[0].geom, sizeof(dContact));
    if (is_ground) {
      for (int i = 0; i < n; ++i) {
        contact[i].surface.mode = dContactBounce;
        contact[i].surface.bounce = 0.5;
        contact[i].surface.bounce_vel = 0.1;

        dJointID c = dJointCreateContact(world_, contact_group_, &contact[i]);
        dJointAttach(c,
                     dGeomGetBody(contact[i].geom.g1),
                     dGeomGetBody(contact[i].geom.g2));
      }
    }
  }


  void simLoop(void)
  {
    dSpaceCollide(space_, 0, &nearCallback);

    dWorldStep(world_, 0.05);
    dJointGroupEmpty(contact_group_);
  }


  void createBall(void)
  {
    ball_.body = dBodyCreate(world_);

    const dReal weight = 1.0;
    const dReal radius = 0.2;

    dMass mass;
    dMassSetZero(&mass);
    dMassSetSphereTotal(&mass, weight, radius);
    dBodySetMass(ball_.body, &mass);
    dBodySetPosition(ball_.body, 0.0, 0.0, 2.0);
    ball_.radius = 0.2;
    ball_.geometry = dCreateSphere(space_, ball_.radius);
    dGeomSetBody(ball_.geometry, ball_.body);
  }
}


int main(int argc, char *argv[])
{
  static_cast<void>(argc);
  static_cast<void>(argv);

  dInitODE();

  world_ = dWorldCreate();
  dWorldSetGravity(world_, 0.0, 0.0, -9.8);
  space_ = dHashSpaceCreate(0);
  contact_group_ = dJointGroupCreate(0);
  ground_ = dCreatePlane(space_, 0, 0, 1, 0);

  createBall();

  enum { SimulationTimes = 40 };
  for (int i = 0; i < SimulationTimes; ++i) {
    simLoop();

    const dReal* position = dBodyGetPosition(ball_.body);
    //const dReal* rotation = dBodyGetRotation(ball_);
    cout << '('
         << position[0] << ", "
         << position[1] << ", "
         << position[2] << ')' << endl;
  }

  dSpaceDestroy(space_);
  dWorldDestroy(world_);
  dCloseODE();

  return 0;
}
