void midpoint_integrate_impl(
     const StateSpace& space,
     const StateSpaceSystem& sys,
     const typename pp::topology_traits<StateSpace>::point_type& start_point,
     typename pp::topology_traits<StateSpace>::point_type& end_point,
     const InputTrajectory& u_traj,
     double start_time,
     double end_time,
     double time_step) {
   typedef typename pp::topology_traits<StateSpace>::point_type PointType;
   typedef typename pp::topology_traits<StateSpace>::point_difference_type PointDiffType;
   
   typedef typename pp::spatial_trajectory_traits<InputTrajectory>::const_waypoint_descriptor InputWaypoint;
   typedef typename pp::spatial_trajectory_traits<InputTrajectory>::point_type InputType;
   std::pair< InputWaypoint, InputType> u_wp = u_traj.get_waypoint_at_time(start_time);
   
   PointDiffType dp = sys.get_state_derivative(space, start_point, u_wp.second.pt, start_time);
   double t = start_time;
   end_point = start_point;
   
   while(((time_step > 0.0) && (t < end_time)) || 
         ((time_step < 0.0) && (t > end_time))) {
     PointType mid_p = space.adjust(end_point, (0.5 * time_step) * dp);
     t += 0.5 * time_step;
     u_wp = u_traj.move_time_diff_from(u_wp, 0.5 * time_step);
     dp = sys.get_state_derivative(space, mid_p, u_wp.second.pt, t);
     
     end_point = space.adjust(end_point, time_step * dp);
     t += 0.5 * time_step;
     u_wp = u_traj.move_time_diff_from(u_wp, 0.5 * time_step);
     dp = sys.get_state_derivative(space, end_point, u_wp.second.pt, t);
   };
 };
typename boost::enable_if< 
  is_readable_matrix<Matrix>,
typename pp::topology_traits<StateSpace>::point_type >::type 
  sample_gaussian_point(const StateSpace& space, 
                        const typename pp::topology_traits<StateSpace>::point_type& mean, 
                        const Matrix& cov) {
  typedef typename pp::topology_traits<StateSpace>::point_difference_type DiffType;
  typedef typename mat_traits<Matrix>::value_type ValueType;
  typedef typename mat_traits<Matrix>::size_type SizeType;
  using std::sqrt;
  using ReaK::to_vect;
  using ReaK::from_vect;
  
  mat< ValueType, mat_structure::square> L;
  try {
    decompose_Cholesky(cov,L);
  } catch(singularity_error&) { 
    mat< ValueType, mat_structure::diagonal> E(cov.get_row_count());
    mat< ValueType, mat_structure::square> U(cov.get_row_count()), V(cov.get_row_count());
    decompose_SVD(cov,U,E,V);
    for(SizeType i = 0; i < cov.get_row_count(); ++i)
      E(i,i) = sqrt(E(i,i));
    L = U * E;
  };
  
  boost::variate_generator< global_rng_type&, boost::normal_distribution<ValueType> > var_rnd(get_global_rng(), boost::normal_distribution<ValueType>());
  
  vect_n<ValueType> z = to_vect<ValueType>(space.difference(mean, mean));
  for(SizeType i = 0; i < z.size(); ++i)
    z[i] = var_rnd();
  
  return space.adjust(mean, from_vect<DiffType>(L * z));
};
 void runge_kutta4_integrate_impl(
     const StateSpace& space,
     const StateSpaceSystem& sys,
     const typename pp::topology_traits<StateSpace>::point_type& start_point,
     typename pp::topology_traits<StateSpace>::point_type& end_point,
     const InputTrajectory& u_traj,
     double start_time,
     double end_time,
     double time_step) {
   typedef typename pp::topology_traits<StateSpace>::point_type PointType;
   typedef typename pp::topology_traits<StateSpace>::point_difference_type PointDiffType;
   
   typedef typename pp::spatial_trajectory_traits<InputTrajectory>::const_waypoint_descriptor InputWaypoint;
   typedef typename pp::spatial_trajectory_traits<InputTrajectory>::point_type InputType;
   std::pair< InputWaypoint, InputType> u_wp = u_traj.get_waypoint_at_time(start_time);
   
   PointDiffType dp = sys.get_state_derivative(space, start_point, u_wp.second.pt, start_time);
   double t = start_time;
   end_point = start_point;
   
   while(((time_step > 0.0) && (t < end_time)) || 
         ((time_step < 0.0) && (t > end_time))) {
   
     PointType w = end_point;
     PointDiffType k1 = time_step * dp;
     end_point = space.adjust(end_point, 0.5 * k1);
     
     t += time_step * 0.5;
     u_wp = u_traj.move_time_diff_from(u_wp, 0.5 * time_step);
     dp = sys.get_state_derivative(space, end_point, u_wp.second.pt, t);
     PointDiffType k2 = time_step * dp;
     end_point = space.adjust(w, 0.5 * k2);
     
     dp = sys.get_state_derivative(space, end_point, u_wp.second.pt, t);
     PointDiffType k3 = time_step * dp;
     end_point = space.adjust(w, k3);
     
     t += time_step * 0.5;
     u_wp = u_traj.move_time_diff_from(u_wp, 0.5 * time_step);
     dp = sys.get_state_derivative(space, end_point, u_wp.second.pt, t);
     end_point = space.adjust(end_point, (1.0 / 6.0) * k1 + (2.0 / 6.0) * k2 + (time_step / 6.0) * dp - (2.0/3.0) * k3);
     
     dp = sys.get_state_derivative(space, end_point, u_wp.second.pt, t);
   };
 };
