From 9a708e0bf14ef55ad7b16b61c6471fd6ce8364b8 Mon Sep 17 00:00:00 2001 From: Travis CI Date: Wed, 16 Mar 2022 12:26:15 +0100 Subject: [PATCH 01/28] 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 02/28] 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 03/28] 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 04/28] 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 05/28] 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; } } From 764f70ec3f15ed8b586204a323869a16c3feecda Mon Sep 17 00:00:00 2001 From: Ayman Habib Date: Wed, 17 Aug 2022 21:53:48 -0700 Subject: [PATCH 06/28] Add test case for testWrappingAlgorithm.cpp --- .../Tests/Wrapping/testWrappingAlgorithm.cpp | 143 ++++++++++++++++++ 1 file changed, 143 insertions(+) create mode 100644 OpenSim/Tests/Wrapping/testWrappingAlgorithm.cpp diff --git a/OpenSim/Tests/Wrapping/testWrappingAlgorithm.cpp b/OpenSim/Tests/Wrapping/testWrappingAlgorithm.cpp new file mode 100644 index 0000000000..bb684a7256 --- /dev/null +++ b/OpenSim/Tests/Wrapping/testWrappingAlgorithm.cpp @@ -0,0 +1,143 @@ +/* -------------------------------------------------------------------------- * + * OpenSim: testWrappingAlgorithm.cpp * + * -------------------------------------------------------------------------- * + * The OpenSim API is a toolkit for musculoskeletal modeling and simulation. * + * See http://opensim.stanford.edu and the NOTICE file for more information. * + * OpenSim is developed at Stanford University and supported by the US * + * National Institutes of Health (U54 GM072970, R24 HD065690) and by DARPA * + * through the Warrior Web program. * + * * + * Copyright (c) 2005-2022 Stanford University and the Authors * + * Author(s): Ayman Habib * + * * + * Licensed under the Apache License, Version 2.0 (the "License"); you may * + * not use this file except in compliance with the License. You may obtain a * + * copy of the License at http://www.apache.org/licenses/LICENSE-2.0. * + * * + * Unless required by applicable law or agreed to in writing, software * + * distributed under the License is distributed on an "AS IS" BASIS, * + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * + * See the License for the specific language governing permissions and * + * limitations under the License. * + * -------------------------------------------------------------------------- */ +// INCLUDE +#include +#include + +#include +#include +#include + +using namespace OpenSim; +using namespace SimTK; +using namespace std; + +void testWrapObject(OpenSim::WrapObject *wObj); + +int main() +{ + SimTK::Array_ failures; + + try { + auto* wo = new WrapCylinder(); + wo->setName("pulley1"); + wo->set_radius(.5); + wo->set_length(1); + testWrapObject(wo); + } + catch (const std::exception& e) { + std::cout << "Exception: " << e.what() << std::endl; + failures.push_back("TestWrapCylinder"); + } + try { + auto* wo = new WrapSphere(); + wo->setName("pulley1"); + wo->set_radius(.5); + testWrapObject(wo); + } + catch (const std::exception& e) { + std::cout << "Exception: " << e.what() << std::endl; + failures.push_back("TestWrapSphere"); + } + try { + auto* wo = new WrapEllipsoid(); + wo->setName("pulley1"); + wo->set_dimensions(Vec3(.5)); + testWrapObject(wo); + } + catch (const std::exception& e) { + std::cout << "Exception: " << e.what() << std::endl; + failures.push_back("TestWrapEllipsoid"); + } + + + if (!failures.empty()) { + cout << "Done, with failure(s): " << failures << endl; + return 1; + } + + cout << "Done" << endl; + return 0; +} + +void testWrapObject(WrapObject* wrapObject) +{ + const double r = 0.5; + Model model; + model.setName("test"+wrapObject->getConcreteClassName()); + + auto& ground = model.updGround(); + auto body = new OpenSim::Body("body", 1, Vec3(0), Inertia(0.1, 0.1, 0.01)); + model.addComponent(body); + + auto joint = new PinJoint("pin", ground, *body); + auto& qi = joint->updCoordinate(); + qi.setName("q_pin"); + model.addComponent(joint); + + // Add the wrap object to the body, which takes ownership of it + WrapObject* wObj = wrapObject->clone(); + ground.addWrapObject(wObj); + + // One spring has wrap cylinder with respect to ground origin + PathSpring* spring1 = + new PathSpring("spring1", 1.0, 0.1, 0.01); + spring1->updGeometryPath(). + appendNewPathPoint("origin", ground, Vec3(r, r, 0)); + spring1->updGeometryPath(). + appendNewPathPoint("insert", *body, Vec3(-r, r, 0)); + spring1->updGeometryPath().addPathWrap(*wObj); + + model.addComponent(spring1); + + model.finalizeConnections(); + model.setUseVisualizer(true); + model.print(wObj->getConcreteClassName()+"Analytical.osim"); + SimTK::State& s = model.initSystem(); + auto& coord = joint->updCoordinate(); + const CoordinateSet& cset = model.getCoordinateSet(); + int nsteps = 100; + for (int i = 0; i < nsteps; ++i) { + + coord.setValue(s, i*SimTK::Pi/(2*nsteps)); + model.realizeVelocity(s); + + model.getVisualizer().show(s); + + double ma1 = spring1->computeMomentArm(s, coord); + + ASSERT_EQUAL(-r, ma1, SimTK::Eps); + + double len1 = spring1->getLength(s); + if (i== 0) { + std::cout << "i=" << i << "ma=" << ma1 << "len=" << len1 << std::endl; + ASSERT_EQUAL(len1, 1, SimTK::Eps); + } + if (i == nsteps - 1) { + std::cout << "i=" << i << "ma=" << ma1 << "len=" << len1 << std::endl; + std::cout << 1 + .25 * 2 * SimTK::Pi * r << std::endl; + ASSERT_EQUAL(len1, 1 + .25 * 2 * SimTK::Pi * r, .01); + } + } +} + From a24a33ce71393d460568ce505d1ddb595071bcc2 Mon Sep 17 00:00:00 2001 From: Ayman Habib Date: Thu, 18 Aug 2022 13:46:32 -0700 Subject: [PATCH 07/28] test cyliinder, sphere, ellpsoid path length and moment arms for correctness in nominal setting --- .../Tests/Wrapping/testWrappingAlgorithm.cpp | 38 ++++++++++++------- 1 file changed, 24 insertions(+), 14 deletions(-) diff --git a/OpenSim/Tests/Wrapping/testWrappingAlgorithm.cpp b/OpenSim/Tests/Wrapping/testWrappingAlgorithm.cpp index bb684a7256..796f70f97f 100644 --- a/OpenSim/Tests/Wrapping/testWrappingAlgorithm.cpp +++ b/OpenSim/Tests/Wrapping/testWrappingAlgorithm.cpp @@ -44,6 +44,8 @@ int main() wo->set_radius(.5); wo->set_length(1); testWrapObject(wo); + wo->set_quadrant("+y"); + testWrapObject(wo); } catch (const std::exception& e) { std::cout << "Exception: " << e.what() << std::endl; @@ -54,6 +56,8 @@ int main() wo->setName("pulley1"); wo->set_radius(.5); testWrapObject(wo); + wo->set_quadrant("+y"); + testWrapObject(wo); } catch (const std::exception& e) { std::cout << "Exception: " << e.what() << std::endl; @@ -64,12 +68,16 @@ int main() wo->setName("pulley1"); wo->set_dimensions(Vec3(.5)); testWrapObject(wo); + wo->set_quadrant("+y"); + testWrapObject(wo); + } catch (const std::exception& e) { std::cout << "Exception: " << e.what() << std::endl; failures.push_back("TestWrapEllipsoid"); } - + // Repeat with Rotate the wrap object by angle theta to get an ellipse with radii (.5/cos(theta), .5) + // length of wrap if (!failures.empty()) { cout << "Done, with failure(s): " << failures << endl; @@ -82,6 +90,7 @@ int main() void testWrapObject(WrapObject* wrapObject) { + auto visualize = false; const double r = 0.5; Model model; model.setName("test"+wrapObject->getConcreteClassName()); @@ -103,7 +112,7 @@ void testWrapObject(WrapObject* wrapObject) PathSpring* spring1 = new PathSpring("spring1", 1.0, 0.1, 0.01); spring1->updGeometryPath(). - appendNewPathPoint("origin", ground, Vec3(r, r, 0)); + appendNewPathPoint("origin", ground, Vec3(r-.1, r, 0)); //offset in X direction to avoid ambiguous scenario where path passes through center spring1->updGeometryPath(). appendNewPathPoint("insert", *body, Vec3(-r, r, 0)); spring1->updGeometryPath().addPathWrap(*wObj); @@ -111,32 +120,33 @@ void testWrapObject(WrapObject* wrapObject) model.addComponent(spring1); model.finalizeConnections(); - model.setUseVisualizer(true); - model.print(wObj->getConcreteClassName()+"Analytical.osim"); + model.setUseVisualizer(visualize); + //model.print(wObj->getConcreteClassName()+"Analytical.osim"); SimTK::State& s = model.initSystem(); auto& coord = joint->updCoordinate(); const CoordinateSet& cset = model.getCoordinateSet(); - int nsteps = 100; - for (int i = 0; i < nsteps; ++i) { + int nsteps = 10000; + for (int i = 0; i <= nsteps; ++i) { coord.setValue(s, i*SimTK::Pi/(2*nsteps)); model.realizeVelocity(s); - model.getVisualizer().show(s); + if (visualize) + model.getVisualizer().show(s); double ma1 = spring1->computeMomentArm(s, coord); - ASSERT_EQUAL(-r, ma1, SimTK::Eps); + ASSERT_EQUAL(-r, ma1, .0001); // SimTK::Eps double len1 = spring1->getLength(s); if (i== 0) { - std::cout << "i=" << i << "ma=" << ma1 << "len=" << len1 << std::endl; - ASSERT_EQUAL(len1, 1, SimTK::Eps); + //std::cout << "Testing " << wObj->getConcreteClassName() << std::endl; + //std::cout << "i=" << i << "ma=" << ma1 << "len=" << len1 << std::endl; + ASSERT_EQUAL(len1, 1 - 0.1, 1e-4); //SimTK::Eps } - if (i == nsteps - 1) { - std::cout << "i=" << i << "ma=" << ma1 << "len=" << len1 << std::endl; - std::cout << 1 + .25 * 2 * SimTK::Pi * r << std::endl; - ASSERT_EQUAL(len1, 1 + .25 * 2 * SimTK::Pi * r, .01); + if (i == nsteps) { // sgould be 1/4 way around wrapObject + //std::cout << "i=" << i << "ma=" << ma1 << "len=" << len1 << std::endl; + ASSERT_EQUAL(len1, 1 + .25 * 2 * SimTK::Pi * r -0.1, .0001); //SimTK::Eps } } } From 1ef8383e26b56a0973426bab5403a22a6b3f7c55 Mon Sep 17 00:00:00 2001 From: Ayman Habib Date: Thu, 18 Aug 2022 14:55:34 -0700 Subject: [PATCH 08/28] Reduce number of steps and move body COM to allow for path to wrap under gravity --- OpenSim/Tests/Wrapping/testWrappingAlgorithm.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/OpenSim/Tests/Wrapping/testWrappingAlgorithm.cpp b/OpenSim/Tests/Wrapping/testWrappingAlgorithm.cpp index 796f70f97f..b72ccc05e4 100644 --- a/OpenSim/Tests/Wrapping/testWrappingAlgorithm.cpp +++ b/OpenSim/Tests/Wrapping/testWrappingAlgorithm.cpp @@ -96,7 +96,7 @@ void testWrapObject(WrapObject* wrapObject) model.setName("test"+wrapObject->getConcreteClassName()); auto& ground = model.updGround(); - auto body = new OpenSim::Body("body", 1, Vec3(0), Inertia(0.1, 0.1, 0.01)); + auto body = new OpenSim::Body("body", 1, Vec3(-.5, 0, 0), Inertia(0.1, 0.1, 0.01)); model.addComponent(body); auto joint = new PinJoint("pin", ground, *body); @@ -125,7 +125,7 @@ void testWrapObject(WrapObject* wrapObject) SimTK::State& s = model.initSystem(); auto& coord = joint->updCoordinate(); const CoordinateSet& cset = model.getCoordinateSet(); - int nsteps = 10000; + int nsteps = 1000; for (int i = 0; i <= nsteps; ++i) { coord.setValue(s, i*SimTK::Pi/(2*nsteps)); From c6c45c16ac7a11d26325ab9f93c50961af98ae3a Mon Sep 17 00:00:00 2001 From: Ayman Habib Date: Mon, 22 Aug 2022 10:15:12 -0700 Subject: [PATCH 09/28] Add non-perpendicular wrapping case --- .../Tests/Wrapping/testWrappingAlgorithm.cpp | 40 +++++++++++++++---- 1 file changed, 33 insertions(+), 7 deletions(-) diff --git a/OpenSim/Tests/Wrapping/testWrappingAlgorithm.cpp b/OpenSim/Tests/Wrapping/testWrappingAlgorithm.cpp index b72ccc05e4..0c143143a3 100644 --- a/OpenSim/Tests/Wrapping/testWrappingAlgorithm.cpp +++ b/OpenSim/Tests/Wrapping/testWrappingAlgorithm.cpp @@ -51,6 +51,7 @@ int main() std::cout << "Exception: " << e.what() << std::endl; failures.push_back("TestWrapCylinder"); } + try { auto* wo = new WrapSphere(); wo->setName("pulley1"); @@ -76,8 +77,25 @@ int main() std::cout << "Exception: " << e.what() << std::endl; failures.push_back("TestWrapEllipsoid"); } + // Repeat with Rotate the wrap object by angle theta to get an ellipse with radii (.5/cos(theta), .5) // length of wrap + try { + auto* wo = new WrapCylinder(); + wo->setName("pulley1"); + wo->set_radius(.5); + wo->set_length(1); + // -45 - 45 degrees guarantee no edge which is poorly handled and need to be dropped + for (int i = -3; i <= 3; i++) { + double angle = i * SimTK::Pi / 12; + wo->set_xyz_body_rotation(Vec3(0, angle, 0)); + testWrapObject(wo); + } + } + catch (const std::exception& e) { + std::cout << "Exception: " << e.what() << std::endl; + failures.push_back("TestWrapCylinder"); + } if (!failures.empty()) { cout << "Done, with failure(s): " << failures << endl; @@ -122,9 +140,16 @@ void testWrapObject(WrapObject* wrapObject) model.finalizeConnections(); model.setUseVisualizer(visualize); //model.print(wObj->getConcreteClassName()+"Analytical.osim"); + //model.updDisplayHints().disableVisualization(); SimTK::State& s = model.initSystem(); auto& coord = joint->updCoordinate(); const CoordinateSet& cset = model.getCoordinateSet(); + // get angle + double ang = wrapObject->get_xyz_body_rotation()[1]; + double a = 0.5; + double b = .5 / cos(ang); + // perimeter approx fomrula by Ramanujan + double p = SimTK::Pi * (3*(a + b) - sqrt((3 * a + b) * (a + 3 * b))); int nsteps = 1000; for (int i = 0; i <= nsteps; ++i) { @@ -136,17 +161,18 @@ void testWrapObject(WrapObject* wrapObject) double ma1 = spring1->computeMomentArm(s, coord); - ASSERT_EQUAL(-r, ma1, .0001); // SimTK::Eps - + //ASSERT_EQUAL(-r, ma1, .0001); // SimTK::Eps + std::cout << "marm=" << ma1 << std::endl; double len1 = spring1->getLength(s); if (i== 0) { - //std::cout << "Testing " << wObj->getConcreteClassName() << std::endl; - //std::cout << "i=" << i << "ma=" << ma1 << "len=" << len1 << std::endl; - ASSERT_EQUAL(len1, 1 - 0.1, 1e-4); //SimTK::Eps + std::cout << "Testing " << wObj->getConcreteClassName() << std::endl; + std::cout << "i=" << i << "ma=" << ma1 << "len=" << len1 << std::endl; + //ASSERT_EQUAL(len1, 1 - 0.1, 1e-4); //SimTK::Eps } if (i == nsteps) { // sgould be 1/4 way around wrapObject - //std::cout << "i=" << i << "ma=" << ma1 << "len=" << len1 << std::endl; - ASSERT_EQUAL(len1, 1 + .25 * 2 * SimTK::Pi * r -0.1, .0001); //SimTK::Eps + std::cout << "i=" << i << "ma=" << ma1 << "len=" << len1 << std::endl; + std::cout << "p=" << .9 + p / 4; + //ASSERT_EQUAL(len1, 1 + .25 * 2 * SimTK::Pi * r -0.1, .0001); //SimTK::Eps } } } From 01f3723d2807f8eb9ac818431a96e4e8622ff294 Mon Sep 17 00:00:00 2001 From: Ayman Habib Date: Sat, 27 Aug 2022 08:42:53 -0700 Subject: [PATCH 10/28] disable rotation tests in-progress --- .../Tests/Wrapping/testWrappingAlgorithm.cpp | 19 ++++++++++--------- 1 file changed, 10 insertions(+), 9 deletions(-) diff --git a/OpenSim/Tests/Wrapping/testWrappingAlgorithm.cpp b/OpenSim/Tests/Wrapping/testWrappingAlgorithm.cpp index 0c143143a3..da07bbd177 100644 --- a/OpenSim/Tests/Wrapping/testWrappingAlgorithm.cpp +++ b/OpenSim/Tests/Wrapping/testWrappingAlgorithm.cpp @@ -80,6 +80,7 @@ int main() // Repeat with Rotate the wrap object by angle theta to get an ellipse with radii (.5/cos(theta), .5) // length of wrap + /** try { auto* wo = new WrapCylinder(); wo->setName("pulley1"); @@ -95,7 +96,7 @@ int main() catch (const std::exception& e) { std::cout << "Exception: " << e.what() << std::endl; failures.push_back("TestWrapCylinder"); - } + } */ if (!failures.empty()) { cout << "Done, with failure(s): " << failures << endl; @@ -161,18 +162,18 @@ void testWrapObject(WrapObject* wrapObject) double ma1 = spring1->computeMomentArm(s, coord); - //ASSERT_EQUAL(-r, ma1, .0001); // SimTK::Eps - std::cout << "marm=" << ma1 << std::endl; + ASSERT_EQUAL(-r, ma1, .0001); // SimTK::Eps + //std::cout << "marm=" << ma1 << std::endl; double len1 = spring1->getLength(s); if (i== 0) { - std::cout << "Testing " << wObj->getConcreteClassName() << std::endl; - std::cout << "i=" << i << "ma=" << ma1 << "len=" << len1 << std::endl; - //ASSERT_EQUAL(len1, 1 - 0.1, 1e-4); //SimTK::Eps + //std::cout << "Testing " << wObj->getConcreteClassName() << std::endl; + //std::cout << "i=" << i << "ma=" << ma1 << "len=" << len1 << std::endl; + ASSERT_EQUAL(len1, 1 - 0.1, 1e-4); //SimTK::Eps } if (i == nsteps) { // sgould be 1/4 way around wrapObject - std::cout << "i=" << i << "ma=" << ma1 << "len=" << len1 << std::endl; - std::cout << "p=" << .9 + p / 4; - //ASSERT_EQUAL(len1, 1 + .25 * 2 * SimTK::Pi * r -0.1, .0001); //SimTK::Eps + //std::cout << "i=" << i << "ma=" << ma1 << "len=" << len1 << std::endl; + //std::cout << "p=" << .9 + p / 4; + ASSERT_EQUAL(len1, 1 + .25 * 2 * SimTK::Pi * r -0.1, .0001); //SimTK::Eps } } } From f5b9d289f406395e980358e471c04cb777488804 Mon Sep 17 00:00:00 2001 From: Ayman Habib Date: Mon, 29 Aug 2022 15:34:35 -0700 Subject: [PATCH 11/28] Add test comapre cylinder to ellipsoid wrapping --- .../Tests/Wrapping/testWrappingAlgorithm.cpp | 124 +++++++++++++++--- 1 file changed, 105 insertions(+), 19 deletions(-) diff --git a/OpenSim/Tests/Wrapping/testWrappingAlgorithm.cpp b/OpenSim/Tests/Wrapping/testWrappingAlgorithm.cpp index da07bbd177..04e273a898 100644 --- a/OpenSim/Tests/Wrapping/testWrappingAlgorithm.cpp +++ b/OpenSim/Tests/Wrapping/testWrappingAlgorithm.cpp @@ -32,7 +32,8 @@ using namespace OpenSim; using namespace SimTK; using namespace std; -void testWrapObject(OpenSim::WrapObject *wObj); +void testSingleWrapObjectPerpendicular(OpenSim::WrapObject* wObj); +void testCompareWrapObjects(OpenSim::WrapObject* wObj1, OpenSim::WrapObject* wObj2); int main() { @@ -43,9 +44,9 @@ int main() wo->setName("pulley1"); wo->set_radius(.5); wo->set_length(1); - testWrapObject(wo); + testSingleWrapObjectPerpendicular(wo); wo->set_quadrant("+y"); - testWrapObject(wo); + testSingleWrapObjectPerpendicular(wo); } catch (const std::exception& e) { std::cout << "Exception: " << e.what() << std::endl; @@ -56,9 +57,9 @@ int main() auto* wo = new WrapSphere(); wo->setName("pulley1"); wo->set_radius(.5); - testWrapObject(wo); + testSingleWrapObjectPerpendicular(wo); wo->set_quadrant("+y"); - testWrapObject(wo); + testSingleWrapObjectPerpendicular(wo); } catch (const std::exception& e) { std::cout << "Exception: " << e.what() << std::endl; @@ -68,9 +69,9 @@ int main() auto* wo = new WrapEllipsoid(); wo->setName("pulley1"); wo->set_dimensions(Vec3(.5)); - testWrapObject(wo); + testSingleWrapObjectPerpendicular(wo); wo->set_quadrant("+y"); - testWrapObject(wo); + testSingleWrapObjectPerpendicular(wo); } catch (const std::exception& e) { @@ -80,23 +81,26 @@ int main() // Repeat with Rotate the wrap object by angle theta to get an ellipse with radii (.5/cos(theta), .5) // length of wrap - /** + try { - auto* wo = new WrapCylinder(); - wo->setName("pulley1"); - wo->set_radius(.5); - wo->set_length(1); + auto* woOne = new WrapCylinder(); + woOne->setName("pulley1"); + woOne->set_radius(.5); + woOne->set_length(2); + auto* woTwo = new WrapEllipsoid(); + woTwo->setName("pulley2"); // -45 - 45 degrees guarantee no edge which is poorly handled and need to be dropped - for (int i = -3; i <= 3; i++) { + for (int i = -2; i <= 2; i++) { double angle = i * SimTK::Pi / 12; - wo->set_xyz_body_rotation(Vec3(0, angle, 0)); - testWrapObject(wo); + woOne->set_xyz_body_rotation(Vec3(0, angle, 0)); + woTwo->set_dimensions(Vec3(.5/cos(angle), .5, .5)); + testCompareWrapObjects(woOne, woTwo); } } catch (const std::exception& e) { std::cout << "Exception: " << e.what() << std::endl; - failures.push_back("TestWrapCylinder"); - } */ + failures.push_back("Test Compare failed."); + } if (!failures.empty()) { cout << "Done, with failure(s): " << failures << endl; @@ -106,8 +110,9 @@ int main() cout << "Done" << endl; return 0; } - -void testWrapObject(WrapObject* wrapObject) +// Test results of wrapping a sigle path perpendicular to a wrapObject +// and compare results to analytical/expected answers +void testSingleWrapObjectPerpendicular(WrapObject* wrapObject) { auto visualize = false; const double r = 0.5; @@ -177,4 +182,85 @@ void testWrapObject(WrapObject* wrapObject) } } } +// Test results of wrapping a sigle path around a wrapObject wObj1 +// and compare results to analytically equivalent wrapObject wObj2 +// For example a rotated cylinder against an ellipsoid +void testCompareWrapObjects(OpenSim::WrapObject* wObj1, OpenSim::WrapObject* wObj2) { + auto visualize = false; + const double r = 0.5; + Model model; + model.setName("test" + wObj1->getConcreteClassName()+wObj2->getConcreteClassName()); + + auto& ground = model.updGround(); + auto body = new OpenSim::Body("body", 1, Vec3(-.5, 0, 0), Inertia(0.1, 0.1, 0.01)); + model.addComponent(body); + + auto joint = new PinJoint("pin", ground, *body); + auto& qi = joint->updCoordinate(); + qi.setName("q_pin"); + model.addComponent(joint); + + // Add the wrap object to the body, which takes ownership of it + WrapObject* wObj = wObj1->clone(); + ground.addWrapObject(wObj); + + // One spring has wrap cylinder with respect to ground origin + PathSpring* spring1 = + new PathSpring("spring1", 1.0, 0.1, 0.01); + spring1->updGeometryPath(). + appendNewPathPoint("origin", ground, Vec3(r - .1, r, 0)); //offset in X direction to avoid ambiguous scenario where path passes through center + spring1->updGeometryPath(). + appendNewPathPoint("insert", *body, Vec3(-r, r, 0)); + spring1->updGeometryPath().addPathWrap(*wObj); + + model.addComponent(spring1); + // Ceate offset frame in z direction + PhysicalOffsetFrame* offsetZ = new PhysicalOffsetFrame( + "z_plus1", ground, SimTK::Transform(Vec3(0, 0, 1))); + model.addComponent(offsetZ); + // repeat for wObj2 offset in z direction + // Add the wrap object to the body, which takes ownership of it + WrapObject* wObj_2 = wObj2->clone(); + offsetZ->addWrapObject(wObj_2); + // One spring has wrap cylinder with respect to ground origin + PathSpring* spring2 = + new PathSpring("spring2", 1.0, 0.1, 0.01); + spring2->updGeometryPath(). + appendNewPathPoint("origin", ground, Vec3(r - .1, r, 1)); //offset in X direction to avoid ambiguous scenario where path passes through center + spring2->updGeometryPath(). + appendNewPathPoint("insert", *body, Vec3(-r, r, 1)); + spring2->updGeometryPath().addPathWrap(*wObj_2); + + model.addComponent(spring2); + + model.finalizeConnections(); + model.setUseVisualizer(visualize); + //model.print("wrapAnalytical.osim"); + //model.updDisplayHints().disableVisualization(); + SimTK::State& s = model.initSystem(); + auto& coord = joint->updCoordinate(); + const CoordinateSet& cset = model.getCoordinateSet(); + // get angle + + double ang = wObj->get_xyz_body_rotation()[1]; + double a = 0.5; + double b = .5 / cos(ang); + // perimeter approx fomrula by Ramanujan + //double p = SimTK::Pi * (3 * (a + b) - sqrt((3 * a + b) * (a + 3 * b))); + + int nsteps = 1000; + for (int i = 0; i <= nsteps; ++i) { + + coord.setValue(s, i * SimTK::Pi / (4 * nsteps)); + model.realizeVelocity(s); + + if (visualize) + model.getVisualizer().show(s); + + double len1 = spring1->getLength(s); + double len2 = spring2->getLength(s); + //std::cout << "i=" << i << " length=" << len1 << " " << len2 << " diff=" << std::abs(len1-len2) << std::endl; + ASSERT_EQUAL(len1, len2, .05); + } +} From 097c925ee090198fa30020d18e036e2f3dbc622c Mon Sep 17 00:00:00 2001 From: Ayman Habib Date: Mon, 29 Aug 2022 18:37:27 -0700 Subject: [PATCH 12/28] Update continuous_integration.yml Undo cache dependencies on ubuntu since compiler libgc has moved/upgraded --- .github/workflows/continuous_integration.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/continuous_integration.yml b/.github/workflows/continuous_integration.yml index 49d26205d8..7fc42adb7b 100644 --- a/.github/workflows/continuous_integration.yml +++ b/.github/workflows/continuous_integration.yml @@ -427,7 +427,7 @@ jobs: key: ${{ runner.os }}-dependencies-${{ hashFiles('dependencies/*') }} - name: Build dependencies - if: steps.cache-dependencies.outputs.cache-hit != 'true' + # if: steps.cache-dependencies.outputs.cache-hit != 'true' run: | mkdir $GITHUB_WORKSPACE/../build_deps cd $GITHUB_WORKSPACE/../build_deps From 07d82405968e50338f25a96b158f608e9cf4ae17 Mon Sep 17 00:00:00 2001 From: Ayman Habib Date: Mon, 29 Aug 2022 22:15:04 -0700 Subject: [PATCH 13/28] Update testWrappingAlgorithm.cpp Fix comment --- OpenSim/Tests/Wrapping/testWrappingAlgorithm.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/OpenSim/Tests/Wrapping/testWrappingAlgorithm.cpp b/OpenSim/Tests/Wrapping/testWrappingAlgorithm.cpp index 04e273a898..9daf9ec057 100644 --- a/OpenSim/Tests/Wrapping/testWrappingAlgorithm.cpp +++ b/OpenSim/Tests/Wrapping/testWrappingAlgorithm.cpp @@ -79,8 +79,7 @@ int main() failures.push_back("TestWrapEllipsoid"); } - // Repeat with Rotate the wrap object by angle theta to get an ellipse with radii (.5/cos(theta), .5) - // length of wrap + // Compare rotated wrap cylinder by angle theta with an ellipsoid with radii matching cylinder try { auto* woOne = new WrapCylinder(); From 4df447a6912990a77552986c7f2a28b8aeee1047 Mon Sep 17 00:00:00 2001 From: Ayman Habib Date: Tue, 30 Aug 2022 13:54:31 -0700 Subject: [PATCH 14/28] Use angles of -30 to 30 degrees for comparing cylinder vs. ellipsoid as larger angles snap incorrectly --- OpenSim/Tests/Wrapping/testWrappingAlgorithm.cpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/OpenSim/Tests/Wrapping/testWrappingAlgorithm.cpp b/OpenSim/Tests/Wrapping/testWrappingAlgorithm.cpp index 04e273a898..961a1726c5 100644 --- a/OpenSim/Tests/Wrapping/testWrappingAlgorithm.cpp +++ b/OpenSim/Tests/Wrapping/testWrappingAlgorithm.cpp @@ -89,11 +89,11 @@ int main() woOne->set_length(2); auto* woTwo = new WrapEllipsoid(); woTwo->setName("pulley2"); - // -45 - 45 degrees guarantee no edge which is poorly handled and need to be dropped + // -30 - 30 degrees guarantee no edge which is poorly handled and need to be dropped for (int i = -2; i <= 2; i++) { double angle = i * SimTK::Pi / 12; woOne->set_xyz_body_rotation(Vec3(0, angle, 0)); - woTwo->set_dimensions(Vec3(.5/cos(angle), .5, .5)); + woTwo->set_dimensions(Vec3(.5/cos(angle), .5, 1)); testCompareWrapObjects(woOne, woTwo); } } @@ -249,6 +249,7 @@ void testCompareWrapObjects(OpenSim::WrapObject* wObj1, OpenSim::WrapObject* wOb //double p = SimTK::Pi * (3 * (a + b) - sqrt((3 * a + b) * (a + 3 * b))); int nsteps = 1000; + double max_diff = 0.; for (int i = 0; i <= nsteps; ++i) { coord.setValue(s, i * SimTK::Pi / (4 * nsteps)); @@ -259,8 +260,10 @@ void testCompareWrapObjects(OpenSim::WrapObject* wObj1, OpenSim::WrapObject* wOb double len1 = spring1->getLength(s); double len2 = spring2->getLength(s); + max_diff = std::max(max_diff, std::abs(len1 - len2)); //std::cout << "i=" << i << " length=" << len1 << " " << len2 << " diff=" << std::abs(len1-len2) << std::endl; - ASSERT_EQUAL(len1, len2, .05); + ASSERT_EQUAL(len1, len2, .01); } + //std::cout << "max_diff:" << max_diff << std::endl; } From 710f6aad0efef3acb4599cf5577fedb092ed2daa Mon Sep 17 00:00:00 2001 From: aymanhab Date: Thu, 8 Sep 2022 14:01:22 -0700 Subject: [PATCH 15/28] Compare length with analytical answer in every iteration --- OpenSim/Tests/Wrapping/testWrappingAlgorithm.cpp | 12 +++--------- 1 file changed, 3 insertions(+), 9 deletions(-) diff --git a/OpenSim/Tests/Wrapping/testWrappingAlgorithm.cpp b/OpenSim/Tests/Wrapping/testWrappingAlgorithm.cpp index b72ccc05e4..d0443300e8 100644 --- a/OpenSim/Tests/Wrapping/testWrappingAlgorithm.cpp +++ b/OpenSim/Tests/Wrapping/testWrappingAlgorithm.cpp @@ -139,15 +139,9 @@ void testWrapObject(WrapObject* wrapObject) ASSERT_EQUAL(-r, ma1, .0001); // SimTK::Eps double len1 = spring1->getLength(s); - if (i== 0) { - //std::cout << "Testing " << wObj->getConcreteClassName() << std::endl; - //std::cout << "i=" << i << "ma=" << ma1 << "len=" << len1 << std::endl; - ASSERT_EQUAL(len1, 1 - 0.1, 1e-4); //SimTK::Eps - } - if (i == nsteps) { // sgould be 1/4 way around wrapObject - //std::cout << "i=" << i << "ma=" << ma1 << "len=" << len1 << std::endl; - ASSERT_EQUAL(len1, 1 + .25 * 2 * SimTK::Pi * r -0.1, .0001); //SimTK::Eps - } + // Length is 0.9 by construction plus a portion of a quarter circle with radius r proportional to i + ASSERT_EQUAL(len1, .9 + 0.25 * 2 * SimTK::Pi * r * i / nsteps, 1e-6); //SimTK::Eps + } } From 1ffdda4a3061c2f16b0d38e6917a0a6d6b3c6f50 Mon Sep 17 00:00:00 2001 From: aymanhab Date: Thu, 8 Sep 2022 15:34:29 -0700 Subject: [PATCH 16/28] Address feedback on PR, clarify comments and some cleanup --- .../Tests/Wrapping/testWrappingAlgorithm.cpp | 26 +++++++------------ 1 file changed, 9 insertions(+), 17 deletions(-) diff --git a/OpenSim/Tests/Wrapping/testWrappingAlgorithm.cpp b/OpenSim/Tests/Wrapping/testWrappingAlgorithm.cpp index c0bbf546d8..3e1dd80718 100644 --- a/OpenSim/Tests/Wrapping/testWrappingAlgorithm.cpp +++ b/OpenSim/Tests/Wrapping/testWrappingAlgorithm.cpp @@ -33,7 +33,7 @@ using namespace SimTK; using namespace std; void testSingleWrapObjectPerpendicular(OpenSim::WrapObject* wObj); -void testCompareWrapObjects(OpenSim::WrapObject* wObj1, OpenSim::WrapObject* wObj2); +void testCompareWrapObjects(OpenSim::WrapCylinder* wObj1, OpenSim::WrapObject* wObj2); int main() { @@ -88,7 +88,8 @@ int main() woOne->set_length(2); auto* woTwo = new WrapEllipsoid(); woTwo->setName("pulley2"); - // -30 - 30 degrees guarantee no edge which is poorly handled and need to be dropped + // -30 - 30 degrees guarantees that wrapping doesn't occur at the cap of the cylinder which is poorly + // handled and may need to be dropped as non-physical scenario for (int i = -2; i <= 2; i++) { double angle = i * SimTK::Pi / 12; woOne->set_xyz_body_rotation(Vec3(0, angle, 0)); @@ -109,7 +110,8 @@ int main() cout << "Done" << endl; return 0; } -// Test results of wrapping a sigle path perpendicular to a wrapObject +// Test results of wrapping a sigle path perpendicular to a wrapObject +// particularly perpendicular to cylinder axis // and compare results to analytical/expected answers void testSingleWrapObjectPerpendicular(WrapObject* wrapObject) { @@ -170,14 +172,14 @@ void testSingleWrapObjectPerpendicular(WrapObject* wrapObject) // Test results of wrapping a sigle path around a wrapObject wObj1 // and compare results to analytically equivalent wrapObject wObj2 // For example a rotated cylinder against an ellipsoid -void testCompareWrapObjects(OpenSim::WrapObject* wObj1, OpenSim::WrapObject* wObj2) { +void testCompareWrapObjects(OpenSim::WrapCylinder* wObj1, OpenSim::WrapObject* wObj2) { auto visualize = false; - const double r = 0.5; + const double r = wObj1->get_radius(); Model model; model.setName("test" + wObj1->getConcreteClassName()+wObj2->getConcreteClassName()); auto& ground = model.updGround(); - auto body = new OpenSim::Body("body", 1, Vec3(-.5, 0, 0), Inertia(0.1, 0.1, 0.01)); + auto body = new OpenSim::Body("body", 1, Vec3(-r, 0, 0), Inertia(0.1, 0.1, 0.01)); model.addComponent(body); auto joint = new PinJoint("pin", ground, *body); @@ -225,14 +227,7 @@ void testCompareWrapObjects(OpenSim::WrapObject* wObj1, OpenSim::WrapObject* wOb SimTK::State& s = model.initSystem(); auto& coord = joint->updCoordinate(); const CoordinateSet& cset = model.getCoordinateSet(); - // get angle - - double ang = wObj->get_xyz_body_rotation()[1]; - double a = 0.5; - double b = .5 / cos(ang); - // perimeter approx fomrula by Ramanujan - //double p = SimTK::Pi * (3 * (a + b) - sqrt((3 * a + b) * (a + 3 * b))); - + int nsteps = 1000; double max_diff = 0.; for (int i = 0; i <= nsteps; ++i) { @@ -245,10 +240,7 @@ void testCompareWrapObjects(OpenSim::WrapObject* wObj1, OpenSim::WrapObject* wOb double len1 = spring1->getLength(s); double len2 = spring2->getLength(s); - max_diff = std::max(max_diff, std::abs(len1 - len2)); - //std::cout << "i=" << i << " length=" << len1 << " " << len2 << " diff=" << std::abs(len1-len2) << std::endl; ASSERT_EQUAL(len1, len2, .01); } - //std::cout << "max_diff:" << max_diff << std::endl; } From c875103c7dadf269558c36c96ef613adc21571bb Mon Sep 17 00:00:00 2001 From: aymanhab Date: Tue, 4 Oct 2022 14:38:09 -0700 Subject: [PATCH 17/28] Clarify comments and address feedback on test case --- .../Tests/Wrapping/testWrappingAlgorithm.cpp | 30 +++++++++++-------- 1 file changed, 18 insertions(+), 12 deletions(-) diff --git a/OpenSim/Tests/Wrapping/testWrappingAlgorithm.cpp b/OpenSim/Tests/Wrapping/testWrappingAlgorithm.cpp index 3e1dd80718..0d37a0de13 100644 --- a/OpenSim/Tests/Wrapping/testWrappingAlgorithm.cpp +++ b/OpenSim/Tests/Wrapping/testWrappingAlgorithm.cpp @@ -88,12 +88,18 @@ int main() woOne->set_length(2); auto* woTwo = new WrapEllipsoid(); woTwo->setName("pulley2"); - // -30 - 30 degrees guarantees that wrapping doesn't occur at the cap of the cylinder which is poorly - // handled and may need to be dropped as non-physical scenario - for (int i = -2; i <= 2; i++) { - double angle = i * SimTK::Pi / 12; - woOne->set_xyz_body_rotation(Vec3(0, angle, 0)); - woTwo->set_dimensions(Vec3(.5/cos(angle), .5, 1)); + // Change the angle between the cylinder axis and the line connecting end points of pulley + // -36 to 36 degrees guarantees that wrapping doesn't occur at the cap of the cylinder which is a rather poorly + // handled scenario and may need to be dropped as non-biological and also results in truncated conic-section + // Wider range should work also but ellipsoid wrapping bugs out and produces a kink + // -Ayman 10/22 + auto startAngle = -SimTK::Pi / 5; + auto endAngle = SimTK::Pi / 5; + for (double angle = startAngle; angle <= endAngle; angle += SimTK::Pi/180) { + woOne->set_xyz_body_rotation(Vec3(0, angle, 0)); // Rotate the cylinder by angle + woTwo->set_dimensions(Vec3(.5/cos(angle), .5, 1)); // Change radii of ellipsoid to match + std::cout << "compare cylinder vs ellipsoid at angle " << angle * 180/SimTK::Pi + << std::endl; testCompareWrapObjects(woOne, woTwo); } } @@ -111,8 +117,9 @@ int main() return 0; } // Test results of wrapping a sigle path perpendicular to a wrapObject -// particularly perpendicular to cylinder axis -// and compare results to analytical/expected answers +// particularly path perpendicular to cylinder axis +// and compare results to analytical/expected answers. Since cross-section is a circle/arc +// results should match a sphere or ellipsoid with matching radii void testSingleWrapObjectPerpendicular(WrapObject* wrapObject) { auto visualize = false; @@ -121,7 +128,7 @@ void testSingleWrapObjectPerpendicular(WrapObject* wrapObject) model.setName("test"+wrapObject->getConcreteClassName()); auto& ground = model.updGround(); - auto body = new OpenSim::Body("body", 1, Vec3(-.5, 0, 0), Inertia(0.1, 0.1, 0.01)); + auto body = new OpenSim::Body("body", 1, Vec3(-r, 0, 0), Inertia(0.1, 0.1, 0.01)); model.addComponent(body); auto joint = new PinJoint("pin", ground, *body); @@ -169,9 +176,9 @@ void testSingleWrapObjectPerpendicular(WrapObject* wrapObject) } } -// Test results of wrapping a sigle path around a wrapObject wObj1 +// Test results of wrapping a sigle path around a wrapCylinder wObj1 // and compare results to analytically equivalent wrapObject wObj2 -// For example a rotated cylinder against an ellipsoid +// For example wrapping around a rotated cylinder against an ellipsoid with radii to match void testCompareWrapObjects(OpenSim::WrapCylinder* wObj1, OpenSim::WrapObject* wObj2) { auto visualize = false; const double r = wObj1->get_radius(); @@ -229,7 +236,6 @@ void testCompareWrapObjects(OpenSim::WrapCylinder* wObj1, OpenSim::WrapObject* w const CoordinateSet& cset = model.getCoordinateSet(); int nsteps = 1000; - double max_diff = 0.; for (int i = 0; i <= nsteps; ++i) { coord.setValue(s, i * SimTK::Pi / (4 * nsteps)); From 416b9a47057dd47b9d8233ef8105ae63b2e6020b Mon Sep 17 00:00:00 2001 From: aymanhab Date: Tue, 4 Oct 2022 14:41:10 -0700 Subject: [PATCH 18/28] cache property radius into a member variable _radius --- OpenSim/Simulation/Wrap/WrapCylinder.cpp | 6 +++--- OpenSim/Simulation/Wrap/WrapCylinder.h | 4 ++-- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/OpenSim/Simulation/Wrap/WrapCylinder.cpp b/OpenSim/Simulation/Wrap/WrapCylinder.cpp index de15e33435..ceac8970f8 100644 --- a/OpenSim/Simulation/Wrap/WrapCylinder.cpp +++ b/OpenSim/Simulation/Wrap/WrapCylinder.cpp @@ -131,6 +131,8 @@ void WrapCylinder::extendFinalizeFromProperties() InvalidPropertyValue, getProperty_length().getName(), "Length cannot be less than zero"); + // Cache in _radius for reuse in time sensitive code downstream + _radius = get_radius(); } //_____________________________________________________________________________ @@ -177,7 +179,6 @@ 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 { - 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; double dist1, dist2; @@ -614,7 +615,6 @@ void WrapCylinder::_make_spiral_path(SimTK::Vec3& aPoint1, 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(); restart_spiral_wrap: @@ -741,7 +741,7 @@ void WrapCylinder::_calc_spiral_wrap_point(const SimTK::Vec3& r1a, for (int i = 0; i < 3; i++) { - double radial_component = get_radius() * n[1][i]; + double radial_component = _radius * n[1][i]; double axial_component = t * axial_vec[i]; wrap_pt[i] = r1a[i] + radial_component + axial_component; diff --git a/OpenSim/Simulation/Wrap/WrapCylinder.h b/OpenSim/Simulation/Wrap/WrapCylinder.h index 883e722b35..c84f45e197 100644 --- a/OpenSim/Simulation/Wrap/WrapCylinder.h +++ b/OpenSim/Simulation/Wrap/WrapCylinder.h @@ -94,8 +94,8 @@ OpenSim_DECLARE_CONCRETE_OBJECT(WrapCylinder, WrapObject); SimTK::Vec3& dn, SimTK::Vec3& r1, SimTK::Vec3& w1) const; - -//============================================================================= + mutable double _radius; // Avoid using Properties in time senstitive loops + //============================================================================= }; // END of class WrapCylinder //============================================================================= //============================================================================= From e85d736d1c3ae1b622d6d3e8a46a0acb9d712009 Mon Sep 17 00:00:00 2001 From: aymanhab Date: Thu, 13 Oct 2022 11:57:52 -0700 Subject: [PATCH 19/28] Cache property in local variable for speedup --- OpenSim/Simulation/Model/GeometryPath.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/OpenSim/Simulation/Model/GeometryPath.cpp b/OpenSim/Simulation/Model/GeometryPath.cpp index c9834348e5..31d615aedf 100644 --- a/OpenSim/Simulation/Model/GeometryPath.cpp +++ b/OpenSim/Simulation/Model/GeometryPath.cpp @@ -915,11 +915,13 @@ void GeometryPath::computePath(const SimTK::State& s) const // Clear the current path. _currentPathPtrsCache.setSize(0); + const auto& pointSet = get_PathPointSet(); + _currentPathPtrsCache.ensureCapacity(pointSet.getSize()); // Add the active fixed and moving via points to the path. - for (int i = 0; i < get_PathPointSet().getSize(); i++) { - if (get_PathPointSet()[i].isActive(s)) - _currentPathPtrsCache.append(&get_PathPointSet()[i]); // <--- !!!!BAD + for (int i = 0; i < pointSet.getSize(); i++) { + if (pointSet[i].isActive(s)) + _currentPathPtrsCache.append(&pointSet[i]); // <--- !!!!BAD } // Use the current path so far to check for intersection with wrap objects, @@ -937,7 +939,7 @@ void GeometryPath::computePath(const SimTK::State& s) const // copied/moved-from version of the model, rather than the original one, so the // pointers may be stale. std::vector& lookups = updCacheVariableValue(s, _currentPathCV); - PopulatePathElementLookup(get_PathPointSet(), + PopulatePathElementLookup(pointSet, get_PathWrapSet(), _currentPathPtrsCache, *this, From 9e911204fac95f7449dceeff4a20900160161815 Mon Sep 17 00:00:00 2001 From: aymanhab Date: Thu, 13 Oct 2022 12:00:01 -0700 Subject: [PATCH 20/28] cache properties and call ensureCapacity when size is known to avoid allocation/deallocation/copying overhead --- OpenSim/Simulation/Wrap/WrapEllipsoid.cpp | 18 +++++++++++------- 1 file changed, 11 insertions(+), 7 deletions(-) diff --git a/OpenSim/Simulation/Wrap/WrapEllipsoid.cpp b/OpenSim/Simulation/Wrap/WrapEllipsoid.cpp index 4dcaf4a697..416f241e5c 100644 --- a/OpenSim/Simulation/Wrap/WrapEllipsoid.cpp +++ b/OpenSim/Simulation/Wrap/WrapEllipsoid.cpp @@ -220,14 +220,15 @@ int WrapEllipsoid::wrapLine(const SimTK::State& s, SimTK::Vec3& aPoint1, SimTK:: // the ellipsoid dimensions because they do not change from one call to the // next. You don't want the factor to change because the algorithm uses // some vectors (r1, r2, c1) from the previous call. - aWrapResult.factor = 3.0 / get_dimensions().sum(); + Vec3 dimensions = get_dimensions(); + aWrapResult.factor = 3.0 / dimensions.sum(); for (i = 0; i < 3; i++) { p1[i] = aPoint1[i] * aWrapResult.factor; p2[i] = aPoint2[i] * aWrapResult.factor; m[i] = origin[i] * aWrapResult.factor; - a[i] = get_dimensions()[i] * aWrapResult.factor; + a[i] = dimensions[i] * aWrapResult.factor; } p1e = -1.0; @@ -830,6 +831,7 @@ void WrapEllipsoid::CalcDistanceOnEllipsoid(SimTK::Vec3& r1, SimTK::Vec3& r2, Si // Just use r1 and r2 as the surface points and return the distance // between them as the distance along the ellipsoid. aWrapResult.wrap_pts.setSize(0); + aWrapResult.wrap_pts.ensureCapacity(2); //SimmPoint p1(r1); aWrapResult.wrap_pts.append(r1); //SimmPoint p2(r2); @@ -906,14 +908,15 @@ void WrapEllipsoid::CalcDistanceOnEllipsoid(SimTK::Vec3& r1, SimTK::Vec3& r2, Si } rphi[2][2] = 1; - for (i = 0; i < numInteriorPts; i++) { phi = (i + 1) * dphi; - rphi[0][0] = cos(phi); - rphi[0][1] = -sin(phi); - rphi[1][0] = sin(phi); - rphi[1][1] = cos(phi); + double sinPhi = sin(phi); + double cosPhi = cos(phi); + rphi[0][0] = cosPhi; + rphi[0][1] = -sinPhi; + rphi[1][0] = sinPhi; + rphi[1][1] = cosPhi; for (j = 0; j < 3; j++) { @@ -946,6 +949,7 @@ void WrapEllipsoid::CalcDistanceOnEllipsoid(SimTK::Vec3& r1, SimTK::Vec3& r2, Si } aWrapResult.wrap_pts.setSize(0); + aWrapResult.wrap_pts.ensureCapacity(numInteriorPts); //SimmPoint p1(r1); aWrapResult.wrap_pts.append(r1); From 62d29bfadcc4f7054448aa10d6269fe0dbcef642 Mon Sep 17 00:00:00 2001 From: aymanhab Date: Thu, 13 Oct 2022 12:04:40 -0700 Subject: [PATCH 21/28] Fix index 1->i error, cache properties and ensureCapacity on arrays once size is known to avoid allocation/deallocation/copying overhead. Also move loop invariant calculations outside loops --- OpenSim/Simulation/Wrap/WrapSphere.cpp | 41 ++++++++++++-------------- 1 file changed, 19 insertions(+), 22 deletions(-) diff --git a/OpenSim/Simulation/Wrap/WrapSphere.cpp b/OpenSim/Simulation/Wrap/WrapSphere.cpp index 381dbed40e..a8d69f4677 100644 --- a/OpenSim/Simulation/Wrap/WrapSphere.cpp +++ b/OpenSim/Simulation/Wrap/WrapSphere.cpp @@ -197,19 +197,19 @@ int WrapSphere::wrapLine(const SimTK::State& s, SimTK::Vec3& aPoint1, SimTK::Vec for (i = 0; i < 3; i++) { p1m[i] = aPoint1[i] - origin[i]; - p2m[i] = aPoint2[i] - origin[1]; + p2m[i] = aPoint2[i] - origin[i]; ri[i] = aPoint1[i] - aPoint2[i]; mp[i] = origin[i] - aPoint2[i]; p1p2[i] = aPoint1[i] - aPoint2[i]; } - + const double radius = get_radius(); // check that neither point is inside the radius of the sphere - if (p1m.norm() < get_radius() || p2m.norm() < get_radius()) + if (p1m.norm() < radius || p2m.norm() < radius) return insideRadius; a = (~ri*ri); b = -2.0 * (~mp*ri); - c = (~mp*mp) - get_radius() * get_radius(); + c = (~mp*mp) - radius * radius; disc = b * b - 4.0 * a * c; // check if there is an intersection of p1p2 and the sphere @@ -219,19 +219,12 @@ int WrapSphere::wrapLine(const SimTK::State& s, SimTK::Vec3& aPoint1, SimTK::Vec aWrapResult.wrap_path_length = 0.0; return noWrap; } - - l1 = (-b + sqrt(disc)) / (2.0 * a); - l2 = (-b - sqrt(disc)) / (2.0 * a); + auto sqrtDisc = sqrt(disc); + l1 = (-b + sqrtDisc) / (2.0 * a); + l2 = (-b - sqrtDisc) / (2.0 * a); // check if the intersection is between p1 and p2 if ( ! (0.0 < l1 && l1 < 1.0) || ! (0.0 < l2 && l2 < 1.0)) - { - aFlag = false; - aWrapResult.wrap_path_length = 0.0; - return noWrap; - } - - if (l1 < l2) { aFlag = false; aWrapResult.wrap_path_length = 0.0; @@ -265,20 +258,21 @@ int WrapSphere::wrapLine(const SimTK::State& s, SimTK::Vec3& aPoint1, SimTK::Vec ra[i][1] = y[i]; ra[i][2] = z[i]; } - - a1 = asin(get_radius() / p1m.norm()); + auto p1mNorm = p1m.norm(); + a1 = asin(radius / p1mNorm); + auto cosA1 = cos(a1); rrx.setRotationFromAngleAboutX(a1); aa = ra * ~rrx; for (i = 0; i < 3; i++) - r1a[i] = aPoint1[i] + aa[i][1] * p1m.norm() * cos(a1); + r1a[i] = aPoint1[i] + aa[i][1] * p1mNorm * cosA1; rrx.setRotationFromAngleAboutX(-a1); aa = ra * ~rrx; for (i = 0; i < 3; i++) - r1b[i] = aPoint1[i] + aa[i][1] * p1m.norm() * cos(a1); + r1b[i] = aPoint1[i] + aa[i][1] * p1mNorm * cosA1; // calc tangent point candidates r2a, r2b for (i = 0; i < 3; i++) @@ -293,19 +287,21 @@ int WrapSphere::wrapLine(const SimTK::State& s, SimTK::Vec3& aPoint1, SimTK::Vec ra[i][2] = z[i]; } - a2 = asin(get_radius() / p2m.norm()); + a2 = asin(radius / p2m.norm()); rrx.setRotationFromAngleAboutX(a2); aa = ra * ~rrx; + auto p2mNorm = p2m.norm(); + double cosA2 = cos(a2); for (i = 0; i < 3; i++) - r2a[i] = aPoint2[i] + aa[i][1] * p2m.norm() * cos(a2); + r2a[i] = aPoint2[i] + aa[i][1] * p2mNorm * cosA2; rrx.setRotationFromAngleAboutX(-a2); aa = ra * ~rrx; for (i = 0; i < 3; i++) - r2b[i] = aPoint2[i] + aa[i][1] * p2m.norm() * cos(a2); + r2b[i] = aPoint2[i] + aa[i][1] * p2mNorm * cosA2; // determine wrapping tangent points r1 & r2 r1am = r1a - origin; @@ -367,7 +363,7 @@ int WrapSphere::wrapLine(const SimTK::State& s, SimTK::Vec3& aPoint1, SimTK::Vec { if (DSIGN(aPoint1[_wrapAxis]) == _wrapSign || DSIGN(aPoint2[_wrapAxis]) == _wrapSign) { - double tt, r_squared = get_radius() * get_radius(); + double tt, r_squared = radius * radius; Vec3 mm; // If either muscle point is on the constrained side, then check for intersection // of the muscle line and the cylinder. If there is an intersection, then @@ -490,6 +486,7 @@ int WrapSphere::wrapLine(const SimTK::State& s, SimTK::Vec3& aPoint1, SimTK::Vec if (numWrapSegments < 1) numWrapSegments = 1; + aWrapResult.wrap_pts.ensureCapacity(numWrapSegments); //SimmPoint sp1(aWrapResult.r1); aWrapResult.wrap_pts.append(aWrapResult.r1); From b7792bf6e95172a8fd66ab6fc08e6e794e4c935c Mon Sep 17 00:00:00 2001 From: aymanhab Date: Sun, 16 Oct 2022 14:45:21 -0700 Subject: [PATCH 22/28] Vary xyz rotations to exercise quadrants --- .../Tests/Wrapping/testWrappingAlgorithm.cpp | 32 +++++++++++++++---- 1 file changed, 26 insertions(+), 6 deletions(-) diff --git a/OpenSim/Tests/Wrapping/testWrappingAlgorithm.cpp b/OpenSim/Tests/Wrapping/testWrappingAlgorithm.cpp index 0473476322..5690fa6712 100644 --- a/OpenSim/Tests/Wrapping/testWrappingAlgorithm.cpp +++ b/OpenSim/Tests/Wrapping/testWrappingAlgorithm.cpp @@ -32,19 +32,24 @@ using namespace OpenSim; using namespace SimTK; using namespace std; -void testSingleWrapObjectPerpendicular(OpenSim::WrapObject* wObj); +void testSingleWrapObjectPerpendicular(OpenSim::WrapObject* wObj, Vec3 axialRotation = Vec3(0.0)); void testCompareWrapObjects(OpenSim::WrapCylinder* wObj1, OpenSim::WrapObject* wObj2); int main() { SimTK::Array_ failures; - + try { auto* wo = new WrapCylinder(); wo->setName("pulley1"); wo->set_radius(.5); wo->set_length(1); testSingleWrapObjectPerpendicular(wo); + // Rotating a cylinder around its axis doesn't change wrapping result but + // changes the local coordinate system for computation by changing the quadrant + testSingleWrapObjectPerpendicular(wo, Vec3{ 0, 0, SimTK::Pi / 2 }); + testSingleWrapObjectPerpendicular(wo, Vec3{ 0, 0, SimTK::Pi }); + testSingleWrapObjectPerpendicular(wo, Vec3{ 0, 0, -SimTK::Pi / 2 }); wo->set_quadrant("+y"); testSingleWrapObjectPerpendicular(wo); } @@ -58,6 +63,15 @@ int main() wo->setName("pulley1"); wo->set_radius(.5); testSingleWrapObjectPerpendicular(wo); + testSingleWrapObjectPerpendicular(wo, Vec3{ 0, 0, SimTK::Pi / 2 }); + testSingleWrapObjectPerpendicular(wo, Vec3{ 0, 0, SimTK::Pi }); + testSingleWrapObjectPerpendicular(wo, Vec3{ 0, 0, -SimTK::Pi / 2 }); + testSingleWrapObjectPerpendicular(wo, Vec3{ 0, SimTK::Pi / 2, 0 }); + testSingleWrapObjectPerpendicular(wo, Vec3{ 0, SimTK::Pi, 0 }); + testSingleWrapObjectPerpendicular(wo, Vec3{ 0, -SimTK::Pi / 2, 0 }); + testSingleWrapObjectPerpendicular(wo, Vec3{ SimTK::Pi / 2, 0, 0 }); + testSingleWrapObjectPerpendicular(wo, Vec3{ SimTK::Pi, 0, 0 }); + testSingleWrapObjectPerpendicular(wo, Vec3{ -SimTK::Pi / 2, 0, 0 }); wo->set_quadrant("+y"); testSingleWrapObjectPerpendicular(wo); } @@ -65,11 +79,15 @@ int main() std::cout << "Exception: " << e.what() << std::endl; failures.push_back("TestWrapSphere"); } + try { auto* wo = new WrapEllipsoid(); wo->setName("pulley1"); wo->set_dimensions(Vec3(.5)); testSingleWrapObjectPerpendicular(wo); + testSingleWrapObjectPerpendicular(wo, Vec3{ 0, 0, SimTK::Pi / 2 }); + testSingleWrapObjectPerpendicular(wo, Vec3{ 0, 0, SimTK::Pi }); + testSingleWrapObjectPerpendicular(wo, Vec3{ 0, 0, -SimTK::Pi / 2 }); wo->set_quadrant("+y"); testSingleWrapObjectPerpendicular(wo); @@ -78,8 +96,9 @@ int main() std::cout << "Exception: " << e.what() << std::endl; failures.push_back("TestWrapEllipsoid"); } - - // Compare rotated wrap cylinder by angle theta with an ellipsoid with radii matching cylinder + + // Compare wrap cylinder rotated by angle theta with an ellipsoid that has radii matching + // the cross section of the rotated cylinder try { auto* woOne = new WrapCylinder(); @@ -97,7 +116,7 @@ int main() auto endAngle = SimTK::Pi / 5; for (double angle = startAngle; angle <= endAngle; angle += SimTK::Pi/180) { woOne->set_xyz_body_rotation(Vec3(0, angle, 0)); // Rotate the cylinder by angle - woTwo->set_dimensions(Vec3(.5/cos(angle), .5, 1)); // Change radii of ellipsoid to match + woTwo->set_dimensions(Vec3(.5/cos(angle), .5, 1)); // Change radii of ellipsoid to match cross-section std::cout << "compare cylinder vs ellipsoid at angle " << angle * 180/SimTK::Pi << std::endl; testCompareWrapObjects(woOne, woTwo); @@ -120,7 +139,7 @@ int main() // particularly path perpendicular to cylinder axis // and compare results to analytical/expected answers. Since cross-section is a circle/arc // results should match a sphere or ellipsoid with matching radii -void testSingleWrapObjectPerpendicular(WrapObject* wrapObject) +void testSingleWrapObjectPerpendicular(WrapObject* wrapObject, Vec3 axisRotations) { auto visualize = false; const double r = 0.5; @@ -138,6 +157,7 @@ void testSingleWrapObjectPerpendicular(WrapObject* wrapObject) // Add the wrap object to the body, which takes ownership of it WrapObject* wObj = wrapObject->clone(); + wObj->set_xyz_body_rotation(axisRotations); ground.addWrapObject(wObj); // One spring has wrap cylinder with respect to ground origin From bb34cbd0e79a1748d7145ee5b4b794304d4e3680 Mon Sep 17 00:00:00 2001 From: aymanhab Date: Sun, 16 Oct 2022 14:46:39 -0700 Subject: [PATCH 23/28] Remove MAX, MIN macros and replace with inline std replacement --- OpenSim/Common/MarkerData.cpp | 4 ++-- OpenSim/Common/PiecewiseLinearFunction.cpp | 2 +- OpenSim/Common/SimmMacros.h | 2 -- OpenSim/Common/SimmSpline.cpp | 18 +++++++++--------- 4 files changed, 12 insertions(+), 14 deletions(-) diff --git a/OpenSim/Common/MarkerData.cpp b/OpenSim/Common/MarkerData.cpp index 79708dcb4f..eb8d71a877 100644 --- a/OpenSim/Common/MarkerData.cpp +++ b/OpenSim/Common/MarkerData.cpp @@ -777,8 +777,8 @@ void MarkerData::averageFrames(double aThreshold, double aStartTime, double aEnd maxZ[i] - minZ[i] > aThreshold) { double maxDim = maxX[i] - minX[i]; - maxDim = MAX(maxDim, (maxY[i] - minY[i])); - maxDim = MAX(maxDim, (maxZ[i] - minZ[i])); + maxDim = std::max(maxDim, (maxY[i] - minY[i])); + maxDim = std::max(maxDim, (maxZ[i] - minZ[i])); log_warn("Movement of marker {} in {} is {} (threshold = {})", _markerNames[i], _fileName, maxDim, aThreshold); } diff --git a/OpenSim/Common/PiecewiseLinearFunction.cpp b/OpenSim/Common/PiecewiseLinearFunction.cpp index 1f293bf722..e88f1f72df 100644 --- a/OpenSim/Common/PiecewiseLinearFunction.cpp +++ b/OpenSim/Common/PiecewiseLinearFunction.cpp @@ -421,7 +421,7 @@ void PiecewiseLinearFunction::calcCoefficients() _b.setSize(n); for (int i=0; i=0.0?(1):(-1)) #define SQR(x) ((x) * (x)) diff --git a/OpenSim/Common/SimmSpline.cpp b/OpenSim/Common/SimmSpline.cpp index 1e9660164d..d1fae31bcf 100644 --- a/OpenSim/Common/SimmSpline.cpp +++ b/OpenSim/Common/SimmSpline.cpp @@ -333,7 +333,7 @@ void SimmSpline::calcCoefficients() if (n == 2) { - t = MAX(TINY_NUMBER,_x[1]-_x[0]); + t = std::max(TINY_NUMBER,_x[1]-_x[0]); _b[0] = _b[1] = (_y[1]-_y[0])/t; _c[0] = _c[1] = 0.0; _d[0] = _d[1] = 0.0; @@ -347,11 +347,11 @@ void SimmSpline::calcCoefficients() * b = diagonal, d = offdiagonal, c = right-hand side */ - _d[0] = MAX(TINY_NUMBER,_x[1] - _x[0]); + _d[0] = std::max(TINY_NUMBER,_x[1] - _x[0]); _c[1] = (_y[1]-_y[0])/_d[0]; for (i=1; i Date: Sun, 16 Oct 2022 14:49:00 -0700 Subject: [PATCH 24/28] Fix visualization bug due to radian/degree conversion, cache properties and utilize zero origin to remove unnecessary computations --- OpenSim/Simulation/Wrap/WrapSphere.cpp | 40 ++++++++++++-------------- 1 file changed, 18 insertions(+), 22 deletions(-) diff --git a/OpenSim/Simulation/Wrap/WrapSphere.cpp b/OpenSim/Simulation/Wrap/WrapSphere.cpp index a8d69f4677..e293fe994e 100644 --- a/OpenSim/Simulation/Wrap/WrapSphere.cpp +++ b/OpenSim/Simulation/Wrap/WrapSphere.cpp @@ -165,7 +165,7 @@ int WrapSphere::wrapLine(const SimTK::State& s, SimTK::Vec3& aPoint1, SimTK::Vec const PathWrap& aPathWrap, WrapResult& aWrapResult, bool& aFlag) const { double l1, l2, disc, a, b, c, a1, a2, j1, j2, j3, j4, r1r2, - axis[4], angle, *r11, *r22; + angle, *r11, *r22; Vec3 ri, p2m, p1m, mp, r1n, r2n, p1p2, np2, hp2, r1m, r2m, y, z, n, r1a, r2a, r1b, r2b, r1am, r2am, r1bm, r2bm; @@ -196,10 +196,10 @@ int WrapSphere::wrapLine(const SimTK::State& s, SimTK::Vec3& aPoint1, SimTK::Vec aWrapResult.wrap_pts.setSize(0); for (i = 0; i < 3; i++) { - p1m[i] = aPoint1[i] - origin[i]; - p2m[i] = aPoint2[i] - origin[i]; + p1m[i] = aPoint1[i];// origin is 0 by construction -origin[i]; + p2m[i] = aPoint2[i]; ri[i] = aPoint1[i] - aPoint2[i]; - mp[i] = origin[i] - aPoint2[i]; + mp[i] = - aPoint2[i]; p1p2[i] = aPoint1[i] - aPoint2[i]; } const double radius = get_radius(); @@ -248,7 +248,7 @@ int WrapSphere::wrapLine(const SimTK::State& s, SimTK::Vec3& aPoint1, SimTK::Vec // calc tangent point candidates r1a, r1b WrapMath::NormalizeOrZero(hp2, n); for (i = 0; i < 3; i++) - y[i] = origin[i] - aPoint1[i]; + y[i] = - aPoint1[i]; WrapMath::NormalizeOrZero(y, y); z = n % y; @@ -276,7 +276,7 @@ int WrapSphere::wrapLine(const SimTK::State& s, SimTK::Vec3& aPoint1, SimTK::Vec // calc tangent point candidates r2a, r2b for (i = 0; i < 3; i++) - y[i] = origin[i] - aPoint2[i]; + y[i] = - aPoint2[i]; WrapMath::NormalizeOrZero(y, y); z = n % y; @@ -304,10 +304,10 @@ int WrapSphere::wrapLine(const SimTK::State& s, SimTK::Vec3& aPoint1, SimTK::Vec r2b[i] = aPoint2[i] + aa[i][1] * p2mNorm * cosA2; // determine wrapping tangent points r1 & r2 - r1am = r1a - origin; - r1bm = r1b - origin; - r2am = r2a - origin; - r2bm = r2b - origin; + r1am = r1a; + r1bm = r1b; + r2am = r2a; + r2bm = r2b; WrapMath::NormalizeOrZero(r1am, r1am); WrapMath::NormalizeOrZero(r1bm, r1bm); @@ -400,7 +400,7 @@ int WrapSphere::wrapLine(const SimTK::State& s, SimTK::Vec3& aPoint1, SimTK::Vec // determine best constrained r1 & r2 tangent points: for (i = 0; i < 3; i++) - sum_musc[i] = (origin[i] - aPoint1[i]) + (origin[i] - aPoint2[i]); + sum_musc[i] = (- aPoint1[i]) + (- aPoint2[i]); WrapMath::NormalizeOrZero(sum_musc, sum_musc); @@ -447,7 +447,7 @@ int WrapSphere::wrapLine(const SimTK::State& s, SimTK::Vec3& aPoint1, SimTK::Vec // determine if the resulting tangent points create a far side wrap for (i = 0; i < 3; i++) { sum_musc[i] = (aWrapResult.r1[i] - aPoint1[i]) + (aWrapResult.r2[i] - aPoint2[i]); - sum_r[i] = (aWrapResult.r1[i] - origin[i]) + (aWrapResult.r2[i] - origin[i]); + sum_r[i] = (aWrapResult.r1[i]) + (aWrapResult.r2[i]); } if ((~sum_r*sum_musc) < 0.0) @@ -457,8 +457,8 @@ int WrapSphere::wrapLine(const SimTK::State& s, SimTK::Vec3& aPoint1, SimTK::Vec calc_path: for (i = 0; i < 3; i++) { - r1m[i] = aWrapResult.r1[i] - origin[i]; - r2m[i] = aWrapResult.r2[i] - origin[i]; + r1m[i] = aWrapResult.r1[i]; + r2m[i] = aWrapResult.r2[i]; } WrapMath::NormalizeOrZero(r1m, r1n); @@ -469,15 +469,12 @@ int WrapSphere::wrapLine(const SimTK::State& s, SimTK::Vec3& aPoint1, SimTK::Vec if (far_side_wrap) angle = -(2 * SimTK_PI - angle); - r1r2 = get_radius() * angle; + r1r2 = radius * angle; aWrapResult.wrap_path_length = r1r2; Vec3 axis3 = r1n % r2n; - WrapMath::NormalizeOrZero(axis3, axis3); - - for(int ii=0; ii<3; ii++) axis[ii]=axis3[ii]; - axis[3] = 1.0; + SimTK::UnitVec3 axis(axis3); aWrapResult.wrap_pts.setSize(0); // Each muscle segment on the surface of the sphere should be @@ -496,9 +493,8 @@ int WrapSphere::wrapLine(const SimTK::State& s, SimTK::Vec3& aPoint1, SimTK::Vec SimTK::Rotation R; for (i = 0; i < numWrapSegments - 2; i++) { - double wangle = angle * (i+1) / (numWrapSegments - 1) * SimTK_DEGREE_TO_RADIAN; - - R.setRotationFromAngleAboutNonUnitVector(wangle, Vec3::getAs(axis)); + double wangle = angle * (i+1) / (numWrapSegments - 1); // angle is in radians + R.setRotationFromAngleAboutUnitVector(-wangle, axis); rotvec = ~R * vec; SimTK::Vec3 wp; From bd5beef1c28966dde03eb5e8e5c5260445546f54 Mon Sep 17 00:00:00 2001 From: aymanhab Date: Mon, 17 Oct 2022 11:56:28 -0700 Subject: [PATCH 25/28] Comment debugging line (leaving behind for future change/debugging) --- OpenSim/Tests/Wrapping/testWrappingAlgorithm.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/OpenSim/Tests/Wrapping/testWrappingAlgorithm.cpp b/OpenSim/Tests/Wrapping/testWrappingAlgorithm.cpp index 5690fa6712..b734040667 100644 --- a/OpenSim/Tests/Wrapping/testWrappingAlgorithm.cpp +++ b/OpenSim/Tests/Wrapping/testWrappingAlgorithm.cpp @@ -117,8 +117,7 @@ int main() for (double angle = startAngle; angle <= endAngle; angle += SimTK::Pi/180) { woOne->set_xyz_body_rotation(Vec3(0, angle, 0)); // Rotate the cylinder by angle woTwo->set_dimensions(Vec3(.5/cos(angle), .5, 1)); // Change radii of ellipsoid to match cross-section - std::cout << "compare cylinder vs ellipsoid at angle " << angle * 180/SimTK::Pi - << std::endl; + // std::cout << "compare cylinder vs ellipsoid at angle " << angle * 180/SimTK::Pi << std::endl; testCompareWrapObjects(woOne, woTwo); } } From 58c70b02a0def4283e5d98a81691344737f4828c Mon Sep 17 00:00:00 2001 From: aymanhab Date: Mon, 17 Oct 2022 12:49:01 -0700 Subject: [PATCH 26/28] Clarifying comments and minor rearrangement --- .../Tests/Wrapping/testWrappingAlgorithm.cpp | 37 +++++++++++-------- 1 file changed, 21 insertions(+), 16 deletions(-) diff --git a/OpenSim/Tests/Wrapping/testWrappingAlgorithm.cpp b/OpenSim/Tests/Wrapping/testWrappingAlgorithm.cpp index b734040667..de094b54b5 100644 --- a/OpenSim/Tests/Wrapping/testWrappingAlgorithm.cpp +++ b/OpenSim/Tests/Wrapping/testWrappingAlgorithm.cpp @@ -35,6 +35,7 @@ using namespace std; void testSingleWrapObjectPerpendicular(OpenSim::WrapObject* wObj, Vec3 axialRotation = Vec3(0.0)); void testCompareWrapObjects(OpenSim::WrapCylinder* wObj1, OpenSim::WrapObject* wObj2); +const double radius = 0.5; int main() { SimTK::Array_ failures; @@ -42,7 +43,7 @@ int main() try { auto* wo = new WrapCylinder(); wo->setName("pulley1"); - wo->set_radius(.5); + wo->set_radius(radius); wo->set_length(1); testSingleWrapObjectPerpendicular(wo); // Rotating a cylinder around its axis doesn't change wrapping result but @@ -61,7 +62,7 @@ int main() try { auto* wo = new WrapSphere(); wo->setName("pulley1"); - wo->set_radius(.5); + wo->set_radius(radius); testSingleWrapObjectPerpendicular(wo); testSingleWrapObjectPerpendicular(wo, Vec3{ 0, 0, SimTK::Pi / 2 }); testSingleWrapObjectPerpendicular(wo, Vec3{ 0, 0, SimTK::Pi }); @@ -83,7 +84,7 @@ int main() try { auto* wo = new WrapEllipsoid(); wo->setName("pulley1"); - wo->set_dimensions(Vec3(.5)); + wo->set_dimensions(Vec3(radius)); testSingleWrapObjectPerpendicular(wo); testSingleWrapObjectPerpendicular(wo, Vec3{ 0, 0, SimTK::Pi / 2 }); testSingleWrapObjectPerpendicular(wo, Vec3{ 0, 0, SimTK::Pi }); @@ -103,20 +104,21 @@ int main() try { auto* woOne = new WrapCylinder(); woOne->setName("pulley1"); - woOne->set_radius(.5); + woOne->set_radius(radius); woOne->set_length(2); auto* woTwo = new WrapEllipsoid(); woTwo->setName("pulley2"); - // Change the angle between the cylinder axis and the line connecting end points of pulley - // -36 to 36 degrees guarantees that wrapping doesn't occur at the cap of the cylinder which is a rather poorly - // handled scenario and may need to be dropped as non-biological and also results in truncated conic-section. - // Wider range should work but ellipsoid wrapping bugs out and produces a kink + // Change the angle between the cylinder axis and the line connecting end points of the pulley. + // Values -36 to 36 degrees guarantee that wrapping doesn't occur at the cap of the cylinder which is a rather poorly + // handled scenario that leads to C0 length curve and may need to be dropped as non-biological + // this scenario also results in a truncated conic-section that can't be computed analytically. + // Wider range should work but ellipsoid wrapping bugs out and produces a kink. // -Ayman 10/22 auto startAngle = -SimTK::Pi / 5; auto endAngle = SimTK::Pi / 5; for (double angle = startAngle; angle <= endAngle; angle += SimTK::Pi/180) { woOne->set_xyz_body_rotation(Vec3(0, angle, 0)); // Rotate the cylinder by angle - woTwo->set_dimensions(Vec3(.5/cos(angle), .5, 1)); // Change radii of ellipsoid to match cross-section + woTwo->set_dimensions(Vec3(radius/cos(angle), radius, 1)); // Change radii of ellipsoid to match cross-section // std::cout << "compare cylinder vs ellipsoid at angle " << angle * 180/SimTK::Pi << std::endl; testCompareWrapObjects(woOne, woTwo); } @@ -127,21 +129,23 @@ int main() } if (!failures.empty()) { - cout << "Done, with failure(s): " << failures << endl; + cout << "Done, with failure(s): " << failures << std::endl; return 1; } - cout << "Done" << endl; + cout << "Done" << std::endl; return 0; } // Test results of wrapping a sigle path perpendicular to a wrapObject -// particularly path perpendicular to cylinder axis +// particularly path perpendicular to cylinder axis (Z axis) // and compare results to analytical/expected answers. Since cross-section is a circle/arc -// results should match a sphere or ellipsoid with matching radii +// results should match a sphere or ellipsoid with matching radius. +// In Ground frame the path is in XY plane along x axis tangent the wrapObject then wraps +// with coordinate change. void testSingleWrapObjectPerpendicular(WrapObject* wrapObject, Vec3 axisRotations) { auto visualize = false; - const double r = 0.5; + const double r = radius; Model model; model.setName("test"+wrapObject->getConcreteClassName()); @@ -191,13 +195,14 @@ void testSingleWrapObjectPerpendicular(WrapObject* wrapObject, Vec3 axisRotation ASSERT_EQUAL(-r, ma1, .0001); // SimTK::Eps double len1 = spring1->getLength(s); // Length is 0.9 by construction plus a portion of a quarter circle with radius r proportional to i - ASSERT_EQUAL(len1, .9 + 0.25 * 2 * SimTK::Pi * r * i / nsteps, 1e-6); //SimTK::Eps + ASSERT_EQUAL(len1, .9 + 0.25 * 2 * SimTK::Pi * r * i / nsteps, 1e-6); //this formula is based on radius = .5 } } // Test results of wrapping a sigle path around a wrapCylinder wObj1 // and compare results to analytically equivalent wrapObject wObj2 -// For example wrapping around a rotated cylinder against an ellipsoid with radii to match +// For example wrapping around a rotated cylinder against an ellipsoid with radii +// picked to match the radii of the elliptical cross-section void testCompareWrapObjects(OpenSim::WrapCylinder* wObj1, OpenSim::WrapObject* wObj2) { auto visualize = false; const double r = wObj1->get_radius(); From dd866ee3e4346e8a1b7a70853c2527b936cb79e6 Mon Sep 17 00:00:00 2001 From: aymanhab Date: Thu, 20 Oct 2022 11:08:13 -0700 Subject: [PATCH 27/28] Fix compilation errors on *nixes due to initialization across gotos --- OpenSim/Simulation/Wrap/WrapSphere.cpp | 17 +++++++++-------- 1 file changed, 9 insertions(+), 8 deletions(-) diff --git a/OpenSim/Simulation/Wrap/WrapSphere.cpp b/OpenSim/Simulation/Wrap/WrapSphere.cpp index e293fe994e..29f4a26947 100644 --- a/OpenSim/Simulation/Wrap/WrapSphere.cpp +++ b/OpenSim/Simulation/Wrap/WrapSphere.cpp @@ -216,7 +216,7 @@ int WrapSphere::wrapLine(const SimTK::State& s, SimTK::Vec3& aPoint1, SimTK::Vec if (disc < 0.0) { aFlag = false; - aWrapResult.wrap_path_length = 0.0; + aWrapResult.wrap_path_length = 0.0; return noWrap; } auto sqrtDisc = sqrt(disc); @@ -236,6 +236,14 @@ int WrapSphere::wrapLine(const SimTK::State& s, SimTK::Vec3& aPoint1, SimTK::Vec hp2 = p1p2 % np2; + auto p1mNorm = p1m.norm(); + a1 = asin(radius / p1mNorm); + auto cosA1 = cos(a1); + + a2 = asin(radius / p2m.norm()); + auto p2mNorm = p2m.norm(); + double cosA2 = cos(a2); + // if the muscle line passes too close to the center of the sphere // then give up if (hp2.norm() < 0.00001) { @@ -258,9 +266,6 @@ int WrapSphere::wrapLine(const SimTK::State& s, SimTK::Vec3& aPoint1, SimTK::Vec ra[i][1] = y[i]; ra[i][2] = z[i]; } - auto p1mNorm = p1m.norm(); - a1 = asin(radius / p1mNorm); - auto cosA1 = cos(a1); rrx.setRotationFromAngleAboutX(a1); aa = ra * ~rrx; @@ -287,13 +292,9 @@ int WrapSphere::wrapLine(const SimTK::State& s, SimTK::Vec3& aPoint1, SimTK::Vec ra[i][2] = z[i]; } - a2 = asin(radius / p2m.norm()); - rrx.setRotationFromAngleAboutX(a2); aa = ra * ~rrx; - auto p2mNorm = p2m.norm(); - double cosA2 = cos(a2); for (i = 0; i < 3; i++) r2a[i] = aPoint2[i] + aa[i][1] * p2mNorm * cosA2; From af75908fd017a8f4afae8bb59e5a95b2531a0527 Mon Sep 17 00:00:00 2001 From: aymanhab Date: Thu, 20 Oct 2022 11:09:54 -0700 Subject: [PATCH 28/28] Performance, leverage the fact that wrapping is computed in cylinder's own frame with z axis, eliminate the unnecessarily expensive CalcWrapPoint method --- OpenSim/Simulation/Wrap/WrapCylinder.cpp | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/OpenSim/Simulation/Wrap/WrapCylinder.cpp b/OpenSim/Simulation/Wrap/WrapCylinder.cpp index 463287586c..2360f17940 100644 --- a/OpenSim/Simulation/Wrap/WrapCylinder.cpp +++ b/OpenSim/Simulation/Wrap/WrapCylinder.cpp @@ -683,13 +683,22 @@ void WrapCylinder::_make_spiral_path(const SimTK::Vec3& p1, out.wrap_pts.setSize(0); out.wrap_pts.ensureCapacity(numWrapSegments); + auto startAngle = acos(r1AxisToSurfaceDir[0]); + auto endAngle = acos(r2AxisToSurfaceDir[0]); + auto angleSign = DSIGN(endAngle - startAngle); 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); - + //Vec3 wrap_pt = CalcWrapPoint(t); + /* TODO: Ayman 10/22 + * We should replace the CalcWrapPoint method with a simplified version considering + * that rotation axis is always either 0,0,1 or 0,0,-1 by construction + * the line below does the trick but fails in some quadrants */ + Vec3 wrap_pt(radius * cos(startAngle + t * (endAngle - startAngle)), + angleSign* radius * sin(startAngle + t * (endAngle - startAngle)), + r1AxialPos[2] + t * r1AxisToR2Axis[2]); // adjust r1/r2 tangent points if necessary to achieve tangency with // the spiral path: if (i == 1