[constraint] Improve speed in constructing A matrix in LCP

Before:

BM_INTEGRATION_boxes
Run on (64 X 3700 MHz CPU s)
CPU Caches:
  L1 Data 32 KiB (x32)
  L1 Instruction 32 KiB (x32)
  L2 Unified 512 KiB (x32)
  L3 Unified 16384 KiB (x8)
Load Average: 31.98, 20.05, 10.93
***WARNING*** CPU scaling is enabled, the benchmark real time measurements may be noisy and will incur extra overhead.
--------------------------------------------------------
Benchmark              Time             CPU   Iterations
--------------------------------------------------------
BM_RunBoxes/2        130 ms          130 ms            5
BM_RunBoxes/5      19524 ms        19523 ms            1

After:

BM_INTEGRATION_boxes
Run on (64 X 3700 MHz CPU s)
CPU Caches:
  L1 Data 32 KiB (x32)
  L1 Instruction 32 KiB (x32)
  L2 Unified 512 KiB (x32)
  L3 Unified 16384 KiB (x8)
Load Average: 39.58, 35.26, 21.57
***WARNING*** CPU scaling is enabled, the benchmark real time measurements may be noisy and will incur extra overhead.
--------------------------------------------------------
Benchmark              Time             CPU   Iterations
--------------------------------------------------------
BM_RunBoxes/2        128 ms          128 ms            5
BM_RunBoxes/5      17537 ms        17535 ms            1

cmake/DARTMacros.cmake

@@ -371,7 +371,7 @@ function(dart_build_tests)
     )
 
     # Link libraries
-    target_link_libraries(${target_name} PRIVATE gtest gtest_main)
+    target_link_libraries(${target_name} PRIVATE GTest::gtest GTest::gtest_main)
     target_link_libraries(
       ${target_name} PRIVATE ${_ARG_LINK_LIBRARIES}
     )

dart/constraint/BoxedLcpConstraintSolver.cpp

@@ -203,7 +203,7 @@ void BoxedLcpConstraintSolver::solveConstrainedGroup(ConstrainedGroup& group)
           if (mFIndex[mOffset[i] + j] >= 0)
             mFIndex[mOffset[i] + j] += mOffset[i];
 
-          // Apply impulse for mipulse test
+          // Apply impulse for impulse test
           {
             DART_PROFILE_SCOPED_N("Unit impulse test");
             constraint->applyUnitImpulse(j);
@@ -220,34 +220,21 @@ void BoxedLcpConstraintSolver::solveConstrainedGroup(ConstrainedGroup& group)
                   mA.data() + index, false);
             }
           }
-
-          // Filling symmetric part of A matrix
-          {
-            DART_PROFILE_SCOPED_N("Fill lower triangle of A");
-            for (std::size_t k = 0; k < i; ++k) {
-              const int indexI = mOffset[i] + j;
-              for (std::size_t l = 0;
-                   l < group.getConstraint(k)->getDimension();
-                   ++l) {
-                const int indexJ = mOffset[k] + l;
-                mA(indexI, indexJ) = mA(indexJ, indexI);
-              }
-            }
-          }
         }
       }
 
-      assert(isSymmetric(
-          n,
-          mA.data(),
-          mOffset[i],
-          mOffset[i] + constraint->getDimension() - 1));
-
       {
         DART_PROFILE_SCOPED_N("Unexcite");
         constraint->unexcite();
       }
     }
+
+    {
+      // Fill lower triangle blocks of A matrix
+      DART_PROFILE_SCOPED_N("Fill lower triangle of A");
+      mA.leftCols(n).triangularView<Eigen::Lower>()
+          = mA.leftCols(n).triangularView<Eigen::Upper>().transpose();
+    }
   }
 
   assert(isSymmetric(n, mA.data()));
@@ -258,7 +245,7 @@ void BoxedLcpConstraintSolver::solveConstrainedGroup(ConstrainedGroup& group)
   //  std::cout << std::endl;
 
   // Solve LCP using the primary solver and fallback to secondary solver when
-  // the parimary solver failed.
+  // the primary solver failed.
   if (mSecondaryBoxedLcpSolver) {
     // Make backups for the secondary LCP solver because the primary solver
     // modifies the original terms.
@@ -283,7 +270,7 @@ void BoxedLcpConstraintSolver::solveConstrainedGroup(ConstrainedGroup& group)
       earlyTermination);
 
   // Sanity check. LCP solvers should not report success with nan values, but
-  // it could happen. So we set the sucees to false for nan values.
+  // it could happen. So we set the success to false for nan values.
   if (success && mX.hasNaN())
     success = false;
 

dart/constraint/DantzigLCPSolver.cpp

@@ -119,7 +119,7 @@ void DantzigLCPSolver::solve(ConstrainedGroup* _group)
       if (findex[offset[i] + j] >= 0)
         findex[offset[i] + j] += offset[i];
 
-      // Apply impulse for mipulse test
+      // Apply impulse for impulse test
       constraint->applyUnitImpulse(j);
 
       // Fill upper triangle blocks of A matrix

dart/constraint/PGSLCPSolver.cpp

@@ -114,7 +114,7 @@ void PGSLCPSolver::solve(ConstrainedGroup* _group)
       if (findex[offset[i] + j] >= 0)
         findex[offset[i] + j] += offset[i];
 
-      // Apply impulse for mipulse test
+      // Apply impulse for impulse test
       constraint->applyUnitImpulse(j);
 
       // Fill upper triangle blocks of A matrix