Skip to content

Commit 9340795

Browse files
SylvainBertrandSylvainBertrand
authored
Switched to full precision when throwing exception. (#62)
1 parent 3e98b52 commit 9340795

12 files changed

+312
-111
lines changed

‎src/main/java/us/ihmc/euclid/exceptions/NotAMatrix2DException.java‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -40,7 +40,7 @@ public NotAMatrix2DException(String message)
4040
*/
4141
public NotAMatrix2DException(Matrix3DReadOnly matrix)
4242
{
43-
super("The matrix is not in XY plane: \n" + matrix);
43+
super("The matrix is not in XY plane: \n" + matrix.toString(null));
4444
}
4545

4646
/**
@@ -68,6 +68,6 @@ public NotAMatrix2DException(Matrix3DReadOnly matrix)
6868
*/
6969
public NotAMatrix2DException(double m00, double m01, double m02, double m10, double m11, double m12, double m20, double m21, double m22)
7070
{
71-
super("The matrix is not in XY plane: \n" + EuclidCoreIOTools.getMatrix3DString(m00, m01, m02, m10, m11, m12, m20, m21, m22));
71+
super("The matrix is not in XY plane: \n" + EuclidCoreIOTools.getMatrix3DString(null, m00, m01, m02, m10, m11, m12, m20, m21, m22));
7272
}
7373
}

‎src/main/java/us/ihmc/euclid/exceptions/NotAPositiveDefiniteMatrixException.java‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -38,7 +38,7 @@ public NotAPositiveDefiniteMatrixException(String message)
3838
*/
3939
public NotAPositiveDefiniteMatrixException(Matrix3DReadOnly matrix)
4040
{
41-
super("The matrix is not a positive definite matrix: \n" + matrix);
41+
super("The matrix is not a positive definite matrix: \n" + matrix.toString(null));
4242
}
4343

4444
/**
@@ -66,6 +66,6 @@ public NotAPositiveDefiniteMatrixException(Matrix3DReadOnly matrix)
6666
*/
6767
public NotAPositiveDefiniteMatrixException(double m00, double m01, double m02, double m10, double m11, double m12, double m20, double m21, double m22)
6868
{
69-
super("The matrix is not a positive definite matrix: \n" + EuclidCoreIOTools.getMatrix3DString(m00, m01, m02, m10, m11, m12, m20, m21, m22));
69+
super("The matrix is not a positive definite matrix: \n" + EuclidCoreIOTools.getMatrix3DString(null, m00, m01, m02, m10, m11, m12, m20, m21, m22));
7070
}
7171
}

‎src/main/java/us/ihmc/euclid/exceptions/NotARotationMatrixException.java‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -38,7 +38,7 @@ public NotARotationMatrixException(String message)
3838
*/
3939
public NotARotationMatrixException(Matrix3DReadOnly matrix)
4040
{
41-
super("The matrix is not a rotation matrix: \n" + matrix);
41+
super("The matrix is not a rotation matrix: \n" + matrix.toString(null));
4242
}
4343

4444
/**
@@ -66,6 +66,6 @@ public NotARotationMatrixException(Matrix3DReadOnly matrix)
6666
*/
6767
public NotARotationMatrixException(double m00, double m01, double m02, double m10, double m11, double m12, double m20, double m21, double m22)
6868
{
69-
super("The matrix is not a rotation matrix: \n" + EuclidCoreIOTools.getMatrix3DString(m00, m01, m02, m10, m11, m12, m20, m21, m22));
69+
super("The matrix is not a rotation matrix: \n" + EuclidCoreIOTools.getMatrix3DString(null, m00, m01, m02, m10, m11, m12, m20, m21, m22));
7070
}
7171
}

‎src/main/java/us/ihmc/euclid/exceptions/NotAnOrientation2DException.java‎

Lines changed: 1 addition & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -39,7 +39,6 @@ public NotAnOrientation2DException(String message)
3939
*/
4040
public NotAnOrientation2DException(Orientation3DReadOnly orientation3DReadOnly)
4141
{
42-
super("The orientation is not in XY plane: \n" + orientation3DReadOnly);
42+
super("The orientation is not in XY plane: \n" + orientation3DReadOnly.toString(null));
4343
}
44-
4544
}

‎src/main/java/us/ihmc/euclid/exceptions/SingularMatrixException.java‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -40,7 +40,7 @@ public SingularMatrixException(String message)
4040
*/
4141
public SingularMatrixException(Matrix3DReadOnly matrix)
4242
{
43-
super("The matrix is singular:\n" + matrix);
43+
super("The matrix is singular:\n" + matrix.toString(null));
4444
}
4545

