Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-06 03:31:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Simplify LinearSystem#slice in Java (#8658)

Browse Source 
Avoids boxing to reduce allocations, combines for loops as an
optimization, and removes massive switch statement because Java's
generics are type erased, so the different type parameters make no
difference.
This commit is contained in:
Gold856
2026-03-06 02:16:44 -05:00
committed by GitHub
parent e9d226491c
commit c8780950c0
1 changed files with 6 additions and 118 deletions
Download Patch File Download Diff File Expand all files Collapse all files

124
wpimath/src/main/java/org/wpilib/math/system/LinearSystem.java
View File

@@ -5,31 +5,9 @@
package org.wpilib.math.system;
import java.util.Arrays;
import java.util.Collections;
import java.util.List;
import java.util.stream.Collectors;
import org.ejml.simple.SimpleMatrix;
import org.wpilib.math.linalg.Matrix;
import org.wpilib.math.numbers.N1;
import org.wpilib.math.numbers.N10;
import org.wpilib.math.numbers.N11;
import org.wpilib.math.numbers.N12;
import org.wpilib.math.numbers.N13;
import org.wpilib.math.numbers.N14;
import org.wpilib.math.numbers.N15;
import org.wpilib.math.numbers.N16;
import org.wpilib.math.numbers.N17;
import org.wpilib.math.numbers.N18;
import org.wpilib.math.numbers.N19;
import org.wpilib.math.numbers.N2;
import org.wpilib.math.numbers.N20;
import org.wpilib.math.numbers.N3;
import org.wpilib.math.numbers.N4;
import org.wpilib.math.numbers.N5;
import org.wpilib.math.numbers.N6;
import org.wpilib.math.numbers.N7;
import org.wpilib.math.numbers.N8;
import org.wpilib.math.numbers.N9;
import org.wpilib.math.system.proto.LinearSystemProto;
import org.wpilib.math.system.struct.LinearSystemStruct;
import org.wpilib.math.util.Nat;
@@ -254,113 +232,23 @@ public class LinearSystem<States extends Num, Inputs extends Num, Outputs extend
+ "errors.");
}
List<Integer> outputIndicesList =
Arrays.stream(outputIndices).distinct().boxed().collect(Collectors.toList());
Collections.sort(outputIndicesList);
int[] outputIndicesList = Arrays.stream(outputIndices).distinct().sorted().toArray();
if (outputIndices.length != outputIndicesList.size()) {
if (outputIndices.length != outputIndicesList.length) {
throw new IllegalArgumentException(
"Duplicate indices exist. This is usually due to model implementation " + "errors.");
}
SimpleMatrix new_C_Storage = new SimpleMatrix(outputIndices.length, m_C.getNumCols());
SimpleMatrix new_D_Storage = new SimpleMatrix(outputIndices.length, m_D.getNumCols());
int row = 0;
for (var index : outputIndicesList) {
var current_row_data = m_C.extractRowVector(index).getData();
new_C_Storage.setRow(row, 0, current_row_data);
new_C_Storage.setRow(row, 0, m_C.extractRowVector(index).getData());
new_D_Storage.setRow(row, 0, m_D.extractRowVector(index).getData());
row++;
}
SimpleMatrix new_D_Storage = new SimpleMatrix(outputIndices.length, m_D.getNumCols());
row = 0;
for (var index : outputIndicesList) {
var current_row_data = m_D.extractRowVector(index).getData();
new_D_Storage.setRow(row, 0, current_row_data);
row++;
}
switch (outputIndices.length) {
case 20:
Matrix<N20, States> new_C20 = new Matrix<>(new_C_Storage);
Matrix<N20, Inputs> new_D20 = new Matrix<>(new_D_Storage);
return new LinearSystem<>(m_A, m_B, new_C20, new_D20);
case 19:
Matrix<N19, States> new_C19 = new Matrix<>(new_C_Storage);
Matrix<N19, Inputs> new_D19 = new Matrix<>(new_D_Storage);
return new LinearSystem<>(m_A, m_B, new_C19, new_D19);
case 18:
Matrix<N18, States> new_C18 = new Matrix<>(new_C_Storage);
Matrix<N18, Inputs> new_D18 = new Matrix<>(new_D_Storage);
return new LinearSystem<>(m_A, m_B, new_C18, new_D18);
case 17:
Matrix<N17, States> new_C17 = new Matrix<>(new_C_Storage);
Matrix<N17, Inputs> new_D17 = new Matrix<>(new_D_Storage);
return new LinearSystem<>(m_A, m_B, new_C17, new_D17);
case 16:
Matrix<N16, States> new_C16 = new Matrix<>(new_C_Storage);
Matrix<N16, Inputs> new_D16 = new Matrix<>(new_D_Storage);
return new LinearSystem<>(m_A, m_B, new_C16, new_D16);
case 15:
Matrix<N15, States> new_C15 = new Matrix<>(new_C_Storage);
Matrix<N15, Inputs> new_D15 = new Matrix<>(new_D_Storage);
return new LinearSystem<>(m_A, m_B, new_C15, new_D15);
case 14:
Matrix<N14, States> new_C14 = new Matrix<>(new_C_Storage);
Matrix<N14, Inputs> new_D14 = new Matrix<>(new_D_Storage);
return new LinearSystem<>(m_A, m_B, new_C14, new_D14);
case 13:
Matrix<N13, States> new_C13 = new Matrix<>(new_C_Storage);
Matrix<N13, Inputs> new_D13 = new Matrix<>(new_D_Storage);
return new LinearSystem<>(m_A, m_B, new_C13, new_D13);
case 12:
Matrix<N12, States> new_C12 = new Matrix<>(new_C_Storage);
Matrix<N12, Inputs> new_D12 = new Matrix<>(new_D_Storage);
return new LinearSystem<>(m_A, m_B, new_C12, new_D12);
case 11:
Matrix<N11, States> new_C11 = new Matrix<>(new_C_Storage);
Matrix<N11, Inputs> new_D11 = new Matrix<>(new_D_Storage);
return new LinearSystem<>(m_A, m_B, new_C11, new_D11);
case 10:
Matrix<N10, States> new_C10 = new Matrix<>(new_C_Storage);
Matrix<N10, Inputs> new_D10 = new Matrix<>(new_D_Storage);
return new LinearSystem<>(m_A, m_B, new_C10, new_D10);
case 9:
Matrix<N9, States> new_C9 = new Matrix<>(new_C_Storage);
Matrix<N9, Inputs> new_D9 = new Matrix<>(new_D_Storage);
return new LinearSystem<>(m_A, m_B, new_C9, new_D9);
case 8:
Matrix<N8, States> new_C8 = new Matrix<>(new_C_Storage);
Matrix<N8, Inputs> new_D8 = new Matrix<>(new_D_Storage);
return new LinearSystem<>(m_A, m_B, new_C8, new_D8);
case 7:
Matrix<N7, States> new_C7 = new Matrix<>(new_C_Storage);
Matrix<N7, Inputs> new_D7 = new Matrix<>(new_D_Storage);
return new LinearSystem<>(m_A, m_B, new_C7, new_D7);
case 6:
Matrix<N6, States> new_C6 = new Matrix<>(new_C_Storage);
Matrix<N6, Inputs> new_D6 = new Matrix<>(new_D_Storage);
return new LinearSystem<>(m_A, m_B, new_C6, new_D6);
case 5:
Matrix<N5, States> new_C5 = new Matrix<>(new_C_Storage);
Matrix<N5, Inputs> new_D5 = new Matrix<>(new_D_Storage);
return new LinearSystem<>(m_A, m_B, new_C5, new_D5);
case 4:
Matrix<N4, States> new_C4 = new Matrix<>(new_C_Storage);
Matrix<N4, Inputs> new_D4 = new Matrix<>(new_D_Storage);
return new LinearSystem<>(m_A, m_B, new_C4, new_D4);
case 3:
Matrix<N3, States> new_C3 = new Matrix<>(new_C_Storage);
Matrix<N3, Inputs> new_D3 = new Matrix<>(new_D_Storage);
return new LinearSystem<>(m_A, m_B, new_C3, new_D3);
case 2:
Matrix<N2, States> new_C2 = new Matrix<>(new_C_Storage);
Matrix<N2, Inputs> new_D2 = new Matrix<>(new_D_Storage);
return new LinearSystem<>(m_A, m_B, new_C2, new_D2);
default:
Matrix<N1, States> new_C1 = new Matrix<>(new_C_Storage);
Matrix<N1, Inputs> new_D1 = new Matrix<>(new_D_Storage);
return new LinearSystem<>(m_A, m_B, new_C1, new_D1);
}
return new LinearSystem<>(m_A, m_B, new Matrix<>(new_C_Storage), new Matrix<>(new_D_Storage));
}
@Override