dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

matrix: adjust printing for if symmetric (lower triangular only)

This commit is contained in:
Daniel Agar 2024-01-16 10:20:21 -05:00 committed by GitHub
parent 64f28c4c07
commit c3ae7b28c0
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
1 changed files with 18 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
src/lib/matrix/matrix/Matrix.hpp
View File

@ -365,7 +365,7 @@ public:
}
}
void print(float eps = 0.00001f) const
void print(float eps = 1e-9) const
{
// print column numbering
if (N > 1) {
@ -393,16 +393,25 @@ public:
printf("\033[1m");
}
// avoid -0.0 for display
if (fabs(d - 0.0) < (double)eps) {
// print fixed width zero
printf(" 0 ");
} else if ((fabs(d) < 1e-4) || (fabs(d) >= 10.0)) {
printf("% .1e ", d);
// if symmetric don't print upper triangular elements
if ((M == N) && (j > i) && (i < N) && (j < M)
&& (fabs(d - static_cast<double>(self(j, i))) < (double)eps)
) {
// print empty space
printf(" ");
} else {
printf("% 6.5f ", d);
// avoid -0.0 for display
if (fabs(d - 0.0) < (double)eps) {
// print fixed width zero
printf(" 0 ");
} else if ((fabs(d) < 1e-4) || (fabs(d) >= 10.0)) {
printf("% .1e ", d);
} else {
printf("% 6.5f ", d);
}
}
// Matrix diagonal elements