4646
/**
@@ -68,6 +68,6 @@ public SingularMatrixException(Matrix3DReadOnly matrix)
6868
*/
6969
public SingularMatrixException(double m00, double m01, double m02, double m10, double m11, double m12, double m20, double m21, double m22)
7070
{
71-
super("The matrix is singular:\n" + EuclidCoreIOTools.getMatrix3DString(m00, m01, m02, m10, m11, m12, m20, m21, m22));
71+
super("The matrix is singular:\n" + EuclidCoreIOTools.getMatrix3DString(null, m00, m01, m02, m10, m11, m12, m20, m21, m22));
7272
}
7373
}

‎src/main/java/us/ihmc/euclid/tools/EuclidCoreIOTools.java‎

Lines changed: 50 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -1,10 +1,5 @@
11
package us.ihmc.euclid.tools;
22

3-
import java.util.Arrays;
4-
import java.util.Collection;
5-
import java.util.Iterator;
6-
import java.util.function.Function;
7-
83
import us.ihmc.euclid.axisAngle.interfaces.AxisAngleReadOnly;
94
import us.ihmc.euclid.matrix.interfaces.Matrix3DReadOnly;
105
import us.ihmc.euclid.orientation.interfaces.Orientation2DReadOnly;
@@ -18,6 +13,11 @@
1813
import us.ihmc.euclid.tuple4D.interfaces.Tuple4DReadOnly;
1914
import us.ihmc.euclid.yawPitchRoll.interfaces.YawPitchRollReadOnly;
2015

16+
import java.util.Arrays;
17+
import java.util.Collection;
18+
import java.util.Iterator;
19+
import java.util.function.Function;
20+
2121
/**
2222
* {@code EuclidCoreIOTools} is intended to gather the input & output tools for printing, saving,
2323
* and loading geometry objects.
@@ -508,10 +508,51 @@ public static String getMatrix3DString(String format,
508508
double m21,
509509
double m22)
510510
{
511-
String ret = getStringOf("/", " \\\n", ", ", format, m00, m01, m02);
512-
ret += getStringOf("|", " |\n", ", ", format, m10, m11, m12);
513-
ret += getStringOf("\\", " /", ", ", format, m20, m21, m22);
514-
return ret;
511+
// First transform all the numbers into strings with the given format and find the maximum length per column.
512+
String m00String = toString(format, m00);
513+
String m01String = toString(format, m01);
514+
String m02String = toString(format, m02);
515+
String m10String = toString(format, m10);
516+
String m11String = toString(format, m11);
517+
String m12String = toString(format, m12);
518+
String m20String = toString(format, m20);
519+
String m21String = toString(format, m21);
520+
String m22String = toString(format, m22);
521+
522+
int c0Length = EuclidCoreTools.max(m00String.length(), m10String.length(), m20String.length());
523+
int c1Length = EuclidCoreTools.max(m01String.length(), m11String.length(), m21String.length());
524+
int c2Length = EuclidCoreTools.max(m02String.length(), m12String.length(), m22String.length());
525+
526+
// Then reformat the strings to have the same length per column.
527+
m00String = centerPadString(m00String, c0Length);
528+
m01String = centerPadString(m01String, c1Length);
529+
m02String = centerPadString(m02String, c2Length);
530+
m10String = centerPadString(m10String, c0Length);
531+
m11String = centerPadString(m11String, c1Length);
532+
m12String = centerPadString(m12String, c2Length);
533+
m20String = centerPadString(m20String, c0Length);
534+
m21String = centerPadString(m21String, c1Length);
535+
m22String = centerPadString(m22String, c2Length);
536+
537+
return "/%s, %s, %s \\\n|%s, %s, %s |\n\\%s, %s, %s /".formatted(m00String,
538+
m01String,
539+
m02String,
540+
m10String,
541+
m11String,
542+
m12String,
543+
m20String,
544+
m21String,
545+
m22String);
546+
}
547+
548+
private static String centerPadString(String in, int desiredLength)
549+
{
550+
int padLength = desiredLength - in.length();
551+
if (padLength < 0)
552+
return in;
553+
int padRight = padLength / 2;
554+
int padLeft = padLength - padRight;
555+
return " ".repeat(padLeft) + in + " ".repeat(padRight);
515556
}
516557

517558
/**

‎src/main/java/us/ihmc/euclid/tools/EuclidCoreTools.java‎

Lines changed: 115 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -797,7 +797,23 @@ public static double shiftAngleInRange(double angleToShift, double angleStart)
797797
* @param c the third argument to compare.
798798
* @return the maximum value of the three arguments.
799799
*/
800-
public static final double max(double a, double b, double c)
800+
public static double max(double a, double b, double c)
801+
{
802+
if (a > b)
803+
return a > c ? a : c;
804+
else
805+
return b > c ? b : c;
806+
}
807+
808+
/**
809+
* Find and return the argument with the maximum value.
810+
*
811+
* @param a the first argument to compare.
812+
* @param b the second argument to compare.
813+
* @param c the third argument to compare.
814+
* @return the maximum value of the three arguments.
815+
*/
816+
public static int max(int a, int b, int c)
801817
{
802818
if (a > b)
803819
return a > c ? a : c;
@@ -832,6 +848,33 @@ public static double max(double a, double b, double c, double d)
832848
}
833849
}
834850

