Java Code Examples for org.apache.commons.math.util.FastMath#min()

/**
 * Build the transformation to bi-diagonal shape of a matrix.
 * @param matrix the matrix to transform.
 */
public BiDiagonalTransformer(RealMatrix matrix) {

    final int m = matrix.getRowDimension();
    final int n = matrix.getColumnDimension();
    final int p = FastMath.min(m, n);
    householderVectors = matrix.getData();
    main      = new double[p];
    secondary = new double[p - 1];
    cachedU   = null;
    cachedB   = null;
    cachedV   = null;

    // transform matrix
    if (m >= n) {
        transformToUpperBiDiagonal();
    } else {
        transformToLowerBiDiagonal();
    }

}
 

/** {@inheritDoc} */
@Override
public T walkInRowOrder(final FieldMatrixChangingVisitor<T> visitor) {
    visitor.start(rows, columns, 0, rows - 1, 0, columns - 1);
    for (int iBlock = 0; iBlock < blockRows; ++iBlock) {
        final int pStart = iBlock * BLOCK_SIZE;
        final int pEnd   = FastMath.min(pStart + BLOCK_SIZE, rows);
        for (int p = pStart; p < pEnd; ++p) {
            for (int jBlock = 0; jBlock < blockColumns; ++jBlock) {
                final int jWidth = blockWidth(jBlock);
                final int qStart = jBlock * BLOCK_SIZE;
                final int qEnd   = FastMath.min(qStart + BLOCK_SIZE, columns);
                final T[] block = blocks[iBlock * blockColumns + jBlock];
                int k = (p - pStart) * jWidth;
                for (int q = qStart; q < qEnd; ++q) {
                    block[k] = visitor.visit(p, q, block[k]);
                    ++k;
                }
            }
         }
    }
    return visitor.end();
}
 

/**
 * Subtract a polynomial from the instance.
 *
 * @param p Polynomial to subtract.
 * @return a new polynomial which is the difference the instance minus {@code p}.
 */
public PolynomialFunction subtract(final PolynomialFunction p) {
    // identify the lowest degree polynomial
    int lowLength  = FastMath.min(coefficients.length, p.coefficients.length);
    int highLength = FastMath.max(coefficients.length, p.coefficients.length);

    // build the coefficients array
    double[] newCoefficients = new double[highLength];
    for (int i = 0; i < lowLength; ++i) {
        newCoefficients[i] = coefficients[i] - p.coefficients[i];
    }
    if (coefficients.length < p.coefficients.length) {
        for (int i = lowLength; i < highLength; ++i) {
            newCoefficients[i] = -p.coefficients[i];
        }
    } else {
        System.arraycopy(coefficients, lowLength, newCoefficients, lowLength,
                         highLength - lowLength);
    }

    return new PolynomialFunction(newCoefficients);
}
 

/** {@inheritDoc} */
@Override
public double walkInRowOrder(final RealMatrixPreservingVisitor visitor) {
    visitor.start(rows, columns, 0, rows - 1, 0, columns - 1);
    for (int iBlock = 0; iBlock < blockRows; ++iBlock) {
        final int pStart = iBlock * BLOCK_SIZE;
        final int pEnd = FastMath.min(pStart + BLOCK_SIZE, rows);
        for (int p = pStart; p < pEnd; ++p) {
            for (int jBlock = 0; jBlock < blockColumns; ++jBlock) {
                final int jWidth = blockWidth(jBlock);
                final int qStart = jBlock * BLOCK_SIZE;
                final int qEnd = FastMath.min(qStart + BLOCK_SIZE, columns);
                final double[] block = blocks[iBlock * blockColumns + jBlock];
                int k = (p - pStart) * jWidth;
                for (int q = qStart; q < qEnd; ++q) {
                    visitor.visit(p, q, block[k]);
                    ++k;
                }
            }
         }
    }
    return visitor.end();
}
 

/**
 * Create a data array in blocks layout.
 * <p>
 * This method can be used to create the array argument of the {@link
 * #BlockRealMatrix(int, int, double[][], boolean)} constructor.
 * </p>
 * @param rows Number of rows in the new matrix.
 * @param columns Number of columns in the new matrix.
 * @return a new data array in blocks layout.
 * @see #toBlocksLayout(double[][])
 * @see #BlockRealMatrix(int, int, double[][], boolean)
 */
