Add searchBinary() method to ControlLinear

Author: nickbianco

OpenSim/Simulation/Control/ControlLinear.cpp

@@ -286,7 +286,7 @@ getParameterList(double aT,Array<int> &rList)
 
     // FIND THE NODE
     _searchNode.setTime(aT);
-    int i = _xNodes.searchBinary(_searchNode);
+    int i = searchBinary(_xNodes, _searchNode);
 
     // LESS THAN TIME OF FIRST NODE
     if(i<0) {
@@ -327,7 +327,7 @@ getParameterList(double aTLower,double aTUpper,Array<int> &rList)
 
     // LOWER NODE
     _searchNode.setTime(aTLower);
-    int iL = _xNodes.searchBinary(_searchNode);
+    int iL = searchBinary(_xNodes, _searchNode);
     if(iL==-1) {
         iL += 1;
     } else if(iL==(size-1)) {
@@ -340,7 +340,7 @@ getParameterList(double aTLower,double aTUpper,Array<int> &rList)
 
     // UPPER NODE
     _searchNode.setTime(aTUpper);
-    int iU = _xNodes.searchBinary(_searchNode);
+    int iU = searchBinary(_xNodes, _searchNode);
     if(iU==-1) {
         return(0);
     } else if( (*_xNodes.get(iU)).isLessThan(_searchNode) ) {
@@ -380,7 +380,7 @@ void ControlLinear::
 setControlValue(ArrayPtrs<ControlLinearNode> &aNodes,double aT,double aValue)
 {
     ControlLinearNode node(aT,aValue);
-    int lower = aNodes.searchBinary(node);
+    int lower = searchBinary(aNodes, node);
 
     // NO NODE
     if(lower<0) {
@@ -426,7 +426,7 @@ getControlValue(ArrayPtrs<ControlLinearNode> &aNodes,double aT)
 
     // GET NODE
     _searchNode.setTime(aT);
-    int i = aNodes.searchBinary(_searchNode);
+    int i = searchBinary(aNodes, _searchNode);
 
     // BEFORE FIRST
     double value;
@@ -512,6 +512,47 @@ extrapolateAfter(ArrayPtrs<ControlLinearNode> &aNodes,double aT) const
     return(value);
 }
 
+int ControlLinear::
+searchBinary(const ArrayPtrs<ControlLinearNode>& nodes,
+        const ControlLinearNode& value) const
+{
+    const int size = nodes.getSize();
+    if(size <= 0) {
+        return -1;
+    }
+
+    int lo = 0;
+    int hi = size - 1;
+    int mid = -1;
+
+    while(lo <= hi) {
+        mid = (lo + hi) / 2;
+        const ControlLinearNode& candidate = *nodes[mid];
+
+        if(value.isLessThan(candidate)) {
+            hi = mid - 1;
+        } else if(candidate.isLessThan(value)) {
+            lo = mid + 1;
+        } else {
+            break;
+        }
+    }
+
+    if(mid < 0) {
+        return -1;
+    }
+
+    if(value.isLessThan(*nodes[mid])) {
+        mid--;
+    }
+
+    if(mid < 0) {
+        return -1;
+    }
+
+    return mid;
+}
+
 //-----------------------------------------------------------------------------
 // CONTROL VALUE
 //-----------------------------------------------------------------------------
@@ -743,7 +784,7 @@ filter(double aT)
     // FIND CONTROL NODE
     // Find the control node at time aT
     _searchNode.setTime(aT);
-    int i = _xNodes.searchBinary(_searchNode);
+    int i = searchBinary(_xNodes, _searchNode);
     // The following property is true after binary search:
     // _xNodes[i].getValue() <= getControlValue(aT)
     // i.e. the node whose index (i) was returned is the node

OpenSim/Simulation/Control/ControlLinear.h

@@ -386,6 +386,8 @@ OpenSim_DECLARE_CONCRETE_OBJECT(ControlLinear, Control);
     double getControlValue(ArrayPtrs<ControlLinearNode> &aNodes,double aT);
     double extrapolateBefore(const ArrayPtrs<ControlLinearNode> &aNodes,double aT) const;
     double extrapolateAfter(ArrayPtrs<ControlLinearNode> &aNodes,double aT) const;
+    int searchBinary(const ArrayPtrs<ControlLinearNode>& nodes,
+            const ControlLinearNode& value) const;
 
 //=============================================================================
 };  // END of class ControlLinear