#include "physics.hpp"

#include "rigid_body.hpp"
#include "physics_controller.hpp"

using namespace std;

// limit the extra velocity during collision/penetration calculations
static const Scalar max_vel_mag = 30.0f;

Vector3 Physics::m_gravity(0.0f, 0.0f, -9.81f);

inline Physics::Stored_data::Stored_data(Rigid_body * rb)
:
position(rb->get_position()),
orientation(rb->get_orientation()),
velocity(rb->get_velocity()),
rotation(rb->get_rotation())
{}


//==============================================================
// Physics
//==============================================================
Physics::Physics() : m_physics_time(0.0f)
{
  TRACE_METHOD_ONLY(ONCE_1);

  m_timestep = 1.0f / 100;
  m_overlap_time = 0.0f;
  m_num_collision_iterations = 5;
  m_num_contact_iterations = 10;
  m_num_penetration_iterations = 10;
  m_penetration_resolve_fraction = 0.008;
  m_reuse_collisions = false;
  m_traverse_dir = TRAVERSE_FORWARD;
  m_allow_smaller_timesteps = false;
  m_freezing_enabled = false;
  m_time_scale = 1.0f;
}

//==============================================================
// add_body
//==============================================================
void Physics::add_body(Rigid_body * body)
{
    m_rigid_bodies.append(body);
}

void Physics::remove_body(Rigid_body * body)
{
    eInt p = m_rigid_bodies.exists(body);
    if(p != -1)
        m_rigid_bodies.removeAt(p);
}

//==============================================================
// add_constraint
//==============================================================
void Physics::add_constraint(Constraint * constraint)
{
  m_constraints.append(constraint);
}
void Physics::remove_constraint(Constraint * constraint)
{
    eInt p = m_constraints.exists(constraint);
    if(p != -1)
        m_constraints.removeAt(p);
}

//==============================================================
// add_controller
//==============================================================
void Physics::add_controller(Physics_controller * controller)
{
  m_controllers.append(controller);
}
void Physics::remove_controller(Physics_controller * controller)
{
    eInt p = m_controllers.exists(controller);
    if(p != -1)
        m_controllers.removeAt(p);
}

//==============================================================
// activate_object
// activate object and bring it into the collision list/do collision detection
//==============================================================
void Physics::activate_object(Rigid_body * body)
{
  if ( (body->get_activity_state() == Rigid_body::ACTIVE) ||
       (body->get_immovable()) )
  {
    return;
  }

  body->set_activity_state(Rigid_body::ACTIVE, 1.0f);

  int orig_num = m_collisions.size();

  if (!body->collision_body())
    return;

  ::detect_frozen_collisions_with_body(body->collision_body(), m_collision_bodies, m_collisions);
  m_collision_bodies.push_back(body->collision_body());

  // now check that any adjacent touching bodies wouldn't accelerate towards us if we moved away
  int new_num = m_collisions.size();
  int i;
  for (i = orig_num ; i < new_num ; ++i)
  {
    // must be a body-body interaction to be interesting
    if (m_collisions[i].body1)
    {
      Rigid_body * other_body = m_collisions[i].body0->rigid_body();
      // the collision normal pointing from body to other_body
      Vector3 this_body_normal = m_collisions[i].dir_to_0;
      if (other_body == body)
      {
        other_body = m_collisions[i].body1->rigid_body();
        this_body_normal *= -1;
      }
      if (other_body->get_activity_state() == Rigid_body::FROZEN)
      {
        // remember that the Rigid_body doesn't apply gravity to sleeping bodies
        Vector3 force_on_other = other_body->get_mass() * get_gravity() + other_body->get_force();
        if (dot(force_on_other, this_body_normal) <= 0.0f)
        {
          // wake it up recursively. after this, the contents of our m_collisions may have been relocated
          activate_object(other_body);
        }
      }
    }
  }
}