public static double[][] createBlocksLayout(final int rows, final int columns) {
    final int blockRows = (rows    + BLOCK_SIZE - 1) / BLOCK_SIZE;
    final int blockColumns = (columns + BLOCK_SIZE - 1) / BLOCK_SIZE;

    final double[][] blocks = new double[blockRows * blockColumns][];
    int blockIndex = 0;
    for (int iBlock = 0; iBlock < blockRows; ++iBlock) {
        final int pStart = iBlock * BLOCK_SIZE;
        final int pEnd = FastMath.min(pStart + BLOCK_SIZE, rows);
        final int iHeight = pEnd - pStart;
        for (int jBlock = 0; jBlock < blockColumns; ++jBlock) {
            final int qStart = jBlock * BLOCK_SIZE;
            final int qEnd = FastMath.min(qStart + BLOCK_SIZE, columns);
            final int jWidth = qEnd - qStart;
            blocks[blockIndex] = new double[iHeight * jWidth];
            ++blockIndex;
        }
    }

    return blocks;
}
 

/** {@inheritDoc} */
@Override
public double walkInOptimizedOrder(final RealMatrixChangingVisitor visitor,
                                   final int startRow, final int endRow,
                                   final int startColumn, final int endColumn) {
    MatrixUtils.checkSubMatrixIndex(this, startRow, endRow, startColumn, endColumn);
    visitor.start(rows, columns, startRow, endRow, startColumn, endColumn);
    for (int iBlock = startRow / BLOCK_SIZE; iBlock < 1 + endRow / BLOCK_SIZE; ++iBlock) {
        final int p0 = iBlock * BLOCK_SIZE;
        final int pStart = FastMath.max(startRow, p0);
        final int pEnd = FastMath.min((iBlock + 1) * BLOCK_SIZE, 1 + endRow);
        for (int jBlock = startColumn / BLOCK_SIZE; jBlock < 1 + endColumn / BLOCK_SIZE; ++jBlock) {
            final int jWidth = blockWidth(jBlock);
            final int q0 = jBlock * BLOCK_SIZE;
            final int qStart = FastMath.max(startColumn, q0);
            final int qEnd = FastMath.min((jBlock + 1) * BLOCK_SIZE, 1 + endColumn);
            final double[] block = blocks[iBlock * blockColumns + jBlock];
            for (int p = pStart; p < pEnd; ++p) {
                int k = (p - p0) * jWidth + qStart - q0;
                for (int q = qStart; q < qEnd; ++q) {
                    block[k] = visitor.visit(p, q, block[k]);
                    ++k;
                }
            }
        }
    }
    return visitor.end();
}
 

/** {@inheritDoc} */
@Override
public T[][] getData() {

    final T[][] data = buildArray(getField(), getRowDimension(), getColumnDimension());
    final int lastColumns = columns - (blockColumns - 1) * BLOCK_SIZE;

    for (int iBlock = 0; iBlock < blockRows; ++iBlock) {
        final int pStart = iBlock * BLOCK_SIZE;
        final int pEnd   = FastMath.min(pStart + BLOCK_SIZE, rows);
        int regularPos   = 0;
        int lastPos      = 0;
        for (int p = pStart; p < pEnd; ++p) {
            final T[] dataP = data[p];
            int blockIndex = iBlock * blockColumns;
            int dataPos    = 0;
            for (int jBlock = 0; jBlock < blockColumns - 1; ++jBlock) {
                System.arraycopy(blocks[blockIndex++], regularPos, dataP, dataPos, BLOCK_SIZE);
                dataPos += BLOCK_SIZE;
            }
            System.arraycopy(blocks[blockIndex], lastPos, dataP, dataPos, lastColumns);
            regularPos += BLOCK_SIZE;
            lastPos    += lastColumns;
        }
    }

    return data;
}
 