851+
/**
852+
* Find and return the argument with the maximum value.
853+
*
854+
* @param a the first argument to compare.
855+
* @param b the second argument to compare.
856+
* @param c the third argument to compare.
857+
* @param d the fourth argument to compare.
858+
* @return the maximum value of the four arguments.
859+
*/
860+
public static int max(int a, int b, int c, int d)
861+
{
862+
if (a > b)
863+
{
864+
if (a > c)
865+
return a > d ? a : d;
866+
else
867+
return c > d ? c : d;
868+
}
869+
else
870+
{
871+
if (b > c)
872+
return b > d ? b : d;
873+
else
874+
return c > d ? c : d;
875+
}
876+
}
877+
835878
/**
836879
* Find and return the argument with the minimum value.
837880
*
@@ -840,7 +883,23 @@ public static double max(double a, double b, double c, double d)
840883
* @param c the third argument to compare.
841884
* @return the minimum value of the three arguments.
842885
*/
843-
public static final double min(double a, double b, double c)
886+
public static double min(double a, double b, double c)
887+
{
888+
if (a < b)
889+
return a < c ? a : c;
890+
else
891+
return b < c ? b : c;
892+
}
893+
894+
/**
895+
* Find and return the argument with the minimum value.
896+
*
897+
* @param a the first argument to compare.
898+
* @param b the second argument to compare.
899+
* @param c the third argument to compare.
900+
* @return the minimum value of the three arguments.
901+
*/
902+
public static int min(int a, int b, int c)
844903
{
845904
if (a < b)
846905
return a < c ? a : c;
@@ -875,6 +934,59 @@ public static double min(double a, double b, double c, double d)
875934
}
876935
}
877936

