Add decorative path point interface to AbstractGeometryPath and downsample Scholz2015GeometryPath decorative curve points

Author: nickbianco

OpenSim/Simulation/Model/AbstractGeometryPath.cpp

@@ -26,6 +26,8 @@
 #include <OpenSim/Simulation/Model/ForceApplier.h>
 #include <OpenSim/Simulation/Model/Model.h>
 
+#include <optional>
+
 using namespace OpenSim;
 
 //=============================================================================
@@ -91,3 +93,71 @@ void AbstractGeometryPath::setPreScaleLength(const SimTK::State&,
 {
     _preScaleLength = preScaleLength;
 }
+
+void AbstractGeometryPath::forEachDecorativePathPoint(
+    const SimTK::State& state,
+    const std::function<void(const DecorativePathPoint&)>& callback) const
+{
+    implForEachDecorativePathPoint(state, callback);
+}
+
+std::vector<AbstractGeometryPath::DecorativePathPoint>
+AbstractGeometryPath::getDecorativePathPoints(const SimTK::State& state) const
+{
+    std::vector<AbstractGeometryPath::DecorativePathPoint> rv;
+    forEachDecorativePathPoint(state,
+            [&rv](const DecorativePathPoint& dp) { rv.push_back(dp); });
+    return rv;
+}
+
+void AbstractGeometryPath::generateDecorations(bool fixed,
+    const ModelDisplayHints& hints, const SimTK::State& s,
+    SimTK::Array_<SimTK::DecorativeGeometry>& geoms) const
+{
+    if (fixed) {
+        return;
+    }
+    if (not get_Appearance().get_visible()) {
+        // Don't render a path that's hidden
+        // (ComputationalBiomechanicsLab/opensim-creator#1166)
+        return;
+    }
+
+    const bool showPathPoints = hints.get_show_path_points();
+    const SimTK::Vec3 color = getColor(s);
+    const double opacity = get_Appearance().get_opacity();
+    const auto representation = get_Appearance().get_representation();
+
+    int index = 0;
+    std::optional<DecorativePathPoint> previous;
+    forEachDecorativePathPoint(s, [&](const DecorativePathPoint& dpp)
+    {
+        if (previous) {
+            // Emit line between points
+            const SimTK::Vec3& p1 = previous->getLocationInGround();
+            const SimTK::Vec3& p2 = dpp.getLocationInGround();
+            geoms.push_back(SimTK::DecorativeLine(p1, p2)
+                .setLineThickness(4)
+                .setScaleFactors(SimTK::Vec3{1.0})
+                .setColor(color)
+                .setOpacity(opacity)
+                .setRepresentation(representation)
+                .setBodyId(0)
+                .setIndexOnBody(index++)
+            );
+        }
+        if (showPathPoints) {
+            geoms.push_back(SimTK::DecorativeSphere(0.005)
+                .setTransform(dpp.getLocationInGround())
+                .setScaleFactors(SimTK::Vec3{1.0})
+                .setColor(color)
+                .setOpacity(opacity)
+                .setRepresentation(representation)
+                .setBodyId(0)
+                .setIndexOnBody(index++)
+            );
+        }
+
+        previous = dpp;
+    });
+}

OpenSim/Simulation/Model/AbstractGeometryPath.h

@@ -27,6 +27,9 @@
 #include <OpenSim/Simulation/Model/ModelComponent.h>
 #include <OpenSim/Simulation/Model/Appearance.h>
 
+#include <functional>
+#include <vector>
+
 #ifdef SWIG
     #ifdef OSIMSIMULATION_API
         #undef OSIMSIMULATION_API
@@ -229,8 +232,86 @@ OpenSim_DECLARE_ABSTRACT_OBJECT(AbstractGeometryPath, ModelComponent);
      */
     double getPreScaleLength(const SimTK::State& s) const;
     void setPreScaleLength(const SimTK::State& s, double preScaleLength);
-    
+
+    /**
+     * Represents a decorative (i.e. visualization-only) point of an
+     * `AbstractGeometryPath`. Can be used by UI implementations to
+     * display the path.
+     */
+    class DecorativePathPoint final {
+    public:
+        explicit DecorativePathPoint(
+            const SimTK::Vec3& locationInGround,
+            const Component* associatedComponent = nullptr) :
+            m_locationInGround{locationInGround},
+            m_maybeAssociatedComponent{associatedComponent}
+        {}
+
+        SimTK::Vec3 getLocationInGround() const { return m_locationInGround; }
+        void setLocationInGround(const SimTK::Vec3& p) {
+            m_locationInGround = p;
+        }
+        const Component* getAssociatedComponent() const {
+            return m_maybeAssociatedComponent;
+        }
+    private:
+        SimTK::Vec3 m_locationInGround;
+        const Component* m_maybeAssociatedComponent;
+    };
+
+    /**
+     * Calls `callback` with each point of a decorative representation of the
+     * path, if such a representation is available.
+     *
+     * @param s        Current simulation state, used to read the state of this
+     *                 path.
+     * @param callback A callback that should be called with each point of the
+     *                 decorative path.
+     */
+    void forEachDecorativePathPoint(const SimTK::State& state,
+        const std::function<void(const DecorativePathPoint&)>& callback) const;
+
+    /**
+     * Get a vector of decorative path points.
+     *
+     * @param s   Current simulation state, used to read the state of this path.
+     */
+    std::vector<DecorativePathPoint> getDecorativePathPoints(
+        const SimTK::State& s) const;
+
+protected:
+    /**
+     * Generates default decorations for this `AbstractGeometryPath`, based on
+     * the decorative points emitted by `implForEachDecorativePathPoint`.
+     *
+     * Derived classes may override this to provide different display behavior
+     * for the path.
+     */
+    void generateDecorations(bool fixed, const ModelDisplayHints&,
+        const SimTK::State&,
+        SimTK::Array_<SimTK::DecorativeGeometry>&) const override;
+
 private:
+    /**
+     * Implementors should call `callback` with each point of a decorative
+     * representation of the path.
+     *
+     * The decorative representation does not necessarily need to be the same
+     * as the computational representation: it only needs to be a "suitable"
+     * visual representation of the path (if any).
+     *
+     * @param s        Current simulation state, used to read the state of this
+     *                 path.
+     * @param callback A callback that should be called with each point of the
+     *                 decorative path.
+     */
+    virtual void implForEachDecorativePathPoint(const SimTK::State& s,
+        const std::function<void(const DecorativePathPoint&)>& callback) const
+    {
+        // No-op: implementations are also permitted to provide no decorative
+        // point representation at all.
+    }
+
     // Used by `(get|set)PreLengthScale`. Used during `extend(Pre|Post)Scale` by
     // downstream users of AbstractGeometryPath to cache the length of the path
     // before scaling.

OpenSim/Simulation/Model/GeometryPath.cpp

@@ -177,6 +177,35 @@ void GeometryPath::extendFinalizeFromProperties()
     }
 }
 
