LieAlgebra MinEnFilter::computeGradient(const LieGroup& S, const MatrixXd& Gk, const MatrixXd& Disps_inhom) {
/* DO NOT CHANGE THIS FUNCTION */
LieAlgebra L;
MatrixXd iEg = ((S.E).lu().solve(Gk.transpose())).transpose();
VectorXd kappa = iEg.col(2);
MatrixXd h = iEg.leftCols(2).cwiseQuotient(kappa*MatrixXd::Ones(1,2));
MatrixXd z = Disps_inhom.leftCols(2) - h;
MatrixXd Qvec(1,2);
Qvec << q1/nPoints,q2/nPoints;
MatrixXd qvec = MatrixXd::Ones(nPoints,1) * Qvec;
MatrixXd b12 = (kappa.cwiseInverse()*MatrixXd::Ones(1,2)).cwiseProduct(qvec.cwiseProduct(z));
MatrixXd kappa2 = kappa.cwiseProduct(kappa);
MatrixXd aux1 = (iEg.leftCols(2)).cwiseProduct(z.cwiseProduct(qvec));
MatrixXd b3 = -((kappa2.cwiseInverse()*MatrixXd::Ones(1,2)).cwiseProduct(aux1)).rowwise().sum();
MatrixXd B(nPoints,4);
B << b12, b3, MatrixXd::Zero(nPoints,1);
MatrixXd Ones = MatrixXd::Ones(1,4);
MatrixXd A1 = iEg.col(0) * Ones;
MatrixXd A2 = iEg.col(1) * Ones;
MatrixXd A3 = iEg.col(2) * Ones;
MatrixXd A4 = iEg.col(3) * Ones;
MatrixXd G1 = (B.cwiseProduct(A1)).colwise().sum();
MatrixXd G2 = (B.cwiseProduct(A2)).colwise().sum();
MatrixXd G3 = (B.cwiseProduct(A3)).colwise().sum();
MatrixXd G4 = (B.cwiseProduct(A4)).colwise().sum();
Matrix4d G;
G << G1,
G2,
G3,
G4;
G = MEFcommon::prSE(G.transpose());
if (order == 1) {
L = LieAlgebra(G);
} else if (order >=2) {
L = LieAlgebra(G, VectorXd::Zero((order-1)*6),order);
} else {
cerr << "Parameter order must be greater than zero." << endl;
exit(1);
}
return L;
}
void VectorDomainLFIntegrator::AssembleRHSElementVect(
const FiniteElement &el, ElementTransformation &Tr, Vector &elvect)
{
int vdim = Q.GetVDim();
int dof = el.GetDof();
double val,cf;
shape.SetSize(dof); // vector of size dof
elvect.SetSize(dof * vdim);
elvect = 0.0;
const IntegrationRule *ir = IntRule;
if (ir == NULL)
{
int intorder = el.GetOrder() + 1;
ir = &IntRules.Get(el.GetGeomType(), intorder);
}
for (int i = 0; i < ir->GetNPoints(); i++)
{
const IntegrationPoint &ip = ir->IntPoint(i);
Tr.SetIntPoint (&ip);
val = Tr.Weight();
el.CalcShape(ip, shape);
Q.Eval (Qvec, Tr, ip);
for (int k = 0; k < vdim; k++)
{
cf = val * Qvec(k);
for (int s = 0; s < dof; s++)
elvect(dof*k+s) += ip.weight * cf * shape(s);
}
}
}
