Add AbstractGeometryPath interface for decorative points and resample Scholz2015GeometryPath curve points

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,64 @@ 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) {
+        ++index;
+        if (showPathPoints) {
+            geoms.push_back(SimTK::DecorativeSphere(0.005)
+                    .setTransform(dpp.getLocationInGround())
+                    .setColor(color)
+                    .setOpacity(0.8)
+                    .setBodyId(0));
+        }
+
+        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)
+                    .setColor(color)
+                    .setBodyId(0)
+                    .setIndexOnBody(index-1));
+        }
+
+        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);
@@ -220,82 +249,6 @@ void GeometryPath::extendConnectToModel(Model& aModel)
     markCacheVariableValid(s, _colorCV);  // it is OK at its default value
 }
 
-//------------------------------------------------------------------------------
-//                         GENERATE DECORATIONS
-//------------------------------------------------------------------------------
-// The GeometryPath takes care of drawing itself here, using information it
-// can extract from the supplied state, including position information and
-// color information that may have been calculated as late as Stage::Dynamics.
-// For example, muscles may want the color to reflect activation level and 
-// other path-using components might want to use forces (tension). We will
-// ensure that the state has been realized to Stage::Dynamics before looking
-// at it. (It is only guaranteed to be at Stage::Position here.)
-void GeometryPath::
-generateDecorations(bool fixed, const ModelDisplayHints& hints, 
-                    const SimTK::State& state, 
-                    SimTK::Array_<SimTK::DecorativeGeometry>& appendToThis) const
-{        
-    // There is no fixed geometry to generate here.
-    if (fixed) { return; }
-
-    const Array<AbstractPathPoint*>& pathPoints = getCurrentPath(state);
-
-    OPENSIM_ASSERT_FRMOBJ(pathPoints.size() > 1);
-
-    const AbstractPathPoint* lastPoint = pathPoints[0];
-    SimTK::MobilizedBodyIndex mbix(0);
-
-    Vec3 lastPos = lastPoint->getLocationInGround(state);
-    if (hints.get_show_path_points())
-        SimTK::DefaultGeometry::drawPathPoint(
-                mbix, lastPos, getColor(state), appendToThis);
-
-    Vec3 pos;
-
-    for (int i = 1; i < pathPoints.getSize(); ++i) {
-        AbstractPathPoint* point = pathPoints[i];
-        PathWrapPoint* pwp = dynamic_cast<PathWrapPoint*>(point);
-
-        if (pwp) {
-            // A PathWrapPoint provides points on the wrapping surface as Vec3s
-            const Array<Vec3>& surfacePoints = pwp->getWrapPath(state);
-            // The surface points are expressed w.r.t. the wrap surface's body frame.
-            // Transform the surface points into the ground reference frame to draw
-            // the surface point as the wrapping portion of the GeometryPath
-            const SimTK::Transform& X_BG =
-                    pwp->getParentFrame().getTransformInGround(state);
-            // Cycle through each surface point and draw it the Ground frame
-            for (int j = 0; j<surfacePoints.getSize(); ++j) {
-                // transform the surface point into the Ground reference frame
-                pos = X_BG*surfacePoints[j];
-                if (hints.get_show_path_points())
-                    SimTK::DefaultGeometry::drawPathPoint(
-                            mbix, pos, getColor(state), appendToThis);
-                // Line segments will be in ground frame
-                appendToThis.push_back(SimTK::DecorativeLine(lastPos, pos)
-                                .setLineThickness(4)
-                                .setColor(getColor(state))
-                                .setBodyId(0)
-                                .setIndexOnBody(j));
-                lastPos = pos;
-            }
-        } 
-        else { // otherwise a regular PathPoint so just draw its location
-            pos = point->getLocationInGround(state);
-            if (hints.get_show_path_points())
-                SimTK::DefaultGeometry::drawPathPoint(
-                        mbix, pos, getColor(state), appendToThis);
-            // Line segments will be in ground frame
-            appendToThis.push_back(SimTK::DecorativeLine(lastPos, pos)
-                            .setLineThickness(4)
-                            .setColor(getColor(state))
-                            .setBodyId(0)
-                            .setIndexOnBody(i));
-            lastPos = pos;
-        }
-    }
-}
-
 //_____________________________________________________________________________
 /*
  * Connect properties to local pointers.

OpenSim/Simulation/Model/GeometryPath.h

@@ -193,17 +193,11 @@ OpenSim_DECLARE_CONCRETE_OBJECT(GeometryPath, AbstractGeometryPath);
     void extendInitStateFromProperties(SimTK::State& s) const override;
     void extendAddToSystem(SimTK::MultibodySystem& system) const override;
 
-    // Visual support GeometryPath drawing in SimTK visualizer.
-    void generateDecorations(
-            bool                                        fixed,
-            const ModelDisplayHints&                    hints,
-            const SimTK::State&                         state,
-            SimTK::Array_<SimTK::DecorativeGeometry>&   appendToThis) const
-            override;
-
     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;