//==============================================================
// preprocess_collision
//==============================================================
inline void Physics::preprocess_collision(Collision_info & collision, Scalar dt)
{
  Rigid_body * body0 = collision.body0->rigid_body();
  Rigid_body * body1 = collision.body1->rigid_body();

  collision.R0 = collision.position - body0->get_position();

	const Scalar allowed_penetration = 0.0f;
	const Scalar timescale = 5 * dt;
	if (collision.penetration_depth > allowed_penetration)
  {
		collision.vr_extra = ((collision.penetration_depth - allowed_penetration) / timescale);
    if (collision.vr_extra > max_vel_mag)
      collision.vr_extra = max_vel_mag;
  }
  else
    collision.vr_extra = 0;

  const Vector3 & N = collision.dir_to_0;

    collision.R1 = collision.position - body1->get_position();
    collision.elasticity = body0->elasticity() * body1->elasticity();
    collision.static_friction = 0.5f * (body0->static_friction() + body1->static_friction());
    collision.dynamic_friction = 0.5f * (body0->dynamic_friction() + body1->dynamic_friction());
    collision.denominator = body0->get_inv_mass() + body1->get_inv_mass() + 
        dot(N, cross(body0->get_world_inv_inertia() * (cross(collision.R0, N)), collision.R0)) + 
        dot(N, cross(body1->get_world_inv_inertia() * (cross(collision.R1, N)), collision.R1));
}

//==============================================================
// process_collision
//==============================================================
bool Physics::process_collision(Collision_info & collision, 
                                Scalar dt,
                                bool override_elasticity,
                                Scalar epsilon)
{
  TRACE_METHOD_ONLY(MULTI_FRAME_1);
  if (collision.denominator < 0.0001f)
    return false;

  Rigid_body * body0 = collision.body0->rigid_body();
  Rigid_body * body1 = 0;

  if (collision.body1)
  {
    body1 = collision.body1->rigid_body();
  }

  const Vector3 & N = collision.dir_to_0;

  Scalar normal_vel;
  if (body1)
  {
    normal_vel =  dot(  (body0->get_velocity() + cross(body0->get_rotation(), collision.R0)) - 
                        (body1->get_velocity() + cross(body1->get_rotation(), collision.R1)), N); 
  }
  else
  {
    normal_vel =  dot(  (body0->get_velocity() + cross(body0->get_rotation(), collision.R0)), N); 
  }

  // coefficient of restitution
  if (!override_elasticity)
  {
    epsilon = collision.elasticity;
  }

  Scalar final_normal_vel = -epsilon * normal_vel;

  // delta_vel is the change in normal velocity. Only add the extra vel if over-riding elasticity
  // during the second phase.
  if (override_elasticity)
  {
    Scalar extra = collision.vr_extra - final_normal_vel;
    if (extra > 0.0f)
      final_normal_vel += extra;
  }

  Scalar delta_vel = final_normal_vel - normal_vel;

  if (delta_vel <= 0.0f)
    return false;

  Scalar numerator = delta_vel;

//    TRACE("epsilon = %5.2f, approach speed = %6.3f\n", epsilon, -dot(Vr0, N));

  Scalar normal_impulse = numerator / collision.denominator;

  bool body0_frozen_pre = body0->get_activity_state() == Rigid_body::FROZEN;
  bool body1_frozen_pre = false;
  if (body1)
    body1_frozen_pre = body1->get_activity_state() == Rigid_body::FROZEN;

  body0->apply_world_impulse(normal_impulse * N, collision.position);
  if (body1)
    body1->apply_world_impulse(-normal_impulse * N, collision.position);

  // if one of the bodies is frozen, then if it is to stay frozen the other body should
  // take the whole impulse
  if (body0_frozen_pre)
  {
    if (!body0->should_be_active())
    {
      // OK - reverse the impulse on body0, and apply it to body1
      body0->apply_world_impulse(-normal_impulse * N, collision.position);
      if (body1)
        body1->apply_world_impulse(-normal_impulse * N, collision.position);
    }
  }
  else if (body1_frozen_pre)
  {
    if (!body1->should_be_active())
    {
      // OK - reverse the impulse on body1, and apply it to body0
      body0->apply_world_impulse(normal_impulse * N, collision.position);
      body1->apply_world_impulse(normal_impulse * N, collision.position);
    }
  }


	// For friction, work out the impulse in the opposite direction to the tangential
	// velocity that would be required to bring this point to a halt. Apply the minimum 
	// of this impulse magnitude, and the one obtained from the normal impulse. This 
	// prevents reversing the velocity direction.
  //
  // However, recalculate the velocity since it's changed. 
  Velocity Vr_new = body0->get_velocity() + cross(body0->get_rotation(), collision.R0);
  if (body1)
    Vr_new -= (body1->get_velocity() + cross(body1->get_rotation(), collision.R1));
//  Velocity Vr_av = 0.5f * (Vr + Vr_new);

	Vector3 tangent_vel = Vr_new - dot(Vr_new, N)  * N;
	Scalar tangent_speed = tangent_vel.mag();
	if (tangent_speed > 0.0f)
	{
		Vector3 T = -tangent_vel / tangent_speed;

    // epsilon = 0
		numerator = tangent_speed;

    Scalar denominator;
    if (body1)
    {
      denominator = body0->get_inv_mass() + body1->get_inv_mass() + 
        dot(T, cross(body0->get_world_inv_inertia() * (cross(collision.R0, T)), collision.R0)) + 
        dot(T, cross(body1->get_world_inv_inertia() * (cross(collision.R1, T)), collision.R1));
    }
    else
    {
      denominator = body0->get_inv_mass() + 
        dot(T, cross(body0->get_world_inv_inertia() * (cross(collision.R0, T)), collision.R0));
    }

		if (denominator > 0.0f)
		{
			Scalar impulse_to_reverse = numerator / denominator;
			Scalar static_friction = collision.static_friction;
      Scalar dynamic_friction = collision.dynamic_friction;

      Scalar impulse_from_normal_impulse = static_friction * normal_impulse;
      Scalar friction_impulse;

      if (impulse_to_reverse < impulse_from_normal_impulse)
        friction_impulse = impulse_to_reverse;
      else
        friction_impulse = dynamic_friction * normal_impulse;

			body0->apply_world_impulse(friction_impulse * T, collision.position);
      if (body1)
  			body1->apply_world_impulse((-friction_impulse) * T, collision.position);
		}
	} // end of friction

  return true;
}

