#ifndef CONSTRAINT_BODY_BODY_POINT_HPP
#define CONSTRAINT_BODY_BODY_POINT_HPP

#include "constraint.hpp"

class Rigid_body;

/// Constraints a point on one body to be fixed to a point on another body
class Constraint_body_body_point : public Constraint
{
public:
  /// can set things up in the constructor...
  /// allowed_distance indicated how much the points are allowed to deviate.
  /// timescale indicates the timescale over which deviation is eliminated
  /// (suggest a few times dt - be careful if there's a variable timestep!)
  /// if timescale < 0 then the value indicates the number of dts
  Constraint_body_body_point(Rigid_body * body0, Position body0_pos,
                             Rigid_body * body1, Position body1_pos,
                             Scalar allowed_distance,
                             Scalar timescale);
  /// A default constructor - but make sue you call initialise
  /// before using!
  Constraint_body_body_point();
  
  void initialise(Rigid_body * body0, Position body0_pos,
                  Rigid_body * body1, Position body1_pos,
                  Scalar allowed_distance,
                  Scalar timescale);
  
  /// gets called to set things up for a series of calls to apply
  void pre_apply(Scalar dt);

  // apply impulses to satisfy the constraint.
  bool apply(Scalar dt);
private:
  Position m_body0_pos;
  Rigid_body * m_body0;
  Position m_body1_pos;
  Rigid_body * m_body1;
  Scalar m_allowed_distance;
  Scalar m_timescale;

  // some values that we calculate once in pre_apply
  Position m_world_pos; // average of the two joint positions
  Position m_r0; // position relative to body 0 (in world space)
  Position m_r1;
  Velocity m_vr_extra; // extra vel for restoring deviation
};

#endif
