Use ForceConsumer point force interface for applying Scholz2015GeometryPath forces

OpenSim/Simulation/Model/Scholz2015GeometryPath.cpp

@@ -222,44 +222,81 @@ void Scholz2015GeometryPath::produceForces(const SimTK::State& state,
     const SimTK::CableSpan& cable = getCableSpan();
     const PathPoint& origin = getOrigin();
     const PathPoint& insertion = getInsertion();
-    SimTK::SpatialVec unitBodyForce;
 
     // Force applied at path origin point.
-    {
-        cable.calcOriginUnitForce(state, unitBodyForce);
-        forceConsumer.consumeBodySpatialVec(state, origin.getParentFrame(),
-                tension * unitBodyForce);
-    }
+    // CableSpan::calcOriginTangentDirection() returns the tangent direction of
+    // the cable at origin point, pointing away from the origin in the direction
+    // of the tension force.
+    forceConsumer.consumePointForce(state,
+            origin.getParentFrame(),
+            origin.getLocation(state),
+            tension * cable.calcOriginTangentDirection(state));
+
 
     // Forces applied to each obstacle body.
     for (const auto& [ix, eltIx] : _obstacleIndexes) {
         if (!cable.isInContactWithObstacle(state, ix)) {
             continue;
         }
 
-        cable.calcCurveSegmentUnitForce(state, ix, unitBodyForce);
-        const auto& obstacle =
-                getElement<Scholz2015GeometryPathObstacle>(eltIx);
+        // Compute the initial and final Frenet frame for the curve segment.
+        const SimTK::Transform X_GP =
+                cable.calcCurveSegmentInitialFrenetFrame(state, ix);
+        const SimTK::Transform X_GQ =
+                cable.calcCurveSegmentFinalFrenetFrame(state, ix);
+
+        // The tangent direction of the curve segment at the obstacle contact
+        // point is the x-axis of the Frenet frame. The force applied to the
+        // obstacle at the initial contact point is in the negative direction
+        // of the tangent.
+        const SimTK::UnitVec3& t_P = -X_GP.R().getAxisUnitVec(SimTK::XAxis);
+        const SimTK::UnitVec3& t_Q = X_GQ.R().getAxisUnitVec(SimTK::XAxis);
+
+        // Transform the Frenet frame positions from the ground frame G to the
+        // obstacle frame O, to conform with the ForceConsumer interface.
+        const Ground& ground = getModel().getGround();
+        const auto& obstacle = getElement<Scholz2015GeometryPathObstacle>(eltIx);
         const auto& frame = obstacle.getContactGeometry().getFrame();
-        forceConsumer.consumeBodySpatialVec(state, frame,
-                tension * unitBodyForce);
+        const SimTK::Vec3& p_GP = X_GP.p();
+        const SimTK::Vec3& p_GQ = X_GQ.p();
+        const SimTK::Vec3 p_OP = ground.findStationLocationInAnotherFrame(
+                state, p_GP, frame);
+        const SimTK::Vec3 p_OQ = ground.findStationLocationInAnotherFrame(
+                state, p_GQ, frame);
+
+        // Consume the forces applied at the initial and final Frenet frames
+        // associated with the obstacle.
+        forceConsumer.consumePointForce(state, frame, p_OP, tension * t_P);
+        forceConsumer.consumePointForce(state, frame, p_OQ, tension * t_Q);
     }
 
     // Forces applied to each via point.
     for (const auto& [ix, eltIx] : _viaPointIndexes) {
-        cable.calcViaPointUnitForce(state, ix, unitBodyForce);
-        const auto& point = getElement<Scholz2015GeometryPathPoint>(eltIx);
-        const auto& frame = point.getPathPoint().getParentFrame();
-        forceConsumer.consumeBodySpatialVec(state, frame,
-                tension * unitBodyForce);
+        // Calculate the via point's initial tangent direction (point from the
+        // via point torwards the origin) and the final tangent direction
+        // (point from the via point towards the insertion).
+        const SimTK::UnitVec3 t_P =
+                -cable.calcViaPointIncomingTangentDirection(state, ix);
+        const SimTK::UnitVec3 t_Q =
+                cable.calcViaPointOutgoingTangentDirection(state, ix);
+
+        // Consume the forces applied at the via point.
+        const PathPoint& point =
+            getElement<Scholz2015GeometryPathPoint>(eltIx).getPathPoint();
+        const auto& frame = point.getParentFrame();
+        const auto& location = point.getLocation(state);
+        forceConsumer.consumePointForce(state, frame, location, tension * t_P);
+        forceConsumer.consumePointForce(state, frame, location, tension * t_Q);
     }
 
     // Force applied at path insertion point.
-    {
-        cable.calcTerminationUnitForce(state, unitBodyForce);
-        forceConsumer.consumeBodySpatialVec(
-            state, insertion.getParentFrame(), unitBodyForce * tension);
-    }
+    // CableSpan::calcTerminationTangentDirection() returns the tangent
+    // direction of the cable at insertion point, pointing towards the insertion
+    // in the opposite direction of the tension force.
+    forceConsumer.consumePointForce(state,
+            insertion.getParentFrame(),
+            insertion.getLocation(state),
+            -tension * cable.calcTerminationTangentDirection(state));
 }
 
 //=============================================================================

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    8a62e27882838708ae98e72c6902704f836aacb9
+              GIT_TAG    80a3f10a6daa26c8aaf18f2c672cf385b227e62c
               CMAKE_ARGS -DBUILD_EXAMPLES:BOOL=OFF
                          -DBUILD_TESTING:BOOL=OFF
                          ${SIMBODY_EXTRA_CMAKE_ARGS})