Avoid tweaking tangent points of WrapCylinder when single object wrap…

Applications/Forward/test/testForward.cpp

@@ -160,7 +160,7 @@ void testArm26() {
     for (int j = 0; j < state->getSize(); ++j) {
         stringstream message;
         message << "t=" << time <<" state# "<< j << " " << standard->getColumnLabels()[j+1] << " std=" << data[j] <<"  computed=" << state->getData()[j] << endl;
-        ASSERT_EQUAL(data[j], state->getData()[j], 1.0e-3, 
+        ASSERT_EQUAL(data[j], state->getData()[j], 5.0e-3, 
             __FILE__, __LINE__, "ASSERT_EQUAL FAILED " + message.str());
         cout << "ASSERT_EQUAL PASSED " << message.str();
     }
@@ -173,7 +173,7 @@ void testArm26() {
     for (int j = 0; j < state->getSize(); ++j) {
         stringstream message;
         message << "t=" << time <<" state# "<< j << " " << standard->getColumnLabels()[j+1] << " std=" << data[j] <<"  computed=" << state->getData()[j] << endl;
-        ASSERT_EQUAL(data[j], state->getData()[j], 1.0e-3, 
+        ASSERT_EQUAL(data[j], state->getData()[j], 5.0e-3, 
             __FILE__, __LINE__, "ASSERT_EQUAL FAILED " + message.str());
         cout << "ASSERT_EQUAL PASSED " << message.str();
     }

OpenSim/Common/ModelDisplayHints.h

@@ -105,8 +105,12 @@ class OSIMCOMMON_API ModelDisplayHints : public Object {
     /** Default construction creates a valid display hints object with all
     hints set to their default values. **/
     ModelDisplayHints() { constructProperties(); }
-
-
+    /** Turn off visualization completely, only use API/modeling.
+    Meshes will not be loaded, pathwrappig intermediate points not computed.
+    Intentionally there's no reverse API to turn on visualization downstream.
+    **/
+    void disableVisualization() { _visualization = false; }
+    bool isVisualizationEnabled() const { return _visualization; }
 private:
     void constructProperties() {
         constructProperty_show_wrap_geometry(true);
@@ -120,6 +124,7 @@ class OSIMCOMMON_API ModelDisplayHints : public Object {
         constructProperty_show_forces(true);
         constructProperty_show_debug_geometry(false);
     }
+    bool _visualization{true};
 };
 
 

OpenSim/Moco/MocoProblemRep.cpp

@@ -182,6 +182,7 @@ void MocoProblemRep::initialize() {
 
     // Grab a writable state from the copied model -- we'll use this to disable
     // its constraints below.
+    m_model_disabled_constraints.updDisplayHints().disableVisualization();
     m_state_disabled_constraints[0] = m_model_disabled_constraints.initSystem();
     m_state_disabled_constraints[1] = m_state_disabled_constraints[0];
 

OpenSim/Simulation/Model/GeometryPath.cpp

@@ -854,6 +854,11 @@ void GeometryPath::computeLengtheningSpeed(const SimTK::State& s) const
     }
 
     setLengtheningSpeed(s, speed);
+    log_trace("length {}, speed {}",
+            calcLengthAfterPathComputation(s, currentPath), speed);
+    log_trace("Path:");
+    for (int i = 0; i < currentPath.getSize(); i++)
+        log_trace("{}", currentPath[i]->getLocation(s));
 }
 
 //_____________________________________________________________________________
@@ -974,7 +979,7 @@ applyWrapObjects(const SimTK::State& s, Array<AbstractPathPoint*>& path) const
                         WrapResult wr;
                         wr.startPoint = pt1;
                         wr.endPoint   = pt2;
-
+                        wr.singleWrap = (wrapSetSize==1);
                         result[i] = wo->wrapPathSegment(s, *path.get(pt1), 
                                                         *path.get(pt2), ws, wr);
                         if (result[i] == WrapObject::mandatoryWrap) {

OpenSim/Simulation/Model/Model.cpp

@@ -1167,6 +1167,7 @@ void Model::generateDecorations
         const SimTK::State&                         state,
         SimTK::Array_<SimTK::DecorativeGeometry>&   appendToThis) const
 {
+    if (!hints.isVisualizationEnabled()) return;
     ComponentList<const Component> allComps = getComponentList();
     ComponentList<Component>::const_iterator iter = allComps.begin();
     while (iter != allComps.end()){

OpenSim/Simulation/Wrap/WrapCylinder.cpp

@@ -24,10 +24,12 @@
 //=============================================================================
 // INCLUDES
 //=============================================================================
+#include <SimTKcommon.h>
 #include "WrapCylinder.h"
 #include "PathWrap.h"
 #include "WrapMath.h"
 #include "WrapResult.h"
+#include <OpenSim/Simulation/Model/Model.h>
 #include <OpenSim/Common/ModelDisplayHints.h>
 #include <OpenSim/Common/Mtx.h>
 #include <OpenSim/Common/SimmMacros.h>
@@ -40,6 +42,7 @@
 using namespace std;
 using namespace OpenSim;
 using SimTK::Vec3;
+using SimTK::UnitVec3;
 
 static const char* wrapTypeName = "cylinder";
 static Vec3 p0(0.0, 0.0, -1.0);
@@ -181,10 +184,10 @@ int WrapCylinder::wrapLine(const SimTK::State& s, SimTK::Vec3& aPoint1, SimTK::V
     double dist1, dist2;
     double t12, t00;
 
-    Vec3 pp, vv, uu, r1a, r1b, r2a, r2b, apex, plane_normal, sum_musc, 
+    Vec3 pp, vv, uu, r1a, r1b, r2a, r2b, apex, sum_musc, 
         r1am, r1bm, r2am, r2bm, p11, p22, r1p, r2p, axispt, near12, 
         vert1, vert2, mpt, apex1, apex2, l1, l2, near00;
-
+    UnitVec3 plane_normal;
     int i, return_code = wrapped;
     bool r1_inter, r2_inter;
     bool constrained   = (bool) (_wrapSign != 0);
@@ -537,11 +540,10 @@ int WrapCylinder::wrapLine(const SimTK::State& s, SimTK::Vec3& aPoint1, SimTK::V
     uu = aPoint1 - apex;
     vv = aPoint2 - apex;
 
-    Mtx::Normalize(3, uu, uu);
-    Mtx::Normalize(3, vv, vv);
+    uu.normalize();
+    vv.normalize();
 
-    Mtx::CrossProduct(uu, vv, plane_normal);
-    Mtx::Normalize(3, plane_normal, plane_normal);
+    SimTK::UnitVec3 plane_nomral(uu % vv);
 
     d = - aPoint1[0] * plane_normal[0] - aPoint1[1] * plane_normal[1] - aPoint1[2] * plane_normal[2];
 
@@ -660,7 +662,18 @@ 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) {
+        // Use one WrapSegment/cord instead of dense list of wrap_pt(s)
+        _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);
+        return;
+    }
     // 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);
@@ -675,7 +688,7 @@ void WrapCylinder::_make_spiral_path(SimTK::Vec3& aPoint1,
 
         // adjust r1/r2 tangent points if necessary to achieve tangency with
         // the spiral path:
-        if (i == 1 && iterations < MAX_ITERATIONS)
+        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);

OpenSim/Simulation/Wrap/WrapMath.cpp

@@ -73,17 +73,15 @@ 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)
 {
-    SimTK::Vec3 cross_prod, vec1, vec2;
-
-    vec1 = p2 - p1;
+    SimTK::Vec3 vec1 = p2 - p1;
 
     double mag1 = Mtx::Normalize(3, vec1, vec1);
 
-    vec2 = p4 - p3;
+    SimTK::Vec3 vec2 = p4 - p3;
 
     double mag2 = Mtx::Normalize(3, vec2, vec2);
 
-    Mtx::CrossProduct(vec1, vec2, cross_prod);
+    SimTK::Vec3 cross_prod = vec1 % vec2;
 
     double denom = cross_prod.normSqr();
 
@@ -132,7 +130,7 @@ IntersectLines(SimTK::Vec3& p1, SimTK::Vec3& p2, SimTK::Vec3& p3, SimTK::Vec3& p
  */
 bool WrapMath::
 IntersectLineSegPlane(SimTK::Vec3& pt1, SimTK::Vec3& pt2, 
-                             SimTK::Vec3& plane, double d,
+                             SimTK::UnitVec3& plane, double d,
                              SimTK::Vec3& inter)
 {
     SimTK::Vec3 vec = pt2 - pt1;
@@ -147,10 +145,7 @@ IntersectLineSegPlane(SimTK::Vec3& pt1, SimTK::Vec3& pt2,
     if ((t < -LINE_EPSILON) || (t > 1.0 + LINE_EPSILON))
         return false;
 
-    inter[0] = pt1[0] + (t * vec[0]);
-    inter[1] = pt1[1] + (t * vec[1]);
-    inter[2] = pt1[2] + (t * vec[2]);
-
+    inter = pt1 + (t * vec);
     return true;
 }
 

OpenSim/Simulation/Wrap/WrapMath.h

@@ -28,8 +28,7 @@
  */
 
 #include <OpenSim/Simulation/osimSimulationDLL.h>
-#include <SimTKcommon/SmallMatrix.h>
-
+#include <SimTKcommon.h>
 
 namespace OpenSim { 
 
@@ -54,7 +53,7 @@ class OSIMSIMULATION_API WrapMath
         SimTK::Vec3& pInt2, double& t);
     static bool
         IntersectLineSegPlane(SimTK::Vec3& pt1, SimTK::Vec3& pt2,
-        SimTK::Vec3& plane, double d, SimTK::Vec3& inter);
+        SimTK::UnitVec3& plane, double d, SimTK::Vec3& inter);
     static void
         ConvertAxisAngleToQuaternion(const SimTK::Vec3& axis,
         double angle, double quat[4]);

OpenSim/Simulation/Wrap/WrapResult.cpp

@@ -57,6 +57,7 @@ void WrapResult::copyData(const WrapResult& aWrapResult) {
         sv[i] = aWrapResult.sv[i];
     }
 
+    singleWrap = aWrapResult.singleWrap;
     // TODO: Should factor be omitted from the copy?
 }
 

OpenSim/Simulation/Wrap/WrapResult.h

@@ -61,8 +61,8 @@ class OSIMSIMULATION_API WrapResult
     // so we can more easily detect any bugs caused by not copying this
     // variable.
     double factor = SimTK::NaN;  // scale factor used to normalize parameters
-
-//=============================================================================
+    bool singleWrap = false; //Flag to indicate if a single wrap object to be considered so we can optimize
+    //=============================================================================
 // METHODS
 //=============================================================================
     //--------------------------------------------------------------------------

OpenSim/Tools/Test/testControllers.cpp

@@ -412,7 +412,7 @@ void testPrescribedControllerFromFile(const std::string& modelFile,
         "testPrescribedControllerFromFile '"+modelName+"'states failed");
 
     CHECK_STORAGE_AGAINST_STANDARD(controls, std_controls, 
-        std::vector<double>(nstates, 0.01), __FILE__, __LINE__,
+        std::vector<double>(nstates, 0.015), __FILE__, __LINE__,
         "testPrescribedControllerFromFile '"+modelName+"'controls failed");
 
     osimModel.disownAllComponents();