/** {@inheritDoc} */
@Override
public T walkInRowOrder(final FieldMatrixPreservingVisitor<T> visitor,
                        final int startRow, final int endRow,
                        final int startColumn, final int endColumn) {
    checkSubMatrixIndex(startRow, endRow, startColumn, endColumn);
    visitor.start(rows, columns, startRow, endRow, startColumn, endColumn);
    for (int iBlock = startRow / BLOCK_SIZE; iBlock < 1 + endRow / BLOCK_SIZE; ++iBlock) {
        final int p0     = iBlock * BLOCK_SIZE;
        final int pStart = FastMath.max(startRow, p0);
        final int pEnd   = FastMath.min((iBlock + 1) * BLOCK_SIZE, 1 + endRow);
        for (int p = pStart; p < pEnd; ++p) {
            for (int jBlock = startColumn / BLOCK_SIZE; jBlock < 1 + endColumn / BLOCK_SIZE; ++jBlock) {
                final int jWidth = blockWidth(jBlock);
                final int q0     = jBlock * BLOCK_SIZE;
                final int qStart = FastMath.max(startColumn, q0);
                final int qEnd   = FastMath.min((jBlock + 1) * BLOCK_SIZE, 1 + endColumn);
                final T[] block = blocks[iBlock * blockColumns + jBlock];
                int k = (p - p0) * jWidth + qStart - q0;
                for (int q = qStart; q < qEnd; ++q) {
                    visitor.visit(p, q, block[k]);
                    ++k;
                }
            }
         }
    }
    return visitor.end();
}
 

/** {@inheritDoc} */
@Override
public double walkInRowOrder(final RealMatrixChangingVisitor visitor,
                             final int startRow, final int endRow,
                             final int startColumn, final int endColumn) {
    MatrixUtils.checkSubMatrixIndex(this, startRow, endRow, startColumn, endColumn);
    visitor.start(rows, columns, startRow, endRow, startColumn, endColumn);
    for (int iBlock = startRow / BLOCK_SIZE; iBlock < 1 + endRow / BLOCK_SIZE; ++iBlock) {
        final int p0 = iBlock * BLOCK_SIZE;
        final int pStart = FastMath.max(startRow, p0);
        final int pEnd = FastMath.min((iBlock + 1) * BLOCK_SIZE, 1 + endRow);
        for (int p = pStart; p < pEnd; ++p) {
            for (int jBlock = startColumn / BLOCK_SIZE; jBlock < 1 + endColumn / BLOCK_SIZE; ++jBlock) {
                final int jWidth = blockWidth(jBlock);
                final int q0 = jBlock * BLOCK_SIZE;
                final int qStart = FastMath.max(startColumn, q0);
                final int qEnd = FastMath.min((jBlock + 1) * BLOCK_SIZE, 1 + endColumn);
                final double[] block = blocks[iBlock * blockColumns + jBlock];
                int k = (p - p0) * jWidth + qStart - q0;
                for (int q = qStart; q < qEnd; ++q) {
                    block[k] = visitor.visit(p, q, block[k]);
                    ++k;
                }
            }
         }
    }
    return visitor.end();
}
 

/** {@inheritDoc} */
public double integrate(final UnivariateRealFunction f, final double min, final double max)
    throws ConvergenceException,  MathUserException, IllegalArgumentException {

    clearResult();
    verifyInterval(min, max);
    verifyIterationCount();

    // compute first estimate with a single step
    double oldt = stage(f, min, max, 1);

    int n = 2;
    for (int i = 0; i < maximalIterationCount; ++i) {

        // improve integral with a larger number of steps
        final double t = stage(f, min, max, n);

        // estimate error
        final double delta = FastMath.abs(t - oldt);
        final double limit =
            FastMath.max(absoluteAccuracy,
                     relativeAccuracy * (FastMath.abs(oldt) + FastMath.abs(t)) * 0.5);

        // check convergence
        if ((i + 1 >= minimalIterationCount) && (delta <= limit)) {
            setResult(t, i);
            return result;
        }

        // prepare next iteration
        double ratio = FastMath.min(4, FastMath.pow(delta / limit, 0.5 / abscissas.length));
        n = FastMath.max((int) (ratio * n), n + 1);
        oldt = t;

    }

    throw new MaxCountExceededException(maximalIterationCount);

}
 

