unique_ptr<Path> CompositePath::clone() const {
CompositePath* newPath = new CompositePath();
for (const unique_ptr<Path>& path : paths) {
newPath->append(path->clone());
}
return unique_ptr<Path>(newPath);
}
TEST(CompositePath, CompositeSubPath) {
// Create a test path
InterpolatedPath path;
path.waypoints.emplace_back(MotionInstant(Point(1, 0), Point(0, 0)), 0s);
path.waypoints.emplace_back(MotionInstant(Point(1, 2), Point(-1, -1)), 1s);
path.waypoints.emplace_back(MotionInstant(Point(-2, 19), Point(1, 1)), 3s);
path.waypoints.emplace_back(MotionInstant(Point(1, 6), Point(0, 0)), 9s);
// Create 6 subPaths and rejoin them together into one compositePath
CompositePath compositePath;
auto diff = 1500ms;
for (auto i = 0ms; i < 7500ms; i += diff) {
compositePath.append(path.subPath(i, i + diff));
}
compositePath.append(path.subPath(7500ms));
// Compare that the compositePath and origonal path are mostly equal
for (RJ::Seconds i = 0ms; i <= 10s; i += 1ms) {
auto org = path.evaluate(i);
auto sub = compositePath.evaluate(i);
if (!org && !sub) break;
ASSERT_TRUE(org);
ASSERT_TRUE(sub);
EXPECT_NEAR(org->motion.vel.x(), sub->motion.vel.x(), 0.000001)
<< "i=" << to_string(i);
EXPECT_NEAR(org->motion.vel.y(), sub->motion.vel.y(), 0.000001)
<< "i=" << to_string(i);
EXPECT_NEAR(org->motion.pos.x(), sub->motion.pos.x(), 0.000001)
<< "i=" << to_string(i);
EXPECT_NEAR(org->motion.pos.y(), sub->motion.pos.y(), 0.00001)
<< "i=" << to_string(i);
}
// Create 9 subPaths from the compositePaths
vector<unique_ptr<Path>> subPaths;
diff = 1000ms;
for (auto i = 0ms; i < 8s; i += diff) {
subPaths.push_back(compositePath.subPath(i, i + diff));
}
subPaths.push_back(compositePath.subPath(8000ms));
// Compare the subPaths of the compositePaths to the origional path and
// check that the results of evaluating the paths are close enough
for (int i = 0; i < 9; i++) {
for (auto j = 0ms; j < 1s; j += 100ms) {
auto time = i * 1000ms + j;
auto org = path.evaluate(time);
auto sub = subPaths[i]->evaluate(j);
ASSERT_TRUE(org);
ASSERT_TRUE(sub);
EXPECT_NEAR(org->motion.vel.x(), sub->motion.vel.x(), 0.000001)
<< "newPathTime=" << to_string(time);
EXPECT_NEAR(org->motion.vel.y(), sub->motion.vel.y(), 0.000001)
<< "newPathTime=" << to_string(time);
EXPECT_NEAR(org->motion.pos.x(), sub->motion.pos.x(), 0.000001)
<< "newPathTime=" << to_string(time);
EXPECT_NEAR(org->motion.pos.y(), sub->motion.pos.y(), 0.00001)
<< "newPathTime=" << to_string(time);
}
}
}
unique_ptr<Path> CompositePath::subPath(float startTime, float endTime) const {
// Check for valid arguments
if (startTime < 0) {
throw invalid_argument("CompositePath::subPath(): startTime(" +
to_string(startTime) +
") can't be less than zero");
}
if (endTime < 0) {
throw invalid_argument("CompositePath::subPath(): endTime(" +
to_string(endTime) +
") can't be less than zero");
}
if (startTime > endTime) {
throw invalid_argument(
"CompositePath::subPath(): startTime(" + to_string(startTime) +
") can't be after endTime(" + to_string(endTime) + ")");
}
if (startTime >= duration) {
debugThrow(invalid_argument("CompositePath::subPath(): startTime(" +
to_string(startTime) +
") can't be greater than the duration(" +
to_string(duration) + ") of the path"));
return unique_ptr<Path>(new CompositePath());
}
if (startTime == 0 && endTime >= duration) {
return this->clone();
}
// Find the first Path in the vector of paths which will be included in the
// subPath
size_t start = 0;
float time = 0;
float lastTime = 0;
while (time <= startTime) {
lastTime = paths[start]->getDuration();
time += lastTime;
start++;
}
// Get the time into the Path in the vector of paths which the subPath will
// start
float firstStartTime = (time - lastTime);
// If the path will only contain that one Path just return a subPath of that
// Path
if (time >= endTime) {
return paths[start - 1]->subPath(startTime - firstStartTime,
endTime - firstStartTime);
} else {
// Create a CompositePath initialized with only that first path.
CompositePath* path = new CompositePath(
paths[start - 1]->subPath(startTime - firstStartTime));
unique_ptr<Path> lastPath;
size_t end;
// Find the last Path in the vector of paths which will be included in
// the subPath and store it in lastPath
if (endTime >= duration) {
lastPath = paths.back()->clone();
end = paths.size() - 1;
} else {
end = start;
while (time < endTime) {
lastTime = paths[start]->getDuration();
time += lastTime;
end++;
}
end--;
lastPath = paths[end]->subPath(0, endTime - (time - lastTime));
}
// Add the ones in the middle
while (start < end) {
path->append(paths[start]->clone());
start++;
}
// Add the last one
path->append(std::move(lastPath));
return unique_ptr<Path>(path);
}
}