+void GeometryPath::implForEachDecorativePathPoint(
+    const SimTK::State& state,
+    const std::function<void(const DecorativePathPoint&)>& callback) const
+{
+    const Array<AbstractPathPoint*>& pathPoints = getCurrentPath(state);
+
+    for (int i = 0; i < pathPoints.size(); ++i) {
+        const AbstractPathPoint& p = *pathPoints[i];
+
+        if (auto* pwp = dynamic_cast<const PathWrapPoint*>(&p)) {
+            // A `PathWrapPoint`'s surface points are expressed w.r.t. the wrap
+            // surface's body frame. Ensure they're transformed to ground.
+            const SimTK::Transform& X_BG =
+                    pwp->getParentFrame().getTransformInGround(state);
+
+            // A `PathWrapPoint`'s surface points should be emitted, but not
+            // associated to a component in the model (they are synthetic).
+            const Array<Vec3>& surfacePoints = pwp->getWrapPath(state);
+
+            for (int j = 0; j < surfacePoints.getSize(); ++j) {
+                callback(DecorativePathPoint{X_BG * surfacePoints[j]});
+            }
+        }
+        else {  // Otherwise, it's a regular `PathPoint`, so just emit it.
+            callback(DecorativePathPoint{p.getLocationInGround(state), &p});
+        }
+    }
+}
+
 void GeometryPath::extendConnectToModel(Model& aModel)
 {
     Super::extendConnectToModel(aModel);

OpenSim/Simulation/Model/GeometryPath.h

@@ -204,6 +204,8 @@ OpenSim_DECLARE_CONCRETE_OBJECT(GeometryPath, AbstractGeometryPath);
     void extendFinalizeFromProperties() override;
 
 private:
+    void implForEachDecorativePathPoint(const SimTK::State&,
+        const std::function<void(const DecorativePathPoint&)>&) const override;
 
     void computePath(const SimTK::State& s ) const;
     void computeLengtheningSpeed(const SimTK::State& s) const;

OpenSim/Simulation/Model/Scholz2015GeometryPath.cpp

@@ -28,8 +28,6 @@
 #include <OpenSim/Simulation/SimbodyEngine/Coordinate.h>
 #include <OpenSim/Simulation/Model/Model.h>
 
-#include <optional>
-
 using namespace OpenSim;
 
 //=============================================================================
@@ -189,6 +187,15 @@ int Scholz2015GeometryPath::getNumPathElements() const {
     return getProperty_path_elements().size();
 }
 
+void Scholz2015GeometryPath::setNumDecorativeCurvePoints(
+        int numDecorativeCurvePoints) {
+    set_num_decorative_curve_points(numDecorativeCurvePoints);
+}
+
+int Scholz2015GeometryPath::getNumDecorativeCurvePoints() const {
+    return get_num_decorative_curve_points();
+}
+
 //=============================================================================
 // ABSTRACT PATH INTERFACE
 //=============================================================================
@@ -212,6 +219,16 @@ double Scholz2015GeometryPath::computeMomentArm(const SimTK::State& s,
     return _maSolver->solve(s, coord,  *this);
 }
 
+void Scholz2015GeometryPath::implForEachDecorativePathPoint(
+    const SimTK::State& state,
+    const std::function<void(const DecorativePathPoint&)>& callback) const
+{
+    getCableSpan().calcResampledDecorativePathPoints(
+        state, get_num_decorative_curve_points(),
+        [&callback](SimTK::Vec3 p) { callback(DecorativePathPoint{p});
+    });
+}
+
 void Scholz2015GeometryPath::produceForces(const SimTK::State& state,
         double tension, ForceConsumer& forceConsumer) const {
 
@@ -329,47 +346,12 @@ void Scholz2015GeometryPath::extendAddToSystem(
     _index = cable.getIndex();
 }
 
-void Scholz2015GeometryPath::generateDecorations(
-        bool fixed,
-        const ModelDisplayHints& hints,
-        const SimTK::State& s,
-        SimTK::Array_<SimTK::DecorativeGeometry>& geoms) const {
-
-    if (fixed) { return; }
-    const bool showPathPoints = hints.get_show_path_points();
-    const SimTK::Vec3 color = getColor(s);
-    int index = 0;
-    std::optional<SimTK::Vec3> previous;
-    getCableSpan().calcDecorativePathPoints(s, [&](SimTK::Vec3 x_G) {
-        if (previous) {
-            // Emit line between points
-            geoms.push_back(SimTK::DecorativeLine(*previous, x_G)
-                .setLineThickness(4)
-                .setScaleFactors(SimTK::Vec3{1.0})
-                .setColor(color)
-                .setBodyId(0)
-                .setIndexOnBody(index++)
-            );
-        }
-        if (showPathPoints) {
-            geoms.push_back(SimTK::DecorativeSphere(0.005)
-                .setTransform(x_G)
-                .setScaleFactors(SimTK::Vec3{1.0})
-                .setColor(color)
-                .setBodyId(0)
-                .setIndexOnBody(index++)
-            );
-        }
-
-        previous = x_G;
-    });
-}
-
 //=============================================================================
 // CONVENIENCE METHODS
 //=============================================================================
 void Scholz2015GeometryPath::constructProperties() {
     constructProperty_path_elements();
+    constructProperty_num_decorative_curve_points(5);
 }
 
 const Scholz2015GeometryPathObstacle* Scholz2015GeometryPath::getObstacle(

OpenSim/Simulation/Model/Scholz2015GeometryPath.h

@@ -421,6 +421,18 @@ OpenSim_DECLARE_CONCRETE_OBJECT(Scholz2015GeometryPath, AbstractGeometryPath);
      */
     int getNumPathElements() const;
 
+    /**
+     * Set the number of decorative path points per curve segment used when
+     * generating path decorations.
+     */
+    void setNumDecorativeCurvePoints(int numDecorativeCurvePoints);
+
+    /**
+     * Get the number of decorative path points per curve segment used when
+     * generating path decorations (default: 5).
+     */
+    int getNumDecorativeCurvePoints() const;
+
     // @}
 
     //** @name `AbstractGeometryPath` interface */
@@ -430,6 +442,8 @@ OpenSim_DECLARE_CONCRETE_OBJECT(Scholz2015GeometryPath, AbstractGeometryPath);
     double computeMomentArm(const SimTK::State& s,
             const Coordinate& coord) const override;
     bool isVisualPath() const override { return true; }
+    void implForEachDecorativePathPoint(const SimTK::State&,
+        const std::function<void(const DecorativePathPoint&)>&) const override;
     // @}
 
     //** @name `ForceProducer` interface */
@@ -442,13 +456,13 @@ OpenSim_DECLARE_CONCRETE_OBJECT(Scholz2015GeometryPath, AbstractGeometryPath);
     // PROPERTIES
     OpenSim_DECLARE_LIST_PROPERTY(path_elements, Scholz2015GeometryPathElement,
         "The list of elements (path points or obstacles) defining the path.");
+    OpenSim_DECLARE_PROPERTY(num_decorative_curve_points, int,
+        "The number of decorative path points per curve segment used when "
+        "generating path decorations (default: 5).");
 
     // MODEL COMPONENT INTERFACE
     void extendConnectToModel(Model& model) override;
     void extendAddToSystem(SimTK::MultibodySystem& system) const override;
-    void generateDecorations(bool fixed, const ModelDisplayHints& hints,
-            const SimTK::State& s,
-            SimTK::Array_<SimTK::DecorativeGeometry>& geoms) const override;
 
     // CONVENIENCE METHODS
     void constructProperties();

dependencies/CMakeLists.txt

@@ -185,7 +185,7 @@ AddDependency(NAME       ezc3d
 AddDependency(NAME       simbody
               DEFAULT    ON
               GIT_URL    https://github.com/simbody/simbody.git
-              GIT_TAG    7f35622b3c5daac919fc39a2865498c03c553e53
+              GIT_TAG    8a62e27882838708ae98e72c6902704f836aacb9
               CMAKE_ARGS -DBUILD_EXAMPLES:BOOL=OFF
                          -DBUILD_TESTING:BOOL=OFF
                          ${SIMBODY_EXTRA_CMAKE_ARGS})