/** {@inheritDoc} */
@Override
public FieldMatrix<T> transpose() {
    final int nRows = getRowDimension();
    final int nCols = getColumnDimension();
    final BlockFieldMatrix<T> out = new BlockFieldMatrix<T>(getField(), nCols, nRows);

    // perform transpose block-wise, to ensure good cache behavior
    int blockIndex = 0;
    for (int iBlock = 0; iBlock < blockColumns; ++iBlock) {
        for (int jBlock = 0; jBlock < blockRows; ++jBlock) {

            // transpose current block
            final T[] outBlock = out.blocks[blockIndex];
            final T[] tBlock   = blocks[jBlock * blockColumns + iBlock];
            final int      pStart   = iBlock * BLOCK_SIZE;
            final int      pEnd     = FastMath.min(pStart + BLOCK_SIZE, columns);
            final int      qStart   = jBlock * BLOCK_SIZE;
            final int      qEnd     = FastMath.min(qStart + BLOCK_SIZE, rows);
            int k = 0;
            for (int p = pStart; p < pEnd; ++p) {
                final int lInc = pEnd - pStart;
                int l = p - pStart;
                for (int q = qStart; q < qEnd; ++q) {
                    outBlock[k] = tBlock[l];
                    ++k;
                    l+= lInc;
                }
            }

            // go to next block
            ++blockIndex;

        }
    }

    return out;
}
 

/** {@inheritDoc} */
@Override
public double walkInOptimizedOrder(final RealMatrixPreservingVisitor visitor,
                                   final int startRow, final int endRow,
                                   final int startColumn, final int endColumn) {
    MatrixUtils.checkSubMatrixIndex(this, startRow, endRow, startColumn, endColumn);
    visitor.start(rows, columns, startRow, endRow, startColumn, endColumn);
    for (int iBlock = startRow / BLOCK_SIZE; iBlock < 1 + endRow / BLOCK_SIZE; ++iBlock) {
        final int p0 = iBlock * BLOCK_SIZE;
        final int pStart = FastMath.max(startRow, p0);
        final int pEnd = FastMath.min((iBlock + 1) * BLOCK_SIZE, 1 + endRow);
        for (int jBlock = startColumn / BLOCK_SIZE; jBlock < 1 + endColumn / BLOCK_SIZE; ++jBlock) {
            final int jWidth = blockWidth(jBlock);
            final int q0 = jBlock * BLOCK_SIZE;
            final int qStart = FastMath.max(startColumn, q0);
            final int qEnd = FastMath.min((jBlock + 1) * BLOCK_SIZE, 1 + endColumn);
            final double[] block = blocks[iBlock * blockColumns + jBlock];
            for (int p = pStart; p < pEnd; ++p) {
                int k = (p - p0) * jWidth + qStart - q0;
                for (int q = qStart; q < qEnd; ++q) {
                    visitor.visit(p, q, block[k]);
                    ++k;
                }
            }
        }
    }
    return visitor.end();
}
 

/** {@inheritDoc} */
@Override
public BlockRealMatrix transpose() {
    final int nRows = getRowDimension();
    final int nCols = getColumnDimension();
    final BlockRealMatrix out = new BlockRealMatrix(nCols, nRows);

    // perform transpose block-wise, to ensure good cache behavior
    int blockIndex = 0;
    for (int iBlock = 0; iBlock < blockColumns; ++iBlock) {
        for (int jBlock = 0; jBlock < blockRows; ++jBlock) {
            // transpose current block
            final double[] outBlock = out.blocks[blockIndex];
            final double[] tBlock = blocks[jBlock * blockColumns + iBlock];
            final int pStart = iBlock * BLOCK_SIZE;
            final int pEnd = FastMath.min(pStart + BLOCK_SIZE, columns);
            final int qStart = jBlock * BLOCK_SIZE;
            final int qEnd = FastMath.min(qStart + BLOCK_SIZE, rows);
            int k = 0;
            for (int p = pStart; p < pEnd; ++p) {
                final int lInc = pEnd - pStart;
                int l = p - pStart;
                for (int q = qStart; q < qEnd; ++q) {
                    outBlock[k] = tBlock[l];
                    ++k;
                    l+= lInc;
                }
            }
            // go to next block
            ++blockIndex;
        }
    }

    return out;
}
 

