constraint: accept null findex in boxed LCP solvers

Author: jslee02

dart/constraint/DantzigBoxedLcpSolver.cpp

@@ -75,7 +75,7 @@ bool DantzigBoxedLcpSolver::solve(
     int* findex,
     bool earlyTermination)
 {
-  if (n <= 0 || !A || !x || !b || !lo || !hi || !findex)
+  if (n <= 0 || !A || !x || !b || !lo || !hi)
     return false;
 
   const int nSkip = math::padding(n);
@@ -89,13 +89,14 @@ bool DantzigBoxedLcpSolver::solve(
   Eigen::VectorXd bVec(n);
   Eigen::VectorXd loVec(n);
   Eigen::VectorXd hiVec(n);
-  Eigen::VectorXi findexVec(n);
+  Eigen::VectorXi findexVec = Eigen::VectorXi::Constant(n, -1);
   Eigen::VectorXd xVec(n);
   for (int i = 0; i < n; ++i) {
     bVec[i] = b[i];
     loVec[i] = lo[i];
     hiVec[i] = hi[i];
-    findexVec[i] = findex[i];
+    if (findex)
+      findexVec[i] = findex[i];
     xVec[i] = x[i];
   }
 

dart/constraint/PgsBoxedLcpSolver.cpp

@@ -91,7 +91,7 @@ bool PgsBoxedLcpSolver::solve(
     int* findex,
     bool earlyTermination)
 {
-  if (n <= 0 || !A || !x || !b || !lo || !hi || !findex)
+  if (n <= 0 || !A || !x || !b || !lo || !hi)
     return false;
 
   const int nSkip = math::padding(n);
@@ -105,13 +105,14 @@ bool PgsBoxedLcpSolver::solve(
   Eigen::VectorXd bVec(n);
   Eigen::VectorXd loVec(n);
   Eigen::VectorXd hiVec(n);
-  Eigen::VectorXi findexVec(n);
+  Eigen::VectorXi findexVec = Eigen::VectorXi::Constant(n, -1);
   Eigen::VectorXd xVec(n);
   for (int i = 0; i < n; ++i) {
     bVec[i] = b[i];
     loVec[i] = lo[i];
     hiVec[i] = hi[i];
-    findexVec[i] = findex[i];
+    if (findex)
+      findexVec[i] = findex[i];
     xVec[i] = x[i];
   }
 

tests/unit/constraint/test_BoxedLcpConstraintSolver.cpp

@@ -163,3 +163,41 @@ TEST(BoxedLcpConstraintSolver, SecondaryCanBeDisabled)
   EXPECT_FALSE(dantzig->getDefaultOptions().earlyTermination);
   EXPECT_EQ(solver.getSecondaryLcpSolver(), nullptr);
 }
+
+//==============================================================================
+TEST(DantzigBoxedLcpSolver, AcceptsNullFindex)
+{
+  constraint::DantzigBoxedLcpSolver solver;
+
+  constexpr int kSize = 1;
+  double A[kSize] = {1.0};
+  double x[kSize] = {0.0};
+  const double target = 0.5;
+  double b[kSize] = {A[0] * target};
+  double lo[kSize] = {0.0};
+  double hi[kSize] = {std::numeric_limits<double>::infinity()};
+
+  const bool success = solver.solve(kSize, A, x, b, 0, lo, hi, nullptr, false);
+
+  EXPECT_TRUE(success);
+  EXPECT_NEAR(x[0], target, 1e-8);
+}
+
+//==============================================================================
+TEST(PgsBoxedLcpSolver, AcceptsNullFindex)
+{
+  constraint::PgsBoxedLcpSolver solver;
+
+  constexpr int kSize = 1;
+  double A[kSize] = {1.0};
+  double x[kSize] = {0.0};
+  const double target = 0.5;
+  double b[kSize] = {A[0] * target};
+  double lo[kSize] = {0.0};
+  double hi[kSize] = {std::numeric_limits<double>::infinity()};
+
+  const bool success = solver.solve(kSize, A, x, b, 0, lo, hi, nullptr, false);
+
+  EXPECT_TRUE(success);
+  EXPECT_NEAR(x[0], target, 1e-6);
+}