//==============================================================
// try_to_activate_all_frozen_objects
//==============================================================
void Physics::try_to_activate_all_frozen_objects(Scalar step_frac)
{
  int num_bodies = m_rigid_bodies.size();
  int i;

  for (i = 0 ; i < num_bodies ; ++i)
  {
    if (m_rigid_bodies[i]->get_activity_state() == Rigid_body::FROZEN)
    {
      if (m_rigid_bodies[i]->should_be_active())
      {
        activate_object(m_rigid_bodies[i]);
      }
      else
      {
        m_rigid_bodies[i]->set_velocity(Velocity(0.0f));
        m_rigid_bodies[i]->set_rotation(Rotation(0.0f));
      }
    }
  }
}


//==============================================================
// detect_all_collisions
//==============================================================
void Physics::detect_all_collisions(Scalar dt)
{
  TRACE_METHOD_ONLY(MULTI_FRAME_1);
  int num_bodies = m_rigid_bodies.size();
  int i;

  m_collision_bodies.clear();
  m_collisions.clear();
  for (i = 0 ; i < num_bodies ; ++i)
  {
    if (m_rigid_bodies[i]->collision_body())
    {
      m_collision_bodies.push_back(m_rigid_bodies[i]->collision_body());
      m_rigid_bodies[i]->collision_body()->collisions().clear();
    }

  }

  ::detect_all_collisions(m_collision_bodies, m_collisions);
}

