Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-20 00:51:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpiutil] jni_util: Add JSpan and CriticalJSpan (#5554)

Browse Source 
These replace JArrayRef et al and support statically sized arrays similar to std::span.
This commit is contained in:
Joseph Eng
2023-08-24 00:02:56 -07:00
committed by GitHub
parent 8f3d6a1d4b
commit 2e4ad35e36
19 changed files with 333 additions and 317 deletions
Download Patch File Download Diff File Expand all files Collapse all files

114
wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp
View File

@@ -72,23 +72,23 @@ Java_edu_wpi_first_math_WPIMathJNI_dareDetailABQR
(JNIEnv* env, jclass, jdoubleArray A, jdoubleArray B, jdoubleArray Q,
jdoubleArray R, jint states, jint inputs, jdoubleArray S)
{
JDoubleArrayRef nativeA{env, A};
JDoubleArrayRef nativeB{env, B};
JDoubleArrayRef nativeQ{env, Q};
JDoubleArrayRef nativeR{env, R};
JSpan<const jdouble> nativeA{env, A};
JSpan<const jdouble> nativeB{env, B};
JSpan<const jdouble> nativeQ{env, Q};
JSpan<const jdouble> nativeR{env, R};
Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
Eigen::RowMajor>>
Amat{nativeA.array().data(), states, states};
Amat{nativeA.data(), states, states};
Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
Eigen::RowMajor>>
Bmat{nativeB.array().data(), states, inputs};
Bmat{nativeB.data(), states, inputs};
Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
Eigen::RowMajor>>
Qmat{nativeQ.array().data(), states, states};
Qmat{nativeQ.data(), states, states};
Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
Eigen::RowMajor>>
Rmat{nativeR.array().data(), inputs, inputs};
Rmat{nativeR.data(), inputs, inputs};
Eigen::MatrixXd RmatCopy{Rmat};
auto R_llt = RmatCopy.llt();
@@ -109,27 +109,27 @@ Java_edu_wpi_first_math_WPIMathJNI_dareDetailABQRN
(JNIEnv* env, jclass, jdoubleArray A, jdoubleArray B, jdoubleArray Q,
jdoubleArray R, jdoubleArray N, jint states, jint inputs, jdoubleArray S)
{
JDoubleArrayRef nativeA{env, A};
JDoubleArrayRef nativeB{env, B};
JDoubleArrayRef nativeQ{env, Q};
JDoubleArrayRef nativeR{env, R};
JDoubleArrayRef nativeN{env, N};
JSpan<const jdouble> nativeA{env, A};
JSpan<const jdouble> nativeB{env, B};
JSpan<const jdouble> nativeQ{env, Q};
JSpan<const jdouble> nativeR{env, R};
JSpan<const jdouble> nativeN{env, N};
Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
Eigen::RowMajor>>
Amat{nativeA.array().data(), states, states};
Amat{nativeA.data(), states, states};
Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
Eigen::RowMajor>>
Bmat{nativeB.array().data(), states, inputs};
Bmat{nativeB.data(), states, inputs};
Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
Eigen::RowMajor>>
Qmat{nativeQ.array().data(), states, states};
Qmat{nativeQ.data(), states, states};
Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
Eigen::RowMajor>>
Rmat{nativeR.array().data(), inputs, inputs};
Rmat{nativeR.data(), inputs, inputs};
Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
Eigen::RowMajor>>
Nmat{nativeN.array().data(), states, inputs};
Nmat{nativeN.data(), states, inputs};
Eigen::MatrixXd Rcopy{Rmat};
auto R_llt = Rcopy.llt();
@@ -150,23 +150,23 @@ Java_edu_wpi_first_math_WPIMathJNI_dareABQR
(JNIEnv* env, jclass, jdoubleArray A, jdoubleArray B, jdoubleArray Q,
jdoubleArray R, jint states, jint inputs, jdoubleArray S)
{
JDoubleArrayRef nativeA{env, A};
JDoubleArrayRef nativeB{env, B};
JDoubleArrayRef nativeQ{env, Q};
JDoubleArrayRef nativeR{env, R};
JSpan<const jdouble> nativeA{env, A};
JSpan<const jdouble> nativeB{env, B};
JSpan<const jdouble> nativeQ{env, Q};
JSpan<const jdouble> nativeR{env, R};
Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
Eigen::RowMajor>>
Amat{nativeA.array().data(), states, states};
Amat{nativeA.data(), states, states};
Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
Eigen::RowMajor>>
Bmat{nativeB.array().data(), states, inputs};
Bmat{nativeB.data(), states, inputs};
Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
Eigen::RowMajor>>
Qmat{nativeQ.array().data(), states, states};
Qmat{nativeQ.data(), states, states};
Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
Eigen::RowMajor>>
Rmat{nativeR.array().data(), inputs, inputs};
Rmat{nativeR.data(), inputs, inputs};
try {
Eigen::MatrixXd result =
@@ -191,27 +191,27 @@ Java_edu_wpi_first_math_WPIMathJNI_dareABQRN
(JNIEnv* env, jclass, jdoubleArray A, jdoubleArray B, jdoubleArray Q,
jdoubleArray R, jdoubleArray N, jint states, jint inputs, jdoubleArray S)
{
JDoubleArrayRef nativeA{env, A};
JDoubleArrayRef nativeB{env, B};
JDoubleArrayRef nativeQ{env, Q};
JDoubleArrayRef nativeR{env, R};
JDoubleArrayRef nativeN{env, N};
JSpan<const jdouble> nativeA{env, A};
JSpan<const jdouble> nativeB{env, B};
JSpan<const jdouble> nativeQ{env, Q};
JSpan<const jdouble> nativeR{env, R};
JSpan<const jdouble> nativeN{env, N};
Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
Eigen::RowMajor>>
Amat{nativeA.array().data(), states, states};
Amat{nativeA.data(), states, states};
Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
Eigen::RowMajor>>
Bmat{nativeB.array().data(), states, inputs};
Bmat{nativeB.data(), states, inputs};
Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
Eigen::RowMajor>>
Qmat{nativeQ.array().data(), states, states};
Qmat{nativeQ.data(), states, states};
Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
Eigen::RowMajor>>
Rmat{nativeR.array().data(), inputs, inputs};
Rmat{nativeR.data(), inputs, inputs};
Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
Eigen::RowMajor>>
Nmat{nativeN.array().data(), states, inputs};
Nmat{nativeN.data(), states, inputs};
try {
Eigen::MatrixXd result =
@@ -235,11 +235,11 @@ JNIEXPORT void JNICALL
Java_edu_wpi_first_math_WPIMathJNI_exp
(JNIEnv* env, jclass, jdoubleArray src, jint rows, jdoubleArray dst)
{
JDoubleArrayRef arrayBody{env, src};
JSpan<const jdouble> arrayBody{env, src};
Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
Eigen::RowMajor>>
Amat{arrayBody.array().data(), rows, rows};
Amat{arrayBody.data(), rows, rows};
Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> Aexp =
Amat.exp();
@@ -256,11 +256,11 @@ Java_edu_wpi_first_math_WPIMathJNI_pow
(JNIEnv* env, jclass, jdoubleArray src, jint rows, jdouble exponent,
jdoubleArray dst)
{
JDoubleArrayRef arrayBody{env, src};
JSpan<const jdouble> arrayBody{env, src};
Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
Eigen::RowMajor>>
Amat{arrayBody.array().data(), rows, rows}; // NOLINT
Amat{arrayBody.data(), rows, rows}; // NOLINT
Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> Apow =
Amat.pow(exponent);
@@ -331,16 +331,16 @@ Java_edu_wpi_first_math_WPIMathJNI_isStabilizable
(JNIEnv* env, jclass, jint states, jint inputs, jdoubleArray aSrc,
jdoubleArray bSrc)
{
JDoubleArrayRef nativeA{env, aSrc};
JDoubleArrayRef nativeB{env, bSrc};
JSpan<const jdouble> nativeA{env, aSrc};
JSpan<const jdouble> nativeB{env, bSrc};
Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
Eigen::RowMajor>>
A{nativeA.array().data(), states, states};
A{nativeA.data(), states, states};
Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
Eigen::RowMajor>>
B{nativeB.array().data(), states, inputs};
B{nativeB.data(), states, inputs};
bool isStabilizable =
frc::IsStabilizable<Eigen::Dynamic, Eigen::Dynamic>(A, B);
@@ -382,7 +382,7 @@ Java_edu_wpi_first_math_WPIMathJNI_toPathweaverJson
{
try {
auto trajectory =
CreateTrajectoryFromElements(JDoubleArrayRef{env, elements});
CreateTrajectoryFromElements(JSpan<const jdouble>{env, elements});
frc::TrajectoryUtil::ToPathweaverJson(trajectory,
JStringRef{env, path}.c_str());
} catch (std::exception& e) {
@@ -429,7 +429,7 @@ Java_edu_wpi_first_math_WPIMathJNI_serializeTrajectory
{
try {
auto trajectory =
CreateTrajectoryFromElements(JDoubleArrayRef{env, elements});
CreateTrajectoryFromElements(JSpan<const jdouble>{env, elements});
return MakeJString(env,
frc::TrajectoryUtil::SerializeTrajectory(trajectory));
} catch (std::exception& e) {
@@ -453,24 +453,20 @@ Java_edu_wpi_first_math_WPIMathJNI_rankUpdate
(JNIEnv* env, jclass, jdoubleArray mat, jint rows, jdoubleArray vec,
jdouble sigma, jboolean lowerTriangular)
{
// TODO: Replace with JSpan<jdouble> matBody{env, mat} when that exists
jdouble* matBody = env->GetDoubleArrayElements(mat, nullptr);
JDoubleArrayRef vecBody{env, vec};
JSpan<jdouble> matBody{env, mat};
JSpan<const jdouble> vecBody{env, vec};
Eigen::Map<
Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
L{matBody, rows, rows};
Eigen::Map<const Eigen::Vector<double, Eigen::Dynamic>> v{
vecBody.array().data(), rows};
L{matBody.data(), rows, rows};
Eigen::Map<const Eigen::Vector<double, Eigen::Dynamic>> v{vecBody.data(),
rows};
if (lowerTriangular == JNI_TRUE) {
Eigen::internal::llt_inplace<double, Eigen::Lower>::rankUpdate(L, v, sigma);
} else {
Eigen::internal::llt_inplace<double, Eigen::Upper>::rankUpdate(L, v, sigma);
}
// TODO: Remove this after JSpan transition
env->ReleaseDoubleArrayElements(mat, matBody, 0);
}
/*
@@ -483,15 +479,15 @@ Java_edu_wpi_first_math_WPIMathJNI_solveFullPivHouseholderQr
(JNIEnv* env, jclass, jdoubleArray A, jint Arows, jint Acols, jdoubleArray B,
jint Brows, jint Bcols, jdoubleArray dst)
{
JDoubleArrayRef nativeA{env, A};
JDoubleArrayRef nativeB{env, B};
JSpan<const jdouble> nativeA{env, A};
JSpan<const jdouble> nativeB{env, B};
Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
Eigen::RowMajor>>
Amat{nativeA.array().data(), Arows, Acols};
Amat{nativeA.data(), Arows, Acols};
Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
Eigen::RowMajor>>
Bmat{nativeB.array().data(), Brows, Bcols};
Bmat{nativeB.data(), Brows, Bcols};
Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> Xmat =
Amat.fullPivHouseholderQr().solve(Bmat);