From 9a708e0bf14ef55ad7b16b61c6471fd6ce8364b8 Mon Sep 17 00:00:00 2001 From: Travis CI Date: Wed, 16 Mar 2022 12:26:15 +0100 Subject: [PATCH 1/5] Refactored WrapCylinder::_calc_spiral_wrap_point for const-correctness and shuffled locals around --- OpenSim/Simulation/Wrap/WrapCylinder.cpp | 91 +++++++++++++++--------- 1 file changed, 59 insertions(+), 32 deletions(-) diff --git a/OpenSim/Simulation/Wrap/WrapCylinder.cpp b/OpenSim/Simulation/Wrap/WrapCylinder.cpp index de15e33435..a2cf9929af 100644 --- a/OpenSim/Simulation/Wrap/WrapCylinder.cpp +++ b/OpenSim/Simulation/Wrap/WrapCylinder.cpp @@ -594,6 +594,21 @@ int WrapCylinder::wrapLine(const SimTK::State& s, SimTK::Vec3& aPoint1, SimTK::V return return_code; } +namespace +{ + // helper method as a stand-in before the WrapMath version is changed + Vec3 Helper_GetClosestPointOnLineFromPoint(const Vec3& linePoint, const Vec3& lineDir, const Vec3& p) + { + Vec3 linePointCopy = linePoint; + Vec3 lineDirCopy = lineDir; + Vec3 pCopy = p; + double t; + Vec3 rv; + WrapMath::GetClosestPointOnLineToPoint(pCopy, linePointCopy,lineDirCopy, rv, t); + return rv; + } +} + //_____________________________________________________________________________ /** * Calculate the wrapping points along a spiral path on the cylinder from aPoint1 @@ -610,49 +625,39 @@ void WrapCylinder::_make_spiral_path(SimTK::Vec3& aPoint1, bool far_side_wrap, WrapResult& aWrapResult) const { - double x, y, t, axial_dist, theta; - Vec3 r1a, r2a, uu, vv, ax, axial_vec, wrap_pt; - double sense = far_side_wrap ? -1.0 : 1.0; - int i, iterations = 0; - const double _radius = get_radius(); + const double sense = far_side_wrap ? -1.0 : 1.0; + const double radius = get_radius(); + int iterations = 0; restart_spiral_wrap: aWrapResult.wrap_pts.setSize(0); // determine the axial vector - - WrapMath::GetClosestPointOnLineToPoint(aWrapResult.r1, p0, dn, r1a, t); - WrapMath::GetClosestPointOnLineToPoint(aWrapResult.r2, p0, dn, r2a, t); - - axial_vec = r2a - r1a; - - axial_dist = axial_vec.norm(); + const Vec3 r1a = Helper_GetClosestPointOnLineFromPoint(p0, dn, aWrapResult.r1); + const Vec3 r2a = Helper_GetClosestPointOnLineFromPoint(p0, dn, aWrapResult.r2); // determine the radial angle - uu = aWrapResult.r1 - r1a; - vv = aWrapResult.r2 - r2a; + const Vec3 uu = (aWrapResult.r1 - r1a)/radius; + Vec3 vv = (aWrapResult.r2 - r2a)/radius; - for (i = 0; i < 3; i++) - { - uu[i] /= _radius; - vv[i] /= _radius; + double theta = acos((~uu * vv) / (uu.norm() * vv.norm())); + if (far_side_wrap) { + theta = 2.0*SimTK_PI - theta; } - theta = acos((~uu * vv) / (uu.norm() * vv.norm())); - - if (far_side_wrap) - theta = 2.0 * SimTK_PI - theta; + const Vec3 axial_vec = r2a - r1a; // use Pythagoras to calculate the length of the spiral path (imaging // a right triangle wrapping around the surface of a cylinder) - x = _radius * theta; - y = axial_dist; - - aWrapResult.wrap_path_length = sqrt(x * x + y * y); + { + const double x = radius * theta; + const double y = axial_vec.norm(); + aWrapResult.wrap_path_length = sqrt(x*x + y*y); + } // build path segments - ax = uu % vv; + Vec3 ax = uu % vv; WrapMath::NormalizeOrZero(ax, ax); vv = ax % uu; @@ -661,15 +666,36 @@ void WrapCylinder::_make_spiral_path(SimTK::Vec3& aPoint1, m.set(0, 0, ax[0]); m.set(0, 1, ax[1]); m.set(0, 2, ax[2]); m.set(1, 0, uu[0]); m.set(1, 1, uu[1]); m.set(1, 2, uu[2]); m.set(2, 0, vv[0]); m.set(2, 1, vv[1]); m.set(2, 2, vv[2]); + // WrapTorus creates a WrapCyl with no connected model, avoid this hack - if (!_model.empty() && !getModel().getDisplayHints().isVisualizationEnabled() && - aWrapResult.singleWrap) { + if (!_model.empty() && + !getModel().getDisplayHints().isVisualizationEnabled() && + aWrapResult.singleWrap) { + // Use one WrapSegment/cord instead of finely sampled list of wrap_pt(s) + Vec3 wrap_pt; _calc_spiral_wrap_point( - r1a, axial_vec, m, ax, sense, 0, theta, wrap_pt); + r1a, + axial_vec, + m, + ax, + sense, + 0, + theta, + wrap_pt); + aWrapResult.wrap_pts.append(wrap_pt); + _calc_spiral_wrap_point( - r1a, axial_vec, m, ax, sense, 1.0, theta, wrap_pt); + r1a, + axial_vec, + m, + ax, + sense, + 1.0, + theta, + wrap_pt); + aWrapResult.wrap_pts.append(wrap_pt); } else { @@ -678,8 +704,9 @@ void WrapCylinder::_make_spiral_path(SimTK::Vec3& aPoint1, int numWrapSegments = (int)(aWrapResult.wrap_path_length / 0.002); if (numWrapSegments < 1) numWrapSegments = 1; - for (i = 0; i < numWrapSegments; i++) { + for (int i = 0; i < numWrapSegments; i++) { double t = (double)i / numWrapSegments; + Vec3 wrap_pt; _calc_spiral_wrap_point( r1a, axial_vec, m, ax, sense, t, theta, wrap_pt); From 4db52a0259343218492008433cd2ac9a7d098866 Mon Sep 17 00:00:00 2001 From: Travis CI Date: Wed, 16 Mar 2022 21:48:13 +0100 Subject: [PATCH 2/5] Reimplemented WrapCylinder::_make_spiral_path for perf --- OpenSim/Simulation/Wrap/WrapCylinder.cpp | 236 +++++++++-------------- OpenSim/Simulation/Wrap/WrapCylinder.h | 25 +-- OpenSim/Simulation/Wrap/WrapMath.cpp | 30 ++- OpenSim/Simulation/Wrap/WrapMath.h | 30 ++- 4 files changed, 148 insertions(+), 173 deletions(-) diff --git a/OpenSim/Simulation/Wrap/WrapCylinder.cpp b/OpenSim/Simulation/Wrap/WrapCylinder.cpp index a2cf9929af..961f5f8f5f 100644 --- a/OpenSim/Simulation/Wrap/WrapCylinder.cpp +++ b/OpenSim/Simulation/Wrap/WrapCylinder.cpp @@ -44,11 +44,28 @@ using SimTK::Vec3; using SimTK::UnitVec3; static const char* wrapTypeName = "cylinder"; -static Vec3 p0(0.0, 0.0, -1.0); -static Vec3 dn(0.0, 0.0, 1.0); +static const Vec3 g_CylinderBottom(0.0, 0.0, -1.0); +static const UnitVec3 g_CylinderDirection(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 +// helper function for calling the wrapmath version +static Vec3 GetClosestPointOnLineToPoint(const Vec3& pt, + const Vec3& linePt, + const UnitVec3& lineDir) +{ + Vec3 rv; + double t; + WrapMath::GetClosestPointOnLineToPoint(pt, linePt, lineDir, rv, t); + return rv; +} + +static double CalcWrapAngle(const UnitVec3& a, const UnitVec3& b, bool reverse) +{ + double ang = acos(SimTK::dot(a, b)); + return !reverse ? ang : -(2.0*SimTK_PI - ang); +} + //============================================================================= // CONSTRUCTOR(S) AND DESTRUCTOR //============================================================================= @@ -208,8 +225,8 @@ int WrapCylinder::wrapLine(const SimTK::State& s, SimTK::Vec3& aPoint1, SimTK::V aWrapResult.wrap_pts.setSize(0); // abort if aPoint1 or aPoint2 is inside the cylinder. - if (WrapMath::CalcDistanceSquaredPointToLine(aPoint1, p0, dn) < r_squared || - WrapMath::CalcDistanceSquaredPointToLine(aPoint2, p0, dn) < r_squared) + if (WrapMath::CalcDistanceSquaredPointToLine(aPoint1, g_CylinderBottom, g_CylinderDirection) < r_squared || + WrapMath::CalcDistanceSquaredPointToLine(aPoint2, g_CylinderBottom, g_CylinderDirection) < r_squared) { return insideRadius; } @@ -242,8 +259,8 @@ int WrapCylinder::wrapLine(const SimTK::State& s, SimTK::Vec3& aPoint1, SimTK::V } // find points p11 & p22 on the cylinder axis closest aPoint1 & aPoint2 - WrapMath::GetClosestPointOnLineToPoint(aPoint1, p0, dn, p11, t); - WrapMath::GetClosestPointOnLineToPoint(aPoint2, p0, dn, p22, t); + WrapMath::GetClosestPointOnLineToPoint(aPoint1, g_CylinderBottom, g_CylinderDirection, p11, t); + WrapMath::GetClosestPointOnLineToPoint(aPoint2, g_CylinderBottom, g_CylinderDirection, p22, t); // find preliminary tangent point candidates r1a & r1b vv = aPoint1 - p11; @@ -259,7 +276,7 @@ int WrapCylinder::wrapLine(const SimTK::State& s, SimTK::Vec3& aPoint1, SimTK::V dist = sqrt(r_squared - dist * dist); - uu = dn % vv; + uu = g_CylinderDirection % vv; for (i = 0; i < 3; i++) { @@ -281,7 +298,7 @@ int WrapCylinder::wrapLine(const SimTK::State& s, SimTK::Vec3& aPoint1, SimTK::V dist = sqrt(r_squared - dist * dist); - uu = dn % vv; + uu = g_CylinderDirection % vv; for (i = 0; i < 3; i++) { @@ -475,7 +492,7 @@ int WrapCylinder::wrapLine(const SimTK::State& s, SimTK::Vec3& aPoint1, SimTK::V mpt[i] = aPoint1[i] + t * (aPoint2[i] - aPoint1[i]); // find closest point on cylinder axis to mpt - WrapMath::GetClosestPointOnLineToPoint(mpt, p0, dn, axispt, t); + WrapMath::GetClosestPointOnLineToPoint(mpt, g_CylinderBottom, g_CylinderDirection, axispt, t); // find normal of plane through aPoint1, aPoint2, axispt l1 = aPoint1 - axispt; @@ -490,9 +507,9 @@ int WrapCylinder::wrapLine(const SimTK::State& s, SimTK::Vec3& aPoint1, SimTK::V // cross plane normal and cylinder axis (each way) to // get vectors pointing from axispt towards mpt and // away from mpt (you can't tell which is which yet). - vert1 = dn % plane_normal; + vert1 = g_CylinderDirection % plane_normal; WrapMath::NormalizeOrZero(vert1, vert1); - vert2 = plane_normal % dn; + vert2 = plane_normal % g_CylinderDirection; WrapMath::NormalizeOrZero(vert2, vert2); // now find two potential apex points, one along each vector @@ -548,11 +565,11 @@ int WrapCylinder::wrapLine(const SimTK::State& s, SimTK::Vec3& aPoint1, SimTK::V for (i = 0; i < 3; i++) { - r1a[i] = aWrapResult.r1[i] - 10.0 * dn[i]; - r2a[i] = aWrapResult.r2[i] - 10.0 * dn[i]; + r1a[i] = aWrapResult.r1[i] - 10.0 * g_CylinderDirection[i]; + r2a[i] = aWrapResult.r2[i] - 10.0 * g_CylinderDirection[i]; - r1b[i] = aWrapResult.r1[i] + 10.0 * dn[i]; - r2b[i] = aWrapResult.r2[i] + 10.0 * dn[i]; + r1b[i] = aWrapResult.r1[i] + 10.0 * g_CylinderDirection[i]; + r2b[i] = aWrapResult.r2[i] + 10.0 * g_CylinderDirection[i]; } r1_inter = WrapMath::IntersectLineSegPlane(r1a, r1b, plane_normal, d, r1p); @@ -594,20 +611,6 @@ int WrapCylinder::wrapLine(const SimTK::State& s, SimTK::Vec3& aPoint1, SimTK::V return return_code; } -namespace -{ - // helper method as a stand-in before the WrapMath version is changed - Vec3 Helper_GetClosestPointOnLineFromPoint(const Vec3& linePoint, const Vec3& lineDir, const Vec3& p) - { - Vec3 linePointCopy = linePoint; - Vec3 lineDirCopy = lineDir; - Vec3 pCopy = p; - double t; - Vec3 rv; - WrapMath::GetClosestPointOnLineToPoint(pCopy, linePointCopy,lineDirCopy, rv, t); - return rv; - } -} //_____________________________________________________________________________ /** @@ -620,112 +623,94 @@ namespace * @param far_side_wrap Boolean indicating if the wrap is the long way around * @param aWrapResult The result of the wrapping (tangent points, etc.) */ -void WrapCylinder::_make_spiral_path(SimTK::Vec3& aPoint1, - SimTK::Vec3& aPoint2, - bool far_side_wrap, - WrapResult& aWrapResult) const +void WrapCylinder::_make_spiral_path(const SimTK::Vec3& p1, + const SimTK::Vec3& p2, + bool doFarSideWrap, + WrapResult& out) const { - const double sense = far_side_wrap ? -1.0 : 1.0; const double radius = get_radius(); int iterations = 0; restart_spiral_wrap: - aWrapResult.wrap_pts.setSize(0); - - // determine the axial vector - const Vec3 r1a = Helper_GetClosestPointOnLineFromPoint(p0, dn, aWrapResult.r1); - const Vec3 r2a = Helper_GetClosestPointOnLineFromPoint(p0, dn, aWrapResult.r2); + const Vec3 r1SurfacePos = out.r1; + const Vec3 r1AxialPos = GetClosestPointOnLineToPoint(r1SurfacePos, g_CylinderBottom, g_CylinderDirection); + const Vec3 r2SurfacePos = out.r2; + const Vec3 r2AxialPos = GetClosestPointOnLineToPoint(r2SurfacePos, g_CylinderBottom, g_CylinderDirection); - // determine the radial angle - const Vec3 uu = (aWrapResult.r1 - r1a)/radius; - Vec3 vv = (aWrapResult.r2 - r2a)/radius; + const Vec3 r1AxisToSurface = r1SurfacePos - r1AxialPos; + const UnitVec3 r1AxisToSurfaceDir(r1AxisToSurface); + const UnitVec3 r2AxisToSurfaceDir(r2SurfacePos - r2AxialPos); - double theta = acos((~uu * vv) / (uu.norm() * vv.norm())); - if (far_side_wrap) { - theta = 2.0*SimTK_PI - theta; - } + const Vec3 r1AxisToR2Axis = r2AxialPos - r1AxialPos; - const Vec3 axial_vec = r2a - r1a; + const double theta = CalcWrapAngle( + r1AxisToSurfaceDir, + r2AxisToSurfaceDir, + doFarSideWrap); - // use Pythagoras to calculate the length of the spiral path (imaging - // a right triangle wrapping around the surface of a cylinder) + // use Pythagoras to calculate the length of the spiral path (imagine + // a right angle where one edge is a line around the circumference and + // the other edge is a line along the cylinder axis at a right angle) { - const double x = radius * theta; - const double y = axial_vec.norm(); - aWrapResult.wrap_path_length = sqrt(x*x + y*y); + const double x = radius * theta; // circumference + const double y = r1AxisToR2Axis.norm(); + out.wrap_path_length = sqrt(x*x + y*y); } - // build path segments - Vec3 ax = uu % vv; - WrapMath::NormalizeOrZero(ax, ax); - - vv = ax % uu; + const UnitVec3 ax{SimTK::cross(r1AxisToSurfaceDir, r2AxisToSurfaceDir)}; - SimTK::Rotation m; - m.set(0, 0, ax[0]); m.set(0, 1, ax[1]); m.set(0, 2, ax[2]); - m.set(1, 0, uu[0]); m.set(1, 1, uu[1]); m.set(1, 2, uu[2]); - m.set(2, 0, vv[0]); m.set(2, 1, vv[1]); m.set(2, 2, vv[2]); + auto CalcWrapPoint = [&](double t) // t = 0.0 to 1.0 + { + SimTK::Mat33 R = SimTK::Rotation{t * theta, ax}; + return r1AxialPos + R*r1AxisToSurface + t*r1AxisToR2Axis; + }; // WrapTorus creates a WrapCyl with no connected model, avoid this hack if (!_model.empty() && !getModel().getDisplayHints().isVisualizationEnabled() && - aWrapResult.singleWrap) { + out.singleWrap) { // Use one WrapSegment/cord instead of finely sampled list of wrap_pt(s) - Vec3 wrap_pt; - _calc_spiral_wrap_point( - r1a, - axial_vec, - m, - ax, - sense, - 0, - theta, - wrap_pt); - - aWrapResult.wrap_pts.append(wrap_pt); - - _calc_spiral_wrap_point( - r1a, - axial_vec, - m, - ax, - sense, - 1.0, - theta, - wrap_pt); - - aWrapResult.wrap_pts.append(wrap_pt); + out.wrap_pts.setSize(2); + out.wrap_pts[0] = CalcWrapPoint(0); + out.wrap_pts[1] = CalcWrapPoint(1.0); } else { + // Each muscle segment on the surface of the cylinder should be // 0.002 meters long. This assumes the model is in meters, of course. - int numWrapSegments = (int)(aWrapResult.wrap_path_length / 0.002); - if (numWrapSegments < 1) numWrapSegments = 1; + int numWrapSegments = static_cast(out.wrap_path_length / 0.002); + if (numWrapSegments < 1) { + numWrapSegments = 1; + } + + out.wrap_pts.setSize(0); for (int i = 0; i < numWrapSegments; i++) { - double t = (double)i / numWrapSegments; - Vec3 wrap_pt; + double t = static_cast(i)/static_cast(numWrapSegments); - _calc_spiral_wrap_point( - r1a, axial_vec, m, ax, sense, t, theta, wrap_pt); + Vec3 wrap_pt = CalcWrapPoint(t); // adjust r1/r2 tangent points if necessary to achieve tangency with // the spiral path: - if (i == 1 && iterations < MAX_ITERATIONS && - !aWrapResult.singleWrap) { - bool did_adjust_r2 = false; - bool did_adjust_r1 = _adjust_tangent_point( - aPoint1, dn, aWrapResult.r1, wrap_pt); + if (i == 1 + && iterations < MAX_ITERATIONS + && !out.singleWrap) { - SimTK::Vec3 temp_wrap_pt; + bool did_adjust_r1 = _adjust_tangent_point( + p1, + g_CylinderDirection, + out.r1, + wrap_pt); - _calc_spiral_wrap_point(r1a, axial_vec, m, ax, sense, 1.0 - t, - theta, temp_wrap_pt); + Vec3 temp_wrap_pt = CalcWrapPoint(1.0 - t); - did_adjust_r2 = _adjust_tangent_point( - aPoint2, dn, aWrapResult.r2, temp_wrap_pt); + bool did_adjust_r2 = _adjust_tangent_point( + p2, + g_CylinderDirection, + out.r2, + temp_wrap_pt); if (did_adjust_r1 || did_adjust_r2) { iterations++; @@ -733,48 +718,11 @@ void WrapCylinder::_make_spiral_path(SimTK::Vec3& aPoint1, } } - aWrapResult.wrap_pts.append(wrap_pt); + out.wrap_pts.append(wrap_pt); } } } -//_____________________________________________________________________________ -/** - * Calculate a new point along a spiral wrapping path. - * - * @param r1a An existing point on the spiral path - * @param axial_vec Vector from r1a parallel to cylinder axis - * @param m A transform matrix used for the radial component - * @param axis Axis of the cylinder - * @param sense The direction of the spiral - * @param t Parameterized amount of angle for this point - * @param theta The total angle of the spiral on the cylinder - * @param wrap_pt The new point on the spiral path - */ -void WrapCylinder::_calc_spiral_wrap_point(const SimTK::Vec3& r1a, - const SimTK::Vec3& axial_vec, - const SimTK::Rotation& m, - const SimTK::Vec3& axis, - double sense, - double t, - double theta, - SimTK::Vec3& wrap_pt) const -{ - SimTK::Rotation R; - double angle = sense * t * theta; - R.setRotationFromAngleAboutNonUnitVector(angle, axis); - - SimTK::Mat33 n = m * ~R; - - for (int i = 0; i < 3; i++) - { - double radial_component = get_radius() * n[1][i]; - double axial_component = t * axial_vec[i]; - - wrap_pt[i] = r1a[i] + radial_component + axial_component; - } -} - //_____________________________________________________________________________ /** * Determine whether the specified tangent point 'r1' @@ -789,10 +737,10 @@ void WrapCylinder::_calc_spiral_wrap_point(const SimTK::Vec3& r1a, * @param w1 A wrapping point (?) * @return Whether or not the point was adjusted */ -bool WrapCylinder::_adjust_tangent_point(SimTK::Vec3& pt1, - SimTK::Vec3& dn, - SimTK::Vec3& r1, - SimTK::Vec3& w1) const +bool WrapCylinder::_adjust_tangent_point(const SimTK::Vec3& pt1, + const SimTK::UnitVec3& dn, + SimTK::Vec3& r1, + const SimTK::Vec3& w1) const { SimTK::Vec3 pr_vec = r1 - pt1; SimTK::Vec3 rw_vec = w1 - r1; diff --git a/OpenSim/Simulation/Wrap/WrapCylinder.h b/OpenSim/Simulation/Wrap/WrapCylinder.h index 883e722b35..256d0ef31d 100644 --- a/OpenSim/Simulation/Wrap/WrapCylinder.h +++ b/OpenSim/Simulation/Wrap/WrapCylinder.h @@ -78,22 +78,15 @@ OpenSim_DECLARE_CONCRETE_OBJECT(WrapCylinder, WrapObject); private: void constructProperties(); - void _make_spiral_path(SimTK::Vec3& aPoint1, SimTK::Vec3& aPoint2, - bool far_side_wrap,WrapResult& aWrapResult) const; - void _calc_spiral_wrap_point(const SimTK::Vec3& r1a, - const SimTK::Vec3& axial_vec, - const SimTK::Rotation& rotation, - const SimTK::Vec3& axis, - double sense, - double t, - double theta, - SimTK::Vec3& wrap_pt) const; - - - bool _adjust_tangent_point(SimTK::Vec3& pt1, - SimTK::Vec3& dn, - SimTK::Vec3& r1, - SimTK::Vec3& w1) const; + void _make_spiral_path(const SimTK::Vec3& aPoint1, + const SimTK::Vec3& aPoint2, + bool farSideWrap, + WrapResult& aWrapResult) const; + + bool _adjust_tangent_point(const SimTK::Vec3& pt1, + const SimTK::UnitVec3& dn, + SimTK::Vec3& r1, + const SimTK::Vec3& w1) const; //============================================================================= }; // END of class WrapCylinder diff --git a/OpenSim/Simulation/Wrap/WrapMath.cpp b/OpenSim/Simulation/Wrap/WrapMath.cpp index f0dd884620..fa8e31beae 100644 --- a/OpenSim/Simulation/Wrap/WrapMath.cpp +++ b/OpenSim/Simulation/Wrap/WrapMath.cpp @@ -68,9 +68,13 @@ using SimTK::Mat33; * @param t parameterized distance along second line from p3 to pInt2 * @return false if lines are parallel, true otherwise */ -bool WrapMath:: -IntersectLines(SimTK::Vec3& p1, SimTK::Vec3& p2, SimTK::Vec3& p3, SimTK::Vec3& p4, - SimTK::Vec3& pInt1, double& s, SimTK::Vec3& pInt2, double& t) +bool WrapMath::IntersectLines(const SimTK::Vec3& p1, + const SimTK::Vec3& p2, + const SimTK::Vec3& p3, + const SimTK::Vec3& p4, + SimTK::Vec3& pInt1, + double& s, + SimTK::Vec3& pInt2, double& t) { SimTK::Vec3 vec1 = p2 - p1; @@ -158,8 +162,24 @@ IntersectLineSegPlane(SimTK::Vec3& pt1, SimTK::Vec3& pt2, * @param t parameterized distance from linePt along line to closestPt */ void WrapMath:: -GetClosestPointOnLineToPoint(SimTK::Vec3& pt, SimTK::Vec3& linePt, SimTK::Vec3& line, - SimTK::Vec3& closestPt, double& t) +GetClosestPointOnLineToPoint(const SimTK::Vec3& pt, + const SimTK::Vec3& linePt, + const SimTK::UnitVec3& lineDir, + SimTK::Vec3& closestPt, + double& t) +{ + Vec3 v1 = pt - linePt; + double vCosTheta = SimTK::dot(v1, lineDir); + closestPt = linePt + vCosTheta*lineDir; + t = vCosTheta; +} + +void WrapMath::GetClosestPointOnLineToPoint( + const SimTK::Vec3& pt, + const SimTK::Vec3& linePt, + const SimTK::Vec3& line, + SimTK::Vec3& closestPt, + double& t) { SimTK::Vec3 v1, v2; diff --git a/OpenSim/Simulation/Wrap/WrapMath.h b/OpenSim/Simulation/Wrap/WrapMath.h index 64f40e11b3..c5b75c5b30 100644 --- a/OpenSim/Simulation/Wrap/WrapMath.h +++ b/OpenSim/Simulation/Wrap/WrapMath.h @@ -48,22 +48,36 @@ class OSIMSIMULATION_API WrapMath //============================================================================= public: static bool - IntersectLines(SimTK::Vec3& p1, SimTK::Vec3& p2, - SimTK::Vec3& p3, SimTK::Vec3& p4, - SimTK::Vec3& pInt1, double& s, - SimTK::Vec3& pInt2, double& t); + IntersectLines(const SimTK::Vec3& p1, + const SimTK::Vec3& p2, + const SimTK::Vec3& p3, + const SimTK::Vec3& p4, + SimTK::Vec3& pInt1, + double& s, + SimTK::Vec3& pInt2, + double& t); static bool IntersectLineSegPlane(SimTK::Vec3& pt1, SimTK::Vec3& pt2, SimTK::UnitVec3& plane, double d, SimTK::Vec3& inter); - static void - GetClosestPointOnLineToPoint(SimTK::Vec3& pt, SimTK::Vec3& linePt, SimTK::Vec3& line, - SimTK::Vec3& closestPt, double& t); + static void GetClosestPointOnLineToPoint( + const SimTK::Vec3& pt, + const SimTK::Vec3& linePt, + const SimTK::UnitVec3& line, + SimTK::Vec3& closestPt, + double& t); + static void GetClosestPointOnLineToPoint( + const SimTK::Vec3& pt, + const SimTK::Vec3& linePt, + const SimTK::Vec3& line, + SimTK::Vec3& closestPt, + double& t); + inline static double CalcDistanceSquaredBetweenPoints( SimTK::Vec3& point1, SimTK::Vec3& point2) { return (point1 - point2).normSqr(); } inline static double CalcDistanceSquaredPointToLine( - SimTK::Vec3& point, SimTK::Vec3& linePt, SimTK::Vec3& line){ + const SimTK::Vec3& point, const SimTK::Vec3& linePt, const SimTK::Vec3& line){ SimTK::Vec3 pToLinePt = (linePt - point); SimTK::Vec3 n = line.normalize(); return (pToLinePt - (~pToLinePt * n) * n).normSqr(); From f628028353d67527d1d02503044f79a0e2e5b70f Mon Sep 17 00:00:00 2001 From: Travis CI Date: Sat, 19 Mar 2022 14:46:39 +0100 Subject: [PATCH 3/5] Cleaned up wrap angle calc --- OpenSim/Simulation/Wrap/WrapCylinder.cpp | 14 ++++---------- 1 file changed, 4 insertions(+), 10 deletions(-) diff --git a/OpenSim/Simulation/Wrap/WrapCylinder.cpp b/OpenSim/Simulation/Wrap/WrapCylinder.cpp index 961f5f8f5f..5b70552a62 100644 --- a/OpenSim/Simulation/Wrap/WrapCylinder.cpp +++ b/OpenSim/Simulation/Wrap/WrapCylinder.cpp @@ -60,12 +60,6 @@ static Vec3 GetClosestPointOnLineToPoint(const Vec3& pt, return rv; } -static double CalcWrapAngle(const UnitVec3& a, const UnitVec3& b, bool reverse) -{ - double ang = acos(SimTK::dot(a, b)); - return !reverse ? ang : -(2.0*SimTK_PI - ang); -} - //============================================================================= // CONSTRUCTOR(S) AND DESTRUCTOR //============================================================================= @@ -644,10 +638,10 @@ void WrapCylinder::_make_spiral_path(const SimTK::Vec3& p1, const Vec3 r1AxisToR2Axis = r2AxialPos - r1AxialPos; - const double theta = CalcWrapAngle( - r1AxisToSurfaceDir, - r2AxisToSurfaceDir, - doFarSideWrap); + double theta = acos(SimTK::dot(r1AxisToSurfaceDir, r2AxisToSurfaceDir)); + if (doFarSideWrap) { + theta -= 2.0*SimTK_PI; + } // use Pythagoras to calculate the length of the spiral path (imagine // a right angle where one edge is a line around the circumference and From 637b0b89633da684df02d6945ded682a14377b92 Mon Sep 17 00:00:00 2001 From: Travis CI Date: Sat, 19 Mar 2022 14:48:38 +0100 Subject: [PATCH 4/5] Cleaned up _make_spiral_path comment --- OpenSim/Simulation/Wrap/WrapCylinder.cpp | 18 ++++++++---------- 1 file changed, 8 insertions(+), 10 deletions(-) diff --git a/OpenSim/Simulation/Wrap/WrapCylinder.cpp b/OpenSim/Simulation/Wrap/WrapCylinder.cpp index 5b70552a62..3125a5ceba 100644 --- a/OpenSim/Simulation/Wrap/WrapCylinder.cpp +++ b/OpenSim/Simulation/Wrap/WrapCylinder.cpp @@ -605,21 +605,19 @@ int WrapCylinder::wrapLine(const SimTK::State& s, SimTK::Vec3& aPoint1, SimTK::V return return_code; } - -//_____________________________________________________________________________ /** - * Calculate the wrapping points along a spiral path on the cylinder from aPoint1 - * to aPoint2. This function may slide aPoint1 and aPoint2 axially along the - * cylinder's surface to achieve tangency to within 1 degree at the two points. + * Calculate the wrapping points along a spiral path on the cylinder from `p1` + * to `p2`. This function may slide `p1` and `p2` axially along the cylinder's + * surface to achieve tangency to within 1 degree at the two points. * - * @param aPoint1 One end of the spiral path - * @param aPoint2 The other end of the spiral path - * @param far_side_wrap Boolean indicating if the wrap is the long way around - * @param aWrapResult The result of the wrapping (tangent points, etc.) + * @param p1 One end of the spiral path + * @param p2 The other end of the spiral path + * @param doFarSideWrap Boolean indicating if the wrap is the long way around + * @param out The result of the wrapping (tangent points, etc.) */ void WrapCylinder::_make_spiral_path(const SimTK::Vec3& p1, const SimTK::Vec3& p2, - bool doFarSideWrap, + const bool doFarSideWrap, WrapResult& out) const { const double radius = get_radius(); From c51eb7f5f01fcac159c82a6eefa1b6070f5ff8b2 Mon Sep 17 00:00:00 2001 From: Travis CI Date: Sat, 19 Mar 2022 14:56:11 +0100 Subject: [PATCH 5/5] Added extremely appropriate TODOs into the wrapping implementation --- OpenSim/Simulation/Wrap/WrapCylinder.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/OpenSim/Simulation/Wrap/WrapCylinder.cpp b/OpenSim/Simulation/Wrap/WrapCylinder.cpp index 3125a5ceba..95721606cd 100644 --- a/OpenSim/Simulation/Wrap/WrapCylinder.cpp +++ b/OpenSim/Simulation/Wrap/WrapCylinder.cpp @@ -672,6 +672,8 @@ void WrapCylinder::_make_spiral_path(const SimTK::Vec3& p1, // Each muscle segment on the surface of the cylinder should be // 0.002 meters long. This assumes the model is in meters, of course. + // + // TODO: design, build, staff, and shoot an ion cannon at this int numWrapSegments = static_cast(out.wrap_path_length / 0.002); if (numWrapSegments < 1) { numWrapSegments = 1; @@ -680,6 +682,8 @@ void WrapCylinder::_make_spiral_path(const SimTK::Vec3& p1, out.wrap_pts.setSize(0); for (int i = 0; i < numWrapSegments; i++) { + // TODO: re-use the ion cannon (above) to annihilate this and + // TODO: replace it with `i/(numWrapSegments-1)` double t = static_cast(i)/static_cast(numWrapSegments); Vec3 wrap_pt = CalcWrapPoint(t); @@ -706,6 +710,8 @@ void WrapCylinder::_make_spiral_path(const SimTK::Vec3& p1, if (did_adjust_r1 || did_adjust_r2) { iterations++; + // TODO: realize that ion cannons (above) aren't enough and resort to + // using an appropriate death star on this goto restart_spiral_wrap; } }