//==============================================================
// handle_all_collisions
//==============================================================
void Physics::handle_all_collisions(Scalar dt)
{
  TRACE_METHOD_ONLY(MULTI_FRAME_1);
  // handle collisions by:
  // 1. store the original position/velocity/force info.
  // 2. predict a new velocity using the external forces (and zero the forces)
  // 3. use the new velocity to predict a new position
  // 4. detect contacts
  // 5. reset the velocity to its original, zero forces
  // 6. classify the contacts based on the "current" velocity, and add/apply impulses
  // 7. repeat step 6 a few times...
  // 8. reset the position/forces to the original

  // then in the next functions
  // 9. apply forces

  // 10.0 detect collisions and handle using inelastic collisions

  int num_bodies = m_rigid_bodies.size();
  const int num_constraints = m_constraints.size();
 
  int i;
  // step 1
  m_stored_data.resize(num_bodies);
  for (i = 0 ; i < num_bodies ; ++i)
  {
    m_stored_data[i] = Stored_data(m_rigid_bodies[i]);
  }

  // step 2
  update_all_velocities(dt);

  // step 3
  update_all_positions(dt);

  // step 4
  detect_all_collisions(dt);
  int orig_num_collisions = m_collisions.size();

  // step 5
  for (i = 0 ; i < num_bodies ; ++i)
  {
    m_rigid_bodies[i]->set_velocity(m_stored_data[i].velocity);
    m_rigid_bodies[i]->set_rotation(m_stored_data[i].rotation);
  }

  // prepare for the collisions
  for (i = 0 ; i < orig_num_collisions ; ++i)
  {
    preprocess_collision(m_collisions[i], dt);
  }

  // prepare all the constraints
  for (i = 0 ; i < num_constraints ; ++i)
  {
  	m_constraints[i]->pre_apply(dt);
  }

  // iterate over the collisions
  int step;
  for (step = 0 ; step < m_num_collision_iterations ; ++step)
  {
    bool got_one = false;
    // step 6
    int i_start, i_end, i_dir;
    int num_collisions = m_collisions.size();
    if (m_traverse_dir == TRAVERSE_FORWARD)
    {
      i_start = 0;
      i_end = num_collisions;
      i_dir = 1;
      m_traverse_dir = TRAVERSE_BACKWARD;
    }
    else
    {
      i_start = num_collisions - 1;
      i_end = -1;
      i_dir = -1;
      m_traverse_dir = TRAVERSE_FORWARD;
    }
    for (i = i_start ; i != i_end; i += i_dir)
    {
      if (process_collision(m_collisions[i], dt, false))
        got_one = true;
    }

    // do all the constraints
    for (i = 0 ; i < num_constraints ; ++i)
    {
      if (m_constraints[i]->apply(dt))
		    got_one = true;
    }

    // wake up any previously staionary frozen objects that were frozen. 
    // Also, if they're moving less than the threshold, freeze them
    if (m_freezing_enabled)
      try_to_activate_all_frozen_objects(step / (m_num_collision_iterations - 1.0f));

    // number of collisions may have increased...
    num_collisions = m_collisions.size();

    // preprocess any new collisions.
    for (i = orig_num_collisions ; i < num_collisions ; ++i)
    {
      preprocess_collision(m_collisions[i], dt);
    }
    orig_num_collisions = num_collisions;

    if (!got_one)
      break;
  }

  // step 7
  for (i = 0 ; i < num_bodies ; ++i)
  {
    m_rigid_bodies[i]->set_position(m_stored_data[i].position);
    m_rigid_bodies[i]->set_orientation(m_stored_data[i].orientation);
  }
}

//==============================================================
// separate_objects
//==============================================================
void Physics::separate_objects(Collision_info & collision, Scalar factor)
{
  TRACE_METHOD_ONLY(MULTI_FRAME_1);
  Rigid_body * body0 = collision.body0->rigid_body();
  // TODO wake up if separation is needed?
  if ( (body0->get_immovable()) || (body0->get_activity_state() == Rigid_body::FROZEN) )
    return;
  
  if ( (collision.body1 == 0) || 
       (collision.body1->rigid_body()->get_immovable()) ||
       (collision.body1->rigid_body()->get_activity_state() == Rigid_body::FROZEN) )
  {
    // ground - just update body 0
    Vector3 delta = (factor * collision.penetration_depth) * collision.dir_to_0;
    Position orig = body0->get_position();
    body0->set_position(orig + delta);
    int i;
    std::vector<int> & colls = collision.body0->collisions();
    int num_colls = colls.size();
    for (i = 0 ; i < num_colls ; ++i)
    {
      Collision_info & info = m_collisions[colls[i]];
      info.penetration_depth -= dot(delta, info.dir_to_0);
    }
  }
  else
  {
    Rigid_body * body1 = collision.body1->rigid_body();
    
    Vector3 delta = (factor * collision.penetration_depth) * collision.dir_to_0;
    // how should we partition the separation? It's not physical, so just 50-50
    delta *= 0.5f;
    const Position orig0 = body0->get_position();
    const Position orig1 = body1->get_position();
    body0->set_position(orig0 + delta);
    body1->set_position(orig1 - delta);
    // now update all the collisions affected
    int i;
    std::vector<int> & colls0 = collision.body0->collisions();
    int num_colls = colls0.size();
    // collisions associated with body 0
    for (i = 0 ; i < num_colls ; ++i)
    {
      Collision_info & info = m_collisions[colls0[i]];
      if (info.body0 == collision.body0)
        info.penetration_depth -= dot(delta, info.dir_to_0);
      else
        info.penetration_depth += dot(delta, info.dir_to_0);
    }
    std::vector<int> & colls1 = collision.body1->collisions();
    num_colls = colls1.size();
    // collisions associated with body 1
    for (i = 0 ; i < num_colls ; ++i)
    {
      Collision_info & info = m_collisions[colls1[i]];
      if (info.body0 == collision.body1)
        info.penetration_depth += dot(delta, info.dir_to_0);
      else
        info.penetration_depth -= dot(delta, info.dir_to_0);
    }
  }
}


