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); } }