/** {@inheritDoc} */
@Override
public T walkInOptimizedOrder(final FieldMatrixPreservingVisitor<T> visitor,
                                   final int startRow, final int endRow,
                                   final int startColumn, final int endColumn)
    throws MatrixIndexException, MatrixVisitorException {
    checkSubMatrixIndex(startRow, endRow, startColumn, endColumn);
    visitor.start(rows, columns, startRow, endRow, startColumn, endColumn);
    for (int iBlock = startRow / BLOCK_SIZE; iBlock < 1 + endRow / BLOCK_SIZE; ++iBlock) {
        final int p0     = iBlock * BLOCK_SIZE;
        final int pStart = FastMath.max(startRow, p0);
        final int pEnd   = FastMath.min((iBlock + 1) * BLOCK_SIZE, 1 + endRow);
        for (int jBlock = startColumn / BLOCK_SIZE; jBlock < 1 + endColumn / BLOCK_SIZE; ++jBlock) {
            final int jWidth = blockWidth(jBlock);
            final int q0     = jBlock * BLOCK_SIZE;
            final int qStart = FastMath.max(startColumn, q0);
            final int qEnd   = FastMath.min((jBlock + 1) * BLOCK_SIZE, 1 + endColumn);
            final T[] block = blocks[iBlock * blockColumns + jBlock];
            for (int p = pStart; p < pEnd; ++p) {
                int k = (p - p0) * jWidth + qStart - q0;
                for (int q = qStart; q < qEnd; ++q) {
                    visitor.visit(p, q, block[k]);
                    ++k;
                }
            }
        }
    }
    return visitor.end();
}
 

/** {@inheritDoc} */
@Override
public BlockRealMatrix subtract(final RealMatrix m) {
    try {
        return subtract((BlockRealMatrix) m);
    } catch (ClassCastException cce) {
        // safety check
        MatrixUtils.checkSubtractionCompatible(this, m);

        final BlockRealMatrix out = new BlockRealMatrix(rows, columns);

        // perform subtraction block-wise, to ensure good cache behavior
        int blockIndex = 0;
        for (int iBlock = 0; iBlock < out.blockRows; ++iBlock) {
            for (int jBlock = 0; jBlock < out.blockColumns; ++jBlock) {

                // perform subtraction on the current block
                final double[] outBlock = out.blocks[blockIndex];
                final double[] tBlock = blocks[blockIndex];
                final int pStart = iBlock * BLOCK_SIZE;
                final int pEnd = FastMath.min(pStart + BLOCK_SIZE, rows);
                final int qStart = jBlock * BLOCK_SIZE;
                final int qEnd = FastMath.min(qStart + BLOCK_SIZE, columns);
                int k = 0;
                for (int p = pStart; p < pEnd; ++p) {
                    for (int q = qStart; q < qEnd; ++q) {
                        outBlock[k] = tBlock[k] - m.getEntry(p, q);
                        ++k;
                    }
                }
                // go to next block
                ++blockIndex;
            }
        }

        return out;
    }
}
 

/**
 * Returns the result of postmultiplying this by m.
 *
 * @param m    matrix to postmultiply by
 * @return     this * m
 * @throws     IllegalArgumentException
 *             if columnDimension(this) != rowDimension(m)
 */
public BlockFieldMatrix<T> multiply(BlockFieldMatrix<T> m) {

    // safety check
    checkMultiplicationCompatible(m);

    final BlockFieldMatrix<T> out = new BlockFieldMatrix<T>(getField(), rows, m.columns);
    final T zero = getField().getZero();

    // perform multiplication block-wise, to ensure good cache behavior
    int blockIndex = 0;
    for (int iBlock = 0; iBlock < out.blockRows; ++iBlock) {

        final int pStart = iBlock * BLOCK_SIZE;
        final int pEnd   = FastMath.min(pStart + BLOCK_SIZE, rows);

        for (int jBlock = 0; jBlock < out.blockColumns; ++jBlock) {
            final int jWidth = out.blockWidth(jBlock);
            final int jWidth2 = jWidth  + jWidth;
            final int jWidth3 = jWidth2 + jWidth;
            final int jWidth4 = jWidth3 + jWidth;

            // select current block
            final T[] outBlock = out.blocks[blockIndex];

            // perform multiplication on current block
            for (int kBlock = 0; kBlock < blockColumns; ++kBlock) {
                final int kWidth = blockWidth(kBlock);
                final T[] tBlock = blocks[iBlock * blockColumns + kBlock];
                final T[] mBlock = m.blocks[kBlock * m.blockColumns + jBlock];
                int k = 0;
                for (int p = pStart; p < pEnd; ++p) {
                    final int lStart = (p - pStart) * kWidth;
                    final int lEnd   = lStart + kWidth;
                    for (int nStart = 0; nStart < jWidth; ++nStart) {
                        T sum = zero;
                        int l = lStart;
                        int n = nStart;
                        while (l < lEnd - 3) {
                            sum = sum.
                                  add(tBlock[l].multiply(mBlock[n])).
                                  add(tBlock[l + 1].multiply(mBlock[n + jWidth])).
                                  add(tBlock[l + 2].multiply(mBlock[n + jWidth2])).
                                  add(tBlock[l + 3].multiply(mBlock[n + jWidth3]));
                            l += 4;
                            n += jWidth4;
                        }
                        while (l < lEnd) {
                            sum = sum.add(tBlock[l++].multiply(mBlock[n]));
                            n += jWidth;
                        }
                        outBlock[k] = outBlock[k].add(sum);
                        ++k;
                    }
                }
            }

            // go to next block
            ++blockIndex;
        }
    }

    return out;
}
 

