diff --git a/OpenSim/Simulation/Wrap/WrapCylinder.cpp b/OpenSim/Simulation/Wrap/WrapCylinder.cpp index 2f0ac179ae..8543a03ac2 100644 --- a/OpenSim/Simulation/Wrap/WrapCylinder.cpp +++ b/OpenSim/Simulation/Wrap/WrapCylinder.cpp @@ -33,6 +33,7 @@ #include #include #include +#include //============================================================================= @@ -49,6 +50,713 @@ static Vec3 dn(0.0, 0.0, 1.0); #define MAX_ITERATIONS 100 #define TANGENCY_THRESHOLD (0.1 * SimTK_DEGREE_TO_RADIAN) // find tangency to within 1 degree +// Geometry related helper structs and functions. +namespace { + constexpr double c_TAU = 2. * SimTK_PI; + + template + struct PathSegment final { + PathSegment(T startPoint, T endPoint) : + start(std::move(startPoint)), + end(std::move(endPoint)) + {} + + template + PathSegment( + const PathSegment& other, + Args&& ...args) : + PathSegment{ + T(other.start, std::forward(args)...), + T(other.end, std::forward(args)...) + } + {} + + T start{}; + T end{}; + }; + + template + T Difference(const PathSegment& path) + { + return path.end - path.start; + } + + using PathSegmentVec2 = PathSegment; + using PathSegmentVec3 = PathSegment; + + // Applies atan2 but maps the resulting range to [0...2pi]. + double ATan2OnPositiveRange(const SimTK::Vec2 point) + { + const double angle = std::atan2(point[1], point[0]); + return angle < 0.? angle + c_TAU : angle; + } + + class Angle final { + // Construct from angle already on range [0...2pi]. + Angle(double angleChecked): _value(angleChecked) + {} + + public: + Angle() = default; + + Angle(const SimTK::Vec2& point) : + _value(ATan2OnPositiveRange(point)) + {} + + // Returns a value on range [0..2pi], or NaN if uninitialized. + double getValue() const { + return _value; + } + + friend Angle operator+(Angle lhs, Angle rhs); + friend Angle operator-(Angle lhs, Angle rhs); + + private: + double _value = SimTK::NaN; + }; + + Angle operator+(Angle lhs, Angle rhs) { + double angle = lhs.getValue() + rhs.getValue(); + // Map angle to [0...2pi] range. Using fmod would be more robust against + // repeated calls, but significantly slower. For the current code this + // robustness would be overkill: + angle = angle > c_TAU? angle - c_TAU : angle; // poor man's fmod() + return Angle{angle}; + } + + Angle operator-(Angle lhs, Angle rhs) { + double angle = lhs.getValue() - rhs.getValue(); + // Map angle to [0...2pi] range. + angle = angle < 0.? angle + c_TAU : angle; // poor man's fmod() + return Angle{angle}; + } + + struct PolarCoordinates final { + PolarCoordinates(const SimTK::Vec2& point) : + angle(point), + magnitude(point.norm()) + {} + + Angle angle{}; + double magnitude = SimTK::NaN; + }; + + enum class RotationDirection { + Positive, + Negative, + }; + + RotationDirection operator!(RotationDirection direction) + { + return direction == RotationDirection::Positive? + RotationDirection::Negative : RotationDirection::Positive; + } + + // Angular distance from start- to end angle, given a direction of rotation. + double AngularDistance( + Angle startAngle, + Angle endAngle, + RotationDirection direction) + { + const double diff = endAngle.getValue() - startAngle.getValue(); + if (diff < 0 && direction == RotationDirection::Positive) { + return diff + c_TAU; + } + if (diff > 0 && direction == RotationDirection::Negative) { + return diff - c_TAU; + } + return diff; + } + + double AngularDistance( + PathSegment path, + RotationDirection direction) + { + return AngularDistance(path.start, path.end, direction); + } + + // Returns the point on the line A-B that is closest to the origin. + SimTK::Vec2 ClosestPointOnLineAB( + const SimTK::Vec2& pointA, + const SimTK::Vec2& pointB) + { + // Given the infinite line through A anb B: If points A and B are on the + // same side of the shortest distance vector to the infinite line, we + // know that the shortest distance to line A-B is simply the distance to + // either point A or point B. + const SimTK::Vec2 vecAB = pointB - pointA; + const double vTa = SimTK::dot(vecAB, pointA); + const double vTb = SimTK::dot(vecAB, pointB); + const bool closestPointIsEitherAOrB = vTb * vTa > 0.; + + // Return closest of A and B. + if (closestPointIsEitherAOrB) { + return std::abs(vTa) < std::abs(vTb)? pointA : pointB; + } + + // Compute closest point on infinite line through A and B. + const double vTv = SimTK::dot(vecAB, vecAB); + return pointA - vecAB * vTa / vTv; + } + + bool IsPointInsideCircle( + const SimTK::Vec2& point, + double radiusSquared) + { + return SimTK::dot(point, point) < radiusSquared; + } +} + +// Construction of 2D Tangent lines to a circle. +namespace { + + // Representation of a line obtained by connecting a point + // to a circle, such that the line is tangent to the circle. + struct TangentLine final { + TangentLine() = default; + + TangentLine( + Angle angleOfTangentPointOnCircle, + double tangentLineLength) : + angleOfTangentPoint(angleOfTangentPointOnCircle), + length(tangentLineLength) + {} + + operator Angle() const + { + return angleOfTangentPoint; + } + + // Angle at which the line touches the circle. + Angle angleOfTangentPoint{}; + // Length of the line segment. + double length = SimTK::NaN; + }; + + // Given a long and short side of a right-angled triangle, returns the + // remaining short side length. + double PythagorasLengthFromLongAndShort( + double longTriangleSide, + double shortTriangleSide) + { + return std::sqrt( + longTriangleSide * longTriangleSide - + shortTriangleSide * shortTriangleSide); + } + + // Representation of the two possible TangentLines conntecting a point and + // circle. + struct BothTangentLines final { + BothTangentLines(const PolarCoordinates& point, double circleRadius) : + // By definition, the tangent point on the circle, the origin and + // the external point form a right angled triangle: + _length(PythagorasLengthFromLongAndShort( + point.magnitude, + circleRadius)), + // That same triangle gives the angle between the external point, + // and the tangent point on the circle: + _anglePointToTangentPoint(SimTK::Vec2(circleRadius, _length)), + _anglePoint(point.angle) + {} + + BothTangentLines(const SimTK::Vec2& point, double circleRadius) : + BothTangentLines(PolarCoordinates(point), circleRadius) + {} + + // Returns the TangentLine obtained when starting from the external + // point and traversing in the given direction to the tangent point. + TangentLine + getTangentLineStartingAtPoint(RotationDirection direction) const + { + Angle angleOfTangentPoint = + direction == RotationDirection::Positive? + _anglePoint + _anglePointToTangentPoint : + _anglePoint - _anglePointToTangentPoint; + return TangentLine{angleOfTangentPoint, _length}; + } + + // Reversed of getTangentLineStartingAtPoint: Returns the TangentLine + // obtained when starting from the circle and traversing in the given + // direction to the external point. + TangentLine + getTangentLineEndingAtPoint(RotationDirection direction) const + { + return getTangentLineStartingAtPoint(!direction); + } + + private: + + // Line segment length, given either direction. + double _length = SimTK::NaN; + // Helpers for constructing the angle of the tangent point. + Angle _anglePointToTangentPoint; + Angle _anglePoint; + }; +} + +// Section defining the active quadrant, and some helpers to check if a point +// lies in it. +namespace { + + enum class WrapSign { + Unconstrained, + Positive, + Negative, + }; + + enum class WrapAxis { + XAxis, + YAxis, + }; + + struct Quadrant { + Quadrant(int wrapSign, int wrapAxis) + { + switch (wrapAxis) { + case 0: + axis = WrapAxis::XAxis; + break; + case 1: + axis = WrapAxis::YAxis; + break; + default: // todo properly throw... + throw std::string("Invalid wrapAxis for wrapCylinder"); + } + switch (wrapSign) { + case 0: + sign = WrapSign::Unconstrained; + break; + case 1: + sign = WrapSign::Positive; + break; + case -1: + sign = WrapSign::Negative; + break; + default: // TODO properly throw.. + throw std::string("Invalid wrapSign for wrapCylinder"); + } + } + + WrapAxis axis; + WrapSign sign; + }; + + bool IsAngleInPositiveYQuadrant(Angle angle) { + return angle.getValue() <= SimTK_PI; + } + + bool IsAngleInPositiveXQuadrant(Angle angle) { + return angle.getValue() <= SimTK_PI * 0.5 + || angle.getValue() >= SimTK_PI * 1.5; + } + + bool IsAngleInQuadrant( + Angle angle, + const Quadrant& quadrant) + { + bool inPositiveQuadrant = quadrant.axis == WrapAxis::XAxis? + IsAngleInPositiveXQuadrant(angle): + IsAngleInPositiveYQuadrant(angle); + return (quadrant.sign == WrapSign::Positive)? + inPositiveQuadrant: + !inPositiveQuadrant; + } + + bool IsPointInActiveQuadrant( + const SimTK::Vec2& point, + Quadrant quadrant) + { + const double coord = quadrant.axis == WrapAxis::XAxis? point[0] : point[1]; + return coord > 0. && quadrant.sign == WrapSign::Positive; + } +} + +// Section on wrapping logic of a 2D circle. +namespace { + + // Returns if wrapping should not even be attempted. + // + // Checks 4 conditions: + // - 1. If either point lies inside the circle. + // - 2. If straight line from start to end does not intersect circle AND either: + // - a. Quadrant is unconstrained, + // - b. Both points lie in the active quadrant. + // - c. One point lies in the non-active quadrant, and closest point in + // lies in the active quadrant. + bool DoNotAttemptWrapping( + const PathSegmentVec2& path, + double radius, + Quadrant quadrant) + { + // Condition 1: Points must not lie inside the circle. + const double radiusSquared = radius * radius; + bool doNotAttemptWrapping = + IsPointInsideCircle(path.start, radiusSquared) || + IsPointInsideCircle(path.end, radiusSquared); + + // Condition 2: Do not wrap if straight line connecting start-end does + // not intersect the circle. + const SimTK::Vec2 closestPointOnLine = ClosestPointOnLineAB( + path.start, + path.end); + const double normClosestPointOnLineSquared = SimTK::dot( + closestPointOnLine, + closestPointOnLine); + if (normClosestPointOnLineSquared <= radiusSquared) { + // Straight line does intersect the circle. + return false; + } + + // Straight line did not intersect the circle, check conditions 2-a,b,c: + + // Condition 2.a: Unconstrained quadrant? + doNotAttemptWrapping |= quadrant.sign == WrapSign::Unconstrained; + + // Condition 2.b: Do not wrap if both points lie in the active quadrant. + const bool startInActiveQuadrant = + IsPointInActiveQuadrant(path.start, quadrant); + const bool endInActiveQuadrant = + IsPointInActiveQuadrant(path.end, quadrant); + doNotAttemptWrapping |= startInActiveQuadrant && endInActiveQuadrant; + + // Condition 2.c: Do not wrap if only one point in non-active, and + // nearest in active quadrant. + const bool closestPointInActiveQuadrant = IsPointInActiveQuadrant( + closestPointOnLine, + quadrant); + doNotAttemptWrapping |= (startInActiveQuadrant != endInActiveQuadrant) + && closestPointInActiveQuadrant; + + return doNotAttemptWrapping; + } + + // Given the wrapping path on a circle, determine if it is valid based on + // the quadrant. + // + // Wrapped path is not valid if: + // 1. Both angles in non-active quadrant. + // 2. Both angles in active-quadrant, while wrapping the entire non-active + // quadrant. + bool IsValidQuadrantWrapped( + const PathSegment& path, + double angularDistance, + const Quadrant& quadrant) + { + if (quadrant.sign == WrapSign::Unconstrained) { + return true; + } + + const bool startOk = IsAngleInQuadrant(path.start, quadrant); + const bool endOk = IsAngleInQuadrant(path.end, quadrant); + + // Condition 1: Invalid if both angles in non-active. + bool notValidWrap = !startOk && !endOk; + + // Condition 2: Invalid if both in active, and wrapping entire + // non-active quadrant. + notValidWrap |= + startOk && endOk && std::abs(angularDistance) > SimTK::Pi; + return !notValidWrap; + } + +} + +// Section on wrapping of a 2D circle. +namespace { + + struct CircleWrap final { + CircleWrap() = default; + + CircleWrap( + RotationDirection direction, + PathSegment path, + Quadrant quadrant) : + wrappedAngularDistance(AngularDistance( + path, + direction)), + tangentLineSegments(path), + wrapIsValid(IsValidQuadrantWrapped( + path, + wrappedAngularDistance, + quadrant)) + {} + + static CircleWrap NoWrap() { + return CircleWrap{}; + } + + double getWrappedPathStartAngle() const { + return tangentLineSegments.start.angleOfTangentPoint.getValue(); + } + + double wrappedAngularDistance = SimTK::NaN; + PathSegment tangentLineSegments{{}, {}}; + bool wrapIsValid = false; + }; + + double ComputeTotalPathLength( + const CircleWrap& wrap, + double circleRadius) + { + return wrap.tangentLineSegments.start.length + + wrap.tangentLineSegments.end.length + + circleRadius * std::abs(wrap.wrappedAngularDistance); + } + +} + +// Section regarding the two possible directions in which you can compute a +// wrapping path on a circle. +namespace { + + // Struct for holding a generic pair, that differ in the wrapping direction. + template + struct PositiveAndNegativeRotatingPair final { + PositiveAndNegativeRotatingPair( + T positiveRotatingVersion, + T negativeRotatingVersion) : + positive(std::move(positiveRotatingVersion)), + negative(std::move(negativeRotatingVersion)) + {} + + PositiveAndNegativeRotatingPair(std::function&& f) : + PositiveAndNegativeRotatingPair + { + f(RotationDirection::Positive), + f(RotationDirection::Negative) + } + {} + + template + PositiveAndNegativeRotatingPair( + const PositiveAndNegativeRotatingPair& other, + Args&& ...args) : + PositiveAndNegativeRotatingPair + { + T(RotationDirection::Positive, other.positive, std::forward(args)...), + T(RotationDirection::Negative, other.negative, std::forward(args)...) + } + {} + + T positive; + T negative; + }; + + // Computes for both positive and negative wrapping directions, the tangent + // line path segments. + PositiveAndNegativeRotatingPair> + ComputePossibleTangentLineSegments( + const PathSegmentVec2& path, + double circleRadius) + { + // Compute for each path segment (start/end) both possible tangent lines + // (positive/negative). + const PathSegment tangentLines(path, circleRadius); + + // Get one of the possible paths by setting the rotation direction. + auto GetPathSegmentsGivenDirection = [&tangentLines]( + RotationDirection direction)->PathSegment + { + return { + tangentLines.start.getTangentLineStartingAtPoint(direction), + tangentLines.end.getTangentLineEndingAtPoint(direction), + }; + }; + return {GetPathSegmentsGivenDirection}; + } + + // Computes for both positive and negative wrapping directions, the wrapping + // path on a circle. + PositiveAndNegativeRotatingPair + ComputePossibleCircleWrapPaths( + const PathSegmentVec2& path, + double circleRadius, + Quadrant quadrant) + { + const PositiveAndNegativeRotatingPair> + possibleTangentSegments = ComputePossibleTangentLineSegments( + path, + circleRadius); + + return {possibleTangentSegments, quadrant}; + } + +} + +// This section wraps up the 2D circle wrapping, by choosing the best path from +// two possible wrapping paths. +namespace { + + CircleWrap TakeBestPath( + const CircleWrap& wrapA, + const CircleWrap& wrapB) + { + // Neither wrapping path is valid. + if (!wrapA.wrapIsValid && !wrapB.wrapIsValid) { + return CircleWrap::NoWrap(); + } + // Both are valid: Take the shortest path. + if (wrapA.wrapIsValid && wrapB.wrapIsValid) { + return std::abs(wrapA.wrappedAngularDistance) + < std::abs(wrapB.wrappedAngularDistance)? + wrapA : wrapB; + } + // Return the path that is valid, if any. + return wrapA.wrapIsValid? wrapA : wrapB; + } + + CircleWrap TakeBestPath( + PositiveAndNegativeRotatingPair possibleWrappings) + { + return TakeBestPath(possibleWrappings.positive, + possibleWrappings.negative); + } + + // Computes the best wrapping path on a circle. + // This function includes the logic for checking the active quadrant, and + // choosing from both possible wrapping directions. + CircleWrap ComputeBestCircleWrappingPath( + const PathSegmentVec2& path, + double circleRadius, + Quadrant quadrant) + { + if (DoNotAttemptWrapping(path, circleRadius, quadrant)) { + return CircleWrap::NoWrap(); + } + + const PositiveAndNegativeRotatingPair possiblePaths = + ComputePossibleCircleWrapPaths( + path, + circleRadius, + quadrant); + + return TakeBestPath(possiblePaths); + } + +} + +// Section regarding the axial component of a cylinder wrapping path: the +// AxialWrap. Axial component of a cylinder wrapping path can be computed given +// the solution of wrapping a 2D circle. The CircleWrap together with the +// AxialWrap gives the CylinderWrap. +namespace { + + PathSegment ComputeAxialWrapPath( + const CircleWrap& circleWrap, + const PathSegment& axialPath, + double circleRadius) + { + // Axial-path start and end can be computed by noting that the + // ratio of axial-distance to tangential-distance must + // remain constant for each path segment. + const double totalTangentialDistance = ComputeTotalPathLength( + circleWrap, + circleRadius); + const double totalAxialDistance = Difference(axialPath); + const double ratioAxialToTangentialDistance = + totalAxialDistance / totalTangentialDistance; + + return { + axialPath.start + circleWrap.tangentLineSegments.start.length + * ratioAxialToTangentialDistance, + axialPath.end - circleWrap.tangentLineSegments.end.length + * ratioAxialToTangentialDistance, + }; + } + + bool IsAxialPathWithinCylinderLength( + const PathSegment wrappedAxialPath, + double cylinderLength) + { + return std::abs(wrappedAxialPath.start) <= cylinderLength / 2. + && std::abs(wrappedAxialPath.end) <= cylinderLength / 2.; + } + + struct AxialWrap { + AxialWrap() = default; + + AxialWrap( + const CircleWrap& circleWrap, + const PathSegment& axialPath, + double circleRadius, + double cylinderLength) : + wrappedPath(ComputeAxialWrapPath(circleWrap, axialPath, circleRadius)), + wrapIsValid(IsAxialPathWithinCylinderLength(wrappedPath, cylinderLength)) + {} + + static AxialWrap NoWrap() + { + return AxialWrap{}; + } + + PathSegment wrappedPath{SimTK::NaN, SimTK::NaN}; + bool wrapIsValid = false; + }; + +} + +// Section on the cylinder wrapping: Combines the Circle- and Axial wrapping. +namespace { + + struct SpiralCylinderWrap final { + + SpiralCylinderWrap( + const PathSegmentVec3& path, + double radius, + double cylinderLength, + const Quadrant& quadrant) : + radius(radius), + circleWrap(ComputeBestCircleWrappingPath( + {path.start.getSubVec<2>(0), path.end.getSubVec<2>(0)}, + radius, + quadrant)) + { + if (circleWrap.wrapIsValid) { + axialWrap = AxialWrap( + circleWrap, + {path.start[2], path.end[2]}, + radius, + cylinderLength); + } + } + + double computeWrappedPathLength() const { + return std::sqrt( + std::pow(radius * circleWrap.wrappedAngularDistance, 2) + + std::pow(Difference(axialWrap.wrappedPath), 2) + ); + } + + // Get the path point on the wrapped surface for anywhere between start + // (t=0) and end (t=1). + SimTK::Vec3 computeWrappedPathPoint(double t) const + { + const double angle = t * circleWrap.wrappedAngularDistance + + circleWrap.getWrappedPathStartAngle(); + const double axialCoord = t * Difference(axialWrap.wrappedPath) + + axialWrap.wrappedPath.start; + return SimTK::Vec3( + std::cos(angle) * radius, + std::sin(angle) * radius, + axialCoord); + } + + int wrapAction() const { + // TODO not using: wrapped? = successful wrap, but may not be 'best' path + // TODO not using: insideRadius + if (circleWrap.wrapIsValid && axialWrap.wrapIsValid) { + return WrapObject::WrapAction::mandatoryWrap; + } + return WrapObject::WrapAction::noWrap; + } + + double radius = SimTK::NaN; + CircleWrap circleWrap = CircleWrap::NoWrap(); + AxialWrap axialWrap = AxialWrap::NoWrap(); + }; + +} + //============================================================================= // CONSTRUCTOR(S) AND DESTRUCTOR //============================================================================= @@ -177,6 +885,8 @@ string WrapCylinder::getDimensionsString() const int WrapCylinder::wrapLine(const SimTK::State& s, SimTK::Vec3& aPoint1, SimTK::Vec3& aPoint2, const PathWrap& aPathWrap, WrapResult& aWrapResult, bool& aFlag) const { + SpiralCylinderWrap wrap = SpiralCylinderWrap({aPoint1, aPoint2}, get_radius(), get_length(), {_wrapAxis, _wrapSign}); + const double& _radius = get_radius(); double dist, p11_dist, p22_dist, t, dot1, dot2, dot3, dot4, d, sin_theta, /* *r11, *r22, */alpha, beta, r_squared = _radius * _radius;