Reduced heap allocation for Quintic Bezier Function

[pepbos]

This PR changes the type of the quintic bezier control points to a Vec6 (instead of a dynamic Vector).

As the name suggests, the quintic bezier toolkit works with six-control points.
These control points are currently stored in either a SimTK::Vector or in the columns of a SimTK::Matrix.
Changing these to use a SimTK::Vec6 or a std::vector<SimTK::Vec6> respectively reduces runtime heap-allocation, resulting in 8% speed increase (wall clock time) when simulating Rajagopal model.

Brief summary of changes

• in SegmentedQuinticBezierToolkit:
  • changed uses of SimTK::Vector to SimTK::Vec6,
  • changed most uses of SimTK::Matrix to std::vector<SimTK::Vec6>,
  • changed return value of calcQuinticBezierCornerControlPoints to a std::pair instead of SimTK::Matrix with two columns.
• renamed mX and mY variable names to ctrlPtsX and ctrlPtsY respectively.
• updated SmoothSegmentedFunction, SmoothSegmentedFunctionFactory and testSmoothSegmentedFunctionFactory to fix interfacing to the above.

Testing I've completed

• Passed the test in testSmoothSegmentedFunctionFactory.
• Rajagopal simulated motion still the same.

CHANGELOG.md

• updated.

---

This change is 

[pepbos]

Hi, @adamkewley @nickbianco @carmichaelong , the unit test still failing on unrelated issue, but perhaps any of you would already like to take a look at this PR?

[adamkewley]

I couldn't see anything that fundamentally changes the behavior/algorithm (and the fact that high-level tests etc. still pass for such a fundamental class change indicates that it probably behaves the same).

I'd suggest, if there is time (this is much less important) a separate PR in which X, Y, these Vec6's etc. are tied into more expressive types than they currently are. A lot of the code is messy (pre- and post-PR) because a lot of it is juggling those types around rather than designing specific types for each part of the problem.

E.g. std::pair<SimTK::Vec6, SimTK::Vec6> is an improvement over the original return value, but it might be better (depending on what "Vec6" means here) to use explicit types:

struct QuinticBezierControlPoint final {
    // 6 double values with appropriate names, if they aren't a truly 6-d quantity
};

struct QuinticBezierSurfacePoint final {
    QuinticBezierControlPoint x;
    QuinticBezierControlPoint y;
};

std::vector<QuinticBezierSurfacePoint> controlPoints;

// etc.

[pepbos]

Thanks @adamkewley I think I got most of your comments in a straightforward way. Perhaps check:
#3481 (comment)
#3481 (comment)
if you agree with the solution

I agree with your point that the XY points are probably a Vec2 (or something like that), but indeed better to set up a new pr (I can look into this).

Thanks!

[nickbianco]

The failures on Windows are related to some issue with the CI finding the SWIG install. Working on a fix for that now. The Mac failure was due to a CMC test that is notorious for failing sporadically (we should fix that at some point). I re-ran the tests, but if that is the only failing, don't let it hold you up from merging once you're happy with the review changes.

[adamkewley]

Thanks @adamkewley I think I got most of your comments in a straightforward way. Perhaps check: #3481 (comment) #3481 (comment) if you agree with the solution

I agree with your point that the XY points are probably a Vec2 (or something like that), but indeed better to set up a new pr (I can look into this).

Thanks!

I checked your resolutions and they're fine

[pepbos]

Well, finally landed on using SimTK::Array_ with int for indexing, instead of std::vector and size_t/ptrdiff_t.

Care to give it another look @adamkewley ?

[adamkewley]

Seems fine 👍

[adamkewley]

🎈