/** {@inheritDoc} */
@Override
public void setSubMatrix(final double[][] subMatrix, final int row, final int column)
    throws MatrixIndexException {

    // safety checks
    final int refLength = subMatrix[0].length;
    if (refLength < 1) {
        throw MathRuntimeException.createIllegalArgumentException(LocalizedFormats.AT_LEAST_ONE_COLUMN);
    }
    final int endRow    = row + subMatrix.length - 1;
    final int endColumn = column + refLength - 1;
    MatrixUtils.checkSubMatrixIndex(this, row, endRow, column, endColumn);
    for (final double[] subRow : subMatrix) {
        if (subRow.length != refLength) {
            throw MathRuntimeException.createIllegalArgumentException(
                    LocalizedFormats.DIFFERENT_ROWS_LENGTHS,
                    refLength, subRow.length);
        }
    }

    // compute blocks bounds
    final int blockStartRow    = row / BLOCK_SIZE;
    final int blockEndRow      = (endRow + BLOCK_SIZE) / BLOCK_SIZE;
    final int blockStartColumn = column / BLOCK_SIZE;
    final int blockEndColumn   = (endColumn + BLOCK_SIZE) / BLOCK_SIZE;

    // perform copy block-wise, to ensure good cache behavior
    for (int iBlock = blockStartRow; iBlock < blockEndRow; ++iBlock) {
        final int iHeight  = blockHeight(iBlock);
        final int firstRow = iBlock * BLOCK_SIZE;
        final int iStart   = FastMath.max(row,    firstRow);
        final int iEnd     = FastMath.min(endRow + 1, firstRow + iHeight);

        for (int jBlock = blockStartColumn; jBlock < blockEndColumn; ++jBlock) {
            final int jWidth      = blockWidth(jBlock);
            final int firstColumn = jBlock * BLOCK_SIZE;
            final int jStart      = FastMath.max(column,    firstColumn);
            final int jEnd        = FastMath.min(endColumn + 1, firstColumn + jWidth);
            final int jLength     = jEnd - jStart;

            // handle one block, row by row
            final double[] block = blocks[iBlock * blockColumns + jBlock];
            for (int i = iStart; i < iEnd; ++i) {
                System.arraycopy(subMatrix[i - row], jStart - column,
                                 block, (i - firstRow) * jWidth + (jStart - firstColumn),
                                 jLength);
            }

        }
    }
}
 

/**
 * Returns the result of postmultiplying this by {@code m}.
 *
 * @param m Matrix to postmultiply by.
 * @return {@code this} * m.
 * @throws MatrixDimensionMismatchException if the matrices are not
 * compatible.
 */
