ExponentialPlusPiecewisePolynomial<CoefficientType>::ExponentialPlusPiecewisePolynomial(const PiecewisePolynomial<CoefficientType>& piecewise_polynomial_part) :
    PiecewiseFunction(piecewise_polynomial_part),
    K(Matrix<CoefficientType, Dynamic, Dynamic>::Zero(piecewise_polynomial_part.rows(), 1)),
    A(Matrix<CoefficientType, Dynamic, Dynamic>::Zero(1, 1)),
    alpha(Matrix<CoefficientType, Dynamic, Dynamic>::Zero(1, piecewise_polynomial_part.getNumberOfSegments())),
    piecewise_polynomial_part(piecewise_polynomial_part)
{
  assert(piecewise_polynomial_part.cols() == 1);
}
Beispiel #2
0
bool PiecewisePolynomial<CoefficientType>::isApprox(
    const PiecewisePolynomial<CoefficientType>& other, double tol) const {
    if (rows() != other.rows() || cols() != other.cols()) return false;

    if (!segmentTimesEqual(other, tol)) return false;

    for (int segment_index = 0; segment_index < getNumberOfSegments();
            segment_index++) {
        const PolynomialMatrix& matrix = polynomials_[segment_index];
        const PolynomialMatrix& other_matrix = other.polynomials_[segment_index];
        for (Eigen::Index row = 0; row < rows(); row++) {
            for (Eigen::Index col = 0; col < cols(); col++) {
                if (!matrix(row, col).isApprox(other_matrix(row, col), tol))
                    return false;
            }
        }
    }
    return true;
}