Use array instead of ArrayList for VariableMatrix storage

Author: calcmogul

wpimath/src/main/java/org/wpilib/math/autodiff/VariableMatrix.java

@@ -4,8 +4,6 @@
 
 package org.wpilib.math.autodiff;
 
-import java.util.ArrayList;
-import java.util.Arrays;
 import java.util.Iterator;
 import java.util.NoSuchElementException;
 import java.util.function.BinaryOperator;
@@ -16,7 +14,7 @@
 
 /** A matrix of autodiff variables. */
 public class VariableMatrix implements AutoCloseable, Iterable<Variable> {
-  private final ArrayList<Variable> m_storage = new ArrayList<>();
+  private final Variable[] m_storage;
   private int m_rows;
   private int m_cols;
 
@@ -34,8 +32,9 @@ public VariableMatrix(int rows, int cols, long[] handles) {
 
     m_rows = rows;
     m_cols = cols;
-    for (int index = 0; index < rows * cols; ++index) {
-      m_storage.add(new Variable(Variable.HANDLE, handles[index]));
+    m_storage = new Variable[rows * cols];
+    for (int index = 0; index < m_storage.length; ++index) {
+      m_storage[index] = new Variable(Variable.HANDLE, handles[index]);
     }
   }
 
@@ -57,8 +56,9 @@ public VariableMatrix(int rows) {
   public VariableMatrix(int rows, int cols) {
     m_rows = rows;
     m_cols = cols;
-    for (int index = 0; index < rows * cols; ++index) {
-      m_storage.add(new Variable());
+    m_storage = new Variable[rows * cols];
+    for (int index = 0; index < m_storage.length; ++index) {
+      m_storage[index] = new Variable();
     }
   }
 
@@ -80,9 +80,12 @@ public VariableMatrix(double[][] list) {
       assert row.length == m_cols;
     }
 
+    m_storage = new Variable[m_rows * m_cols];
+    int index = 0;
     for (var row : list) {
       for (var elem : row) {
-        m_storage.add(new Variable(elem));
+        m_storage[index] = new Variable(elem);
+        ++index;
       }
     }
   }
@@ -105,8 +108,13 @@ public VariableMatrix(Variable[][] list) {
       assert row.length == m_cols;
     }
 
+    m_storage = new Variable[m_rows * m_cols];
+    int index = 0;
     for (var row : list) {
-      m_storage.addAll(Arrays.asList(row));
+      for (var elem : row) {
+        m_storage[index] = elem;
+        ++index;
+      }
     }
   }
 
@@ -118,9 +126,10 @@ public VariableMatrix(Variable[][] list) {
   public VariableMatrix(SimpleMatrix values) {
     m_rows = values.getNumRows();
     m_cols = values.getNumCols();
+    m_storage = new Variable[m_rows * m_cols];
     for (int row = 0; row < values.getNumRows(); ++row) {
       for (int col = 0; col < values.getNumCols(); ++col) {
-        m_storage.add(new Variable(values.get(row, col)));
+        m_storage[row * m_cols + col] = new Variable(values.get(row, col));
       }
     }
   }
@@ -133,7 +142,7 @@ public VariableMatrix(SimpleMatrix values) {
   public VariableMatrix(Variable variable) {
     m_rows = 1;
     m_cols = 1;
-    m_storage.add(variable);
+    m_storage = new Variable[] {variable};
   }
 
   /**
@@ -144,17 +153,18 @@ public VariableMatrix(Variable variable) {
   public VariableMatrix(VariableBlock values) {
     m_rows = values.rows();
     m_cols = values.cols();
+    m_storage = new Variable[m_rows * m_cols];
     for (int row = 0; row < m_rows; ++row) {
       for (int col = 0; col < m_cols; ++col) {
-        m_storage.add(values.get(row, col));
+        m_storage[row * m_cols + col] = values.get(row, col);
       }
     }
   }
 
   @Override
   public void close() {
     for (int index = 0; index < rows() * cols(); ++index) {
-      m_storage.get(index).close();
+      m_storage[index].close();
     }
   }
 
@@ -258,7 +268,7 @@ public VariableMatrix set(double value) {
   public VariableMatrix set(Variable value) {
     assert rows() == 1 && cols() == 1;
 
-    m_storage.set(0, value);
+    m_storage[0] = value;
 
     return this;
   }
@@ -273,7 +283,7 @@ public VariableMatrix set(Variable value) {
   public void set(int row, int col, Variable value) {
     assert row >= 0 && row < rows();
     assert col >= 0 && col < cols();
-    m_storage.set(row * cols() + col, value);
+    m_storage[row * cols() + col] = value;
   }
 
   /**
@@ -286,7 +296,7 @@ public void set(int row, int col, Variable value) {
   public void set(int row, int col, double value) {
     assert row >= 0 && row < rows();
     assert col >= 0 && col < cols();
-    m_storage.set(row * cols() + col, new Variable(value));
+    m_storage[row * cols() + col] = new Variable(value);
   }
 
   /**
@@ -307,7 +317,7 @@ public void set(int index, double value) {
    */
   public void set(int index, Variable value) {
     assert index >= 0 && index < rows() * cols();
-    m_storage.set(index, value);
+    m_storage[index] = value;
   }
 
   /**
@@ -355,7 +365,7 @@ public void setValue(SimpleMatrix values) {
   public Variable get(int row, int col) {
     assert row >= 0 && row < rows();
     assert col >= 0 && col < cols();
-    return m_storage.get(row * cols() + col);
+    return m_storage[row * cols() + col];
   }
 
   /**
@@ -366,7 +376,7 @@ public Variable get(int row, int col) {
    */
   public Variable get(int index) {
     assert index >= 0 && index < rows() * cols();
-    return m_storage.get(index);
+    return m_storage[index];
   }
 
   /**
@@ -850,7 +860,7 @@ public VariableMatrix cwiseMap(UnaryOperator<Variable> unaryOp) {
    * @return Number of elements in matrix.
    */
   public int size() {
-    return m_storage.size();
+    return m_storage.length;
   }
 
   @Override
@@ -1024,7 +1034,7 @@ public static VariableMatrix solve(VariableMatrix A, VariableMatrix B) {
   long[] getHandles() {
     var handles = new long[size()];
     for (int index = 0; index < size(); ++index) {
-      handles[index] = m_storage.get(index).getHandle();
+      handles[index] = m_storage[index].getHandle();
     }
     return handles;
   }