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);
}
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;
}