public BlockRealMatrix multiply(BlockRealMatrix m) {
    // safety check
    MatrixUtils.checkMultiplicationCompatible(this, m);

    final BlockRealMatrix out = new BlockRealMatrix(rows, m.columns);

    // perform multiplication block-wise, to ensure good cache behavior
    int blockIndex = 0;
    for (int iBlock = 0; iBlock < out.blockRows; ++iBlock) {

        final int pStart = iBlock * BLOCK_SIZE;
        final int pEnd = FastMath.min(pStart + BLOCK_SIZE, rows);

        for (int jBlock = 0; jBlock < out.blockColumns; ++jBlock) {
            final int jWidth = out.blockWidth(jBlock);
            final int jWidth2 = jWidth  + jWidth;
            final int jWidth3 = jWidth2 + jWidth;
            final int jWidth4 = jWidth3 + jWidth;

            // select current block
            final double[] outBlock = out.blocks[blockIndex];

            // perform multiplication on current block
            for (int kBlock = 0; kBlock < blockColumns; ++kBlock) {
                final int kWidth = blockWidth(kBlock);
                final double[] tBlock = blocks[iBlock * blockColumns + kBlock];
                final double[] mBlock = m.blocks[kBlock * m.blockColumns + jBlock];
                int k = 0;
                for (int p = pStart; p < pEnd; ++p) {
                    final int lStart = (p - pStart) * kWidth;
                    final int lEnd = lStart + kWidth;
                    for (int nStart = 0; nStart < jWidth; ++nStart) {
                        double sum = 0;
                        int l = lStart;
                        int n = nStart;
                        while (l < lEnd - 3) {
                            sum += tBlock[l] * mBlock[n] +
                                   tBlock[l + 1] * mBlock[n + jWidth] +
                                   tBlock[l + 2] * mBlock[n + jWidth2] +
                                   tBlock[l + 3] * mBlock[n + jWidth3];
                            l += 4;
                            n += jWidth4;
                        }
                        while (l < lEnd) {
                            sum += tBlock[l++] * mBlock[n];
                            n += jWidth;
                        }
                        outBlock[k] += sum;
                        ++k;
                    }
                }
            }
            // go to next block
            ++blockIndex;
        }
    }

    return out;
}
 

/** Initialize the integration step.
 * @param equations differential equations set
 * @param forward forward integration indicator
 * @param order order of the method
 * @param scale scaling vector for the state vector (can be shorter than state vector)
 * @param t0 start time
 * @param y0 state vector at t0
 * @param yDot0 first time derivative of y0
 * @param y1 work array for a state vector
 * @param yDot1 work array for the first time derivative of y1
 * @return first integration step
 * @exception DerivativeException this exception is propagated to
 * the caller if the underlying user function triggers one
 */
public double initializeStep(final FirstOrderDifferentialEquations equations,
                             final boolean forward, final int order, final double[] scale,
                             final double t0, final double[] y0, final double[] yDot0,
                             final double[] y1, final double[] yDot1)
    throws DerivativeException {

  if (initialStep > 0) {
    // use the user provided value
    return forward ? initialStep : -initialStep;
  }

  // very rough first guess : h = 0.01 * ||y/scale|| / ||y'/scale||
  // this guess will be used to perform an Euler step
  double ratio;
  double yOnScale2 = 0;
  double yDotOnScale2 = 0;
  for (int j = 0; j < scale.length; ++j) {
    ratio         = y0[j] / scale[j];
    yOnScale2    += ratio * ratio;
    ratio         = yDot0[j] / scale[j];
    yDotOnScale2 += ratio * ratio;
  }

  double h = ((yOnScale2 < 1.0e-10) || (yDotOnScale2 < 1.0e-10)) ?
             1.0e-6 : (0.01 * FastMath.sqrt(yOnScale2 / yDotOnScale2));
  if (! forward) {
    h = -h;
  }

  // perform an Euler step using the preceding rough guess
  for (int j = 0; j < y0.length; ++j) {
    y1[j] = y0[j] + h * yDot0[j];
  }
  computeDerivatives(t0 + h, y1, yDot1);

  // estimate the second derivative of the solution
  double yDDotOnScale = 0;
  for (int j = 0; j < scale.length; ++j) {
    ratio         = (yDot1[j] - yDot0[j]) / scale[j];
    yDDotOnScale += ratio * ratio;
  }
  yDDotOnScale = FastMath.sqrt(yDDotOnScale) / h;

  // step size is computed such that
  // h^order * max (||y'/tol||, ||y''/tol||) = 0.01
  final double maxInv2 = FastMath.max(FastMath.sqrt(yDotOnScale2), yDDotOnScale);
  final double h1 = (maxInv2 < 1.0e-15) ?
                    FastMath.max(1.0e-6, 0.001 * FastMath.abs(h)) :
                    FastMath.pow(0.01 / maxInv2, 1.0 / order);
  h = FastMath.min(100.0 * FastMath.abs(h), h1);
  h = FastMath.max(h, 1.0e-12 * FastMath.abs(t0));  // avoids cancellation when computing t1 - t0
  if (h < getMinStep()) {
    h = getMinStep();
  }
  if (h > getMaxStep()) {
    h = getMaxStep();
  }
  if (! forward) {
    h = -h;
  }

  return h;

}
 

