Skip to content

[RSDK-13550] Apply colinearization before computing blends#189

Merged
acmorrow merged 2 commits intoviam-modules:mainfrom
acmorrow:RSDK-13550-fix-colinear
Mar 10, 2026
Merged

[RSDK-13550] Apply colinearization before computing blends#189
acmorrow merged 2 commits intoviam-modules:mainfrom
acmorrow:RSDK-13550-fix-colinear

Conversation

@acmorrow
Copy link
Contributor

There was a tricky bug that only showed up when using path colinearization. Basically, as a side effect of colinearization, we were allowing linear segment growth to change the tangent angle, resulting in tangent discontinuities at C-L and L-C junctions, which forced us to s_dot=0: very slow.

Instead, colinearize first, then blend.

@acmorrow acmorrow requested review from JohnN193 and nfranczak March 10, 2026 17:16
Copy link
Contributor

@nfranczak nfranczak left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

needs make format

std::optional<waypoint_accumulator> colinearized;
if (opts.max_linear_deviation() != 0.0) {
std::vector<decltype(waypoints_range.begin())> skipped;
for (auto base = waypoints_range.begin(); base != waypoints_range.end();) {
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

mutters quietly to themselves about ranges and pipes.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I tried! I really did. But the stateful nature of it makes it not play super well with ranges.

if (!can_coalesce(*base, *candidate, *target)) {
break;
}
if (!std::ranges::all_of(skipped, [&](auto elt) { return can_coalesce(*base, *elt, *target); })) {
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

elt?

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

element

@acmorrow acmorrow merged commit 4dbdc06 into viam-modules:main Mar 10, 2026
7 checks passed
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Labels

None yet

Projects

None yet

Development

Successfully merging this pull request may close these issues.

3 participants