//==============================================================
// handle_all_contacts
//==============================================================
void Physics::handle_all_constraints(Scalar dt)
{
  TRACE_METHOD_ONLY(MULTI_FRAME_1);
  const int num_constraints = m_constraints.size();
  int i;

  if (!m_reuse_collisions)
    detect_all_collisions(dt);

  const int num_collisions = m_collisions.size();

  // prepare all the constraints
  for (i = 0 ; i < num_constraints ; ++i)
  {
  	m_constraints[i]->pre_apply(dt);
  }

  // prepare all the collisions (TODO is this needed?)
  for (i = 0 ; i < num_collisions ; ++i)
  {
    preprocess_collision(m_collisions[i], dt);
  }

  int step;
  for (step = 0 ; step < m_num_contact_iterations ; ++step)
  {
    bool got_one = false;
	// half the steps have epsilon < 0, half have it 0
//    Scalar epsilon = -1.0f + 2.0f * (step + 1)/ m_num_contact_iterations;
//    Scalar epsilon = -1.0f + (step + 1)/ m_num_contact_iterations;
//    if (epsilon > 0.0f) epsilon = 0.0f;
    Scalar epsilon = 0.0f;

    int i_start, i_end, i_dir;
    if (m_traverse_dir == TRAVERSE_FORWARD)
    {
      i_start = 0;
      i_end = num_collisions;
      i_dir = 1;
      m_traverse_dir = TRAVERSE_BACKWARD;
    }
    else
    {
      i_start = num_collisions - 1;
      i_end = -1;
      i_dir = -1;
      m_traverse_dir = TRAVERSE_FORWARD;
    }

    // Do all the contacts
    for (i = i_start ; i != i_end; i += i_dir)
    {
      // apply an inelastic collision to prevent penetration
      if (process_collision(m_collisions[i], dt, true, epsilon))
        got_one = true;
    }

    // do all the constraints
    for (i = 0 ; i < num_constraints ; ++i)
    {
      if (m_constraints[i]->apply(dt))
		    got_one = true;
    }

    if (!got_one)
      break;
  }

  TRACE_FILE_IF(FRAME_2)
    TRACE("iterations = %d\n", step);

  // separate penetrating objects
  for (step = 0 ; step < m_num_penetration_iterations ; ++step)
  {
    bool got_one = false;
    for (i = 0 ; i < num_collisions ; ++i)
    {
      if (m_collisions[i].penetration_depth > 0.0001f)
      {
        separate_objects(m_collisions[i], m_penetration_resolve_fraction);
        got_one = true;
      }
    }
    if (!got_one)
      break;
  }
}

//==============================================================
// do_all_external_forces
//==============================================================
void Physics::get_all_external_forces(Scalar dt)
{
  TRACE_METHOD_ONLY(MULTI_FRAME_1);
  int num_bodies = m_rigid_bodies.size();
  int i;
  for (i = 0 ; i < num_bodies ; ++i)
  {
    m_rigid_bodies[i]->add_external_forces(dt);
  }

  int num_controllers = m_controllers.size();
  for (i = 0 ; i < num_controllers ; ++i)
  {
    m_controllers[i]->update(dt);
  }
}

//==============================================================
// update_all_velocities
//==============================================================
void Physics::update_all_velocities(Scalar dt)
{
  TRACE_METHOD_ONLY(MULTI_FRAME_1);
  int num_bodies = m_rigid_bodies.size();
  int i;
  for (i = 0 ; i < num_bodies ; ++i)
  {
    m_rigid_bodies[i]->update_velocity(dt);
  }
}

//==============================================================
// clear_all_forces
//==============================================================
void Physics::clear_all_forces()
{
  TRACE_METHOD_ONLY(MULTI_FRAME_1);
  int num_bodies = m_rigid_bodies.size();
  int i;
  for (i = 0 ; i < num_bodies ; ++i)
  {
    m_rigid_bodies[i]->clear_forces();
  }
}