/** Set the time of the interpolated point.
 * <p>This method should <strong>not</strong> be called before the
 * integration is over because some internal variables are set only
 * once the last step has been handled.</p>
 * <p>Setting the time outside of the integration interval is now
 * allowed (it was not allowed up to version 5.9 of Mantissa), but
 * should be used with care since the accuracy of the interpolator
 * will probably be very poor far from this interval. This allowance
 * has been added to simplify implementation of search algorithms
 * near the interval endpoints.</p>
 * @param time time of the interpolated point
 */
public void setInterpolatedTime(final double time) {

    // initialize the search with the complete steps table
    int iMin = 0;
    final StepInterpolator sMin = steps.get(iMin);
    double tMin = 0.5 * (sMin.getPreviousTime() + sMin.getCurrentTime());

    int iMax = steps.size() - 1;
    final StepInterpolator sMax = steps.get(iMax);
    double tMax = 0.5 * (sMax.getPreviousTime() + sMax.getCurrentTime());

    // handle points outside of the integration interval
    // or in the first and last step
    if (locatePoint(time, sMin) <= 0) {
      index = iMin;
      sMin.setInterpolatedTime(time);
      return;
    }
    if (locatePoint(time, sMax) >= 0) {
      index = iMax;
      sMax.setInterpolatedTime(time);
      return;
    }

    // reduction of the table slice size
    while (iMax - iMin > 5) {

      // use the last estimated index as the splitting index
      final StepInterpolator si = steps.get(index);
      final int location = locatePoint(time, si);
      if (location < 0) {
        iMax = index;
        tMax = 0.5 * (si.getPreviousTime() + si.getCurrentTime());
      } else if (location > 0) {
        iMin = index;
        tMin = 0.5 * (si.getPreviousTime() + si.getCurrentTime());
      } else {
        // we have found the target step, no need to continue searching
        si.setInterpolatedTime(time);
        return;
      }

      // compute a new estimate of the index in the reduced table slice
      final int iMed = (iMin + iMax) / 2;
      final StepInterpolator sMed = steps.get(iMed);
      final double tMed = 0.5 * (sMed.getPreviousTime() + sMed.getCurrentTime());

      if ((FastMath.abs(tMed - tMin) < 1e-6) || (FastMath.abs(tMax - tMed) < 1e-6)) {
        // too close to the bounds, we estimate using a simple dichotomy
        index = iMed;
      } else {
        // estimate the index using a reverse quadratic polynom
        // (reverse means we have i = P(t), thus allowing to simply
        // compute index = P(time) rather than solving a quadratic equation)
        final double d12 = tMax - tMed;
        final double d23 = tMed - tMin;
        final double d13 = tMax - tMin;
        final double dt1 = time - tMax;
        final double dt2 = time - tMed;
        final double dt3 = time - tMin;
        final double iLagrange = ((dt2 * dt3 * d23) * iMax -
                                  (dt1 * dt3 * d13) * iMed +
                                  (dt1 * dt2 * d12) * iMin) /
                                 (d12 * d23 * d13);
        index = (int) FastMath.rint(iLagrange);
      }

      // force the next size reduction to be at least one tenth
      final int low  = FastMath.max(iMin + 1, (9 * iMin + iMax) / 10);
      final int high = FastMath.min(iMax - 1, (iMin + 9 * iMax) / 10);
      if (index < low) {
        index = low;
      } else if (index > high) {
        index = high;
      }

    }

    // now the table slice is very small, we perform an iterative search
    index = iMin;
    while ((index <= iMax) && (locatePoint(time, steps.get(index)) > 0)) {
      ++index;
    }

    steps.get(index).setInterpolatedTime(time);

}