diff --git a/OpenSim/Simulation/Model/Scholz2015GeometryPath.cpp b/OpenSim/Simulation/Model/Scholz2015GeometryPath.cpp index 42e377681f..8c292b7d9a 100644 --- a/OpenSim/Simulation/Model/Scholz2015GeometryPath.cpp +++ b/OpenSim/Simulation/Model/Scholz2015GeometryPath.cpp @@ -222,14 +222,16 @@ 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) { @@ -237,29 +239,64 @@ void Scholz2015GeometryPath::produceForces(const SimTK::State& state, continue; } - cable.calcCurveSegmentUnitForce(state, ix, unitBodyForce); - const auto& obstacle = - getElement(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(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(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(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)); } //============================================================================= diff --git a/dependencies/CMakeLists.txt b/dependencies/CMakeLists.txt index 9656c03644..5cc756f98b 100644 --- a/dependencies/CMakeLists.txt +++ b/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})