kindred/create
1
1
Fork 0
Code Issues 53 Pull Requests Actions Packages Projects 9 Releases 2 Wiki Activity
Files
e4d304f9344d9223cf49ac5ea7c88899f9316ea2
create/src/Mod/Sketcher/App/planegcs/GCS.cpp
Ladislav Michl e4d304f934 Base: Implement TimeElapsed 
Some instances of TimeInfo serve the sole purpose of measuring time
duration. Using system time is unfortunate as it returns wall clock,
which is not guaranteed to be monotonic. Replace such a usage with
the new TimeElapsed class based on steady clock.
2024-03-05 12:29:24 +01:00

5968 lines
250 KiB
C++
Raw Blame History

/***************************************************************************
* 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)
#pragma warning(disable : 4244)
#pragma warning(disable : 4996)
#endif
#undef _GCS_DEBUG
#undef _GCS_DEBUG_SOLVER_JACOBIAN_QR_DECOMPOSITION_TRIANGULAR_MATRIX
#undef _DEBUG_TO_FILE
// This has to be included BEFORE any EIGEN include
// This format is Sage compatible, so you can just copy/paste the matrix into Sage
#ifdef _GCS_DEBUG
#define EIGEN_DEFAULT_IO_FORMAT Eigen::IOFormat(3, 0, ",", ",\n", "[", "]", "[", "]")
/* Parameters:
*
* StreamPrecision,
* int _flags = 0,
* const std::string & _coeffSeparator = " ",
* const std::string & _rowSeparator = "\n",
* const std::string & _rowPrefix = "",
* const std::string & _rowSuffix = "",
* const std::string & _matPrefix = "",
* const std::string & _matSuffix = "" )*/
#endif
#include <algorithm>
#include <cfloat>
#include <future>
#include <iostream>
#include <limits>
#include "GCS.h"
#include "qp_eq.h"
// NOTE: In CMakeList.txt -DEIGEN_NO_DEBUG is set (it does not work with a define here), to solve
// this: this is needed to fix this SparseQR crash
// https://forum.freecad.org/viewtopic.php?f=10&t=11341&p=92146#p92146, until Eigen library fixes
// its own problem with the assertion (definitely not solved in 3.2.0 branch) NOTE2: solved in
// eigen3.3
// Extraction of Q matrix for Debugging used to crash
#ifdef _GCS_DEBUG_SOLVER_JACOBIAN_QR_DECOMPOSITION_TRIANGULAR_MATRIX
#if EIGEN_VERSION >= 30304
#define SPARSE_Q_MATRIX
#endif
#endif
#if EIGEN_VERSION > 30290 // This regulates that only starting in Eigen 3.3, the problem with
// https://forum.freecad.org/viewtopic.php?f=3&t=4651&start=40
// was solved in Eigen:
// https://forum.freecad.org/viewtopic.php?f=10&t=12769&start=60#p106492
// https://forum.kde.org/viewtopic.php?f=74&t=129439
#define EIGEN_STOCK_FULLPIVLU_COMPUTE
#endif
// #undef EIGEN_SPARSEQR_COMPATIBLE
#ifdef EIGEN_SPARSEQR_COMPATIBLE
#include <Eigen/OrderingMethods>
#endif
// _GCS_EXTRACT_SOLVER_SUBSYSTEM_ to be enabled in Constraints.h when needed.
#if defined(_GCS_EXTRACT_SOLVER_SUBSYSTEM_) || defined(_DEBUG_TO_FILE)
#include <fstream>
#define CASE_NOT_IMP(X) \
case X: { \
subsystemfile << "//" #X "not yet implemented" << std::endl; \
break; \
}
#endif
#include <Base/Console.h>
#include <FCConfig.h>
#include <boost/graph/connected_components.hpp>
#include <boost_graph_adjacency_list.hpp>
using MatrixIndexType = Eigen::FullPivHouseholderQR<Eigen::MatrixXd>::IntDiagSizeVectorType;
#ifndef EIGEN_STOCK_FULLPIVLU_COMPUTE
namespace Eigen
{
using MatrixdType = Matrix<double, -1, -1, 0, -1, -1>;
template<>
FullPivLU<MatrixdType>& FullPivLU<MatrixdType>::compute(const MatrixdType& matrix)
{
m_isInitialized = true;
m_lu = matrix;
const Index size = matrix.diagonalSize();
const Index rows = matrix.rows();
const Index cols = matrix.cols();
// will store the transpositions, before we accumulate them at the end.
// can't accumulate on-the-fly because that will be done in reverse order for the rows.
m_rowsTranspositions.resize(matrix.rows());
m_colsTranspositions.resize(matrix.cols());
// number of NONTRIVIAL transpositions, i.e. m_rowsTranspositions[i]!=i
Index number_of_transpositions = 0;
// the generic case is that in which all pivots are nonzero (invertible case)
m_nonzero_pivots = size;
m_maxpivot = RealScalar(0);
RealScalar cutoff(0);
for (Index k = 0; k < size; ++k) {
// First, we need to find the pivot.
// biggest coefficient in the remaining bottom-right corner (starting at row k, col k)
Index row_of_biggest_in_corner, col_of_biggest_in_corner;
RealScalar biggest_in_corner;
biggest_in_corner = m_lu.bottomRightCorner(rows - k, cols - k)
.cwiseAbs()
.maxCoeff(&row_of_biggest_in_corner, &col_of_biggest_in_corner);
// correct the values! since they were computed in the corner,
row_of_biggest_in_corner += k;
col_of_biggest_in_corner += k; // need to add k to them.
// when k==0, biggest_in_corner is the biggest coeff absolute value in the original
// matrix
if (k == 0) {
cutoff = biggest_in_corner * NumTraits<Scalar>::epsilon();
}
// if the pivot (hence the corner) is "zero", terminate to avoid generating nan/inf
// values. Notice that using an exact comparison (biggest_in_corner==0) here, as
// Golub-van Loan do in their pseudo-code, results in numerical instability! The cutoff
// here has been validated by running the unit test 'lu' with many repetitions.
if (biggest_in_corner < cutoff) {
// before exiting, make sure to initialize the still uninitialized transpositions
// in a sane state without destroying what we already have.
m_nonzero_pivots = k;
for (Index i = k; i < size; ++i) {
m_rowsTranspositions.coeffRef(i) = i;
m_colsTranspositions.coeffRef(i) = i;
}
break;
}
if (biggest_in_corner > m_maxpivot) {
m_maxpivot = biggest_in_corner;
}
// Now that we've found the pivot, we need to apply the row/col swaps to
// bring it to the location (k,k).
m_rowsTranspositions.coeffRef(k) = row_of_biggest_in_corner;
m_colsTranspositions.coeffRef(k) = col_of_biggest_in_corner;
if (k != row_of_biggest_in_corner) {
m_lu.row(k).swap(m_lu.row(row_of_biggest_in_corner));
++number_of_transpositions;
}
if (k != col_of_biggest_in_corner) {
m_lu.col(k).swap(m_lu.col(col_of_biggest_in_corner));
++number_of_transpositions;
}
// Now that the pivot is at the right location, we update the remaining
// bottom-right corner by Gaussian elimination.
if (k < rows - 1) {
m_lu.col(k).tail(rows - k - 1) /= m_lu.coeff(k, k);
}
if (k < size - 1) {
m_lu.block(k + 1, k + 1, rows - k - 1, cols - k - 1).noalias() -=
m_lu.col(k).tail(rows - k - 1) * m_lu.row(k).tail(cols - k - 1);
}
}
// the main loop is over, we still have to accumulate the transpositions to find the
// permutations P and Q
m_p.setIdentity(rows);
for (Index k = size - 1; k >= 0; --k) {
m_p.applyTranspositionOnTheRight(k, m_rowsTranspositions.coeff(k));
}
m_q.setIdentity(cols);
for (Index k = 0; k < size; ++k) {
m_q.applyTranspositionOnTheRight(k, m_colsTranspositions.coeff(k));
}
m_det_pq = (number_of_transpositions % 2) ? -1 : 1;
return *this;
}
} // namespace Eigen
#endif
namespace GCS
{
class SolverReportingManager
{
public:
SolverReportingManager(SolverReportingManager const&) = delete;
SolverReportingManager(SolverReportingManager&&) = delete;
SolverReportingManager& operator=(SolverReportingManager const&) = delete;
SolverReportingManager& operator=(SolverReportingManager&&) = delete;
static SolverReportingManager& Manager();
inline void LogString(const std::string& str);
inline void LogToConsole(const std::string& str);
inline void LogToFile(const std::string& str);
void LogQRSystemInformation(const System& system,
int paramsNum = 0,
int constrNum = 0,
int rank = 0);
void LogGroupOfConstraints(const std::string& str,
std::vector<std::vector<Constraint*>> constraintgroups);
void LogSetOfConstraints(const std::string& str, std::set<Constraint*> constraintset);
void LogGroupOfParameters(const std::string& str,
std::vector<std::vector<double*>> parametergroups);
void LogMatrix(const std::string str, Eigen::MatrixXd matrix);
void LogMatrix(const std::string str, MatrixIndexType matrix);
private:
SolverReportingManager();
~SolverReportingManager();
inline void initStream();
inline void flushStream();
private:
#ifdef _DEBUG_TO_FILE
std::ofstream stream;
#endif
};
SolverReportingManager::SolverReportingManager()
{
initStream();
}
SolverReportingManager::~SolverReportingManager()
{
#ifdef _DEBUG_TO_FILE
stream.flush();
stream.close();
#endif
}
void SolverReportingManager::initStream()
{
#ifdef _DEBUG_TO_FILE
if (!stream.is_open()) {
stream.open("GCS_debug.txt", std::ofstream::out | std::ofstream::app);
}
#endif
}
void SolverReportingManager::flushStream()
{
// Akwardly in some systems flushing does not force the write to the file, requiring a close
#ifdef _DEBUG_TO_FILE
stream.flush();
stream.close();
#endif
}
SolverReportingManager& SolverReportingManager::Manager()
{
static SolverReportingManager theInstance;
return theInstance;
}
void SolverReportingManager::LogToConsole(const std::string& str)
{
Base::Console().Log(str.c_str());
}
void SolverReportingManager::LogToFile(const std::string& str)
{
#ifdef _DEBUG_TO_FILE
initStream();
stream << str << std::endl;
flushStream();
#else
(void)(str); // silence unused parameter
LogToConsole("Debugging to file not enabled!");
#endif
}
void SolverReportingManager::LogString(const std::string& str)
{
LogToConsole(str);
#ifdef _DEBUG_TO_FILE
LogToFile(str);
#endif
}
void SolverReportingManager::LogQRSystemInformation(const System& system,
int paramsNum,
int constrNum,
int rank)
{
std::stringstream tempstream;
tempstream << (system.qrAlgorithm == EigenSparseQR
? "EigenSparseQR"
: (system.qrAlgorithm == EigenDenseQR ? "DenseQR" : ""));
if (paramsNum > 0) {
tempstream
#ifdef EIGEN_SPARSEQR_COMPATIBLE
<< ", Threads: " << Eigen::nbThreads()
#endif
#ifdef EIGEN_VECTORIZE
<< ", Vectorization: On"
#endif
<< ", Pivot Threshold: " << system.qrpivotThreshold << ", Params: " << paramsNum
<< ", Constr: " << constrNum << ", Rank: " << rank << std::endl;
}
else {
tempstream
#ifdef EIGEN_SPARSEQR_COMPATIBLE
<< ", Threads: " << Eigen::nbThreads()
#endif
#ifdef EIGEN_VECTORIZE
<< ", Vectorization: On"
#endif
<< ", Empty Sketch, nothing to solve" << std::endl;
}
LogString(tempstream.str());
}
void SolverReportingManager::LogGroupOfConstraints(
const std::string& str,
std::vector<std::vector<Constraint*>> constraintgroups)
{
std::stringstream tempstream;
tempstream << str << ":" << '\n';
for (const auto& group : constraintgroups) {
tempstream << "[";
for (auto c : group) {
tempstream << c->getTag() << " ";
}
tempstream << "]" << '\n';
}
LogString(tempstream.str());
}
void SolverReportingManager::LogSetOfConstraints(const std::string& str,
std::set<Constraint*> constraintset)
{
std::stringstream tempstream;
tempstream << str << ": [";
for (auto c : constraintset) {
tempstream << c->getTag() << " ";
}
tempstream << "]" << '\n';
LogString(tempstream.str());
}
void SolverReportingManager::LogGroupOfParameters(const std::string& str,
std::vector<std::vector<double*>> parametergroups)
{
std::stringstream tempstream;
tempstream << str << ":" << '\n';
for (size_t i = 0; i < parametergroups.size(); i++) {
tempstream << "[";
for (auto p : parametergroups[i]) {
tempstream << std::hex << p << " ";
}
tempstream << "]" << '\n';
}
LogString(tempstream.str());
}
#ifdef _GCS_DEBUG
void SolverReportingManager::LogMatrix(const std::string str, Eigen::MatrixXd matrix)
{
std::stringstream tempstream;
tempstream << '\n' << " " << str << " =" << '\n';
tempstream << "[" << '\n';
tempstream << matrix << '\n';
tempstream << "]" << '\n';
LogString(tempstream.str());
}
void SolverReportingManager::LogMatrix(const std::string str, MatrixIndexType matrix)
{
std::stringstream tempstream;
stream << '\n' << " " << str << " =" << '\n';
stream << "[" << '\n';
stream << matrix << '\n';
stream << "]" << '\n';
LogString(tempstream.str());
}
#endif
using Graph = boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS>;
///////////////////////////////////////
// Solver
///////////////////////////////////////
// System
System::System()
: plist(0)
, pdrivenlist(0)
, pDependentParameters(0)
, clist(0)
, c2p()
, p2c()
, subSystems(0)
, subSystemsAux(0)
, reference(0)
, dofs(0)
, hasUnknowns(false)
, hasDiagnosis(false)
, isInit(false)
, emptyDiagnoseMatrix(true)
, maxIter(100)
, maxIterRedundant(100)
, sketchSizeMultiplier(false)
, sketchSizeMultiplierRedundant(false)
, convergence(1e-10)
, convergenceRedundant(1e-10)
, qrAlgorithm(EigenSparseQR)
, dogLegGaussStep(FullPivLU)
, qrpivotThreshold(1E-13)
, debugMode(Minimal)
, LM_eps(1E-10)
, LM_eps1(1E-80)
, LM_tau(1E-3)
, DL_tolg(1E-80)
, DL_tolx(1E-80)
, DL_tolf(1E-10)
, LM_epsRedundant(1E-10)
, LM_eps1Redundant(1E-80)
, LM_tauRedundant(1E-3)
, DL_tolgRedundant(1E-80)
, DL_tolxRedundant(1E-80)
, DL_tolfRedundant(1E-10)
{
// currently Eigen only supports multithreading for multiplications
// There is no appreciable gain from using more threads
#ifdef EIGEN_SPARSEQR_COMPATIBLE
Eigen::setNbThreads(1);
#endif
}
System::~System()
{
clear();
}
void System::clear()
{
plist.clear();
pdrivenlist.clear();
pIndex.clear();
pDependentParameters.clear();
pDependentParametersGroups.clear();
hasUnknowns = false;
hasDiagnosis = false;
emptyDiagnoseMatrix = true;
redundant.clear();
conflictingTags.clear();
redundantTags.clear();
partiallyRedundantTags.clear();
reference.clear();
clearSubSystems();
free(clist);
c2p.clear();
p2c.clear();
}
void System::invalidatedDiagnosis()
{
hasDiagnosis = false;
pDependentParameters.clear();
pDependentParametersGroups.clear();
}
void System::clearByTag(int tagId)
{
std::vector<Constraint*> constrvec;
for (std::vector<Constraint*>::const_iterator constr = clist.begin(); constr != clist.end();
++constr) {
if ((*constr)->getTag() == tagId) {
constrvec.push_back(*constr);
}
}
for (std::vector<Constraint*>::const_iterator constr = constrvec.begin();
constr != constrvec.end();
++constr) {
removeConstraint(*constr);
}
}
int System::addConstraint(Constraint* constr)
{
isInit = false;
if (constr->getTag() >= 0) { // negatively tagged constraints have no impact
hasDiagnosis = false; // on the diagnosis
}
clist.push_back(constr);
VEC_pD constr_params = constr->params();
for (VEC_pD::const_iterator param = constr_params.begin(); param != constr_params.end();
++param) {
// jacobi.set(constr, *param, 0.);
c2p[constr].push_back(*param);
p2c[*param].push_back(constr);
}
return clist.size() - 1;
}
void System::removeConstraint(Constraint* constr)
{
std::vector<Constraint*>::iterator it;
it = std::find(clist.begin(), clist.end(), constr);
if (it == clist.end()) {
return;
}
clist.erase(it);
if (constr->getTag() >= 0) {
hasDiagnosis = false;
}
clearSubSystems();
VEC_pD constr_params = c2p[constr];
for (VEC_pD::const_iterator param = constr_params.begin(); param != constr_params.end();
++param) {
std::vector<Constraint*>& constraints = p2c[*param];
it = std::find(constraints.begin(), constraints.end(), constr);
constraints.erase(it);
}
c2p.erase(constr);
delete (constr);
}
// basic constraints
int System::addConstraintEqual(double* param1,
double* param2,
int tagId,
bool driving,
Constraint::Alignment internalalignment)
{
Constraint* constr = new ConstraintEqual(param1, param2);
constr->setTag(tagId);
constr->setDriving(driving);
constr->setInternalAlignment(internalalignment);
return addConstraint(constr);
}
int System::addConstraintProportional(double* param1,
double* param2,
double ratio,
int tagId,
bool driving)
{
Constraint* constr = new ConstraintEqual(param1, param2, ratio);
constr->setTag(tagId);
constr->setDriving(driving);
return addConstraint(constr);
}
int System::addConstraintDifference(double* param1,
double* param2,
double* difference,
int tagId,
bool driving)
{
Constraint* constr = new ConstraintDifference(param1, param2, difference);
constr->setTag(tagId);
constr->setDriving(driving);
return addConstraint(constr);
}
int System::addConstraintP2PDistance(Point& p1,
Point& p2,
double* distance,
int tagId,
bool driving)
{
Constraint* constr = new ConstraintP2PDistance(p1, p2, distance);
constr->setTag(tagId);
constr->setDriving(driving);
return addConstraint(constr);
}
int System::addConstraintP2PAngle(Point& p1,
Point& p2,
double* angle,
double incrAngle,
int tagId,
bool driving)
{
Constraint* constr = new ConstraintP2PAngle(p1, p2, angle, incrAngle);
constr->setTag(tagId);
constr->setDriving(driving);
return addConstraint(constr);
}
int System::addConstraintP2PAngle(Point& p1, Point& p2, double* angle, int /*tagId*/, bool driving)
{
return addConstraintP2PAngle(p1, p2, angle, 0., 0, driving);
}
int System::addConstraintP2LDistance(Point& p, Line& l, double* distance, int tagId, bool driving)
{
Constraint* constr = new ConstraintP2LDistance(p, l, distance);
constr->setTag(tagId);
constr->setDriving(driving);
return addConstraint(constr);
}
int System::addConstraintPointOnLine(Point& p, Line& l, int tagId, bool driving)
{
Constraint* constr = new ConstraintPointOnLine(p, l);
constr->setTag(tagId);
constr->setDriving(driving);
return addConstraint(constr);
}
int System::addConstraintPointOnLine(Point& p, Point& lp1, Point& lp2, int tagId, bool driving)
{
Constraint* constr = new ConstraintPointOnLine(p, lp1, lp2);
constr->setTag(tagId);
constr->setDriving(driving);
return addConstraint(constr);
}
int System::addConstraintPointOnPerpBisector(Point& p, Line& l, int tagId, bool driving)
{
Constraint* constr = new ConstraintPointOnPerpBisector(p, l);
constr->setTag(tagId);
constr->setDriving(driving);
return addConstraint(constr);
}
int System::addConstraintPointOnPerpBisector(Point& p,
Point& lp1,
Point& lp2,
int tagId,
bool driving)
{
Constraint* constr = new ConstraintPointOnPerpBisector(p, lp1, lp2);
constr->setTag(tagId);
constr->setDriving(driving);
return addConstraint(constr);
}
int System::addConstraintParallel(Line& l1, Line& l2, int tagId, bool driving)
{
Constraint* constr = new ConstraintParallel(l1, l2);
constr->setTag(tagId);
constr->setDriving(driving);
return addConstraint(constr);
}
int System::addConstraintPerpendicular(Line& l1, Line& l2, int tagId, bool driving)
{
Constraint* constr = new ConstraintPerpendicular(l1, l2);
constr->setTag(tagId);
constr->setDriving(driving);
return addConstraint(constr);
}
int System::addConstraintPerpendicular(Point& l1p1,
Point& l1p2,
Point& l2p1,
Point& l2p2,
int tagId,
bool driving)
{
Constraint* constr = new ConstraintPerpendicular(l1p1, l1p2, l2p1, l2p2);
constr->setTag(tagId);
constr->setDriving(driving);
return addConstraint(constr);
}
int System::addConstraintL2LAngle(Line& l1, Line& l2, double* angle, int tagId, bool driving)
{
Constraint* constr = new ConstraintL2LAngle(l1, l2, angle);
constr->setTag(tagId);
constr->setDriving(driving);
return addConstraint(constr);
}
int System::addConstraintL2LAngle(Point& l1p1,
Point& l1p2,
Point& l2p1,
Point& l2p2,
double* angle,
int tagId,
bool driving)
{
Constraint* constr = new ConstraintL2LAngle(l1p1, l1p2, l2p1, l2p2, angle);
constr->setTag(tagId);
constr->setDriving(driving);
return addConstraint(constr);
}
int System::addConstraintAngleViaPoint(Curve& crv1,
Curve& crv2,
Point& p,
double* angle,
int tagId,
bool driving)
{
Constraint* constr = new ConstraintAngleViaPoint(crv1, crv2, p, angle);
constr->setTag(tagId);
constr->setDriving(driving);
return addConstraint(constr);
}
int System::addConstraintAngleViaTwoPoints(Curve& crv1,
Curve& crv2,
Point& p1,
Point& p2,
double* angle,
int tagId,
bool driving)
{
Constraint* constr = new ConstraintAngleViaTwoPoints(crv1, crv2, p1, p2, angle);
constr->setTag(tagId);
constr->setDriving(driving);
return addConstraint(constr);
}
int System::addConstraintAngleViaPointAndParam(Curve& crv1,
Curve& crv2,
Point& p,
double* cparam,
double* angle,
int tagId,
bool driving)
{
Constraint* constr = new ConstraintAngleViaPointAndParam(crv1, crv2, p, cparam, angle);
constr->setTag(tagId);
constr->setDriving(driving);
return addConstraint(constr);
}
int System::addConstraintAngleViaPointAndTwoParams(Curve& crv1,
Curve& crv2,
Point& p,
double* cparam1,
double* cparam2,
double* angle,
int tagId,
bool driving)
{
Constraint* constr =
new ConstraintAngleViaPointAndTwoParams(crv1, crv2, p, cparam1, cparam2, angle);
constr->setTag(tagId);
constr->setDriving(driving);
return addConstraint(constr);
}
int System::addConstraintMidpointOnLine(Line& l1, Line& l2, int tagId, bool driving)
{
Constraint* constr = new ConstraintMidpointOnLine(l1, l2);
constr->setTag(tagId);
constr->setDriving(driving);
return addConstraint(constr);
}
int System::addConstraintMidpointOnLine(Point& l1p1,
Point& l1p2,
Point& l2p1,
Point& l2p2,
int tagId,
bool driving)
{
Constraint* constr = new ConstraintMidpointOnLine(l1p1, l1p2, l2p1, l2p2);
constr->setTag(tagId);
constr->setDriving(driving);
return addConstraint(constr);
}
int System::addConstraintTangentCircumf(Point& p1,
Point& p2,
double* rad1,
double* rad2,
bool internal,
int tagId,
bool driving)
{
Constraint* constr = new ConstraintTangentCircumf(p1, p2, rad1, rad2, internal);
constr->setTag(tagId);
constr->setDriving(driving);
return addConstraint(constr);
}
int System::addConstraintTangentAtBSplineKnot(BSpline& b,
Line& l,
unsigned int knotindex,
int tagId,
bool driving)
{
Constraint* constr = new ConstraintSlopeAtBSplineKnot(b, l, knotindex);
constr->setTag(tagId);
constr->setDriving(driving);
return addConstraint(constr);
}
int System::addConstraintC2CDistance(Circle& c1, Circle& c2, double* dist, int tagId, bool driving)
{
Constraint* constr = new ConstraintC2CDistance(c1, c2, dist);
constr->setTag(tagId);
constr->setDriving(driving);
return addConstraint(constr);
}
int System::addConstraintC2LDistance(Circle& c, Line& l, double* dist, int tagId, bool driving)
{
Constraint* constr = new ConstraintC2LDistance(c, l, dist);
constr->setTag(tagId);
constr->setDriving(driving);
return addConstraint(constr);
}
int System::addConstraintP2CDistance(Point& p, Circle& c, double* distance, int tagId, bool driving)
{
Constraint* constr = new ConstraintP2CDistance(p, c, distance);
constr->setTag(tagId);
constr->setDriving(driving);
return addConstraint(constr);
}
// derived constraints
int System::addConstraintP2PCoincident(Point& p1, Point& p2, int tagId, bool driving)
{
addConstraintEqual(p1.x, p2.x, tagId, driving);
return addConstraintEqual(p1.y, p2.y, tagId, driving);
}
int System::addConstraintHorizontal(Line& l, int tagId, bool driving)
{
return addConstraintEqual(l.p1.y, l.p2.y, tagId, driving);
}
int System::addConstraintHorizontal(Point& p1, Point& p2, int tagId, bool driving)
{
return addConstraintEqual(p1.y, p2.y, tagId, driving);
}
int System::addConstraintVertical(Line& l, int tagId, bool driving)
{
return addConstraintEqual(l.p1.x, l.p2.x, tagId, driving);
}
int System::addConstraintVertical(Point& p1, Point& p2, int tagId, bool driving)
{
return addConstraintEqual(p1.x, p2.x, tagId, driving);
}
int System::addConstraintCoordinateX(Point& p, double* x, int tagId, bool driving)
{
return addConstraintEqual(p.x, x, tagId, driving);
}
int System::addConstraintCoordinateY(Point& p, double* y, int tagId, bool driving)
{
return addConstraintEqual(p.y, y, tagId, driving);
}
int System::addConstraintArcRules(Arc& a, int tagId, bool driving)
{
addConstraintCurveValue(a.start, a, a.startAngle, tagId, driving);
return addConstraintCurveValue(a.end, a, a.endAngle, tagId, driving);
}
int System::addConstraintPointOnCircle(Point& p, Circle& c, int tagId, bool driving)
{
return addConstraintP2PDistance(p, c.center, c.rad, tagId, driving);
}
int System::addConstraintPointOnEllipse(Point& p, Ellipse& e, int tagId, bool driving)
{
Constraint* constr = new ConstraintPointOnEllipse(p, e);
constr->setTag(tagId);
constr->setDriving(driving);
return addConstraint(constr);
}
int System::addConstraintPointOnHyperbolicArc(Point& p, ArcOfHyperbola& e, int tagId, bool driving)
{
Constraint* constr = new ConstraintPointOnHyperbola(p, e);
constr->setTag(tagId);
constr->setDriving(driving);
return addConstraint(constr);
}
int System::addConstraintPointOnParabolicArc(Point& p, ArcOfParabola& e, int tagId, bool driving)
{
Constraint* constr = new ConstraintPointOnParabola(p, e);
constr->setTag(tagId);
constr->setDriving(driving);
return addConstraint(constr);
}
int System::addConstraintPointOnBSpline(Point& p,
BSpline& b,
double* pointparam,
int tagId,
bool driving)
{
Constraint* constr = new ConstraintPointOnBSpline(p.x, pointparam, 0, b);
constr->setTag(tagId);
constr->setDriving(driving);
addConstraint(constr);
constr = new ConstraintPointOnBSpline(p.y, pointparam, 1, b);
constr->setTag(tagId);
constr->setDriving(driving);
return addConstraint(constr);
}
int System::addConstraintArcOfEllipseRules(ArcOfEllipse& a, int tagId, bool driving)
{
addConstraintCurveValue(a.start, a, a.startAngle, tagId, driving);
return addConstraintCurveValue(a.end, a, a.endAngle, tagId, driving);
}
int System::addConstraintCurveValue(Point& p, Curve& a, double* u, int tagId, bool driving)
{
Constraint* constr = new ConstraintCurveValue(p, p.x, a, u);
constr->setTag(tagId);
constr->setDriving(driving);
addConstraint(constr);
constr = new ConstraintCurveValue(p, p.y, a, u);
constr->setTag(tagId);
constr->setDriving(driving);
return addConstraint(constr);
}
int System::addConstraintArcOfHyperbolaRules(ArcOfHyperbola& a, int tagId, bool driving)
{
addConstraintCurveValue(a.start, a, a.startAngle, tagId, driving);
return addConstraintCurveValue(a.end, a, a.endAngle, tagId, driving);
}
int System::addConstraintArcOfParabolaRules(ArcOfParabola& a, int tagId, bool driving)
{
addConstraintCurveValue(a.start, a, a.startAngle, tagId, driving);
return addConstraintCurveValue(a.end, a, a.endAngle, tagId, driving);
}
int System::addConstraintPointOnArc(Point& p, Arc& a, int tagId, bool driving)
{
return addConstraintP2PDistance(p, a.center, a.rad, tagId, driving);
}
int System::addConstraintPerpendicularLine2Arc(Point& p1,
Point& p2,
Arc& a,
int tagId,
bool driving)
{
addConstraintP2PCoincident(p2, a.start, tagId, driving);
double dx = *(p2.x) - *(p1.x);
double dy = *(p2.y) - *(p1.y);
if (dx * cos(*(a.startAngle)) + dy * sin(*(a.startAngle)) > 0) {
return addConstraintP2PAngle(p1, p2, a.startAngle, 0, tagId, driving);
}
else {
return addConstraintP2PAngle(p1, p2, a.startAngle, pi, tagId, driving);
}
}
int System::addConstraintPerpendicularArc2Line(Arc& a,
Point& p1,
Point& p2,
int tagId,
bool driving)
{
addConstraintP2PCoincident(p1, a.end, tagId, driving);
double dx = *(p2.x) - *(p1.x);
double dy = *(p2.y) - *(p1.y);
if (dx * cos(*(a.endAngle)) + dy * sin(*(a.endAngle)) > 0) {
return addConstraintP2PAngle(p1, p2, a.endAngle, 0, tagId, driving);
}
else {
return addConstraintP2PAngle(p1, p2, a.endAngle, pi, tagId, driving);
}
}
int System::addConstraintPerpendicularCircle2Arc(Point& center,
double* radius,
Arc& a,
int tagId,
bool driving)
{
addConstraintP2PDistance(a.start, center, radius, tagId, driving);
double incrAngle = *(a.startAngle) < *(a.endAngle) ? pi_2 : -pi_2;
double tangAngle = *a.startAngle + incrAngle;
double dx = *(a.start.x) - *(center.x);
double dy = *(a.start.y) - *(center.y);
if (dx * cos(tangAngle) + dy * sin(tangAngle) > 0) {
return addConstraintP2PAngle(center, a.start, a.startAngle, incrAngle, tagId, driving);
}
else {
return addConstraintP2PAngle(center, a.start, a.startAngle, -incrAngle, tagId, driving);
}
}
int System::addConstraintPerpendicularArc2Circle(Arc& a,
Point& center,
double* radius,
int tagId,
bool driving)
{
addConstraintP2PDistance(a.end, center, radius, tagId, driving);
double incrAngle = *(a.startAngle) < *(a.endAngle) ? -pi_2 : pi_2;
double tangAngle = *a.endAngle + incrAngle;
double dx = *(a.end.x) - *(center.x);
double dy = *(a.end.y) - *(center.y);
if (dx * cos(tangAngle) + dy * sin(tangAngle) > 0) {
return addConstraintP2PAngle(center, a.end, a.endAngle, incrAngle, tagId, driving);
}
else {
return addConstraintP2PAngle(center, a.end, a.endAngle, -incrAngle, tagId, driving);
}
}
int System::addConstraintPerpendicularArc2Arc(Arc& a1,
bool reverse1,
Arc& a2,
bool reverse2,
int tagId,
bool driving)
{
Point& p1 = reverse1 ? a1.start : a1.end;
Point& p2 = reverse2 ? a2.end : a2.start;
addConstraintP2PCoincident(p1, p2, tagId, driving);
return addConstraintPerpendicular(a1.center, p1, a2.center, p2, tagId, driving);
}
int System::addConstraintTangent(Line& l, Circle& c, int tagId, bool driving)
{
return addConstraintP2LDistance(c.center, l, c.rad, tagId, driving);
}
int System::addConstraintTangent(Line& l, Ellipse& e, int tagId, bool driving)
{
Constraint* constr = new ConstraintEllipseTangentLine(l, e);
constr->setTag(tagId);
constr->setDriving(driving);
return addConstraint(constr);
}
int System::addConstraintTangent(Line& l, Arc& a, int tagId, bool driving)
{
return addConstraintP2LDistance(a.center, l, a.rad, tagId, driving);
}
int System::addConstraintTangent(Circle& c1, Circle& c2, int tagId, bool driving)
{
double dx = *(c2.center.x) - *(c1.center.x);
double dy = *(c2.center.y) - *(c1.center.y);
double d = sqrt(dx * dx + dy * dy);
return addConstraintTangentCircumf(c1.center,
c2.center,
c1.rad,
c2.rad,
(d < *c1.rad || d < *c2.rad),
tagId,
driving);
}
int System::addConstraintTangent(Arc& a1, Arc& a2, int tagId, bool driving)
{
double dx = *(a2.center.x) - *(a1.center.x);
double dy = *(a2.center.y) - *(a1.center.y);
double d = sqrt(dx * dx + dy * dy);
return addConstraintTangentCircumf(a1.center,
a2.center,
a1.rad,
a2.rad,
(d < *a1.rad || d < *a2.rad),
tagId,
driving);
}
int System::addConstraintTangent(Circle& c, Arc& a, int tagId, bool driving)
{
double dx = *(a.center.x) - *(c.center.x);
double dy = *(a.center.y) - *(c.center.y);
double d = sqrt(dx * dx + dy * dy);
return addConstraintTangentCircumf(c.center,
a.center,
c.rad,
a.rad,
(d < *c.rad || d < *a.rad),
tagId,
driving);
}
int System::addConstraintCircleRadius(Circle& c, double* radius, int tagId, bool driving)
{
return addConstraintEqual(c.rad, radius, tagId, driving);
}
int System::addConstraintArcRadius(Arc& a, double* radius, int tagId, bool driving)
{
return addConstraintEqual(a.rad, radius, tagId, driving);
}
int System::addConstraintCircleDiameter(Circle& c, double* diameter, int tagId, bool driving)
{
return addConstraintProportional(c.rad, diameter, 0.5, tagId, driving);
}
int System::addConstraintArcDiameter(Arc& a, double* diameter, int tagId, bool driving)
{
return addConstraintProportional(a.rad, diameter, 0.5, tagId, driving);
}
int System::addConstraintEqualLength(Line& l1, Line& l2, int tagId, bool driving)
{
Constraint* constr = new ConstraintEqualLineLength(l1, l2);
constr->setTag(tagId);
constr->setDriving(driving);
return addConstraint(constr);
}
int System::addConstraintEqualRadius(Circle& c1, Circle& c2, int tagId, bool driving)
{
return addConstraintEqual(c1.rad, c2.rad, tagId, driving);
}
int System::addConstraintEqualRadii(Ellipse& e1, Ellipse& e2, int tagId, bool driving)
{
addConstraintEqual(e1.radmin, e2.radmin, tagId, driving);
Constraint* constr = new ConstraintEqualMajorAxesConic(&e1, &e2);
constr->setTag(tagId);
constr->setDriving(driving);
return addConstraint(constr);
}
int System::addConstraintEqualRadii(ArcOfHyperbola& a1, ArcOfHyperbola& a2, int tagId, bool driving)
{
addConstraintEqual(a1.radmin, a2.radmin, tagId, driving);
Constraint* constr = new ConstraintEqualMajorAxesConic(&a1, &a2);
constr->setTag(tagId);
constr->setDriving(driving);
return addConstraint(constr);
}
int System::addConstraintEqualFocus(ArcOfParabola& a1, ArcOfParabola& a2, int tagId, bool driving)
{
Constraint* constr = new ConstraintEqualFocalDistance(&a1, &a2);
constr->setTag(tagId);
constr->setDriving(driving);
return addConstraint(constr);
}
int System::addConstraintEqualRadius(Circle& c1, Arc& a2, int tagId, bool driving)
{
return addConstraintEqual(c1.rad, a2.rad, tagId, driving);
}
int System::addConstraintEqualRadius(Arc& a1, Arc& a2, int tagId, bool driving)
{
return addConstraintEqual(a1.rad, a2.rad, tagId, driving);
}
int System::addConstraintP2PSymmetric(Point& p1, Point& p2, Line& l, int tagId, bool driving)
{
addConstraintPerpendicular(p1, p2, l.p1, l.p2, tagId, driving);
return addConstraintMidpointOnLine(p1, p2, l.p1, l.p2, tagId, driving);
}
int System::addConstraintP2PSymmetric(Point& p1, Point& p2, Point& p, int tagId, bool driving)
{
addConstraintPointOnPerpBisector(p, p1, p2, tagId, driving);
return addConstraintPointOnLine(p, p1, p2, tagId, driving);
}
int System::addConstraintSnellsLaw(Curve& ray1,
Curve& ray2,
Curve& boundary,
Point p,
double* n1,
double* n2,
bool flipn1,
bool flipn2,
int tagId,
bool driving)
{
Constraint* constr = new ConstraintSnell(ray1, ray2, boundary, p, n1, n2, flipn1, flipn2);
constr->setTag(tagId);
constr->setDriving(driving);
return addConstraint(constr);
}
int System::addConstraintInternalAlignmentPoint2Ellipse(Ellipse& e,
Point& p1,
InternalAlignmentType alignmentType,
int tagId,
bool driving)
{
Constraint* constr = new ConstraintInternalAlignmentPoint2Ellipse(e, p1, alignmentType);
constr->setTag(tagId);
constr->setDriving(driving);
constr->setInternalAlignment(Constraint::Alignment::InternalAlignment);
return addConstraint(constr);
}
int System::addConstraintInternalAlignmentPoint2Hyperbola(Hyperbola& e,
Point& p1,
InternalAlignmentType alignmentType,
int tagId,
bool driving)
{
Constraint* constr = new ConstraintInternalAlignmentPoint2Hyperbola(e, p1, alignmentType);
constr->setTag(tagId);
constr->setDriving(driving);
constr->setInternalAlignment(Constraint::Alignment::InternalAlignment);
return addConstraint(constr);
}
int System::addConstraintInternalAlignmentEllipseMajorDiameter(Ellipse& e,
Point& p1,
Point& p2,
int tagId,
bool driving)
{
double X_1 = *p1.x;
double Y_1 = *p1.y;
double X_2 = *p2.x;
double Y_2 = *p2.y;
double X_c = *e.center.x;
double Y_c = *e.center.y;
double X_F1 = *e.focus1.x;
double Y_F1 = *e.focus1.y;
double b = *e.radmin;
double closertopositivemajor =
pow(X_1 - X_c
- (X_F1 - X_c) * sqrt(pow(b, 2) + pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2))
/ sqrt(pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2)),
2)
- pow(X_2 - X_c
- (X_F1 - X_c) * sqrt(pow(b, 2) + pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2))
/ sqrt(pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2)),
2)
+ pow(Y_1 - Y_c
- (Y_F1 - Y_c) * sqrt(pow(b, 2) + pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2))
/ sqrt(pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2)),
2)
- pow(Y_2 - Y_c
- (Y_F1 - Y_c) * sqrt(pow(b, 2) + pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2))
/ sqrt(pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2)),
2);
if (closertopositivemajor > 0) {
// p2 is closer to positivemajor. Assign constraints back-to-front.
addConstraintInternalAlignmentPoint2Ellipse(e, p2, EllipsePositiveMajorX, tagId, driving);
addConstraintInternalAlignmentPoint2Ellipse(e, p2, EllipsePositiveMajorY, tagId, driving);
addConstraintInternalAlignmentPoint2Ellipse(e, p1, EllipseNegativeMajorX, tagId, driving);
return addConstraintInternalAlignmentPoint2Ellipse(e,
p1,
EllipseNegativeMajorY,
tagId,
driving);
}
else {
// p1 is closer to positivemajor
addConstraintInternalAlignmentPoint2Ellipse(e, p1, EllipsePositiveMajorX, tagId, driving);
addConstraintInternalAlignmentPoint2Ellipse(e, p1, EllipsePositiveMajorY, tagId, driving);
addConstraintInternalAlignmentPoint2Ellipse(e, p2, EllipseNegativeMajorX, tagId, driving);
return addConstraintInternalAlignmentPoint2Ellipse(e,
p2,
EllipseNegativeMajorY,
tagId,
driving);
}
}
int System::addConstraintInternalAlignmentEllipseMinorDiameter(Ellipse& e,
Point& p1,
Point& p2,
int tagId,
bool driving)
{
double X_1 = *p1.x;
double Y_1 = *p1.y;
double X_2 = *p2.x;
double Y_2 = *p2.y;
double X_c = *e.center.x;
double Y_c = *e.center.y;
double X_F1 = *e.focus1.x;
double Y_F1 = *e.focus1.y;
double b = *e.radmin;
double closertopositiveminor =
pow(X_1 - X_c + b * (Y_F1 - Y_c) / sqrt(pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2)), 2)
- pow(X_2 - X_c + b * (Y_F1 - Y_c) / sqrt(pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2)), 2)
+ pow(-Y_1 + Y_c + b * (X_F1 - X_c) / sqrt(pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2)), 2)
- pow(-Y_2 + Y_c + b * (X_F1 - X_c) / sqrt(pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2)), 2);
if (closertopositiveminor > 0) {
addConstraintInternalAlignmentPoint2Ellipse(e, p2, EllipsePositiveMinorX, tagId, driving);
addConstraintInternalAlignmentPoint2Ellipse(e, p2, EllipsePositiveMinorY, tagId, driving);
addConstraintInternalAlignmentPoint2Ellipse(e, p1, EllipseNegativeMinorX, tagId, driving);
return addConstraintInternalAlignmentPoint2Ellipse(e,
p1,
EllipseNegativeMinorY,
tagId,
driving);
}
else {
addConstraintInternalAlignmentPoint2Ellipse(e, p1, EllipsePositiveMinorX, tagId, driving);
addConstraintInternalAlignmentPoint2Ellipse(e, p1, EllipsePositiveMinorY, tagId, driving);
addConstraintInternalAlignmentPoint2Ellipse(e, p2, EllipseNegativeMinorX, tagId, driving);
return addConstraintInternalAlignmentPoint2Ellipse(e,
p2,
EllipseNegativeMinorY,
tagId,
driving);
}
}
int System::addConstraintInternalAlignmentEllipseFocus1(Ellipse& e,
Point& p1,
int tagId,
bool driving)
{
addConstraintEqual(e.focus1.x, p1.x, tagId, driving, Constraint::Alignment::InternalAlignment);
return addConstraintEqual(e.focus1.y,
p1.y,
tagId,
driving,
Constraint::Alignment::InternalAlignment);
}
int System::addConstraintInternalAlignmentEllipseFocus2(Ellipse& e,
Point& p1,
int tagId,
bool driving)
{
addConstraintInternalAlignmentPoint2Ellipse(e, p1, EllipseFocus2X, tagId, driving);
return addConstraintInternalAlignmentPoint2Ellipse(e, p1, EllipseFocus2Y, tagId, driving);
}
int System::addConstraintInternalAlignmentHyperbolaMajorDiameter(Hyperbola& e,
Point& p1,
Point& p2,
int tagId,
bool driving)
{
double X_1 = *p1.x;
double Y_1 = *p1.y;
double X_2 = *p2.x;
double Y_2 = *p2.y;
double X_c = *e.center.x;
double Y_c = *e.center.y;
double X_F1 = *e.focus1.x;
double Y_F1 = *e.focus1.y;
double b = *e.radmin;
double closertopositivemajor =
pow(-X_1 + X_c
+ (X_F1 - X_c) * (-pow(b, 2) + pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2))
/ sqrt(pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2)),
2)
- pow(-X_2 + X_c
+ (X_F1 - X_c) * (-pow(b, 2) + pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2))
/ sqrt(pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2)),
2)
+ pow(-Y_1 + Y_c
+ (Y_F1 - Y_c) * (-pow(b, 2) + pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2))
/ sqrt(pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2)),
2)
- pow(-Y_2 + Y_c
+ (Y_F1 - Y_c) * (-pow(b, 2) + pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2))
/ sqrt(pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2)),
2);
if (closertopositivemajor > 0) {
// p2 is closer to positivemajor. Assign constraints back-to-front.
addConstraintInternalAlignmentPoint2Hyperbola(e,
p2,
HyperbolaPositiveMajorX,
tagId,
driving);
addConstraintInternalAlignmentPoint2Hyperbola(e,
p2,
HyperbolaPositiveMajorY,
tagId,
driving);
addConstraintInternalAlignmentPoint2Hyperbola(e,
p1,
HyperbolaNegativeMajorX,
tagId,
driving);
return addConstraintInternalAlignmentPoint2Hyperbola(e,
p1,
HyperbolaNegativeMajorY,
tagId,
driving);
}
else {
// p1 is closer to positivemajor
addConstraintInternalAlignmentPoint2Hyperbola(e,
p1,
HyperbolaPositiveMajorX,
tagId,
driving);
addConstraintInternalAlignmentPoint2Hyperbola(e,
p1,
HyperbolaPositiveMajorY,
tagId,
driving);
addConstraintInternalAlignmentPoint2Hyperbola(e,
p2,
HyperbolaNegativeMajorX,
tagId,
driving);
return addConstraintInternalAlignmentPoint2Hyperbola(e,
p2,
HyperbolaNegativeMajorY,
tagId,
driving);
}
}
int System::addConstraintInternalAlignmentHyperbolaMinorDiameter(Hyperbola& e,
Point& p1,
Point& p2,
int tagId,
bool driving)
{
double X_1 = *p1.x;
double Y_1 = *p1.y;
double X_2 = *p2.x;
double Y_2 = *p2.y;
double X_c = *e.center.x;
double Y_c = *e.center.y;
double X_F1 = *e.focus1.x;
double Y_F1 = *e.focus1.y;
double b = *e.radmin;
double closertopositiveminor =
pow(-X_1 + X_c + b * (Y_F1 - Y_c) / sqrt(pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2))
+ (X_F1 - X_c) * (-pow(b, 2) + pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2))
/ sqrt(pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2)),
2)
- pow(-X_2 + X_c + b * (Y_F1 - Y_c) / sqrt(pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2))
+ (X_F1 - X_c) * (-pow(b, 2) + pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2))
/ sqrt(pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2)),
2)
+ pow(-Y_1 + Y_c - b * (X_F1 - X_c) / sqrt(pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2))
+ (Y_F1 - Y_c) * (-pow(b, 2) + pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2))
/ sqrt(pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2)),
2)
- pow(-Y_2 + Y_c - b * (X_F1 - X_c) / sqrt(pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2))
+ (Y_F1 - Y_c) * (-pow(b, 2) + pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2))
/ sqrt(pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2)),
2);
if (closertopositiveminor < 0) {
addConstraintInternalAlignmentPoint2Hyperbola(e,
p2,
HyperbolaPositiveMinorX,
tagId,
driving);
addConstraintInternalAlignmentPoint2Hyperbola(e,
p2,
HyperbolaPositiveMinorY,
tagId,
driving);
addConstraintInternalAlignmentPoint2Hyperbola(e,
p1,
HyperbolaNegativeMinorX,
tagId,
driving);
return addConstraintInternalAlignmentPoint2Hyperbola(e,
p1,
HyperbolaNegativeMinorY,
tagId,
driving);
}
else {
addConstraintInternalAlignmentPoint2Hyperbola(e,
p1,
HyperbolaPositiveMinorX,
tagId,
driving);
addConstraintInternalAlignmentPoint2Hyperbola(e,
p1,
HyperbolaPositiveMinorY,
tagId,
driving);
addConstraintInternalAlignmentPoint2Hyperbola(e,
p2,
HyperbolaNegativeMinorX,
tagId,
driving);
return addConstraintInternalAlignmentPoint2Hyperbola(e,
p2,
HyperbolaNegativeMinorY,
tagId,
driving);
}
}
int System::addConstraintInternalAlignmentHyperbolaFocus(Hyperbola& e,
Point& p1,
int tagId,
bool driving)
{
addConstraintEqual(e.focus1.x, p1.x, tagId, driving, Constraint::Alignment::InternalAlignment);
return addConstraintEqual(e.focus1.y,
p1.y,
tagId,
driving,
Constraint::Alignment::InternalAlignment);
}
int System::addConstraintInternalAlignmentParabolaFocus(Parabola& e,
Point& p1,
int tagId,
bool driving)
{
addConstraintEqual(e.focus1.x, p1.x, tagId, driving, Constraint::Alignment::InternalAlignment);
return addConstraintEqual(e.focus1.y,
p1.y,
tagId,
driving,
Constraint::Alignment::InternalAlignment);
}
int System::addConstraintInternalAlignmentBSplineControlPoint(BSpline& b,
Circle& c,
unsigned int poleindex,
int tagId,
bool driving)
{
addConstraintEqual(b.poles[poleindex].x,
c.center.x,
tagId,
driving,
Constraint::Alignment::InternalAlignment);
addConstraintEqual(b.poles[poleindex].y,
c.center.y,
tagId,
driving,
Constraint::Alignment::InternalAlignment);
return addConstraintEqual(b.weights[poleindex],
c.rad,
tagId,
driving,
Constraint::Alignment::InternalAlignment);
}
int System::addConstraintInternalAlignmentKnotPoint(BSpline& b,
Point& p,
unsigned int knotindex,
int tagId,
bool driving)
{
if (b.periodic && knotindex == 0) {
// This is done here since knotpoints themselves aren't stored
addConstraintP2PCoincident(p, b.start, tagId, driving);
addConstraintP2PCoincident(p, b.end, tagId, driving);
}
size_t numpoles = b.degree - b.mult[knotindex] + 1;
if (numpoles == 0) {
numpoles = 1;
}
// `startpole` is the first pole affecting the knot with `knotindex`
size_t startpole = 0;
std::vector<double*> pvec;
pvec.push_back(p.x);
std::vector<double> factors(numpoles, 1.0 / numpoles);
// Only poles with indices `[i, i+1,... i+b.degree]` affect the interval
// `flattenedknots[b.degree+i]` to `flattenedknots[b.degree+i+1]`.
// When a knot has higher multiplicity, it can be seen as spanning
// multiple of these intervals, and thus is only affected by an
// intersection of the poles that affect it.
// The `knotindex` gives us the intervals, so work backwards and find
// the affecting poles.
// Note that this works also for periodic B-splines, just that the poles wrap around if needed.
for (size_t j = 1; j <= knotindex; ++j) {
startpole += b.mult[j];
}
// For the last knot, even the upper limit of the last interval range
// is included. So offset for that.
// For periodic B-splines the `flattenedknots` are defined differently,
// so this is not needed for them.
if (!b.periodic && startpole >= b.poles.size()) {
startpole = b.poles.size() - 1;
}
// Calculate the factors to be passed to weighted linear combination constraint.
// The if condition has a small performance benefit, but that is not why it is here.
// One case when numpoles <= 1 is for the last knot of a non-periodic B-spline.
// In this case, the interval `k` passed to `getLinCombFactor` is degenerate, and this is the
// cleanest way to handle it.
if (numpoles > 1) {
for (size_t i = 0; i < numpoles; ++i) {
factors[i] =
b.getLinCombFactor(*(b.knots[knotindex]), startpole + b.degree, startpole + i);
}
}
// The mod operation is to adjust for periodic B-splines.
// This can be separated for performance reasons but it will be less readable.
for (size_t i = 0; i < numpoles; ++i) {
pvec.push_back(b.poles[(startpole + i) % b.poles.size()].x);
}
for (size_t i = 0; i < numpoles; ++i) {
pvec.push_back(b.weights[(startpole + i) % b.poles.size()]);
}
Constraint* constr = new ConstraintWeightedLinearCombination(numpoles, pvec, factors);
constr->setTag(tagId);
constr->setDriving(driving);
constr->setInternalAlignment(Constraint::Alignment::InternalAlignment);
addConstraint(constr);
pvec.clear();
pvec.push_back(p.y);
for (size_t i = 0; i < numpoles; ++i) {
pvec.push_back(b.poles[(startpole + i) % b.poles.size()].y);
}
for (size_t i = 0; i < numpoles; ++i) {
pvec.push_back(b.weights[(startpole + i) % b.poles.size()]);
}
constr = new ConstraintWeightedLinearCombination(numpoles, pvec, factors);
constr->setTag(tagId);
constr->setDriving(driving);
constr->setInternalAlignment(Constraint::Alignment::InternalAlignment);
return addConstraint(constr);
}
// calculates angle between two curves at point of their intersection p. If two
// points are supplied, p is used for first curve and p2 for second, yielding a
// remote angle computation (this is useful when the endpoints haven't) been
// made coincident yet
double System::calculateAngleViaPoint(const Curve& crv1, const Curve& crv2, Point& p) const
{
return calculateAngleViaPoint(crv1, crv2, p, p);
}
double
System::calculateAngleViaPoint(const Curve& crv1, const Curve& crv2, Point& p1, Point& p2) const
{
GCS::DeriVector2 n1 = crv1.CalculateNormal(p1);
GCS::DeriVector2 n2 = crv2.CalculateNormal(p2);
return atan2(-n2.x * n1.y + n2.y * n1.x, n2.x * n1.x + n2.y * n1.y);
}
double System::calculateAngleViaParams(const Curve& crv1,
const Curve& crv2,
double* param1,
double* param2) const
{
GCS::DeriVector2 n1 = crv1.CalculateNormal(param1);
GCS::DeriVector2 n2 = crv2.CalculateNormal(param2);
return atan2(-n2.x * n1.y + n2.y * n1.x, n2.x * n1.x + n2.y * n1.y);
}
void System::calculateNormalAtPoint(const Curve& crv,
const Point& p,
double& rtnX,
double& rtnY) const
{
GCS::DeriVector2 n1 = crv.CalculateNormal(p);
rtnX = n1.x;
rtnY = n1.y;
}
double System::calculateConstraintErrorByTag(int tagId)
{
int cnt = 0; // how many constraints have been accumulated
double sqErr = 0.0; // accumulator of squared errors
double err = 0.0; // last computed signed error value
for (std::vector<Constraint*>::const_iterator constr = clist.begin(); constr != clist.end();
++constr) {
if ((*constr)->getTag() == tagId) {
err = (*constr)->error();
sqErr += err * err;
cnt++;
};
}
switch (cnt) {
case 0: // constraint not found!
return std::numeric_limits<double>::quiet_NaN();
break;
case 1:
return err;
break;
default:
return sqrt(sqErr / (double)cnt);
}
}
void System::rescaleConstraint(int id, double coeff)
{
if (id >= static_cast<int>(clist.size()) || id < 0) {
return;
}
if (clist[id]) {
clist[id]->rescale(coeff);
}
}
void System::declareUnknowns(VEC_pD& params)
{
plist = params;
pIndex.clear();
for (int i = 0; i < int(plist.size()); ++i) {
pIndex[plist[i]] = i;
}
hasUnknowns = true;
}
void System::declareDrivenParams(VEC_pD& params)
{
pdrivenlist = params;
}
void System::initSolution(Algorithm alg)
{
// - Stores the current parameters values in the vector "reference"
// - identifies any decoupled subsystems and partitions the original
// system into corresponding components
// - Stores the current parameters in the vector "reference"
// - Identifies the equality constraints tagged with ids >= 0
// and prepares a corresponding system reduction
// - Organizes the rest of constraints into two subsystems for
// tag ids >=0 and < 0 respectively and applies the
// system reduction specified in the previous step
isInit = false;
if (!hasUnknowns) {
return;
}
// storing reference configuration
setReference();
// diagnose conflicting or redundant constraints
if (!hasDiagnosis) {
diagnose(alg);
if (!hasDiagnosis) {
return;
}
}
std::vector<Constraint*> clistR;
if (!redundant.empty()) {
for (std::vector<Constraint*>::const_iterator constr = clist.begin(); constr != clist.end();
++constr) {
if (redundant.count(*constr) == 0) {
clistR.push_back(*constr);
}
}
}
else {
clistR = clist;
}
// partitioning into decoupled components
Graph g;
for (int i = 0; i < int(plist.size() + clistR.size()); i++) {
boost::add_vertex(g);
}
int cvtid = int(plist.size());
for (std::vector<Constraint*>::const_iterator constr = clistR.begin(); constr != clistR.end();
++constr, cvtid++) {
VEC_pD& cparams = c2p[*constr];
for (VEC_pD::const_iterator param = cparams.begin(); param != cparams.end(); ++param) {
MAP_pD_I::const_iterator it = pIndex.find(*param);
if (it != pIndex.end()) {
boost::add_edge(cvtid, it->second, g);
}
}
}
VEC_I components(boost::num_vertices(g));
int componentsSize = 0;
if (!components.empty()) {
componentsSize = boost::connected_components(g, &components[0]);
}
// identification of equality constraints and parameter reduction
std::set<Constraint*> reducedConstrs; // constraints that will be eliminated through reduction
reductionmaps.clear(); // destroy any maps
reductionmaps.resize(componentsSize); // create empty maps to be filled in
{
VEC_pD reducedParams = plist;
for (std::vector<Constraint*>::const_iterator constr = clistR.begin();
constr != clistR.end();
++constr) {
if ((*constr)->getTag() >= 0 && (*constr)->getTypeId() == Equal) {
MAP_pD_I::const_iterator it1, it2;
it1 = pIndex.find((*constr)->params()[0]);
it2 = pIndex.find((*constr)->params()[1]);
if (it1 != pIndex.end() && it2 != pIndex.end()) {
reducedConstrs.insert(*constr);
double* p_kept = reducedParams[it1->second];
double* p_replaced = reducedParams[it2->second];
for (int i = 0; i < int(plist.size()); ++i) {
if (reducedParams[i] == p_replaced) {
reducedParams[i] = p_kept;
}
}
}
}
}
for (int i = 0; i < int(plist.size()); ++i) {
if (plist[i] != reducedParams[i]) {
int cid = components[i];
reductionmaps[cid][plist[i]] = reducedParams[i];
}
}
}
clists.clear(); // destroy any lists
clists.resize(componentsSize); // create empty lists to be filled in
int i = int(plist.size());
for (std::vector<Constraint*>::const_iterator constr = clistR.begin(); constr != clistR.end();
++constr, i++) {
if (reducedConstrs.count(*constr) == 0) {
int cid = components[i];
clists[cid].push_back(*constr);
}
}
plists.clear(); // destroy any lists
plists.resize(componentsSize); // create empty lists to be filled in
for (int i = 0; i < int(plist.size()); ++i) {
int cid = components[i];
plists[cid].push_back(plist[i]);
}
// calculates subSystems and subSystemsAux from clists, plists and reductionmaps
clearSubSystems();
for (std::size_t cid = 0; cid < clists.size(); cid++) {
std::vector<Constraint*> clist0, clist1;
for (std::vector<Constraint*>::const_iterator constr = clists[cid].begin();
constr != clists[cid].end();
++constr) {
if ((*constr)->getTag() >= 0) {
clist0.push_back(*constr);
}
else { // move or distance from reference constraints
clist1.push_back(*constr);
}
}
subSystems.push_back(nullptr);
subSystemsAux.push_back(nullptr);
if (!clist0.empty()) {
subSystems[cid] = new SubSystem(clist0, plists[cid], reductionmaps[cid]);
}
if (!clist1.empty()) {
subSystemsAux[cid] = new SubSystem(clist1, plists[cid], reductionmaps[cid]);
}
}
isInit = true;
}
void System::setReference()
{
reference.clear();
reference.reserve(plist.size());
for (VEC_pD::const_iterator param = plist.begin(); param != plist.end(); ++param) {
reference.push_back(**param);
}
}
void System::resetToReference()
{
if (reference.size() == plist.size()) {
VEC_D::const_iterator ref = reference.begin();
VEC_pD::iterator param = plist.begin();
for (; ref != reference.end(); ++ref, ++param) {
**param = *ref;
}
}
}
int System::solve(VEC_pD& params, bool isFine, Algorithm alg, bool isRedundantsolving)
{
declareUnknowns(params);
initSolution();
return solve(isFine, alg, isRedundantsolving);
}
int System::solve(bool isFine, Algorithm alg, bool isRedundantsolving)
{
if (!isInit) {
return Failed;
}
bool isReset = false;
// return success by default in order to permit coincidence constraints to be applied
// even if no other system has to be solved
int res = Success;
for (int cid = 0; cid < int(subSystems.size()); cid++) {
if ((subSystems[cid] || subSystemsAux[cid]) && !isReset) {
resetToReference();
isReset = true;
}
if (subSystems[cid] && subSystemsAux[cid]) {
res = std::max(res,
solve(subSystems[cid], subSystemsAux[cid], isFine, isRedundantsolving));
}
else if (subSystems[cid]) {
res = std::max(res, solve(subSystems[cid], isFine, alg, isRedundantsolving));
}
else if (subSystemsAux[cid]) {
res = std::max(res, solve(subSystemsAux[cid], isFine, alg, isRedundantsolving));
}
}
if (res == Success) {
for (std::set<Constraint*>::const_iterator constr = redundant.begin();
constr != redundant.end();
++constr) {
// DeepSOIC: there used to be a comparison of signed error value to
// convergence, which makes no sense. Potentially I fixed bug, and
// chances are low I've broken anything.
double err = (*constr)->error();
if (err * err > (isRedundantsolving ? convergenceRedundant : convergence)) {
res = Converged;
return res;
}
}
}
return res;
}
int System::solve(SubSystem* subsys, bool isFine, Algorithm alg, bool isRedundantsolving)
{
if (alg == BFGS) {
return solve_BFGS(subsys, isFine, isRedundantsolving);
}
else if (alg == LevenbergMarquardt) {
return solve_LM(subsys, isRedundantsolving);
}
else if (alg == DogLeg) {
return solve_DL(subsys, isRedundantsolving);
}
else {
return Failed;
}
}
int System::solve_BFGS(SubSystem* subsys, bool /*isFine*/, bool isRedundantsolving)
{
#ifdef _GCS_EXTRACT_SOLVER_SUBSYSTEM_
extractSubsystem(subsys, isRedundantsolving);
#endif
int xsize = subsys->pSize();
if (xsize == 0) {
return Success;
}
subsys->redirectParams();
Eigen::MatrixXd D = Eigen::MatrixXd::Identity(xsize, xsize);
Eigen::VectorXd x(xsize);
Eigen::VectorXd xdir(xsize);
Eigen::VectorXd grad(xsize);
Eigen::VectorXd h(xsize);
Eigen::VectorXd y(xsize);
Eigen::VectorXd Dy(xsize);
// Initial unknowns vector and initial gradient vector
subsys->getParams(x);
subsys->calcGrad(grad);
// Initial search direction opposed to gradient (steepest-descent)
xdir = -grad;
lineSearch(subsys, xdir);
double err = subsys->error();
h = x;
subsys->getParams(x);
h = x - h; // = x - xold
// double convergence = isFine ? convergence : XconvergenceRough;
int maxIterNumber =
(isRedundantsolving
? (sketchSizeMultiplierRedundant ? maxIterRedundant * xsize : maxIterRedundant)
: (sketchSizeMultiplier ? maxIter * xsize : maxIter));
if (debugMode == IterationLevel) {
std::stringstream stream;
stream << "BFGS: convergence: " << (isRedundantsolving ? convergenceRedundant : convergence)
<< ", xsize: " << xsize << ", maxIter: " << maxIterNumber << "\n";
const std::string tmp = stream.str();
Base::Console().Log(tmp.c_str());
}
double divergingLim = 1e6 * err + 1e12;
double h_norm;
for (int iter = 1; iter < maxIterNumber; iter++) {
h_norm = h.norm();
if (h_norm <= (isRedundantsolving ? convergenceRedundant : convergence) || err <= smallF) {
if (debugMode == IterationLevel) {
std::stringstream stream;
stream << "BFGS Converged!!: "
<< ", err: " << err << ", h_norm: " << h_norm << "\n";
const std::string tmp = stream.str();
Base::Console().Log(tmp.c_str());
}
break;
}
if (err > divergingLim || err != err) { // check for diverging and NaN
if (debugMode == IterationLevel) {
std::stringstream stream;
stream << "BFGS Failed: Diverging!!: "
<< ", err: " << err << ", divergingLim: " << divergingLim << "\n";
const std::string tmp = stream.str();
Base::Console().Log(tmp.c_str());
}
break;
}
y = grad;
subsys->calcGrad(grad);
y = grad - y; // = grad - gradold
double hty = h.dot(y);
// make sure that hty is never 0
if (hty == 0) {
hty = .0000000001;
}
Dy = D * y;
double ytDy = y.dot(Dy);
// Now calculate the BFGS update on D
D += (1. + ytDy / hty) / hty * h * h.transpose();
D -= 1. / hty * (h * Dy.transpose() + Dy * h.transpose());
xdir = -D * grad;
lineSearch(subsys, xdir);
err = subsys->error();
h = x;
subsys->getParams(x);
h = x - h; // = x - xold
if (debugMode == IterationLevel) {
std::stringstream stream;
stream << "BFGS, Iteration: " << iter << ", err: " << err << ", h_norm: " << h_norm
<< "\n";
const std::string tmp = stream.str();
Base::Console().Log(tmp.c_str());
}
}
subsys->revertParams();
if (err <= smallF) {
return Success;
}
if (h.norm() <= (isRedundantsolving ? convergenceRedundant : convergence)) {
return Converged;
}
return Failed;
}
int System::solve_LM(SubSystem* subsys, bool isRedundantsolving)
{
#ifdef _GCS_EXTRACT_SOLVER_SUBSYSTEM_
extractSubsystem(subsys, isRedundantsolving);
#endif
int xsize = subsys->pSize();
int csize = subsys->cSize();
if (xsize == 0) {
return Success;
}
Eigen::VectorXd e(csize),
e_new(csize); // vector of all function errors (every constraint is one function)
Eigen::MatrixXd J(csize, xsize); // Jacobi of the subsystem
Eigen::MatrixXd A(xsize, xsize);
Eigen::VectorXd x(xsize), h(xsize), x_new(xsize), g(xsize), diag_A(xsize);
subsys->redirectParams();
subsys->getParams(x);
subsys->calcResidual(e);
e *= -1;
int maxIterNumber =
(isRedundantsolving
? (sketchSizeMultiplierRedundant ? maxIterRedundant * xsize : maxIterRedundant)
: (sketchSizeMultiplier ? maxIter * xsize : maxIter));
double divergingLim = 1e6 * e.squaredNorm() + 1e12;
double eps = (isRedundantsolving ? LM_epsRedundant : LM_eps);
double eps1 = (isRedundantsolving ? LM_eps1Redundant : LM_eps1);
double tau = (isRedundantsolving ? LM_tauRedundant : LM_tau);
if (debugMode == IterationLevel) {
std::stringstream stream;
stream << "LM: eps: " << eps << ", eps1: " << eps1 << ", tau: " << tau
<< ", convergence: " << (isRedundantsolving ? convergenceRedundant : convergence)
<< ", xsize: " << xsize << ", maxIter: " << maxIterNumber << "\n";
const std::string tmp = stream.str();
Base::Console().Log(tmp.c_str());
}
double nu = 2, mu = 0;
int iter = 0, stop = 0;
for (iter = 0; iter < maxIterNumber && !stop; ++iter) {
// check error
double err = e.squaredNorm();
if (err <= eps * eps) { // error is small, Success
stop = 1;
break;
}
else if (err > divergingLim || err != err) { // check for diverging and NaN
stop = 6;
break;
}
// J^T J, J^T e
subsys->calcJacobi(J);
;
A = J.transpose() * J;
g = J.transpose() * e;
// Compute ||J^T e||_inf
double g_inf = g.lpNorm<Eigen::Infinity>();
diag_A = A.diagonal(); // save diagonal entries so that augmentation can be later canceled
// check for convergence
if (g_inf <= eps1) {
stop = 2;
break;
}
// compute initial damping factor
if (iter == 0) {
mu = tau * diag_A.lpNorm<Eigen::Infinity>();
}
double h_norm;
// determine increment using adaptive damping
int k = 0;
while (k < 50) {
// augment normal equations A = A+uI
for (int i = 0; i < xsize; ++i) {
A(i, i) += mu;
}
// solve augmented functions A*h=-g
h = A.fullPivLu().solve(g);
double rel_error = (A * h - g).norm() / g.norm();
// check if solving works
if (rel_error < 1e-5) {
// restrict h according to maxStep
double scale = subsys->maxStep(h);
if (scale < 1.) {
h *= scale;
}
// compute par's new estimate and ||d_par||^2
x_new = x + h;
h_norm = h.squaredNorm();
if (h_norm <= eps1 * eps1 * x.norm()) { // relative change in p is small, stop
stop = 3;
break;
}
else if (h_norm
>= (x.norm() + eps1) / (DBL_EPSILON * DBL_EPSILON)) { // almost singular
stop = 4;
break;
}
subsys->setParams(x_new);
subsys->calcResidual(e_new);
e_new *= -1;
double dF = e.squaredNorm() - e_new.squaredNorm();
double dL = h.dot(mu * h + g);
if (dF > 0. && dL > 0.) { // reduction in error, increment is accepted
double tmp = 2 * dF / dL - 1.;
mu *= std::max(1. / 3., 1. - tmp * tmp * tmp);
nu = 2;
// update par's estimate
x = x_new;
e = e_new;
break;
}
}
// if this point is reached, either the linear system could not be solved or
// the error did not reduce; in any case, the increment must be rejected
mu *= nu;
nu *= 2.0;
for (int i = 0; i < xsize; ++i) { // restore diagonal J^T J entries
A(i, i) = diag_A(i);
}
k++;
}
if (k > 50) {
stop = 7;
break;
}
if (debugMode == IterationLevel) {
std::stringstream stream;
// Iteration: 1, residual: 1e-3, tolg: 1e-5, tolx: 1e-3
stream << "LM, Iteration: " << iter << ", err(eps): " << err
<< ", g_inf(eps1): " << g_inf << ", h_norm: " << h_norm << "\n";
const std::string tmp = stream.str();
Base::Console().Log(tmp.c_str());
}
}
if (iter >= maxIterNumber) {
stop = 5;
}
subsys->revertParams();
return (stop == 1) ? Success : Failed;
}
int System::solve_DL(SubSystem* subsys, bool isRedundantsolving)
{
#ifdef _GCS_EXTRACT_SOLVER_SUBSYSTEM_
extractSubsystem(subsys, isRedundantsolving);
#endif
double tolg = (isRedundantsolving ? DL_tolgRedundant : DL_tolg);
double tolx = (isRedundantsolving ? DL_tolxRedundant : DL_tolx);
double tolf = (isRedundantsolving ? DL_tolfRedundant : DL_tolf);
int xsize = subsys->pSize();
int csize = subsys->cSize();
if (xsize == 0) {
return Success;
}
int maxIterNumber =
(isRedundantsolving
? (sketchSizeMultiplierRedundant ? maxIterRedundant * xsize : maxIterRedundant)
: (sketchSizeMultiplier ? maxIter * xsize : maxIter));
if (debugMode == IterationLevel) {
std::stringstream stream;
stream << "DL: tolg: " << tolg << ", tolx: " << tolx << ", tolf: " << tolf
<< ", convergence: " << (isRedundantsolving ? convergenceRedundant : convergence)
<< ", dogLegGaussStep: "
<< (dogLegGaussStep == FullPivLU
? "FullPivLU"
: (dogLegGaussStep == LeastNormFullPivLU ? "LeastNormFullPivLU"
: "LeastNormLdlt"))
<< ", xsize: " << xsize << ", csize: " << csize << ", maxIter: " << maxIterNumber
<< "\n";
const std::string tmp = stream.str();
Base::Console().Log(tmp.c_str());
}
Eigen::VectorXd x(xsize), x_new(xsize);
Eigen::VectorXd fx(csize), fx_new(csize);
Eigen::MatrixXd Jx(csize, xsize), Jx_new(csize, xsize);
Eigen::VectorXd g(xsize), h_sd(xsize), h_gn(xsize), h_dl(xsize);
subsys->redirectParams();
double err;
subsys->getParams(x);
subsys->calcResidual(fx, err);
subsys->calcJacobi(Jx);
g = Jx.transpose() * (-fx);
// get the infinity norm fx_inf and g_inf
double g_inf = g.lpNorm<Eigen::Infinity>();
double fx_inf = fx.lpNorm<Eigen::Infinity>();
double divergingLim = 1e6 * err + 1e12;
double delta = 0.1;
double alpha = 0.;
double nu = 2.;
int iter = 0, stop = 0, reduce = 0;
while (!stop) {
// check if finished
if (fx_inf <= tolf) { // Success
stop = 1;
}
else if (g_inf <= tolg) {
stop = 2;
}
else if (delta <= tolx * (tolx + x.norm())) {
stop = 2;
}
else if (iter >= maxIterNumber) {
stop = 4;
}
else if (err > divergingLim || err != err) { // check for diverging and NaN
stop = 6;
}
else {
// get the steepest descent direction
alpha = g.squaredNorm() / (Jx * g).squaredNorm();
h_sd = alpha * g;
// get the gauss-newton step
// https://forum.freecad.org/viewtopic.php?f=10&t=12769&start=50#p106220
// https://forum.kde.org/viewtopic.php?f=74&t=129439#p346104
switch (dogLegGaussStep) {
case FullPivLU:
h_gn = Jx.fullPivLu().solve(-fx);
break;
case LeastNormFullPivLU:
h_gn = Jx.adjoint() * (Jx * Jx.adjoint()).fullPivLu().solve(-fx);
break;
case LeastNormLdlt:
h_gn = Jx.adjoint() * (Jx * Jx.adjoint()).ldlt().solve(-fx);
break;
}
double rel_error = (Jx * h_gn + fx).norm() / fx.norm();
if (rel_error > 1e15) {
break;
}
// compute the dogleg step
if (h_gn.norm() < delta) {
h_dl = h_gn;
if (h_dl.norm() <= tolx * (tolx + x.norm())) {
stop = 5;
break;
}
}
else if (alpha * g.norm() >= delta) {
h_dl = (delta / (alpha * g.norm())) * h_sd;
}
else {
// compute beta
double beta = 0;
Eigen::VectorXd b = h_gn - h_sd;
double bb = (b.transpose() * b).norm();
double gb = (h_sd.transpose() * b).norm();
double c = (delta + h_sd.norm()) * (delta - h_sd.norm());
if (gb > 0) {
beta = c / (gb + sqrt(gb * gb + c * bb));
}
else {
beta = (sqrt(gb * gb + c * bb) - gb) / bb;
}
// and update h_dl and dL with beta
h_dl = h_sd + beta * b;
}
}
// see if we are already finished
if (stop) {
break;
}
// get the new values
double err_new;
x_new = x + h_dl;
subsys->setParams(x_new);
subsys->calcResidual(fx_new, err_new);
subsys->calcJacobi(Jx_new);
// calculate the linear model and the update ratio
double dL = err - 0.5 * (fx + Jx * h_dl).squaredNorm();
double dF = err - err_new;
double rho = dL / dF;
if (dF > 0 && dL > 0) {
x = x_new;
Jx = Jx_new;
fx = fx_new;
err = err_new;
g = Jx.transpose() * (-fx);
// get infinity norms
g_inf = g.lpNorm<Eigen::Infinity>();
fx_inf = fx.lpNorm<Eigen::Infinity>();
}
else {
rho = -1;
}
// update delta
if (fabs(rho - 1.) < 0.2 && h_dl.norm() > delta / 3. && reduce <= 0) {
delta = 3 * delta;
nu = 2;
reduce = 0;
}
else if (rho < 0.25) {
delta = delta / nu;
nu = 2 * nu;
reduce = 2;
}
else {
reduce--;
}
if (debugMode == IterationLevel) {
std::stringstream stream;
// Iteration: 1, residual: 1e-3, tolg: 1e-5, tolx: 1e-3
stream << "DL, Iteration: " << iter << ", fx_inf(tolf): " << fx_inf
<< ", g_inf(tolg): " << g_inf << ", delta(f(tolx)): " << delta
<< ", err(divergingLim): " << err << "\n";
const std::string tmp = stream.str();
Base::Console().Log(tmp.c_str());
}
// count this iteration and start again
iter++;
}
subsys->revertParams();
if (debugMode == IterationLevel) {
std::stringstream stream;
stream << "DL: stopcode: " << stop << ((stop == 1) ? ", Success" : ", Failed") << "\n";
const std::string tmp = stream.str();
Base::Console().Log(tmp.c_str());
}
return (stop == 1) ? Success : Failed;
}
#ifdef _GCS_EXTRACT_SOLVER_SUBSYSTEM_
void System::extractSubsystem(SubSystem* subsys, bool isRedundantsolving)
{
VEC_pD plistout; // std::vector<double *>
std::vector<Constraint*> clist_;
VEC_pD clist_params_;
subsys->getParamList(plistout);
subsys->getConstraintList(clist_);
std::ofstream subsystemfile;
subsystemfile.open("subsystemfile.txt", std::ofstream::out | std::ofstream::app);
subsystemfile << std::endl;
subsystemfile << std::endl;
subsystemfile << "Solving: " << (isRedundantsolving ? "Redundant" : "Normal")
<< " Subsystem Dump starts here................................" << std::endl;
int ip = 0;
subsystemfile << "GCS::VEC_pD plist_;" << std::endl; // all SYSTEM params
subsystemfile << "std::vector<GCS::Constraint *> clist_;"
<< std::endl; // SUBSYSTEM constraints
subsystemfile << "GCS::VEC_pD plistsub_;" << std::endl; // all SUBSYSTEM params
// constraint params not within SYSTEM params
subsystemfile << "GCS::VEC_pD clist_params_;" << std::endl;
// these are all the parameters, including those not in the subsystem to
// which constraints in the subsystem may make reference.
for (VEC_pD::iterator it = plist.begin(); it != plist.end(); ++it, ++ip) {
subsystemfile << "plist_.push_back(new double(" << *(*it) << ")); // " << ip
<< " address: " << (void*)(*it) << std::endl;
}
int ips = 0;
for (VEC_pD::iterator it = plistout.begin(); it != plistout.end(); ++it, ++ips) {
VEC_pD::iterator p = std::find(plist.begin(), plist.end(), (*it));
size_t p_index = std::distance(plist.begin(), p);
if (p_index == plist.size()) {
subsystemfile << "// Error: Subsystem parameter not in system params"
<< "address: " << (void*)(*it) << std::endl;
}
subsystemfile << "plistsub_.push_back(plist_[" << p_index << "]); // " << ips << std::endl;
}
int ic = 0; // constraint index
int icp = 0; // index of constraint params not within SYSTEM params
for (std::vector<Constraint*>::iterator it = clist_.begin(); it != clist_.end(); ++it, ++ic) {
switch ((*it)->getTypeId()) {
case Equal: { // 2
VEC_pD::iterator p1 = std::find(plist.begin(), plist.end(), (*it)->pvec[0]);
VEC_pD::iterator p2 = std::find(plist.begin(), plist.end(), (*it)->pvec[1]);
size_t i1 = std::distance(plist.begin(), p1);
size_t i2 = std::distance(plist.begin(), p2);
bool npb1 = false;
VEC_pD::iterator np1 =
std::find(clist_params_.begin(), clist_params_.end(), (*it)->pvec[0]);
size_t ni1 = std::distance(clist_params_.begin(), np1);
if (i1 == plist.size()) {
if (ni1 == clist_params_.size()) {
subsystemfile
<< "// Address not in System params...rebuilding into clist_params_"
<< std::endl;
clist_params_.push_back((*it)->pvec[0]);
subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[0])
<< ")); // " << icp << " address: " << (void*)(*it)->pvec[0]
<< std::endl;
icp++;
}
npb1 = true;
}
bool npb2 = false;
VEC_pD::iterator np2 =
std::find(clist_params_.begin(), clist_params_.end(), (*it)->pvec[1]);
size_t ni2 = std::distance(clist_params_.begin(), np2);
if (i2 == plist.size()) {
if (ni2 == clist_params_.size()) {
subsystemfile
<< "// Address not in System params...rebuilding into clist_params_"
<< std::endl;
clist_params_.push_back((*it)->pvec[1]);
subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[1])
<< ")); // " << icp << " address: " << (void*)(*it)->pvec[1]
<< std::endl;
icp++;
}
npb2 = true;
}
subsystemfile << "clist_.push_back(new ConstraintEqual("
<< (npb1 ? ("clist_params_[") : ("plist_[")) << (npb1 ? ni1 : i1)
<< "]," << (npb2 ? ("clist_params_[") : ("plist_["))
<< (npb2 ? ni2 : i2) << "])); // addresses = " << (*it)->pvec[0]
<< "," << (*it)->pvec[1] << std::endl;
break;
}
case Difference: { // 3
VEC_pD::iterator p1 = std::find(plist.begin(), plist.end(), (*it)->pvec[0]);
VEC_pD::iterator p2 = std::find(plist.begin(), plist.end(), (*it)->pvec[1]);
VEC_pD::iterator p3 = std::find(plist.begin(), plist.end(), (*it)->pvec[2]);
size_t i1 = std::distance(plist.begin(), p1);
size_t i2 = std::distance(plist.begin(), p2);
size_t i3 = std::distance(plist.begin(), p3);
bool npb1 = false;
VEC_pD::iterator np1 =
std::find(clist_params_.begin(), clist_params_.end(), (*it)->pvec[0]);
size_t ni1 = std::distance(clist_params_.begin(), np1);
if (i1 == plist.size()) {
if (ni1 == clist_params_.size()) {
subsystemfile
<< "// Address not in System params...rebuilding into clist_params_"
<< std::endl;
clist_params_.push_back((*it)->pvec[0]);
subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[0])
<< ")); // " << icp << " address: " << (void*)(*it)->pvec[0]
<< std::endl;
icp++;
}
npb1 = true;
}
bool npb2 = false;
VEC_pD::iterator np2 =
std::find(clist_params_.begin(), clist_params_.end(), (*it)->pvec[1]);
size_t ni2 = std::distance(clist_params_.begin(), np2);
if (i2 == plist.size()) {
if (ni2 == clist_params_.size()) {
subsystemfile
<< "// Address not in System params...rebuilding into clist_params_"
<< std::endl;
clist_params_.push_back((*it)->pvec[1]);
subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[1])
<< ")); // " << icp << " address: " << (void*)(*it)->pvec[1]
<< std::endl;
icp++;
}
npb2 = true;
}
bool npb3 = false;
VEC_pD::iterator np3 =
std::find(clist_params_.begin(), clist_params_.end(), (*it)->pvec[2]);
size_t ni3 = std::distance(clist_params_.begin(), np3);
if (i3 == plist.size()) {
if (ni3 == clist_params_.size()) {
subsystemfile
<< "// Address not in System params...rebuilding into clist_params_"
<< std::endl;
clist_params_.push_back((*it)->pvec[2]);
subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[2])
<< ")); // " << icp << " address: " << (void*)(*it)->pvec[2]
<< std::endl;
icp++;
}
npb3 = true;
}
subsystemfile << "clist_.push_back(new ConstraintDifference("
<< (npb1 ? ("clist_params_[") : ("plist_[")) << (npb1 ? ni1 : i1)
<< "]," << (npb2 ? ("clist_params_[") : ("plist_["))
<< (npb2 ? ni2 : i2) << "],"
<< (npb3 ? ("clist_params_[") : ("plist_[")) << (npb3 ? ni3 : i3)
<< "])); // addresses = " << (*it)->pvec[0] << "," << (*it)->pvec[1]
<< "," << (*it)->pvec[2] << std::endl;
break;
}
case P2PDistance: { // 5
VEC_pD::iterator p1 = std::find(plist.begin(), plist.end(), (*it)->pvec[0]);
VEC_pD::iterator p2 = std::find(plist.begin(), plist.end(), (*it)->pvec[1]);
VEC_pD::iterator p3 = std::find(plist.begin(), plist.end(), (*it)->pvec[2]);
VEC_pD::iterator p4 = std::find(plist.begin(), plist.end(), (*it)->pvec[3]);
VEC_pD::iterator p5 = std::find(plist.begin(), plist.end(), (*it)->pvec[4]);
size_t i1 = std::distance(plist.begin(), p1);
size_t i2 = std::distance(plist.begin(), p2);
size_t i3 = std::distance(plist.begin(), p3);
size_t i4 = std::distance(plist.begin(), p4);
size_t i5 = std::distance(plist.begin(), p5);
bool npb1 = false;
VEC_pD::iterator np1 =
std::find(clist_params_.begin(), clist_params_.end(), (*it)->pvec[0]);
size_t ni1 = std::distance(clist_params_.begin(), np1);
if (i1 == plist.size()) {
if (ni1 == clist_params_.size()) {
subsystemfile
<< "// Address not in System params...rebuilding into clist_params_"
<< std::endl;
clist_params_.push_back((*it)->pvec[0]);
subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[0])
<< ")); // " << icp << " address: " << (void*)(*it)->pvec[0]
<< std::endl;
icp++;
}
npb1 = true;
}
bool npb2 = false;
VEC_pD::iterator np2 =
std::find(clist_params_.begin(), clist_params_.end(), (*it)->pvec[1]);
size_t ni2 = std::distance(clist_params_.begin(), np2);
if (i2 == plist.size()) {
if (ni2 == clist_params_.size()) {
subsystemfile
<< "// Address not in System params...rebuilding into clist_params_"
<< std::endl;
clist_params_.push_back((*it)->pvec[1]);
subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[1])
<< ")); // " << icp << " address: " << (void*)(*it)->pvec[1]
<< std::endl;
icp++;
}
npb2 = true;
}
bool npb3 = false;
VEC_pD::iterator np3 =
std::find(clist_params_.begin(), clist_params_.end(), (*it)->pvec[2]);
size_t ni3 = std::distance(clist_params_.begin(), np3);
if (i3 == plist.size()) {
if (ni3 == clist_params_.size()) {
subsystemfile
<< "// Address not in System params...rebuilding into clist_params_"
<< std::endl;
clist_params_.push_back((*it)->pvec[2]);
subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[2])
<< ")); // " << icp << " address: " << (void*)(*it)->pvec[2]
<< std::endl;
icp++;
}
npb3 = true;
}
bool npb4 = false;
VEC_pD::iterator np4 =
std::find(clist_params_.begin(), clist_params_.end(), (*it)->pvec[3]);
size_t ni4 = std::distance(clist_params_.begin(), np4);
if (i4 == plist.size()) {
if (ni4 == clist_params_.size()) {
subsystemfile
<< "// Address not in System params...rebuilding into clist_params_"
<< std::endl;
clist_params_.push_back((*it)->pvec[3]);
subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[3])
<< ")); // " << icp << " address: " << (void*)(*it)->pvec[3]
<< std::endl;
icp++;
}
npb4 = true;
}
bool npb5 = false;
VEC_pD::iterator np5 =
std::find(clist_params_.begin(), clist_params_.end(), (*it)->pvec[4]);
size_t ni5 = std::distance(clist_params_.begin(), np5);
if (i5 == plist.size()) {
if (ni5 == clist_params_.size()) {
subsystemfile
<< "// Address not in System params...rebuilding into clist_params_"
<< std::endl;
clist_params_.push_back((*it)->pvec[4]);
subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[4])
<< ")); // " << icp << " address: " << (void*)(*it)->pvec[4]
<< std::endl;
icp++;
}
npb5 = true;
}
subsystemfile << "ConstraintP2PDistance * c" << ic
<< "=new ConstraintP2PDistance();" << std::endl;
subsystemfile << "c" << ic << "->pvec.push_back("
<< (npb1 ? ("clist_params_[") : ("plist_[")) << (npb1 ? ni1 : i1)
<< "]);" << std::endl;
subsystemfile << "c" << ic << "->pvec.push_back("
<< (npb2 ? ("clist_params_[") : ("plist_[")) << (npb2 ? ni2 : i2)
<< "]);" << std::endl;
subsystemfile << "c" << ic << "->pvec.push_back("
<< (npb3 ? ("clist_params_[") : ("plist_[")) << (npb3 ? ni3 : i3)
<< "]);" << std::endl;
subsystemfile << "c" << ic << "->pvec.push_back("
<< (npb4 ? ("clist_params_[") : ("plist_[")) << (npb4 ? ni4 : i4)
<< "]);" << std::endl;
subsystemfile << "c" << ic << "->pvec.push_back("
<< (npb5 ? ("clist_params_[") : ("plist_[")) << (npb5 ? ni5 : i5)
<< "]);" << std::endl;
subsystemfile << "c" << ic << "->origpvec=c" << ic << "->pvec;" << std::endl;
subsystemfile << "c" << ic << "->rescale();" << std::endl;
subsystemfile << "clist_.push_back(c" << ic
<< "); // addresses = " << (*it)->pvec[0] << "," << (*it)->pvec[1]
<< "," << (*it)->pvec[2] << "," << (*it)->pvec[3] << ","
<< (*it)->pvec[4] << std::endl;
break;
}
case P2PAngle: { // 5
VEC_pD::iterator p1 = std::find(plist.begin(), plist.end(), (*it)->pvec[0]);
VEC_pD::iterator p2 = std::find(plist.begin(), plist.end(), (*it)->pvec[1]);
VEC_pD::iterator p3 = std::find(plist.begin(), plist.end(), (*it)->pvec[2]);
VEC_pD::iterator p4 = std::find(plist.begin(), plist.end(), (*it)->pvec[3]);
VEC_pD::iterator p5 = std::find(plist.begin(), plist.end(), (*it)->pvec[4]);
size_t i1 = std::distance(plist.begin(), p1);
size_t i2 = std::distance(plist.begin(), p2);
size_t i3 = std::distance(plist.begin(), p3);
size_t i4 = std::distance(plist.begin(), p4);
size_t i5 = std::distance(plist.begin(), p5);
bool npb1 = false;
VEC_pD::iterator np1 =
std::find(clist_params_.begin(), clist_params_.end(), (*it)->pvec[0]);
size_t ni1 = std::distance(clist_params_.begin(), np1);
if (i1 == plist.size()) {
if (ni1 == clist_params_.size()) {
subsystemfile
<< "// Address not in System params...rebuilding into clist_params_"
<< std::endl;
clist_params_.push_back((*it)->pvec[0]);
subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[0])
<< ")); // " << icp << " address: " << (void*)(*it)->pvec[0]
<< std::endl;
icp++;
}
npb1 = true;
}
bool npb2 = false;
VEC_pD::iterator np2 =
std::find(clist_params_.begin(), clist_params_.end(), (*it)->pvec[1]);
size_t ni2 = std::distance(clist_params_.begin(), np2);
if (i2 == plist.size()) {
if (ni2 == clist_params_.size()) {
subsystemfile
<< "// Address not in System params...rebuilding into clist_params_"
<< std::endl;
clist_params_.push_back((*it)->pvec[1]);
subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[1])
<< ")); // " << icp << " address: " << (void*)(*it)->pvec[1]
<< std::endl;
icp++;
}
npb2 = true;
}
bool npb3 = false;
VEC_pD::iterator np3 =
std::find(clist_params_.begin(), clist_params_.end(), (*it)->pvec[2]);
size_t ni3 = std::distance(clist_params_.begin(), np3);
if (i3 == plist.size()) {
if (ni3 == clist_params_.size()) {
subsystemfile
<< "// Address not in System params...rebuilding into clist_params_"
<< std::endl;
clist_params_.push_back((*it)->pvec[2]);
subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[2])
<< ")); // " << icp << " address: " << (void*)(*it)->pvec[2]
<< std::endl;
icp++;
}
npb3 = true;
}
bool npb4 = false;
VEC_pD::iterator np4 =
std::find(clist_params_.begin(), clist_params_.end(), (*it)->pvec[3]);
size_t ni4 = std::distance(clist_params_.begin(), np4);
if (i4 == plist.size()) {
if (ni4 == clist_params_.size()) {
subsystemfile
<< "// Address not in System params...rebuilding into clist_params_"
<< std::endl;
clist_params_.push_back((*it)->pvec[3]);
subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[3])
<< ")); // " << icp << " address: " << (void*)(*it)->pvec[3]
<< std::endl;
icp++;
}
npb4 = true;
}
bool npb5 = false;
VEC_pD::iterator np5 =
std::find(clist_params_.begin(), clist_params_.end(), (*it)->pvec[4]);
size_t ni5 = std::distance(clist_params_.begin(), np5);
if (i5 == plist.size()) {
if (ni5 == clist_params_.size()) {
subsystemfile
<< "// Address not in System params...rebuilding into clist_params_"
<< std::endl;
clist_params_.push_back((*it)->pvec[4]);
subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[4])
<< ")); // " << icp << " address: " << (void*)(*it)->pvec[4]
<< std::endl;
icp++;
}
npb5 = true;
}
subsystemfile << "ConstraintP2PAngle * c" << ic << "=new ConstraintP2PAngle();"
<< std::endl;
subsystemfile << "c" << ic << "->pvec.push_back("
<< (npb1 ? ("clist_params_[") : ("plist_[")) << (npb1 ? ni1 : i1)
<< "]);" << std::endl;
subsystemfile << "c" << ic << "->pvec.push_back("
<< (npb2 ? ("clist_params_[") : ("plist_[")) << (npb2 ? ni2 : i2)
<< "]);" << std::endl;
subsystemfile << "c" << ic << "->pvec.push_back("
<< (npb3 ? ("clist_params_[") : ("plist_[")) << (npb3 ? ni3 : i3)
<< "]);" << std::endl;
subsystemfile << "c" << ic << "->pvec.push_back("
<< (npb4 ? ("clist_params_[") : ("plist_[")) << (npb4 ? ni4 : i4)
<< "]);" << std::endl;
subsystemfile << "c" << ic << "->pvec.push_back("
<< (npb5 ? ("clist_params_[") : ("plist_[")) << (npb5 ? ni5 : i5)
<< "]);" << std::endl;
subsystemfile << "c" << ic << "->origpvec=c" << ic << "->pvec;" << std::endl;
subsystemfile << "c" << ic << "->rescale();" << std::endl;
subsystemfile << "clist_.push_back(c" << ic
<< "); // addresses = " << (*it)->pvec[0] << "," << (*it)->pvec[1]
<< "," << (*it)->pvec[2] << "," << (*it)->pvec[3] << ","
<< (*it)->pvec[4] << std::endl;
break;
}
case P2LDistance: { // 7
VEC_pD::iterator p1 = std::find(plist.begin(), plist.end(), (*it)->pvec[0]);
VEC_pD::iterator p2 = std::find(plist.begin(), plist.end(), (*it)->pvec[1]);
VEC_pD::iterator p3 = std::find(plist.begin(), plist.end(), (*it)->pvec[2]);
VEC_pD::iterator p4 = std::find(plist.begin(), plist.end(), (*it)->pvec[3]);
VEC_pD::iterator p5 = std::find(plist.begin(), plist.end(), (*it)->pvec[4]);
VEC_pD::iterator p6 = std::find(plist.begin(), plist.end(), (*it)->pvec[5]);
VEC_pD::iterator p7 = std::find(plist.begin(), plist.end(), (*it)->pvec[6]);
size_t i1 = std::distance(plist.begin(), p1);
size_t i2 = std::distance(plist.begin(), p2);
size_t i3 = std::distance(plist.begin(), p3);
size_t i4 = std::distance(plist.begin(), p4);
size_t i5 = std::distance(plist.begin(), p5);
size_t i6 = std::distance(plist.begin(), p6);
size_t i7 = std::distance(plist.begin(), p7);
bool npb1 = false;
VEC_pD::iterator np1 =
std::find(clist_params_.begin(), clist_params_.end(), (*it)->pvec[0]);
size_t ni1 = std::distance(clist_params_.begin(), np1);
if (i1 == plist.size()) {
if (ni1 == clist_params_.size()) {
subsystemfile
<< "// Address not in System params...rebuilding into clist_params_"
<< std::endl;
clist_params_.push_back((*it)->pvec[0]);
subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[0])
<< ")); // " << icp << " address: " << (void*)(*it)->pvec[0]
<< std::endl;
icp++;
}
npb1 = true;
}
bool npb2 = false;
VEC_pD::iterator np2 =
std::find(clist_params_.begin(), clist_params_.end(), (*it)->pvec[1]);
size_t ni2 = std::distance(clist_params_.begin(), np2);
if (i2 == plist.size()) {
if (ni2 == clist_params_.size()) {
subsystemfile
<< "// Address not in System params...rebuilding into clist_params_"
<< std::endl;
clist_params_.push_back((*it)->pvec[1]);
subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[1])
<< ")); // " << icp << " address: " << (void*)(*it)->pvec[1]
<< std::endl;
icp++;
}
npb2 = true;
}
bool npb3 = false;
VEC_pD::iterator np3 =
std::find(clist_params_.begin(), clist_params_.end(), (*it)->pvec[2]);
size_t ni3 = std::distance(clist_params_.begin(), np3);
if (i3 == plist.size()) {
if (ni3 == clist_params_.size()) {
subsystemfile
<< "// Address not in System params...rebuilding into clist_params_"
<< std::endl;
clist_params_.push_back((*it)->pvec[2]);
subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[2])
<< ")); // " << icp << " address: " << (void*)(*it)->pvec[2]
<< std::endl;
icp++;
}
npb3 = true;
}
bool npb4 = false;
VEC_pD::iterator np4 =
std::find(clist_params_.begin(), clist_params_.end(), (*it)->pvec[3]);
size_t ni4 = std::distance(clist_params_.begin(), np4);
if (i4 == plist.size()) {
if (ni4 == clist_params_.size()) {
subsystemfile
<< "// Address not in System params...rebuilding into clist_params_"
<< std::endl;
clist_params_.push_back((*it)->pvec[3]);
subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[3])
<< ")); // " << icp << " address: " << (void*)(*it)->pvec[3]
<< std::endl;
icp++;
}
npb4 = true;
}
bool npb5 = false;
VEC_pD::iterator np5 =
std::find(clist_params_.begin(), clist_params_.end(), (*it)->pvec[4]);
size_t ni5 = std::distance(clist_params_.begin(), np5);
if (i5 == plist.size()) {
if (ni5 == clist_params_.size()) {
subsystemfile
<< "// Address not in System params...rebuilding into clist_params_"
<< std::endl;
clist_params_.push_back((*it)->pvec[4]);
subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[4])
<< ")); // " << icp << " address: " << (void*)(*it)->pvec[4]
<< std::endl;
icp++;
}
npb5 = true;
}
bool npb6 = false;
VEC_pD::iterator np6 =
std::find(clist_params_.begin(), clist_params_.end(), (*it)->pvec[5]);
size_t ni6 = std::distance(clist_params_.begin(), np6);
if (i6 == plist.size()) {
if (ni6 == clist_params_.size()) {
subsystemfile
<< "// Address not in System params...rebuilding into clist_params_"
<< std::endl;
clist_params_.push_back((*it)->pvec[5]);
subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[5])
<< ")); // " << icp << " address: " << (void*)(*it)->pvec[5]
<< std::endl;
icp++;
}
npb6 = true;
}
bool npb7 = false;
VEC_pD::iterator np7 =
std::find(clist_params_.begin(), clist_params_.end(), (*it)->pvec[6]);
size_t ni7 = std::distance(clist_params_.begin(), np7);
if (i7 == plist.size()) {
if (ni7 == clist_params_.size()) {
subsystemfile
<< "// Address not in System params...rebuilding into clist_params_"
<< std::endl;
clist_params_.push_back((*it)->pvec[6]);
subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[6])
<< ")); // " << icp << " address: " << (void*)(*it)->pvec[6]
<< std::endl;
icp++;
}
npb7 = true;
}
subsystemfile << "ConstraintP2LDistance * c" << ic
<< "=new ConstraintP2LDistance();" << std::endl;
subsystemfile << "c" << ic << "->pvec.push_back("
<< (npb1 ? ("clist_params_[") : ("plist_[")) << (npb1 ? ni1 : i1)
<< "]);" << std::endl;
subsystemfile << "c" << ic << "->pvec.push_back("
<< (npb2 ? ("clist_params_[") : ("plist_[")) << (npb2 ? ni2 : i2)
<< "]);" << std::endl;
subsystemfile << "c" << ic << "->pvec.push_back("
<< (npb3 ? ("clist_params_[") : ("plist_[")) << (npb3 ? ni3 : i3)
<< "]);" << std::endl;
subsystemfile << "c" << ic << "->pvec.push_back("
<< (npb4 ? ("clist_params_[") : ("plist_[")) << (npb4 ? ni4 : i4)
<< "]);" << std::endl;
subsystemfile << "c" << ic << "->pvec.push_back("
<< (npb5 ? ("clist_params_[") : ("plist_[")) << (npb5 ? ni5 : i5)
<< "]);" << std::endl;
subsystemfile << "c" << ic << "->pvec.push_back("
<< (npb6 ? ("clist_params_[") : ("plist_[")) << (npb6 ? ni6 : i6)
<< "]);" << std::endl;
subsystemfile << "c" << ic << "->pvec.push_back("
<< (npb7 ? ("clist_params_[") : ("plist_[")) << (npb7 ? ni7 : i7)
<< "]);" << std::endl;
subsystemfile << "c" << ic << "->origpvec=c" << ic << "->pvec;" << std::endl;
subsystemfile << "c" << ic << "->rescale();" << std::endl;
subsystemfile << "clist_.push_back(c" << ic
<< "); // addresses = " << (*it)->pvec[0] << "," << (*it)->pvec[1]
<< "," << (*it)->pvec[2] << "," << (*it)->pvec[3] << ","
<< (*it)->pvec[4] << "," << (*it)->pvec[5] << "," << (*it)->pvec[6]
<< std::endl;
break;
}
case PointOnLine: { // 6
VEC_pD::iterator p1 = std::find(plist.begin(), plist.end(), (*it)->pvec[0]);
VEC_pD::iterator p2 = std::find(plist.begin(), plist.end(), (*it)->pvec[1]);
VEC_pD::iterator p3 = std::find(plist.begin(), plist.end(), (*it)->pvec[2]);
VEC_pD::iterator p4 = std::find(plist.begin(), plist.end(), (*it)->pvec[3]);
VEC_pD::iterator p5 = std::find(plist.begin(), plist.end(), (*it)->pvec[4]);
VEC_pD::iterator p6 = std::find(plist.begin(), plist.end(), (*it)->pvec[5]);
size_t i1 = std::distance(plist.begin(), p1);
size_t i2 = std::distance(plist.begin(), p2);
size_t i3 = std::distance(plist.begin(), p3);
size_t i4 = std::distance(plist.begin(), p4);
size_t i5 = std::distance(plist.begin(), p5);
size_t i6 = std::distance(plist.begin(), p6);
bool npb1 = false;
VEC_pD::iterator np1 =
std::find(clist_params_.begin(), clist_params_.end(), (*it)->pvec[0]);
size_t ni1 = std::distance(clist_params_.begin(), np1);
if (i1 == plist.size()) {
if (ni1 == clist_params_.size()) {
subsystemfile
<< "// Address not in System params...rebuilding into clist_params_"
<< std::endl;
clist_params_.push_back((*it)->pvec[0]);
subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[0])
<< ")); // " << icp << " address: " << (void*)(*it)->pvec[0]
<< std::endl;
icp++;
}
npb1 = true;
}
bool npb2 = false;
VEC_pD::iterator np2 =
std::find(clist_params_.begin(), clist_params_.end(), (*it)->pvec[1]);
size_t ni2 = std::distance(clist_params_.begin(), np2);
if (i2 == plist.size()) {
if (ni2 == clist_params_.size()) {
subsystemfile
<< "// Address not in System params...rebuilding into clist_params_"
<< std::endl;
clist_params_.push_back((*it)->pvec[1]);
subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[1])
<< ")); // " << icp << " address: " << (void*)(*it)->pvec[1]
<< std::endl;
icp++;
}
npb2 = true;
}
bool npb3 = false;
VEC_pD::iterator np3 =
std::find(clist_params_.begin(), clist_params_.end(), (*it)->pvec[2]);
size_t ni3 = std::distance(clist_params_.begin(), np3);
if (i3 == plist.size()) {
if (ni3 == clist_params_.size()) {
subsystemfile
<< "// Address not in System params...rebuilding into clist_params_"
<< std::endl;
clist_params_.push_back((*it)->pvec[2]);
subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[2])
<< ")); // " << icp << " address: " << (void*)(*it)->pvec[2]
<< std::endl;
icp++;
}
npb3 = true;
}
bool npb4 = false;
VEC_pD::iterator np4 =
std::find(clist_params_.begin(), clist_params_.end(), (*it)->pvec[3]);
size_t ni4 = std::distance(clist_params_.begin(), np4);
if (i4 == plist.size()) {
if (ni4 == clist_params_.size()) {
subsystemfile
<< "// Address not in System params...rebuilding into clist_params_"
<< std::endl;
clist_params_.push_back((*it)->pvec[3]);
subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[3])
<< ")); // " << icp << " address: " << (void*)(*it)->pvec[3]
<< std::endl;
icp++;
}
npb4 = true;
}
bool npb5 = false;
VEC_pD::iterator np5 =
std::find(clist_params_.begin(), clist_params_.end(), (*it)->pvec[4]);
size_t ni5 = std::distance(clist_params_.begin(), np5);
if (i5 == plist.size()) {
if (ni5 == clist_params_.size()) {
subsystemfile
<< "// Address not in System params...rebuilding into clist_params_"
<< std::endl;
clist_params_.push_back((*it)->pvec[4]);
subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[4])
<< ")); // " << icp << " address: " << (void*)(*it)->pvec[4]
<< std::endl;
icp++;
}
npb5 = true;
}
bool npb6 = false;
VEC_pD::iterator np6 =
std::find(clist_params_.begin(), clist_params_.end(), (*it)->pvec[5]);
size_t ni6 = std::distance(clist_params_.begin(), np6);
if (i6 == plist.size()) {
if (ni6 == clist_params_.size()) {
subsystemfile
<< "// Address not in System params...rebuilding into clist_params_"
<< std::endl;
clist_params_.push_back((*it)->pvec[5]);
subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[5])
<< ")); // " << icp << " address: " << (void*)(*it)->pvec[5]
<< std::endl;
icp++;
}
npb6 = true;
}
subsystemfile << "ConstraintPointOnLine * c" << ic
<< "=new ConstraintPointOnLine();" << std::endl;
subsystemfile << "c" << ic << "->pvec.push_back("
<< (npb1 ? ("clist_params_[") : ("plist_[")) << (npb1 ? ni1 : i1)
<< "]);" << std::endl;
subsystemfile << "c" << ic << "->pvec.push_back("
<< (npb2 ? ("clist_params_[") : ("plist_[")) << (npb2 ? ni2 : i2)
<< "]);" << std::endl;
subsystemfile << "c" << ic << "->pvec.push_back("
<< (npb3 ? ("clist_params_[") : ("plist_[")) << (npb3 ? ni3 : i3)
<< "]);" << std::endl;
subsystemfile << "c" << ic << "->pvec.push_back("
<< (npb4 ? ("clist_params_[") : ("plist_[")) << (npb4 ? ni4 : i4)
<< "]);" << std::endl;
subsystemfile << "c" << ic << "->pvec.push_back("
<< (npb5 ? ("clist_params_[") : ("plist_[")) << (npb5 ? ni5 : i5)
<< "]);" << std::endl;
subsystemfile << "c" << ic << "->pvec.push_back("
<< (npb6 ? ("clist_params_[") : ("plist_[")) << (npb6 ? ni6 : i6)
<< "]);" << std::endl;
subsystemfile << "c" << ic << "->origpvec=c" << ic << "->pvec;" << std::endl;
subsystemfile << "c" << ic << "->rescale();" << std::endl;
subsystemfile << "clist_.push_back(c" << ic
<< "); // addresses = " << (*it)->pvec[0] << "," << (*it)->pvec[1]
<< "," << (*it)->pvec[2] << "," << (*it)->pvec[3] << ","
<< (*it)->pvec[4] << "," << (*it)->pvec[5] << std::endl;
break;
}
case PointOnPerpBisector: { // 6
VEC_pD::iterator p1 = std::find(plist.begin(), plist.end(), (*it)->pvec[0]);
VEC_pD::iterator p2 = std::find(plist.begin(), plist.end(), (*it)->pvec[1]);
VEC_pD::iterator p3 = std::find(plist.begin(), plist.end(), (*it)->pvec[2]);
VEC_pD::iterator p4 = std::find(plist.begin(), plist.end(), (*it)->pvec[3]);
VEC_pD::iterator p5 = std::find(plist.begin(), plist.end(), (*it)->pvec[4]);
VEC_pD::iterator p6 = std::find(plist.begin(), plist.end(), (*it)->pvec[5]);
size_t i1 = std::distance(plist.begin(), p1);
size_t i2 = std::distance(plist.begin(), p2);
size_t i3 = std::distance(plist.begin(), p3);
size_t i4 = std::distance(plist.begin(), p4);
size_t i5 = std::distance(plist.begin(), p5);
size_t i6 = std::distance(plist.begin(), p6);
bool npb1 = false;
VEC_pD::iterator np1 =
std::find(clist_params_.begin(), clist_params_.end(), (*it)->pvec[0]);
size_t ni1 = std::distance(clist_params_.begin(), np1);
if (i1 == plist.size()) {
if (ni1 == clist_params_.size()) {
subsystemfile
<< "// Address not in System params...rebuilding into clist_params_"
<< std::endl;
clist_params_.push_back((*it)->pvec[0]);
subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[0])
<< ")); // " << icp << " address: " << (void*)(*it)->pvec[0]
<< std::endl;
icp++;
}
npb1 = true;
}
bool npb2 = false;
VEC_pD::iterator np2 =
std::find(clist_params_.begin(), clist_params_.end(), (*it)->pvec[1]);
size_t ni2 = std::distance(clist_params_.begin(), np2);
if (i2 == plist.size()) {
if (ni2 == clist_params_.size()) {
subsystemfile
<< "// Address not in System params...rebuilding into clist_params_"
<< std::endl;
clist_params_.push_back((*it)->pvec[1]);
subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[1])
<< ")); // " << icp << " address: " << (void*)(*it)->pvec[1]
<< std::endl;
icp++;
}
npb2 = true;
}
bool npb3 = false;
VEC_pD::iterator np3 =
std::find(clist_params_.begin(), clist_params_.end(), (*it)->pvec[2]);
size_t ni3 = std::distance(clist_params_.begin(), np3);
if (i3 == plist.size()) {
if (ni3 == clist_params_.size()) {
subsystemfile
<< "// Address not in System params...rebuilding into clist_params_"
<< std::endl;
clist_params_.push_back((*it)->pvec[2]);
subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[2])
<< ")); // " << icp << " address: " << (void*)(*it)->pvec[2]
<< std::endl;
icp++;
}
npb3 = true;
}
bool npb4 = false;
VEC_pD::iterator np4 =
std::find(clist_params_.begin(), clist_params_.end(), (*it)->pvec[3]);
size_t ni4 = std::distance(clist_params_.begin(), np4);
if (i4 == plist.size()) {
if (ni4 == clist_params_.size()) {
subsystemfile
<< "// Address not in System params...rebuilding into clist_params_"
<< std::endl;
clist_params_.push_back((*it)->pvec[3]);
subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[3])
<< ")); // " << icp << " address: " << (void*)(*it)->pvec[3]
<< std::endl;
icp++;
}
npb4 = true;
}
bool npb5 = false;
VEC_pD::iterator np5 =
std::find(clist_params_.begin(), clist_params_.end(), (*it)->pvec[4]);
size_t ni5 = std::distance(clist_params_.begin(), np5);
if (i5 == plist.size()) {
if (ni5 == clist_params_.size()) {
subsystemfile
<< "// Address not in System params...rebuilding into clist_params_"
<< std::endl;
clist_params_.push_back((*it)->pvec[4]);
subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[4])
<< ")); // " << icp << " address: " << (void*)(*it)->pvec[4]
<< std::endl;
icp++;
}
npb5 = true;
}
bool npb6 = false;
VEC_pD::iterator np6 =
std::find(clist_params_.begin(), clist_params_.end(), (*it)->pvec[5]);
size_t ni6 = std::distance(clist_params_.begin(), np6);
if (i6 == plist.size()) {
if (ni6 == clist_params_.size()) {
subsystemfile
<< "// Address not in System params...rebuilding into clist_params_"
<< std::endl;
clist_params_.push_back((*it)->pvec[5]);
subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[5])
<< ")); // " << icp << " address: " << (void*)(*it)->pvec[5]
<< std::endl;
icp++;
}
npb6 = true;
}
subsystemfile << "ConstraintPointOnPerpBisector * c" << ic
<< "=new ConstraintPointOnPerpBisector();" << std::endl;
subsystemfile << "c" << ic << "->pvec.push_back("
<< (npb1 ? ("clist_params_[") : ("plist_[")) << (npb1 ? ni1 : i1)
<< "]);" << std::endl;
subsystemfile << "c" << ic << "->pvec.push_back("
<< (npb2 ? ("clist_params_[") : ("plist_[")) << (npb2 ? ni2 : i2)
<< "]);" << std::endl;
subsystemfile << "c" << ic << "->pvec.push_back("
<< (npb3 ? ("clist_params_[") : ("plist_[")) << (npb3 ? ni3 : i3)
<< "]);" << std::endl;
subsystemfile << "c" << ic << "->pvec.push_back("
<< (npb4 ? ("clist_params_[") : ("plist_[")) << (npb4 ? ni4 : i4)
<< "]);" << std::endl;
subsystemfile << "c" << ic << "->pvec.push_back("
<< (npb5 ? ("clist_params_[") : ("plist_[")) << (npb5 ? ni5 : i5)
<< "]);" << std::endl;
subsystemfile << "c" << ic << "->pvec.push_back("
<< (npb6 ? ("clist_params_[") : ("plist_[")) << (npb6 ? ni6 : i6)
<< "]);" << std::endl;
subsystemfile << "c" << ic << "->origpvec=c" << ic << "->pvec;" << std::endl;
subsystemfile << "c" << ic << "->rescale();" << std::endl;
subsystemfile << "clist_.push_back(c" << ic
<< "); // addresses = " << (*it)->pvec[0] << "," << (*it)->pvec[1]
<< "," << (*it)->pvec[2] << "," << (*it)->pvec[3] << ","
<< (*it)->pvec[4] << "," << (*it)->pvec[5] << std::endl;
break;
}
case Parallel: { // 8
VEC_pD::iterator p1 = std::find(plist.begin(), plist.end(), (*it)->pvec[0]);
VEC_pD::iterator p2 = std::find(plist.begin(), plist.end(), (*it)->pvec[1]);
VEC_pD::iterator p3 = std::find(plist.begin(), plist.end(), (*it)->pvec[2]);
VEC_pD::iterator p4 = std::find(plist.begin(), plist.end(), (*it)->pvec[3]);
VEC_pD::iterator p5 = std::find(plist.begin(), plist.end(), (*it)->pvec[4]);
VEC_pD::iterator p6 = std::find(plist.begin(), plist.end(), (*it)->pvec[5]);
VEC_pD::iterator p7 = std::find(plist.begin(), plist.end(), (*it)->pvec[6]);
VEC_pD::iterator p8 = std::find(plist.begin(), plist.end(), (*it)->pvec[7]);
size_t i1 = std::distance(plist.begin(), p1);
size_t i2 = std::distance(plist.begin(), p2);
size_t i3 = std::distance(plist.begin(), p3);
size_t i4 = std::distance(plist.begin(), p4);
size_t i5 = std::distance(plist.begin(), p5);
size_t i6 = std::distance(plist.begin(), p6);
size_t i7 = std::distance(plist.begin(), p7);
size_t i8 = std::distance(plist.begin(), p8);
bool npb1 = false;
VEC_pD::iterator np1 =
std::find(clist_params_.begin(), clist_params_.end(), (*it)->pvec[0]);
size_t ni1 = std::distance(clist_params_.begin(), np1);
if (i1 == plist.size()) {
if (ni1 == clist_params_.size()) {
subsystemfile
<< "// Address not in System params...rebuilding into clist_params_"
<< std::endl;
clist_params_.push_back((*it)->pvec[0]);
subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[0])
<< ")); // " << icp << " address: " << (void*)(*it)->pvec[0]
<< std::endl;
icp++;
}
npb1 = true;
}
bool npb2 = false;
VEC_pD::iterator np2 =
std::find(clist_params_.begin(), clist_params_.end(), (*it)->pvec[1]);
size_t ni2 = std::distance(clist_params_.begin(), np2);
if (i2 == plist.size()) {
if (ni2 == clist_params_.size()) {
subsystemfile
<< "// Address not in System params...rebuilding into clist_params_"
<< std::endl;
clist_params_.push_back((*it)->pvec[1]);
subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[1])
<< ")); // " << icp << " address: " << (void*)(*it)->pvec[1]
<< std::endl;
icp++;
}
npb2 = true;
}
bool npb3 = false;
VEC_pD::iterator np3 =
std::find(clist_params_.begin(), clist_params_.end(), (*it)->pvec[2]);
size_t ni3 = std::distance(clist_params_.begin(), np3);
if (i3 == plist.size()) {
if (ni3 == clist_params_.size()) {
subsystemfile
<< "// Address not in System params...rebuilding into clist_params_"
<< std::endl;
clist_params_.push_back((*it)->pvec[2]);
subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[2])
<< ")); // " << icp << " address: " << (void*)(*it)->pvec[2]
<< std::endl;
icp++;
}
npb3 = true;
}
bool npb4 = false;
VEC_pD::iterator np4 =
std::find(clist_params_.begin(), clist_params_.end(), (*it)->pvec[3]);
size_t ni4 = std::distance(clist_params_.begin(), np4);
if (i4 == plist.size()) {
if (ni4 == clist_params_.size()) {
subsystemfile
<< "// Address not in System params...rebuilding into clist_params_"
<< std::endl;
clist_params_.push_back((*it)->pvec[3]);
subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[3])
<< ")); // " << icp << " address: " << (void*)(*it)->pvec[3]
<< std::endl;
icp++;
}
npb4 = true;
}
bool npb5 = false;
VEC_pD::iterator np5 =
std::find(clist_params_.begin(), clist_params_.end(), (*it)->pvec[4]);
size_t ni5 = std::distance(clist_params_.begin(), np5);
if (i5 == plist.size()) {
if (ni5 == clist_params_.size()) {
subsystemfile
<< "// Address not in System params...rebuilding into clist_params_"
<< std::endl;
clist_params_.push_back((*it)->pvec[4]);
subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[4])
<< ")); // " << icp << " address: " << (void*)(*it)->pvec[4]
<< std::endl;
icp++;
}
npb5 = true;
}
bool npb6 = false;
VEC_pD::iterator np6 =
std::find(clist_params_.begin(), clist_params_.end(), (*it)->pvec[5]);
size_t ni6 = std::distance(clist_params_.begin(), np6);
if (i6 == plist.size()) {
if (ni6 == clist_params_.size()) {
subsystemfile
<< "// Address not in System params...rebuilding into clist_params_"
<< std::endl;
clist_params_.push_back((*it)->pvec[5]);
subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[5])
<< ")); // " << icp << " address: " << (void*)(*it)->pvec[5]
<< std::endl;
icp++;
}
npb6 = true;
}
bool npb7 = false;
VEC_pD::iterator np7 =
std::find(clist_params_.begin(), clist_params_.end(), (*it)->pvec[6]);
size_t ni7 = std::distance(clist_params_.begin(), np7);
if (i7 == plist.size()) {
if (ni7 == clist_params_.size()) {
subsystemfile
<< "// Address not in System params...rebuilding into clist_params_"
<< std::endl;
clist_params_.push_back((*it)->pvec[6]);
subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[6])
<< ")); // " << icp << " address: " << (void*)(*it)->pvec[6]
<< std::endl;
icp++;
}
npb7 = true;
}
bool npb8 = false;
VEC_pD::iterator np8 =
std::find(clist_params_.begin(), clist_params_.end(), (*it)->pvec[7]);
size_t ni8 = std::distance(clist_params_.begin(), np8);
if (i8 == plist.size()) {
if (ni8 == clist_params_.size()) {
subsystemfile
<< "// Address not in System params...rebuilding into clist_params_"
<< std::endl;
clist_params_.push_back((*it)->pvec[7]);
subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[7])
<< ")); // " << icp << " address: " << (void*)(*it)->pvec[7]
<< std::endl;
icp++;
}
npb8 = true;
}
subsystemfile << "ConstraintParallel * c" << ic << "=new ConstraintParallel();"
<< std::endl;
subsystemfile << "c" << ic << "->pvec.push_back("
<< (npb1 ? ("clist_params_[") : ("plist_[")) << (npb1 ? ni1 : i1)
<< "]);" << std::endl;
subsystemfile << "c" << ic << "->pvec.push_back("
<< (npb2 ? ("clist_params_[") : ("plist_[")) << (npb2 ? ni2 : i2)
<< "]);" << std::endl;
subsystemfile << "c" << ic << "->pvec.push_back("
<< (npb3 ? ("clist_params_[") : ("plist_[")) << (npb3 ? ni3 : i3)
<< "]);" << std::endl;
subsystemfile << "c" << ic << "->pvec.push_back("
<< (npb4 ? ("clist_params_[") : ("plist_[")) << (npb4 ? ni4 : i4)
<< "]);" << std::endl;
subsystemfile << "c" << ic << "->pvec.push_back("
<< (npb5 ? ("clist_params_[") : ("plist_[")) << (npb5 ? ni5 : i5)
<< "]);" << std::endl;
subsystemfile << "c" << ic << "->pvec.push_back("
<< (npb6 ? ("clist_params_[") : ("plist_[")) << (npb6 ? ni6 : i6)
<< "]);" << std::endl;
subsystemfile << "c" << ic << "->pvec.push_back("
<< (npb7 ? ("clist_params_[") : ("plist_[")) << (npb7 ? ni7 : i7)
<< "]);" << std::endl;
subsystemfile << "c" << ic << "->pvec.push_back("
<< (npb8 ? ("clist_params_[") : ("plist_[")) << (npb8 ? ni8 : i8)
<< "]);" << std::endl;
subsystemfile << "c" << ic << "->origpvec=c" << ic << "->pvec;" << std::endl;
subsystemfile << "c" << ic << "->rescale();" << std::endl;
subsystemfile << "clist_.push_back(c" << ic
<< "); // addresses = " << (*it)->pvec[0] << "," << (*it)->pvec[1]
<< "," << (*it)->pvec[2] << "," << (*it)->pvec[3] << ","
<< (*it)->pvec[4] << "," << (*it)->pvec[5] << "," << (*it)->pvec[6]
<< "," << (*it)->pvec[7] << std::endl;
break;
}
case Perpendicular: { // 8
VEC_pD::iterator p1 = std::find(plist.begin(), plist.end(), (*it)->pvec[0]);
VEC_pD::iterator p2 = std::find(plist.begin(), plist.end(), (*it)->pvec[1]);
VEC_pD::iterator p3 = std::find(plist.begin(), plist.end(), (*it)->pvec[2]);
VEC_pD::iterator p4 = std::find(plist.begin(), plist.end(), (*it)->pvec[3]);
VEC_pD::iterator p5 = std::find(plist.begin(), plist.end(), (*it)->pvec[4]);
VEC_pD::iterator p6 = std::find(plist.begin(), plist.end(), (*it)->pvec[5]);
VEC_pD::iterator p7 = std::find(plist.begin(), plist.end(), (*it)->pvec[6]);
VEC_pD::iterator p8 = std::find(plist.begin(), plist.end(), (*it)->pvec[7]);
size_t i1 = std::distance(plist.begin(), p1);
size_t i2 = std::distance(plist.begin(), p2);
size_t i3 = std::distance(plist.begin(), p3);
size_t i4 = std::distance(plist.begin(), p4);
size_t i5 = std::distance(plist.begin(), p5);
size_t i6 = std::distance(plist.begin(), p6);
size_t i7 = std::distance(plist.begin(), p7);
size_t i8 = std::distance(plist.begin(), p8);
bool npb1 = false;
VEC_pD::iterator np1 =
std::find(clist_params_.begin(), clist_params_.end(), (*it)->pvec[0]);
size_t ni1 = std::distance(clist_params_.begin(), np1);
if (i1 == plist.size()) {
if (ni1 == clist_params_.size()) {
subsystemfile
<< "// Address not in System params...rebuilding into clist_params_"
<< std::endl;
clist_params_.push_back((*it)->pvec[0]);
subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[0])
<< ")); // " << icp << " address: " << (void*)(*it)->pvec[0]
<< std::endl;
icp++;
}
npb1 = true;
}
bool npb2 = false;
VEC_pD::iterator np2 =
std::find(clist_params_.begin(), clist_params_.end(), (*it)->pvec[1]);
size_t ni2 = std::distance(clist_params_.begin(), np2);
if (i2 == plist.size()) {
if (ni2 == clist_params_.size()) {
subsystemfile
<< "// Address not in System params...rebuilding into clist_params_"
<< std::endl;
clist_params_.push_back((*it)->pvec[1]);
subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[1])
<< ")); // " << icp << " address: " << (void*)(*it)->pvec[1]
<< std::endl;
icp++;
}
npb2 = true;
}
bool npb3 = false;
VEC_pD::iterator np3 =
std::find(clist_params_.begin(), clist_params_.end(), (*it)->pvec[2]);
size_t ni3 = std::distance(clist_params_.begin(), np3);
if (i3 == plist.size()) {
if (ni3 == clist_params_.size()) {
subsystemfile
<< "// Address not in System params...rebuilding into clist_params_"
<< std::endl;
clist_params_.push_back((*it)->pvec[2]);
subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[2])
<< ")); // " << icp << " address: " << (void*)(*it)->pvec[2]
<< std::endl;
icp++;
}
npb3 = true;
}
bool npb4 = false;
VEC_pD::iterator np4 =
std::find(clist_params_.begin(), clist_params_.end(), (*it)->pvec[3]);
size_t ni4 = std::distance(clist_params_.begin(), np4);
if (i4 == plist.size()) {
if (ni4 == clist_params_.size()) {
subsystemfile
<< "// Address not in System params...rebuilding into clist_params_"
<< std::endl;
clist_params_.push_back((*it)->pvec[3]);
subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[3])
<< ")); // " << icp << " address: " << (void*)(*it)->pvec[3]
<< std::endl;
icp++;
}
npb4 = true;
}
bool npb5 = false;
VEC_pD::iterator np5 =
std::find(clist_params_.begin(), clist_params_.end(), (*it)->pvec[4]);
size_t ni5 = std::distance(clist_params_.begin(), np5);
if (i5 == plist.size()) {
if (ni5 == clist_params_.size()) {
subsystemfile
<< "// Address not in System params...rebuilding into clist_params_"
<< std::endl;
clist_params_.push_back((*it)->pvec[4]);
subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[4])
<< ")); // " << icp << " address: " << (void*)(*it)->pvec[4]
<< std::endl;
icp++;
}
npb5 = true;
}
bool npb6 = false;
VEC_pD::iterator np6 =
std::find(clist_params_.begin(), clist_params_.end(), (*it)->pvec[5]);
size_t ni6 = std::distance(clist_params_.begin(), np6);
if (i6 == plist.size()) {
if (ni6 == clist_params_.size()) {
subsystemfile
<< "// Address not in System params...rebuilding into clist_params_"
<< std::endl;
clist_params_.push_back((*it)->pvec[5]);
subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[5])
<< ")); // " << icp << " address: " << (void*)(*it)->pvec[5]
<< std::endl;
icp++;
}
npb6 = true;
}
bool npb7 = false;
VEC_pD::iterator np7 =
std::find(clist_params_.begin(), clist_params_.end(), (*it)->pvec[6]);
size_t ni7 = std::distance(clist_params_.begin(), np7);
if (i7 == plist.size()) {
if (ni7 == clist_params_.size()) {
subsystemfile
<< "// Address not in System params...rebuilding into clist_params_"
<< std::endl;
clist_params_.push_back((*it)->pvec[6]);
subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[6])
<< ")); // " << icp << " address: " << (void*)(*it)->pvec[6]
<< std::endl;
icp++;
}
npb7 = true;
}
bool npb8 = false;
VEC_pD::iterator np8 =
std::find(clist_params_.begin(), clist_params_.end(), (*it)->pvec[7]);
size_t ni8 = std::distance(clist_params_.begin(), np8);
if (i8 == plist.size()) {
if (ni8 == clist_params_.size()) {
subsystemfile
<< "// Address not in System params...rebuilding into clist_params_"
<< std::endl;
clist_params_.push_back((*it)->pvec[7]);
subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[7])
<< ")); // " << icp << " address: " << (void*)(*it)->pvec[7]
<< std::endl;
icp++;
}
npb8 = true;
}
subsystemfile << "ConstraintPerpendicular * c" << ic
<< "=new ConstraintPerpendicular();" << std::endl;
subsystemfile << "c" << ic << "->pvec.push_back("
<< (npb1 ? ("clist_params_[") : ("plist_[")) << (npb1 ? ni1 : i1)
<< "]);" << std::endl;
subsystemfile << "c" << ic << "->pvec.push_back("
<< (npb2 ? ("clist_params_[") : ("plist_[")) << (npb2 ? ni2 : i2)
<< "]);" << std::endl;
subsystemfile << "c" << ic << "->pvec.push_back("
<< (npb3 ? ("clist_params_[") : ("plist_[")) << (npb3 ? ni3 : i3)
<< "]);" << std::endl;
subsystemfile << "c" << ic << "->pvec.push_back("
<< (npb4 ? ("clist_params_[") : ("plist_[")) << (npb4 ? ni4 : i4)
<< "]);" << std::endl;
subsystemfile << "c" << ic << "->pvec.push_back("
<< (npb5 ? ("clist_params_[") : ("plist_[")) << (npb5 ? ni5 : i5)
<< "]);" << std::endl;
subsystemfile << "c" << ic << "->pvec.push_back("
<< (npb6 ? ("clist_params_[") : ("plist_[")) << (npb6 ? ni6 : i6)
<< "]);" << std::endl;
subsystemfile << "c" << ic << "->pvec.push_back("
<< (npb7 ? ("clist_params_[") : ("plist_[")) << (npb7 ? ni7 : i7)
<< "]);" << std::endl;
subsystemfile << "c" << ic << "->pvec.push_back("
<< (npb8 ? ("clist_params_[") : ("plist_[")) << (npb8 ? ni8 : i8)
<< "]);" << std::endl;
subsystemfile << "c" << ic << "->origpvec=c" << ic << "->pvec;" << std::endl;
subsystemfile << "c" << ic << "->rescale();" << std::endl;
subsystemfile << "clist_.push_back(c" << ic
<< "); // addresses = " << (*it)->pvec[0] << "," << (*it)->pvec[1]
<< "," << (*it)->pvec[2] << "," << (*it)->pvec[3] << ","
<< (*it)->pvec[4] << "," << (*it)->pvec[5] << "," << (*it)->pvec[6]
<< "," << (*it)->pvec[7] << std::endl;
break;
}
case L2LAngle: { // 9
VEC_pD::iterator p1 = std::find(plist.begin(), plist.end(), (*it)->pvec[0]);
VEC_pD::iterator p2 = std::find(plist.begin(), plist.end(), (*it)->pvec[1]);
VEC_pD::iterator p3 = std::find(plist.begin(), plist.end(), (*it)->pvec[2]);
VEC_pD::iterator p4 = std::find(plist.begin(), plist.end(), (*it)->pvec[3]);
VEC_pD::iterator p5 = std::find(plist.begin(), plist.end(), (*it)->pvec[4]);
VEC_pD::iterator p6 = std::find(plist.begin(), plist.end(), (*it)->pvec[5]);
VEC_pD::iterator p7 = std::find(plist.begin(), plist.end(), (*it)->pvec[6]);
VEC_pD::iterator p8 = std::find(plist.begin(), plist.end(), (*it)->pvec[7]);
VEC_pD::iterator p9 = std::find(plist.begin(), plist.end(), (*it)->pvec[8]);
size_t i1 = std::distance(plist.begin(), p1);
size_t i2 = std::distance(plist.begin(), p2);
size_t i3 = std::distance(plist.begin(), p3);
size_t i4 = std::distance(plist.begin(), p4);
size_t i5 = std::distance(plist.begin(), p5);
size_t i6 = std::distance(plist.begin(), p6);
size_t i7 = std::distance(plist.begin(), p7);
size_t i8 = std::distance(plist.begin(), p8);
size_t i9 = std::distance(plist.begin(), p9);
bool npb1 = false;
VEC_pD::iterator np1 =
std::find(clist_params_.begin(), clist_params_.end(), (*it)->pvec[0]);
size_t ni1 = std::distance(clist_params_.begin(), np1);
if (i1 == plist.size()) {
if (ni1 == clist_params_.size()) {
subsystemfile
<< "// Address not in System params...rebuilding into clist_params_"
<< std::endl;
clist_params_.push_back((*it)->pvec[0]);
subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[0])
<< ")); // " << icp << " address: " << (void*)(*it)->pvec[0]
<< std::endl;
icp++;
}
npb1 = true;
}
bool npb2 = false;
VEC_pD::iterator np2 =
std::find(clist_params_.begin(), clist_params_.end(), (*it)->pvec[1]);
size_t ni2 = std::distance(clist_params_.begin(), np2);
if (i2 == plist.size()) {
if (ni2 == clist_params_.size()) {
subsystemfile
<< "// Address not in System params...rebuilding into clist_params_"
<< std::endl;
clist_params_.push_back((*it)->pvec[1]);
subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[1])
<< ")); // " << icp << " address: " << (void*)(*it)->pvec[1]
<< std::endl;
icp++;
}
npb2 = true;
}
bool npb3 = false;
VEC_pD::iterator np3 =
std::find(clist_params_.begin(), clist_params_.end(), (*it)->pvec[2]);
size_t ni3 = std::distance(clist_params_.begin(), np3);
if (i3 == plist.size()) {
if (ni3 == clist_params_.size()) {
subsystemfile
<< "// Address not in System params...rebuilding into clist_params_"
<< std::endl;
clist_params_.push_back((*it)->pvec[2]);
subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[2])
<< ")); // " << icp << " address: " << (void*)(*it)->pvec[2]
<< std::endl;
icp++;
}
npb3 = true;
}
bool npb4 = false;
VEC_pD::iterator np4 =
std::find(clist_params_.begin(), clist_params_.end(), (*it)->pvec[3]);
size_t ni4 = std::distance(clist_params_.begin(), np4);
if (i4 == plist.size()) {
if (ni4 == clist_params_.size()) {
subsystemfile
<< "// Address not in System params...rebuilding into clist_params_"
<< std::endl;
clist_params_.push_back((*it)->pvec[3]);
subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[3])
<< ")); // " << icp << " address: " << (void*)(*it)->pvec[3]
<< std::endl;
icp++;
}
npb4 = true;
}
bool npb5 = false;
VEC_pD::iterator np5 =
std::find(clist_params_.begin(), clist_params_.end(), (*it)->pvec[4]);
size_t ni5 = std::distance(clist_params_.begin(), np5);
if (i5 == plist.size()) {
if (ni5 == clist_params_.size()) {
subsystemfile
<< "// Address not in System params...rebuilding into clist_params_"
<< std::endl;
clist_params_.push_back((*it)->pvec[4]);
subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[4])
<< ")); // " << icp << " address: " << (void*)(*it)->pvec[4]
<< std::endl;
icp++;
}
npb5 = true;
}
bool npb6 = false;
VEC_pD::iterator np6 =
std::find(clist_params_.begin(), clist_params_.end(), (*it)->pvec[5]);
size_t ni6 = std::distance(clist_params_.begin(), np6);
if (i6 == plist.size()) {
if (ni6 == clist_params_.size()) {
subsystemfile
<< "// Address not in System params...rebuilding into clist_params_"
<< std::endl;
clist_params_.push_back((*it)->pvec[5]);
subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[5])
<< ")); // " << icp << " address: " << (void*)(*it)->pvec[5]
<< std::endl;
icp++;
}
npb6 = true;
}
bool npb7 = false;
VEC_pD::iterator np7 =
std::find(clist_params_.begin(), clist_params_.end(), (*it)->pvec[6]);
size_t ni7 = std::distance(clist_params_.begin(), np7);
if (i7 == plist.size()) {
if (ni7 == clist_params_.size()) {
subsystemfile
<< "// Address not in System params...rebuilding into clist_params_"
<< std::endl;
clist_params_.push_back((*it)->pvec[6]);
subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[6])
<< ")); // " << icp << " address: " << (void*)(*it)->pvec[6]
<< std::endl;
icp++;
}
npb7 = true;
}
bool npb8 = false;
VEC_pD::iterator np8 =
std::find(clist_params_.begin(), clist_params_.end(), (*it)->pvec[7]);
size_t ni8 = std::distance(clist_params_.begin(), np8);
if (i8 == plist.size()) {
if (ni8 == clist_params_.size()) {
subsystemfile
<< "// Address not in System params...rebuilding into clist_params_"
<< std::endl;
clist_params_.push_back((*it)->pvec[7]);
subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[7])
<< ")); // " << icp << " address: " << (void*)(*it)->pvec[7]
<< std::endl;
icp++;
}
npb8 = true;
}
bool npb9 = false;
VEC_pD::iterator np9 =
std::find(clist_params_.begin(), clist_params_.end(), (*it)->pvec[8]);
size_t ni9 = std::distance(clist_params_.begin(), np9);
if (i9 == plist.size()) {
if (ni9 == clist_params_.size()) {
subsystemfile
<< "// Address not in System params...rebuilding into clist_params_"
<< std::endl;
clist_params_.push_back((*it)->pvec[8]);
subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[8])
<< ")); // " << icp << " address: " << (void*)(*it)->pvec[8]
<< std::endl;
icp++;
}
npb9 = true;
}
subsystemfile << "ConstraintL2LAngle * c" << ic << "=new ConstraintL2LAngle();"
<< std::endl;
subsystemfile << "c" << ic << "->pvec.push_back("
<< (npb1 ? ("clist_params_[") : ("plist_[")) << (npb1 ? ni1 : i1)
<< "]);" << std::endl;
subsystemfile << "c" << ic << "->pvec.push_back("
<< (npb2 ? ("clist_params_[") : ("plist_[")) << (npb2 ? ni2 : i2)
<< "]);" << std::endl;
subsystemfile << "c" << ic << "->pvec.push_back("
<< (npb3 ? ("clist_params_[") : ("plist_[")) << (npb3 ? ni3 : i3)
<< "]);" << std::endl;
subsystemfile << "c" << ic << "->pvec.push_back("
<< (npb4 ? ("clist_params_[") : ("plist_[")) << (npb4 ? ni4 : i4)
<< "]);" << std::endl;
subsystemfile << "c" << ic << "->pvec.push_back("
<< (npb5 ? ("clist_params_[") : ("plist_[")) << (npb5 ? ni5 : i5)
<< "]);" << std::endl;
subsystemfile << "c" << ic << "->pvec.push_back("
<< (npb6 ? ("clist_params_[") : ("plist_[")) << (npb6 ? ni6 : i6)
<< "]);" << std::endl;
subsystemfile << "c" << ic << "->pvec.push_back("
<< (npb7 ? ("clist_params_[") : ("plist_[")) << (npb7 ? ni7 : i7)
<< "]);" << std::endl;
subsystemfile << "c" << ic << "->pvec.push_back("
<< (npb8 ? ("clist_params_[") : ("plist_[")) << (npb8 ? ni8 : i8)
<< "]);" << std::endl;
subsystemfile << "c" << ic << "->pvec.push_back("
<< (npb9 ? ("clist_params_[") : ("plist_[")) << (npb9 ? ni9 : i9)
<< "]);" << std::endl;
subsystemfile << "c" << ic << "->origpvec=c" << ic << "->pvec;" << std::endl;
subsystemfile << "c" << ic << "->rescale();" << std::endl;
subsystemfile << "clist_.push_back(c" << ic
<< "); // addresses = " << (*it)->pvec[0] << "," << (*it)->pvec[1]
<< "," << (*it)->pvec[2] << "," << (*it)->pvec[3] << ","
<< (*it)->pvec[4] << "," << (*it)->pvec[5] << "," << (*it)->pvec[6]
<< "," << (*it)->pvec[7] << "," << (*it)->pvec[8] << std::endl;
break;
}
case MidpointOnLine: { // 8
VEC_pD::iterator p1 = std::find(plist.begin(), plist.end(), (*it)->pvec[0]);
VEC_pD::iterator p2 = std::find(plist.begin(), plist.end(), (*it)->pvec[1]);
VEC_pD::iterator p3 = std::find(plist.begin(), plist.end(), (*it)->pvec[2]);
VEC_pD::iterator p4 = std::find(plist.begin(), plist.end(), (*it)->pvec[3]);
VEC_pD::iterator p5 = std::find(plist.begin(), plist.end(), (*it)->pvec[4]);
VEC_pD::iterator p6 = std::find(plist.begin(), plist.end(), (*it)->pvec[5]);
VEC_pD::iterator p7 = std::find(plist.begin(), plist.end(), (*it)->pvec[6]);
VEC_pD::iterator p8 = std::find(plist.begin(), plist.end(), (*it)->pvec[7]);
size_t i1 = std::distance(plist.begin(), p1);
size_t i2 = std::distance(plist.begin(), p2);
size_t i3 = std::distance(plist.begin(), p3);
size_t i4 = std::distance(plist.begin(), p4);
size_t i5 = std::distance(plist.begin(), p5);
size_t i6 = std::distance(plist.begin(), p6);
size_t i7 = std::distance(plist.begin(), p7);
size_t i8 = std::distance(plist.begin(), p8);
bool npb1 = false;
VEC_pD::iterator np1 =
std::find(clist_params_.begin(), clist_params_.end(), (*it)->pvec[0]);
size_t ni1 = std::distance(clist_params_.begin(), np1);
if (i1 == plist.size()) {
if (ni1 == clist_params_.size()) {
subsystemfile
<< "// Address not in System params...rebuilding into clist_params_"
<< std::endl;
clist_params_.push_back((*it)->pvec[0]);
subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[0])
<< ")); // " << icp << " address: " << (void*)(*it)->pvec[0]
<< std::endl;
icp++;
}
npb1 = true;
}
bool npb2 = false;
VEC_pD::iterator np2 =
std::find(clist_params_.begin(), clist_params_.end(), (*it)->pvec[1]);
size_t ni2 = std::distance(clist_params_.begin(), np2);
if (i2 == plist.size()) {
if (ni2 == clist_params_.size()) {
subsystemfile
<< "// Address not in System params...rebuilding into clist_params_"
<< std::endl;
clist_params_.push_back((*it)->pvec[1]);
subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[1])
<< ")); // " << icp << " address: " << (void*)(*it)->pvec[1]
<< std::endl;
icp++;
}
npb2 = true;
}
bool npb3 = false;
VEC_pD::iterator np3 =
std::find(clist_params_.begin(), clist_params_.end(), (*it)->pvec[2]);
size_t ni3 = std::distance(clist_params_.begin(), np3);
if (i3 == plist.size()) {
if (ni3 == clist_params_.size()) {
subsystemfile
<< "// Address not in System params...rebuilding into clist_params_"
<< std::endl;
clist_params_.push_back((*it)->pvec[2]);
subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[2])
<< ")); // " << icp << " address: " << (void*)(*it)->pvec[2]
<< std::endl;
icp++;
}
npb3 = true;
}
bool npb4 = false;
VEC_pD::iterator np4 =
std::find(clist_params_.begin(), clist_params_.end(), (*it)->pvec[3]);
size_t ni4 = std::distance(clist_params_.begin(), np4);
if (i4 == plist.size()) {
if (ni4 == clist_params_.size()) {
subsystemfile
<< "// Address not in System params...rebuilding into clist_params_"
<< std::endl;
clist_params_.push_back((*it)->pvec[3]);
subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[3])
<< ")); // " << icp << " address: " << (void*)(*it)->pvec[3]
<< std::endl;
icp++;
}
npb4 = true;
}
bool npb5 = false;
VEC_pD::iterator np5 =
std::find(clist_params_.begin(), clist_params_.end(), (*it)->pvec[4]);
size_t ni5 = std::distance(clist_params_.begin(), np5);
if (i5 == plist.size()) {
if (ni5 == clist_params_.size()) {
subsystemfile
<< "// Address not in System params...rebuilding into clist_params_"
<< std::endl;
clist_params_.push_back((*it)->pvec[4]);
subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[4])
<< ")); // " << icp << " address: " << (void*)(*it)->pvec[4]
<< std::endl;
icp++;
}
npb5 = true;
}
bool npb6 = false;
VEC_pD::iterator np6 =
std::find(clist_params_.begin(), clist_params_.end(), (*it)->pvec[5]);
size_t ni6 = std::distance(clist_params_.begin(), np6);
if (i6 == plist.size()) {
if (ni6 == clist_params_.size()) {
subsystemfile
<< "// Address not in System params...rebuilding into clist_params_"
<< std::endl;
clist_params_.push_back((*it)->pvec[5]);
subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[5])
<< ")); // " << icp << " address: " << (void*)(*it)->pvec[5]
<< std::endl;
icp++;
}
npb6 = true;
}
bool npb7 = false;
VEC_pD::iterator np7 =
std::find(clist_params_.begin(), clist_params_.end(), (*it)->pvec[6]);
size_t ni7 = std::distance(clist_params_.begin(), np7);
if (i7 == plist.size()) {
if (ni7 == clist_params_.size()) {
subsystemfile
<< "// Address not in System params...rebuilding into clist_params_"
<< std::endl;
clist_params_.push_back((*it)->pvec[6]);
subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[6])
<< ")); // " << icp << " address: " << (void*)(*it)->pvec[6]
<< std::endl;
icp++;
}
npb7 = true;
}
bool npb8 = false;
VEC_pD::iterator np8 =
std::find(clist_params_.begin(), clist_params_.end(), (*it)->pvec[7]);
size_t ni8 = std::distance(clist_params_.begin(), np8);
if (i8 == plist.size()) {
if (ni8 == clist_params_.size()) {
subsystemfile
<< "// Address not in System params...rebuilding into clist_params_"
<< std::endl;
clist_params_.push_back((*it)->pvec[7]);
subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[7])
<< ")); // " << icp << " address: " << (void*)(*it)->pvec[7]
<< std::endl;
icp++;
}
npb8 = true;
}
subsystemfile << "ConstraintMidpointOnLine * c" << ic
<< "=new ConstraintMidpointOnLine();" << std::endl;
subsystemfile << "c" << ic << "->pvec.push_back("
<< (npb1 ? ("clist_params_[") : ("plist_[")) << (npb1 ? ni1 : i1)
<< "]);" << std::endl;
subsystemfile << "c" << ic << "->pvec.push_back("
<< (npb2 ? ("clist_params_[") : ("plist_[")) << (npb2 ? ni2 : i2)
<< "]);" << std::endl;
subsystemfile << "c" << ic << "->pvec.push_back("
<< (npb3 ? ("clist_params_[") : ("plist_[")) << (npb3 ? ni3 : i3)
<< "]);" << std::endl;
subsystemfile << "c" << ic << "->pvec.push_back("
<< (npb4 ? ("clist_params_[") : ("plist_[")) << (npb4 ? ni4 : i4)
<< "]);" << std::endl;
subsystemfile << "c" << ic << "->pvec.push_back("
<< (npb5 ? ("clist_params_[") : ("plist_[")) << (npb5 ? ni5 : i5)
<< "]);" << std::endl;
subsystemfile << "c" << ic << "->pvec.push_back("
<< (npb6 ? ("clist_params_[") : ("plist_[")) << (npb6 ? ni6 : i6)
<< "]);" << std::endl;
subsystemfile << "c" << ic << "->pvec.push_back("
<< (npb7 ? ("clist_params_[") : ("plist_[")) << (npb7 ? ni7 : i7)
<< "]);" << std::endl;
subsystemfile << "c" << ic << "->pvec.push_back("
<< (npb8 ? ("clist_params_[") : ("plist_[")) << (npb8 ? ni8 : i8)
<< "]);" << std::endl;
subsystemfile << "c" << ic << "->origpvec=c" << ic << "->pvec;" << std::endl;
subsystemfile << "c" << ic << "->rescale();" << std::endl;
subsystemfile << "clist_.push_back(c" << ic
<< "); // addresses = " << (*it)->pvec[0] << "," << (*it)->pvec[1]
<< "," << (*it)->pvec[2] << "," << (*it)->pvec[3] << ","
<< (*it)->pvec[4] << "," << (*it)->pvec[5] << "," << (*it)->pvec[6]
<< "," << (*it)->pvec[7] << std::endl;
break;
}
case TangentCircumf: { // 6
VEC_pD::iterator p1 = std::find(plist.begin(), plist.end(), (*it)->pvec[0]);
VEC_pD::iterator p2 = std::find(plist.begin(), plist.end(), (*it)->pvec[1]);
VEC_pD::iterator p3 = std::find(plist.begin(), plist.end(), (*it)->pvec[2]);
VEC_pD::iterator p4 = std::find(plist.begin(), plist.end(), (*it)->pvec[3]);
VEC_pD::iterator p5 = std::find(plist.begin(), plist.end(), (*it)->pvec[4]);
VEC_pD::iterator p6 = std::find(plist.begin(), plist.end(), (*it)->pvec[5]);
size_t i1 = std::distance(plist.begin(), p1);
size_t i2 = std::distance(plist.begin(), p2);
size_t i3 = std::distance(plist.begin(), p3);
size_t i4 = std::distance(plist.begin(), p4);
size_t i5 = std::distance(plist.begin(), p5);
size_t i6 = std::distance(plist.begin(), p6);
bool npb1 = false;
VEC_pD::iterator np1 =
std::find(clist_params_.begin(), clist_params_.end(), (*it)->pvec[0]);
size_t ni1 = std::distance(clist_params_.begin(), np1);
if (i1 == plist.size()) {
if (ni1 == clist_params_.size()) {
subsystemfile
<< "// Address not in System params...rebuilding into clist_params_"
<< std::endl;
clist_params_.push_back((*it)->pvec[0]);
subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[0])
<< ")); // " << icp << " address: " << (void*)(*it)->pvec[0]
<< std::endl;
icp++;
}
npb1 = true;
}
bool npb2 = false;
VEC_pD::iterator np2 =
std::find(clist_params_.begin(), clist_params_.end(), (*it)->pvec[1]);
size_t ni2 = std::distance(clist_params_.begin(), np2);
if (i2 == plist.size()) {
if (ni2 == clist_params_.size()) {
subsystemfile
<< "// Address not in System params...rebuilding into clist_params_"
<< std::endl;
clist_params_.push_back((*it)->pvec[1]);
subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[1])
<< ")); // " << icp << " address: " << (void*)(*it)->pvec[1]
<< std::endl;
icp++;
}
npb2 = true;
}
bool npb3 = false;
VEC_pD::iterator np3 =
std::find(clist_params_.begin(), clist_params_.end(), (*it)->pvec[2]);
size_t ni3 = std::distance(clist_params_.begin(), np3);
if (i3 == plist.size()) {
if (ni3 == clist_params_.size()) {
subsystemfile
<< "// Address not in System params...rebuilding into clist_params_"
<< std::endl;
clist_params_.push_back((*it)->pvec[2]);
subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[2])
<< ")); // " << icp << " address: " << (void*)(*it)->pvec[2]
<< std::endl;
icp++;
}
npb3 = true;
}
bool npb4 = false;
VEC_pD::iterator np4 =
std::find(clist_params_.begin(), clist_params_.end(), (*it)->pvec[3]);
size_t ni4 = std::distance(clist_params_.begin(), np4);
if (i4 == plist.size()) {
if (ni4 == clist_params_.size()) {
subsystemfile
<< "// Address not in System params...rebuilding into clist_params_"
<< std::endl;
clist_params_.push_back((*it)->pvec[3]);
subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[3])
<< ")); // " << icp << " address: " << (void*)(*it)->pvec[3]
<< std::endl;
icp++;
}
npb4 = true;
}
bool npb5 = false;
VEC_pD::iterator np5 =
std::find(clist_params_.begin(), clist_params_.end(), (*it)->pvec[4]);
size_t ni5 = std::distance(clist_params_.begin(), np5);
if (i5 == plist.size()) {
if (ni5 == clist_params_.size()) {
subsystemfile
<< "// Address not in System params...rebuilding into clist_params_"
<< std::endl;
clist_params_.push_back((*it)->pvec[4]);
subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[4])
<< ")); // " << icp << " address: " << (void*)(*it)->pvec[4]
<< std::endl;
icp++;
}
npb5 = true;
}
bool npb6 = false;
VEC_pD::iterator np6 =
std::find(clist_params_.begin(), clist_params_.end(), (*it)->pvec[5]);
size_t ni6 = std::distance(clist_params_.begin(), np6);
if (i6 == plist.size()) {
if (ni6 == clist_params_.size()) {
subsystemfile
<< "// Address not in System params...rebuilding into clist_params_"
<< std::endl;
clist_params_.push_back((*it)->pvec[5]);
subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[5])
<< ")); // " << icp << " address: " << (void*)(*it)->pvec[5]
<< std::endl;
icp++;
}
npb6 = true;
}
subsystemfile << "ConstraintTangentCircumf * c" << ic
<< "=new ConstraintTangentCircumf("
<< (static_cast<ConstraintTangentCircumf*>(*it)->getInternal()
? "true"
: "false")
<< ");" << std::endl;
subsystemfile << "c" << ic << "->pvec.push_back("
<< (npb1 ? ("clist_params_[") : ("plist_[")) << (npb1 ? ni1 : i1)
<< "]);" << std::endl;
subsystemfile << "c" << ic << "->pvec.push_back("
<< (npb2 ? ("clist_params_[") : ("plist_[")) << (npb2 ? ni2 : i2)
<< "]);" << std::endl;
subsystemfile << "c" << ic << "->pvec.push_back("
<< (npb3 ? ("clist_params_[") : ("plist_[")) << (npb3 ? ni3 : i3)
<< "]);" << std::endl;
subsystemfile << "c" << ic << "->pvec.push_back("
<< (npb4 ? ("clist_params_[") : ("plist_[")) << (npb4 ? ni4 : i4)
<< "]);" << std::endl;
subsystemfile << "c" << ic << "->pvec.push_back("
<< (npb5 ? ("clist_params_[") : ("plist_[")) << (npb5 ? ni5 : i5)
<< "]);" << std::endl;
subsystemfile << "c" << ic << "->pvec.push_back("
<< (npb6 ? ("clist_params_[") : ("plist_[")) << (npb6 ? ni6 : i6)
<< "]);" << std::endl;
subsystemfile << "c" << ic << "->origpvec=c" << ic << "->pvec;" << std::endl;
subsystemfile << "c" << ic << "->rescale();" << std::endl;
subsystemfile << "clist_.push_back(c" << ic
<< "); // addresses = " << (*it)->pvec[0] << "," << (*it)->pvec[1]
<< "," << (*it)->pvec[2] << "," << (*it)->pvec[3] << ","
<< (*it)->pvec[4] << "," << (*it)->pvec[5] << std::endl;
break;
}
case PointOnEllipse: { // 7
VEC_pD::iterator p1 = std::find(plist.begin(), plist.end(), (*it)->pvec[0]);
VEC_pD::iterator p2 = std::find(plist.begin(), plist.end(), (*it)->pvec[1]);
VEC_pD::iterator p3 = std::find(plist.begin(), plist.end(), (*it)->pvec[2]);
VEC_pD::iterator p4 = std::find(plist.begin(), plist.end(), (*it)->pvec[3]);
VEC_pD::iterator p5 = std::find(plist.begin(), plist.end(), (*it)->pvec[4]);
VEC_pD::iterator p6 = std::find(plist.begin(), plist.end(), (*it)->pvec[5]);
VEC_pD::iterator p7 = std::find(plist.begin(), plist.end(), (*it)->pvec[6]);
size_t i1 = std::distance(plist.begin(), p1);
size_t i2 = std::distance(plist.begin(), p2);
size_t i3 = std::distance(plist.begin(), p3);
size_t i4 = std::distance(plist.begin(), p4);
size_t i5 = std::distance(plist.begin(), p5);
size_t i6 = std::distance(plist.begin(), p6);
size_t i7 = std::distance(plist.begin(), p7);
bool npb1 = false;
VEC_pD::iterator np1 =
std::find(clist_params_.begin(), clist_params_.end(), (*it)->pvec[0]);
size_t ni1 = std::distance(clist_params_.begin(), np1);
if (i1 == plist.size()) {
if (ni1 == clist_params_.size()) {
subsystemfile
<< "// Address not in System params...rebuilding into clist_params_"
<< std::endl;
clist_params_.push_back((*it)->pvec[0]);
subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[0])
<< ")); // " << icp << " address: " << (void*)(*it)->pvec[0]
<< std::endl;
icp++;
}
npb1 = true;
}
bool npb2 = false;
VEC_pD::iterator np2 =
std::find(clist_params_.begin(), clist_params_.end(), (*it)->pvec[1]);
size_t ni2 = std::distance(clist_params_.begin(), np2);
if (i2 == plist.size()) {
if (ni2 == clist_params_.size()) {
subsystemfile
<< "// Address not in System params...rebuilding into clist_params_"
<< std::endl;
clist_params_.push_back((*it)->pvec[1]);
subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[1])
<< ")); // " << icp << " address: " << (void*)(*it)->pvec[1]
<< std::endl;
icp++;
}
npb2 = true;
}
bool npb3 = false;
VEC_pD::iterator np3 =
std::find(clist_params_.begin(), clist_params_.end(), (*it)->pvec[2]);
size_t ni3 = std::distance(clist_params_.begin(), np3);
if (i3 == plist.size()) {
if (ni3 == clist_params_.size()) {
subsystemfile
<< "// Address not in System params...rebuilding into clist_params_"
<< std::endl;
clist_params_.push_back((*it)->pvec[2]);
subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[2])
<< ")); // " << icp << " address: " << (void*)(*it)->pvec[2]
<< std::endl;
icp++;
}
npb3 = true;
}
bool npb4 = false;
VEC_pD::iterator np4 =
std::find(clist_params_.begin(), clist_params_.end(), (*it)->pvec[3]);
size_t ni4 = std::distance(clist_params_.begin(), np4);
if (i4 == plist.size()) {
if (ni4 == clist_params_.size()) {
subsystemfile
<< "// Address not in System params...rebuilding into clist_params_"
<< std::endl;
clist_params_.push_back((*it)->pvec[3]);
subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[3])
<< ")); // " << icp << " address: " << (void*)(*it)->pvec[3]
<< std::endl;
icp++;
}
npb4 = true;
}
bool npb5 = false;
VEC_pD::iterator np5 =
std::find(clist_params_.begin(), clist_params_.end(), (*it)->pvec[4]);
size_t ni5 = std::distance(clist_params_.begin(), np5);
if (i5 == plist.size()) {
if (ni5 == clist_params_.size()) {
subsystemfile
<< "// Address not in System params...rebuilding into clist_params_"
<< std::endl;
clist_params_.push_back((*it)->pvec[4]);
subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[4])
<< ")); // " << icp << " address: " << (void*)(*it)->pvec[4]
<< std::endl;
icp++;
}
npb5 = true;
}
bool npb6 = false;
VEC_pD::iterator np6 =
std::find(clist_params_.begin(), clist_params_.end(), (*it)->pvec[5]);
size_t ni6 = std::distance(clist_params_.begin(), np6);
if (i6 == plist.size()) {
if (ni6 == clist_params_.size()) {
subsystemfile
<< "// Address not in System params...rebuilding into clist_params_"
<< std::endl;
clist_params_.push_back((*it)->pvec[5]);
subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[5])
<< ")); // " << icp << " address: " << (void*)(*it)->pvec[5]
<< std::endl;
icp++;
}
npb6 = true;
}
bool npb7 = false;
VEC_pD::iterator np7 =
std::find(clist_params_.begin(), clist_params_.end(), (*it)->pvec[6]);
size_t ni7 = std::distance(clist_params_.begin(), np7);
if (i7 == plist.size()) {
if (ni7 == clist_params_.size()) {
subsystemfile
<< "// Address not in System params...rebuilding into clist_params_"
<< std::endl;
clist_params_.push_back((*it)->pvec[6]);
subsystemfile << "clist_params_.push_back(new double(" << *((*it)->pvec[6])
<< ")); // " << icp << " address: " << (void*)(*it)->pvec[6]
<< std::endl;
icp++;
}
npb7 = true;
}
subsystemfile << "ConstraintPointOnEllipse * c" << ic
<< "=new ConstraintPointOnEllipse();" << std::endl;
subsystemfile << "c" << ic << "->pvec.push_back("
<< (npb1 ? ("clist_params_[") : ("plist_[")) << (npb1 ? ni1 : i1)
<< "]);" << std::endl;
subsystemfile << "c" << ic << "->pvec.push_back("
<< (npb2 ? ("clist_params_[") : ("plist_[")) << (npb2 ? ni2 : i2)
<< "]);" << std::endl;
subsystemfile << "c" << ic << "->pvec.push_back("
<< (npb3 ? ("clist_params_[") : ("plist_[")) << (npb3 ? ni3 : i3)
<< "]);" << std::endl;
subsystemfile << "c" << ic << "->pvec.push_back("
<< (npb4 ? ("clist_params_[") : ("plist_[")) << (npb4 ? ni4 : i4)
<< "]);" << std::endl;
subsystemfile << "c" << ic << "->pvec.push_back("
<< (npb5 ? ("clist_params_[") : ("plist_[")) << (npb5 ? ni5 : i5)
<< "]);" << std::endl;
subsystemfile << "c" << ic << "->pvec.push_back("
<< (npb6 ? ("clist_params_[") : ("plist_[")) << (npb6 ? ni6 : i6)
<< "]);" << std::endl;
subsystemfile << "c" << ic << "->pvec.push_back("
<< (npb7 ? ("clist_params_[") : ("plist_[")) << (npb7 ? ni7 : i7)
<< "]);" << std::endl;
subsystemfile << "c" << ic << "->origpvec=c" << ic << "->pvec;" << std::endl;
subsystemfile << "c" << ic << "->rescale();" << std::endl;
subsystemfile << "clist_.push_back(c" << ic
<< "); // addresses = " << (*it)->pvec[0] << "," << (*it)->pvec[1]
<< "," << (*it)->pvec[2] << "," << (*it)->pvec[3] << ","
<< (*it)->pvec[4] << "," << (*it)->pvec[5] << "," << (*it)->pvec[6]
<< std::endl;
break;
}
CASE_NOT_IMP(TangentEllipseLine)
CASE_NOT_IMP(InternalAlignmentPoint2Ellipse)
CASE_NOT_IMP(EqualMajorAxesEllipse)
CASE_NOT_IMP(EllipticalArcRangeToEndPoints)
CASE_NOT_IMP(AngleViaPoint)
CASE_NOT_IMP(Snell)
CASE_NOT_IMP(None)
}
}
subsystemfile.close();
}
#endif
// The following solver variant solves a system compound of two subsystems
// treating the first of them as of higher priority than the second
int System::solve(SubSystem* subsysA, SubSystem* subsysB, bool /*isFine*/, bool isRedundantsolving)
{
int xsizeA = subsysA->pSize();
int xsizeB = subsysB->pSize();
int csizeA = subsysA->cSize();
VEC_pD plistAB(xsizeA + xsizeB);
{
VEC_pD plistA, plistB;
subsysA->getParamList(plistA);
subsysB->getParamList(plistB);
std::sort(plistA.begin(), plistA.end());
std::sort(plistB.begin(), plistB.end());
VEC_pD::const_iterator it;
it = std::set_union(plistA.begin(),
plistA.end(),
plistB.begin(),
plistB.end(),
plistAB.begin());
plistAB.resize(it - plistAB.begin());
}
int xsize = plistAB.size();
Eigen::MatrixXd B = Eigen::MatrixXd::Identity(xsize, xsize);
Eigen::MatrixXd JA(csizeA, xsize);
Eigen::MatrixXd Y, Z;
Eigen::VectorXd resA(csizeA);
Eigen::VectorXd lambda(csizeA), lambda0(csizeA), lambdadir(csizeA);
Eigen::VectorXd x(xsize), x0(xsize), xdir(xsize), xdir1(xsize);
Eigen::VectorXd grad(xsize);
Eigen::VectorXd h(xsize);
Eigen::VectorXd y(xsize);
Eigen::VectorXd Bh(xsize);
// We assume that there are no common constraints in subsysA and subsysB
subsysA->redirectParams();
subsysB->redirectParams();
subsysB->getParams(plistAB, x);
subsysA->getParams(plistAB, x);
subsysB->setParams(plistAB, x); // just to ensure that A and B are synchronized
subsysB->calcGrad(plistAB, grad);
subsysA->calcJacobi(plistAB, JA);
subsysA->calcResidual(resA);
// double convergence = isFine ? XconvergenceFine : XconvergenceRough;
int maxIterNumber =
(isRedundantsolving
? (sketchSizeMultiplierRedundant ? maxIterRedundant * xsize : maxIterRedundant)
: (sketchSizeMultiplier ? maxIter * xsize : maxIter));
double divergingLim = 1e6 * subsysA->error() + 1e12;
double mu = 0;
lambda.setZero();
for (int iter = 1; iter < maxIterNumber; iter++) {
int status = qp_eq(B, grad, JA, resA, xdir, Y, Z);
if (status) {
break;
}
x0 = x;
lambda0 = lambda;
lambda = Y.transpose() * (B * xdir + grad);
lambdadir = lambda - lambda0;
// line search
{
double eta = 0.25;
double tau = 0.5;
double rho = 0.5;
double alpha = 1;
alpha = std::min(alpha, subsysA->maxStep(plistAB, xdir));
// Eq. 18.36
mu = std::max(mu,
(grad.dot(xdir) + std::max(0., 0.5 * xdir.dot(B * xdir)))
/ ((1. - rho) * resA.lpNorm<1>()));
// Eq. 18.27
double f0 = subsysB->error() + mu * resA.lpNorm<1>();
// Eq. 18.29
double deriv = grad.dot(xdir) - mu * resA.lpNorm<1>();
x = x0 + alpha * xdir;
subsysA->setParams(plistAB, x);
subsysB->setParams(plistAB, x);
subsysA->calcResidual(resA);
double f = subsysB->error() + mu * resA.lpNorm<1>();
// line search, Eq. 18.28
bool first = true;
while (f > f0 + eta * alpha * deriv) {
if (first) {
xdir1 = -Y * resA;
x += xdir1; // = x0 + alpha * xdir + xdir1
subsysA->setParams(plistAB, x);
subsysB->setParams(plistAB, x);
subsysA->calcResidual(resA);
f = subsysB->error() + mu * resA.lpNorm<1>();
if (f < f0 + eta * alpha * deriv) {
break;
}
}
alpha = tau * alpha;
if (alpha < 1e-8) { // let the linesearch fail
alpha = 0.;
}
x = x0 + alpha * xdir;
subsysA->setParams(plistAB, x);
subsysB->setParams(plistAB, x);
subsysA->calcResidual(resA);
f = subsysB->error() + mu * resA.lpNorm<1>();
if (alpha < 1e-8) { // let the linesearch fail
break;
}
}
lambda = lambda0 + alpha * lambdadir;
}
h = x - x0;
y = grad - JA.transpose() * lambda;
{
subsysB->calcGrad(plistAB, grad);
subsysA->calcJacobi(plistAB, JA);
subsysA->calcResidual(resA);
}
y = grad - JA.transpose() * lambda - y; // Eq. 18.13
if (iter > 1) {
double yTh = y.dot(h);
if (yTh != 0) {
Bh = B * h;
// Now calculate the BFGS update on B
B += 1. / yTh * y * y.transpose();
B -= 1. / h.dot(Bh) * (Bh * Bh.transpose());
}
}
double err = subsysA->error();
if (h.norm() <= (isRedundantsolving ? convergenceRedundant : convergence)
&& err <= smallF) {
break;
}
if (err > divergingLim || err != err) { // check for diverging and NaN
break;
}
}
int ret;
if (subsysA->error() <= smallF) {
ret = Success;
}
else if (h.norm() <= (isRedundantsolving ? convergenceRedundant : convergence)) {
ret = Converged;
}
else {
ret = Failed;
}
subsysA->revertParams();
subsysB->revertParams();
return ret;
}
void System::applySolution()
{
for (int cid = 0; cid < int(subSystems.size()); cid++) {
if (subSystemsAux[cid]) {
subSystemsAux[cid]->applySolution();
}
if (subSystems[cid]) {
subSystems[cid]->applySolution();
}
for (MAP_pD_pD::const_iterator it = reductionmaps[cid].begin();
it != reductionmaps[cid].end();
++it) {
*(it->first) = *(it->second);
}
}
}
void System::undoSolution()
{
resetToReference();
}
void System::makeReducedJacobian(Eigen::MatrixXd& J,
std::map<int, int>& jacobianconstraintmap,
GCS::VEC_pD& pdiagnoselist,
std::map<int, int>& tagmultiplicity)
{
// construct specific parameter list for diagonose ignoring driven constraint parameters
for (int j = 0; j < int(plist.size()); j++) {
auto result1 = std::find(std::begin(pdrivenlist), std::end(pdrivenlist), plist[j]);
if (result1 == std::end(pdrivenlist)) {
pdiagnoselist.push_back(plist[j]);
}
}
J = Eigen::MatrixXd::Zero(clist.size(), pdiagnoselist.size());
int jacobianconstraintcount = 0;
int allcount = 0;
for (std::vector<Constraint*>::iterator constr = clist.begin(); constr != clist.end();
++constr) {
(*constr)->revertParams();
++allcount;
if ((*constr)->getTag() >= 0 && (*constr)->isDriving()) {
jacobianconstraintcount++;
for (int j = 0; j < int(pdiagnoselist.size()); j++) {
J(jacobianconstraintcount - 1, j) = (*constr)->grad(pdiagnoselist[j]);
}
// parallel processing: create tag multiplicity map
if (tagmultiplicity.find((*constr)->getTag()) == tagmultiplicity.end()) {
tagmultiplicity[(*constr)->getTag()] = 0;
}
else {
tagmultiplicity[(*constr)->getTag()]++;
}
jacobianconstraintmap[jacobianconstraintcount - 1] = allcount - 1;
}
}
if (jacobianconstraintcount == 0) { // only driven constraints
J.resize(0, 0);
}
}
int System::diagnose(Algorithm alg)
{
// Analyses the constrainess grad of the system and provides feedback
// The vector "conflictingTags" will hold a group of conflicting constraints
// Hint 1: Only constraints with tag >= 0 are taken into account
// Hint 2: Constraints tagged with 0 are treated as high priority
// constraints and they are excluded from the returned
// list of conflicting constraints. Therefore, this function
// will provide no feedback about possible conflicts between
// two high priority constraints. For this reason, tagging
// constraints with 0 should be used carefully.
hasDiagnosis = false;
if (!hasUnknowns) {
dofs = -1;
return dofs;
}
#ifdef _DEBUG_TO_FILE
SolverReportingManager::Manager().LogToFile("GCS::System::diagnose()\n");
#endif
// Input parameters' lists:
// plist => list of all the parameters of the system, e.g. each coordinate
// of a point
// pdrivenlist => list of the parameters that are driven by other parameters
// (e.g. value of driven constraints)
// When adding an external geometry or a constraint on an external geometry the array
// 'plist' is empty.
// So, we must abort here because otherwise we would create an invalid matrix and make
// the application eventually crash. This fixes issues #0002372/#0002373.
if (plist.empty() || (plist.size() - pdrivenlist.size()) == 0) {
hasDiagnosis = true;
emptyDiagnoseMatrix = true;
dofs = 0;
return dofs;
}
redundant.clear();
conflictingTags.clear();
redundantTags.clear();
partiallyRedundantTags.clear();
// This QR diagnosis uses a reduced Jacobian matrix to calculate the rank of the system
// and identify conflicting and redundant constraints.
//
// reduced Jacobian matrix
// The Jacobian has been reduced to:
// 1. only contain driving constraints, but keep a full size (zero padded).
// 2. remove the parameters of the values of driven constraints.
Eigen::MatrixXd J;
// maps the index of the rows of the reduced jacobian matrix (solver constraints) to
// the index those constraints would have in a full size Jacobian matrix
std::map<int, int> jacobianconstraintmap;
// list of parameters to be diagnosed in this routine (removes value parameters from driven
// constraints)
GCS::VEC_pD pdiagnoselist;
// tag multiplicity gives the number of solver constraints associated with the same tag
// A tag generally corresponds to the Sketcher constraint index - There are special tag values,
// like 0 and -1.
std::map<int, int> tagmultiplicity;
makeReducedJacobian(J, jacobianconstraintmap, pdiagnoselist, tagmultiplicity);
// this function will exit with a diagnosis and, unless overridden by functions below, with full
// DoFs
hasDiagnosis = true;
dofs = pdiagnoselist.size();
if (J.rows() > 0) {
emptyDiagnoseMatrix = false;
}
// There is a legacy decision to use QR decomposition. I (abdullah) do not know all the
// consideration taken in that decisions. I see that:
// - QR decomposition is able to provide information about the rank and
// redundant/conflicting
// constraints
// - The QR decomposition of J and the QR decomposition of the transpose of J are unrelated
// (for reasons see below):
// https://mathoverflow.net/questions/338729/translate-between-qr-decomposition-of-a-and-a-transpose
// - QR is cheaper than a SVD decomposition
// - QR is more expensive than a rank revealing LU factorization
// - QR is less stable than SVD with respect to rank
// - It is unclear whether it is possible to obtain information about redundancy with SVD
// and LU
// Given this legacy decision, the following is observed:
// - A = QR decomposition can be used for the diagonise of dependency of the "columns" of A.
// the reason is that matrix R is upper triangular with columns of A showing the
// dependencies.
// - The same does not apply to the "rows".
// - For this reason, to enable a full diagnose of constraints, a QR decomposition must be
// done
// on the transpose of the Jacobian matrix (J), this is JT.
// Eigen capabilities:
// - If Eigen full pivoting QR decomposition is used, it is possible to track the rows of JT
// during the decomposition. This can be leveraged to identify a set of independent rows
// of JT (geometry) that form a rank N basis. However, because the R matrix is of the JT
// decomposition and not the J decomposition, it is not possible to reduce the system to
// identify exactly which rows are dependent.
// - The effect is that it provides a set of parameters of geometry that are not constraint,
// but it does not identify ALL geometries that are not fixed.
// - If SpareQR is used, then it is not possible to track the rows of JT during
// decomposition.
// I do not know if it is still possible to obtain geometry information at all from
// SparseQR. After several years these questions remain open:
// https://stackoverflow.com/questions/49009771/getting-rows-transpositions-with-sparse-qr
// https://forum.kde.org/viewtopic.php?f=74&t=151239
//
// Implementation below:
//
// Two QR decompositions are used below. One for diagnosis of constraints and a second one
// for diagnosis of parameters, i.e. to identify whether the parameter is fully constraint
// (independent) or not (i.e. it is dependent).
// QR decomposition method selection: SparseQR vs DenseQR
#ifndef EIGEN_SPARSEQR_COMPATIBLE
if (qrAlgorithm == EigenSparseQR) {
Base::Console().Warning("SparseQR not supported by you current version of Eigen. It "
"requires Eigen 3.2.2 or higher. Falling back to Dense QR\n");
qrAlgorithm = EigenDenseQR;
}
#endif
if (qrAlgorithm == EigenDenseQR) {
#ifdef PROFILE_DIAGNOSE
Base::TimeElapsed DenseQR_start_time;
#endif
if (J.rows() > 0) {
int rank = 0; // rank is not cheap to retrieve from qrJT in DenseQR
Eigen::MatrixXd R;
Eigen::FullPivHouseholderQR<Eigen::MatrixXd> qrJT;
// Here we give the system the possibility to run the two QR decompositions in parallel,
// depending on the load of the system so we are using the default std::launch::async |
// std::launch::deferred policy, as nobody better than the system nows if it can run the
// task in parallel or is oversubscribed and should deferred it. Care to wait() for the
// future before any prospective detection of conflicting/redundant, because the
// redundant solve modifies pdiagnoselist and it would NOT be thread-safe. Care to call
// the thread with silent=true, unless the present thread does not use Base::Console, or
// the launch policy is set to std::launch::deferred policy, as it is not thread-safe to
// use them in both at the same time.
//
// identifyDependentParametersDenseQR(J, jacobianconstraintmap, pdiagnoselist, true)
//
auto fut = std::async(&System::identifyDependentParametersDenseQR,
this,
J,
jacobianconstraintmap,
pdiagnoselist,
true);
makeDenseQRDecomposition(J, jacobianconstraintmap, qrJT, rank, R);
int paramsNum = qrJT.rows();
int constrNum = qrJT.cols();
// This function is legacy code that was used to obtain partial geometry dependency
// information from a SINGLE Dense QR decomposition. I am reluctant to remove it from
// here until everything new is well tested.
// identifyDependentGeometryParametersInTransposedJacobianDenseQRDecomposition( qrJT,
// pdiagnoselist, paramsNum, rank);
fut.wait(); // wait for the execution of identifyDependentParametersSparseQR to finish
dofs = paramsNum - rank; // unless overconstraint, which will be overridden below
// Detecting conflicting or redundant constraints
if (constrNum > rank) { // conflicting or redundant constraints
int nonredundantconstrNum;
identifyConflictingRedundantConstraints(alg,
qrJT,
jacobianconstraintmap,
tagmultiplicity,
pdiagnoselist,
R,
constrNum,
rank,
nonredundantconstrNum);
if (paramsNum == rank && nonredundantconstrNum > rank) { // over-constrained
dofs = paramsNum - nonredundantconstrNum;
}
}
}
#ifdef PROFILE_DIAGNOSE
Base::TimeElapsed DenseQR_end_time;
auto SolveTime = Base::TimeElapsed::diffTimeF(DenseQR_start_time, DenseQR_end_time);
Base::Console().Log("\nDenseQR - Lapsed Time: %f seconds\n", SolveTime);
#endif
}
#ifdef EIGEN_SPARSEQR_COMPATIBLE
else if (qrAlgorithm == EigenSparseQR) {
#ifdef PROFILE_DIAGNOSE
Base::TimeElapsed SparseQR_start_time;
#endif
if (J.rows() > 0) {
int rank = 0;
Eigen::MatrixXd R;
Eigen::SparseQR<Eigen::SparseMatrix<double>, Eigen::COLAMDOrdering<int>> SqrJT;
// Here we give the system the possibility to run the two QR decompositions in parallel,
// depending on the load of the system so we are using the default std::launch::async |
// std::launch::deferred policy, as nobody better than the system nows if it can run the
// task in parallel or is oversubscribed and should deferred it. Care to wait() for the
// future before any prospective detection of conflicting/redundant, because the
// redundant solve modifies pdiagnoselist and it would NOT be thread-safe. Care to call
// the thread with silent=true, unless the present thread does not use Base::Console, or
// the launch policy is set to std::launch::deferred policy, as it is not thread-safe to
// use them in both at the same time.
//
// identifyDependentParametersSparseQR(J, jacobianconstraintmap, pdiagnoselist, true)
//
// Debug:
// auto fut =
// std::async(std::launch::deferred,&System::identifyDependentParametersSparseQR, this,
// J, jacobianconstraintmap, pdiagnoselist, false);
auto fut = std::async(&System::identifyDependentParametersSparseQR,
this,
J,
jacobianconstraintmap,
pdiagnoselist,
/*silent=*/true);
makeSparseQRDecomposition(J,
jacobianconstraintmap,
SqrJT,
rank,
R,
/*transposed=*/true,
/*silent=*/false);
int paramsNum = SqrJT.rows();
int constrNum = SqrJT.cols();
fut.wait(); // wait for the execution of identifyDependentParametersSparseQR to finish
dofs = paramsNum - rank; // unless overconstraint, which will be overridden below
// Detecting conflicting or redundant constraints
if (constrNum > rank) {
int nonredundantconstrNum;
identifyConflictingRedundantConstraints(alg,
SqrJT,
jacobianconstraintmap,
tagmultiplicity,
pdiagnoselist,
R,
constrNum,
rank,
nonredundantconstrNum);
if (paramsNum == rank && nonredundantconstrNum > rank) { // over-constrained
dofs = paramsNum - nonredundantconstrNum;
}
}
}
#ifdef PROFILE_DIAGNOSE
Base::TimeElapsed SparseQR_end_time;
auto SolveTime = Base::TimeElapsed::diffTimeF(SparseQR_start_time, SparseQR_end_time);
Base::Console().Log("\nSparseQR - Lapsed Time: %f seconds\n", SolveTime);
#endif
}
#endif
return dofs;
}
void System::makeDenseQRDecomposition(const Eigen::MatrixXd& J,
const std::map<int, int>& jacobianconstraintmap,
Eigen::FullPivHouseholderQR<Eigen::MatrixXd>& qrJT,
int& rank,
Eigen::MatrixXd& R,
bool transposeJ,
bool silent)
{
#ifdef _GCS_DEBUG
if (!silent) {
SolverReportingManager::Manager().LogMatrix("J", J);
}
#endif
#ifdef _GCS_DEBUG_SOLVER_JACOBIAN_QR_DECOMPOSITION_TRIANGULAR_MATRIX
Eigen::MatrixXd Q; // Obtaining the Q matrix with Sparse QR is buggy, see comments below
Eigen::MatrixXd R2; // Intended for a trapezoidal matrix, where R is the top triangular matrix
// of the R2 trapezoidal matrix
#endif
// For a transposed J SJG rows are paramsNum and cols are constrNum
// For a non-transposed J SJG rows are constrNum and cols are paramsNum
int rowsNum = 0;
int colsNum = 0;
if (J.rows() > 0) {
Eigen::MatrixXd JG;
if (transposeJ) {
JG = J.topRows(jacobianconstraintmap.size()).transpose();
}
else {
JG = J.topRows(jacobianconstraintmap.size());
}
if (JG.rows() > 0 && JG.cols() > 0) {
qrJT.compute(JG);
rowsNum = qrJT.rows();
colsNum = qrJT.cols();
qrJT.setThreshold(qrpivotThreshold);
rank = qrJT.rank();
if (colsNum >= rowsNum) {
R = qrJT.matrixQR().triangularView<Eigen::Upper>();
}
else {
R = qrJT.matrixQR().topRows(colsNum).triangularView<Eigen::Upper>();
}
}
else {
rowsNum = JG.rows();
colsNum = JG.cols();
}
#ifdef _GCS_DEBUG_SOLVER_JACOBIAN_QR_DECOMPOSITION_TRIANGULAR_MATRIX
R2 = qrJT.matrixQR();
Q = qrJT.matrixQ();
#endif
}
if (debugMode == IterationLevel && !silent) {
SolverReportingManager::Manager().LogQRSystemInformation(*this, rowsNum, colsNum, rank);
}
#ifdef _GCS_DEBUG_SOLVER_JACOBIAN_QR_DECOMPOSITION_TRIANGULAR_MATRIX
if (J.rows() > 0 && !silent) {
SolverReportingManager::Manager().LogMatrix("R", R);
SolverReportingManager::Manager().LogMatrix("R2", R2);
SolverReportingManager::Manager().LogMatrix("Q", Q);
SolverReportingManager::Manager().LogMatrix("RowTransp", qrJT.rowsTranspositions());
}
#endif
}
#ifdef EIGEN_SPARSEQR_COMPATIBLE
void System::makeSparseQRDecomposition(
const Eigen::MatrixXd& J,
const std::map<int, int>& jacobianconstraintmap,
Eigen::SparseQR<Eigen::SparseMatrix<double>, Eigen::COLAMDOrdering<int>>& SqrJT,
int& rank,
Eigen::MatrixXd& R,
bool transposeJ,
bool silent)
{
Eigen::SparseMatrix<double> SJ;
// this creation is not optimized (done using triplets)
// however the time this takes is negligible compared to the
// time the QR decomposition itself takes
SJ = J.sparseView();
SJ.makeCompressed();
#ifdef _GCS_DEBUG
if (!silent) {
SolverReportingManager::Manager().LogMatrix("J", J);
}
#endif
#ifdef _GCS_DEBUG_SOLVER_JACOBIAN_QR_DECOMPOSITION_TRIANGULAR_MATRIX
Eigen::MatrixXd Q; // Obtaining the Q matrix with Sparse QR is buggy, see comments below
Eigen::MatrixXd R2; // Intended for a trapezoidal matrix, where R is the top triangular matrix
// of the R2 trapezoidal matrix
#endif
// For a transposed J SJG rows are paramsNum and cols are constrNum
// For a non-transposed J SJG rows are constrNum and cols are paramsNum
int rowsNum = 0;
int colsNum = 0;
if (SJ.rows() > 0) {
Eigen::SparseMatrix<double> SJG;
if (transposeJ) {
SJG = SJ.topRows(jacobianconstraintmap.size()).transpose();
}
else {
SJG = SJ.topRows(jacobianconstraintmap.size());
}
if (SJG.rows() > 0 && SJG.cols() > 0) {
SqrJT.compute(SJG);
// Do not ask for Q Matrix!!
// At Eigen 3.2 still has a bug that this only works for square matrices
// if enabled it will crash
#ifdef SPARSE_Q_MATRIX
Q = SqrJT.matrixQ();
// Q = QS;
#endif
rowsNum = SqrJT.rows();
colsNum = SqrJT.cols();
SqrJT.setPivotThreshold(qrpivotThreshold);
rank = SqrJT.rank();
if (colsNum >= rowsNum) {
R = SqrJT.matrixR().triangularView<Eigen::Upper>();
}
else {
R = SqrJT.matrixR().topRows(colsNum).triangularView<Eigen::Upper>();
}
#ifdef _GCS_DEBUG_SOLVER_JACOBIAN_QR_DECOMPOSITION_TRIANGULAR_MATRIX
R2 = SqrJT.matrixR();
#endif
}
else {
rowsNum = SJG.rows();
colsNum = SJG.cols();
}
}
if (debugMode == IterationLevel && !silent) {
SolverReportingManager::Manager().LogQRSystemInformation(*this, rowsNum, colsNum, rank);
}
#ifdef _GCS_DEBUG_SOLVER_JACOBIAN_QR_DECOMPOSITION_TRIANGULAR_MATRIX
if (J.rows() > 0 && !silent) {
SolverReportingManager::Manager().LogMatrix("R", R);
SolverReportingManager::Manager().LogMatrix("R2", R2);
#ifdef SPARSE_Q_MATRIX
SolverReportingManager::Manager().LogMatrix("Q", Q);
#endif
}
#endif //_GCS_DEBUG_SOLVER_JACOBIAN_QR_DECOMPOSITION_TRIANGULAR_MATRIX
}
#endif // EIGEN_SPARSEQR_COMPATIBLE
void System::identifyDependentParametersDenseQR(const Eigen::MatrixXd& J,
const std::map<int, int>& jacobianconstraintmap,
const GCS::VEC_pD& pdiagnoselist,
bool silent)
{
Eigen::FullPivHouseholderQR<Eigen::MatrixXd> qrJ;
Eigen::MatrixXd Rparams;
int rank;
makeDenseQRDecomposition(J, jacobianconstraintmap, qrJ, rank, Rparams, false, true);
identifyDependentParameters(qrJ, Rparams, rank, pdiagnoselist, silent);
}
#ifdef EIGEN_SPARSEQR_COMPATIBLE
void System::identifyDependentParametersSparseQR(const Eigen::MatrixXd& J,
const std::map<int, int>& jacobianconstraintmap,
const GCS::VEC_pD& pdiagnoselist,
bool silent)
{
Eigen::SparseQR<Eigen::SparseMatrix<double>, Eigen::COLAMDOrdering<int>> SqrJ;
Eigen::MatrixXd Rparams;
int nontransprank;
makeSparseQRDecomposition(J,
jacobianconstraintmap,
SqrJ,
nontransprank,
Rparams,
false,
true); // do not transpose allow to diagnose parameters
identifyDependentParameters(SqrJ, Rparams, nontransprank, pdiagnoselist, silent);
}
#endif
template<typename T>
void System::identifyDependentParameters(T& qrJ,
Eigen::MatrixXd& Rparams,
int rank,
const GCS::VEC_pD& pdiagnoselist,
bool silent)
{
(void)silent; // silent is only used in debug code, but it is important as Base::Console is not
// thread-safe. Removes warning in non Debug mode.
// int constrNum = SqrJ.rows(); // this is the other way around than for the transposed J
// int paramsNum = SqrJ.cols();
eliminateNonZerosOverPivotInUpperTriangularMatrix(Rparams, rank);
#ifdef _GCS_DEBUG
if (!silent) {
SolverReportingManager::Manager().LogMatrix("Rparams_nonzeros_over_pilot", Rparams);
}
#endif
pDependentParametersGroups.resize(qrJ.cols() - rank);
for (int j = rank; j < qrJ.cols(); j++) {
for (int row = 0; row < rank; row++) {
if (fabs(Rparams(row, j)) > 1e-10) {
int origCol = qrJ.colsPermutation().indices()[row];
pDependentParametersGroups[j - rank].push_back(pdiagnoselist[origCol]);
pDependentParameters.push_back(pdiagnoselist[origCol]);
}
}
int origCol = qrJ.colsPermutation().indices()[j];
pDependentParametersGroups[j - rank].push_back(pdiagnoselist[origCol]);
pDependentParameters.push_back(pdiagnoselist[origCol]);
}
#ifdef _GCS_DEBUG
if (!silent) {
SolverReportingManager::Manager().LogMatrix("PermMatrix",
(Eigen::MatrixXd)qrJ.colsPermutation());
SolverReportingManager::Manager().LogGroupOfParameters("ParameterGroups",
pDependentParametersGroups);
}
#endif
}
void System::identifyDependentGeometryParametersInTransposedJacobianDenseQRDecomposition(
const Eigen::FullPivHouseholderQR<Eigen::MatrixXd>& qrJT,
const GCS::VEC_pD& pdiagnoselist,
int paramsNum,
int rank)
{
// DETECTING CONSTRAINT SOLVER PARAMETERS
//
// NOTE: This is only true for dense QR with full pivoting, because solve parameters get
// reordered. I am unable to adapt it to Sparse QR. (abdullah). See:
//
// https://stackoverflow.com/questions/49009771/getting-rows-transpositions-with-sparse-qr
// https://forum.kde.org/viewtopic.php?f=74&t=151239
//
// R (original version, not R here which is trimmed to not have empty rows)
// has paramsNum rows, the first "rank" rows correspond to parameters that are constraint
// Calculate the Permutation matrix from the Transposition matrix
Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic> rowPermutations;
rowPermutations.setIdentity(paramsNum);
// P.J.P' = Q.R see https://eigen.tuxfamily.org/dox/classEigen_1_1FullPivHouseholderQR.html
const MatrixIndexType rowTranspositions = qrJT.rowsTranspositions();
for (int k = 0; k < rank; ++k) {
rowPermutations.applyTranspositionOnTheRight(k, rowTranspositions.coeff(k));
}
#ifdef _GCS_DEBUG_SOLVER_JACOBIAN_QR_DECOMPOSITION_TRIANGULAR_MATRIX
std::stringstream stream;
#endif
// params (in the order of J) shown as independent from QR
std::set<int> indepParamCols;
std::set<int> depParamCols;
for (int j = 0; j < rank; j++) {
int origRow = rowPermutations.indices()[j];
indepParamCols.insert(origRow);
// NOTE: Q*R = transpose(J), so the row of R corresponds to the col of J (the rows of
// transpose(J)). The cols of J are the parameters, the rows are the constraints.
#ifdef _GCS_DEBUG_SOLVER_JACOBIAN_QR_DECOMPOSITION_TRIANGULAR_MATRIX
stream << "R row " << j << " = J col " << origRow << std::endl;
#endif
}
#ifdef _GCS_DEBUG_SOLVER_JACOBIAN_QR_DECOMPOSITION_TRIANGULAR_MATRIX
std::string tmp = stream.str();
SolverReportingManager::Manager().LogString(tmp);
#endif
// If not independent, must be dependent
for (int j = 0; j < paramsNum; j++) {
auto result = indepParamCols.find(j);
if (result == indepParamCols.end()) {
depParamCols.insert(j);
}
}
#ifdef _GCS_DEBUG_SOLVER_JACOBIAN_QR_DECOMPOSITION_TRIANGULAR_MATRIX
stream.flush();
stream << "Indep params: [";
for (auto indep : indepParamCols) {
stream << indep;
}
stream << "]" << std::endl;
stream << "Dep params: [";
for (auto dep : depParamCols) {
stream << dep;
}
stream << "]" << std::endl;
tmp = stream.str();
SolverReportingManager::Manager().LogString(tmp);
#endif
for (auto param : depParamCols) {
pDependentParameters.push_back(pdiagnoselist[param]);
}
}
void System::eliminateNonZerosOverPivotInUpperTriangularMatrix(Eigen::MatrixXd& R, int rank)
{
for (int i = 1; i < rank; i++) {
// eliminate non zeros above pivot
assert(R(i, i) != 0);
for (int row = 0; row < i; row++) {
if (R(row, i) != 0) {
double coef = R(row, i) / R(i, i);
R.block(row, i + 1, 1, R.cols() - i - 1) -=
coef * R.block(i, i + 1, 1, R.cols() - i - 1);
R(row, i) = 0;
}
}
}
}
template<typename T>
void System::identifyConflictingRedundantConstraints(
Algorithm alg,
const T& qrJT,
const std::map<int, int>& jacobianconstraintmap,
const std::map<int, int>& tagmultiplicity,
GCS::VEC_pD& pdiagnoselist,
Eigen::MatrixXd& R,
int constrNum,
int rank,
int& nonredundantconstrNum)
{
eliminateNonZerosOverPivotInUpperTriangularMatrix(R, rank);
std::vector<std::vector<Constraint*>> conflictGroups(constrNum - rank);
for (int j = rank; j < constrNum; j++) {
for (int row = 0; row < rank; row++) {
if (fabs(R(row, j)) > 1e-10) {
int origCol = qrJT.colsPermutation().indices()[row];
conflictGroups[j - rank].push_back(clist[jacobianconstraintmap.at(origCol)]);
}
}
int origCol = qrJT.colsPermutation().indices()[j];
conflictGroups[j - rank].push_back(clist[jacobianconstraintmap.at(origCol)]);
}
// Augment the information regarding the group of constraints that are conflicting or redundant.
if (debugMode == IterationLevel) {
SolverReportingManager::Manager().LogGroupOfConstraints(
"Analysing groups of constraints of special interest",
conflictGroups);
}
// try to remove the conflicting constraints and solve the
// system in order to check if the removed constraints were
// just redundant but not really conflicting
std::set<Constraint*> skipped;
SET_I satisfiedGroups;
while (1) {
// conflictingMap contains all the eligible constraints of conflict groups not yet
// satisfied. as groups get satisfied, the map created on every iteration is smaller, until
// such time it is empty and the infinite loop is exited. The guarantee that the loop will
// be exited originates from the fact that in each iteration the algorithm will select one
// constraint from the conflict groups, which will satisfy at least one group.
std::map<Constraint*, SET_I> conflictingMap;
for (std::size_t i = 0; i < conflictGroups.size(); i++) {
if (satisfiedGroups.count(i) == 0) {
for (std::size_t j = 0; j < conflictGroups[i].size(); j++) {
Constraint* constr = conflictGroups[i][j];
bool isinternalalignment =
(constr->isInternalAlignment() == Constraint::Alignment::InternalAlignment);
bool priorityconstraint = (constr->getTag() == 0);
if (!priorityconstraint && !isinternalalignment) { // exclude constraints
// tagged with zero and
// internal alignment
conflictingMap[constr].insert(i);
}
}
}
}
if (conflictingMap.empty()) {
break;
}
int maxPopularity = 0;
Constraint* mostPopular = nullptr;
for (std::map<Constraint*, SET_I>::const_iterator it = conflictingMap.begin();
it != conflictingMap.end();
++it) {
int numberofsets = static_cast<int>(
it->second.size()); // number of sets in which the constraint appears
/* This is a heuristic algorithm to propose the user which constraints from a
* redundant/conflicting set should be removed. It is based on the following principles:
* 1. if the current constraint is more popular than previous ones (appears in more
* sets), take it. This prioritises removal of constraints that cause several
* independent groups of constraints to be conflicting/redundant. It is based on the
* observation that the redundancy/conflict is caused by the lesser amount of
* constraints.
* 2. if there is already a constraint ranking in the contest, and the current one is as
* popular, prefer the constraint that removes a lesser amount of DoFs. This prioritises
* removal of sketcher constraints (not solver constraints) that generates a higher
* amount of solver constraints. It is based on the observation that constraints taking
* a higher amount of DoFs (such as symmetry) are preferred by the user, who may not see
* the redundancy of simpler ones.
* 3. if there is already a constraint ranking in the context, the current one is as
* popular, and they remove the same amount of DoFs, prefer removal of the latest
* introduced.
*/
if ((numberofsets > maxPopularity || // (1)
(numberofsets == maxPopularity && mostPopular
&& tagmultiplicity.at(it->first->getTag())
< tagmultiplicity.at(mostPopular->getTag()))
|| // (2)
(numberofsets == maxPopularity && mostPopular
&& tagmultiplicity.at(it->first->getTag())
== tagmultiplicity.at(mostPopular->getTag())
&& it->first->getTag() > mostPopular->getTag())) // (3)
) {
mostPopular = it->first;
maxPopularity = numberofsets;
}
}
if (maxPopularity > 0) {
// adding for skipping not only the mostPopular, but also any other constraint in the
// conflicting map associated with the same tag (namely any other solver
// constraint associated with the same sketcher constraint that is also conflicting)
auto maxPopularityTag = mostPopular->getTag();
for (const auto& c : conflictingMap) {
if (c.first->getTag() == maxPopularityTag) {
skipped.insert(c.first);
for (SET_I::const_iterator it = conflictingMap[c.first].begin();
it != conflictingMap[c.first].end();
++it) {
satisfiedGroups.insert(*it);
}
}
}
}
}
// Augment information regarding the choice made by popularity contest
if (debugMode == IterationLevel) {
SolverReportingManager::Manager().LogSetOfConstraints("Chosen redundants", skipped);
}
std::vector<Constraint*> clistTmp;
clistTmp.reserve(clist.size());
for (std::vector<Constraint*>::iterator constr = clist.begin(); constr != clist.end();
++constr) {
if ((*constr)->isDriving() && skipped.count(*constr) == 0) {
clistTmp.push_back(*constr);
}
}
SubSystem* subSysTmp = new SubSystem(clistTmp, pdiagnoselist);
int res = solve(subSysTmp, true, alg, true);
if (debugMode == Minimal || debugMode == IterationLevel) {
std::string solvername;
switch (alg) {
case 0:
solvername = "BFGS";
break;
case 1: // solving with the LevenbergMarquardt solver
solvername = "LevenbergMarquardt";
break;
case 2: // solving with the BFGS solver
solvername = "DogLeg";
break;
}
Base::Console().Log("Sketcher::RedundantSolving-%s-\n", solvername.c_str());
}
if (res == Success) {
subSysTmp->applySolution();
for (std::set<Constraint*>::const_iterator constr = skipped.begin();
constr != skipped.end();
++constr) {
double err = (*constr)->error();
if (err * err < convergenceRedundant) {
redundant.insert(*constr);
}
}
resetToReference();
if (debugMode == Minimal || debugMode == IterationLevel) {
Base::Console().Log("Sketcher Redundant solving: %d redundants\n", redundant.size());
}
std::vector<std::vector<Constraint*>> conflictGroupsOrig = conflictGroups;
conflictGroups.clear();
for (int i = conflictGroupsOrig.size() - 1; i >= 0; i--) {
bool isRedundant = false;
for (std::size_t j = 0; j < conflictGroupsOrig[i].size(); j++) {
if (redundant.count(conflictGroupsOrig[i][j]) > 0) {
isRedundant = true;
if (debugMode == IterationLevel) {
Base::Console().Log("(Partially) Redundant, Group %d, index %d, Tag: %d\n",
i,
j,
(conflictGroupsOrig[i][j])->getTag());
}
break;
}
}
if (!isRedundant) {
conflictGroups.push_back(conflictGroupsOrig[i]);
}
else {
constrNum--;
}
}
}
delete subSysTmp;
// simplified output of conflicting tags
SET_I conflictingTagsSet;
for (std::size_t i = 0; i < conflictGroups.size(); i++) {
for (std::size_t j = 0; j < conflictGroups[i].size(); j++) {
bool isinternalalignment = (conflictGroups[i][j]->isInternalAlignment()
== Constraint::Alignment::InternalAlignment);
if (conflictGroups[i][j]->getTag() != 0
&& !isinternalalignment) { // exclude constraints tagged with zero and internal
// alignment
conflictingTagsSet.insert(conflictGroups[i][j]->getTag());
}
}
}
conflictingTags.resize(conflictingTagsSet.size());
std::copy(conflictingTagsSet.begin(), conflictingTagsSet.end(), conflictingTags.begin());
// output of redundant tags
SET_I redundantTagsSet, partiallyRedundantTagsSet;
for (std::set<Constraint*>::iterator constr = redundant.begin(); constr != redundant.end();
++constr) {
redundantTagsSet.insert((*constr)->getTag());
partiallyRedundantTagsSet.insert((*constr)->getTag());
}
// remove tags represented at least in one non-redundant constraint
for (std::vector<Constraint*>::iterator constr = clist.begin(); constr != clist.end();
++constr) {
if (redundant.count(*constr) == 0) {
redundantTagsSet.erase((*constr)->getTag());
}
}
redundantTags.resize(redundantTagsSet.size());
std::copy(redundantTagsSet.begin(), redundantTagsSet.end(), redundantTags.begin());
for (auto r : redundantTagsSet) {
partiallyRedundantTagsSet.erase(r);
}
partiallyRedundantTags.resize(partiallyRedundantTagsSet.size());
std::copy(partiallyRedundantTagsSet.begin(),
partiallyRedundantTagsSet.end(),
partiallyRedundantTags.begin());
nonredundantconstrNum = constrNum;
}
void System::clearSubSystems()
{
isInit = false;
free(subSystems);
free(subSystemsAux);
subSystems.clear();
subSystemsAux.clear();
}
double lineSearch(SubSystem* subsys, Eigen::VectorXd& xdir)
{
double f1, f2, f3, alpha1, alpha2, alpha3, alphaStar;
double alphaMax = subsys->maxStep(xdir);
Eigen::VectorXd x0, x;
// Save initial values
subsys->getParams(x0);
// Start at the initial position alpha1 = 0
alpha1 = 0.;
f1 = subsys->error();
// Take a step of alpha2 = 1
alpha2 = 1.;
x = x0 + alpha2 * xdir;
subsys->setParams(x);
f2 = subsys->error();
// Take a step of alpha3 = 2*alpha2
alpha3 = alpha2 * 2;
x = x0 + alpha3 * xdir;
subsys->setParams(x);
f3 = subsys->error();
// Now reduce or lengthen alpha2 and alpha3 until the minimum is
// Bracketed by the triplet f1>f2<f3
while (f2 > f1 || f2 > f3) {
if (f2 > f1) {
// If f2 is greater than f1 then we shorten alpha2 and alpha3 closer to f1
// Effectively both are shortened by a factor of two.
alpha3 = alpha2;
f3 = f2;
alpha2 = alpha2 / 2;
x = x0 + alpha2 * xdir;
subsys->setParams(x);
f2 = subsys->error();
}
else if (f2 > f3) {
if (alpha3 >= alphaMax) {
break;
}
// If f2 is greater than f3 then we increase alpha2 and alpha3 away from f1
// Effectively both are lengthened by a factor of two.
alpha2 = alpha3;
f2 = f3;
alpha3 = alpha3 * 2;
x = x0 + alpha3 * xdir;
subsys->setParams(x);
f3 = subsys->error();
}
}
// Get the alpha for the minimum f of the quadratic approximation
alphaStar = alpha2 + ((alpha2 - alpha1) * (f1 - f3)) / (3 * (f1 - 2 * f2 + f3));
// Guarantee that the new alphaStar is within the bracket
if (alphaStar >= alpha3 || alphaStar <= alpha1) {
alphaStar = alpha2;
}
if (alphaStar > alphaMax) {
alphaStar = alphaMax;
}
if (alphaStar != alphaStar) {
alphaStar = 0.;
}
// Take a final step to alphaStar
x = x0 + alphaStar * xdir;
subsys->setParams(x);
return alphaStar;
}
void free(VEC_pD& doublevec)
{
for (VEC_pD::iterator it = doublevec.begin(); it != doublevec.end(); ++it) {
if (*it) {
delete *it;
}
}
doublevec.clear();
}
void free(std::vector<Constraint*>& constrvec)
{
for (std::vector<Constraint*>::iterator constr = constrvec.begin(); constr != constrvec.end();
++constr) {
if (*constr) {
switch ((*constr)->getTypeId()) {
case Equal:
delete static_cast<ConstraintEqual*>(*constr);
break;
case Difference:
delete static_cast<ConstraintDifference*>(*constr);
break;
case P2PDistance:
delete static_cast<ConstraintP2PDistance*>(*constr);
break;
case P2PAngle:
delete static_cast<ConstraintP2PAngle*>(*constr);
break;
case P2LDistance:
delete static_cast<ConstraintP2LDistance*>(*constr);
break;
case PointOnLine:
delete static_cast<ConstraintPointOnLine*>(*constr);
break;
case Parallel:
delete static_cast<ConstraintParallel*>(*constr);
break;
case Perpendicular:
delete static_cast<ConstraintPerpendicular*>(*constr);
break;
case L2LAngle:
delete static_cast<ConstraintL2LAngle*>(*constr);
break;
case MidpointOnLine:
delete static_cast<ConstraintMidpointOnLine*>(*constr);
break;
case None:
default:
delete *constr;
}
}
}
constrvec.clear();
}
void free(std::vector<SubSystem*>& subsysvec)
{
for (std::vector<SubSystem*>::iterator it = subsysvec.begin(); it != subsysvec.end(); ++it) {
if (*it) {
delete *it;
}
}
}
} // namespace GCS
Reference in New Issue View Git Blame Copy Permalink