void robot_model::FloatingJointModel::getVariableRandomValues(random_numbers::RandomNumberGenerator &rng, std::vector<double> &values, const Bounds &bounds) const
{
  std::size_t s = values.size();
  values.resize(s + 7);
  
  if (bounds[0].second >= std::numeric_limits<double>::max() || bounds[0].first <= -std::numeric_limits<double>::max())
    values[s] = 0.0;
  else
    values[s] = rng.uniformReal(bounds[0].first, bounds[0].second);
  if (bounds[1].second >= std::numeric_limits<double>::max() || bounds[1].first <= -std::numeric_limits<double>::max())
    values[s + 1] = 0.0;
  else
    values[s + 1] = rng.uniformReal(bounds[1].first, bounds[1].second);
  if (bounds[2].second >= std::numeric_limits<double>::max() || bounds[2].first <= -std::numeric_limits<double>::max())
    values[s + 2] = 0.0;
  else
    values[s + 2] = rng.uniformReal(bounds[2].first, bounds[2].second);

  double q[4]; rng.quaternion(q);
  values[s + 3] = q[0];
  values[s + 4] = q[1];
  values[s + 5] = q[2];
  values[s + 6] = q[3];
}
void robot_model::FloatingJointModel::getVariableRandomValuesNearBy(random_numbers::RandomNumberGenerator &rng, std::vector<double> &values, const Bounds &bounds,
                                                                        const std::vector<double> &near, const double distance) const
{   
  std::size_t s = values.size();
  values.resize(s + 7);
  
  if (bounds[0].second >= std::numeric_limits<double>::max() || bounds[0].first <= -std::numeric_limits<double>::max())
    values[s] = 0.0;
  else
    values[s] = rng.uniformReal(std::max(bounds[0].first, near[s] - distance),
                                std::min(bounds[0].second, near[s] + distance));
  if (bounds[1].second >= std::numeric_limits<double>::max() || bounds[1].first <= -std::numeric_limits<double>::max())
    values[s + 1] = 0.0;
  else
    values[s + 1] = rng.uniformReal(std::max(bounds[1].first, near[s + 1] - distance),
                                    std::min(bounds[1].second, near[s + 1] + distance));
  if (bounds[2].second >= std::numeric_limits<double>::max() || bounds[2].first <= -std::numeric_limits<double>::max())
    values[s + 2] = 0.0;
  else
    values[s + 2] = rng.uniformReal(std::max(bounds[2].first, near[s + 2] - distance),
                                    std::min(bounds[2].second, near[s + 2] + distance));

  double da = angular_distance_weight_ * distance;  
  if (da >= .25 * boost::math::constants::pi<double>())
  {
    double q[4]; rng.quaternion(q);
    values[s + 3] = q[0];
    values[s + 4] = q[1];
    values[s + 5] = q[2];
    values[s + 6] = q[3];
  }
  else
  { 
    //taken from OMPL
    // sample angle & axis
    double ax = rng.gaussian01();
    double ay = rng.gaussian01();
    double az = rng.gaussian01();
    double angle = 2.0 * pow(rng.uniform01(), 1.0/3.0) * da;
    // convert to quaternion
    double q[4];
    double norm = sqrt(ax * ax + ay * ay + az * az);
    if (norm < 1e-6)
    {
      q[0] = q[1] = q[2] = 0.0;
      q[3] = 1.0;
    }
    else
    {
      double s = sin(angle / 2.0);
      q[0] = s * ax / norm;
      q[1] = s * ay / norm;
      q[2] = s * az / norm;
      q[3] = cos(angle / 2.0);
    }
    // multiply quaternions: near * q
    values[s + 3] = near[s + 6]*q[0] + near[s + 3]*q[3] + near[s + 4]*q[2] - near[s + 5]*q[1];
    values[s + 4] = near[s + 6]*q[1] + near[s + 4]*q[3] + near[s + 5]*q[0] - near[s + 3]*q[2];
    values[s + 5] = near[s + 6]*q[2] + near[s + 5]*q[3] + near[s + 3]*q[1] - near[s + 4]*q[0];
    values[s + 6] = near[s + 6]*q[3] - near[s + 3]*q[0] - near[s + 4]*q[1] - near[s + 5]*q[2];
  }
}