937+
/**
938+
* Find and return the argument with the minimum value.
939+
*
940+
* @param a the first argument to compare.
941+
* @param b the second argument to compare.
942+
* @param c the third argument to compare.
943+
* @param d the fourth argument to compare.
944+
* @return the minimum value of the four arguments.
945+
*/
946+
public static int min(int a, int b, int c, int d)
947+
{
948+
if (a < b)
949+
{
950+
if (a < c)
951+
return a < d ? a : d;
952+
else
953+
return c < d ? c : d;
954+
}
955+
else
956+
{
957+
if (b < c)
958+
return b < d ? b : d;
959+
else
960+
return c < d ? c : d;
961+
}
962+
}
963+
964+
/**
965+
* Find and return the argument with the value in between the two others.
966+
*
967+
* @param a the first argument to compare.
968+
* @param b the second argument to compare.
969+
* @param c the third argument to compare.
970+
* @return the value in between the two other arguments.
971+
*/
972+
public static double med(double a, double b, double c)
973+
{
974+
if (a > b)
975+
{
976+
if (a > c)
977+
return b > c ? b : c;
978+
else
979+
return a;
980+
}
981+
else
982+
{
983+
if (b > c)
984+
return a > c ? a : c;
985+
else
986+
return b;
987+
}
988+
}
989+
878990
/**
879991
* Find and return the argument with the value in between the two others.
880992
*
@@ -883,7 +995,7 @@ public static double min(double a, double b, double c, double d)
883995
* @param c the third argument to compare.
884996
* @return the value in between the two other arguments.
885997
*/
886-
public static final double med(double a, double b, double c)
998+
public static int med(int a, int b, int c)
887999
{
8881000
if (a > b)
8891001
{

‎src/test/java/us/ihmc/euclid/matrix/Matrix3DReadOnlyTest.java‎

Lines changed: 9 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -1,16 +1,8 @@
11
package us.ihmc.euclid.matrix;
22

3-
import static org.junit.jupiter.api.Assertions.assertEquals;
4-
import static org.junit.jupiter.api.Assertions.assertFalse;
5-
import static org.junit.jupiter.api.Assertions.assertTrue;
6-
import static us.ihmc.euclid.EuclidTestConstants.ITERATIONS;
7-
8-
import java.util.Random;
9-
103
import org.ejml.data.DMatrix;
114
import org.ejml.data.DMatrixRMaj;
125
import org.junit.jupiter.api.Test;
13-
146
import us.ihmc.euclid.exceptions.NotAMatrix2DException;
157
import us.ihmc.euclid.exceptions.NotARotationMatrixException;
168
import us.ihmc.euclid.exceptions.NotAnOrientation2DException;
@@ -24,6 +16,11 @@
2416
import us.ihmc.euclid.tuple3D.Vector3D;
2517
import us.ihmc.euclid.tuple4D.Vector4D;
2618

19+
import java.util.Random;
20+
21+
import static org.junit.jupiter.api.Assertions.*;
22+
import static us.ihmc.euclid.EuclidTestConstants.ITERATIONS;
23+
2724
public abstract class Matrix3DReadOnlyTest<T extends Matrix3DReadOnly>
2825
{
2926
public static final double SMALL_EPS = 1.0e-12;
@@ -276,7 +273,7 @@ public void testCheckIfRotationMatrix() throws Exception
276273

277274
T matrix1 = createMatrix(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
278275
EuclidCoreTestTools.assertExceptionIsThrown(() -> matrix1.checkIfRotationMatrix(),
279-
"The matrix is not a rotation matrix: \n" + matrix1,
276+
"The matrix is not a rotation matrix: \n" + matrix1.toString(null),
280277
NotARotationMatrixException.class);
281278

282279
T matrix2 = createMatrix(1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0);
@@ -291,7 +288,9 @@ public void testCheckIfMatrix2D() throws Exception
291288

292289
double d = EuclidCoreRandomTools.nextDouble(random, 5.0);
293290
T matrix1 = createMatrix(0.0, 0.0, d, 0.0, 0.0, d, d, d, d);
294-
EuclidCoreTestTools.assertExceptionIsThrown(() -> matrix1.checkIfMatrix2D(), "The matrix is not in XY plane: \n" + matrix1, NotAMatrix2DException.class);
291+
EuclidCoreTestTools.assertExceptionIsThrown(() -> matrix1.checkIfMatrix2D(),
292+
"The matrix is not in XY plane: \n" + matrix1.toString(null),
293+
NotAMatrix2DException.class);
295294

296295
T matrix2 = createMatrix(1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0);
297296
matrix2.checkIfMatrix2D();

‎src/test/java/us/ihmc/euclid/tools/EuclidCoreIOToolsTest.java‎

Lines changed: 6 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -217,13 +217,13 @@ public void testGetMatrix3DStringRegression()
217217
assertEquals(expected, EuclidCoreIOTools.getMatrix3DString(t));
218218
assertEquals(expected, EuclidCoreIOTools.getMatrix3DString(EuclidCoreIOTools.DEFAULT_FORMAT, t));
219219
assertEquals("null", EuclidCoreIOTools.getMatrix3DString(null));
220-
expected = "/0.1639445, -0.5595418, 0.9690608 \\\n"//
221-
+ "|0.0275034, -0.6188257, 0.7012089 |\n"//
222-
+ "\\-0.0728161, 0.6787342, -0.4460624 /";
220+
expected = "/ 0.1639445, -0.5595418, 0.9690608 \\\n"//
221+
+ "| 0.0275034, -0.6188257, 0.7012089 |\n"//
222+
+ "\\-0.0728161, 0.6787342, -0.4460624 /";
223223
assertEquals(expected, EuclidCoreIOTools.getMatrix3DString(randomFormat, t));
224-
expected = "/0.16394446573547383, -0.559541839804184, 0.9690607506715772 \\\n"//
225-
+ "|0.02750342450772747, -0.6188257166341338, 0.7012089076126404 |\n"//
226-
+ "\\-0.0728161024676699, 0.6787341699515741, -0.4460623648325692 /";
224+
expected = "/0.16394446573547383, -0.559541839804184, 0.9690607506715772 \\\n"//
225+
+ "|0.02750342450772747, -0.6188257166341338, 0.7012089076126404 |\n"//
226+
+ "\\-0.0728161024676699, 0.6787341699515741, -0.4460623648325692 /";
227227
assertEquals(expected, EuclidCoreIOTools.getMatrix3DString(null, t));
228228
}
229229

0 commit comments

Comments
 (0)