public static void assertVector4dEquals(String message, Vector4d expected, Vector4d actual, double delta) { assertEquals(message + " [X component]",expected.getX(), actual.getX(),delta); assertEquals(message + " [Y component]",expected.getY(), actual.getY(),delta); assertEquals(message + " [Z component]",expected.getZ(), actual.getZ(),delta); assertEquals(message + " [W component]",expected.getW(), actual.getW(),delta); }
/** * Transform vector by multiplying it by this transform and put result back * into vector. * * @param vector */ public final void transform(Vector4d vector) { if (vector.getW() != 1.0) { throw new RuntimeException("Final element of vector must be 1."); } double x = mat00 * vector.getX() + mat01 * vector.getY() + mat02 * vector.getZ() + mat03; double y = mat10 * vector.getX() + mat11 * vector.getY() + mat12 * vector.getZ() + mat13; vector.setZ(mat20 * vector.getX() + mat21 * vector.getY() + mat22 * vector.getZ() + mat23); vector.setX(x); vector.setY(y); vector.setW(1.0); }
/** * Multiply a 4x4 matrix by a 4x1 vector. Since result is stored in vector, the matrix must be 4x4. * @param matrix * @param vector */ public static void mult(DenseMatrix64F matrix, Vector4d vector) { if (matrix.numCols != 4 || matrix.numRows != 4) { throw new RuntimeException("Improperly sized matrices."); } double x = vector.getX(); double y = vector.getY(); double z = vector.getZ(); double w = vector.getW(); vector.setX(matrix.get(0, 0) * x + matrix.get(0, 1) * y + matrix.get(0, 2) * z + matrix.get(0, 3) * w); vector.setY(matrix.get(1, 0) * x + matrix.get(1, 1) * y + matrix.get(1, 2) * z + matrix.get(1, 3) * w); vector.setZ(matrix.get(2, 0) * x + matrix.get(2, 1) * y + matrix.get(2, 2) * z + matrix.get(2, 3) * w); vector.setW(matrix.get(3, 0) * x + matrix.get(3, 1) * y + matrix.get(3, 2) * z + matrix.get(3, 3) * w); }