/** Test adding translation vectors */
  @Test
  public void testTranslation() {
    int testdim = 5;
    AffineTransformation t = new AffineTransformation(testdim);
    assertTrue(t.getDimensionality() == testdim);
    Matrix tm = t.getTransformation();
    assertEquals(
        "initial transformation matrix should be unity", tm, Matrix.unitMatrix(testdim + 1));

    // translation vector
    double[] tv = new double[testdim];
    for (int i = 0; i < testdim; i++) {
      tv[i] = i + testdim;
    }
    t.addTranslation(tv);

    Matrix tm2 = t.getTransformation();
    // Manually do the same changes to the matrix tm
    for (int i = 0; i < testdim; i++) {
      tm.set(i, testdim, i + testdim);
    }
    // Compare the results
    assertEquals("Translation wasn't added correctly to matrix.", tm, tm2);

    // test application to a vector
    double[] v1 = new double[testdim];
    double[] v2t = new double[testdim];
    for (int i = 0; i < testdim; i++) {
      v1[i] = i * i + testdim;
      v2t[i] = i * i + i + 2 * testdim;
    }

    double[] v1t = t.apply(v1);
    assertTrue("Vector wasn't translated properly forward.", Arrays.equals(v2t, v1t));
    double[] v2b = t.applyInverse(v2t);
    assertTrue("Vector wasn't translated properly backwards.", Arrays.equals(v1, v2b));
    double[] v1b = t.applyInverse(v1t);
    assertTrue("Vector wasn't translated properly back and forward.", Arrays.equals(v1, v1b));

    // Translation
    double[] vd = minus(v1, v2b);
    double[] vtd = minus(v1t, v2t);
    assertTrue("Translation changed vector difference.", Arrays.equals(vd, vtd));

    // Translation shouldn't change relative vectors.
    assertTrue(
        "Relative vectors weren't left unchanged by translation!",
        Arrays.equals(v1, t.applyRelative(v1)));
    assertTrue(
        "Relative vectors weren't left unchanged by translation!",
        Arrays.equals(v2t, t.applyRelative(v2t)));
    assertTrue(
        "Relative vectors weren't left unchanged by translation!",
        Arrays.equals(v1t, t.applyRelative(v1t)));
    assertTrue(
        "Relative vectors weren't left unchanged by translation!",
        Arrays.equals(v2b, t.applyRelative(v2b)));
  }
  /** Test identity transform */
  @Test
  public void testIdentityTransform() {
    int testdim = 5;
    AffineTransformation t = new AffineTransformation(testdim);
    assertTrue(t.getDimensionality() == testdim);
    Matrix tm = t.getTransformation();
    assertEquals(
        "initial transformation matrix should be unity", tm, Matrix.unitMatrix(testdim + 1));

    // test application to a vector
    double[] dv = new double[testdim];
    for (int i = 0; i < testdim; i++) {
      dv[i] = i * i + testdim;
    }
    double[] v3 = t.apply(dv);
    assertTrue("identity transformation wasn't identical", Arrays.equals(dv, v3));

    double[] v4 = t.applyInverse(dv);
    assertTrue("inverse of identity wasn't identity", Arrays.equals(dv, v4));
  }
  /** Test direct inclusion of matrices */
  @Test
  public void testMatrix() {
    int testdim = 5;
    int axis1 = 1;
    int axis2 = 3;

    assert (axis1 < testdim);
    assert (axis2 < testdim);
    // don't change the angle; we'll be using that executing the rotation
    // three times will be identity (approximately)
    double angle = Math.toRadians(360 / 3);
    AffineTransformation t = new AffineTransformation(testdim);
    assertTrue(t.getDimensionality() == testdim);
    Matrix tm = t.getTransformation();
    assertEquals(
        "initial transformation matrix should be unity", tm, Matrix.unitMatrix(testdim + 1));

    // rotation matrix
    double[][] rm = new double[testdim][testdim];
    for (int i = 0; i < testdim; i++) {
      rm[i][i] = 1;
    }
    // add the rotation
    rm[axis1][axis1] = +Math.cos(angle);
    rm[axis1][axis2] = -Math.sin(angle);
    rm[axis2][axis1] = +Math.sin(angle);
    rm[axis2][axis2] = +Math.cos(angle);
    t.addMatrix(new Matrix(rm));
    Matrix tm2 = t.getTransformation();

    // We know that we didn't do any translations and tm is the unity matrix
    // so we can manually do the rotation on it, too.
    tm.set(axis1, axis1, +Math.cos(angle));
    tm.set(axis1, axis2, -Math.sin(angle));
    tm.set(axis2, axis1, +Math.sin(angle));
    tm.set(axis2, axis2, +Math.cos(angle));

    // Compare the results
    assertEquals("Rotation wasn't added correctly to matrix.", tm, tm2);

    // test application to a vector
    double[] v1 = new double[testdim];
    for (int i = 0; i < testdim; i++) {
      v1[i] = i * i + testdim;
    }
    double[] v2 = t.apply(v1);
    double[] v3 = t.applyInverse(v2);
    assertTrue("Forward-Backward didn't work correctly.", euclideanLength(minus(v1, v3)) < 0.0001);
    double[] v4 = t.apply(t.apply(t.apply(v1)));
    assertTrue(
        "Triple-Rotation by 120 degree didn't work", euclideanLength(minus(v1, v4)) < 0.0001);

    // Rotation shouldn't disagree for relative vectors.
    // (they just are not affected by translation!)
    assertTrue(
        "Relative vectors were affected differently by pure rotation!",
        Arrays.equals(v2, t.applyRelative(v1)));

    // should do the same as built-in rotation!
    AffineTransformation t2 = new AffineTransformation(testdim);
    t2.addRotation(axis1, axis2, angle);
    double[] t2v2 = t2.apply(v1);
    assertTrue(
        "Manual rotation and AffineTransformation.addRotation disagree.",
        euclideanLength(minus(v2, t2v2)) < 0.0001);
  }