Skip to content
Permalink
Branch: master
Find file Copy path
Find file Copy path
Fetching contributors…
Cannot retrieve contributors at this time
356 lines (311 sloc) 10.9 KB
/***************************************************************************
* Copyright (c) Konstantinos Poulios (logari81@gmail.com) 2011 *
* *
* This file is part of the FreeCAD CAx development system. *
* *
* This library is free software; you can redistribute it and/or *
* modify it under the terms of the GNU Library General Public *
* License as published by the Free Software Foundation; either *
* version 2 of the License, or (at your option) any later version. *
* *
* This library is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
* GNU Library General Public License for more details. *
* *
* You should have received a copy of the GNU Library General Public *
* License along with this library; see the file COPYING.LIB. If not, *
* write to the Free Software Foundation, Inc., 59 Temple Place, *
* Suite 330, Boston, MA 02111-1307, USA *
* *
***************************************************************************/
#include <iostream>
#include <iterator>
#include "SubSystem.h"
namespace GCS
{
// SubSystem
SubSystem::SubSystem(std::vector<Constraint *> &clist_, VEC_pD &params)
: clist(clist_)
{
MAP_pD_pD dummymap;
initialize(params, dummymap);
}
SubSystem::SubSystem(std::vector<Constraint *> &clist_, VEC_pD &params,
MAP_pD_pD &reductionmap)
: clist(clist_)
{
initialize(params, reductionmap);
}
SubSystem::~SubSystem()
{
}
void SubSystem::initialize(VEC_pD &params, MAP_pD_pD &reductionmap)
{
csize = static_cast<int>(clist.size());
// tmpplist will contain the subset of parameters from params that are
// relevant for the constraints listed in clist
VEC_pD tmpplist;
{
SET_pD s1(params.begin(), params.end());
SET_pD s2;
for (std::vector<Constraint *>::iterator constr=clist.begin();
constr != clist.end(); ++constr) {
(*constr)->revertParams(); // ensure that the constraint points to the original parameters
VEC_pD constr_params = (*constr)->params();
s2.insert(constr_params.begin(), constr_params.end());
}
std::set_intersection(s1.begin(), s1.end(), s2.begin(), s2.end(),
std::back_inserter(tmpplist) );
}
plist.clear();
MAP_pD_I rindex;
if (reductionmap.size() > 0) {
int i=0;
MAP_pD_I pindex;
for (VEC_pD::const_iterator itt=tmpplist.begin();
itt != tmpplist.end(); ++itt) {
MAP_pD_pD::const_iterator itr = reductionmap.find(*itt);
if (itr != reductionmap.end()) {
MAP_pD_I::const_iterator itp = pindex.find(itr->second);
if (itp == pindex.end()) { // the reduction target is not in plist yet, so add it now
plist.push_back(itr->second);
rindex[itr->first] = i;
pindex[itr->second] = i;
i++;
}
else // the reduction target is already in plist, just inform rindex
rindex[itr->first] = itp->second;
}
else if (pindex.find(*itt) == pindex.end()) { // not in plist yet, so add it now
plist.push_back(*itt);
pindex[*itt] = i;
i++;
}
}
}
else
plist = tmpplist;
psize = static_cast<int>(plist.size());
pvals.resize(psize);
pmap.clear();
for (int j=0; j < psize; j++) {
pmap[plist[j]] = &pvals[j];
pvals[j] = *plist[j];
}
for (MAP_pD_I::const_iterator itr=rindex.begin(); itr != rindex.end(); ++itr)
pmap[itr->first] = &pvals[itr->second];
c2p.clear();
p2c.clear();
for (std::vector<Constraint *>::iterator constr=clist.begin();
constr != clist.end(); ++constr) {
(*constr)->revertParams(); // ensure that the constraint points to the original parameters
VEC_pD constr_params_orig = (*constr)->params();
SET_pD constr_params;
for (VEC_pD::const_iterator p=constr_params_orig.begin();
p != constr_params_orig.end(); ++p) {
MAP_pD_pD::const_iterator pmapfind = pmap.find(*p);
if (pmapfind != pmap.end())
constr_params.insert(pmapfind->second);
}
for (SET_pD::const_iterator p=constr_params.begin();
p != constr_params.end(); ++p) {
// jacobi.set(*constr, *p, 0.);
c2p[*constr].push_back(*p);
p2c[*p].push_back(*constr);
}
// (*constr)->redirectParams(pmap); // redirect parameters to pvec
}
}
void SubSystem::redirectParams()
{
// copying values to pvals
for (MAP_pD_pD::const_iterator p=pmap.begin();
p != pmap.end(); ++p)
*(p->second) = *(p->first);
// redirect constraints to point to pvals
for (std::vector<Constraint *>::iterator constr=clist.begin();
constr != clist.end(); ++constr) {
(*constr)->revertParams(); // this line will normally not be necessary
(*constr)->redirectParams(pmap);
}
}
void SubSystem::revertParams()
{
for (std::vector<Constraint *>::iterator constr=clist.begin();
constr != clist.end(); ++constr)
(*constr)->revertParams();
}
void SubSystem::getParamMap(MAP_pD_pD &pmapOut)
{
pmapOut = pmap;
}
void SubSystem::getParamList(VEC_pD &plistOut)
{
plistOut = plist;
}
void SubSystem::getParams(VEC_pD &params, Eigen::VectorXd &xOut)
{
if (xOut.size() != int(params.size()))
xOut.setZero(params.size());
for (int j=0; j < int(params.size()); j++) {
MAP_pD_pD::const_iterator
pmapfind = pmap.find(params[j]);
if (pmapfind != pmap.end())
xOut[j] = *(pmapfind->second);
}
}
void SubSystem::getParams(Eigen::VectorXd &xOut)
{
if (xOut.size() != psize)
xOut.setZero(psize);
for (int i=0; i < psize; i++)
xOut[i] = pvals[i];
}
void SubSystem::setParams(VEC_pD &params, Eigen::VectorXd &xIn)
{
assert(xIn.size() == int(params.size()));
for (int j=0; j < int(params.size()); j++) {
MAP_pD_pD::const_iterator
pmapfind = pmap.find(params[j]);
if (pmapfind != pmap.end())
*(pmapfind->second) = xIn[j];
}
}
void SubSystem::setParams(Eigen::VectorXd &xIn)
{
assert(xIn.size() == psize);
for (int i=0; i < psize; i++)
pvals[i] = xIn[i];
}
void SubSystem::getConstraintList(std::vector<Constraint *> &clist_)
{
clist_= clist;
}
double SubSystem::error()
{
double err = 0.;
for (std::vector<Constraint *>::const_iterator constr=clist.begin();
constr != clist.end(); ++constr) {
double tmp = (*constr)->error();
err += tmp*tmp;
}
err *= 0.5;
return err;
}
void SubSystem::calcResidual(Eigen::VectorXd &r)
{
assert(r.size() == csize);
int i=0;
for (std::vector<Constraint *>::const_iterator constr=clist.begin();
constr != clist.end(); ++constr, i++) {
r[i] = (*constr)->error();
}
}
void SubSystem::calcResidual(Eigen::VectorXd &r, double &err)
{
assert(r.size() == csize);
int i=0;
err = 0.;
for (std::vector<Constraint *>::const_iterator constr=clist.begin();
constr != clist.end(); ++constr, i++) {
r[i] = (*constr)->error();
err += r[i]*r[i];
}
err *= 0.5;
}
/*
void SubSystem::calcJacobi()
{
assert(grad.size() != xsize);
for (MAP_pD_pD::const_iterator param=pmap.begin();
param != pmap.end(); ++param) {
// assert(p2c.find(param->second) != p2c.end());
std::vector<Constraint *> constrs=p2c[param->second];
for (std::vector<Constraint *>::const_iterator constr = constrs.begin();
constr != constrs.end(); ++constr)
jacobi.set(*constr,param->second,(*constr)->grad(param->second));
}
}
*/
void SubSystem::calcJacobi(VEC_pD &params, Eigen::MatrixXd &jacobi)
{
jacobi.setZero(csize, params.size());
for (int j=0; j < int(params.size()); j++) {
MAP_pD_pD::const_iterator
pmapfind = pmap.find(params[j]);
if (pmapfind != pmap.end())
for (int i=0; i < csize; i++)
jacobi(i,j) = clist[i]->grad(pmapfind->second);
}
}
void SubSystem::calcJacobi(Eigen::MatrixXd &jacobi)
{
calcJacobi(plist, jacobi);
}
void SubSystem::calcGrad(VEC_pD &params, Eigen::VectorXd &grad)
{
assert(grad.size() == int(params.size()));
grad.setZero();
for (int j=0; j < int(params.size()); j++) {
MAP_pD_pD::const_iterator
pmapfind = pmap.find(params[j]);
if (pmapfind != pmap.end()) {
// assert(p2c.find(pmapfind->second) != p2c.end());
std::vector<Constraint *> constrs=p2c[pmapfind->second];
for (std::vector<Constraint *>::const_iterator constr = constrs.begin();
constr != constrs.end(); ++constr)
grad[j] += (*constr)->error() * (*constr)->grad(pmapfind->second);
}
}
}
void SubSystem::calcGrad(Eigen::VectorXd &grad)
{
calcGrad(plist, grad);
}
double SubSystem::maxStep(VEC_pD &params, Eigen::VectorXd &xdir)
{
assert(xdir.size() == int(params.size()));
MAP_pD_D dir;
for (int j=0; j < int(params.size()); j++) {
MAP_pD_pD::const_iterator pmapfind = pmap.find(params[j]);
if (pmapfind != pmap.end())
dir[pmapfind->second] = xdir[j];
}
double alpha=1e10;
for (std::vector<Constraint *>::iterator constr=clist.begin();
constr != clist.end(); ++constr)
alpha = (*constr)->maxStep(dir, alpha);
return alpha;
}
double SubSystem::maxStep(Eigen::VectorXd &xdir)
{
return maxStep(plist, xdir);
}
void SubSystem::applySolution()
{
for (MAP_pD_pD::const_iterator it=pmap.begin();
it != pmap.end(); ++it)
*(it->first) = *(it->second);
}
void SubSystem::analyse(Eigen::MatrixXd & /*J*/, Eigen::MatrixXd & /*ker*/, Eigen::MatrixXd & /*img*/)
{
}
void SubSystem::report()
{
}
void SubSystem::printResidual()
{
Eigen::VectorXd r(csize);
int i=0;
double err = 0.;
for (std::vector<Constraint *>::const_iterator constr=clist.begin();
constr != clist.end(); ++constr, i++) {
r[i] = (*constr)->error();
err += r[i]*r[i];
}
err *= 0.5;
std::cout << "Residual r = " << r.transpose() << std::endl;
std::cout << "Residual err = " << err << std::endl;
}
} //namespace GCS
You can’t perform that action at this time.