DenseVector ConstraintBSpline::eval(const DenseVector &x) const
{
// Only x-variables (B-spline input variables) are adjusted to bounds
DenseVector xy = x;
DenseVector xa = adjustToDomainBounds(x);
DenseVector xx = xa.block(0,0,variables.size()-numConstraints,1);
DenseVector yy = xy.block(variables.size()-numConstraints,0,numConstraints,1);
double by = bspline.eval(xx);
DenseVector y = DenseVector::Zero(numConstraints);
y(0) = by - yy(0);
// adjustToDomainBounds(x);
// y.resize(numConstraints);
// VecD xx = x.block(0,0,variables.size()-numConstraints,1);
// VecD yy = x.block(variables.size()-numConstraints,0,numConstraints,1);
// VecD by = bspline->evaluate(xx);
// y = by - yy;
return y;
}
// Old implementation of Jacobian
DenseMatrix BSplineBasis::evalBasisJacobianOld(DenseVector &x) const
{
// Jacobian basis matrix
DenseMatrix J; J.setZero(getNumBasisFunctions(), numVariables);
// Calculate partial derivatives
for (unsigned int i = 0; i < numVariables; i++)
{
// One column in basis jacobian
DenseVector bi; bi.setOnes(1);
for (unsigned int j = 0; j < numVariables; j++)
{
DenseVector temp = bi;
DenseVector xi;
if (j == i)
{
// Differentiated basis
xi = bases.at(j).evaluateFirstDerivative(x(j));
}
else
{
// Normal basis
xi = bases.at(j).evaluate(x(j));
}
bi = kroneckerProduct(temp, xi);
}
// Fill out column
J.block(0,i,bi.rows(),1) = bi.block(0,0,bi.rows(),1);
}
return J;
}
DenseVector ConstraintBSpline::evalHessian(const DenseVector &x) const
{
DenseVector xa = adjustToDomainBounds(x);
DenseVector ddx = DenseVector::Zero(nnzHessian);
// Get x-values
DenseVector xx = xa.block(0,0,bspline.getNumVariables(),1);
// Calculate Hessian
DenseMatrix H = bspline.evalHessian(xx);
// H is symmetric so fill out lower left triangle only
int idx = 0;
for (int row = 0; row < H.rows(); row++)
{
for (int col = 0; col <= row; col++)
{
//if (H(row,col) != 0)
//{
ddx(idx++) = H(row,col);
//}
}
}
return ddx;
}
DenseVector ConstraintBSpline::evalJacobian(const DenseVector &x) const
{
DenseVector xa = adjustToDomainBounds(x);
DenseVector dx = DenseVector::Zero(nnzJacobian);
//return centralDifference(xa);
// Get x-values
DenseVector xx = xa.block(0,0,bspline.getNumVariables(),1);
// Evaluate B-spline Jacobian
DenseMatrix jac = bspline.evalJacobian(xx);
// Derivatives on inputs x
int k = 0;
for (int i = 0; i < jac.rows(); i++)
{
for (int j = 0; j < jac.cols(); j++)
{
dx(k++) = jac(i,j);
}
}
// Derivatives on outputs y
for (unsigned int i = 0; i < numConstraints; i++)
{
dx(k++) = -1;
}
return dx;
}