//==============================================================
// update_all_positions
//==============================================================
void Physics::update_all_positions(Scalar dt)
{
  TRACE_METHOD_ONLY(MULTI_FRAME_1);
  int num_bodies = m_rigid_bodies.size();
  int i;
  for (i = 0 ; i < num_bodies ; ++i)
  {
    m_rigid_bodies[i]->update_position(dt);
  }
}

//==============================================================
// try_to_freeze_all_objects
//==============================================================
void Physics::try_to_freeze_all_objects(Scalar dt)
{
  TRACE_METHOD_ONLY(MULTI_FRAME_1);
  int num_bodies = m_rigid_bodies.size();
  int i;
  for (i = 0 ; i < num_bodies ; ++i)
  {
    m_rigid_bodies[i]->try_to_freeze(dt);
  }
}

//==============================================================
// activate_all_frozen_objects_left_hanging
//==============================================================
void Physics::activate_all_frozen_objects_left_hanging()
{
  const int num_bodies = m_rigid_bodies.size();
  int i;
  for (i = 0 ; i < num_bodies ; ++i)
  {
    if ( (m_rigid_bodies[i]->get_activity_state() == Rigid_body::ACTIVE) &&
         (m_rigid_bodies[i]->collision_body()) )
    {
      Rigid_body * this_body = m_rigid_bodies[i];
      // first activate any bodies due to the movement of this body
      this_body->do_movement_activations();

      // now record any movement notifications that are needed
      std::vector<int> & collisions = m_rigid_bodies[i]->collision_body()->collisions();
      if (!collisions.empty())
      {
        // walk through the object's contact list
        int j;
        const int num_collisions = collisions.size();
        for (j = 0 ; j < num_collisions ; ++j)
        {
          assert1(collisions[j] < (int) m_collisions.size());
          const Collision_info & coll = m_collisions[collisions[j]];
          // must be a body-body interaction to be interesting
          if (coll.body1)
          {
            Rigid_body * other_body = coll.body0->rigid_body();
            Vector3 this_body_normal = coll.dir_to_0;
            if (other_body == this_body)
            {
              other_body = coll.body1->rigid_body();
              this_body_normal *= -1;
            }
            if (other_body->get_activity_state() == Rigid_body::FROZEN)
            {
              this_body->add_movement_activation(this_body->get_position(), other_body);
            }
          }
        }
      }
    }
  }
}

//==============================================================
// do_timestep
//==============================================================
void Physics::do_timestep(Scalar dt)
{
  TRACE_METHOD_ONLY(MULTI_FRAME_1);

  get_all_external_forces(dt);

  handle_all_collisions(dt);

  update_all_velocities(dt);

  clear_all_forces();

  handle_all_constraints(dt);

  if (m_freezing_enabled)
  {
    try_to_freeze_all_objects(dt);
    activate_all_frozen_objects_left_hanging();
  }
  update_all_positions(dt);
}

//==============================================================
// integrate
//==============================================================
void Physics::integrate(Scalar dt)
{
  TRACE_METHOD_ONLY(FRAME_1);

  dt *= m_time_scale;

  Scalar total_time = dt + m_overlap_time;

  if (total_time > 0.1f)
  {
    TRACE("dt > 0.1f\n");
    total_time = 0.1f;
  }
  
  // split the timestep into fixed size chunks

  int num_loops = (int) (total_time / m_timestep);
  Scalar timestep = m_timestep;
  
  if (m_allow_smaller_timesteps)
  {
    if (num_loops == 0)
      num_loops = 1;
    timestep = total_time / num_loops;
  }
    
  m_overlap_time = total_time - num_loops * timestep;

  int i;
  for (i = 0 ; i < num_loops ; ++i)
  {
    m_physics_time += timestep;
    do_timestep(timestep);
  }
}

//==============================================================
// enable_freezing
//==============================================================
void Physics::enable_freezing(bool freeze)
{
  m_freezing_enabled = freeze;

  if (!m_freezing_enabled)
  {
    int num_bodies = m_rigid_bodies.size();
    int i;
    for (i = 0 ; i < num_bodies ; ++i)
    {
      m_rigid_bodies[i]->set_activity_state(Rigid_body::ACTIVE);
    }
  }
}
