diff --git a/robo4j-math/pom.xml b/robo4j-math/pom.xml index 21518181..6c06e1c2 100644 --- a/robo4j-math/pom.xml +++ b/robo4j-math/pom.xml @@ -42,5 +42,9 @@ junit-jupiter-params test + + org.slf4j + slf4j-api + \ No newline at end of file diff --git a/robo4j-math/src/main/java/com/robo4j/math/features/FeatureExtraction.java b/robo4j-math/src/main/java/com/robo4j/math/features/FeatureExtraction.java index cc83e4ae..d2c5851a 100644 --- a/robo4j-math/src/main/java/com/robo4j/math/features/FeatureExtraction.java +++ b/robo4j-math/src/main/java/com/robo4j/math/features/FeatureExtraction.java @@ -16,299 +16,300 @@ */ package com.robo4j.math.features; +import com.robo4j.math.geometry.CurvaturePoint2f; +import com.robo4j.math.geometry.Line2f; +import com.robo4j.math.geometry.Point2f; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + import java.util.ArrayList; import java.util.Collection; import java.util.Iterator; import java.util.List; -import com.robo4j.math.geometry.CurvaturePoint2f; -import com.robo4j.math.geometry.Line2f; -import com.robo4j.math.geometry.Point2f; - /** * Simple and fast feature extraction from lists of Point2f. - * + * * @author Marcus Hirt (@hirt) * @author Miroslav Wengner (@miragemiko) */ // TODO review usage public class FeatureExtraction { - /** - * The residual variance of the Lidar Lite (25 mm). - */ - private static final double RESIDUAL_VARIANCE = 0.025f; - - /** - * Auxiliary constant parameter for segmenting - */ - private static final double AUXILIARY_CONSTANT = Math.toRadians(10); - - /** - * Length variance constant - */ - private static final double Uk = 0.02; - - /** - * Minimal samples for a Line2D - */ - private static final int MIN_Line2D_SAMPLES = 4; - - /** - * Minimal angle deviation for a Line2D - */ - private static final float Line2D_ANGLE_THRESHOLD = 0.5f; - - /** - * Minimal angle deviation to be part of a possible corner - */ - private static final float CURVATURE_THRESHOLD = (float) (Math.PI / 6); - - /** - * Minimal angle deviation to be a corner - */ - private static final float CORNER_THRESHOLD = (float) (Math.PI / 3); - - /** - * Calculates the segments in the scan, using the Borg and Aldon adaptive - * break algorithm. - * - * @param Point2fs - * the Point2fs to segment. - * @param angularResolution - * of the scan in radians. - * - * @return the Point2fs broken up into segments. - */ - public static List> segment(List Point2fs, float angularResolution) { - List> segments = new ArrayList>(10); - - Iterator iterator = Point2fs.iterator(); - List currentSegment = new ArrayList(); - segments.add(currentSegment); - - Point2f lastPoint2D = iterator.next(); - currentSegment.add(lastPoint2D); - - while (iterator.hasNext()) { - Point2f nextPoint2D = iterator.next(); - float delta = nextPoint2D.distance(lastPoint2D); - double maxRange = segmentMaxRange(lastPoint2D.getRange(), angularResolution); - if (delta > maxRange) { - currentSegment = new ArrayList(); - segments.add(currentSegment); - } - currentSegment.add(nextPoint2D); - lastPoint2D = nextPoint2D; - } - return segments; - } - - public static float calculateVectorAngle(Point2f b, Point2f center, Point2f f) { - if (b.equals(center) || f.equals(center)) { - return 0; - } - - double bDeltaX = center.getX() - b.getX(); - double bDeltaY = center.getY() - b.getY(); - - double fDeltaX = f.getX() - center.getX(); - double fDeltaY = f.getY() - center.getY(); - - return (float) (Math.atan2(fDeltaX, fDeltaY) - Math.atan2(bDeltaX, bDeltaY)); - } - - private static double segmentMaxRange(float lastRange, float angularResolution) { - return lastRange * Math.sin(angularResolution) / Math.sin(AUXILIARY_CONSTANT - angularResolution) + 3 * RESIDUAL_VARIANCE; - } - - public static float[] calculateSimpleVectorAngles(List Point2fs) { - if (Point2fs.size() < 5) { - return null; - } - - float[] alphas = new float[Point2fs.size()]; - for (int i = 0; i < Point2fs.size(); i++) { - Point2f before = i == 0 ? Point2fs.get(0) : Point2fs.get(i - 1); - Point2f center = Point2fs.get(i); - Point2f following = i == Point2fs.size() - 1 ? Point2fs.get(i) : Point2fs.get(i + 1); - alphas[i] = calculateVectorAngle(before, center, following); - } - return alphas; - } - - public static FeatureSet getFeatures(List sample, float angularResolution) { - return extractFeatures(sample, angularResolution); - } - - private static FeatureSet extractFeatures(List sample, float angularResolution) { - List> segments = segment(sample, angularResolution); - List corners = new ArrayList<>(); - List Line2fs = new ArrayList<>(); - for (List Point2fs : segments) { - if (Point2fs.size() < MIN_Line2D_SAMPLES) { - continue; - } - float[] deltaAngles = calculateSamplePoint2DDeltaAngles(Point2fs); - if (deltaAngles == null) { - continue; - } - Line2fs.addAll(extractLine2Ds(Point2fs, deltaAngles)); - corners.addAll(extractCorners(Point2fs, deltaAngles)); - } + private static final Logger LOGGER = LoggerFactory.getLogger(FeatureExtraction.class); + /** + * The residual variance of the Lidar Lite (25 mm). + */ + private static final double RESIDUAL_VARIANCE = 0.025f; + + /** + * Auxiliary constant parameter for segmenting + */ + private static final double AUXILIARY_CONSTANT = Math.toRadians(10); + + /** + * Length variance constant + */ + private static final double Uk = 0.02; + + /** + * Minimal samples for a Line2D + */ + private static final int MIN_Line2D_SAMPLES = 4; + + /** + * Minimal angle deviation for a Line2D + */ + private static final float Line2D_ANGLE_THRESHOLD = 0.5f; + + /** + * Minimal angle deviation to be part of a possible corner + */ + private static final float CURVATURE_THRESHOLD = (float) (Math.PI / 6); + + /** + * Minimal angle deviation to be a corner + */ + private static final float CORNER_THRESHOLD = (float) (Math.PI / 3); + + /** + * Calculates the segments in the scan, using the Borg and Aldon adaptive + * break algorithm. + * + * @param Point2fs the Point2fs to segment. + * @param angularResolution of the scan in radians. + * @return the Point2fs broken up into segments. + */ + public static List> segment(List Point2fs, float angularResolution) { + List> segments = new ArrayList>(10); + + Iterator iterator = Point2fs.iterator(); + List currentSegment = new ArrayList(); + segments.add(currentSegment); + + Point2f lastPoint2D = iterator.next(); + currentSegment.add(lastPoint2D); + + while (iterator.hasNext()) { + Point2f nextPoint2D = iterator.next(); + float delta = nextPoint2D.distance(lastPoint2D); + double maxRange = segmentMaxRange(lastPoint2D.getRange(), angularResolution); + if (delta > maxRange) { + currentSegment = new ArrayList(); + segments.add(currentSegment); + } + currentSegment.add(nextPoint2D); + lastPoint2D = nextPoint2D; + } + return segments; + } + + public static float calculateVectorAngle(Point2f b, Point2f center, Point2f f) { + if (b.equals(center) || f.equals(center)) { + return 0; + } + + double bDeltaX = center.getX() - b.getX(); + double bDeltaY = center.getY() - b.getY(); + + double fDeltaX = f.getX() - center.getX(); + double fDeltaY = f.getY() - center.getY(); + + return (float) (Math.atan2(fDeltaX, fDeltaY) - Math.atan2(bDeltaX, bDeltaY)); + } + + private static double segmentMaxRange(float lastRange, float angularResolution) { + return lastRange * Math.sin(angularResolution) / Math.sin(AUXILIARY_CONSTANT - angularResolution) + 3 * RESIDUAL_VARIANCE; + } + + public static float[] calculateSimpleVectorAngles(List Point2fs) { + if (Point2fs.size() < 5) { + return null; + } + + float[] alphas = new float[Point2fs.size()]; + for (int i = 0; i < Point2fs.size(); i++) { + Point2f before = i == 0 ? Point2fs.get(0) : Point2fs.get(i - 1); + Point2f center = Point2fs.get(i); + Point2f following = i == Point2fs.size() - 1 ? Point2fs.get(i) : Point2fs.get(i + 1); + alphas[i] = calculateVectorAngle(before, center, following); + } + return alphas; + } + + public static FeatureSet getFeatures(List sample, float angularResolution) { + return extractFeatures(sample, angularResolution); + } + + private static FeatureSet extractFeatures(List sample, float angularResolution) { + List> segments = segment(sample, angularResolution); + List corners = new ArrayList<>(); + List Line2fs = new ArrayList<>(); + for (List Point2fs : segments) { + if (Point2fs.size() < MIN_Line2D_SAMPLES) { + continue; + } + float[] deltaAngles = calculateSamplePoint2DDeltaAngles(Point2fs); + if (deltaAngles == null) { + continue; + } + Line2fs.addAll(extractLine2Ds(Point2fs, deltaAngles)); + corners.addAll(extractCorners(Point2fs, deltaAngles)); + } return new FeatureSet(Line2fs, corners); - } - - @SuppressWarnings("unused") - private static Collection extractCornersOld(List Point2fs, float[] deltaAngles) { - List corners = new ArrayList<>(); - for (int i = 0; i < deltaAngles.length; i++) { - if (Math.abs(deltaAngles[i]) > CURVATURE_THRESHOLD) { - int maxIndex = i; - float maxPhi = deltaAngles[i]; - int j = i + 1; - float totalPhi = maxPhi; - while (j < deltaAngles.length - 1) { - if (Math.abs(deltaAngles[j]) > CURVATURE_THRESHOLD && Math.signum(deltaAngles[i]) == Math.signum(deltaAngles[j])) { - totalPhi += deltaAngles[j]; - if (deltaAngles[j] > maxPhi) { - maxPhi = deltaAngles[j]; - maxIndex = j; - } - j++; - } else { - i = j; - break; - } - } - - if (Math.abs(totalPhi) > CORNER_THRESHOLD) { - corners.add(CurvaturePoint2f.fromPoint(Point2fs.get(maxIndex), totalPhi)); - } - } - } - return corners; - } - - private static Collection extractCorners(List Point2fs, float[] deltaAngles) { - List corners = new ArrayList<>(); - for (int i = 0; i < deltaAngles.length; i++) { - if (Math.abs(deltaAngles[i]) > CURVATURE_THRESHOLD) { - int maxIndex = i; - float maxPhi = deltaAngles[i]; - float totalPhi = maxPhi; - int last = Math.min(i + 4, deltaAngles.length); - for (int k = i + 1; k < last; k++) { - totalPhi += deltaAngles[k]; - if (deltaAngles[k] > maxPhi) { - maxPhi = deltaAngles[k]; - maxIndex = k; - } - i = k; - } - - if (Math.abs(totalPhi) > CORNER_THRESHOLD && Math.signum(totalPhi) == Math.signum(maxPhi) && maxIndex - 3 >= 0 - && maxIndex + 4 < deltaAngles.length) { - Point2f p = Point2fs.get(maxIndex); - Point2f b = Point2fs.get(maxIndex - 3); - Point2f f = Point2fs.get(maxIndex + 3); - float cornerAlpha = calculateVectorAngle(b, p, f); - if (cornerAlpha > CORNER_THRESHOLD) { - corners.add(CurvaturePoint2f.fromPoint(p, cornerAlpha)); - } - } - } - } - return corners; - } - - public static float[] calculateSamplePoint2DDeltaAngles(List Point2fs) { - if (Point2fs.size() < 5) { - return null; - } - - float[] alphas = new float[Point2fs.size()]; - for (int i = 0; i < Point2fs.size(); i++) { - if (i == 0 || i == Point2fs.size() - 1) { - alphas[i] = 0; - continue; - } - int kb = calculateKB(Point2fs, i); - int kf = calculateKF(Point2fs, i); - Point2f before = Point2fs.get(i - kb); - Point2f center = Point2fs.get(i); - Point2f following = Point2fs.get(i + kf); - alphas[i] = calculateVectorAngle(before, center, following); - } - return alphas; - } - - public static int calculateKF(List Point2fs, int Point2DIndex) { - if (Point2DIndex >= Point2fs.size() - 1) { - return 0; - } - double length = 0; - double distance = 0; - Point2f startPoint2D = Point2fs.get(Point2DIndex); - int i = Point2DIndex; - while (i < Point2fs.size() - 1) { - length += Point2fs.get(i + 1).distance(Point2fs.get(i)); - distance = Point2fs.get(i + 1).distance(startPoint2D); - if ((length - Uk) >= distance) { - break; - } - i++; - } - return i - Point2DIndex; - } - - public static int calculateKB(List Point2fs, int Point2DIndex) { - if (Point2DIndex < 1) { - return 0; - } - float length = 0; - float distance = 0; - Point2f startPoint2D = Point2fs.get(Point2DIndex); - int i = Point2DIndex; - while (i > 0) { - length += Point2fs.get(i - 1).distance(Point2fs.get(i)); - distance = Point2fs.get(i - 1).distance(startPoint2D); - if ((length - Uk) >= distance) { - break; - } - i--; - } - return Point2DIndex - i; - } - - public static void main(String[] args) { - Point2f b = Point2f.fromPolar(18, 18); - Point2f center = Point2f.fromPolar(19, 19); - Point2f f = Point2f.fromPolar(20, 20); - float radians = calculateVectorAngle(b, center, f); - - System.out.println("Vec angle: " + Math.toDegrees(radians) + " radians: " + radians); - } - - private static List extractLine2Ds(List Point2fs, float[] deltaAngles) { - List Line2fs = new ArrayList<>(); - for (int i = 0; i < deltaAngles.length - MIN_Line2D_SAMPLES;) { - while (i < deltaAngles.length - 1 && Math.abs(deltaAngles[i]) > Line2D_ANGLE_THRESHOLD) { - i++; - } - int j = i; - while (j < deltaAngles.length - 2 && (Math.abs(deltaAngles[j]) <= Line2D_ANGLE_THRESHOLD)) { - j++; - } - if (j - i - 1 >= MIN_Line2D_SAMPLES) { - Line2fs.add(new Line2f(Point2fs.get(i), Point2fs.get(j))); - } - i = j; - } - return Line2fs; - } - - public static float getAngularResolution(List Point2fs) { - return Point2fs.get(1).getAngle() - Point2fs.get(0).getAngle(); - } + } + + @SuppressWarnings("unused") + private static Collection extractCornersOld(List Point2fs, float[] deltaAngles) { + List corners = new ArrayList<>(); + for (int i = 0; i < deltaAngles.length; i++) { + if (Math.abs(deltaAngles[i]) > CURVATURE_THRESHOLD) { + int maxIndex = i; + float maxPhi = deltaAngles[i]; + int j = i + 1; + float totalPhi = maxPhi; + while (j < deltaAngles.length - 1) { + if (Math.abs(deltaAngles[j]) > CURVATURE_THRESHOLD && Math.signum(deltaAngles[i]) == Math.signum(deltaAngles[j])) { + totalPhi += deltaAngles[j]; + if (deltaAngles[j] > maxPhi) { + maxPhi = deltaAngles[j]; + maxIndex = j; + } + j++; + } else { + i = j; + break; + } + } + + if (Math.abs(totalPhi) > CORNER_THRESHOLD) { + corners.add(CurvaturePoint2f.fromPoint(Point2fs.get(maxIndex), totalPhi)); + } + } + } + return corners; + } + + private static Collection extractCorners(List Point2fs, float[] deltaAngles) { + List corners = new ArrayList<>(); + for (int i = 0; i < deltaAngles.length; i++) { + if (Math.abs(deltaAngles[i]) > CURVATURE_THRESHOLD) { + int maxIndex = i; + float maxPhi = deltaAngles[i]; + float totalPhi = maxPhi; + int last = Math.min(i + 4, deltaAngles.length); + for (int k = i + 1; k < last; k++) { + totalPhi += deltaAngles[k]; + if (deltaAngles[k] > maxPhi) { + maxPhi = deltaAngles[k]; + maxIndex = k; + } + i = k; + } + + if (Math.abs(totalPhi) > CORNER_THRESHOLD && Math.signum(totalPhi) == Math.signum(maxPhi) && maxIndex - 3 >= 0 + && maxIndex + 4 < deltaAngles.length) { + Point2f p = Point2fs.get(maxIndex); + Point2f b = Point2fs.get(maxIndex - 3); + Point2f f = Point2fs.get(maxIndex + 3); + float cornerAlpha = calculateVectorAngle(b, p, f); + if (cornerAlpha > CORNER_THRESHOLD) { + corners.add(CurvaturePoint2f.fromPoint(p, cornerAlpha)); + } + } + } + } + return corners; + } + + public static float[] calculateSamplePoint2DDeltaAngles(List Point2fs) { + if (Point2fs.size() < 5) { + return null; + } + + float[] alphas = new float[Point2fs.size()]; + for (int i = 0; i < Point2fs.size(); i++) { + if (i == 0 || i == Point2fs.size() - 1) { + alphas[i] = 0; + continue; + } + int kb = calculateKB(Point2fs, i); + int kf = calculateKF(Point2fs, i); + Point2f before = Point2fs.get(i - kb); + Point2f center = Point2fs.get(i); + Point2f following = Point2fs.get(i + kf); + alphas[i] = calculateVectorAngle(before, center, following); + } + return alphas; + } + + public static int calculateKF(List Point2fs, int Point2DIndex) { + if (Point2DIndex >= Point2fs.size() - 1) { + return 0; + } + double length = 0; + double distance = 0; + Point2f startPoint2D = Point2fs.get(Point2DIndex); + int i = Point2DIndex; + while (i < Point2fs.size() - 1) { + length += Point2fs.get(i + 1).distance(Point2fs.get(i)); + distance = Point2fs.get(i + 1).distance(startPoint2D); + if ((length - Uk) >= distance) { + break; + } + i++; + } + return i - Point2DIndex; + } + + public static int calculateKB(List Point2fs, int Point2DIndex) { + if (Point2DIndex < 1) { + return 0; + } + float length = 0; + float distance = 0; + Point2f startPoint2D = Point2fs.get(Point2DIndex); + int i = Point2DIndex; + while (i > 0) { + length += Point2fs.get(i - 1).distance(Point2fs.get(i)); + distance = Point2fs.get(i - 1).distance(startPoint2D); + if ((length - Uk) >= distance) { + break; + } + i--; + } + return Point2DIndex - i; + } + + // TODO: consider as an example to remove main + public static void main(String[] args) { + Point2f b = Point2f.fromPolar(18, 18); + Point2f center = Point2f.fromPolar(19, 19); + Point2f f = Point2f.fromPolar(20, 20); + float radians = calculateVectorAngle(b, center, f); + + LOGGER.debug("Vec angle: {} radians: {}", Math.toDegrees(radians), radians); + } + + private static List extractLine2Ds(List Point2fs, float[] deltaAngles) { + List Line2fs = new ArrayList<>(); + for (int i = 0; i < deltaAngles.length - MIN_Line2D_SAMPLES; ) { + while (i < deltaAngles.length - 1 && Math.abs(deltaAngles[i]) > Line2D_ANGLE_THRESHOLD) { + i++; + } + int j = i; + while (j < deltaAngles.length - 2 && (Math.abs(deltaAngles[j]) <= Line2D_ANGLE_THRESHOLD)) { + j++; + } + if (j - i - 1 >= MIN_Line2D_SAMPLES) { + Line2fs.add(new Line2f(Point2fs.get(i), Point2fs.get(j))); + } + i = j; + } + return Line2fs; + } + + public static float getAngularResolution(List Point2fs) { + return Point2fs.get(1).getAngle() - Point2fs.get(0).getAngle(); + } } diff --git a/robo4j-math/src/main/java/com/robo4j/math/geometry/MatrixEmpty.java b/robo4j-math/src/main/java/com/robo4j/math/geometry/MatrixEmpty.java index fcfc6477..fbdd5e5d 100644 --- a/robo4j-math/src/main/java/com/robo4j/math/geometry/MatrixEmpty.java +++ b/robo4j-math/src/main/java/com/robo4j/math/geometry/MatrixEmpty.java @@ -17,13 +17,12 @@ package com.robo4j.math.geometry; -import java.lang.System.Logger; - /** - * Empty Matrix dimensions equal to zero + * Empty Matrix dimensions equal to zero */ public class MatrixEmpty implements Matrix { public static final int DIMENSION_ZERO = 0; + @Override public int getRows() { return DIMENSION_ZERO; diff --git a/robo4j-math/src/main/java/module-info.java b/robo4j-math/src/main/java/module-info.java index 861f3d97..cef19ea4 100644 --- a/robo4j-math/src/main/java/module-info.java +++ b/robo4j-math/src/main/java/module-info.java @@ -20,6 +20,7 @@ */ module robo4j.math { requires transitive jdk.jfr; + requires org.slf4j; exports com.robo4j.math.features; exports com.robo4j.math.geometry; diff --git a/robo4j-math/src/test/java/com/robo4j/math/geometry/MatrixTest.java b/robo4j-math/src/test/java/com/robo4j/math/geometry/MatrixTest.java index ffc3a9ca..03be3ac1 100644 --- a/robo4j-math/src/test/java/com/robo4j/math/geometry/MatrixTest.java +++ b/robo4j-math/src/test/java/com/robo4j/math/geometry/MatrixTest.java @@ -18,165 +18,172 @@ package com.robo4j.math.geometry; import org.junit.jupiter.api.Test; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; import static org.junit.jupiter.api.Assertions.assertEquals; /** * Matrix tests. - * + * * @author Marcus Hirt (@hirt) * @author Miroslav Wengner (@miragemiko) */ class MatrixTest { - static Integer [] TESTVALUES_3D_INT = new Integer[] {2, 3, 5, 7, 11, 13, 17, 19, 23}; - static Float [] TESTVALUES_3D_FLOAT = new Float[] {2.0f, 3.0f, 5.0f, 7.0f, 11.0f, 13.0f, 17.0f, 19.0f, 23.0f}; - static Double [] TESTVALUES_3D_DOUBLE = new Double[] {2.0, 3.0, 5.0, 7.0, 11.0, 13.0, 17.0, 19.0, 23.0}; - static Integer [] TESTVALUES_4D_INT = new Integer[] {2, 3, 5, 7, 11, 13, 17, 19, 23, 29, 31, 37, 41, 43, 47, 53}; - static Float [] TESTVALUES_4D_FLOAT = new Float[] {2.0f, 3.0f, 5.0f, 7.0f, 11.0f, 13.0f, 17.0f, 19.0f, 23.0f, 29.0f, 31.0f, 37.0f, 41.0f, 43.0f, 47.0f, 53.0f}; - static Double [] TESTVALUES_4D_DOUBLE = new Double[] {2.0, 3.0, 5.0, 7.0, 11.0, 13.0, 17.0, 19.0, 23.0, 29.0, 31.0, 37.0, 41.0, 43.0, 47.0, 53.0}; - - @Test - public void testCreation() { - Matrix matrix = createMatrix(TESTVALUES_3D_INT); - assertEquals(3, matrix.getRows()); - assertEquals(3, matrix.getColumns()); - validate(matrix, TESTVALUES_3D_INT); - - matrix = createMatrix(TESTVALUES_3D_FLOAT); - assertEquals(3, matrix.getRows()); - assertEquals(3, matrix.getColumns()); - validate(matrix, TESTVALUES_3D_FLOAT); - - matrix = createMatrix(TESTVALUES_3D_DOUBLE); - assertEquals(3, matrix.getRows()); - assertEquals(3, matrix.getColumns()); - validate(matrix, TESTVALUES_3D_DOUBLE); - - // 4D - matrix = createMatrix(TESTVALUES_4D_INT); - assertEquals(4, matrix.getRows()); - assertEquals(4, matrix.getColumns()); - validate(matrix, TESTVALUES_4D_INT); - - matrix = createMatrix(TESTVALUES_4D_FLOAT); - assertEquals(4, matrix.getRows()); - assertEquals(4, matrix.getColumns()); - validate(matrix, TESTVALUES_4D_FLOAT); - - matrix = createMatrix(TESTVALUES_4D_DOUBLE); - assertEquals(4, matrix.getRows()); - assertEquals(4, matrix.getColumns()); - validate(matrix, TESTVALUES_4D_DOUBLE); - } - - @Test - void testTranspose() { - Matrix matrix = createMatrix(TESTVALUES_3D_INT); - Matrix matrixTransposed = createMatrix(TESTVALUES_3D_INT); - matrixTransposed.transpose(); - validateTranspose(matrix, matrixTransposed); - - matrix = createMatrix(TESTVALUES_3D_FLOAT); - matrixTransposed = createMatrix(TESTVALUES_3D_FLOAT); - matrixTransposed.transpose(); - validateTranspose(matrix, matrixTransposed); - - matrix = createMatrix(TESTVALUES_3D_DOUBLE); - matrixTransposed = createMatrix(TESTVALUES_3D_DOUBLE); - matrixTransposed.transpose(); - validateTranspose(matrix, matrixTransposed); - - // 4D - matrix = createMatrix(TESTVALUES_4D_INT); - matrixTransposed = createMatrix(TESTVALUES_4D_INT); - matrixTransposed.transpose(); - validateTranspose(matrix, matrixTransposed); - - matrix = createMatrix(TESTVALUES_4D_FLOAT); - matrixTransposed = createMatrix(TESTVALUES_4D_FLOAT); - matrixTransposed.transpose(); - validateTranspose(matrix, matrixTransposed); - - matrix = createMatrix(TESTVALUES_4D_DOUBLE); - matrixTransposed = createMatrix(TESTVALUES_4D_DOUBLE); - matrixTransposed.transpose(); - validateTranspose(matrix, matrixTransposed); - } - - @Test - void testSysOut() { - System.out.println(createMatrix(TESTVALUES_3D_INT)); - System.out.println(createMatrix(TESTVALUES_3D_FLOAT)); - System.out.println(createMatrix(TESTVALUES_3D_DOUBLE)); - System.out.println(createMatrix(TESTVALUES_4D_INT)); - System.out.println(createMatrix(TESTVALUES_4D_FLOAT)); - System.out.println(createMatrix(TESTVALUES_4D_DOUBLE)); - } - - private void validate(Matrix matrix, Number[] numbers) { - for (int i = 0; i < matrix.getRows(); i++) { - for (int j = 0; j < matrix.getColumns(); j++) { - assertEquals(numbers[i * matrix.getColumns() + j], matrix.getNumber(i, j)); - } - } - } - - private void validateTranspose(Matrix matrix, Matrix transpose) { - for (int i = 0; i < matrix.getRows(); i++) { - for (int j = 0; j < matrix.getColumns(); j++) { - assertEquals(matrix.getNumber(i, j), transpose.getNumber(j, i)); - } - } - } - - - private Matrix createMatrix(Number[] testValues) { - if (testValues.length == 9) { - if (testValues.getClass() == Integer[].class) { - return new Matrix3i(toIntArray(testValues)); - } - if (testValues.getClass() == Float[].class) { - return new Matrix3f(toFloatArray(testValues)); - } - if (testValues.getClass() == Double[].class) { - return new Matrix3d(toDoubleArray(testValues)); - } - } - if (testValues.length == 16) { - if (testValues.getClass() == Integer[].class) { - return new Matrix4i(toIntArray(testValues)); - } - if (testValues.getClass() == Float[].class) { - return new Matrix4f(toFloatArray(testValues)); - } - if (testValues.getClass() == Double[].class) { - return new Matrix4d(toDoubleArray(testValues)); - } - } - - return null; - } - - private static int[] toIntArray(Number[] testValues) { - int [] vals = new int[testValues.length]; - for (int i = 0; i < vals.length; i++) { - vals[i] = testValues[i].intValue(); - } - return vals; - } - private static float[] toFloatArray(Number[] testValues) { - float [] vals = new float[testValues.length]; - for (int i = 0; i < vals.length; i++) { - vals[i] = testValues[i].intValue(); - } - return vals; - } - private static double[] toDoubleArray(Number[] testValues) { - double [] vals = new double[testValues.length]; - for (int i = 0; i < vals.length; i++) { - vals[i] = testValues[i].intValue(); - } - return vals; - } - + private static final Logger LOGGER = LoggerFactory.getLogger(MatrixTest.class); + + static Integer[] TESTVALUES_3D_INT = new Integer[]{2, 3, 5, 7, 11, 13, 17, 19, 23}; + static Float[] TESTVALUES_3D_FLOAT = new Float[]{2.0f, 3.0f, 5.0f, 7.0f, 11.0f, 13.0f, 17.0f, 19.0f, 23.0f}; + static Double[] TESTVALUES_3D_DOUBLE = new Double[]{2.0, 3.0, 5.0, 7.0, 11.0, 13.0, 17.0, 19.0, 23.0}; + static Integer[] TESTVALUES_4D_INT = new Integer[]{2, 3, 5, 7, 11, 13, 17, 19, 23, 29, 31, 37, 41, 43, 47, 53}; + static Float[] TESTVALUES_4D_FLOAT = new Float[]{2.0f, 3.0f, 5.0f, 7.0f, 11.0f, 13.0f, 17.0f, 19.0f, 23.0f, 29.0f, 31.0f, 37.0f, 41.0f, 43.0f, 47.0f, 53.0f}; + static Double[] TESTVALUES_4D_DOUBLE = new Double[]{2.0, 3.0, 5.0, 7.0, 11.0, 13.0, 17.0, 19.0, 23.0, 29.0, 31.0, 37.0, 41.0, 43.0, 47.0, 53.0}; + + // TODO : improve test structure + @Test + public void testCreation() { + Matrix matrix = createMatrix(TESTVALUES_3D_INT); + assertEquals(3, matrix.getRows()); + assertEquals(3, matrix.getColumns()); + validate(matrix, TESTVALUES_3D_INT); + + matrix = createMatrix(TESTVALUES_3D_FLOAT); + assertEquals(3, matrix.getRows()); + assertEquals(3, matrix.getColumns()); + validate(matrix, TESTVALUES_3D_FLOAT); + + matrix = createMatrix(TESTVALUES_3D_DOUBLE); + assertEquals(3, matrix.getRows()); + assertEquals(3, matrix.getColumns()); + validate(matrix, TESTVALUES_3D_DOUBLE); + + // 4D + matrix = createMatrix(TESTVALUES_4D_INT); + assertEquals(4, matrix.getRows()); + assertEquals(4, matrix.getColumns()); + validate(matrix, TESTVALUES_4D_INT); + + matrix = createMatrix(TESTVALUES_4D_FLOAT); + assertEquals(4, matrix.getRows()); + assertEquals(4, matrix.getColumns()); + validate(matrix, TESTVALUES_4D_FLOAT); + + matrix = createMatrix(TESTVALUES_4D_DOUBLE); + assertEquals(4, matrix.getRows()); + assertEquals(4, matrix.getColumns()); + validate(matrix, TESTVALUES_4D_DOUBLE); + } + + @Test + void testTranspose() { + Matrix matrix = createMatrix(TESTVALUES_3D_INT); + Matrix matrixTransposed = createMatrix(TESTVALUES_3D_INT); + matrixTransposed.transpose(); + validateTranspose(matrix, matrixTransposed); + + matrix = createMatrix(TESTVALUES_3D_FLOAT); + matrixTransposed = createMatrix(TESTVALUES_3D_FLOAT); + matrixTransposed.transpose(); + validateTranspose(matrix, matrixTransposed); + + matrix = createMatrix(TESTVALUES_3D_DOUBLE); + matrixTransposed = createMatrix(TESTVALUES_3D_DOUBLE); + matrixTransposed.transpose(); + validateTranspose(matrix, matrixTransposed); + + // 4D + matrix = createMatrix(TESTVALUES_4D_INT); + matrixTransposed = createMatrix(TESTVALUES_4D_INT); + matrixTransposed.transpose(); + validateTranspose(matrix, matrixTransposed); + + matrix = createMatrix(TESTVALUES_4D_FLOAT); + matrixTransposed = createMatrix(TESTVALUES_4D_FLOAT); + matrixTransposed.transpose(); + validateTranspose(matrix, matrixTransposed); + + matrix = createMatrix(TESTVALUES_4D_DOUBLE); + matrixTransposed = createMatrix(TESTVALUES_4D_DOUBLE); + matrixTransposed.transpose(); + validateTranspose(matrix, matrixTransposed); + } + + @Test + void testSysOut() { + LOGGER.debug("{}", createMatrix(TESTVALUES_3D_INT)); + LOGGER.debug("{}", createMatrix(TESTVALUES_3D_FLOAT)); + LOGGER.debug("{}", createMatrix(TESTVALUES_3D_DOUBLE)); + LOGGER.debug("{}", createMatrix(TESTVALUES_4D_INT)); + LOGGER.debug("{}", createMatrix(TESTVALUES_4D_FLOAT)); + LOGGER.debug("{}", createMatrix(TESTVALUES_4D_DOUBLE)); + } + + private void validate(Matrix matrix, Number[] numbers) { + for (int i = 0; i < matrix.getRows(); i++) { + for (int j = 0; j < matrix.getColumns(); j++) { + assertEquals(numbers[i * matrix.getColumns() + j], matrix.getNumber(i, j)); + } + } + } + + private void validateTranspose(Matrix matrix, Matrix transpose) { + for (int i = 0; i < matrix.getRows(); i++) { + for (int j = 0; j < matrix.getColumns(); j++) { + assertEquals(matrix.getNumber(i, j), transpose.getNumber(j, i)); + } + } + } + + + private Matrix createMatrix(Number[] testValues) { + if (testValues.length == 9) { + if (testValues.getClass() == Integer[].class) { + return new Matrix3i(toIntArray(testValues)); + } + if (testValues.getClass() == Float[].class) { + return new Matrix3f(toFloatArray(testValues)); + } + if (testValues.getClass() == Double[].class) { + return new Matrix3d(toDoubleArray(testValues)); + } + } + if (testValues.length == 16) { + if (testValues.getClass() == Integer[].class) { + return new Matrix4i(toIntArray(testValues)); + } + if (testValues.getClass() == Float[].class) { + return new Matrix4f(toFloatArray(testValues)); + } + if (testValues.getClass() == Double[].class) { + return new Matrix4d(toDoubleArray(testValues)); + } + } + + return null; + } + + private static int[] toIntArray(Number[] testValues) { + int[] vals = new int[testValues.length]; + for (int i = 0; i < vals.length; i++) { + vals[i] = testValues[i].intValue(); + } + return vals; + } + + private static float[] toFloatArray(Number[] testValues) { + float[] vals = new float[testValues.length]; + for (int i = 0; i < vals.length; i++) { + vals[i] = testValues[i].intValue(); + } + return vals; + } + + private static double[] toDoubleArray(Number[] testValues) { + double[] vals = new double[testValues.length]; + for (int i = 0; i < vals.length; i++) { + vals[i] = testValues[i].intValue(); + } + return vals; + } + }