typename pp::topology_traits<StateSpace>::point_type 
  sample_gaussian_point(const StateSpace& space, 
                        const typename pp::topology_traits<StateSpace>::point_type& mean, 
                        const mat<ValueType, mat_structure::diagonal>& cov) {
  typedef typename pp::topology_traits<StateSpace>::point_difference_type DiffType;
  typedef typename mat_traits< mat<ValueType, mat_structure::diagonal> >::size_type SizeType;
  using std::sqrt;
  using ReaK::to_vect;
  using ReaK::from_vect;
  
  boost::variate_generator< global_rng_type&, boost::normal_distribution<ValueType> > var_rnd(get_global_rng(), boost::normal_distribution<ValueType>());
  
  vect_n<ValueType> z = to_vect<ValueType>(space.difference(mean, mean));
  for(SizeType i = 0; i < z.size(); ++i)
    z[i] = var_rnd() * sqrt(cov(i,i));
  
  return space.adjust(mean, from_vect<DiffType>(z));
};
 void dormand_prince45_integrate_impl(
     const StateSpace& space,
     const StateSpaceSystem& sys,
     const typename pp::topology_traits<StateSpace>::point_type& start_point,
     typename pp::topology_traits<StateSpace>::point_type& end_point,
     const InputTrajectory& u_traj,
     double start_time,
     double end_time,
     double time_step,
     double tolerance,
     double min_step,
     double max_step) {
   using std::fabs;
   using std::pow;
   using ReaK::to_vect;
   typedef typename pp::topology_traits<StateSpace>::point_type PointType;
   typedef typename pp::topology_traits<StateSpace>::point_difference_type PointDiffType;
   
   if ((time_step == 0.0) ||
       ((time_step > 0.0) && (start_time > end_time)) ||
       ((time_step < 0.0) && (end_time > start_time)) ||
       (tolerance <= 0.0) ||
       (min_step > max_step))
     throw impossible_integration(start_time, end_time, time_step);
   
   typedef typename pp::spatial_trajectory_traits<InputTrajectory>::const_waypoint_descriptor InputWaypoint;
   typedef typename pp::spatial_trajectory_traits<InputTrajectory>::point_type InputType;
   std::pair< InputWaypoint, InputType> u_wp = u_traj.get_waypoint_at_time(start_time);
   
   PointDiffType dp = sys.get_state_derivative(space, start_point, u_wp.second.pt, start_time);
   double t = start_time;
   end_point = start_point;
   
   while(((time_step > 0.0) && (t < end_time)) || 
         ((time_step < 0.0) && (t > end_time))) {
     
     PointType prevY = end_point;
     PointDiffType k1 = time_step * dp;
     end_point = space.adjust(end_point, 0.2 * k1);
     
     t += time_step / 5.0;
     u_wp = u_traj.move_time_diff_from(u_wp, time_step / 5.0);
     dp = sys.get_state_derivative(space, end_point, u_wp.second.pt, t);
     PointDiffType k2 = time_step * dp;
     end_point = space.adjust(prevY, (3.0 / 40.0) * k1 + (9.0 / 40.0) * k2);
     
     t += time_step / 10.0;
     u_wp = u_traj.move_time_diff_from(u_wp, time_step / 10.0);
     dp = sys.get_state_derivative(space, end_point, u_wp.second.pt, t);
     PointDiffType k3 = time_step * dp;
     end_point = space.adjust(prevY, (44.0 / 45.0) * k1 - (56.0 / 15.0) * k2 + (32.0 / 9.0) * k3);
     
     t += time_step / 2.0;
     u_wp = u_traj.move_time_diff_from(u_wp, time_step / 2.0);
     dp = sys.get_state_derivative(space, end_point, u_wp.second.pt, t);
     PointDiffType k4 = time_step * dp;
     end_point = space.adjust(prevY, (19372.0 / 6561.0) * k1 - (25360.0 / 2187.0) * k2 + (64448.0 / 6561.0) * k3 - (212.0 / 729.0) * k4);
     
     t += 4.0 * time_step / 45.0;
     u_wp = u_traj.move_time_diff_from(u_wp, 4.0 * time_step / 45.0);
     dp = sys.get_state_derivative(space, end_point, u_wp.second.pt, t);
     PointDiffType k5 = time_step * dp;
     end_point = space.adjust(prevY, (9017.0 / 3168.0) * k1 - (355.0 / 33.0) * k2 - (46732.0 / 5247.0) * k3 + (49.0 / 176.0) * k4 - (5103.0 / 18656.0) * k5);
     
     t += time_step / 9.0;
     u_wp = u_traj.move_time_diff_from(u_wp, time_step / 9.0);
     dp = sys.get_state_derivative(space, end_point, u_wp.second.pt, t);
     PointDiffType k6 = time_step * dp;
     end_point = space.adjust(prevY, (35.0 / 384.0) * k1 + (500.0 / 1113.0) * k3 + (125.0 / 192.0) * k4 - (2187.0 / 6784.0) * k5 + (11.0 / 84.0) * k6);
     
     dp = sys.get_state_derivative(space, end_point, u_wp.second.pt, t);
     PointDiffType k7 = time_step * dp;
     
     vect_n<double> err_vect = to_vect<double>((5170.0 / 57600.0) * k1 + (7571.0 / 16695.0) * k3 + (393.0 / 640.0) * k4 - (92097.0 / 339200.0) * k5 + (187.0 / 2100.0) * k6 - (39.0 / 40.0) * k7);
     double Rmax = 0.0;
     std::size_t worst_DOF = 0;
     for(std::size_t i = 0; i < err_vect.size(); ++i) {
       double R = fabs(err_vect[i] / time_step);
       if(R > Rmax) {
         Rmax = R;
         worst_DOF = i;
       };
     };
     
     if(Rmax > tolerance) {
       if(fabs(time_step) <= min_step)
         throw untolerable_integration(tolerance, Rmax, worst_DOF, time_step, t);
       
       t -= time_step;
       u_wp = u_traj.move_time_diff_from(u_wp, -time_step);
       end_point = prevY;
       dp = sys.get_state_derivative(space, end_point, u_wp.second.pt, t);
       double R = 0.84 * pow(tolerance / Rmax, 0.25);
       if(R < 0.1)
         time_step *= 0.1;
       else
         time_step *= R;
     } else {
       end_point = space.adjust(prevY, (5170.0 / 57600.0) * k1 + (7571.0 / 16695.0) * k3 + (393.0 / 640.0) * k4 - (92097.0 / 339200.0) * k5 + (187.0 / 2100.0) * k6 + (1.0 / 40.0) * k7);
       dp = sys.get_state_derivative(space, end_point, u_wp.second.pt, t);
       
       double R = 0.84 * pow(tolerance / Rmax, 0.25);
       if(R >= 4.0)
         time_step *= 4.0;
       else if(R > 1.0)
         time_step *= R;
       //if(((time_step > 0.0) && (t + time_step > end_time)) || ((time_step < 0.0) && (t + time_step < end_time)))
         //time_step = end_time - t;
     };
     
     if(fabs(time_step) < min_step)
       time_step *= fabs(min_step / time_step);
     if(fabs(time_step) > max_step)
       time_step *= fabs(max_step / time_step);
   };
 };
 void hamming_mod_integrate_impl(
     const StateSpace& space,
     const StateSpaceSystem& sys,
     const typename pp::topology_traits<StateSpace>::point_type& start_point,
     typename pp::topology_traits<StateSpace>::point_type& end_point,
     const InputTrajectory& u_traj,
     double start_time,
     double end_time,
     double time_step) {
   if ((time_step == 0.0) ||
       ((time_step > 0.0) && (start_time > end_time)) ||
       ((time_step < 0.0) && (start_time < end_time)))
     throw impossible_integration(start_time, end_time, time_step);
   
   typedef typename pp::topology_traits<StateSpace>::point_type PointType;
   typedef typename pp::topology_traits<StateSpace>::point_difference_type PointDiffType;
   
   typedef typename pp::spatial_trajectory_traits<InputTrajectory>::const_waypoint_descriptor InputWaypoint;
   typedef typename pp::spatial_trajectory_traits<InputTrajectory>::point_type InputType;
   std::pair< InputWaypoint, InputType> u_wp = u_traj.get_waypoint_at_time(start_time);
   
   double back_time = start_time;
   PointDiffType dp = sys.get_state_derivative(space, start_point, u_wp.second.pt, start_time);
   PointType w = start_point;
   PointType Y_n[3];
   PointDiffType Yp_n[3];
   
   for(std::size_t i = 0; i < 3; ++i) {
     PointDiffType k1 = time_step * dp;
     Y_n[i] = space.adjust(w, -0.25 * k1);
     
     back_time -= time_step * 0.25;
     u_wp = u_traj.move_time_diff_from(u_wp, -0.25 * time_step);
     Yp_n[i] = sys.get_state_derivative(space, Y_n[i], u_wp.second.pt, back_time);
     PointDiffType k2 = time_step * Yp_n_1;
     Y_n[i] = space.adjust(Y_n[i], (5.0 / 32.0) * k1 - (9.0 / 32.0) * k2);
     
     back_time -= time_step * 0.125;
     u_wp = u_traj.move_time_diff_from(u_wp, -0.125 * time_step);
     Yp_n[i] = sys.get_state_derivative(space, Y_n[i], u_wp.second.pt, back_time);
     PointDiffType k3 = time_step * Yp_n[i];
     Y_n[i] = space.adjust(Y_n[i], (1250865.0 / 351520.0) * k2 - (276165.0 / 351520.0) * k1 - (1167360.0 / 351520.0) * k3);
     
     back_time -= 57.0 * time_step / 104.0;
     u_wp = u_traj.move_time_diff_from(u_wp, -57.0 / 104.0 * time_step);
     Yp_n[i] = sys.get_state_derivative(space, Y_n[i], u_wp.second.pt, back_time);
     PointDiffType k4 = time_step * Yp_n[i];
     Y_n[i] = space.adjust(w, 8.0 * k2 - (439.0 / 216.0) * k1 - (3680.0 / 513.0) * k3 + (845.0 / 4104.0) * k4);
     
     back_time -= time_step / 13.0;
     u_wp = u_traj.move_time_diff_from(u_wp, -time_step / 13.0);
     Yp_n[i] = sys.get_state_derivative(space, Y_n[i], u_wp.second.pt, back_time);
     PointDiffType k5 = time_step * Yp_n[i];
     Y_n[i] = space.adjust(w, (8.0 / 27.0) * k1 - 2.0 * k2 + (3544.0 / 2565.0) * k3 - (1859.0 / 4104.0) * k4 + (11.0 / 40.0) * k5);
     
     back_time += time_step * 0.5;
     u_wp = u_traj.move_time_diff_from(u_wp, 0.5 * time_step);
     Yp_n[i] = sys.get_state_derivative(space, Y_n[i], u_wp.second.pt, back_time);
     Y_n[i] = space.adjust(w, (-16.0 / 135.0) * k1 - (6656.0 / 12825.0) * k3 - (28561.0 / 56430.0) * k4 + (9.0 / 50.0) * k5 - (2.0 * time_step / 55.0) * Yp_n[i]);
     
     back_time -= time_step * 0.5;
     u_wp = u_traj.move_time_diff_from(u_wp, -0.5 * time_step);
     Yp_n[i] = sys.get_state_derivative(space, Y_n[i], u_wp.second.pt, back_time);
     
     w = Y_n[i];
     dp = Yp_n[i];
   };
   
   PointType C = start_point;
   PointType P_n = start_point; // this makes sure the initial error sweep is zero.
   
   double t = start_time;
   end_point = start_point;
   u_wp = u_traj.get_waypoint_at_time(t);
   dp = sys.get_state_derivative(space, end_point, u_wp.second.pt, t);
   
   while(((time_step > 0.0) && (t < end_time)) || 
         ((time_step < 0.0) && (t > end_time))) {
     
     PointType P = space.adjust(Y_n[2], (time_step * 4.0 / 3.0) * (2.0 * (dp + Yp_n[1]) - Yp_n[0]));
     PointType M = space.adjust(P, (112.0 / 121.0) * space.difference(C, P_n));
     
     t += time_step;
     u_wp = u_traj.move_time_diff_from(u_wp, time_step);
     PointDiffType Mp = sys.get_state_derivative(space, M, u_wp.second.pt, t);
     
     C = space.adjust(end_point, 0.125 * space.difference(end_point, Y_n[1]) + (time_step * 3.0 / 8.0) * (Mp + 2.0 * dp - Yp_n[0]));
     
     Y_n[2] = Y_n[1];
     Y_n[1] = Y_n[0];
     Y_n[0] = end_point;
     end_point = space.adjust(C, (9.0 / 121.0) * space.difference(P, C));
     P_n = P;
     
     Yp_n[1] = Yp_n[0];
     Yp_n[0] = dp;
     dp = sys.get_state_derivative(space, end_point, u_wp.second.pt, t);
     
   };
 };