diff --git a/OpenSim/Simulation/Model/GeometryPath.cpp b/OpenSim/Simulation/Model/GeometryPath.cpp index 8c679d720f..b5c7e3b891 100644 --- a/OpenSim/Simulation/Model/GeometryPath.cpp +++ b/OpenSim/Simulation/Model/GeometryPath.cpp @@ -129,6 +129,24 @@ static void PopulatePathElementLookup( } } +// Returns a normalized direction vector from `a` to `b` in ground, or a vector containing `NaN`s +// if `a` and `b` are at the same location. +// +// Two points can be conincient due to infeasible wrapping of the path. E.g. when the origin or +// insertion enters the wrapping surface. This is a temporary fix, since the wrap algorithm +// should return NaN for the points and/or throw an Exception- aseth +static SimTK::Vec3 directionBetweenPointsInGroundOrNaNIfCoincident(const SimTK::State& s, const OpenSim::AbstractPathPoint& a, const OpenSim::AbstractPathPoint& b) +{ + Vec3 dir = b.getLocationInGround(s) - a.getLocationInGround(s); + if (dir.norm() < SimTK::SignificantReal) { + dir = dir*SimTK::NaN; + } + else { + dir = dir.normalize(); + } + return dir; +} + //============================================================================= // CONSTRUCTOR(S) AND DESTRUCTOR //============================================================================= @@ -386,103 +404,55 @@ void GeometryPath::produceForces(const SimTK::State& s, double tension, ForceConsumer& forceConsumer) const { - AbstractPathPoint* start = NULL; - AbstractPathPoint* end = NULL; - const SimTK::MobilizedBody* bo = NULL; - const SimTK::MobilizedBody* bf = NULL; - const Array& currentPath = getCurrentPath(s); - int np = currentPath.getSize(); - - const SimTK::SimbodyMatterSubsystem& matter = - getModel().getMatterSubsystem(); - - // start point, end point, direction, and force vectors in ground - Vec3 po(0), pf(0), dir(0), force(0); - // partial velocity of point in body expressed in ground - Vec3 dPodq_G(0), dPfdq_G(0); - - // gen force (torque) due to moving point under tension - double fo, ff; - - for (int i = 0; i < np-1; ++i) { - start = currentPath[i]; - end = currentPath[i+1]; - - bo = &start->getParentFrame().getMobilizedBody(); - bf = &end->getParentFrame().getMobilizedBody(); - - if (bo != bf) { - // Find the positions of start and end in the inertial frame. - po = start->getLocationInGround(s); - pf = end->getLocationInGround(s); - - // Form a vector from start to end, in the inertial frame. - dir = (pf - po); - - // Check that the two points are not coincident. - // This can happen due to infeasible wrapping of the path, - // when the origin or insertion enters the wrapping surface. - // This is a temporary fix, since the wrap algorithm should - // return NaN for the points and/or throw an Exception- aseth - if (dir.norm() < SimTK::SignificantReal){ - dir = dir*SimTK::NaN; - } - else{ - dir = dir.normalize(); - } + // Retains the body index from the previous iteration of the main loop + // and the previous direction between the previous point and the current + // one (if applicable). This is used to ensure that only one point force + // is produced per path point (#3903, #3891). + MobilizedBodyIndex previousBodyIndex = MobilizedBodyIndex::Invalid(); + SimTK::Vec3 previousDirection(0.0); - force = tension*dir; - - const MovingPathPoint* mppo = - dynamic_cast(start); + const Array& currentPath = getCurrentPath(s); + for (int i = 0; i < currentPath.getSize(); ++i) { - // do the same for the end point of this segment of the path - const MovingPathPoint* mppf = - dynamic_cast(end); + const AbstractPathPoint& currentPoint = *currentPath[i]; + const MobilizedBodyIndex currentBodyIndex = currentPoint.getParentFrame().getMobilizedBodyIndex(); - // add in the tension point forces to body forces - if (mppo) {// moving path point location is a function of the state - forceConsumer.consumePointForce(s, start->getParentFrame(), mppo->getLocation(s), force); - } - else { - forceConsumer.consumePointForce(s, start->getParentFrame(), start->getLocation(s), force); - } + SimTK::Vec3 force(0.0); + if (previousBodyIndex.isValid() && currentBodyIndex != previousBodyIndex) { + force -= tension * previousDirection; + } - if (mppf) {// moving path point location is a function of the state - forceConsumer.consumePointForce(s, end->getParentFrame(), mppf->getLocation(s), -force); + const AbstractPathPoint* nextPoint = i < currentPath.getSize()-1 ? currentPath[i+1] : nullptr; + if (nextPoint && nextPoint->getParentFrame().getMobilizedBodyIndex() != currentBodyIndex) { + const SimTK::Vec3 currentDirection = directionBetweenPointsInGroundOrNaNIfCoincident(s, currentPoint, *nextPoint); + const SimTK::Vec3 currentToNextForce = tension * currentDirection; + force += currentToNextForce; + + // Additionally, account for the work done due to the movement of a `MovingPathPoint` + // relative to the body it is on. + if (const auto* movingCurrentPoint = dynamic_cast(¤tPoint)) { + const Vec3 dPodq_G = currentPoint.getParentFrame().expressVectorInGround(s, currentPoint.getdPointdQ(s)); + const double fo = ~dPodq_G*currentToNextForce; + forceConsumer.consumeGeneralizedForce(s, movingCurrentPoint->getXCoordinate(), fo); } - else { - // transform of the frame of the point to the base mobilized body - forceConsumer.consumePointForce(s, end->getParentFrame(), end->getLocation(s), -force); + if (const auto* nextMovingPoint = dynamic_cast(nextPoint)) { + const Vec3 dPfdq_G = nextPoint->getParentFrame().expressVectorInGround(s, nextPoint->getdPointdQ(s)); + const double ff = ~dPfdq_G*(-currentToNextForce); + forceConsumer.consumeGeneralizedForce(s, nextMovingPoint->getXCoordinate(), ff); } - // Now account for the work being done by virtue of the moving - // path point motion relative to the body it is on - if(mppo){ - // torque (genforce) contribution due to relative movement - // of a via point w.r.t. the body it is connected to. - dPodq_G = bo->expressVectorInGroundFrame(s, start->getdPointdQ(s)); - fo = ~dPodq_G*force; - - // get the mobilized body the coordinate is couple to. - const SimTK::MobilizedBody& mpbod = - matter.getMobilizedBody(mppo->getXCoordinate().getBodyIndex()); - - // apply the generalized (mobility) force to the coordinate's body - forceConsumer.consumeGeneralizedForce(s, mppo->getXCoordinate(), fo); - } - - if(mppf){ - dPfdq_G = bf->expressVectorInGroundFrame(s, end->getdPointdQ(s)); - ff = ~dPfdq_G*(-force); - - // get the mobilized body the coordinate is couple to. - const SimTK::MobilizedBody& mpbod = - matter.getMobilizedBody(mppf->getXCoordinate().getBodyIndex()); + previousBodyIndex = currentBodyIndex; + previousDirection = currentDirection; + } + else { + previousBodyIndex = MobilizedBodyIndex::Invalid(); + previousDirection = SimTK::Vec3(0.0f); + } - forceConsumer.consumeGeneralizedForce(s, mppf->getXCoordinate(), ff); - } - } + // If applicable, produce the force + if (force != SimTK::Vec3(0.0)) { + forceConsumer.consumePointForce(s, currentPoint.getParentFrame(), currentPoint.getLocation(s), force); + } } } diff --git a/dependencies/CMakeLists.txt b/dependencies/CMakeLists.txt index cac81ba0ec..763312b7b1 100644 --- a/dependencies/CMakeLists.txt +++ b/dependencies/CMakeLists.txt @@ -105,7 +105,6 @@ function(AddDependency) list(APPEND CMAKE_ARGS -DCMAKE_BUILD_TYPE:STRING=${CMAKE_BUILD_TYPE}) endif() - list(APPEND CMAKE_ARGS ${DEP_CMAKE_ARGS}) # Forward cmake arguments to dependencies. list(APPEND CMAKE_ARGS @@ -140,6 +139,9 @@ function(AddDependency) list(APPEND CMAKE_ARGS -DSWIG_EXECUTABLE:FILEPATH=${SWIG_EXECUTABLE}) endif() + # Append the dependency-specific CMake arguments. + list(APPEND CMAKE_ARGS ${DEP_CMAKE_ARGS}) + if(DEP_GIT_URL) ExternalProject_Add(${DEP_NAME} DEPENDS ${DEP_DEPENDS} @@ -193,6 +195,11 @@ AddDependency(NAME docopt GIT_URL https://github.com/docopt/docopt.cpp.git GIT_TAG 3dd23e3280f213bacefdf5fcb04857bf52e90917) +set(SPDLOG_CXX_FLAGS "${CMAKE_CXX_FLAGS}") +if(MSVC) + # `spdlog` transitively uses a deprecated `stdext::checked_array_iterator` + set(SPDLOG_CXX_FLAGS "${SPDLOG_CXX_FLAGS} /D_SILENCE_STDEXT_ARR_ITERS_DEPRECATION_WARNING") +endif() AddDependency(NAME spdlog DEFAULT ON @@ -201,7 +208,8 @@ AddDependency(NAME spdlog CMAKE_ARGS -DSPDLOG_BUILD_BENCH:BOOL=OFF -DSPDLOG_BUILD_TESTS:BOOL=OFF -DSPDLOG_BUILD_EXAMPLE:BOOL=OFF - -DCMAKE_POSITION_INDEPENDENT_CODE:BOOL=ON) + -DCMAKE_POSITION_INDEPENDENT_CODE:BOOL=ON + -DCMAKE_CXX_FLAGS:STRING=${SPDLOG_CXX_FLAGS}) AddDependency(NAME catch2 DEFAULT ON