358 lines
11 KiB
C++
358 lines
11 KiB
C++
/***************************************************************************
|
|
* Copyright (c) 2011 Konstantinos Poulios <logari81@gmail.com> *
|
|
* *
|
|
* 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 *
|
|
* *
|
|
***************************************************************************/
|
|
|
|
#ifdef _MSC_VER
|
|
#pragma warning(disable : 4251)
|
|
#endif
|
|
|
|
#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.empty()) {
|
|
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(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()) {
|
|
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
|