diff --git a/wpimath/src/main/java/edu/wpi/first/math/VecBuilder.java b/wpimath/src/main/java/edu/wpi/first/math/VecBuilder.java index 670611ad66..9ba91b0ea5 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/VecBuilder.java +++ b/wpimath/src/main/java/edu/wpi/first/math/VecBuilder.java @@ -14,6 +14,8 @@ import edu.wpi.first.math.numbers.N6; import edu.wpi.first.math.numbers.N7; import edu.wpi.first.math.numbers.N8; import edu.wpi.first.math.numbers.N9; +import java.util.Objects; +import org.ejml.simple.SimpleMatrix; /** * A specialization of {@link MatBuilder} for constructing vectors (Nx1 matrices). @@ -26,7 +28,15 @@ public class VecBuilder extends MatBuilder { } private Vector fillVec(double... data) { - return new Vector<>(fill(data)); + if (Objects.requireNonNull(data).length != this.m_rows.getNum()) { + throw new IllegalArgumentException( + "Invalid vector data provided. Wanted " + + this.m_rows.getNum() + + " vector, but got " + + data.length + + " elements"); + } + return new Vector<>(new SimpleMatrix(this.m_rows.getNum(), 1, true, data)); } /**