void url::
rebuild(const boost::string_ref &scheme,
const boost::optional<boost::string_ref> &host,
const boost::optional<uint16_t> &port,
const boost::optional<boost::string_ref> &path,
const boost::optional<boost::string_ref> &query,
const boost::optional<boost::string_ref> &fragment,
const boost::optional<boost::string_ref> &user_info)
{
std::string str;
str.append(scheme.data(), scheme.size());
if (has_authority()) {
str.append("://");
if (user_info) {
str.append(user_info->data(), user_info->size());
str.append("@");
}
str.append(host->data(), host->size());
if (port) {
str.append(":");
str.append(std::to_string(*port));
}
}
else {
str.append(":");
}
if (path) {
str.append(path->data(), path->size());
}
if (query) {
str.append("?");
str.append(query->data(), query->size());
}
if (fragment) {
str.append("#");
str.append(fragment->data(), fragment->size());
}
url new_url { std::move(str) };
swap(new_url);
}
face_set_ptr face_manager<T>::get_face_set(const std::string &name, boost::optional<font_set> fset)
{
if (fset && fset->size() > 0)
{
return get_face_set(*fset);
}
else
{
return get_face_set(name);
}
}
vector<CubicBezierControlPoints> RRTPlanner::generateCubicBezierPath(
const vector<Geometry2d::Point>& points,
const MotionConstraints& motionConstraints, Geometry2d::Point vi,
Geometry2d::Point vf, const boost::optional<vector<float>>& times) {
size_t length = points.size();
size_t curvesNum = length - 1;
vector<double> pointsX(length);
vector<double> pointsY(length);
vector<double> ks(length - 1);
vector<double> ks2(length - 1);
for (int i = 0; i < length; i++) {
pointsX[i] = points[i].x;
pointsY[i] = points[i].y;
}
const float startSpeed = vi.mag();
const float endSpeed = vf.mag();
if (times) {
assert(times->size() == points.size());
for (int i = 0; i < curvesNum; i++) {
ks[i] = 1.0 / (times->at(i + 1) - times->at(i));
ks2[i] = ks[i] * ks[i];
if (std::isnan(ks[i])) {
debugThrow(
"Something went wrong. Points are too close to each other "
"probably");
return vector<CubicBezierControlPoints>();
}
}
} else {
for (int i = 0; i < curvesNum; i++) {
ks[i] = 1.0 / (getTime(points, i + 1, motionConstraints, startSpeed,
endSpeed) -
getTime(points, i, motionConstraints, startSpeed,
endSpeed));
ks2[i] = ks[i] * ks[i];
if (std::isnan(ks[i])) {
debugThrow(
"Something went wrong. Points are too close to each other "
"probably");
return vector<CubicBezierControlPoints>();
}
}
}
VectorXd solutionX =
RRTPlanner::cubicBezierCalc(vi.x, vf.x, pointsX, ks, ks2);
VectorXd solutionY =
RRTPlanner::cubicBezierCalc(vi.y, vf.y, pointsY, ks, ks2);
vector<CubicBezierControlPoints> path;
for (int i = 0; i < curvesNum; i++) {
Point p0 = points[i];
Point p1 = Geometry2d::Point(solutionX(i * 2), solutionY(i * 2));
Point p2 =
Geometry2d::Point(solutionX(i * 2 + 1), solutionY(i * 2 + 1));
Point p3 = points[i + 1];
path.emplace_back(p0, p1, p2, p3);
}
return path;
}
