diff --git a/.gitignore b/.gitignore index 9491a2f..e612e3c 100644 --- a/.gitignore +++ b/.gitignore @@ -360,4 +360,8 @@ MigrationBackup/ .ionide/ # Fody - auto-generated XML schema -FodyWeavers.xsd \ No newline at end of file +FodyWeavers.xsd +/eigen-5.0.0 +/include +/src +*.json diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..ac0b9d7 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,50 @@ +cmake_minimum_required(VERSION 3.16) +project(DRAWer_2_0 LANGUAGES CXX) + +set(CMAKE_CXX_STANDARD 20) +set(CMAKE_CXX_STANDARD_REQUIRED ON) + +find_package(QT NAMES Qt6 Qt5 REQUIRED COMPONENTS Core) +find_package(Qt${QT_VERSION_MAJOR} + COMPONENTS + Core + Gui + Widgets +) +qt_standard_project_setup() + +set(INCLUDE_BOOST_DIR "C:/local/boost_1_89_0") +set(INCLUDE_EIGEN_DIR ${CMAKE_CURRENT_SOURCE_DIR}/eigen-5.0.0) + +set(PROJECT_SOURCES + main.cpp + DRAWer_2_0.ui + DRAWer_2_0.cpp + Canvas.cpp + GCS/GCS.cpp + GCS/SubSystem.cpp + GCS/Constraints.cpp + GCS/Geo.cpp + GCS/qp_eq.cpp +) + +qt_add_executable(${PROJECT_NAME} ${PROJECT_SOURCES}) + +set_target_properties(${PROJECT_NAME} + PROPERTIES + WIN32_EXECUTABLE TRUE +) + +target_include_directories(${PROJECT_NAME} PUBLIC + ${INCLUDE_BOOST_DIR} + ${INCLUDE_EIGEN_DIR} + ${CMAKE_CURRENT_SOURCE_DIR}/GCS/headers +) + +target_link_libraries(${PROJECT_NAME} + PUBLIC + Qt::Core + Qt::Gui + Qt::Widgets +) + diff --git a/Canvas.cpp b/Canvas.cpp new file mode 100644 index 0000000..6af8eed --- /dev/null +++ b/Canvas.cpp @@ -0,0 +1,83 @@ +#include "Canvas.h" +void Canvas::changeMode(Mode _mode) +{ + mode = _mode; +} + +void Canvas::mousePressEvent(QMouseEvent* event) +{ + QPointF scene = event->pos(); + +#ifdef _DEBUG + qDebug() << "Scene point in" << scene.x() << scene.y(); + changeMode(DrawingLine); +#endif + + if (mode == DrawingLine) { + if (!current_line) { + current_line = new Line(); + double* x1 = new double(scene.x()); + double* y1 = new double(scene.y()); + + double* x2 = new double(scene.x()); + double* y2 = new double(scene.y()); + + params.push_back(x1); + params.push_back(y1); + + current_line->p1.x = x1; + current_line->p1.y = y1; + current_line->p2.x = x2; + current_line->p2.y = y2; + } + else { + *current_line->p2.x = scene.x(); + *current_line->p2.y = scene.y(); + + params.push_back(current_line->p2.x); + params.push_back(current_line->p2.y); + + lines.append(current_line); + current_line = nullptr; + } + +//#ifdef _DEBUG +// if (lines.size()) { +// qDebug() << "Line coords" << lines[lines.size() - 1]->p1.get_X() << +// lines[lines.size() - 1]->p1.get_Y() << +// lines[lines.size() - 1]->p2.get_X() << +// lines[lines.size() - 1]->p2.get_Y(); +// changeMode(None); +// } +//#endif + + } + +} + +Canvas::Canvas(QWidget *parent) + : QWidget(parent) +{ + current_line = nullptr; + setMouseTracking(true); + setBackgroundRole(QPalette::Base); + setAutoFillBackground(true); +} + +Canvas::~Canvas() +{ + for (Line* line : lines) { + delete line; + } + + for (double* param : params) { + delete param; + } + + lines.clear(); + params.clear(); + + if (current_line) + delete current_line; +} + diff --git a/Canvas.h b/Canvas.h new file mode 100644 index 0000000..7385c2b --- /dev/null +++ b/Canvas.h @@ -0,0 +1,34 @@ +#pragma once + +#include +#include +#ifdef _DEBUG +#include +#endif + +#include "GCS/Geo.h" +using namespace GCS; + +enum Mode { + None, + DrawingLine +}; + + +class Canvas : public QWidget +{ + Q_OBJECT +private: + void changeMode(Mode); + Mode mode = Mode::None; + QVector lines; + QVector points; + QVector params; + Line* current_line; +protected: + void mousePressEvent(QMouseEvent*) override; +public: + Canvas(QWidget *parent); + ~Canvas(); +}; + diff --git a/DRAWer_2_0.cpp b/DRAWer_2_0.cpp new file mode 100644 index 0000000..8637ffb --- /dev/null +++ b/DRAWer_2_0.cpp @@ -0,0 +1,18 @@ +#include "DRAWer_2_0.h" + +DRAWer_2_0::DRAWer_2_0(QWidget *parent) + : QMainWindow(parent) +{ + ui.setupUi(this); + counter = 0; +} + +DRAWer_2_0::~DRAWer_2_0() +{} + + +void DRAWer_2_0::on_pushButton_clicked() +{ + ui.label->setText(QString::number(counter++)); +} + diff --git a/DRAWer_2_0.h b/DRAWer_2_0.h new file mode 100644 index 0000000..38d52f8 --- /dev/null +++ b/DRAWer_2_0.h @@ -0,0 +1,21 @@ +#pragma once + +#include +#include "ui_DRAWer_2_0.h" + +class DRAWer_2_0 : public QMainWindow +{ + Q_OBJECT + +public: + DRAWer_2_0(QWidget* parent = nullptr); + ~DRAWer_2_0(); + +private slots: + void on_pushButton_clicked(); + +private: + Ui::DRAWer_2_0Class ui; + int counter; +}; + diff --git a/DRAWer_2_0.ui b/DRAWer_2_0.ui new file mode 100644 index 0000000..1cf3121 --- /dev/null +++ b/DRAWer_2_0.ui @@ -0,0 +1,135 @@ + + + DRAWer_2_0Class + + + + 0 + 0 + 700 + 600 + + + + + 1080 + 1080 + + + + DRAWer_2_0 + + + + + + + 23 + + + + + + 75 + 24 + + + + Click me + + + + + + + + 75 + 24 + + + + PushButton + + + + + + + + 75 + 24 + + + + PushButton + + + + + + + + + + 100 + 100 + + + + TextLabel + + + Qt::AlignmentFlag::AlignCenter + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + + + + + + 0 + 0 + 700 + 22 + + + + + + RightToolBarArea + + + false + + + + + + + + Canvas + QWidget +
canvas.h
+ 1 + + + + + diff --git a/GCS/Constraints.cpp b/GCS/Constraints.cpp new file mode 100644 index 0000000..14477eb --- /dev/null +++ b/GCS/Constraints.cpp @@ -0,0 +1,3583 @@ +/*************************************************************************** + * Copyright (c) 2011 Konstantinos Poulios * + * * + * This file is part of the FreeCAD CAx development system. * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of the GNU Library General Public * + * License as published by the Free Software Foundation; either * + * version 2 of the License, or (at your option) any later version. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * + * GNU Library General Public License for more details. * + * * + * You should have received a copy of the GNU Library General Public * + * License along with this library; see the file COPYING.LIB. If not, * + * write to the Free Software Foundation, Inc., 59 Temple Place, * + * Suite 330, Boston, MA 02111-1307, USA * + * * + ***************************************************************************/ + +#ifdef _MSC_VER +#pragma warning(disable : 4251) +#endif + +#include +#include + +#include +#define DEBUG_DERIVS 0 + +#include +#include + +#include "Constraints.h" + + +namespace GCS +{ + +/////////////////////////////////////// +// Constraints +/////////////////////////////////////// + +Constraint::Constraint() + : origpvec(0) + , pvec(0) + , scale(1.) + , tag(0) + , pvecChangedFlag(true) + , driving(true) + , internalAlignment(Alignment::NoInternalAlignment) +{} + +void Constraint::redirectParams(const MAP_pD_pD& redirectionmap) +{ + int i = 0; + for (VEC_pD::iterator param = origpvec.begin(); param != origpvec.end(); ++param, i++) { + MAP_pD_pD::const_iterator it = redirectionmap.find(*param); + if (it != redirectionmap.end()) { + pvec[i] = it->second; + } + } + pvecChangedFlag = true; +} + +void Constraint::revertParams() +{ + pvec = origpvec; + pvecChangedFlag = true; +} + +ConstraintType Constraint::getTypeId() +{ + return None; +} + +void Constraint::rescale(double coef) +{ + scale = coef * 1.0; +} + +double Constraint::error() +{ + return 0.0; +} + +double Constraint::grad(double* /*param*/) +{ + return 0.0; +} + +double Constraint::maxStep(MAP_pD_D& /*dir*/, double lim) +{ + return lim; +} + +int Constraint::findParamInPvec(double* param) +{ + int ret = -1; + for (std::size_t i = 0; i < pvec.size(); i++) { + if (param == pvec[i]) { + ret = static_cast(i); + break; + } + } + return ret; +} + + +// -------------------------------------------------------- +// Equal +ConstraintEqual::ConstraintEqual(double* p1, double* p2, double p1p2ratio) +{ + ratio = p1p2ratio; + pvec.push_back(p1); + pvec.push_back(p2); + origpvec = pvec; + rescale(); +} + +ConstraintType ConstraintEqual::getTypeId() +{ + return Equal; +} + +void ConstraintEqual::rescale(double coef) +{ + scale = coef * 1.; +} + +double ConstraintEqual::error() +{ + return scale * (*param1() - ratio * (*param2())); +} + +double ConstraintEqual::grad(double* param) +{ + double deriv = 0.; + if (param == param1()) { + deriv += 1; + } + if (param == param2()) { + deriv += -1; + } + return scale * deriv; +} + + +// -------------------------------------------------------- +// Weighted Linear Combination +ConstraintWeightedLinearCombination::ConstraintWeightedLinearCombination( + size_t givennumpoles, + const std::vector& givenpvec, + const std::vector& givenfactors) + : factors(givenfactors) + , numpoles(givennumpoles) +{ + pvec = givenpvec; + assert(pvec.size() == 2 * numpoles + 1); + assert(factors.size() == numpoles); + origpvec = pvec; + rescale(); +} + +ConstraintType ConstraintWeightedLinearCombination::getTypeId() +{ + return WeightedLinearCombination; +} + +void ConstraintWeightedLinearCombination::rescale(double coef) +{ + scale = coef * 1.; +} + +double ConstraintWeightedLinearCombination::error() +{ + // Explanation of the math here: + // https://forum.freecad.org/viewtopic.php?f=9&t=71130&start=120#p635538 + + double sum = 0; + double wsum = 0; + + for (size_t i = 0; i < numpoles; ++i) { + double wcontrib = *weightat(i) * factors[i]; + wsum += wcontrib; + sum += *poleat(i) * wcontrib; + } + + return scale * ((*thepoint()) * wsum - sum); +} + +double ConstraintWeightedLinearCombination::grad(double* param) +{ + // Equations are from here: + // https://forum.freecad.org/viewtopic.php?f=9&t=71130&start=120#p635538 + + double deriv = 0.; + + if (param == thepoint()) { + // Eq. (11) + double wsum = 0; + for (size_t i = 0; i < numpoles; ++i) { + wsum += *weightat(i) * factors[i]; + } + deriv = wsum; + return scale * deriv; + } + + for (size_t i = 0; i < numpoles; ++i) { + if (param == poleat(i)) { + // Eq. (12) + deriv = -(*weightat(i) * factors[i]); + return scale * deriv; + } + if (param == weightat(i)) { + // Eq. (13) + deriv = (*thepoint() - *poleat(i)) * factors[i]; + return scale * deriv; + } + } + + return scale * deriv; +} + + +// -------------------------------------------------------- +// Center of Gravity +ConstraintCenterOfGravity::ConstraintCenterOfGravity(const std::vector& givenpvec, + const std::vector& givenweights) + : weights(givenweights) +{ + pvec = givenpvec; + numpoints = pvec.size() - 1; + assert(pvec.size() > 1); + assert(weights.size() == numpoints); + origpvec = pvec; + rescale(); +} + +ConstraintType ConstraintCenterOfGravity::getTypeId() +{ + return CenterOfGravity; +} + +void ConstraintCenterOfGravity::rescale(double coef) +{ + scale = coef * 1.; +} + +double ConstraintCenterOfGravity::error() +{ + double sum = 0; + for (size_t i = 0; i < numpoints; ++i) { + sum += *pointat(i) * weights[i]; + } + + return scale * (*thecenter() - sum); +} + +double ConstraintCenterOfGravity::grad(double* param) +{ + double deriv = 0.; + if (param == thecenter()) { + deriv = 1; + } + + for (size_t i = 0; i < numpoints; ++i) { + if (param == pointat(i)) { + deriv = -weights[i]; + } + } + + return scale * deriv; +} + + +// -------------------------------------------------------- +// Slope at B-spline knot +ConstraintSlopeAtBSplineKnot::ConstraintSlopeAtBSplineKnot(BSpline& b, Line& l, size_t knotindex) +{ + // set up pvec: pole x-coords, pole y-coords, pole weights, + // line point 1 coords, line point 2 coords + + numpoles = b.degree - b.mult[knotindex] + 1; + // slope at knot doesn't make sense if there's only C0 continuity + assert(numpoles >= 2); + + pvec.reserve(3 * numpoles + 4); + + // `startpole` is the first pole affecting the knot with `knotindex` + size_t startpole = 0; + // See `System::addConstraintInternalAlignmentKnotPoint()` for some elaboration + for (size_t j = 1; j <= knotindex; ++j) { + startpole += b.mult[j]; + } + if (!b.periodic && startpole >= b.poles.size()) { + startpole = b.poles.size() - 1; + } + + 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.poles[(startpole + i) % b.poles.size()].y); + } + for (size_t i = 0; i < numpoles; ++i) { + pvec.push_back(b.weights[(startpole + i) % b.weights.size()]); + } + pvec.push_back(l.p1.x); + pvec.push_back(l.p1.y); + pvec.push_back(l.p2.x); + pvec.push_back(l.p2.y); + + // Set up factors to get slope at knot point + std::vector tempfactors((numpoles + 1), 1.0 / (numpoles + 1)); + factors.resize(numpoles); + slopefactors.resize(numpoles); + for (size_t i = 0; i < numpoles + 1; ++i) { + tempfactors[i] = b.getLinCombFactor(*(b.knots[knotindex]), + startpole + b.degree, + startpole + i, + b.degree - 1) + / (b.flattenedknots[startpole + b.degree + i] - b.flattenedknots[startpole + i]); + } + for (size_t i = 0; i < numpoles; ++i) { + factors[i] = b.getLinCombFactor(*(b.knots[knotindex]), startpole + b.degree, startpole + i); + slopefactors[i] = b.degree * (tempfactors[i] - tempfactors[i + 1]); + } + + origpvec = pvec; + rescale(); +} + +ConstraintType ConstraintSlopeAtBSplineKnot::getTypeId() +{ + return SlopeAtBSplineKnot; +} + +void ConstraintSlopeAtBSplineKnot::rescale(double coef) +{ + double slopex = 0., slopey = 0.; + + for (size_t i = 0; i < numpoles; ++i) { + slopex += *polexat(i) * slopefactors[i]; + slopey += *poleyat(i) * slopefactors[i]; + } + + scale = coef / sqrt((slopex * slopex + slopey * slopey)); +} + +double ConstraintSlopeAtBSplineKnot::error() +{ + double xsum = 0., xslopesum = 0.; + double ysum = 0., yslopesum = 0.; + double wsum = 0., wslopesum = 0.; + + for (size_t i = 0; i < numpoles; ++i) { + double wcontrib = *weightat(i) * factors[i]; + double wslopecontrib = *weightat(i) * slopefactors[i]; + wsum += wcontrib; + xsum += *polexat(i) * wcontrib; + ysum += *poleyat(i) * wcontrib; + wslopesum += wslopecontrib; + xslopesum += *polexat(i) * wslopecontrib; + yslopesum += *poleyat(i) * wslopecontrib; + } + + // This is actually wsum^2 * the respective slopes + // See Eq (19) from: + // https://forum.freecad.org/viewtopic.php?f=9&t=71130&start=120#p635538 + double slopex = wsum * xslopesum - wslopesum * xsum; + double slopey = wsum * yslopesum - wslopesum * ysum; + + // Normalizing it ensures that the cross product is not zero just because + // one vector is zero. + double linex = *linep2x() - *linep1x(); + double liney = *linep2y() - *linep1y(); + double dirx = linex / sqrt(linex * linex + liney * liney); + double diry = liney / sqrt(linex * linex + liney * liney); + + // error is the cross product + return scale * (slopex * diry - slopey * dirx); +} + +double ConstraintSlopeAtBSplineKnot::grad(double* param) +{ + // Equations are from here: + // https://forum.freecad.org/viewtopic.php?f=9&t=71130&start=120#p635538 + double result = 0.0; + double linex = *linep2x() - *linep1x(); + double liney = *linep2y() - *linep1y(); + double dirx = linex / sqrt(linex * linex + liney * liney); + double diry = liney / sqrt(linex * linex + liney * liney); + + for (size_t i = 0; i < numpoles; ++i) { + if (param == polexat(i)) { + // Eq. (21) + double wsum = 0., wslopesum = 0.; + for (size_t j = 0; j < numpoles; ++j) { + double wcontrib = *weightat(j) * factors[j]; + double wslopecontrib = *weightat(j) * slopefactors[j]; + wsum += wcontrib; + wslopesum += wslopecontrib; + } + result = (wsum * slopefactors[i] - wslopesum * factors[i]) * diry; + return scale * result; + } + if (param == poleyat(i)) { + // Eq. (21) + double wsum = 0., wslopesum = 0.; + for (size_t i = 0; i < numpoles; ++i) { + double wcontrib = *weightat(i) * factors[i]; + double wslopecontrib = *weightat(i) * slopefactors[i]; + wsum += wcontrib; + wslopesum += wslopecontrib; + } + result = -(wsum * slopefactors[i] - wslopesum * factors[i]) * dirx; + return scale * result; + } + if (param == weightat(i)) { + // Eq. (22) + double xsum = 0., xslopesum = 0.; + double ysum = 0., yslopesum = 0.; + for (size_t j = 0; j < numpoles; ++j) { + double wcontrib = *weightat(j) * factors[j]; + double wslopecontrib = *weightat(j) * slopefactors[j]; + xsum += wcontrib * (*polexat(j) - *polexat(i)); + xslopesum += wslopecontrib * (*polexat(j) - *polexat(i)); + ysum += wcontrib * (*poleyat(j) - *poleyat(i)); + yslopesum += wslopecontrib * (*poleyat(j) - *poleyat(i)); + } + result = (factors[i] * xslopesum - slopefactors[i] * xsum) * diry + - (factors[i] * yslopesum - slopefactors[i] * ysum) * dirx; + return scale * result; + } + } + + double slopex = 0., slopey = 0.; + + auto getSlopes = [&]() { + double xsum = 0., xslopesum = 0.; + double ysum = 0., yslopesum = 0.; + double wsum = 0., wslopesum = 0.; + + for (size_t i = 0; i < numpoles; ++i) { + double wcontrib = *weightat(i) * factors[i]; + double wslopecontrib = *weightat(i) * slopefactors[i]; + wsum += wcontrib; + xsum += *polexat(i) * wcontrib; + ysum += *poleyat(i) * wcontrib; + wslopesum += wslopecontrib; + xslopesum += *polexat(i) * wslopecontrib; + yslopesum += *poleyat(i) * wslopecontrib; + } + + // This is actually wsum^2 * the respective slopes + slopex = wsum * xslopesum - wslopesum * xsum; + slopey = wsum * yslopesum - wslopesum * ysum; + }; + + if (param == linep1x()) { + getSlopes(); + double dDirxDLinex = (liney * liney) / pow(linex * linex + liney * liney, 1.5); + double dDiryDLinex = -(linex * liney) / pow(linex * linex + liney * liney, 1.5); + // NOTE: d(linex)/d(x1) = -1 + result = slopex * (-dDiryDLinex) - slopey * (-dDirxDLinex); + return scale * result; + } + if (param == linep2x()) { + getSlopes(); + double dDirxDLinex = (liney * liney) / pow(linex * linex + liney * liney, 1.5); + double dDiryDLinex = -(linex * liney) / pow(linex * linex + liney * liney, 1.5); + // NOTE: d(linex)/d(x2) = 1 + result = slopex * dDiryDLinex - slopey * dDirxDLinex; + return scale * result; + } + if (param == linep1y()) { + getSlopes(); + double dDirxDLiney = -(linex * liney) / pow(linex * linex + liney * liney, 1.5); + double dDiryDLiney = (linex * linex) / pow(linex * linex + liney * liney, 1.5); + // NOTE: d(liney)/d(y1) = -1 + result = slopex * (-dDiryDLiney) - slopey * (-dDirxDLiney); + return scale * result; + } + if (param == linep2y()) { + getSlopes(); + double dDirxDLiney = -(linex * liney) / pow(linex * linex + liney * liney, 1.5); + double dDiryDLiney = (linex * linex) / pow(linex * linex + liney * liney, 1.5); + // NOTE: d(liney)/d(y2) = 1 + result = slopex * dDiryDLiney - slopey * dDirxDLiney; + return scale * result; + } + + return scale * result; +} + + +// -------------------------------------------------------- +// Point On BSpline +ConstraintPointOnBSpline::ConstraintPointOnBSpline(double* point, + double* initparam, + int coordidx, + BSpline& b) + : bsp(b) +{ + // This is always going to be true + numpoints = bsp.degree + 1; + + pvec.reserve(2 + 2 * b.poles.size()); + pvec.push_back(point); + pvec.push_back(initparam); + + setStartPole(*initparam); + + for (size_t i = 0; i < b.poles.size(); ++i) { + if (coordidx == 0) { + pvec.push_back(b.poles[i].x); + } + else { + pvec.push_back(b.poles[i].y); + } + } + for (size_t i = 0; i < b.weights.size(); ++i) { + pvec.push_back(b.weights[i]); + } + + if (bsp.flattenedknots.empty()) { + bsp.setupFlattenedKnots(); + } + + origpvec = pvec; + rescale(); +} + +ConstraintType ConstraintPointOnBSpline::getTypeId() +{ + return PointOnBSpline; +} + +void ConstraintPointOnBSpline::setStartPole(double u) +{ + // The startpole logic is repeated in a lot of places, + // for example in GCS and slope at knot + // find relevant poles + startpole = 0; + for (size_t j = 1; j < bsp.mult.size() && *(bsp.knots[j]) <= u; ++j) { + startpole += bsp.mult[j]; + } + if (!bsp.periodic && startpole >= bsp.poles.size()) { + startpole = bsp.poles.size() - bsp.degree - 1; + } +} + +void ConstraintPointOnBSpline::rescale(double coef) +{ + scale = coef * 1.0; +} + +double ConstraintPointOnBSpline::error() +{ + if (*theparam() < bsp.flattenedknots[startpole + bsp.degree] + || *theparam() > bsp.flattenedknots[startpole + bsp.degree + 1]) { + setStartPole(*theparam()); + } + + double sum = 0; + double wsum = 0; + + // TODO: maybe make it global so it doesn't have to be created every time + VEC_D d(numpoints); + for (size_t i = 0; i < numpoints; ++i) { + d[i] = *poleat(i) * *weightat(i); + } + sum = BSpline::splineValue(*theparam(), + startpole + bsp.degree, + bsp.degree, + d, + bsp.flattenedknots); + for (size_t i = 0; i < numpoints; ++i) { + d[i] = *weightat(i); + } + wsum = BSpline::splineValue(*theparam(), + startpole + bsp.degree, + bsp.degree, + d, + bsp.flattenedknots); + + // TODO: Change the poles as the point moves between pieces + + return scale * (*thepoint() * wsum - sum); +} + +double ConstraintPointOnBSpline::grad(double* gcsparam) +{ + double deriv = 0.; + if (gcsparam == thepoint()) { + VEC_D d(numpoints); + for (size_t i = 0; i < numpoints; ++i) { + d[i] = *weightat(i); + } + double wsum = BSpline::splineValue(*theparam(), + startpole + bsp.degree, + bsp.degree, + d, + bsp.flattenedknots); + deriv += wsum; + } + + if (gcsparam == theparam()) { + VEC_D d(numpoints - 1); + for (size_t i = 1; i < numpoints; ++i) { + d[i - 1] = (*poleat(i) * *weightat(i) - *poleat(i - 1) * *weightat(i - 1)) + / (bsp.flattenedknots[startpole + i + bsp.degree] + - bsp.flattenedknots[startpole + i]); + } + double slopevalue = BSpline::splineValue(*theparam(), + startpole + bsp.degree, + bsp.degree - 1, + d, + bsp.flattenedknots); + for (size_t i = 1; i < numpoints; ++i) { + d[i - 1] = (*weightat(i) - *weightat(i - 1)) + / (bsp.flattenedknots[startpole + i + bsp.degree] + - bsp.flattenedknots[startpole + i]); + } + double wslopevalue = BSpline::splineValue(*theparam(), + startpole + bsp.degree, + bsp.degree - 1, + d, + bsp.flattenedknots); + deriv += (*thepoint() * wslopevalue - slopevalue) * bsp.degree; + } + + for (size_t i = 0; i < numpoints; ++i) { + if (gcsparam == poleat(i)) { + auto factorsI = + bsp.getLinCombFactor(*theparam(), startpole + bsp.degree, startpole + i); + deriv += -(*weightat(i) * factorsI); + } + if (gcsparam == weightat(i)) { + auto factorsI = + bsp.getLinCombFactor(*theparam(), startpole + bsp.degree, startpole + i); + deriv += (*thepoint() - *poleat(i)) * factorsI; + } + } + + return scale * deriv; +} + +// Difference +ConstraintDifference::ConstraintDifference(double* p1, double* p2, double* d) +{ + pvec.push_back(p1); + pvec.push_back(p2); + pvec.push_back(d); + origpvec = pvec; + rescale(); +} + +ConstraintType ConstraintDifference::getTypeId() +{ + return Difference; +} + +void ConstraintDifference::rescale(double coef) +{ + scale = coef * 1.; +} + +double ConstraintDifference::error() +{ + return scale * (*param2() - *param1() - *difference()); +} + +double ConstraintDifference::grad(double* param) +{ + double deriv = 0.; + if (param == param1()) { + deriv += -1; + } + if (param == param2()) { + deriv += 1; + } + if (param == difference()) { + deriv += -1; + } + return scale * deriv; +} + + +// -------------------------------------------------------- +// P2PDistance +ConstraintP2PDistance::ConstraintP2PDistance(Point& p1, Point& p2, double* d) +{ + pvec.push_back(p1.x); + pvec.push_back(p1.y); + pvec.push_back(p2.x); + pvec.push_back(p2.y); + pvec.push_back(d); + origpvec = pvec; + rescale(); +} + +ConstraintType ConstraintP2PDistance::getTypeId() +{ + return P2PDistance; +} + +void ConstraintP2PDistance::rescale(double coef) +{ + scale = coef * 1.; +} + +double ConstraintP2PDistance::error() +{ + double dx = (*p1x() - *p2x()); + double dy = (*p1y() - *p2y()); + double d = sqrt(dx * dx + dy * dy); + double dist = *distance(); + return scale * (d - dist); +} + +double ConstraintP2PDistance::grad(double* param) +{ + double deriv = 0.; + if (param == p1x() || param == p1y() || param == p2x() || param == p2y()) { + double dx = (*p1x() - *p2x()); + double dy = (*p1y() - *p2y()); + double d = sqrt(dx * dx + dy * dy); + if (param == p1x()) { + deriv += dx / d; + } + if (param == p1y()) { + deriv += dy / d; + } + if (param == p2x()) { + deriv += -dx / d; + } + if (param == p2y()) { + deriv += -dy / d; + } + } + if (param == distance()) { + deriv += -1.; + } + + return scale * deriv; +} + +double ConstraintP2PDistance::maxStep(MAP_pD_D& dir, double lim) +{ + MAP_pD_D::iterator it; + it = dir.find(distance()); + if (it != dir.end()) { + if (it->second < 0.) { + lim = std::min(lim, -(*distance()) / it->second); + } + } + // restrict actual distance change + double ddx = 0., ddy = 0.; + it = dir.find(p1x()); + if (it != dir.end()) { + ddx += it->second; + } + it = dir.find(p1y()); + if (it != dir.end()) { + ddy += it->second; + } + it = dir.find(p2x()); + if (it != dir.end()) { + ddx -= it->second; + } + it = dir.find(p2y()); + if (it != dir.end()) { + ddy -= it->second; + } + double dd = sqrt(ddx * ddx + ddy * ddy); + double dist = *distance(); + if (dd > dist) { + double dx = (*p1x() - *p2x()); + double dy = (*p1y() - *p2y()); + double d = sqrt(dx * dx + dy * dy); + if (dd > d) { + lim = std::min(lim, std::max(d, dist) / dd); + } + } + return lim; +} + + +// -------------------------------------------------------- +// P2PAngle +ConstraintP2PAngle::ConstraintP2PAngle(Point& p1, Point& p2, double* a, double da_) + : da(da_) +{ + pvec.push_back(p1.x); + pvec.push_back(p1.y); + pvec.push_back(p2.x); + pvec.push_back(p2.y); + pvec.push_back(a); + origpvec = pvec; + rescale(); +} + +ConstraintType ConstraintP2PAngle::getTypeId() +{ + return P2PAngle; +} + +void ConstraintP2PAngle::rescale(double coef) +{ + scale = coef * 1.; +} + +double ConstraintP2PAngle::error() +{ + double dx = (*p2x() - *p1x()); + double dy = (*p2y() - *p1y()); + double a = *angle() + da; + double ca = cos(a); + double sa = sin(a); + double x = dx * ca + dy * sa; + double y = -dx * sa + dy * ca; + return scale * atan2(y, x); +} + +double ConstraintP2PAngle::grad(double* param) +{ + double deriv = 0.; + if (param == p1x() || param == p1y() || param == p2x() || param == p2y()) { + double dx = (*p2x() - *p1x()); + double dy = (*p2y() - *p1y()); + double a = *angle() + da; + double ca = cos(a); + double sa = sin(a); + double x = dx * ca + dy * sa; + double y = -dx * sa + dy * ca; + double r2 = dx * dx + dy * dy; + dx = -y / r2; + dy = x / r2; + if (param == p1x()) { + deriv += (-ca * dx + sa * dy); + } + if (param == p1y()) { + deriv += (-sa * dx - ca * dy); + } + if (param == p2x()) { + deriv += (ca * dx - sa * dy); + } + if (param == p2y()) { + deriv += (sa * dx + ca * dy); + } + } + if (param == angle()) { + deriv += -1; + } + + return scale * deriv; +} + +double ConstraintP2PAngle::maxStep(MAP_pD_D& dir, double lim) +{ + constexpr double pi_18 = std::numbers::pi / 18; + + MAP_pD_D::iterator it = dir.find(angle()); + if (it != dir.end()) { + double step = std::abs(it->second); + if (step > pi_18) { + lim = std::min(lim, pi_18 / step); + } + } + return lim; +} + + +// -------------------------------------------------------- +// P2LDistance +ConstraintP2LDistance::ConstraintP2LDistance(Point& p, Line& l, double* d) +{ + pvec.push_back(p.x); + pvec.push_back(p.y); + pvec.push_back(l.p1.x); + pvec.push_back(l.p1.y); + pvec.push_back(l.p2.x); + pvec.push_back(l.p2.y); + pvec.push_back(d); + origpvec = pvec; + rescale(); +} + +ConstraintType ConstraintP2LDistance::getTypeId() +{ + return P2LDistance; +} + +void ConstraintP2LDistance::rescale(double coef) +{ + scale = coef; +} + +double ConstraintP2LDistance::error() +{ + double x0 = *p0x(), x1 = *p1x(), x2 = *p2x(); + double y0 = *p0y(), y1 = *p1y(), y2 = *p2y(); + double dist = *distance(); + double dx = x2 - x1; + double dy = y2 - y1; + double d = sqrt(dx * dx + dy * dy); // line length + double area = std::abs(-x0 * dy + y0 * dx + x1 * y2 - x2 * y1); + return scale * (area / d - dist); +} + +double ConstraintP2LDistance::grad(double* param) +{ + double deriv = 0.; + + if (param == p0x() || param == p0y() || param == p1x() || param == p1y() || param == p2x() + || param == p2y()) { + double x0 = *p0x(), x1 = *p1x(), x2 = *p2x(); + double y0 = *p0y(), y1 = *p1y(), y2 = *p2y(); + double dx = x2 - x1; + double dy = y2 - y1; + double d2 = dx * dx + dy * dy; + double d = sqrt(d2); + double area = -x0 * dy + y0 * dx + x1 * y2 - x2 * y1; + if (param == p0x()) { + deriv += (y1 - y2) / d; + } + if (param == p0y()) { + deriv += (x2 - x1) / d; + } + if (param == p1x()) { + deriv += ((y2 - y0) * d + (dx / d) * area) / d2; + } + if (param == p1y()) { + deriv += ((x0 - x2) * d + (dy / d) * area) / d2; + } + if (param == p2x()) { + deriv += ((y0 - y1) * d - (dx / d) * area) / d2; + } + if (param == p2y()) { + deriv += ((x1 - x0) * d - (dy / d) * area) / d2; + } + if (area < 0) { + deriv *= -1; + } + } + if (param == distance()) { + deriv += -1; + } + + return scale * deriv; +} + +double ConstraintP2LDistance::maxStep(MAP_pD_D& dir, double lim) +{ + MAP_pD_D::iterator it; + it = dir.find(distance()); + if (it != dir.end()) { + if (it->second < 0.) { + lim = std::min(lim, -(*distance()) / it->second); + } + } + // restrict actual area change + double darea = 0.; + double x0 = *p0x(), x1 = *p1x(), x2 = *p2x(); + double y0 = *p0y(), y1 = *p1y(), y2 = *p2y(); + it = dir.find(p0x()); + if (it != dir.end()) { + darea += (y1 - y2) * it->second; + } + it = dir.find(p0y()); + if (it != dir.end()) { + darea += (x2 - x1) * it->second; + } + it = dir.find(p1x()); + if (it != dir.end()) { + darea += (y2 - y0) * it->second; + } + it = dir.find(p1y()); + if (it != dir.end()) { + darea += (x0 - x2) * it->second; + } + it = dir.find(p2x()); + if (it != dir.end()) { + darea += (y0 - y1) * it->second; + } + it = dir.find(p2y()); + if (it != dir.end()) { + darea += (x1 - x0) * it->second; + } + + darea = std::abs(darea); + if (darea > 0.) { + double dx = x2 - x1; + double dy = y2 - y1; + double area = 0.3 * (*distance()) * sqrt(dx * dx + dy * dy); + if (darea > area) { + area = std::max(area, 0.3 * std::abs(-x0 * dy + y0 * dx + x1 * y2 - x2 * y1)); + if (darea > area) { + lim = std::min(lim, area / darea); + } + } + } + return lim; +} + + +// -------------------------------------------------------- +// PointOnLine +ConstraintPointOnLine::ConstraintPointOnLine(Point& p, Line& l) +{ + pvec.push_back(p.x); + pvec.push_back(p.y); + pvec.push_back(l.p1.x); + pvec.push_back(l.p1.y); + pvec.push_back(l.p2.x); + pvec.push_back(l.p2.y); + origpvec = pvec; + rescale(); +} + +ConstraintPointOnLine::ConstraintPointOnLine(Point& p, Point& lp1, Point& lp2) +{ + pvec.push_back(p.x); + pvec.push_back(p.y); + pvec.push_back(lp1.x); + pvec.push_back(lp1.y); + pvec.push_back(lp2.x); + pvec.push_back(lp2.y); + origpvec = pvec; + rescale(); +} + +ConstraintType ConstraintPointOnLine::getTypeId() +{ + return PointOnLine; +} + +void ConstraintPointOnLine::rescale(double coef) +{ + scale = coef; +} + +double ConstraintPointOnLine::error() +{ + double x0 = *p0x(), x1 = *p1x(), x2 = *p2x(); + double y0 = *p0y(), y1 = *p1y(), y2 = *p2y(); + double dx = x2 - x1; + double dy = y2 - y1; + double d = sqrt(dx * dx + dy * dy); + double area = -x0 * dy + y0 * dx + x1 * y2 - x2 * y1; + return scale * area / d; +} + +double ConstraintPointOnLine::grad(double* param) +{ + double deriv = 0.; + if (param == p0x() || param == p0y() || param == p1x() || param == p1y() || param == p2x() + || param == p2y()) { + double x0 = *p0x(), x1 = *p1x(), x2 = *p2x(); + double y0 = *p0y(), y1 = *p1y(), y2 = *p2y(); + double dx = x2 - x1; + double dy = y2 - y1; + double d2 = dx * dx + dy * dy; + double d = sqrt(d2); + double area = -x0 * dy + y0 * dx + x1 * y2 - x2 * y1; + if (param == p0x()) { + deriv += (y1 - y2) / d; + } + if (param == p0y()) { + deriv += (x2 - x1) / d; + } + if (param == p1x()) { + deriv += ((y2 - y0) * d + (dx / d) * area) / d2; + } + if (param == p1y()) { + deriv += ((x0 - x2) * d + (dy / d) * area) / d2; + } + if (param == p2x()) { + deriv += ((y0 - y1) * d - (dx / d) * area) / d2; + } + if (param == p2y()) { + deriv += ((x1 - x0) * d - (dy / d) * area) / d2; + } + } + return scale * deriv; +} + + +// -------------------------------------------------------- +// PointOnPerpBisector +ConstraintPointOnPerpBisector::ConstraintPointOnPerpBisector(Point& p, Line& l) +{ + pvec.push_back(p.x); + pvec.push_back(p.y); + pvec.push_back(l.p1.x); + pvec.push_back(l.p1.y); + pvec.push_back(l.p2.x); + pvec.push_back(l.p2.y); + origpvec = pvec; + rescale(); +} + +ConstraintPointOnPerpBisector::ConstraintPointOnPerpBisector(Point& p, Point& lp1, Point& lp2) +{ + pvec.push_back(p.x); + pvec.push_back(p.y); + pvec.push_back(lp1.x); + pvec.push_back(lp1.y); + pvec.push_back(lp2.x); + pvec.push_back(lp2.y); + origpvec = pvec; + rescale(); +} + +ConstraintType ConstraintPointOnPerpBisector::getTypeId() +{ + return PointOnPerpBisector; +} + +void ConstraintPointOnPerpBisector::rescale(double coef) +{ + scale = coef; +} + +void ConstraintPointOnPerpBisector::errorgrad(double* err, double* grad, double* param) +{ + DeriVector2 p0(Point(p0x(), p0y()), param); + DeriVector2 p1(Point(p1x(), p1y()), param); + DeriVector2 p2(Point(p2x(), p2y()), param); + + DeriVector2 d1 = p0.subtr(p1); + DeriVector2 d2 = p0.subtr(p2); + DeriVector2 D = p2.subtr(p1).getNormalized(); + + double projd1, dprojd1; + projd1 = d1.scalarProd(D, &dprojd1); + + double projd2, dprojd2; + projd2 = d2.scalarProd(D, &dprojd2); + + if (err) { + *err = projd1 + projd2; + } + if (grad) { + *grad = dprojd1 + dprojd2; + } +} + +double ConstraintPointOnPerpBisector::error() +{ + double err; + errorgrad(&err, nullptr, nullptr); + return scale * err; +} + +double ConstraintPointOnPerpBisector::grad(double* param) +{ + // first of all, check that we need to compute anything. + if (findParamInPvec(param) == -1) { + return 0.0; + } + + double deriv; + errorgrad(nullptr, &deriv, param); + + return deriv * scale; +} + + +// -------------------------------------------------------- +// Parallel +ConstraintParallel::ConstraintParallel(Line& l1, Line& l2) +{ + pvec.push_back(l1.p1.x); + pvec.push_back(l1.p1.y); + pvec.push_back(l1.p2.x); + pvec.push_back(l1.p2.y); + pvec.push_back(l2.p1.x); + pvec.push_back(l2.p1.y); + pvec.push_back(l2.p2.x); + pvec.push_back(l2.p2.y); + origpvec = pvec; + rescale(); +} + +ConstraintType ConstraintParallel::getTypeId() +{ + return Parallel; +} + +void ConstraintParallel::rescale(double coef) +{ + double dx1 = (*l1p1x() - *l1p2x()); + double dy1 = (*l1p1y() - *l1p2y()); + double dx2 = (*l2p1x() - *l2p2x()); + double dy2 = (*l2p1y() - *l2p2y()); + scale = coef / sqrt((dx1 * dx1 + dy1 * dy1) * (dx2 * dx2 + dy2 * dy2)); +} + +double ConstraintParallel::error() +{ + double dx1 = (*l1p1x() - *l1p2x()); + double dy1 = (*l1p1y() - *l1p2y()); + double dx2 = (*l2p1x() - *l2p2x()); + double dy2 = (*l2p1y() - *l2p2y()); + return scale * (dx1 * dy2 - dy1 * dx2); +} + +double ConstraintParallel::grad(double* param) +{ + double deriv = 0.; + if (param == l1p1x()) { + deriv += (*l2p1y() - *l2p2y()); // = dy2 + } + if (param == l1p2x()) { + deriv += -(*l2p1y() - *l2p2y()); // = -dy2 + } + if (param == l1p1y()) { + deriv += -(*l2p1x() - *l2p2x()); // = -dx2 + } + if (param == l1p2y()) { + deriv += (*l2p1x() - *l2p2x()); // = dx2 + } + + if (param == l2p1x()) { + deriv += -(*l1p1y() - *l1p2y()); // = -dy1 + } + if (param == l2p2x()) { + deriv += (*l1p1y() - *l1p2y()); // = dy1 + } + if (param == l2p1y()) { + deriv += (*l1p1x() - *l1p2x()); // = dx1 + } + if (param == l2p2y()) { + deriv += -(*l1p1x() - *l1p2x()); // = -dx1 + } + + return scale * deriv; +} + + +// -------------------------------------------------------- +// Perpendicular +ConstraintPerpendicular::ConstraintPerpendicular(Line& l1, Line& l2) +{ + pvec.push_back(l1.p1.x); + pvec.push_back(l1.p1.y); + pvec.push_back(l1.p2.x); + pvec.push_back(l1.p2.y); + pvec.push_back(l2.p1.x); + pvec.push_back(l2.p1.y); + pvec.push_back(l2.p2.x); + pvec.push_back(l2.p2.y); + origpvec = pvec; + rescale(); +} + +ConstraintPerpendicular::ConstraintPerpendicular(Point& l1p1, Point& l1p2, Point& l2p1, Point& l2p2) +{ + pvec.push_back(l1p1.x); + pvec.push_back(l1p1.y); + pvec.push_back(l1p2.x); + pvec.push_back(l1p2.y); + pvec.push_back(l2p1.x); + pvec.push_back(l2p1.y); + pvec.push_back(l2p2.x); + pvec.push_back(l2p2.y); + origpvec = pvec; + rescale(); +} + +ConstraintType ConstraintPerpendicular::getTypeId() +{ + return Perpendicular; +} + +void ConstraintPerpendicular::rescale(double coef) +{ + double dx1 = (*l1p1x() - *l1p2x()); + double dy1 = (*l1p1y() - *l1p2y()); + double dx2 = (*l2p1x() - *l2p2x()); + double dy2 = (*l2p1y() - *l2p2y()); + scale = coef / sqrt((dx1 * dx1 + dy1 * dy1) * (dx2 * dx2 + dy2 * dy2)); +} + +double ConstraintPerpendicular::error() +{ + double dx1 = (*l1p1x() - *l1p2x()); + double dy1 = (*l1p1y() - *l1p2y()); + double dx2 = (*l2p1x() - *l2p2x()); + double dy2 = (*l2p1y() - *l2p2y()); + return scale * (dx1 * dx2 + dy1 * dy2); +} + +double ConstraintPerpendicular::grad(double* param) +{ + double deriv = 0.; + if (param == l1p1x()) { + deriv += (*l2p1x() - *l2p2x()); // = dx2 + } + if (param == l1p2x()) { + deriv += -(*l2p1x() - *l2p2x()); // = -dx2 + } + if (param == l1p1y()) { + deriv += (*l2p1y() - *l2p2y()); // = dy2 + } + if (param == l1p2y()) { + deriv += -(*l2p1y() - *l2p2y()); // = -dy2 + } + + if (param == l2p1x()) { + deriv += (*l1p1x() - *l1p2x()); // = dx1 + } + if (param == l2p2x()) { + deriv += -(*l1p1x() - *l1p2x()); // = -dx1 + } + if (param == l2p1y()) { + deriv += (*l1p1y() - *l1p2y()); // = dy1 + } + if (param == l2p2y()) { + deriv += -(*l1p1y() - *l1p2y()); // = -dy1 + } + + return scale * deriv; +} + + +// -------------------------------------------------------- +// L2LAngle +ConstraintL2LAngle::ConstraintL2LAngle(Line& l1, Line& l2, double* a) +{ + pvec.push_back(l1.p1.x); + pvec.push_back(l1.p1.y); + pvec.push_back(l1.p2.x); + pvec.push_back(l1.p2.y); + pvec.push_back(l2.p1.x); + pvec.push_back(l2.p1.y); + pvec.push_back(l2.p2.x); + pvec.push_back(l2.p2.y); + pvec.push_back(a); + origpvec = pvec; + rescale(); +} + +ConstraintL2LAngle::ConstraintL2LAngle(Point& l1p1, + Point& l1p2, + Point& l2p1, + Point& l2p2, + double* a) +{ + pvec.push_back(l1p1.x); + pvec.push_back(l1p1.y); + pvec.push_back(l1p2.x); + pvec.push_back(l1p2.y); + pvec.push_back(l2p1.x); + pvec.push_back(l2p1.y); + pvec.push_back(l2p2.x); + pvec.push_back(l2p2.y); + pvec.push_back(a); + origpvec = pvec; + rescale(); +} + +ConstraintType ConstraintL2LAngle::getTypeId() +{ + return L2LAngle; +} + +void ConstraintL2LAngle::rescale(double coef) +{ + scale = coef * 1.; +} + +double ConstraintL2LAngle::error() +{ + double dx1 = (*l1p2x() - *l1p1x()); + double dy1 = (*l1p2y() - *l1p1y()); + double dx2 = (*l2p2x() - *l2p1x()); + double dy2 = (*l2p2y() - *l2p1y()); + double a = atan2(dy1, dx1) + *angle(); + double ca = cos(a); + double sa = sin(a); + double x2 = dx2 * ca + dy2 * sa; + double y2 = -dx2 * sa + dy2 * ca; + return scale * atan2(y2, x2); +} + +double ConstraintL2LAngle::grad(double* param) +{ + double deriv = 0.; + if (param == l1p1x() || param == l1p1y() || param == l1p2x() || param == l1p2y()) { + double dx1 = (*l1p2x() - *l1p1x()); + double dy1 = (*l1p2y() - *l1p1y()); + double r2 = dx1 * dx1 + dy1 * dy1; + if (param == l1p1x()) { + deriv += -dy1 / r2; + } + if (param == l1p1y()) { + deriv += dx1 / r2; + } + if (param == l1p2x()) { + deriv += dy1 / r2; + } + if (param == l1p2y()) { + deriv += -dx1 / r2; + } + } + if (param == l2p1x() || param == l2p1y() || param == l2p2x() || param == l2p2y()) { + double dx1 = (*l1p2x() - *l1p1x()); + double dy1 = (*l1p2y() - *l1p1y()); + double dx2 = (*l2p2x() - *l2p1x()); + double dy2 = (*l2p2y() - *l2p1y()); + double a = atan2(dy1, dx1) + *angle(); + double ca = cos(a); + double sa = sin(a); + double x2 = dx2 * ca + dy2 * sa; + double y2 = -dx2 * sa + dy2 * ca; + double r2 = dx2 * dx2 + dy2 * dy2; + dx2 = -y2 / r2; + dy2 = x2 / r2; + if (param == l2p1x()) { + deriv += (-ca * dx2 + sa * dy2); + } + if (param == l2p1y()) { + deriv += (-sa * dx2 - ca * dy2); + } + if (param == l2p2x()) { + deriv += (ca * dx2 - sa * dy2); + } + if (param == l2p2y()) { + deriv += (sa * dx2 + ca * dy2); + } + } + if (param == angle()) { + deriv += -1; + } + + return scale * deriv; +} + +double ConstraintL2LAngle::maxStep(MAP_pD_D& dir, double lim) +{ + constexpr double pi_18 = std::numbers::pi / 18; + + MAP_pD_D::iterator it = dir.find(angle()); + if (it != dir.end()) { + double step = std::abs(it->second); + if (step > pi_18) { + lim = std::min(lim, pi_18 / step); + } + } + return lim; +} + + +// -------------------------------------------------------- +// MidpointOnLine +ConstraintMidpointOnLine::ConstraintMidpointOnLine(Line& l1, Line& l2) +{ + pvec.push_back(l1.p1.x); + pvec.push_back(l1.p1.y); + pvec.push_back(l1.p2.x); + pvec.push_back(l1.p2.y); + pvec.push_back(l2.p1.x); + pvec.push_back(l2.p1.y); + pvec.push_back(l2.p2.x); + pvec.push_back(l2.p2.y); + origpvec = pvec; + rescale(); +} + +ConstraintMidpointOnLine::ConstraintMidpointOnLine(Point& l1p1, + Point& l1p2, + Point& l2p1, + Point& l2p2) +{ + pvec.push_back(l1p1.x); + pvec.push_back(l1p1.y); + pvec.push_back(l1p2.x); + pvec.push_back(l1p2.y); + pvec.push_back(l2p1.x); + pvec.push_back(l2p1.y); + pvec.push_back(l2p2.x); + pvec.push_back(l2p2.y); + origpvec = pvec; + rescale(); +} + +ConstraintType ConstraintMidpointOnLine::getTypeId() +{ + return MidpointOnLine; +} + +void ConstraintMidpointOnLine::rescale(double coef) +{ + scale = coef * 1; +} + +double ConstraintMidpointOnLine::error() +{ + double x0 = ((*l1p1x()) + (*l1p2x())) / 2; + double y0 = ((*l1p1y()) + (*l1p2y())) / 2; + double x1 = *l2p1x(), x2 = *l2p2x(); + double y1 = *l2p1y(), y2 = *l2p2y(); + double dx = x2 - x1; + double dy = y2 - y1; + double d = sqrt(dx * dx + dy * dy); + double area = -x0 * dy + y0 * dx + x1 * y2 - x2 * y1; // = 2*(triangle area) + return scale * area / d; +} + +double ConstraintMidpointOnLine::grad(double* param) +{ + double deriv = 0.; + if (param == l1p1x() || param == l1p1y() || param == l1p2x() || param == l1p2y() + || param == l2p1x() || param == l2p1y() || param == l2p2x() || param == l2p2y()) { + double x0 = ((*l1p1x()) + (*l1p2x())) / 2; + double y0 = ((*l1p1y()) + (*l1p2y())) / 2; + double x1 = *l2p1x(), x2 = *l2p2x(); + double y1 = *l2p1y(), y2 = *l2p2y(); + double dx = x2 - x1; + double dy = y2 - y1; + double d2 = dx * dx + dy * dy; + double d = sqrt(d2); + double area = -x0 * dy + y0 * dx + x1 * y2 - x2 * y1; + if (param == l1p1x()) { + deriv += (y1 - y2) / (2 * d); + } + if (param == l1p1y()) { + deriv += (x2 - x1) / (2 * d); + } + if (param == l1p2x()) { + deriv += (y1 - y2) / (2 * d); + } + if (param == l1p2y()) { + deriv += (x2 - x1) / (2 * d); + } + if (param == l2p1x()) { + deriv += ((y2 - y0) * d + (dx / d) * area) / d2; + } + if (param == l2p1y()) { + deriv += ((x0 - x2) * d + (dy / d) * area) / d2; + } + if (param == l2p2x()) { + deriv += ((y0 - y1) * d - (dx / d) * area) / d2; + } + if (param == l2p2y()) { + deriv += ((x1 - x0) * d - (dy / d) * area) / d2; + } + } + return scale * deriv; +} + + +// -------------------------------------------------------- +// TangentCircumf +ConstraintTangentCircumf::ConstraintTangentCircumf(Point& p1, + Point& p2, + double* rad1, + double* rad2, + bool internal_) +{ + internal = internal_; + pvec.push_back(p1.x); + pvec.push_back(p1.y); + pvec.push_back(p2.x); + pvec.push_back(p2.y); + pvec.push_back(rad1); + pvec.push_back(rad2); + origpvec = pvec; + rescale(); +} + +ConstraintType ConstraintTangentCircumf::getTypeId() +{ + return TangentCircumf; +} + +void ConstraintTangentCircumf::rescale(double coef) +{ + scale = coef * 1; +} + +double ConstraintTangentCircumf::error() +{ + double dx = (*c1x() - *c2x()); + double dy = (*c1y() - *c2y()); + if (internal) { + return scale * ((dx * dx + dy * dy) - (*r1() - *r2()) * (*r1() - *r2())); + } + else { + return scale * ((dx * dx + dy * dy) - (*r1() + *r2()) * (*r1() + *r2())); + } +} + +double ConstraintTangentCircumf::grad(double* param) +{ + double deriv = 0.; + if (param == c1x() || param == c1y() || param == c2x() || param == c2y() || param == r1() + || param == r2()) { + double dx = (*c1x() - *c2x()); + double dy = (*c1y() - *c2y()); + if (param == c1x()) { + deriv += 2 * dx; + } + if (param == c1y()) { + deriv += 2 * dy; + } + if (param == c2x()) { + deriv += 2 * -dx; + } + if (param == c2y()) { + deriv += 2 * -dy; + } + if (internal) { + if (param == r1()) { + deriv += 2 * (*r2() - *r1()); + } + if (param == r2()) { + deriv += 2 * (*r1() - *r2()); + } + } + else { + if (param == r1()) { + deriv += -2 * (*r1() + *r2()); + } + if (param == r2()) { + deriv += -2 * (*r1() + *r2()); + } + } + } + return scale * deriv; +} + + +// -------------------------------------------------------- +// ConstraintPointOnEllipse +ConstraintPointOnEllipse::ConstraintPointOnEllipse(Point& p, Ellipse& e) +{ + pvec.push_back(p.x); + pvec.push_back(p.y); + pvec.push_back(e.center.x); + pvec.push_back(e.center.y); + pvec.push_back(e.focus1.x); + pvec.push_back(e.focus1.y); + pvec.push_back(e.radmin); + origpvec = pvec; + rescale(); +} + +ConstraintType ConstraintPointOnEllipse::getTypeId() +{ + return PointOnEllipse; +} + +void ConstraintPointOnEllipse::rescale(double coef) +{ + scale = coef * 1; +} + +double ConstraintPointOnEllipse::error() +{ + double X_0 = *p1x(); + double Y_0 = *p1y(); + double X_c = *cx(); + double Y_c = *cy(); + double X_F1 = *f1x(); + double Y_F1 = *f1y(); + double b = *rmin(); + + double err = sqrt(pow(X_0 - X_F1, 2) + pow(Y_0 - Y_F1, 2)) + + sqrt(pow(X_0 + X_F1 - 2 * X_c, 2) + pow(Y_0 + Y_F1 - 2 * Y_c, 2)) + - 2 * sqrt(pow(b, 2) + pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2)); + return scale * err; +} + +double ConstraintPointOnEllipse::grad(double* param) +{ + double deriv = 0.; + if (param == p1x() || param == p1y() || param == f1x() || param == f1y() || param == cx() + || param == cy() || param == rmin()) { + + double X_0 = *p1x(); + double Y_0 = *p1y(); + double X_c = *cx(); + double Y_c = *cy(); + double X_F1 = *f1x(); + double Y_F1 = *f1y(); + double b = *rmin(); + + if (param == p1x()) { + deriv += (X_0 - X_F1) / sqrt(pow(X_0 - X_F1, 2) + pow(Y_0 - Y_F1, 2)) + + (X_0 + X_F1 - 2 * X_c) + / sqrt(pow(X_0 + X_F1 - 2 * X_c, 2) + pow(Y_0 + Y_F1 - 2 * Y_c, 2)); + } + if (param == p1y()) { + deriv += (Y_0 - Y_F1) / sqrt(pow(X_0 - X_F1, 2) + pow(Y_0 - Y_F1, 2)) + + (Y_0 + Y_F1 - 2 * Y_c) + / sqrt(pow(X_0 + X_F1 - 2 * X_c, 2) + pow(Y_0 + Y_F1 - 2 * Y_c, 2)); + } + if (param == f1x()) { + deriv += -(X_0 - X_F1) / sqrt(pow(X_0 - X_F1, 2) + pow(Y_0 - Y_F1, 2)) + - 2 * (X_F1 - X_c) / sqrt(pow(b, 2) + pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2)) + + (X_0 + X_F1 - 2 * X_c) + / sqrt(pow(X_0 + X_F1 - 2 * X_c, 2) + pow(Y_0 + Y_F1 - 2 * Y_c, 2)); + } + if (param == f1y()) { + deriv += -(Y_0 - Y_F1) / sqrt(pow(X_0 - X_F1, 2) + pow(Y_0 - Y_F1, 2)) + - 2 * (Y_F1 - Y_c) / sqrt(pow(b, 2) + pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2)) + + (Y_0 + Y_F1 - 2 * Y_c) + / sqrt(pow(X_0 + X_F1 - 2 * X_c, 2) + pow(Y_0 + Y_F1 - 2 * Y_c, 2)); + } + if (param == cx()) { + deriv += 2 * (X_F1 - X_c) / sqrt(pow(b, 2) + pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2)) + - 2 * (X_0 + X_F1 - 2 * X_c) + / sqrt(pow(X_0 + X_F1 - 2 * X_c, 2) + pow(Y_0 + Y_F1 - 2 * Y_c, 2)); + } + if (param == cy()) { + deriv += 2 * (Y_F1 - Y_c) / sqrt(pow(b, 2) + pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2)) + - 2 * (Y_0 + Y_F1 - 2 * Y_c) + / sqrt(pow(X_0 + X_F1 - 2 * X_c, 2) + pow(Y_0 + Y_F1 - 2 * Y_c, 2)); + } + if (param == rmin()) { + deriv += -2 * b / sqrt(pow(b, 2) + pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2)); + } + } + return scale * deriv; +} + + +// -------------------------------------------------------- +// ConstraintEllipseTangentLine +ConstraintEllipseTangentLine::ConstraintEllipseTangentLine(Line& l, Ellipse& e) +{ + this->l = l; + this->l.PushOwnParams(pvec); + + this->e = e; + this->e.PushOwnParams(pvec); // DeepSOIC: hopefully, this won't push arc's parameters + origpvec = pvec; + pvecChangedFlag = true; + rescale(); +} + +void ConstraintEllipseTangentLine::ReconstructGeomPointers() +{ + int i = 0; + l.ReconstructOnNewPvec(pvec, i); + e.ReconstructOnNewPvec(pvec, i); + pvecChangedFlag = false; +} + +ConstraintType ConstraintEllipseTangentLine::getTypeId() +{ + return TangentEllipseLine; +} + +void ConstraintEllipseTangentLine::rescale(double coef) +{ + scale = coef * 1; +} + +void ConstraintEllipseTangentLine::errorgrad(double* err, double* grad, double* param) +{ + // DeepSOIC equation + // https://forum.freecad.org/viewtopic.php?f=10&t=7520&start=140 + + if (pvecChangedFlag) { + ReconstructGeomPointers(); + } + DeriVector2 p1(l.p1, param); + DeriVector2 p2(l.p2, param); + DeriVector2 f1(e.focus1, param); + DeriVector2 c(e.center, param); + DeriVector2 f2 = c.linCombi(2.0, f1, -1.0); // 2*cv - f1v + + // mirror F1 against the line + DeriVector2 nl = l.CalculateNormal(l.p1, param).getNormalized(); + double distF1L = 0, ddistF1L = 0; // distance F1 to line + distF1L = f1.subtr(p1).scalarProd(nl, &ddistF1L); + DeriVector2 f1m = f1.sum(nl.multD(-2 * distF1L, -2 * ddistF1L)); // f1m = f1 mirrored + + // calculate distance form f1m to f2 + double distF1mF2, ddistF1mF2; + distF1mF2 = f2.subtr(f1m).length(ddistF1mF2); + + // calculate major radius (to compare the distance to) + double dradmin = (param == e.radmin) ? 1.0 : 0.0; + double radmaj, dradmaj; + radmaj = e.getRadMaj(c, f1, *e.radmin, dradmin, dradmaj); + + if (err) { + *err = distF1mF2 - 2 * radmaj; + } + if (grad) { + *grad = ddistF1mF2 - 2 * dradmaj; + } +} + +double ConstraintEllipseTangentLine::error() +{ + double err; + errorgrad(&err, nullptr, nullptr); + return scale * err; +} + +double ConstraintEllipseTangentLine::grad(double* param) +{ + // first of all, check that we need to compute anything. + if (findParamInPvec(param) == -1) { + return 0.0; + } + + double deriv; + errorgrad(nullptr, &deriv, param); + + return deriv * scale; +} + + +// -------------------------------------------------------- +// ConstraintInternalAlignmentPoint2Ellipse +ConstraintInternalAlignmentPoint2Ellipse::ConstraintInternalAlignmentPoint2Ellipse( + Ellipse& e, + Point& p1, + InternalAlignmentType alignmentType) +{ + this->p = p1; + pvec.push_back(p.x); + pvec.push_back(p.y); + this->e = e; + this->e.PushOwnParams(pvec); + this->AlignmentType = alignmentType; + origpvec = pvec; + rescale(); +} + +void ConstraintInternalAlignmentPoint2Ellipse::ReconstructGeomPointers() +{ + int i = 0; + p.x = pvec[i]; + i++; + p.y = pvec[i]; + i++; + e.ReconstructOnNewPvec(pvec, i); + pvecChangedFlag = false; +} + +ConstraintType ConstraintInternalAlignmentPoint2Ellipse::getTypeId() +{ + return InternalAlignmentPoint2Ellipse; +} + +void ConstraintInternalAlignmentPoint2Ellipse::rescale(double coef) +{ + scale = coef * 1; +} + +void ConstraintInternalAlignmentPoint2Ellipse::errorgrad(double* err, double* grad, double* param) +{ + if (pvecChangedFlag) { + ReconstructGeomPointers(); + } + + // todo: prefill only what's needed, not everything + + DeriVector2 c(e.center, param); + DeriVector2 f1(e.focus1, param); + DeriVector2 emaj = f1.subtr(c).getNormalized(); + DeriVector2 emin = emaj.rotate90ccw(); + DeriVector2 pv(p, param); + double b, db; // minor radius + b = *e.radmin; + db = (e.radmin == param) ? 1.0 : 0.0; + + // major radius + double a, da; + a = e.getRadMaj(c, f1, b, db, da); + + DeriVector2 poa; // point to align to + bool by_y_not_by_x = + false; // a flag to indicate if the alignment error function is for y (false - x, true - y) + + switch (AlignmentType) { + case EllipsePositiveMajorX: + case EllipsePositiveMajorY: + poa = c.sum(emaj.multD(a, da)); + by_y_not_by_x = AlignmentType == EllipsePositiveMajorY; + break; + case EllipseNegativeMajorX: + case EllipseNegativeMajorY: + poa = c.sum(emaj.multD(-a, -da)); + by_y_not_by_x = AlignmentType == EllipseNegativeMajorY; + break; + case EllipsePositiveMinorX: + case EllipsePositiveMinorY: + poa = c.sum(emin.multD(b, db)); + by_y_not_by_x = AlignmentType == EllipsePositiveMinorY; + break; + case EllipseNegativeMinorX: + case EllipseNegativeMinorY: + poa = c.sum(emin.multD(-b, -db)); + by_y_not_by_x = AlignmentType == EllipseNegativeMinorY; + break; + case EllipseFocus2X: + case EllipseFocus2Y: + poa = c.linCombi(2.0, f1, -1.0); + by_y_not_by_x = AlignmentType == EllipseFocus2Y; + break; + default: + // shouldn't happen + poa = pv; // align to the point itself, doing nothing essentially + } + if (err) { + *err = by_y_not_by_x ? pv.y - poa.y : pv.x - poa.x; + } + if (grad) { + *grad = by_y_not_by_x ? pv.dy - poa.dy : pv.dx - poa.dx; + } +} + +double ConstraintInternalAlignmentPoint2Ellipse::error() +{ + double err; + errorgrad(&err, nullptr, nullptr); + return scale * err; +} + +double ConstraintInternalAlignmentPoint2Ellipse::grad(double* param) +{ + // first of all, check that we need to compute anything. + if (findParamInPvec(param) == -1) { + return 0.0; + } + + double deriv; + errorgrad(nullptr, &deriv, param); + + return deriv * scale; +} + + +// -------------------------------------------------------- +// ConstraintInternalAlignmentPoint2Hyperbola +ConstraintInternalAlignmentPoint2Hyperbola::ConstraintInternalAlignmentPoint2Hyperbola( + Hyperbola& e, + Point& p1, + InternalAlignmentType alignmentType) +{ + this->p = p1; + pvec.push_back(p.x); + pvec.push_back(p.y); + this->e = e; + this->e.PushOwnParams(pvec); + this->AlignmentType = alignmentType; + origpvec = pvec; + rescale(); +} + +void ConstraintInternalAlignmentPoint2Hyperbola::ReconstructGeomPointers() +{ + int i = 0; + p.x = pvec[i]; + i++; + p.y = pvec[i]; + i++; + e.ReconstructOnNewPvec(pvec, i); + pvecChangedFlag = false; +} + +ConstraintType ConstraintInternalAlignmentPoint2Hyperbola::getTypeId() +{ + return InternalAlignmentPoint2Hyperbola; +} + +void ConstraintInternalAlignmentPoint2Hyperbola::rescale(double coef) +{ + scale = coef * 1; +} + +void ConstraintInternalAlignmentPoint2Hyperbola::errorgrad(double* err, double* grad, double* param) +{ + if (pvecChangedFlag) { + ReconstructGeomPointers(); + } + + // todo: prefill only what's needed, not everything + + DeriVector2 c(e.center, param); + DeriVector2 f1(e.focus1, param); + DeriVector2 emaj = f1.subtr(c).getNormalized(); + DeriVector2 emin = emaj.rotate90ccw(); + DeriVector2 pv(p, param); + + double b, db; // minor radius + b = *e.radmin; + db = (e.radmin == param) ? 1.0 : 0.0; + + // major radius + double a, da; + a = e.getRadMaj(c, f1, b, db, da); + + DeriVector2 poa; // point to align to + bool by_y_not_by_x = + false; // a flag to indicate if the alignment error function is for y (false - x, true - y) + + switch (AlignmentType) { + case HyperbolaPositiveMajorX: + case HyperbolaPositiveMajorY: + poa = c.sum(emaj.multD(a, da)); + by_y_not_by_x = AlignmentType == HyperbolaPositiveMajorY; + break; + case HyperbolaNegativeMajorX: + case HyperbolaNegativeMajorY: + poa = c.sum(emaj.multD(-a, -da)); + by_y_not_by_x = AlignmentType == HyperbolaNegativeMajorY; + break; + case HyperbolaPositiveMinorX: + case HyperbolaPositiveMinorY: { + DeriVector2 pa = c.sum(emaj.multD(a, da)); + // DeriVector2 A(pa.x,pa.y); + // poa = A.sum(emin.multD(b, db)); + poa = pa.sum(emin.multD(b, db)); + by_y_not_by_x = AlignmentType == HyperbolaPositiveMinorY; + break; + } + case HyperbolaNegativeMinorX: + case HyperbolaNegativeMinorY: { + DeriVector2 pa = c.sum(emaj.multD(a, da)); + // DeriVector2 A(pa.x,pa.y); + // poa = A.sum(emin.multD(-b, -db)); + poa = pa.sum(emin.multD(-b, -db)); + by_y_not_by_x = AlignmentType == HyperbolaNegativeMinorY; + break; + } + default: + // shouldn't happen + poa = pv; // align to the point itself, doing nothing essentially + } + + if (err) { + *err = by_y_not_by_x ? pv.y - poa.y : pv.x - poa.x; + } + if (grad) { + *grad = by_y_not_by_x ? pv.dy - poa.dy : pv.dx - poa.dx; + } +} + +double ConstraintInternalAlignmentPoint2Hyperbola::error() +{ + double err; + errorgrad(&err, nullptr, nullptr); + return scale * err; +} + +double ConstraintInternalAlignmentPoint2Hyperbola::grad(double* param) +{ + // first of all, check that we need to compute anything. + if (findParamInPvec(param) == -1) { + return 0.0; + } + + double deriv; + errorgrad(nullptr, &deriv, param); + + return deriv * scale; +} + + +// -------------------------------------------------------- +// ConstraintEqualMajorAxesEllipse +ConstraintEqualMajorAxesConic::ConstraintEqualMajorAxesConic(MajorRadiusConic* a1, + MajorRadiusConic* a2) +{ + this->e1 = a1; + this->e1->PushOwnParams(pvec); + this->e2 = a2; + this->e2->PushOwnParams(pvec); + origpvec = pvec; + pvecChangedFlag = true; + rescale(); +} + +void ConstraintEqualMajorAxesConic::ReconstructGeomPointers() +{ + int i = 0; + e1->ReconstructOnNewPvec(pvec, i); + e2->ReconstructOnNewPvec(pvec, i); + pvecChangedFlag = false; +} + +ConstraintType ConstraintEqualMajorAxesConic::getTypeId() +{ + return EqualMajorAxesConic; +} + +void ConstraintEqualMajorAxesConic::rescale(double coef) +{ + scale = coef * 1; +} + +void ConstraintEqualMajorAxesConic::errorgrad(double* err, double* grad, double* param) +{ + if (pvecChangedFlag) { + ReconstructGeomPointers(); + } + double a1, da1; + a1 = e1->getRadMaj(param, da1); + double a2, da2; + a2 = e2->getRadMaj(param, da2); + if (err) { + *err = a2 - a1; + } + if (grad) { + *grad = da2 - da1; + } +} + +double ConstraintEqualMajorAxesConic::error() +{ + double err; + errorgrad(&err, nullptr, nullptr); + return scale * err; +} + +double ConstraintEqualMajorAxesConic::grad(double* param) +{ + // first of all, check that we need to compute anything. + if (findParamInPvec(param) == -1) { + return 0.0; + } + + double deriv; + errorgrad(nullptr, &deriv, param); + + return deriv * scale; +} + +// ConstraintEqualFocalDistance +ConstraintEqualFocalDistance::ConstraintEqualFocalDistance(ArcOfParabola* a1, ArcOfParabola* a2) +{ + this->e1 = a1; + this->e1->PushOwnParams(pvec); + this->e2 = a2; + this->e2->PushOwnParams(pvec); + origpvec = pvec; + pvecChangedFlag = true; + rescale(); +} + +void ConstraintEqualFocalDistance::ReconstructGeomPointers() +{ + int i = 0; + e1->ReconstructOnNewPvec(pvec, i); + e2->ReconstructOnNewPvec(pvec, i); + pvecChangedFlag = false; +} + +ConstraintType ConstraintEqualFocalDistance::getTypeId() +{ + return EqualFocalDistance; +} + +void ConstraintEqualFocalDistance::rescale(double coef) +{ + scale = coef * 1; +} + +void ConstraintEqualFocalDistance::errorgrad(double* err, double* grad, double* param) +{ + if (pvecChangedFlag) { + ReconstructGeomPointers(); + } + + DeriVector2 focus1(this->e1->focus1, param); + DeriVector2 vertex1(this->e1->vertex, param); + + DeriVector2 focalvect1 = vertex1.subtr(focus1); + + double focal1, dfocal1; + + focal1 = focalvect1.length(dfocal1); + + DeriVector2 focus2(this->e2->focus1, param); + DeriVector2 vertex2(this->e2->vertex, param); + + DeriVector2 focalvect2 = vertex2.subtr(focus2); + + double focal2, dfocal2; + + focal2 = focalvect2.length(dfocal2); + + if (err) { + *err = focal2 - focal1; + } + if (grad) { + *grad = dfocal2 - dfocal1; + } +} + +double ConstraintEqualFocalDistance::error() +{ + double err; + errorgrad(&err, nullptr, nullptr); + return scale * err; +} + +double ConstraintEqualFocalDistance::grad(double* param) +{ + // first of all, check that we need to compute anything. + if (findParamInPvec(param) == -1) { + return 0.0; + } + + double deriv; + errorgrad(nullptr, &deriv, param); + + return deriv * scale; +} + + +// -------------------------------------------------------- +// ConstraintCurveValue +ConstraintCurveValue::ConstraintCurveValue(Point& p, double* pcoord, Curve& crv, double* u) +{ + pvec.push_back(p.x); + pvec.push_back(p.y); + pvec.push_back(pcoord); + pvec.push_back(u); + crv.PushOwnParams(pvec); + this->crv = crv.Copy(); + pvecChangedFlag = true; + origpvec = pvec; + rescale(); +} + +ConstraintCurveValue::~ConstraintCurveValue() +{ + delete this->crv; + this->crv = nullptr; +} + +void ConstraintCurveValue::ReconstructGeomPointers() +{ + int i = 0; + p.x = pvec[i]; + i++; + p.y = pvec[i]; + i++; + i++; // we have an inline function for point coordinate + i++; // we have an inline function for the parameterU + this->crv->ReconstructOnNewPvec(pvec, i); + pvecChangedFlag = false; +} + +ConstraintType ConstraintCurveValue::getTypeId() +{ + return CurveValue; +} + +void ConstraintCurveValue::rescale(double coef) +{ + scale = coef * 1; +} + +void ConstraintCurveValue::errorgrad(double* err, double* grad, double* param) +{ + if (pvecChangedFlag) { + ReconstructGeomPointers(); + } + + double u, du; + u = *(this->u()); + du = (param == this->u()) ? 1.0 : 0.0; + + DeriVector2 P_to; // point of curve at parameter value of u, in global coordinates + P_to = this->crv->Value(u, du, param); + + DeriVector2 P_from(this->p, param); // point to be constrained + + DeriVector2 err_vec = P_from.subtr(P_to); + + if (this->pcoord() == this->p.x) { // this constraint is for X projection + if (err) { + *err = err_vec.x; + } + if (grad) { + *grad = err_vec.dx; + } + } + else if (this->pcoord() == this->p.y) { // this constraint is for Y projection + if (err) { + *err = err_vec.y; + } + if (grad) { + *grad = err_vec.dy; + } + } + else { + assert(false /*this constraint is neither X nor Y. Nothing to do..*/); + } +} + +double ConstraintCurveValue::error() +{ + double err; + errorgrad(&err, nullptr, nullptr); + return scale * err; +} + +double ConstraintCurveValue::grad(double* param) +{ + // first of all, check that we need to compute anything. + if (findParamInPvec(param) == -1) { + return 0.0; + } + + double deriv; + errorgrad(nullptr, &deriv, param); + + return deriv * scale; +} + +double ConstraintCurveValue::maxStep(MAP_pD_D& /*dir*/, double lim) +{ + return lim; +} + + +// -------------------------------------------------------- +// ConstraintPointOnHyperbola +ConstraintPointOnHyperbola::ConstraintPointOnHyperbola(Point& p, Hyperbola& e) +{ + pvec.push_back(p.x); + pvec.push_back(p.y); + pvec.push_back(e.center.x); + pvec.push_back(e.center.y); + pvec.push_back(e.focus1.x); + pvec.push_back(e.focus1.y); + pvec.push_back(e.radmin); + origpvec = pvec; + rescale(); +} + +ConstraintPointOnHyperbola::ConstraintPointOnHyperbola(Point& p, ArcOfHyperbola& e) +{ + pvec.push_back(p.x); + pvec.push_back(p.y); + pvec.push_back(e.center.x); + pvec.push_back(e.center.y); + pvec.push_back(e.focus1.x); + pvec.push_back(e.focus1.y); + pvec.push_back(e.radmin); + origpvec = pvec; + rescale(); +} + +ConstraintType ConstraintPointOnHyperbola::getTypeId() +{ + return PointOnHyperbola; +} + +void ConstraintPointOnHyperbola::rescale(double coef) +{ + scale = coef * 1; +} + +double ConstraintPointOnHyperbola::error() +{ + double X_0 = *p1x(); + double Y_0 = *p1y(); + double X_c = *cx(); + double Y_c = *cy(); + double X_F1 = *f1x(); + double Y_F1 = *f1y(); + double b = *rmin(); + + // Full sage worksheet at: + // https://forum.freecad.org/viewtopic.php?f=10&t=8038&p=110447#p110447 + // + // Err = |PF2| - |PF1| - 2*a + // sage code: + // C = vector([X_c,Y_c]) + // F2 = C+(C-F1) + // X_F2 = F2[0] + // Y_F2 = F2[1] + // a = sqrt((F1-C)*(F1-C)-b*b); + // show(a) + // DM=sqrt((P-F2)*(P-F2))-sqrt((P-F1)*(P-F1))-2*a + // show(DM.simplify_radical()) + double err = -sqrt(pow(X_0 - X_F1, 2) + pow(Y_0 - Y_F1, 2)) + + sqrt(pow(X_0 + X_F1 - 2 * X_c, 2) + pow(Y_0 + Y_F1 - 2 * Y_c, 2)) + - 2 * sqrt(-pow(b, 2) + pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2)); + return scale * err; +} + +double ConstraintPointOnHyperbola::grad(double* param) +{ + double deriv = 0.; + if (param == p1x() || param == p1y() || param == f1x() || param == f1y() || param == cx() + || param == cy() || param == rmin()) { + + double X_0 = *p1x(); + double Y_0 = *p1y(); + double X_c = *cx(); + double Y_c = *cy(); + double X_F1 = *f1x(); + double Y_F1 = *f1y(); + double b = *rmin(); + + if (param == p1x()) { + deriv += -(X_0 - X_F1) / sqrt(pow(X_0 - X_F1, 2) + pow(Y_0 - Y_F1, 2)) + + (X_0 + X_F1 - 2 * X_c) + / sqrt(pow(X_0 + X_F1 - 2 * X_c, 2) + pow(Y_0 + Y_F1 - 2 * Y_c, 2)); + } + if (param == p1y()) { + deriv += -(Y_0 - Y_F1) / sqrt(pow(X_0 - X_F1, 2) + pow(Y_0 - Y_F1, 2)) + + (Y_0 + Y_F1 - 2 * Y_c) + / sqrt(pow(X_0 + X_F1 - 2 * X_c, 2) + pow(Y_0 + Y_F1 - 2 * Y_c, 2)); + } + if (param == f1x()) { + deriv += (X_0 - X_F1) / sqrt(pow(X_0 - X_F1, 2) + pow(Y_0 - Y_F1, 2)) + - 2 * (X_F1 - X_c) / sqrt(-pow(b, 2) + pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2)) + + (X_0 + X_F1 - 2 * X_c) + / sqrt(pow(X_0 + X_F1 - 2 * X_c, 2) + pow(Y_0 + Y_F1 - 2 * Y_c, 2)); + } + if (param == f1y()) { + deriv += (Y_0 - Y_F1) / sqrt(pow(X_0 - X_F1, 2) + pow(Y_0 - Y_F1, 2)) + - 2 * (Y_F1 - Y_c) / sqrt(-pow(b, 2) + pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2)) + + (Y_0 + Y_F1 - 2 * Y_c) + / sqrt(pow(X_0 + X_F1 - 2 * X_c, 2) + pow(Y_0 + Y_F1 - 2 * Y_c, 2)); + } + if (param == cx()) { + deriv += 2 * (X_F1 - X_c) / sqrt(-pow(b, 2) + pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2)) + - 2 * (X_0 + X_F1 - 2 * X_c) + / sqrt(pow(X_0 + X_F1 - 2 * X_c, 2) + pow(Y_0 + Y_F1 - 2 * Y_c, 2)); + } + if (param == cy()) { + deriv += 2 * (Y_F1 - Y_c) / sqrt(-pow(b, 2) + pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2)) + - 2 * (Y_0 + Y_F1 - 2 * Y_c) + / sqrt(pow(X_0 + X_F1 - 2 * X_c, 2) + pow(Y_0 + Y_F1 - 2 * Y_c, 2)); + } + if (param == rmin()) { + deriv += 2 * b / sqrt(-pow(b, 2) + pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2)); + } + } + return scale * deriv; +} + + +// -------------------------------------------------------- +// ConstraintPointOnParabola +ConstraintPointOnParabola::ConstraintPointOnParabola(Point& p, Parabola& e) +{ + pvec.push_back(p.x); + pvec.push_back(p.y); + e.PushOwnParams(pvec); + this->parab = e.Copy(); + pvecChangedFlag = true; + origpvec = pvec; + rescale(); +} + +ConstraintPointOnParabola::ConstraintPointOnParabola(Point& p, ArcOfParabola& e) +{ + pvec.push_back(p.x); + pvec.push_back(p.y); + e.PushOwnParams(pvec); + this->parab = e.Copy(); + pvecChangedFlag = true; + origpvec = pvec; + rescale(); +} + +ConstraintPointOnParabola::~ConstraintPointOnParabola() +{ + delete this->parab; + this->parab = nullptr; +} + +void ConstraintPointOnParabola::ReconstructGeomPointers() +{ + int i = 0; + p.x = pvec[i]; + i++; + p.y = pvec[i]; + i++; + this->parab->ReconstructOnNewPvec(pvec, i); + pvecChangedFlag = false; +} + +ConstraintType ConstraintPointOnParabola::getTypeId() +{ + return PointOnParabola; +} + +void ConstraintPointOnParabola::rescale(double coef) +{ + scale = coef * 1; +} + +void ConstraintPointOnParabola::errorgrad(double* err, double* grad, double* param) +{ + if (pvecChangedFlag) { + ReconstructGeomPointers(); + } + + DeriVector2 focus(this->parab->focus1, param); + DeriVector2 vertex(this->parab->vertex, param); + + DeriVector2 point(this->p, param); // point to be constrained to parabola + + DeriVector2 focalvect = focus.subtr(vertex); + + DeriVector2 xdir = focalvect.getNormalized(); + + DeriVector2 point_to_focus = point.subtr(focus); + + double focal, dfocal; + + focal = focalvect.length(dfocal); + + double pf, dpf; + + pf = point_to_focus.length(dpf); + + double proj, dproj; + + proj = point_to_focus.scalarProd(xdir, &dproj); + + if (err) { + *err = pf - 2 * focal - proj; + } + if (grad) { + *grad = dpf - 2 * dfocal - dproj; + } +} + +double ConstraintPointOnParabola::error() +{ + double err; + errorgrad(&err, nullptr, nullptr); + return scale * err; +} + +double ConstraintPointOnParabola::grad(double* param) +{ + // first of all, check that we need to compute anything. + if (findParamInPvec(param) == -1) { + return 0.0; + } + + double deriv; + errorgrad(nullptr, &deriv, param); + + return deriv * scale; +} + + +// -------------------------------------------------------- +// ConstraintAngleViaPoint +ConstraintAngleViaPoint::ConstraintAngleViaPoint(Curve& acrv1, Curve& acrv2, Point p, double* angle) +{ + pvec.push_back(angle); + pvec.push_back(p.x); + pvec.push_back(p.y); + acrv1.PushOwnParams(pvec); + acrv2.PushOwnParams(pvec); + crv1 = acrv1.Copy(); + crv2 = acrv2.Copy(); + origpvec = pvec; + pvecChangedFlag = true; + rescale(); +} + +ConstraintAngleViaPoint::~ConstraintAngleViaPoint() +{ + delete crv1; + crv1 = nullptr; + delete crv2; + crv2 = nullptr; +} + +void ConstraintAngleViaPoint::ReconstructGeomPointers() +{ + int cnt = 0; + cnt++; // skip angle - we have an inline function for that + poa.x = pvec[cnt]; + cnt++; + poa.y = pvec[cnt]; + cnt++; + crv1->ReconstructOnNewPvec(pvec, cnt); + crv2->ReconstructOnNewPvec(pvec, cnt); + pvecChangedFlag = false; +} + +ConstraintType ConstraintAngleViaPoint::getTypeId() +{ + return AngleViaPoint; +} + +void ConstraintAngleViaPoint::rescale(double coef) +{ + scale = coef * 1.; +} + +double ConstraintAngleViaPoint::error() +{ + if (pvecChangedFlag) { + ReconstructGeomPointers(); + } + double ang = *angle(); + DeriVector2 n1 = crv1->CalculateNormal(poa); + DeriVector2 n2 = crv2->CalculateNormal(poa); + + // rotate n1 by angle + DeriVector2 n1r(n1.x * cos(ang) - n1.y * sin(ang), n1.x * sin(ang) + n1.y * cos(ang)); + + // calculate angle between n1r and n2. Since we have rotated the n1, the angle is the error + // function. for our atan2, y is a dot product (n2) * (n1r rotated ccw by 90 degrees). + // x is a dot product (n2) * (n1r) + double err = atan2(-n2.x * n1r.y + n2.y * n1r.x, n2.x * n1r.x + n2.y * n1r.y); + // essentially, the function is equivalent to atan2(n2)-(atan2(n1)+angle). The only difference + // is behavior when normals are zero (the intended result is also zero in this case). + return scale * err; +} + +double ConstraintAngleViaPoint::grad(double* param) +{ + // first of all, check that we need to compute anything. + if (findParamInPvec(param) == -1) { + return 0.0; + } + + double deriv = 0.; + + if (pvecChangedFlag) { + ReconstructGeomPointers(); + } + + if (param == angle()) { + deriv += -1.0; + } + DeriVector2 n1 = crv1->CalculateNormal(poa, param); + DeriVector2 n2 = crv2->CalculateNormal(poa, param); + deriv -= ((-n1.dx) * n1.y / pow(n1.length(), 2) + n1.dy * n1.x / pow(n1.length(), 2)); + deriv += ((-n2.dx) * n2.y / pow(n2.length(), 2) + n2.dy * n2.x / pow(n2.length(), 2)); + + return scale * deriv; +} + +// -------------------------------------------------------- +// ConstraintAngleViaTwoPoints +ConstraintAngleViaTwoPoints::ConstraintAngleViaTwoPoints(Curve& acrv1, + Curve& acrv2, + Point p1, + Point p2, + double* angle) +{ + pvec.push_back(angle); + pvec.push_back(p1.x); + pvec.push_back(p1.y); + pvec.push_back(p2.x); + pvec.push_back(p2.y); + acrv1.PushOwnParams(pvec); + acrv2.PushOwnParams(pvec); + crv1 = acrv1.Copy(); + crv2 = acrv2.Copy(); + origpvec = pvec; + pvecChangedFlag = true; + rescale(); +} + +ConstraintAngleViaTwoPoints::~ConstraintAngleViaTwoPoints() +{ + delete crv1; + crv1 = nullptr; + delete crv2; + crv2 = nullptr; +} + +void ConstraintAngleViaTwoPoints::ReconstructGeomPointers() +{ + int cnt = 0; + cnt++; // skip angle - we have an inline function for that + poa1.x = pvec[cnt]; + cnt++; + poa1.y = pvec[cnt]; + cnt++; + poa2.x = pvec[cnt]; + cnt++; + poa2.y = pvec[cnt]; + cnt++; + crv1->ReconstructOnNewPvec(pvec, cnt); + crv2->ReconstructOnNewPvec(pvec, cnt); + pvecChangedFlag = false; +} + +ConstraintType ConstraintAngleViaTwoPoints::getTypeId() +{ + return AngleViaTwoPoints; +} + +void ConstraintAngleViaTwoPoints::rescale(double coef) +{ + scale = coef * 1.; +} + +double ConstraintAngleViaTwoPoints::error() +{ + if (pvecChangedFlag) { + ReconstructGeomPointers(); + } + double ang = *angle(); + DeriVector2 n1 = crv1->CalculateNormal(poa1); + DeriVector2 n2 = crv2->CalculateNormal(poa2); + + // rotate n1 by angle + DeriVector2 n1r(n1.x * cos(ang) - n1.y * sin(ang), n1.x * sin(ang) + n1.y * cos(ang)); + + // calculate angle between n1r and n2. Since we have rotated the n1, the angle is the error + // function. for our atan2, y is a dot product (n2) * (n1r rotated ccw by 90 degrees). + // x is a dot product (n2) * (n1r) + double err = atan2(-n2.x * n1r.y + n2.y * n1r.x, n2.x * n1r.x + n2.y * n1r.y); + // essentially, the function is equivalent to atan2(n2)-(atan2(n1)+angle). The only difference + // is behavior when normals are zero (the intended result is also zero in this case). + return scale * err; +} + +double ConstraintAngleViaTwoPoints::grad(double* param) +{ + // first of all, check that we need to compute anything. + if (findParamInPvec(param) == -1) { + return 0.0; + } + + double deriv = 0.; + + if (pvecChangedFlag) { + ReconstructGeomPointers(); + } + + if (param == angle()) { + deriv += -1.0; + } + DeriVector2 n1 = crv1->CalculateNormal(poa1, param); + DeriVector2 n2 = crv2->CalculateNormal(poa2, param); + deriv -= ((-n1.dx) * n1.y / pow(n1.length(), 2) + n1.dy * n1.x / pow(n1.length(), 2)); + deriv += ((-n2.dx) * n2.y / pow(n2.length(), 2) + n2.dy * n2.x / pow(n2.length(), 2)); + + return scale * deriv; +} + +// -------------------------------------------------------- +// ConstraintAngleViaPointAndParam +ConstraintAngleViaPointAndParam::ConstraintAngleViaPointAndParam(Curve& acrv1, + Curve& acrv2, + Point p, + double* cparam, + double* angle) +{ + pvec.push_back(angle); + pvec.push_back(p.x); + pvec.push_back(p.y); + pvec.push_back(cparam); + acrv1.PushOwnParams(pvec); + acrv2.PushOwnParams(pvec); + crv1 = acrv1.Copy(); + crv2 = acrv2.Copy(); + origpvec = pvec; + pvecChangedFlag = true; + rescale(); +} + +ConstraintAngleViaPointAndParam::~ConstraintAngleViaPointAndParam() +{ + delete crv1; + crv1 = nullptr; + delete crv2; + crv2 = nullptr; +} + +void ConstraintAngleViaPointAndParam::ReconstructGeomPointers() +{ + int cnt = 0; + cnt++; // skip angle - we have an inline function for that + poa.x = pvec[cnt]; + cnt++; + poa.y = pvec[cnt]; + cnt++; + cnt++; // skip cparam + crv1->ReconstructOnNewPvec(pvec, cnt); + crv2->ReconstructOnNewPvec(pvec, cnt); + pvecChangedFlag = false; +} + +ConstraintType ConstraintAngleViaPointAndParam::getTypeId() +{ + return AngleViaPointAndParam; +} + +void ConstraintAngleViaPointAndParam::rescale(double coef) +{ + scale = coef * 1.; +} + +double ConstraintAngleViaPointAndParam::error() +{ + if (pvecChangedFlag) { + ReconstructGeomPointers(); + } + double ang = *angle(); + DeriVector2 n1 = crv1->CalculateNormal(cparam()); + DeriVector2 n2 = crv2->CalculateNormal(poa); + + // rotate n1 by angle + DeriVector2 n1r(n1.x * cos(ang) - n1.y * sin(ang), n1.x * sin(ang) + n1.y * cos(ang)); + + // calculate angle between n1r and n2. Since we have rotated the n1, the angle is the error + // function. for our atan2, y is a dot product (n2) * (n1r rotated ccw by 90 degrees). + // x is a dot product (n2) * (n1r) + double err = atan2(-n2.x * n1r.y + n2.y * n1r.x, n2.x * n1r.x + n2.y * n1r.y); + // essentially, the function is equivalent to atan2(n2)-(atan2(n1)+angle). The only difference + // is behavior when normals are zero (the intended result is also zero in this case). + return scale * err; +} + +double ConstraintAngleViaPointAndParam::grad(double* param) +{ + // first of all, check that we need to compute anything. + if (findParamInPvec(param) == -1) { + return 0.0; + } + + double deriv = 0.; + + if (pvecChangedFlag) { + ReconstructGeomPointers(); + } + + if (param == angle()) { + deriv += -1.0; + } + DeriVector2 n1 = crv1->CalculateNormal(cparam(), param); + DeriVector2 n2 = crv2->CalculateNormal(poa, param); + deriv -= ((-n1.dx) * n1.y / pow(n1.length(), 2) + n1.dy * n1.x / pow(n1.length(), 2)); + deriv += ((-n2.dx) * n2.y / pow(n2.length(), 2) + n2.dy * n2.x / pow(n2.length(), 2)); + + return scale * deriv; +} + +// -------------------------------------------------------- +// ConstraintAngleViaPointAndTwoParams +ConstraintAngleViaPointAndTwoParams::ConstraintAngleViaPointAndTwoParams(Curve& acrv1, + Curve& acrv2, + Point p, + double* cparam1, + double* cparam2, + double* angle) +{ + pvec.push_back(angle); + pvec.push_back(p.x); + pvec.push_back(p.y); + pvec.push_back(cparam1); + pvec.push_back(cparam2); + acrv1.PushOwnParams(pvec); + acrv2.PushOwnParams(pvec); + crv1 = acrv1.Copy(); + crv2 = acrv2.Copy(); + origpvec = pvec; + pvecChangedFlag = true; + rescale(); +} + +ConstraintAngleViaPointAndTwoParams::~ConstraintAngleViaPointAndTwoParams() +{ + delete crv1; + crv1 = nullptr; + delete crv2; + crv2 = nullptr; +} + +void ConstraintAngleViaPointAndTwoParams::ReconstructGeomPointers() +{ + int cnt = 0; + cnt++; // skip angle - we have an inline function for that + poa.x = pvec[cnt]; + cnt++; + poa.y = pvec[cnt]; + cnt++; + cnt++; // skip cparam1 - we have an inline function for that + cnt++; // skip cparam2 - we have an inline function for that + crv1->ReconstructOnNewPvec(pvec, cnt); + crv2->ReconstructOnNewPvec(pvec, cnt); + pvecChangedFlag = false; +} + +ConstraintType ConstraintAngleViaPointAndTwoParams::getTypeId() +{ + return AngleViaPointAndTwoParams; +} + +void ConstraintAngleViaPointAndTwoParams::rescale(double coef) +{ + scale = coef * 1.; +} + +double ConstraintAngleViaPointAndTwoParams::error() +{ + if (pvecChangedFlag) { + ReconstructGeomPointers(); + } + double ang = *angle(); + DeriVector2 n1 = crv1->CalculateNormal(cparam1()); + DeriVector2 n2 = crv2->CalculateNormal(cparam2()); + + // rotate n1 by angle + DeriVector2 n1r(n1.x * cos(ang) - n1.y * sin(ang), n1.x * sin(ang) + n1.y * cos(ang)); + + // calculate angle between n1r and n2. Since we have rotated the n1, the angle is the error + // function. for our atan2, y is a dot product (n2) * (n1r rotated ccw by 90 degrees). + // x is a dot product (n2) * (n1r) + double err = atan2(-n2.x * n1r.y + n2.y * n1r.x, n2.x * n1r.x + n2.y * n1r.y); + // essentially, the function is equivalent to atan2(n2)-(atan2(n1)+angle). The only difference + // is behavior when normals are zero (the intended result is also zero in this case). + return scale * err; +} + +double ConstraintAngleViaPointAndTwoParams::grad(double* param) +{ + // first of all, check that we need to compute anything. + if (findParamInPvec(param) == -1) { + return 0.0; + } + + double deriv = 0.; + + if (pvecChangedFlag) { + ReconstructGeomPointers(); + } + + if (param == angle()) { + deriv += -1.0; + } + DeriVector2 n1 = crv1->CalculateNormal(cparam1(), param); + DeriVector2 n2 = crv2->CalculateNormal(cparam2(), param); + deriv -= ((-n1.dx) * n1.y / pow(n1.length(), 2) + n1.dy * n1.x / pow(n1.length(), 2)); + deriv += ((-n2.dx) * n2.y / pow(n2.length(), 2) + n2.dy * n2.x / pow(n2.length(), 2)); + + return scale * deriv; +} + + +// -------------------------------------------------------- +// ConstraintSnell +ConstraintSnell::ConstraintSnell(Curve& ray1, + Curve& ray2, + Curve& boundary, + Point p, + double* n1, + double* n2, + bool flipn1, + bool flipn2) +{ + pvec.push_back(n1); + pvec.push_back(n2); + pvec.push_back(p.x); + pvec.push_back(p.y); + ray1.PushOwnParams(pvec); + ray2.PushOwnParams(pvec); + boundary.PushOwnParams(pvec); + this->ray1 = ray1.Copy(); + this->ray2 = ray2.Copy(); + this->boundary = boundary.Copy(); + origpvec = pvec; + pvecChangedFlag = true; + + this->flipn1 = flipn1; + this->flipn2 = flipn2; + + rescale(); +} + +ConstraintSnell::~ConstraintSnell() +{ + delete ray1; + ray1 = nullptr; + delete ray2; + ray2 = nullptr; + delete boundary; + boundary = nullptr; +} + +void ConstraintSnell::ReconstructGeomPointers() +{ + int cnt = 0; + cnt++; + cnt++; // skip n1, n2 - we have an inline function for that + poa.x = pvec[cnt]; + cnt++; + poa.y = pvec[cnt]; + cnt++; + ray1->ReconstructOnNewPvec(pvec, cnt); + ray2->ReconstructOnNewPvec(pvec, cnt); + boundary->ReconstructOnNewPvec(pvec, cnt); + pvecChangedFlag = false; +} + +ConstraintType ConstraintSnell::getTypeId() +{ + return Snell; +} + +void ConstraintSnell::rescale(double coef) +{ + scale = coef * 1.; +} + +// error and gradient combined. Values are returned through pointers. +void ConstraintSnell::errorgrad(double* err, double* grad, double* param) +{ + if (pvecChangedFlag) { + ReconstructGeomPointers(); + } + DeriVector2 tang1 = ray1->CalculateNormal(poa, param).rotate90cw().getNormalized(); + DeriVector2 tang2 = ray2->CalculateNormal(poa, param).rotate90cw().getNormalized(); + DeriVector2 tangB = boundary->CalculateNormal(poa, param).rotate90cw().getNormalized(); + double sin1, dsin1, sin2, dsin2; + sin1 = tang1.scalarProd(tangB, &dsin1); // sinus of angle of incidence + sin2 = tang2.scalarProd(tangB, &dsin2); + if (flipn1) { + sin1 = -sin1; + dsin1 = -dsin1; + } + if (flipn2) { + sin2 = -sin2; + dsin2 = -dsin2; + } + + double dn1 = (param == n1()) ? 1.0 : 0.0; + double dn2 = (param == n2()) ? 1.0 : 0.0; + if (err) { + *err = *n1() * sin1 - *n2() * sin2; + } + if (grad) { + *grad = dn1 * sin1 + *n1() * dsin1 - dn2 * sin2 - *n2() * dsin2; + } +} + +double ConstraintSnell::error() +{ + double err; + errorgrad(&err, nullptr, nullptr); + return scale * err; +} + +double ConstraintSnell::grad(double* param) +{ + // first of all, check that we need to compute anything. + if (findParamInPvec(param) == -1) { + return 0.0; + } + + double deriv; + errorgrad(nullptr, &deriv, param); + + return scale * deriv; +} + + +// -------------------------------------------------------- +// ConstraintEqualLineLength +ConstraintEqualLineLength::ConstraintEqualLineLength(Line& l1, Line& l2) +{ + this->l1 = l1; + this->l1.PushOwnParams(pvec); + + this->l2 = l2; + this->l2.PushOwnParams(pvec); + origpvec = pvec; + pvecChangedFlag = true; + rescale(); +} + +void ConstraintEqualLineLength::ReconstructGeomPointers() +{ + int i = 0; + l1.ReconstructOnNewPvec(pvec, i); + l2.ReconstructOnNewPvec(pvec, i); + pvecChangedFlag = false; +} + +ConstraintType ConstraintEqualLineLength::getTypeId() +{ + return EqualLineLength; +} + +void ConstraintEqualLineLength::rescale(double coef) +{ + scale = coef * 1; +} + +void ConstraintEqualLineLength::errorgrad(double* err, double* grad, double* param) +{ + if (pvecChangedFlag) { + ReconstructGeomPointers(); + } + + DeriVector2 p1(l1.p1, param); + DeriVector2 p2(l1.p2, param); + DeriVector2 p3(l2.p1, param); + DeriVector2 p4(l2.p2, param); + + DeriVector2 v1 = p1.subtr(p2); + DeriVector2 v2 = p3.subtr(p4); + + double length1, dlength1; + length1 = v1.length(dlength1); + + double length2, dlength2; + length2 = v2.length(dlength2); + + if (err) { + *err = length2 - length1; + } + + if (grad) { + *grad = dlength2 - dlength1; + // if the one of the lines gets vertical or horizontal, the gradients will become zero. this + // will affect the diagnose function and the detection of dependent/independent parameters. + // + // So here we maintain the very small derivative of 1e-10 when the gradient is under such + // value, such that the diagnose function with pivot threshold of 1e-13 treats the value as + // non-zero and correctly detects and can tell apart when a parameter is fully constrained + // or just locked into a maximum/minimum + if (fabs(*grad) < 1e-10) { + double surrogate = 1e-10; + if (param == l1.p1.x) { + *grad = v1.x > 0 ? surrogate : -surrogate; + } + if (param == l1.p1.y) { + *grad = v1.y > 0 ? surrogate : -surrogate; + } + if (param == l1.p2.x) { + *grad = v1.x > 0 ? -surrogate : surrogate; + } + if (param == l1.p2.y) { + *grad = v1.y > 0 ? -surrogate : surrogate; + } + if (param == l2.p1.x) { + *grad = v2.x > 0 ? surrogate : -surrogate; + } + if (param == l2.p1.y) { + *grad = v2.y > 0 ? surrogate : -surrogate; + } + if (param == l2.p2.x) { + *grad = v2.x > 0 ? -surrogate : surrogate; + } + if (param == l2.p2.y) { + *grad = v2.y > 0 ? -surrogate : surrogate; + } + } + } +} + +double ConstraintEqualLineLength::error() +{ + double err; + errorgrad(&err, nullptr, nullptr); + return scale * err; +} + +double ConstraintEqualLineLength::grad(double* param) +{ + if (findParamInPvec(param) == -1) { + return 0.0; + } + + double deriv; + errorgrad(nullptr, &deriv, param); + + return deriv * scale; +} + + +// -------------------------------------------------------- +// ConstraintC2CDistance +ConstraintC2CDistance::ConstraintC2CDistance(Circle& c1, Circle& c2, double* d) +{ + this->d = d; + pvec.push_back(d); + + this->c1 = c1; + this->c1.PushOwnParams(pvec); + + this->c2 = c2; + this->c2.PushOwnParams(pvec); + + origpvec = pvec; + pvecChangedFlag = true; + rescale(); +} + +void ConstraintC2CDistance::ReconstructGeomPointers() +{ + int i = 0; + i++; // skip the first parameter as there is the inline function distance for it + c1.ReconstructOnNewPvec(pvec, i); + c2.ReconstructOnNewPvec(pvec, i); + pvecChangedFlag = false; +} + +ConstraintType ConstraintC2CDistance::getTypeId() +{ + return C2CDistance; +} + +void ConstraintC2CDistance::rescale(double coef) +{ + scale = coef * 1; +} + +void ConstraintC2CDistance::errorgrad(double* err, double* grad, double* param) +{ + if (pvecChangedFlag) { + ReconstructGeomPointers(); + } + + DeriVector2 ct1(c1.center, param); + DeriVector2 ct2(c2.center, param); + + DeriVector2 vector_ct12 = ct1.subtr(ct2); + + double length_ct12, dlength_ct12; + length_ct12 = vector_ct12.length(dlength_ct12); + + // outer case (defined as the centers of the circles are outside the center of the other + // circles) it may well be that the circles intersect. + if (length_ct12 >= *c1.rad && length_ct12 >= *c2.rad) { + if (err) { + *err = length_ct12 - (*c2.rad + *c1.rad + *distance()); + } + else if (grad) { + double drad = (param == c2.rad || param == c1.rad || param == distance()) ? -1.0 : 0.0; + *grad = dlength_ct12 + drad; + } + } + else { + double* bigradius = (*c1.rad >= *c2.rad) ? c1.rad : c2.rad; + double* smallradius = (*c1.rad >= *c2.rad) ? c2.rad : c1.rad; + + double smallspan = *smallradius + length_ct12 + *distance(); + + if (err) { + *err = *bigradius - smallspan; + } + else if (grad) { + double drad = 0.0; + + if (param == bigradius) { + drad = 1.0; + } + else if (param == smallradius) { + drad = -1.0; + } + else if (param == distance()) { + drad = (*distance() < 0.) ? 1.0 : -1.0; + } + if (length_ct12 > 1e-13) { + *grad = -dlength_ct12 + drad; + } + else { // concentric case + *grad = drad; + } + } + } +} + +double ConstraintC2CDistance::error() +{ + double err; + errorgrad(&err, nullptr, nullptr); + return scale * err; +} + +double ConstraintC2CDistance::grad(double* param) +{ + if (findParamInPvec(param) == -1) { + return 0.0; + } + + double deriv; + errorgrad(nullptr, &deriv, param); + + return deriv * scale; +} + +// -------------------------------------------------------- +// ConstraintC2LDistance +ConstraintC2LDistance::ConstraintC2LDistance(Circle& c, Line& l, double* d) +{ + this->d = d; + pvec.push_back(d); + + this->circle = c; + this->circle.PushOwnParams(pvec); + + this->line = l; + this->line.PushOwnParams(pvec); + + origpvec = pvec; + pvecChangedFlag = true; + rescale(); +} + +ConstraintType ConstraintC2LDistance::getTypeId() +{ + return C2LDistance; +} + +void ConstraintC2LDistance::rescale(double coef) +{ + scale = coef; +} + +void ConstraintC2LDistance::ReconstructGeomPointers() +{ + int i = 0; + i++; // skip the first parameter as there is the inline function distance for it + circle.ReconstructOnNewPvec(pvec, i); + line.ReconstructOnNewPvec(pvec, i); + pvecChangedFlag = false; +} + +void ConstraintC2LDistance::errorgrad(double* err, double* grad, double* param) +{ + if (pvecChangedFlag) { + ReconstructGeomPointers(); + } + + DeriVector2 ct(circle.center, param); + DeriVector2 p1(line.p1, param); + DeriVector2 p2(line.p2, param); + DeriVector2 v_line = p2.subtr(p1); + DeriVector2 v_p1ct = ct.subtr(p1); + + // center to line distance (=h) and its derivative (=dh) + double darea = 0.0; + double area = v_line.crossProdNorm(v_p1ct, darea); // parallelogram oriented area + + double dlength; + double length = v_line.length(dlength); + + // vector product (cross vector) has a magnitude corresponding to the area of + // the parallelogram defined by the vectors above. The area of the triangle is + // half the parallelogram area. The height of the triangle is the area divided by + // the base, which is the distance from the center of the circle to the line. + // + // However, the vector (which points in z direction), can be positive or negative. + // the area is the absolute value + double h = std::abs(area) / length; + + // darea is the magnitude of a vector in the z direction, which makes the area vector + // increase or decrease. If area vector is negative a negative value makes the area increase + // and a positive value makes it decrease. + darea = std::signbit(area) ? -darea : darea; + + double dh = (darea - h * dlength) / length; + + if (err) { + *err = *distance() + *circle.rad - h; + } + else if (grad) { + if (param == distance() || param == circle.rad) { + *grad = 1.0; + } + else { + *grad = -dh; + } + } +} + +double ConstraintC2LDistance::error() +{ + double err; + errorgrad(&err, nullptr, nullptr); + return scale * err; +} + +double ConstraintC2LDistance::grad(double* param) +{ + if (findParamInPvec(param) == -1) { + return 0.0; + } + + double deriv; + errorgrad(nullptr, &deriv, param); + + return deriv * scale; +} + +// -------------------------------------------------------- +// ConstraintP2CDistance +ConstraintP2CDistance::ConstraintP2CDistance(Point& p, Circle& c, double* d) +{ + this->d = d; + pvec.push_back(d); + + this->circle = c; + this->circle.PushOwnParams(pvec); + + this->pt = p; + this->pt.PushOwnParams(pvec); + + origpvec = pvec; + pvecChangedFlag = true; + rescale(); +} + +ConstraintType ConstraintP2CDistance::getTypeId() +{ + return P2CDistance; +} + +void ConstraintP2CDistance::rescale(double coef) +{ + scale = coef; +} + +void ConstraintP2CDistance::ReconstructGeomPointers() +{ + int i = 0; + i++; // skip the first parameter as there is the inline function distance for it + circle.ReconstructOnNewPvec(pvec, i); + pt.ReconstructOnNewPvec(pvec, i); + pvecChangedFlag = false; +} + +void ConstraintP2CDistance::errorgrad(double* err, double* grad, double* param) +{ + if (pvecChangedFlag) { + ReconstructGeomPointers(); + } + + DeriVector2 ct(circle.center, param); + DeriVector2 p(pt, param); + DeriVector2 v_length = ct.subtr(p); + + double dlength; + double length = v_length.length(dlength); + + if (err) { + *err = *circle.rad + *distance() - length; + if (length < *circle.rad) { + *err = *circle.rad - *distance() - length; + } + } + else if (grad) { + if (param == distance()) { + *grad = 1.0; + if (length < *circle.rad) { + *grad = -1.0; + } + } + else if (param == circle.rad) { + *grad = 1.0; + } + else { + *grad = -dlength; + } + } +} + +double ConstraintP2CDistance::error() +{ + double err; + errorgrad(&err, nullptr, nullptr); + return scale * err; +} + +double ConstraintP2CDistance::grad(double* param) +{ + if (findParamInPvec(param) == -1) { + return 0.0; + } + + double deriv; + errorgrad(nullptr, &deriv, param); + + return deriv * scale; +} + +// -------------------------------------------------------- +// ConstraintArcLength +ConstraintArcLength::ConstraintArcLength(Arc& a, double* d) +{ + this->d = d; + pvec.push_back(d); + + this->arc = a; + this->arc.PushOwnParams(pvec); + + origpvec = pvec; + pvecChangedFlag = true; + rescale(); +} + +void ConstraintArcLength::ReconstructGeomPointers() +{ + int i = 0; + i++; // skip the first parameter as there is the inline function distance for it + arc.ReconstructOnNewPvec(pvec, i); + pvecChangedFlag = false; +} + +ConstraintType ConstraintArcLength::getTypeId() +{ + return ArcLength; +} + +void ConstraintArcLength::rescale(double coef) +{ + scale = coef; +} + +void ConstraintArcLength::errorgrad(double* err, double* grad, double* param) +{ + if (pvecChangedFlag) { + ReconstructGeomPointers(); + } + + double rad = *arc.rad; + double endA = *arc.endAngle; + double startA = *arc.startAngle; + // Assume positive angles and CCW arc + while (startA < 0.) { + startA += 2. * std::numbers::pi; + } + while (endA < startA) { + endA += 2. * std::numbers::pi; + } + if (err) { + *err = rad * (endA - startA) - *distance(); + } + else if (grad) { + if (param == distance()) { + // if constraint is not driving it varies on distance(). + *grad = -1.; + } + else { + double dRad = param == arc.rad ? 1. : 0.; + double dStartA = param == arc.startAngle ? 1. : 0.; + double dEndA = param == arc.endAngle ? 1. : 0.; + *grad = rad * (dEndA - dStartA) + dRad * (endA - startA); + } + } +} + +double ConstraintArcLength::error() +{ + double err; + errorgrad(&err, nullptr, nullptr); + return scale * err; +} + +double ConstraintArcLength::grad(double* param) +{ + if (findParamInPvec(param) == -1) { + return 0.0; + } + + double deriv; + errorgrad(nullptr, &deriv, param); + + return deriv * scale; +} + + +} // namespace GCS diff --git a/GCS/Constraints.h b/GCS/Constraints.h new file mode 100644 index 0000000..c7dc8c9 --- /dev/null +++ b/GCS/Constraints.h @@ -0,0 +1,1413 @@ +/*************************************************************************** + * Copyright (c) 2011 Konstantinos Poulios * + * * + * 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 * + * * + ***************************************************************************/ + +#ifndef PLANEGCS_CONSTRAINTS_H +#define PLANEGCS_CONSTRAINTS_H + +#include "Geo.h" + +// This enables debugging code intended to extract information to file bug reports against Eigen, +// not for production code +// #define _GCS_EXTRACT_SOLVER_SUBSYSTEM_ + +#ifdef _GCS_EXTRACT_SOLVER_SUBSYSTEM_ +#define _PROTECTED_UNLESS_EXTRACT_MODE_ public +#else +#define _PROTECTED_UNLESS_EXTRACT_MODE_ protected +#endif + + +namespace GCS +{ + +/////////////////////////////////////// +// Constraints +/////////////////////////////////////// + +enum ConstraintType +{ + None = 0, + Equal = 1, + Difference = 2, + P2PDistance = 3, + P2PAngle = 4, + P2LDistance = 5, + PointOnLine = 6, + PointOnPerpBisector = 7, + Parallel = 8, + Perpendicular = 9, + L2LAngle = 10, + MidpointOnLine = 11, + TangentCircumf = 12, + PointOnEllipse = 13, + TangentEllipseLine = 14, + InternalAlignmentPoint2Ellipse = 15, + EqualMajorAxesConic = 16, + EllipticalArcRangeToEndPoints = 17, + AngleViaPoint = 18, + Snell = 19, + CurveValue = 20, + PointOnHyperbola = 21, + InternalAlignmentPoint2Hyperbola = 22, + PointOnParabola = 23, + EqualFocalDistance = 24, + EqualLineLength = 25, + CenterOfGravity = 26, + WeightedLinearCombination = 27, + SlopeAtBSplineKnot = 28, + PointOnBSpline = 29, + C2CDistance = 30, + C2LDistance = 31, + P2CDistance = 32, + AngleViaPointAndParam = 33, + AngleViaPointAndTwoParams = 34, + AngleViaTwoPoints = 35, + ArcLength = 36, +}; + +enum InternalAlignmentType +{ + EllipsePositiveMajorX = 0, + EllipsePositiveMajorY = 1, + EllipseNegativeMajorX = 2, + EllipseNegativeMajorY = 3, + EllipsePositiveMinorX = 4, + EllipsePositiveMinorY = 5, + EllipseNegativeMinorX = 6, + EllipseNegativeMinorY = 7, + EllipseFocus2X = 8, + EllipseFocus2Y = 9, + HyperbolaPositiveMajorX = 10, + HyperbolaPositiveMajorY = 11, + HyperbolaNegativeMajorX = 12, + HyperbolaNegativeMajorY = 13, + HyperbolaPositiveMinorX = 14, + HyperbolaPositiveMinorY = 15, + HyperbolaNegativeMinorX = 16, + HyperbolaNegativeMinorY = 17 +}; + +class Constraint +{ + +public: + enum class Alignment + { + NoInternalAlignment, + InternalAlignment + }; + + _PROTECTED_UNLESS_EXTRACT_MODE_ + : VEC_pD origpvec; // is used only as a reference for redirecting and reverting pvec + VEC_pD pvec; + double scale; + int tag; + // indicates that pvec has changed and saved pointers must be reconstructed (currently used only + // in AngleViaPoint) + bool pvecChangedFlag; + bool driving; + Alignment internalAlignment; + +public: + Constraint(); + virtual ~Constraint() + {} + + inline VEC_pD params() + { + return pvec; + } + + void redirectParams(const MAP_pD_pD& redirectionmap); + void revertParams(); + void setTag(int tagId) + { + tag = tagId; + } + int getTag() + { + return tag; + } + + void setDriving(bool isdriving) + { + driving = isdriving; + } + bool isDriving() const + { + return driving; + } + + void setInternalAlignment(Alignment isinternalalignment) + { + internalAlignment = isinternalalignment; + } + Alignment isInternalAlignment() const + { + return internalAlignment; + } + + virtual ConstraintType getTypeId(); + virtual void rescale(double coef = 1.); + virtual double error(); + virtual double grad(double*); + // virtual void grad(MAP_pD_D &deriv); --> TODO: vectorized grad version + virtual double maxStep(MAP_pD_D& dir, double lim = 1.); + // Finds first occurrence of param in pvec. This is useful to test if a constraint depends + // on the parameter (it may not actually depend on it, e.g. angle-via-point doesn't depend + // on ellipse's b (radmin), but b will be included within the constraint anyway. + // Returns -1 if not found. + int findParamInPvec(double* param); +}; + +// Equal +class ConstraintEqual: public Constraint +{ +private: + double ratio; + inline double* param1() + { + return pvec[0]; + } + inline double* param2() + { + return pvec[1]; + } + +public: + ConstraintEqual(double* p1, double* p2, double p1p2ratio = 1.0); + ConstraintType getTypeId() override; + void rescale(double coef = 1.) override; + double error() override; + double grad(double*) override; +}; + +// Center of Gravity +class ConstraintCenterOfGravity: public Constraint +{ + inline double* thecenter() + { + return pvec[0]; + } + inline double* pointat(size_t i) + { + return pvec[1 + i]; + } + +public: + /// Constrains that the first parameter is center of gravity of rest + /// Let `pvec = [q, p_1, p_2,...]`, and + /// `givenweights = [f_1, f_2,...]`, then this constraint ensures + /// `q = sum(p_i*f_i)`. + ConstraintCenterOfGravity(const std::vector& givenpvec, + const std::vector& givenweights); + ConstraintType getTypeId() override; + void rescale(double coef = 1.) override; + double error() override; + double grad(double*) override; + +private: + std::vector weights; + std::size_t numpoints; +}; + +// Weighted Linear Combination +class ConstraintWeightedLinearCombination: public Constraint +{ + inline double* thepoint() + { + return pvec[0]; + } + inline double* poleat(size_t i) + { + return pvec[1 + i]; + } + inline double* weightat(size_t i) + { + return pvec[1 + numpoles + i]; + } + +public: + /// Constrains that the first element in pvec is a linear combination + /// of the next numpoints elements in homogeneous coordinates with + /// weights given in the last numpoints elements. + /// Let `pvec = [q, p_1, p_2,... w_1, w_2,...]`, and + /// `givenfactors = [f_1, f_2,...]`, then this constraint ensures + /// `q*sum(w_i*f_i) = sum(p_i*w_i*f_i)`. + /// + /// This constraint is currently used to ensure that a B-spline knot + /// point remains at the position determined by it's poles. + /// In that case, `q` is the x (or y) coordinate of the knot, `p_i` are + /// the x (or y) coordinates of the poles, and `w_i` are their weights. + /// Finally, `f_i` are obtained using `BSpline::getLinCombFactor()`. + ConstraintWeightedLinearCombination(size_t givennumpoints, + const std::vector& givenpvec, + const std::vector& givenfactors); + ConstraintType getTypeId() override; + void rescale(double coef = 1.) override; + double error() override; + double grad(double*) override; + +private: + std::vector factors; + size_t numpoles; +}; + +// Slope at knot +class ConstraintSlopeAtBSplineKnot: public Constraint +{ +private: + inline double* polexat(size_t i) + { + return pvec[i]; + } + inline double* poleyat(size_t i) + { + return pvec[numpoles + i]; + } + inline double* weightat(size_t i) + { + return pvec[2 * numpoles + i]; + } + inline double* linep1x() + { + return pvec[3 * numpoles + 0]; + } + inline double* linep1y() + { + return pvec[3 * numpoles + 1]; + } + inline double* linep2x() + { + return pvec[3 * numpoles + 2]; + } + inline double* linep2y() + { + return pvec[3 * numpoles + 3]; + } + +public: + // TODO: Should be able to make the geometries passed const + // Constrains the slope at a (C1 continuous) knot of the b-spline + ConstraintSlopeAtBSplineKnot(BSpline& b, Line& l, size_t knotindex); + ConstraintType getTypeId() override; + void rescale(double coef = 1.) override; + double error() override; + double grad(double*) override; + +private: + std::vector factors; + std::vector slopefactors; + size_t numpoles; +}; + +// Point On BSpline +class ConstraintPointOnBSpline: public Constraint +{ +private: + inline double* thepoint() + { + return pvec[0]; + } + // TODO: better name because param has a different meaning here? + inline double* theparam() + { + return pvec[1]; + } + inline double* poleat(size_t i) + { + return pvec[2 + (startpole + i) % bsp.poles.size()]; + } + inline double* weightat(size_t i) + { + return pvec[2 + bsp.poles.size() + (startpole + i) % bsp.weights.size()]; + } + void setStartPole(double u); + +public: + /// TODO: Explain how it's provided + /// coordidx = 0 if x, 1 if y + ConstraintPointOnBSpline(double* point, double* initparam, int coordidx, BSpline& b); + ConstraintType getTypeId() override; + void rescale(double coef = 1.) override; + double error() override; + double grad(double*) override; + size_t numpoints; + BSpline& bsp; + size_t startpole; +}; + +// Difference +class ConstraintDifference: public Constraint +{ +private: + inline double* param1() + { + return pvec[0]; + } + inline double* param2() + { + return pvec[1]; + } + inline double* difference() + { + return pvec[2]; + } + +public: + ConstraintDifference(double* p1, double* p2, double* d); + ConstraintType getTypeId() override; + void rescale(double coef = 1.) override; + double error() override; + double grad(double*) override; +}; + +// P2PDistance +class ConstraintP2PDistance: public Constraint +{ +private: + inline double* p1x() + { + return pvec[0]; + } + inline double* p1y() + { + return pvec[1]; + } + inline double* p2x() + { + return pvec[2]; + } + inline double* p2y() + { + return pvec[3]; + } + inline double* distance() + { + return pvec[4]; + } + +public: + ConstraintP2PDistance(Point& p1, Point& p2, double* d); +#ifdef _GCS_EXTRACT_SOLVER_SUBSYSTEM_ + inline ConstraintP2PDistance() + {} +#endif + ConstraintType getTypeId() override; + void rescale(double coef = 1.) override; + double error() override; + double grad(double*) override; + double maxStep(MAP_pD_D& dir, double lim = 1.) override; +}; + +// P2PAngle +class ConstraintP2PAngle: public Constraint +{ +private: + inline double* p1x() + { + return pvec[0]; + } + inline double* p1y() + { + return pvec[1]; + } + inline double* p2x() + { + return pvec[2]; + } + inline double* p2y() + { + return pvec[3]; + } + inline double* angle() + { + return pvec[4]; + } + double da; + +public: + ConstraintP2PAngle(Point& p1, Point& p2, double* a, double da_ = 0.); +#ifdef _GCS_EXTRACT_SOLVER_SUBSYSTEM_ + inline ConstraintP2PAngle() + {} +#endif + ConstraintType getTypeId() override; + void rescale(double coef = 1.) override; + double error() override; + double grad(double*) override; + double maxStep(MAP_pD_D& dir, double lim = 1.) override; +}; + +// P2LDistance +class ConstraintP2LDistance: public Constraint +{ +private: + inline double* p0x() + { + return pvec[0]; + } + inline double* p0y() + { + return pvec[1]; + } + inline double* p1x() + { + return pvec[2]; + } + inline double* p1y() + { + return pvec[3]; + } + inline double* p2x() + { + return pvec[4]; + } + inline double* p2y() + { + return pvec[5]; + } + inline double* distance() + { + return pvec[6]; + } + +public: + ConstraintP2LDistance(Point& p, Line& l, double* d); +#ifdef _GCS_EXTRACT_SOLVER_SUBSYSTEM_ + inline ConstraintP2LDistance() + {} +#endif + ConstraintType getTypeId() override; + void rescale(double coef = 1.) override; + double error() override; + double grad(double*) override; + double maxStep(MAP_pD_D& dir, double lim = 1.) override; + double abs(double darea); +}; + +// PointOnLine +class ConstraintPointOnLine: public Constraint +{ +private: + inline double* p0x() + { + return pvec[0]; + } + inline double* p0y() + { + return pvec[1]; + } + inline double* p1x() + { + return pvec[2]; + } + inline double* p1y() + { + return pvec[3]; + } + inline double* p2x() + { + return pvec[4]; + } + inline double* p2y() + { + return pvec[5]; + } + +public: + ConstraintPointOnLine(Point& p, Line& l); + ConstraintPointOnLine(Point& p, Point& lp1, Point& lp2); +#ifdef _GCS_EXTRACT_SOLVER_SUBSYSTEM_ + inline ConstraintPointOnLine() + {} +#endif + ConstraintType getTypeId() override; + void rescale(double coef = 1.) override; + double error() override; + double grad(double*) override; +}; + +// PointOnPerpBisector +class ConstraintPointOnPerpBisector: public Constraint +{ +private: + inline double* p0x() + { + return pvec[0]; + } + inline double* p0y() + { + return pvec[1]; + } + inline double* p1x() + { + return pvec[2]; + } + inline double* p1y() + { + return pvec[3]; + } + inline double* p2x() + { + return pvec[4]; + } + inline double* p2y() + { + return pvec[5]; + } + void errorgrad(double* err, double* grad, double* param); + +public: + ConstraintPointOnPerpBisector(Point& p, Line& l); + ConstraintPointOnPerpBisector(Point& p, Point& lp1, Point& lp2); +#ifdef _GCS_EXTRACT_SOLVER_SUBSYSTEM_ + inline ConstraintPointOnPerpBisector() {}; +#endif + ConstraintType getTypeId() override; + void rescale(double coef = 1.) override; + + double error() override; + double grad(double*) override; +}; + +// Parallel +class ConstraintParallel: public Constraint +{ +private: + inline double* l1p1x() + { + return pvec[0]; + } + inline double* l1p1y() + { + return pvec[1]; + } + inline double* l1p2x() + { + return pvec[2]; + } + inline double* l1p2y() + { + return pvec[3]; + } + inline double* l2p1x() + { + return pvec[4]; + } + inline double* l2p1y() + { + return pvec[5]; + } + inline double* l2p2x() + { + return pvec[6]; + } + inline double* l2p2y() + { + return pvec[7]; + } + +public: + ConstraintParallel(Line& l1, Line& l2); +#ifdef _GCS_EXTRACT_SOLVER_SUBSYSTEM_ + inline ConstraintParallel() + {} +#endif + ConstraintType getTypeId() override; + void rescale(double coef = 1.) override; + double error() override; + double grad(double*) override; +}; + +// Perpendicular +class ConstraintPerpendicular: public Constraint +{ +private: + inline double* l1p1x() + { + return pvec[0]; + } + inline double* l1p1y() + { + return pvec[1]; + } + inline double* l1p2x() + { + return pvec[2]; + } + inline double* l1p2y() + { + return pvec[3]; + } + inline double* l2p1x() + { + return pvec[4]; + } + inline double* l2p1y() + { + return pvec[5]; + } + inline double* l2p2x() + { + return pvec[6]; + } + inline double* l2p2y() + { + return pvec[7]; + } + +public: + ConstraintPerpendicular(Line& l1, Line& l2); + ConstraintPerpendicular(Point& l1p1, Point& l1p2, Point& l2p1, Point& l2p2); +#ifdef _GCS_EXTRACT_SOLVER_SUBSYSTEM_ + inline ConstraintPerpendicular() + {} +#endif + ConstraintType getTypeId() override; + void rescale(double coef = 1.) override; + double error() override; + double grad(double*) override; +}; + +// L2LAngle +class ConstraintL2LAngle: public Constraint +{ +private: + inline double* l1p1x() + { + return pvec[0]; + } + inline double* l1p1y() + { + return pvec[1]; + } + inline double* l1p2x() + { + return pvec[2]; + } + inline double* l1p2y() + { + return pvec[3]; + } + inline double* l2p1x() + { + return pvec[4]; + } + inline double* l2p1y() + { + return pvec[5]; + } + inline double* l2p2x() + { + return pvec[6]; + } + inline double* l2p2y() + { + return pvec[7]; + } + inline double* angle() + { + return pvec[8]; + } + +public: + ConstraintL2LAngle(Line& l1, Line& l2, double* a); + ConstraintL2LAngle(Point& l1p1, Point& l1p2, Point& l2p1, Point& l2p2, double* a); +#ifdef _GCS_EXTRACT_SOLVER_SUBSYSTEM_ + inline ConstraintL2LAngle() + {} +#endif + ConstraintType getTypeId() override; + void rescale(double coef = 1.) override; + double error() override; + double grad(double*) override; + double maxStep(MAP_pD_D& dir, double lim = 1.) override; +}; + +// MidpointOnLine +class ConstraintMidpointOnLine: public Constraint +{ +private: + inline double* l1p1x() + { + return pvec[0]; + } + inline double* l1p1y() + { + return pvec[1]; + } + inline double* l1p2x() + { + return pvec[2]; + } + inline double* l1p2y() + { + return pvec[3]; + } + inline double* l2p1x() + { + return pvec[4]; + } + inline double* l2p1y() + { + return pvec[5]; + } + inline double* l2p2x() + { + return pvec[6]; + } + inline double* l2p2y() + { + return pvec[7]; + } + +public: + ConstraintMidpointOnLine(Line& l1, Line& l2); + ConstraintMidpointOnLine(Point& l1p1, Point& l1p2, Point& l2p1, Point& l2p2); +#ifdef _GCS_EXTRACT_SOLVER_SUBSYSTEM_ + inline ConstraintMidpointOnLine() + {} +#endif + ConstraintType getTypeId() override; + void rescale(double coef = 1.) override; + double error() override; + double grad(double*) override; +}; + +// TangentCircumf +class ConstraintTangentCircumf: public Constraint +{ +private: + inline double* c1x() + { + return pvec[0]; + } + inline double* c1y() + { + return pvec[1]; + } + inline double* c2x() + { + return pvec[2]; + } + inline double* c2y() + { + return pvec[3]; + } + inline double* r1() + { + return pvec[4]; + } + inline double* r2() + { + return pvec[5]; + } + bool internal; + +public: + ConstraintTangentCircumf(Point& p1, + Point& p2, + double* rd1, + double* rd2, + bool internal_ = false); +#ifdef _GCS_EXTRACT_SOLVER_SUBSYSTEM_ + inline ConstraintTangentCircumf(bool internal_) + { + internal = internal_; + } +#endif + inline bool getInternal() + { + return internal; + }; + ConstraintType getTypeId() override; + void rescale(double coef = 1.) override; + double error() override; + double grad(double*) override; +}; +// PointOnEllipse +class ConstraintPointOnEllipse: public Constraint +{ +private: + inline double* p1x() + { + return pvec[0]; + } + inline double* p1y() + { + return pvec[1]; + } + inline double* cx() + { + return pvec[2]; + } + inline double* cy() + { + return pvec[3]; + } + inline double* f1x() + { + return pvec[4]; + } + inline double* f1y() + { + return pvec[5]; + } + inline double* rmin() + { + return pvec[6]; + } + +public: + ConstraintPointOnEllipse(Point& p, Ellipse& e); + ConstraintPointOnEllipse(Point& p, ArcOfEllipse& a); +#ifdef _GCS_EXTRACT_SOLVER_SUBSYSTEM_ + inline ConstraintPointOnEllipse() + {} +#endif + ConstraintType getTypeId() override; + void rescale(double coef = 1.) override; + double error() override; + double grad(double*) override; +}; + +class ConstraintEllipseTangentLine: public Constraint +{ +private: + Line l; + Ellipse e; + // writes pointers in pvec to the parameters of crv1, crv2 and poa + void ReconstructGeomPointers(); + // error and gradient combined. Values are returned through pointers. + void errorgrad(double* err, double* grad, double* param); + +public: + ConstraintEllipseTangentLine(Line& l, Ellipse& e); + ConstraintType getTypeId() override; + void rescale(double coef = 1.) override; + double error() override; + double grad(double*) override; +}; + +class ConstraintInternalAlignmentPoint2Ellipse: public Constraint +{ +public: + ConstraintInternalAlignmentPoint2Ellipse(Ellipse& e, + Point& p1, + InternalAlignmentType alignmentType); + ConstraintType getTypeId() override; + void rescale(double coef = 1.) override; + double error() override; + double grad(double*) override; + +private: + // error and gradient combined. Values are returned through pointers. + void errorgrad(double* err, double* grad, double* param); + // writes pointers in pvec to the parameters of crv1, crv2 and poa + void ReconstructGeomPointers(); + Ellipse e; + Point p; + InternalAlignmentType AlignmentType; +}; + +class ConstraintInternalAlignmentPoint2Hyperbola: public Constraint +{ +public: + ConstraintInternalAlignmentPoint2Hyperbola(Hyperbola& e, + Point& p1, + InternalAlignmentType alignmentType); + ConstraintType getTypeId() override; + void rescale(double coef = 1.) override; + double error() override; + double grad(double*) override; + +private: + // error and gradient combined. Values are returned through pointers. + void errorgrad(double* err, double* grad, double* param); + // writes pointers in pvec to the parameters of crv1, crv2 and poa + void ReconstructGeomPointers(); + Hyperbola e; + Point p; + InternalAlignmentType AlignmentType; +}; + +class ConstraintEqualMajorAxesConic: public Constraint +{ +private: + MajorRadiusConic* e1; + MajorRadiusConic* e2; + // writes pointers in pvec to the parameters of crv1, crv2 and poa + void ReconstructGeomPointers(); + // error and gradient combined. Values are returned through pointers. + void errorgrad(double* err, double* grad, double* param); + +public: + ConstraintEqualMajorAxesConic(MajorRadiusConic* a1, MajorRadiusConic* a2); + ConstraintType getTypeId() override; + void rescale(double coef = 1.) override; + double error() override; + double grad(double*) override; +}; + +class ConstraintEqualFocalDistance: public Constraint +{ +private: + ArcOfParabola* e1; + ArcOfParabola* e2; + // writes pointers in pvec to the parameters of crv1, crv2 and poa + void ReconstructGeomPointers(); + // error and gradient combined. Values are returned through pointers. + void errorgrad(double* err, double* grad, double* param); + +public: + ConstraintEqualFocalDistance(ArcOfParabola* a1, ArcOfParabola* a2); + ConstraintType getTypeId() override; + void rescale(double coef = 1.) override; + double error() override; + double grad(double*) override; +}; + +class ConstraintCurveValue: public Constraint +{ +private: + // defines, which coordinate of point is being constrained by this constraint + inline double* pcoord() + { + return pvec[2]; + } + inline double* u() + { + return pvec[3]; + } + // error and gradient combined. Values are returned through pointers. + void errorgrad(double* err, double* grad, double* param); + // writes pointers in pvec to the parameters of crv1, crv2 and poa + void ReconstructGeomPointers(); + Curve* crv; + Point p; + +public: + /** + * @brief ConstraintCurveValue: solver constraint that ties parameter value with point + * coordinates, according to curve's parametric equation. + * @param p : endpoint to be constrained + * @param pcoord : pointer to point coordinate to be constrained. Must be either p.x or p.y + * @param crv : the curve (crv->Value() must be functional) + * @param u : pointer to u parameter corresponding to the point + */ + ConstraintCurveValue(Point& p, double* pcoord, Curve& crv, double* u); + ~ConstraintCurveValue() override; + ConstraintType getTypeId() override; + void rescale(double coef = 1.) override; + double error() override; + double grad(double*) override; + double maxStep(MAP_pD_D& dir, double lim = 1.) override; +}; + +// PointOnHyperbola +class ConstraintPointOnHyperbola: public Constraint +{ +private: + inline double* p1x() + { + return pvec[0]; + } + inline double* p1y() + { + return pvec[1]; + } + inline double* cx() + { + return pvec[2]; + } + inline double* cy() + { + return pvec[3]; + } + inline double* f1x() + { + return pvec[4]; + } + inline double* f1y() + { + return pvec[5]; + } + inline double* rmin() + { + return pvec[6]; + } + +public: + ConstraintPointOnHyperbola(Point& p, Hyperbola& e); + ConstraintPointOnHyperbola(Point& p, ArcOfHyperbola& a); +#ifdef _GCS_EXTRACT_SOLVER_SUBSYSTEM_ + inline ConstraintPointOnHyperbola() + {} +#endif + ConstraintType getTypeId() override; + void rescale(double coef = 1.) override; + double error() override; + double grad(double*) override; +}; + +// PointOnParabola +class ConstraintPointOnParabola: public Constraint +{ +private: + // error and gradient combined. Values are returned through pointers. + void errorgrad(double* err, double* grad, double* param); + // writes pointers in pvec to the parameters of crv1, crv2 and poa + void ReconstructGeomPointers(); + Parabola* parab; + Point p; + +public: + ConstraintPointOnParabola(Point& p, Parabola& e); + ConstraintPointOnParabola(Point& p, ArcOfParabola& a); + ~ConstraintPointOnParabola() override; +#ifdef _GCS_EXTRACT_SOLVER_SUBSYSTEM_ + inline ConstraintPointOnParabola() + {} +#endif + ConstraintType getTypeId() override; + void rescale(double coef = 1.) override; + double error() override; + double grad(double*) override; +}; + +class ConstraintAngleViaPoint: public Constraint +{ +private: + inline double* angle() + { + return pvec[0]; + }; + Curve* crv1; + Curve* crv2; + // These two pointers hold copies of the curves that were passed on + // constraint creation. The curves must be deleted upon destruction of + // the constraint. It is necessary to have copies, since messing with + // original objects that were passed is a very bad idea (but messing is + // necessary, because we need to support redirectParams()/revertParams + // functions. + // The pointers in the curves need to be reconstructed if pvec was redirected + // (test pvecChangedFlag variable before use!) + // poa=point of angle //needs to be reconstructed if pvec was redirected/reverted. The point is + // easily shallow-copied by C++, so no pointer type here and no delete is necessary. + Point poa; + // writes pointers in pvec to the parameters of crv1, crv2 and poa + void ReconstructGeomPointers(); + +public: + ConstraintAngleViaPoint(Curve& acrv1, Curve& acrv2, Point p, double* angle); + ~ConstraintAngleViaPoint() override; + ConstraintType getTypeId() override; + void rescale(double coef = 1.) override; + double error() override; + double grad(double*) override; +}; + +class ConstraintAngleViaTwoPoints: public Constraint +{ +private: + inline double* angle() + { + return pvec[0]; + }; + Curve* crv1; + Curve* crv2; + // These two pointers hold copies of the curves that were passed on + // constraint creation. The curves must be deleted upon destruction of + // the constraint. It is necessary to have copies, since messing with + // original objects that were passed is a very bad idea (but messing is + // necessary, because we need to support redirectParams()/revertParams + // functions. + // The pointers in the curves need to be reconstructed if pvec was redirected + // (test pvecChangedFlag variable before use!) + // poa=point of angle //needs to be reconstructed if pvec was redirected/reverted. The points + // are easily shallow-copied by C++, so no pointer type here and no delete is necessary. We use + // two points in this method as a workaround for B-splines (and friends). There, normals at + // general points are not implemented, just at their stored start/end points. + Point poa1; + Point poa2; + // writes pointers in pvec to the parameters of crv1, crv2 and poa + void ReconstructGeomPointers(); + +public: + ConstraintAngleViaTwoPoints(Curve& acrv1, Curve& acrv2, Point p1, Point p2, double* angle); + ~ConstraintAngleViaTwoPoints() override; + ConstraintType getTypeId() override; + void rescale(double coef = 1.) override; + double error() override; + double grad(double*) override; +}; + +// snell's law angles constrainer. Point needs to lie on all three curves to be constraied. +class ConstraintSnell: public Constraint +{ +private: + inline double* n1() + { + return pvec[0]; + }; + inline double* n2() + { + return pvec[1]; + }; + Curve* ray1; + Curve* ray2; + Curve* boundary; + // These pointers hold copies of the curves that were passed on + // constraint creation. The curves must be deleted upon destruction of + // the constraint. It is necessary to have copies, since messing with + // original objects that were passed is a very bad idea (but messing is + // necessary, because we need to support redirectParams()/revertParams + // functions. + // The pointers in the curves need to be reconstructed if pvec was redirected + // (test pvecChangedFlag variable before use!) + // poa=point of refraction //needs to be reconstructed if pvec was redirected/reverted. The + // point is easily shallow-copied by C++, so no pointer type here and no delete is necessary. + Point poa; + bool flipn1, flipn2; + // writes pointers in pvec to the parameters of crv1, crv2 and poa + void ReconstructGeomPointers(); + // error and gradient combined. Values are returned through pointers. + void errorgrad(double* err, double* grad, double* param); + +public: + // n1dn2 = n1 divided by n2. from n1 to n2. flipn1 = true instructs to flip ray1's tangent + ConstraintSnell(Curve& ray1, + Curve& ray2, + Curve& boundary, + Point p, + double* n1, + double* n2, + bool flipn1, + bool flipn2); + ~ConstraintSnell() override; + ConstraintType getTypeId() override; + void rescale(double coef = 1.) override; + double error() override; + double grad(double*) override; +}; + +class ConstraintAngleViaPointAndParam: public Constraint +{ +private: + inline double* angle() + { + return pvec[0]; + }; + inline double* cparam() + { + return pvec[3]; + }; + Curve* crv1; + Curve* crv2; + // These two pointers hold copies of the curves that were passed on + // constraint creation. The curves must be deleted upon destruction of + // the constraint. It is necessary to have copies, since messing with + // original objects that were passed is a very bad idea (but messing is + // necessary, because we need to support redirectParams()/revertParams + // functions. + // The pointers in the curves need to be reconstructed if pvec was redirected + // (test pvecChangedFlag variable before use!) + Point poa; // poa=point of angle //needs to be reconstructed if pvec was redirected/reverted. + // The point is easily shallow-copied by C++, so no pointer type here and no delete + // is necessary. + void + ReconstructGeomPointers(); // writes pointers in pvec to the parameters of crv1, crv2 and poa +public: + // We assume first curve needs param1 + ConstraintAngleViaPointAndParam(Curve& acrv1, + Curve& acrv2, + Point p, + double* param1, + double* angle); + ~ConstraintAngleViaPointAndParam() override; + ConstraintType getTypeId() override; + void rescale(double coef = 1.) override; + double error() override; + double grad(double*) override; +}; + +// TODO: Do we need point here at all? +class ConstraintAngleViaPointAndTwoParams: public Constraint +{ +private: + inline double* angle() + { + return pvec[0]; + }; + inline double* cparam1() + { + return pvec[3]; + }; + inline double* cparam2() + { + return pvec[4]; + }; + Curve* crv1; + Curve* crv2; + // These two pointers hold copies of the curves that were passed on + // constraint creation. The curves must be deleted upon destruction of + // the constraint. It is necessary to have copies, since messing with + // original objects that were passed is a very bad idea (but messing is + // necessary, because we need to support redirectParams()/revertParams + // functions. + // The pointers in the curves need to be reconstructed if pvec was redirected + // (test pvecChangedFlag variable before use!) + Point poa; // poa=point of angle //needs to be reconstructed if pvec was redirected/reverted. + // The point is easily shallow-copied by C++, so no pointer type here and no delete + // is necessary. + void + ReconstructGeomPointers(); // writes pointers in pvec to the parameters of crv1, crv2 and poa +public: + ConstraintAngleViaPointAndTwoParams(Curve& acrv1, + Curve& acrv2, + Point p, + double* param1, + double* param2, + double* angle); + ~ConstraintAngleViaPointAndTwoParams() override; + ConstraintType getTypeId() override; + void rescale(double coef = 1.) override; + double error() override; + double grad(double*) override; +}; + +class ConstraintEqualLineLength: public Constraint +{ +private: + Line l1; + Line l2; + // writes pointers in pvec to the parameters of line1, line2 + void ReconstructGeomPointers(); + // error and gradient combined. Values are returned through pointers. + void errorgrad(double* err, double* grad, double* param); + +public: + ConstraintEqualLineLength(Line& l1, Line& l2); + ConstraintType getTypeId() override; + void rescale(double coef = 1.) override; + double error() override; + double grad(double*) override; +}; + +class ConstraintC2CDistance: public Constraint +{ +private: + Circle c1; + Circle c2; + double* d; + inline double* distance() + { + return pvec[0]; + } + // writes pointers in pvec to the parameters of c1, c2 + void ReconstructGeomPointers(); + // error and gradient combined. Values are returned through pointers. + void errorgrad(double* err, double* grad, double* param); + +public: + ConstraintC2CDistance(Circle& c1, Circle& c2, double* d); + ConstraintType getTypeId() override; + void rescale(double coef = 1.) override; + double error() override; + double grad(double*) override; +}; + +// C2LDistance +class ConstraintC2LDistance: public Constraint +{ +private: + Circle circle; + Line line; + double* d; + inline double* distance() + { + return pvec[0]; + } + // writes pointers in pvec to the parameters of c, l + void ReconstructGeomPointers(); + // error and gradient combined. Values are returned through pointers. + void errorgrad(double* err, double* grad, double* param); + +public: + ConstraintC2LDistance(Circle& c, Line& l, double* d); + ConstraintType getTypeId() override; + void rescale(double coef = 1.) override; + double error() override; + double grad(double*) override; +}; + +// P2CDistance +class ConstraintP2CDistance: public Constraint +{ +private: + Circle circle; + Point pt; + double* d; + inline double* distance() + { + return pvec[0]; + } + void ReconstructGeomPointers(); // writes pointers in pvec to the parameters of c + void + errorgrad(double* err, + double* grad, + double* param); // error and gradient combined. Values are returned through pointers. +public: + ConstraintP2CDistance(Point& p, Circle& c, double* d); + ConstraintType getTypeId() override; + void rescale(double coef = 1.) override; + double error() override; + double grad(double*) override; +}; + +// ArcLength +class ConstraintArcLength: public Constraint +{ +private: + Arc arc; + double* d; + inline double* distance() + { + return pvec[0]; + } + void ReconstructGeomPointers(); // writes pointers in pvec to the parameters of a + void + errorgrad(double* err, + double* grad, + double* param); // error and gradient combined. Values are returned through pointers. +public: + ConstraintArcLength(Arc& a, double* d); + ConstraintType getTypeId() override; + void rescale(double coef = 1.) override; + double error() override; + double grad(double*) override; +}; + +} // namespace GCS + +#endif // PLANEGCS_CONSTRAINTS_H diff --git a/GCS/GCS.cpp b/GCS/GCS.cpp new file mode 100644 index 0000000..a1221db --- /dev/null +++ b/GCS/GCS.cpp @@ -0,0 +1,5839 @@ +#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 +#include +#include +#include +#include + +#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 +#endif + +// _GCS_EXTRACT_SOLVER_SUBSYSTEM_ to be enabled in Constraints.h when needed. +#if defined(_GCS_EXTRACT_SOLVER_SUBSYSTEM_) || defined(_DEBUG_TO_FILE) +#include + +#define CASE_NOT_IMP(X) \ + case X: { \ + subsystemfile << "//" #X "not yet implemented" << std::endl; \ + break; \ + } +#endif + +#define BOOST_NO_CXX98_FUNCTION_BASE +#include +#include + +#include +#include + +using MatrixIndexType = Eigen::FullPivHouseholderQR::IntDiagSizeVectorType; + +#ifndef EIGEN_STOCK_FULLPIVLU_COMPUTE +namespace Eigen +{ + +using MatrixdType = Matrix; + +template<> +FullPivLU& FullPivLU::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::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> constraintgroups); + void LogSetOfConstraints(const std::string& str, std::set constraintset); + void LogGroupOfParameters(const std::string& str, + std::vector> 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) +{ +} + +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> 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 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> 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; + +/////////////////////////////////////// +// 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() +{ + pts.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(); + deleteAllContent(clist); + c2p.clear(); + p2c.clear(); +} + +void System::invalidatedDiagnosis() +{ + hasDiagnosis = false; + pDependentParameters.clear(); + pDependentParametersGroups.clear(); +} + +void System::clearByTag(int tagId, bool is_fixed_point) +{ + std::vector constrvec; + for (std::vector::const_iterator constr = clist.begin(); constr != clist.end(); + ++constr) { + if ((*constr)->getTag() == tagId) { + constrvec.push_back(*constr); + } + } + for (std::vector::const_iterator constr = constrvec.begin(); + constr != constrvec.end(); + ++constr) { + removeConstraint(*constr); + } + if (is_fixed_point) { + for (size_t i = 0; i < pts.size(); ++i) { + if (pts[i].first == tagId) { + delete pts[i].second.first; + delete pts[i].second.second; + pts.erase(pts.begin() + tagId - 1); + } + } + } +} + +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::iterator it; + it = std::ranges::find(clist, 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& constraints = p2c[*param]; + it = std::ranges::find(constraints, constr); + constraints.erase(it); + } + c2p.erase(constr); + + delete (constr); +} + +// basic constraints + +int System::addConstraintFixed(Point& p, int tagId, bool driving) +{ + // TODO - избавиться от утечек памяти + pts.emplace_back(tagId, std::make_pair(new double(p.get_X()), new double(p.get_Y()))); + + addConstraintCoordinateX(p, pts.back().second.first, tagId, driving); + return addConstraintCoordinateY(p, pts.back().second.second, tagId, driving); +} + +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); +} + +int System::addConstraintArcLength(Arc& a, double* distance, int tagId, bool driving) +{ + Constraint* constr = new ConstraintArcLength(a, 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) +{ + using std::numbers::pi; + + 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) +{ + using std::numbers::pi; + + 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) +{ + using std::numbers::pi; + + 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) +{ + using std::numbers::pi; + + 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 pvec; + pvec.push_back(p.x); + + std::vector 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::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::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(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 still no diagnosis after explicitly calling `diagnose`, nothing to do here + if (!hasDiagnosis) { + return; + } + + std::vector clistR; + if (!redundant.empty()) { + std::copy_if(clist.begin(), clist.end(), std::back_inserter(clistR), [this](auto constr) { + return this->redundant.count(constr) == 0; + }); + } + 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 (const auto constr : clistR) { + VEC_pD& cparams = c2p[constr]; + for (const auto param : cparams) { + MAP_pD_I::const_iterator it = pIndex.find(param); + if (it != pIndex.end()) { + boost::add_edge(cvtid, it->second, g); + } + } + ++cvtid; + } + + 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 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 (const auto& constr : clistR) { + if (!(constr->getTag() >= 0 && constr->getTypeId() == Equal)) { + continue; + } + const auto it1 = pIndex.find(constr->params()[0]); + const auto it2 = pIndex.find(constr->params()[1]); + if (it1 == pIndex.end() || it2 == pIndex.end()) { + continue; + } + reducedConstrs.insert(constr); + double* p_kept = reducedParams[it1->second]; + double* p_replaced = reducedParams[it2->second]; + std::replace(reducedParams.begin(), reducedParams.end(), p_replaced, p_kept); + } + for (size_t i = 0; i < plist.size(); ++i) { + if (plist[i] != reducedParams[i]) { + int cid = components[i]; + reductionmaps[cid][plist[i]] = reducedParams[i]; + } + } + } + + // TODO: Why are the later (constraint-related) items added first? + // Adding plist-related items first would simplify assignment of `i`, but is not a big expense + // overall. Leaving as is to avoid any unintended consequences. + clists.clear(); // destroy any lists + clists.resize(componentsSize); // create empty lists to be filled in + size_t i = plist.size(); + for (const auto& constr : clistR) { + if (reducedConstrs.count(constr) == 0) { + int cid = components[i]; + clists[cid].push_back(constr); + } + ++i; + } + + plists.clear(); // destroy any lists + plists.resize(componentsSize); // create empty lists to be filled in + for (size_t i = 0; i < plist.size(); ++i) { + int cid = components[i]; + plists[cid].push_back(plist[i]); + } + + // calculates subSystems and subSystemsAux from clists, plists and reductionmaps + clearSubSystems(); + subSystems.resize(clists.size(), nullptr); + subSystemsAux.resize(clists.size(), nullptr); + for (std::size_t cid = 0; cid < clists.size(); ++cid) { + std::vector clist0, clist1; + std::partition_copy(clists[cid].begin(), + clists[cid].end(), + std::back_inserter(clist0), + std::back_inserter(clist1), + [](auto constr) { + return constr->getTag() >= 0; + }); + + 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(alg); + 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::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 = (sketchSizeMultiplier ? maxIter * xsize : maxIter); + double convCriterion = convergence; + if (isRedundantsolving) { + maxIterNumber = + (sketchSizeMultiplierRedundant ? maxIterRedundant * xsize : maxIterRedundant); + convCriterion = convergenceRedundant; + } + + if (debugMode == IterationLevel) { + std::stringstream stream; + stream << "BFGS: convergence: " << convCriterion << ", xsize: " << xsize + << ", maxIter: " << maxIterNumber << "\n"; + + const std::string tmp = stream.str(); + } + + double divergingLim = 1e6 * err + 1e12; + double h_norm {}; + + for (int iter = 1; iter < maxIterNumber; ++iter) { + h_norm = h.norm(); + if (h_norm <= convCriterion || err <= smallF) { + if (debugMode == IterationLevel) { + std::stringstream stream; + stream << "BFGS Converged!!: " + << ", err: " << err << ", h_norm: " << h_norm << "\n"; + + const std::string tmp = stream.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(); + } + 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(); + } + } + + subsys->revertParams(); + + if (err <= smallF) { + return Success; + } + if (h.norm() <= convCriterion) { + 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 = (sketchSizeMultiplier ? maxIter * xsize : maxIter); + + double divergingLim = 1e6 * e.squaredNorm() + 1e12; + + double eps = LM_eps; + double eps1 = LM_eps1; + double tau = LM_tau; + + if (isRedundantsolving) { + maxIterNumber = + (sketchSizeMultiplierRedundant ? maxIterRedundant * xsize : maxIterRedundant); + eps = LM_epsRedundant; + eps1 = LM_eps1Redundant; + tau = LM_tauRedundant; + } + + 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(); + } + + 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(); + 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(); + } + + 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(); + + constexpr double epsilon = std::numeric_limits::epsilon(); + if (h_norm <= eps1 * eps1 * x.norm()) { + // relative change in p is small, stop + stop = 3; + break; + } + else if (h_norm >= (x.norm() + eps1) / (epsilon * 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(); + } + } + + 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 + + int xsize = subsys->pSize(); + int csize = subsys->cSize(); + + if (xsize == 0) { + return Success; + } + + double tolg = DL_tolg; + double tolx = DL_tolx; + double tolf = DL_tolf; + + int maxIterNumber = (sketchSizeMultiplier ? maxIter * xsize : maxIter); + if (isRedundantsolving) { + tolg = DL_tolgRedundant; + tolx = DL_tolxRedundant; + tolf = DL_tolfRedundant; + + maxIterNumber = + (sketchSizeMultiplierRedundant ? maxIterRedundant * xsize : maxIterRedundant); + } + + 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(); + } + + 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(); + double fx_inf = fx.lpNorm(); + + 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; + break; + } + else if (g_inf <= tolg) { + stop = 2; + break; + } + else if (delta <= tolx * (tolx + x.norm())) { + stop = 2; + break; + } + else if (iter >= maxIterNumber) { + stop = 4; + break; + } + else if (err > divergingLim || err != err) { + // check for diverging and NaN + stop = 6; + break; + } + + // 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; + } + + // 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(); + fx_inf = fx.lpNorm(); + } + 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(); + } + + // 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(); + } + + return (stop == 1) ? Success : Failed; +} + +#ifdef _GCS_EXTRACT_SOLVER_SUBSYSTEM_ +void System::extractSubsystem(SubSystem* subsys, bool isRedundantsolving) +{ + VEC_pD plistout; // std::vector + std::vector 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 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::ranges::find(plist, (*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::iterator it = clist_.begin(); it != clist_.end(); ++it, ++ic) { + + switch ((*it)->getTypeId()) { + case Equal: { // 2 + VEC_pD::iterator p1 = std::ranges::find(plist, (*it)->pvec[0]); + VEC_pD::iterator p2 = std::ranges::find(plist, (*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::ranges::find(clist_params_, (*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::ranges::find(clist_params_, (*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::ranges::find(plist, (*it)->pvec[0]); + VEC_pD::iterator p2 = std::ranges::find(plist, (*it)->pvec[1]); + VEC_pD::iterator p3 = std::ranges::find(plist, (*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::ranges::find(clist_params_, (*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::ranges::find(clist_params_, (*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::ranges::find(clist_params_, (*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::ranges::find(plist, (*it)->pvec[0]); + VEC_pD::iterator p2 = std::ranges::find(plist, (*it)->pvec[1]); + VEC_pD::iterator p3 = std::ranges::find(plist, (*it)->pvec[2]); + VEC_pD::iterator p4 = std::ranges::find(plist, (*it)->pvec[3]); + VEC_pD::iterator p5 = std::ranges::find(plist, (*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::ranges::find(clist_params_, (*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::ranges::find(clist_params_, (*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::ranges::find(clist_params_, (*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::ranges::find(clist_params_, (*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::ranges::find(clist_params_, (*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::ranges::find(plist, (*it)->pvec[0]); + VEC_pD::iterator p2 = std::ranges::find(plist, (*it)->pvec[1]); + VEC_pD::iterator p3 = std::ranges::find(plist, (*it)->pvec[2]); + VEC_pD::iterator p4 = std::ranges::find(plist, (*it)->pvec[3]); + VEC_pD::iterator p5 = std::ranges::find(plist, (*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::ranges::find(clist_params_, (*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::ranges::find(clist_params_, (*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::ranges::find(clist_params_, (*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::ranges::find(clist_params_, (*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::ranges::find(clist_params_, (*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::ranges::find(plist, (*it)->pvec[0]); + VEC_pD::iterator p2 = std::ranges::find(plist, (*it)->pvec[1]); + VEC_pD::iterator p3 = std::ranges::find(plist, (*it)->pvec[2]); + VEC_pD::iterator p4 = std::ranges::find(plist, (*it)->pvec[3]); + VEC_pD::iterator p5 = std::ranges::find(plist, (*it)->pvec[4]); + VEC_pD::iterator p6 = std::ranges::find(plist, (*it)->pvec[5]); + VEC_pD::iterator p7 = std::ranges::find(plist, (*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::ranges::find(clist_params_, (*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::ranges::find(clist_params_, (*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::ranges::find(clist_params_, (*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::ranges::find(clist_params_, (*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::ranges::find(clist_params_, (*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::ranges::find(clist_params_, (*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::ranges::find(clist_params_, (*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::ranges::find(plist, (*it)->pvec[0]); + VEC_pD::iterator p2 = std::ranges::find(plist, (*it)->pvec[1]); + VEC_pD::iterator p3 = std::ranges::find(plist, (*it)->pvec[2]); + VEC_pD::iterator p4 = std::ranges::find(plist, (*it)->pvec[3]); + VEC_pD::iterator p5 = std::ranges::find(plist, (*it)->pvec[4]); + VEC_pD::iterator p6 = std::ranges::find(plist, (*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::ranges::find(clist_params_, (*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::ranges::find(clist_params_, (*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::ranges::find(clist_params_, (*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::ranges::find(clist_params_, (*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::ranges::find(clist_params_, (*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::ranges::find(clist_params_, (*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::ranges::find(plist, (*it)->pvec[0]); + VEC_pD::iterator p2 = std::ranges::find(plist, (*it)->pvec[1]); + VEC_pD::iterator p3 = std::ranges::find(plist, (*it)->pvec[2]); + VEC_pD::iterator p4 = std::ranges::find(plist, (*it)->pvec[3]); + VEC_pD::iterator p5 = std::ranges::find(plist, (*it)->pvec[4]); + VEC_pD::iterator p6 = std::ranges::find(plist, (*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::ranges::find(clist_params_, (*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::ranges::find(clist_params_, (*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::ranges::find(clist_params_, (*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::ranges::find(clist_params_, (*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::ranges::find(clist_params_, (*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::ranges::find(clist_params_, (*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::ranges::find(plist, (*it)->pvec[0]); + VEC_pD::iterator p2 = std::ranges::find(plist, (*it)->pvec[1]); + VEC_pD::iterator p3 = std::ranges::find(plist, (*it)->pvec[2]); + VEC_pD::iterator p4 = std::ranges::find(plist, (*it)->pvec[3]); + VEC_pD::iterator p5 = std::ranges::find(plist, (*it)->pvec[4]); + VEC_pD::iterator p6 = std::ranges::find(plist, (*it)->pvec[5]); + VEC_pD::iterator p7 = std::ranges::find(plist, (*it)->pvec[6]); + VEC_pD::iterator p8 = std::ranges::find(plist, (*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::ranges::find(clist_params_, (*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::ranges::find(clist_params_, (*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::ranges::find(clist_params_, (*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::ranges::find(clist_params_, (*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::ranges::find(clist_params_, (*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::ranges::find(clist_params_, (*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::ranges::find(clist_params_, (*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::ranges::find(clist_params_, (*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::ranges::find(plist, (*it)->pvec[0]); + VEC_pD::iterator p2 = std::ranges::find(plist, (*it)->pvec[1]); + VEC_pD::iterator p3 = std::ranges::find(plist, (*it)->pvec[2]); + VEC_pD::iterator p4 = std::ranges::find(plist, (*it)->pvec[3]); + VEC_pD::iterator p5 = std::ranges::find(plist, (*it)->pvec[4]); + VEC_pD::iterator p6 = std::ranges::find(plist, (*it)->pvec[5]); + VEC_pD::iterator p7 = std::ranges::find(plist, (*it)->pvec[6]); + VEC_pD::iterator p8 = std::ranges::find(plist, (*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::ranges::find(clist_params_, (*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::ranges::find(clist_params_, (*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::ranges::find(clist_params_, (*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::ranges::find(clist_params_, (*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::ranges::find(clist_params_, (*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::ranges::find(clist_params_, (*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::ranges::find(clist_params_, (*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::ranges::find(clist_params_, (*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::ranges::find(plist, (*it)->pvec[0]); + VEC_pD::iterator p2 = std::ranges::find(plist, (*it)->pvec[1]); + VEC_pD::iterator p3 = std::ranges::find(plist, (*it)->pvec[2]); + VEC_pD::iterator p4 = std::ranges::find(plist, (*it)->pvec[3]); + VEC_pD::iterator p5 = std::ranges::find(plist, (*it)->pvec[4]); + VEC_pD::iterator p6 = std::ranges::find(plist, (*it)->pvec[5]); + VEC_pD::iterator p7 = std::ranges::find(plist, (*it)->pvec[6]); + VEC_pD::iterator p8 = std::ranges::find(plist, (*it)->pvec[7]); + VEC_pD::iterator p9 = std::ranges::find(plist, (*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::ranges::find(clist_params_, (*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::ranges::find(clist_params_, (*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::ranges::find(clist_params_, (*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::ranges::find(clist_params_, (*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::ranges::find(clist_params_, (*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::ranges::find(clist_params_, (*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::ranges::find(clist_params_, (*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::ranges::find(clist_params_, (*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::ranges::find(clist_params_, (*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::ranges::find(plist, (*it)->pvec[0]); + VEC_pD::iterator p2 = std::ranges::find(plist, (*it)->pvec[1]); + VEC_pD::iterator p3 = std::ranges::find(plist, (*it)->pvec[2]); + VEC_pD::iterator p4 = std::ranges::find(plist, (*it)->pvec[3]); + VEC_pD::iterator p5 = std::ranges::find(plist, (*it)->pvec[4]); + VEC_pD::iterator p6 = std::ranges::find(plist, (*it)->pvec[5]); + VEC_pD::iterator p7 = std::ranges::find(plist, (*it)->pvec[6]); + VEC_pD::iterator p8 = std::ranges::find(plist, (*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::ranges::find(clist_params_, (*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::ranges::find(clist_params_, (*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::ranges::find(clist_params_, (*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::ranges::find(clist_params_, (*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::ranges::find(clist_params_, (*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::ranges::find(clist_params_, (*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::ranges::find(clist_params_, (*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::ranges::find(clist_params_, (*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::ranges::find(plist, (*it)->pvec[0]); + VEC_pD::iterator p2 = std::ranges::find(plist, (*it)->pvec[1]); + VEC_pD::iterator p3 = std::ranges::find(plist, (*it)->pvec[2]); + VEC_pD::iterator p4 = std::ranges::find(plist, (*it)->pvec[3]); + VEC_pD::iterator p5 = std::ranges::find(plist, (*it)->pvec[4]); + VEC_pD::iterator p6 = std::ranges::find(plist, (*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::ranges::find(clist_params_, (*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::ranges::find(clist_params_, (*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::ranges::find(clist_params_, (*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::ranges::find(clist_params_, (*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::ranges::find(clist_params_, (*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::ranges::find(clist_params_, (*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(*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::ranges::find(plist, (*it)->pvec[0]); + VEC_pD::iterator p2 = std::ranges::find(plist, (*it)->pvec[1]); + VEC_pD::iterator p3 = std::ranges::find(plist, (*it)->pvec[2]); + VEC_pD::iterator p4 = std::ranges::find(plist, (*it)->pvec[3]); + VEC_pD::iterator p5 = std::ranges::find(plist, (*it)->pvec[4]); + VEC_pD::iterator p6 = std::ranges::find(plist, (*it)->pvec[5]); + VEC_pD::iterator p7 = std::ranges::find(plist, (*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::ranges::find(clist_params_, (*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::ranges::find(clist_params_, (*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::ranges::find(clist_params_, (*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::ranges::find(clist_params_, (*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::ranges::find(clist_params_, (*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::ranges::find(clist_params_, (*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::ranges::find(clist_params_, (*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& jacobianconstraintmap, + GCS::VEC_pD& pdiagnoselist, + std::map& 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::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 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 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(); + + // 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 (J.rows() == 0) { + return dofs; + } + + // From here on, presuming `J.rows() > 0`. + emptyDiagnoseMatrix = false; + + if (qrAlgorithm == EigenDenseQR) { +#ifdef PROFILE_DIAGNOSE + Base::TimeElapsed DenseQR_start_time; +#endif + + int rank = 0; // rank is not cheap to retrieve from qrJT in DenseQR + Eigen::MatrixXd R; + Eigen::FullPivHouseholderQR 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); + + 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 + int rank = 0; + Eigen::MatrixXd R; + Eigen::SparseQR, Eigen::COLAMDOrdering> 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); + + Console::Log("\nSparseQR - Lapsed Time: %f seconds\n", SolveTime); +#endif + } +#endif + + return dofs; +} + +void System::makeDenseQRDecomposition(const Eigen::MatrixXd& J, + const std::map& jacobianconstraintmap, + Eigen::FullPivHouseholderQR& 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(); + } + else { + R = qrJT.matrixQR().topRows(colsNum).triangularView(); + } + } + 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& jacobianconstraintmap, + Eigen::SparseQR, Eigen::COLAMDOrdering>& SqrJT, + int& rank, + Eigen::MatrixXd& R, + bool transposeJ, + bool silent) +{ + + Eigen::SparseMatrix 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 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(); + } + else { + R = SqrJT.matrixR().topRows(colsNum).triangularView(); + } + +#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& jacobianconstraintmap, + const GCS::VEC_pD& pdiagnoselist, + bool silent) +{ + Eigen::FullPivHouseholderQR 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& jacobianconstraintmap, + const GCS::VEC_pD& pdiagnoselist, + bool silent) +{ + Eigen::SparseQR, Eigen::COLAMDOrdering> SqrJ; + Eigen::MatrixXd Rparams; + + int nontransprank; + + makeSparseQRDecomposition(J, + jacobianconstraintmap, + SqrJ, + nontransprank, + Rparams, + false, + true); // do not transpose allow one to diagnose parameters + + identifyDependentParameters(SqrJ, Rparams, nontransprank, pdiagnoselist, silent); +} +#endif + +template +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& 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 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 indepParamCols; + std::set 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 +void System::identifyConflictingRedundantConstraints( + Algorithm alg, + const T& qrJT, + const std::map& jacobianconstraintmap, + const std::map& tagmultiplicity, + GCS::VEC_pD& pdiagnoselist, + Eigen::MatrixXd& R, + int constrNum, + int rank, + int& nonredundantconstrNum) +{ + eliminateNonZerosOverPivotInUpperTriangularMatrix(R, rank); + + std::vector> 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 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 conflictingMap; + for (std::size_t i = 0; i < conflictGroups.size(); i++) { + if (satisfiedGroups.count(i) != 0) { + continue; + } + + for (const auto& constr : conflictGroups[i]) { + 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; + } + + /* 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. + */ + auto iterMostPopular = std::max_element( + conflictingMap.begin(), + conflictingMap.end(), + [&tagmultiplicity](const auto& pair1, const auto& pair2) { + size_t sizeOfSet1 = pair1.second.size(); + size_t sizeOfSet2 = pair2.second.size(); + auto tag1 = pair1.first->getTag(); + auto tag2 = pair2.first->getTag(); + + return (sizeOfSet2 > sizeOfSet1 // (1) + || (sizeOfSet2 == sizeOfSet1 + && tagmultiplicity.at(tag2) < tagmultiplicity.at(tag1)) // (2) + || (sizeOfSet2 == sizeOfSet1 + && tagmultiplicity.at(tag2) == tagmultiplicity.at(tag1) + && tag2 > tag1)); // (3) + }); + + Constraint* mostPopular = iterMostPopular->first; + int maxPopularity = iterMostPopular->second.size(); + + if (!(maxPopularity > 0)) { + continue; + } + + // 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& [constr, conflSet] : conflictingMap) { + if (!(constr->getTag() == maxPopularityTag)) { + continue; + } + + skipped.insert(constr); + std::copy(conflSet.begin(), + conflSet.end(), + std::inserter(satisfiedGroups, satisfiedGroups.begin())); + } + } + + // Augment information regarding the choice made by popularity contest + if (debugMode == IterationLevel) { + SolverReportingManager::Manager().LogSetOfConstraints("Chosen redundants", skipped); + } + + std::vector clistTmp; + clistTmp.reserve(clist.size()); + std::copy_if(clist.begin(), + clist.end(), + std::back_inserter(clistTmp), + [&skipped](const auto& constr) { + return (constr->isDriving() && skipped.count(constr) == 0); + }); + + 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; + } + } + + if (res == Success) { + subSysTmp->applySolution(); + std::copy_if(skipped.begin(), + skipped.end(), + std::inserter(redundant, redundant.begin()), + [this](const auto& constr) { + double err = constr->error(); + return (err * err < this->convergenceRedundant); + }); + resetToReference(); + + // TODO: Figure out why we need to iterate in reverse order and add explanation here. + std::vector> conflictGroupsOrig = conflictGroups; + conflictGroups.clear(); + for (int i = conflictGroupsOrig.size() - 1; i >= 0; i--) { + auto iterRedundantEntry = std::find_if(conflictGroupsOrig[i].begin(), + conflictGroupsOrig[i].end(), + [this](const auto item) { + return (this->redundant.count(item) > 0); + }); + bool hasRedundant = (iterRedundantEntry != conflictGroupsOrig[i].end()); + if (!hasRedundant) { + conflictGroups.push_back(conflictGroupsOrig[i]); + continue; + } + + constrNum--; + } + } + delete subSysTmp; + + // simplified output of conflicting tags + SET_I conflictingTagsSet; + for (const auto& cGroup : conflictGroups) { + // exclude internal alignment + std::transform(cGroup.begin(), + cGroup.end(), + std::inserter(conflictingTagsSet, conflictingTagsSet.begin()), + [](const auto& constr) { + bool isinternalalignment = (constr->isInternalAlignment() + == Constraint::Alignment::InternalAlignment); + return (isinternalalignment ? 0 : constr->getTag()); + }); + } + + // exclude constraints tagged with zero + conflictingTagsSet.erase(0); + + conflictingTags.resize(conflictingTagsSet.size()); + std::copy(conflictingTagsSet.begin(), conflictingTagsSet.end(), conflictingTags.begin()); + + // output of redundant tags + SET_I redundantTagsSet, partiallyRedundantTagsSet; + for (const auto& constr : redundant) { + redundantTagsSet.insert(constr->getTag()); + partiallyRedundantTagsSet.insert(constr->getTag()); + } + + // remove tags represented at least in one non-redundant constraint + for (const auto& constr : clist) { + 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; + deleteAllContent(subSystems); + deleteAllContent(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 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 deleteAllContent(VEC_pD& doublevec) +{ + for (auto& doubleptr : doublevec) { + delete doubleptr; + } + doublevec.clear(); +} + +void deleteAllContent(std::vector& constrvec) +{ + for (auto& constr : constrvec) { + delete constr; + } + constrvec.clear(); +} + +void deleteAllContent(std::vector& subsysvec) +{ + for (auto& subsys : subsysvec) { + delete subsys; + } +} + +} // namespace GCS diff --git a/GCS/GCS.h b/GCS/GCS.h new file mode 100644 index 0000000..456751e --- /dev/null +++ b/GCS/GCS.h @@ -0,0 +1,645 @@ +/*************************************************************************** + * Copyright (c) 2011 Konstantinos Poulios * + * * + * 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 * + * * + ***************************************************************************/ + +#ifndef PLANEGCS_GCS_H +#define PLANEGCS_GCS_H + +#include +#include + +#include "SubSystem.h" + + +#define EIGEN_VERSION \ + (EIGEN_WORLD_VERSION * 10000 + EIGEN_MAJOR_VERSION * 100 + EIGEN_MINOR_VERSION) + +#if EIGEN_VERSION >= 30202 +#define EIGEN_SPARSEQR_COMPATIBLE +#include +#endif + +namespace GCS +{ +/////////////////////////////////////// +// Other BFGS Solver parameters +/////////////////////////////////////// +#define XconvergenceRough 1e-8 +#define smallF 1e-20 + +/////////////////////////////////////// +// Solver +/////////////////////////////////////// + +enum SolveStatus +{ + Success = 0, // Found a solution zeroing the error function + Converged = 1, // Found a solution minimizing the error function + Failed = 2, // Failed to find any solution + SuccessfulSolutionInvalid = 3, // This is a solution where the solver succeeded, but the + // resulting geometry is OCE-invalid +}; + +enum Algorithm +{ + BFGS = 0, + LevenbergMarquardt = 1, + DogLeg = 2 +}; + +enum DogLegGaussStep +{ + FullPivLU = 0, + LeastNormFullPivLU = 1, + LeastNormLdlt = 2 +}; + +enum QRAlgorithm +{ + EigenDenseQR = 0, + EigenSparseQR = 1 +}; + +enum DebugMode +{ + NoDebug = 0, + Minimal = 1, + IterationLevel = 2 +}; + +// Magic numbers for Constraint tags +// - Positive Tags identify a higher level constraint form which the solver constraint +// originates +// - Negative Tags represent temporary constraints, used for example in moving operations, these +// have a different handling in component splitting, see GCS::initSolution. Lifetime is defined +// by the container object via GCS::clearByTag. +// - -1 is typically used as tag for these temporary constraints, its parameters are +// enforced with +// a lower priority than the main system (real sketcher constraints). It gives a nice +// effect when dragging the edge of an unconstrained circle, that the center won't move +// if the edge can be dragged, and only when/if the edge cannot be dragged, e.g. radius +// constraint, the center is moved). +enum SpecialTag +{ + DefaultTemporaryConstraint = -1 +}; + +class System +{ + // This is the main class. It holds all constraints and information + // about partitioning into subsystems and solution strategies +private: + VEC_pD plist; // list of the unknown parameters + VEC_pD pdrivenlist; // list of parameters of driven constraints + MAP_pD_I pIndex; + + VEC_FIXED_POINTS_COORDS pts; // list of fixed points by tag + + VEC_pD pDependentParameters; // list of dependent parameters by the system + + // This is a map of primary and secondary identifiers that are found dependent by the solver + // GCS ignores from a type point + std::vector> pDependentParametersGroups; + + std::vector clist; + std::map c2p; // constraint to parameter adjacency list + std::map> p2c; // parameter to constraint adjacency list + + std::vector subSystems, subSystemsAux; + void clearSubSystems(); + + VEC_D reference; + void setReference(); // copies the current parameter values to reference + void resetToReference(); // reverts all parameter values to the stored reference + + std::vector plists; // partitioned plist except equality constraints + // partitioned clist except equality constraints + std::vector> clists; + std::vector reductionmaps; // for simplification of equality constraints + + int dofs; + std::set redundant; + VEC_I conflictingTags, redundantTags, partiallyRedundantTags; + + bool hasUnknowns; // if plist is filled with the unknown parameters + bool hasDiagnosis; // if dofs, conflictingTags, redundantTags are up to date + bool isInit; // if plists, clists, reductionmaps are up to date + + bool emptyDiagnoseMatrix; // false only if there is at least one driving constraint. + + int solve_BFGS(SubSystem* subsys, bool isFine = true, bool isRedundantsolving = false); + int solve_LM(SubSystem* subsys, bool isRedundantsolving = false); + int solve_DL(SubSystem* subsys, bool isRedundantsolving = false); + + void makeReducedJacobian(Eigen::MatrixXd& J, + std::map& jacobianconstraintmap, + GCS::VEC_pD& pdiagnoselist, + std::map& tagmultiplicity); + + void makeDenseQRDecomposition(const Eigen::MatrixXd& J, + const std::map& jacobianconstraintmap, + Eigen::FullPivHouseholderQR& qrJT, + int& rank, + Eigen::MatrixXd& R, + bool transposeJ = true, + bool silent = false); + +#ifdef EIGEN_SPARSEQR_COMPATIBLE + void makeSparseQRDecomposition( + const Eigen::MatrixXd& J, + const std::map& jacobianconstraintmap, + Eigen::SparseQR, Eigen::COLAMDOrdering>& SqrJT, + int& rank, + Eigen::MatrixXd& R, + bool transposeJ = true, + bool silent = false); +#endif + // This function name is long for a reason: + // - Only for DenseQR + // - Only for Transposed Jacobian QR decomposition + void identifyDependentGeometryParametersInTransposedJacobianDenseQRDecomposition( + const Eigen::FullPivHouseholderQR& qrJT, + const GCS::VEC_pD& pdiagnoselist, + int paramsNum, + int rank); + + template + void identifyConflictingRedundantConstraints(Algorithm alg, + const T& qrJT, + const std::map& jacobianconstraintmap, + const std::map& tagmultiplicity, + GCS::VEC_pD& pdiagnoselist, + Eigen::MatrixXd& R, + int constrNum, + int rank, + int& nonredundantconstrNum); + + void eliminateNonZerosOverPivotInUpperTriangularMatrix(Eigen::MatrixXd& R, int rank); + +#ifdef EIGEN_SPARSEQR_COMPATIBLE + void identifyDependentParametersSparseQR(const Eigen::MatrixXd& J, + const std::map& jacobianconstraintmap, + const GCS::VEC_pD& pdiagnoselist, + bool silent = true); +#endif + + void identifyDependentParametersDenseQR(const Eigen::MatrixXd& J, + const std::map& jacobianconstraintmap, + const GCS::VEC_pD& pdiagnoselist, + bool silent = true); + + template + void identifyDependentParameters(T& qrJ, + Eigen::MatrixXd& Rparams, + int rank, + const GCS::VEC_pD& pdiagnoselist, + bool silent = true); + +#ifdef _GCS_EXTRACT_SOLVER_SUBSYSTEM_ + void extractSubsystem(SubSystem* subsys, bool isRedundantsolving); +#endif +public: + int maxIter; + int maxIterRedundant; + bool sketchSizeMultiplier; // if true note that the total number of iterations allowed is + // MaxIterations *xLength + bool sketchSizeMultiplierRedundant; + double convergence; + double convergenceRedundant; + QRAlgorithm qrAlgorithm; + DogLegGaussStep dogLegGaussStep; + double qrpivotThreshold; + DebugMode debugMode; + double LM_eps; + double LM_eps1; + double LM_tau; + double DL_tolg; + double DL_tolx; + double DL_tolf; + double LM_epsRedundant; + double LM_eps1Redundant; + double LM_tauRedundant; + double DL_tolgRedundant; + double DL_tolxRedundant; + double DL_tolfRedundant; + +public: + System(); + /*System(std::vector clist_);*/ + ~System(); + + void clear(); + void clearByTag(int tagId, bool is_fixed_point = false); + int addConstraint(Constraint* constr); + void removeConstraint(Constraint* constr); + + // basic constraints + int addConstraintFixed(Point& p, int tagId = 0, bool driving = true); + + int addConstraintEqual( + double* param1, + double* param2, + int tagId = 0, + bool driving = true, + Constraint::Alignment internalalignment = Constraint::Alignment::NoInternalAlignment); + int addConstraintProportional(double* param1, + double* param2, + double ratio, + int tagId, + bool driving = true); + int addConstraintDifference(double* param1, + double* param2, + double* difference, + int tagId = 0, + bool driving = true); + int addConstraintP2PDistance(Point& p1, + Point& p2, + double* distance, + int tagId = 0, + bool driving = true); + int addConstraintP2PAngle(Point& p1, + Point& p2, + double* angle, + double incrAngle, + int tagId = 0, + bool driving = true); + int + addConstraintP2PAngle(Point& p1, Point& p2, double* angle, int tagId = 0, bool driving = true); + int addConstraintP2LDistance(Point& p, + Line& l, + double* distance, + int tagId = 0, + bool driving = true); + int addConstraintPointOnLine(Point& p, Line& l, int tagId = 0, bool driving = true); + int + addConstraintPointOnLine(Point& p, Point& lp1, Point& lp2, int tagId = 0, bool driving = true); + int addConstraintPointOnPerpBisector(Point& p, Line& l, int tagId = 0, bool driving = true); + int addConstraintPointOnPerpBisector(Point& p, + Point& lp1, + Point& lp2, + int tagId = 0, + bool driving = true); + int addConstraintParallel(Line& l1, Line& l2, int tagId = 0, bool driving = true); + int addConstraintPerpendicular(Line& l1, Line& l2, int tagId = 0, bool driving = true); + int addConstraintPerpendicular(Point& l1p1, + Point& l1p2, + Point& l2p1, + Point& l2p2, + int tagId = 0, + bool driving = true); + int + addConstraintL2LAngle(Line& l1, Line& l2, double* angle, int tagId = 0, bool driving = true); + int addConstraintL2LAngle(Point& l1p1, + Point& l1p2, + Point& l2p1, + Point& l2p2, + double* angle, + int tagId = 0, + bool driving = true); + int addConstraintAngleViaPoint(Curve& crv1, + Curve& crv2, + Point& p, + double* angle, + int tagId = 0, + bool driving = true); + int addConstraintAngleViaTwoPoints(Curve& crv1, + Curve& crv2, + Point& p1, + Point& p2, + double* angle, + int tagId = 0, + bool driving = true); + int addConstraintAngleViaPointAndParam(Curve& crv1, + Curve& crv2, + Point& p, + double* cparam, + double* angle, + int tagId = 0, + bool driving = true); + int addConstraintAngleViaPointAndTwoParams(Curve& crv1, + Curve& crv2, + Point& p, + double* cparam1, + double* cparam2, + double* angle, + int tagId = 0, + bool driving = true); + int addConstraintMidpointOnLine(Line& l1, Line& l2, int tagId = 0, bool driving = true); + int addConstraintMidpointOnLine(Point& l1p1, + Point& l1p2, + Point& l2p1, + Point& l2p2, + int tagId = 0, + bool driving = true); + int addConstraintTangentCircumf(Point& p1, + Point& p2, + double* rd1, + double* rd2, + bool internal = false, + int tagId = 0, + bool driving = true); + int addConstraintTangentAtBSplineKnot(BSpline& b, + Line& l, + unsigned int knotindex, + int tagId = 0, + bool driving = true); + + // derived constraints + int addConstraintP2PCoincident(Point& p1, Point& p2, int tagId = 0, bool driving = true); + int addConstraintHorizontal(Line& l, int tagId = 0, bool driving = true); + int addConstraintHorizontal(Point& p1, Point& p2, int tagId = 0, bool driving = true); + int addConstraintVertical(Line& l, int tagId = 0, bool driving = true); + int addConstraintVertical(Point& p1, Point& p2, int tagId = 0, bool driving = true); + int addConstraintCoordinateX(Point& p, double* x, int tagId = 0, bool driving = true); + int addConstraintCoordinateY(Point& p, double* y, int tagId = 0, bool driving = true); + int addConstraintArcRules(Arc& a, int tagId = 0, bool driving = true); + int addConstraintPointOnCircle(Point& p, Circle& c, int tagId = 0, bool driving = true); + int addConstraintPointOnEllipse(Point& p, Ellipse& e, int tagId = 0, bool driving = true); + int addConstraintPointOnHyperbolicArc(Point& p, + ArcOfHyperbola& e, + int tagId = 0, + bool driving = true); + int addConstraintPointOnParabolicArc(Point& p, + ArcOfParabola& e, + int tagId = 0, + bool driving = true); + int addConstraintPointOnBSpline(Point& p, + BSpline& b, + double* pointparam, + int tagId, + bool driving = true); + int addConstraintArcOfEllipseRules(ArcOfEllipse& a, int tagId = 0, bool driving = true); + int addConstraintCurveValue(Point& p, Curve& a, double* u, int tagId = 0, bool driving = true); + int addConstraintArcOfHyperbolaRules(ArcOfHyperbola& a, int tagId = 0, bool driving = true); + int addConstraintArcOfParabolaRules(ArcOfParabola& a, int tagId = 0, bool driving = true); + int addConstraintPointOnArc(Point& p, Arc& a, int tagId = 0, bool driving = true); + int addConstraintPerpendicularLine2Arc(Point& p1, + Point& p2, + Arc& a, + int tagId = 0, + bool driving = true); + int addConstraintPerpendicularArc2Line(Arc& a, + Point& p1, + Point& p2, + int tagId = 0, + bool driving = true); + int addConstraintPerpendicularCircle2Arc(Point& center, + double* radius, + Arc& a, + int tagId = 0, + bool driving = true); + int addConstraintPerpendicularArc2Circle(Arc& a, + Point& center, + double* radius, + int tagId = 0, + bool driving = true); + int addConstraintPerpendicularArc2Arc(Arc& a1, + bool reverse1, + Arc& a2, + bool reverse2, + int tagId = 0, + bool driving = true); + int addConstraintTangent(Line& l, Circle& c, int tagId = 0, bool driving = true); + int addConstraintTangent(Line& l, Ellipse& e, int tagId = 0, bool driving = true); + int addConstraintTangent(Line& l, Arc& a, int tagId = 0, bool driving = true); + int addConstraintTangent(Circle& c1, Circle& c2, int tagId = 0, bool driving = true); + int addConstraintTangent(Arc& a1, Arc& a2, int tagId = 0, bool driving = true); + int addConstraintTangent(Circle& c, Arc& a, int tagId = 0, bool driving = true); + + int addConstraintCircleRadius(Circle& c, double* radius, int tagId = 0, bool driving = true); + int addConstraintArcRadius(Arc& a, double* radius, int tagId = 0, bool driving = true); + int + addConstraintCircleDiameter(Circle& c, double* diameter, int tagId = 0, bool driving = true); + int addConstraintArcDiameter(Arc& a, double* diameter, int tagId = 0, bool driving = true); + int addConstraintEqualLength(Line& l1, Line& l2, int tagId = 0, bool driving = true); + int addConstraintEqualRadius(Circle& c1, Circle& c2, int tagId = 0, bool driving = true); + int addConstraintEqualRadii(Ellipse& e1, Ellipse& e2, int tagId = 0, bool driving = true); + int addConstraintEqualRadii(ArcOfHyperbola& a1, + ArcOfHyperbola& a2, + int tagId = 0, + bool driving = true); + int addConstraintEqualRadius(Circle& c1, Arc& a2, int tagId = 0, bool driving = true); + int addConstraintEqualRadius(Arc& a1, Arc& a2, int tagId = 0, bool driving = true); + int addConstraintEqualFocus(ArcOfParabola& a1, + ArcOfParabola& a2, + int tagId = 0, + bool driving = true); + int + addConstraintP2PSymmetric(Point& p1, Point& p2, Line& l, int tagId = 0, bool driving = true); + int + addConstraintP2PSymmetric(Point& p1, Point& p2, Point& p, int tagId = 0, bool driving = true); + int addConstraintSnellsLaw(Curve& ray1, + Curve& ray2, + Curve& boundary, + Point p, + double* n1, + double* n2, + bool flipn1, + bool flipn2, + int tagId, + bool driving = true); + + int + addConstraintC2CDistance(Circle& c1, Circle& c2, double* dist, int tagId, bool driving = true); + int addConstraintC2LDistance(Circle& c, Line& l, double* dist, int tagId, bool driving = true); + int addConstraintP2CDistance(Point& p, + Circle& c, + double* distance, + int tagId = 0, + bool driving = true); + int addConstraintArcLength(Arc& a, double* dist, int tagId, bool driving = true); + + // internal alignment constraints + int addConstraintInternalAlignmentPoint2Ellipse(Ellipse& e, + Point& p1, + InternalAlignmentType alignmentType, + int tagId = 0, + bool driving = true); + int addConstraintInternalAlignmentEllipseMajorDiameter(Ellipse& e, + Point& p1, + Point& p2, + int tagId = 0, + bool driving = true); + int addConstraintInternalAlignmentEllipseMinorDiameter(Ellipse& e, + Point& p1, + Point& p2, + int tagId = 0, + bool driving = true); + int addConstraintInternalAlignmentEllipseFocus1(Ellipse& e, + Point& p1, + int tagId = 0, + bool driving = true); + int addConstraintInternalAlignmentEllipseFocus2(Ellipse& e, + Point& p1, + int tagId = 0, + bool driving = true); + int addConstraintInternalAlignmentPoint2Hyperbola(Hyperbola& e, + Point& p1, + InternalAlignmentType alignmentType, + int tagId = 0, + bool driving = true); + int addConstraintInternalAlignmentHyperbolaMajorDiameter(Hyperbola& e, + Point& p1, + Point& p2, + int tagId = 0, + bool driving = true); + int addConstraintInternalAlignmentHyperbolaMinorDiameter(Hyperbola& e, + Point& p1, + Point& p2, + int tagId = 0, + bool driving = true); + int addConstraintInternalAlignmentHyperbolaFocus(Hyperbola& e, + Point& p1, + int tagId = 0, + bool driving = true); + int addConstraintInternalAlignmentParabolaFocus(Parabola& e, + Point& p1, + int tagId = 0, + bool driving = true); + int addConstraintInternalAlignmentBSplineControlPoint(BSpline& b, + Circle& c, + unsigned int poleindex, + int tag = 0, + bool driving = true); + int addConstraintInternalAlignmentKnotPoint(BSpline& b, + Point& p, + unsigned int knotindex, + int tagId = 0, + bool driving = true); + + double calculateAngleViaPoint(const Curve& crv1, const Curve& crv2, Point& p) const; + double calculateAngleViaPoint(const Curve& crv1, const Curve& crv2, Point& p1, Point& p2) const; + double calculateAngleViaParams(const Curve& crv1, + const Curve& crv2, + double* param1, + double* param2) const; + void calculateNormalAtPoint(const Curve& crv, const Point& p, double& rtnX, double& rtnY) const; + + // Calculates errors of all constraints which have a tag equal to + // the one supplied. Individual errors are summed up using RMS. + // If none are found, NAN is returned + // If there's only one, a signed value is returned. + // Effectively, it calculates the error of a UI constraint + double calculateConstraintErrorByTag(int tagId); + + void rescaleConstraint(int id, double coeff); + + void declareUnknowns(VEC_pD& params); + void declareDrivenParams(VEC_pD& params); + void initSolution(Algorithm alg = DogLeg); + + int solve(bool isFine = true, Algorithm alg = DogLeg, bool isRedundantsolving = false); + int solve(VEC_pD& params, + bool isFine = true, + Algorithm alg = DogLeg, + bool isRedundantsolving = false); + int solve(SubSystem* subsys, + bool isFine = true, + Algorithm alg = DogLeg, + bool isRedundantsolving = false); + int solve(SubSystem* subsysA, + SubSystem* subsysB, + bool isFine = true, + bool isRedundantsolving = false); + + void applySolution(); + void undoSolution(); + // FIXME: looks like XconvergenceFine is not the solver precision, at least in DogLeg + // solver. + // Note: Yes, every solver has a different way of interpreting precision + // but one has to study what is this needed for in order to decide + // what to return (this is unchanged from previous versions) + double getFinePrecision() + { + return convergence; + } + + int diagnose(Algorithm alg = DogLeg); + int dofsNumber() const + { + return hasDiagnosis ? dofs : -1; + } + void getConflicting(VEC_I& conflictingOut) const + { + conflictingOut = hasDiagnosis ? conflictingTags : VEC_I(0); + } + void getRedundant(VEC_I& redundantOut) const + { + redundantOut = hasDiagnosis ? redundantTags : VEC_I(0); + } + void getPartiallyRedundant(VEC_I& partiallyredundantOut) const + { + partiallyredundantOut = hasDiagnosis ? partiallyRedundantTags : VEC_I(0); + } + void getDependentParams(VEC_pD& pdependentparameterlist) const + { + pdependentparameterlist = pDependentParameters; + } + void + getDependentParamsGroups(std::vector>& pdependentparametergroups) const + { + pdependentparametergroups = pDependentParametersGroups; + } + bool isEmptyDiagnoseMatrix() const + { + return emptyDiagnoseMatrix; + } + + bool hasConflicting() const + { + return !(hasDiagnosis && conflictingTags.empty()); + } + bool hasRedundant() const + { + return !(hasDiagnosis && redundantTags.empty()); + } + bool hasPartiallyRedundant() const + { + return !(hasDiagnosis && partiallyRedundantTags.empty()); + } + + void invalidatedDiagnosis(); + + // Unit testing interface - not intended for use by production code +protected: + size_t _getNumberOfConstraints(int tagID = -1) + { + if (tagID < 0) { + return clist.size(); + } + return std::count_if(clist.begin(), clist.end(), [tagID](Constraint* constraint) { + return constraint->getTag() == tagID; + }); + } +}; + + +/////////////////////////////////////// +// Helper elements +/////////////////////////////////////// + +void deleteAllContent(VEC_pD& doublevec); +void deleteAllContent(std::vector& constrvec); +void deleteAllContent(std::vector& subsysvec); + +} // namespace GCS + +#endif // PLANEGCS_GCS_H diff --git a/GCS/Geo.cpp b/GCS/Geo.cpp new file mode 100644 index 0000000..cbb4e30 --- /dev/null +++ b/GCS/Geo.cpp @@ -0,0 +1,1183 @@ +/*************************************************************************** + * Copyright (c) 2014 Victor Titov (DeepSOIC) * + * * + * 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 * + * * + ***************************************************************************/ + +#define DEBUG_DERIVS 0 +#if DEBUG_DERIVS +#endif + +#include + +#include "Geo.h" + + +namespace GCS +{ + +//----------------Point +int Point::PushOwnParams(VEC_pD& pvec) +{ + int cnt = 0; + pvec.push_back(x); + cnt++; + pvec.push_back(y); + cnt++; + return cnt; +} + +void Point::ReconstructOnNewPvec(VEC_pD& pvec, int& cnt) +{ + x = pvec[cnt]; + cnt++; + y = pvec[cnt]; + cnt++; +} + +//----------------DeriVector2 +DeriVector2::DeriVector2(const Point& p, const double* derivparam) +{ + x = *p.x; + y = *p.y; + dx = 0.0; + dy = 0.0; + if (derivparam == p.x) { + dx = 1.0; + } + if (derivparam == p.y) { + dy = 1.0; + } +} + +double DeriVector2::length(double& dlength) const +{ + double l = length(); + if (l == 0) { + dlength = 1.0; + return l; + } + else { + dlength = (x * dx + y * dy) / l; + return l; + } +} + +DeriVector2 DeriVector2::getNormalized() const +{ + double l = length(); + if (l == 0.0) { + return DeriVector2(0, 0, dx, dy); + } + else { + DeriVector2 rtn; + rtn.x = x / l; + rtn.y = y / l; + // first, simply scale the derivative accordingly. + rtn.dx = dx / l; + rtn.dy = dy / l; + // next, remove the collinear part of dx,dy (make a projection onto a normal) + double dsc = rtn.dx * rtn.x + rtn.dy * rtn.y; // scalar product d*v + rtn.dx -= dsc * rtn.x; // subtract the projection + rtn.dy -= dsc * rtn.y; + return rtn; + } +} + +double DeriVector2::scalarProd(const DeriVector2& v2, double* dprd) const +{ + if (dprd) { + *dprd = dx * v2.x + x * v2.dx + dy * v2.y + y * v2.dy; + }; + return x * v2.x + y * v2.y; +} + +DeriVector2 DeriVector2::divD(double val, double dval) const +{ + return DeriVector2(x / val, + y / val, + dx / val - x * dval / (val * val), + dy / val - y * dval / (val * val)); +} + +double DeriVector2::crossProdNorm(const DeriVector2& v2, double& dprd) const +{ + dprd = dx * v2.y + x * v2.dy - dy * v2.x - y * v2.dx; + return x * v2.y - y * v2.x; +} + +DeriVector2 Curve::Value(double /*u*/, double /*du*/, const double* /*derivparam*/) const +{ + assert(false /*Value() is not implemented*/); + return DeriVector2(); +} + +//----------------Line + +DeriVector2 Line::CalculateNormal(const Point& p, const double* derivparam) const +{ + (void)p; + DeriVector2 p1v(p1, derivparam); + DeriVector2 p2v(p2, derivparam); + + return p2v.subtr(p1v).rotate90ccw(); +} + +DeriVector2 Line::Value(double u, double du, const double* derivparam) const +{ + DeriVector2 p1v(p1, derivparam); + DeriVector2 p2v(p2, derivparam); + + DeriVector2 line_vec = p2v.subtr(p1v); + return p1v.sum(line_vec.multD(u, du)); +} + +int Line::PushOwnParams(VEC_pD& pvec) +{ + int cnt = 0; + pvec.push_back(p1.x); + cnt++; + pvec.push_back(p1.y); + cnt++; + pvec.push_back(p2.x); + cnt++; + pvec.push_back(p2.y); + cnt++; + return cnt; +} +void Line::ReconstructOnNewPvec(VEC_pD& pvec, int& cnt) +{ + p1.x = pvec[cnt]; + cnt++; + p1.y = pvec[cnt]; + cnt++; + p2.x = pvec[cnt]; + cnt++; + p2.y = pvec[cnt]; + cnt++; +} +Line* Line::Copy() +{ + Line* crv = new Line(*this); + return crv; +} + + +//---------------circle + +DeriVector2 Circle::CalculateNormal(const Point& p, const double* derivparam) const +{ + DeriVector2 cv(center, derivparam); + DeriVector2 pv(p, derivparam); + + return cv.subtr(pv); +} + +DeriVector2 Circle::Value(double u, double du, const double* derivparam) const +{ + DeriVector2 cv(center, derivparam); + double r, dr; + r = *(this->rad); + dr = (derivparam == this->rad) ? 1.0 : 0.0; + DeriVector2 ex(r, 0.0, dr, 0.0); + DeriVector2 ey = ex.rotate90ccw(); + double si, dsi, co, dco; + si = std::sin(u); + dsi = du * std::cos(u); + co = std::cos(u); + dco = du * (-std::sin(u)); + return cv.sum(ex.multD(co, dco).sum(ey.multD(si, dsi))); +} + +int Circle::PushOwnParams(VEC_pD& pvec) +{ + int cnt = 0; + pvec.push_back(center.x); + cnt++; + pvec.push_back(center.y); + cnt++; + pvec.push_back(rad); + cnt++; + return cnt; +} +void Circle::ReconstructOnNewPvec(VEC_pD& pvec, int& cnt) +{ + center.x = pvec[cnt]; + cnt++; + center.y = pvec[cnt]; + cnt++; + rad = pvec[cnt]; + cnt++; +} +Circle* Circle::Copy() +{ + Circle* crv = new Circle(*this); + return crv; +} + +//------------arc +int Arc::PushOwnParams(VEC_pD& pvec) +{ + int cnt = 0; + cnt += Circle::PushOwnParams(pvec); + pvec.push_back(start.x); + cnt++; + pvec.push_back(start.y); + cnt++; + pvec.push_back(end.x); + cnt++; + pvec.push_back(end.y); + cnt++; + pvec.push_back(startAngle); + cnt++; + pvec.push_back(endAngle); + cnt++; + return cnt; +} +void Arc::ReconstructOnNewPvec(VEC_pD& pvec, int& cnt) +{ + Circle::ReconstructOnNewPvec(pvec, cnt); + start.x = pvec[cnt]; + cnt++; + start.y = pvec[cnt]; + cnt++; + end.x = pvec[cnt]; + cnt++; + end.y = pvec[cnt]; + cnt++; + startAngle = pvec[cnt]; + cnt++; + endAngle = pvec[cnt]; + cnt++; +} +Arc* Arc::Copy() +{ + Arc* crv = new Arc(*this); + return crv; +} + + +//--------------ellipse + +// this function is exposed to allow reusing pre-filled derivectors in constraints code +double Ellipse::getRadMaj(const DeriVector2& center, + const DeriVector2& f1, + double b, + double db, + double& ret_dRadMaj) const +{ + double cf, dcf; + cf = f1.subtr(center).length(dcf); + DeriVector2 hack( + b, + cf, + db, + dcf); // hack = a nonsense vector to calculate major radius with derivatives, useful just + // because the calculation formula is the same as vector length formula + return hack.length(ret_dRadMaj); +} + +// returns major radius. The derivative by derivparam is returned into ret_dRadMaj argument. +double Ellipse::getRadMaj(double* derivparam, double& ret_dRadMaj) const +{ + DeriVector2 c(center, derivparam); + DeriVector2 f1(focus1, derivparam); + return getRadMaj(c, f1, *radmin, radmin == derivparam ? 1.0 : 0.0, ret_dRadMaj); +} + +// returns the major radius (plain value, no derivatives) +double Ellipse::getRadMaj() const +{ + double dradmaj; // dummy + return getRadMaj(nullptr, dradmaj); +} + +DeriVector2 Ellipse::CalculateNormal(const Point& p, const double* derivparam) const +{ + // fill some vectors in + DeriVector2 cv(center, derivparam); + DeriVector2 f1v(focus1, derivparam); + DeriVector2 pv(p, derivparam); + + // calculation. + // focus2: + DeriVector2 f2v = cv.linCombi(2.0, f1v, -1.0); // 2*cv - f1v + + // pf1, pf2 = vectors from p to focus1,focus2 + DeriVector2 pf1 = f1v.subtr(pv); + DeriVector2 pf2 = f2v.subtr(pv); + // return sum of normalized pf2, pf2 + DeriVector2 ret = pf1.getNormalized().sum(pf2.getNormalized()); + + return ret; +} + +DeriVector2 Ellipse::Value(double u, double du, const double* derivparam) const +{ + // In local coordinate system, value() of ellipse is: + //(a*cos(u), b*sin(u)) + // In global, it is (vector formula): + // center + a_vec*cos(u) + b_vec*sin(u). + // That's what is being computed here. + + // + DeriVector2 c(this->center, derivparam); + DeriVector2 f1(this->focus1, derivparam); + + DeriVector2 emaj = f1.subtr(c).getNormalized(); + DeriVector2 emin = emaj.rotate90ccw(); + double b, db; + b = *(this->radmin); + db = this->radmin == derivparam ? 1.0 : 0.0; + double a, da; + a = this->getRadMaj(c, f1, b, db, da); + DeriVector2 a_vec = emaj.multD(a, da); + DeriVector2 b_vec = emin.multD(b, db); + // + + // sin, cos with derivatives: + double co, dco, si, dsi; + co = std::cos(u); + dco = -std::sin(u) * du; + si = std::sin(u); + dsi = std::cos(u) * du; + + DeriVector2 ret; // point of ellipse at parameter value of u, in global coordinates + ret = a_vec.multD(co, dco).sum(b_vec.multD(si, dsi)).sum(c); + return ret; +} + +int Ellipse::PushOwnParams(VEC_pD& pvec) +{ + int cnt = 0; + pvec.push_back(center.x); + cnt++; + pvec.push_back(center.y); + cnt++; + pvec.push_back(focus1.x); + cnt++; + pvec.push_back(focus1.y); + cnt++; + pvec.push_back(radmin); + cnt++; + return cnt; +} +void Ellipse::ReconstructOnNewPvec(VEC_pD& pvec, int& cnt) +{ + center.x = pvec[cnt]; + cnt++; + center.y = pvec[cnt]; + cnt++; + focus1.x = pvec[cnt]; + cnt++; + focus1.y = pvec[cnt]; + cnt++; + radmin = pvec[cnt]; + cnt++; +} +Ellipse* Ellipse::Copy() +{ + Ellipse* crv = new Ellipse(*this); + return crv; +} + + +//---------------arc of ellipse +int ArcOfEllipse::PushOwnParams(VEC_pD& pvec) +{ + int cnt = 0; + cnt += Ellipse::PushOwnParams(pvec); + pvec.push_back(start.x); + cnt++; + pvec.push_back(start.y); + cnt++; + pvec.push_back(end.x); + cnt++; + pvec.push_back(end.y); + cnt++; + pvec.push_back(startAngle); + cnt++; + pvec.push_back(endAngle); + cnt++; + return cnt; +} +void ArcOfEllipse::ReconstructOnNewPvec(VEC_pD& pvec, int& cnt) +{ + Ellipse::ReconstructOnNewPvec(pvec, cnt); + start.x = pvec[cnt]; + cnt++; + start.y = pvec[cnt]; + cnt++; + end.x = pvec[cnt]; + cnt++; + end.y = pvec[cnt]; + cnt++; + startAngle = pvec[cnt]; + cnt++; + endAngle = pvec[cnt]; + cnt++; +} +ArcOfEllipse* ArcOfEllipse::Copy() +{ + ArcOfEllipse* crv = new ArcOfEllipse(*this); + return crv; +} + +//---------------hyperbola + +// this function is exposed to allow reusing pre-filled derivectors in constraints code +double Hyperbola::getRadMaj(const DeriVector2& center, + const DeriVector2& f1, + double b, + double db, + double& ret_dRadMaj) const +{ + double cf, dcf; + cf = f1.subtr(center).length(dcf); + double a, da; + a = sqrt(cf * cf - b * b); + da = (dcf * cf - db * b) / a; + ret_dRadMaj = da; + return a; +} + +// returns major radius. The derivative by derivparam is returned into ret_dRadMaj argument. +double Hyperbola::getRadMaj(double* derivparam, double& ret_dRadMaj) const +{ + DeriVector2 c(center, derivparam); + DeriVector2 f1(focus1, derivparam); + return getRadMaj(c, f1, *radmin, radmin == derivparam ? 1.0 : 0.0, ret_dRadMaj); +} + +// returns the major radius (plain value, no derivatives) +double Hyperbola::getRadMaj() const +{ + double dradmaj; // dummy + return getRadMaj(nullptr, dradmaj); +} + +DeriVector2 Hyperbola::CalculateNormal(const Point& p, const double* derivparam) const +{ + // fill some vectors in + DeriVector2 cv(center, derivparam); + DeriVector2 f1v(focus1, derivparam); + DeriVector2 pv(p, derivparam); + + // calculation. + // focus2: + DeriVector2 f2v = cv.linCombi(2.0, f1v, -1.0); // 2*cv - f1v + + // pf1, pf2 = vectors from p to focus1,focus2 + DeriVector2 pf1 = f1v.subtr(pv).mult( + -1.0); // <--- differs from ellipse normal calculation code by inverting this vector + DeriVector2 pf2 = f2v.subtr(pv); + // return sum of normalized pf2, pf2 + DeriVector2 ret = pf1.getNormalized().sum(pf2.getNormalized()); + + return ret; +} + +DeriVector2 Hyperbola::Value(double u, double du, const double* derivparam) const +{ + + // In local coordinate system, value() of hyperbola is: + //(a*cosh(u), b*sinh(u)) + // In global, it is (vector formula): + // center + a_vec*cosh(u) + b_vec*sinh(u). + // That's what is being computed here. + + // + DeriVector2 c(this->center, derivparam); + DeriVector2 f1(this->focus1, derivparam); + + DeriVector2 emaj = f1.subtr(c).getNormalized(); + DeriVector2 emin = emaj.rotate90ccw(); + double b, db; + b = *(this->radmin); + db = this->radmin == derivparam ? 1.0 : 0.0; + double a, da; + a = this->getRadMaj(c, f1, b, db, da); + DeriVector2 a_vec = emaj.multD(a, da); + DeriVector2 b_vec = emin.multD(b, db); + // + + // sinh, cosh with derivatives: + double co, dco, si, dsi; + co = std::cosh(u); + dco = std::sinh(u) * du; + si = std::sinh(u); + dsi = std::cosh(u) * du; + + DeriVector2 ret; // point of hyperbola at parameter value of u, in global coordinates + ret = a_vec.multD(co, dco).sum(b_vec.multD(si, dsi)).sum(c); + return ret; +} + +int Hyperbola::PushOwnParams(VEC_pD& pvec) +{ + int cnt = 0; + pvec.push_back(center.x); + cnt++; + pvec.push_back(center.y); + cnt++; + pvec.push_back(focus1.x); + cnt++; + pvec.push_back(focus1.y); + cnt++; + pvec.push_back(radmin); + cnt++; + return cnt; +} +void Hyperbola::ReconstructOnNewPvec(VEC_pD& pvec, int& cnt) +{ + center.x = pvec[cnt]; + cnt++; + center.y = pvec[cnt]; + cnt++; + focus1.x = pvec[cnt]; + cnt++; + focus1.y = pvec[cnt]; + cnt++; + radmin = pvec[cnt]; + cnt++; +} +Hyperbola* Hyperbola::Copy() +{ + Hyperbola* crv = new Hyperbola(*this); + return crv; +} + +//--------------- arc of hyperbola +int ArcOfHyperbola::PushOwnParams(VEC_pD& pvec) +{ + int cnt = 0; + cnt += Hyperbola::PushOwnParams(pvec); + pvec.push_back(start.x); + cnt++; + pvec.push_back(start.y); + cnt++; + pvec.push_back(end.x); + cnt++; + pvec.push_back(end.y); + cnt++; + pvec.push_back(startAngle); + cnt++; + pvec.push_back(endAngle); + cnt++; + return cnt; +} +void ArcOfHyperbola::ReconstructOnNewPvec(VEC_pD& pvec, int& cnt) +{ + Hyperbola::ReconstructOnNewPvec(pvec, cnt); + start.x = pvec[cnt]; + cnt++; + start.y = pvec[cnt]; + cnt++; + end.x = pvec[cnt]; + cnt++; + end.y = pvec[cnt]; + cnt++; + startAngle = pvec[cnt]; + cnt++; + endAngle = pvec[cnt]; + cnt++; +} +ArcOfHyperbola* ArcOfHyperbola::Copy() +{ + ArcOfHyperbola* crv = new ArcOfHyperbola(*this); + return crv; +} + +//---------------parabola + +DeriVector2 Parabola::CalculateNormal(const Point& p, const double* derivparam) const +{ + // fill some vectors in + DeriVector2 cv(vertex, derivparam); + DeriVector2 f1v(focus1, derivparam); + DeriVector2 pv(p, derivparam); + + // the normal is the vector from the focus to the intersection of ano thru the point p and + // direction of the symmetry axis of the parabola with the directrix. As both point to directrix + // and point to focus are of equal magnitude, we can work with unitary vectors to calculate the + // normal, substraction of those vectors. + + DeriVector2 ret = cv.subtr(f1v).getNormalized().subtr(f1v.subtr(pv).getNormalized()); + + return ret; +} + +DeriVector2 Parabola::Value(double u, double du, const double* derivparam) const +{ + + // In local coordinate system, value() of parabola is: + // P(U) = O + U*U/(4.*F)*XDir + U*YDir + + DeriVector2 c(this->vertex, derivparam); + DeriVector2 f1(this->focus1, derivparam); + + DeriVector2 fv = f1.subtr(c); + + double f, df; + + f = fv.length(df); + + DeriVector2 xdir = fv.getNormalized(); + DeriVector2 ydir = xdir.rotate90ccw(); + + DeriVector2 dirx = xdir.multD(u, du).multD(u, du).divD(4 * f, 4 * df); + DeriVector2 diry = ydir.multD(u, du); + + DeriVector2 dir = dirx.sum(diry); + + DeriVector2 ret; // point of parabola at parameter value of u, in global coordinates + + ret = c.sum(dir); + + return ret; +} + +int Parabola::PushOwnParams(VEC_pD& pvec) +{ + int cnt = 0; + pvec.push_back(vertex.x); + cnt++; + pvec.push_back(vertex.y); + cnt++; + pvec.push_back(focus1.x); + cnt++; + pvec.push_back(focus1.y); + cnt++; + return cnt; +} + +void Parabola::ReconstructOnNewPvec(VEC_pD& pvec, int& cnt) +{ + vertex.x = pvec[cnt]; + cnt++; + vertex.y = pvec[cnt]; + cnt++; + focus1.x = pvec[cnt]; + cnt++; + focus1.y = pvec[cnt]; + cnt++; +} + +Parabola* Parabola::Copy() +{ + Parabola* crv = new Parabola(*this); + return crv; +} + +//--------------- arc of hyperbola +int ArcOfParabola::PushOwnParams(VEC_pD& pvec) +{ + int cnt = 0; + cnt += Parabola::PushOwnParams(pvec); + pvec.push_back(start.x); + cnt++; + pvec.push_back(start.y); + cnt++; + pvec.push_back(end.x); + cnt++; + pvec.push_back(end.y); + cnt++; + pvec.push_back(startAngle); + cnt++; + pvec.push_back(endAngle); + cnt++; + return cnt; +} +void ArcOfParabola::ReconstructOnNewPvec(VEC_pD& pvec, int& cnt) +{ + Parabola::ReconstructOnNewPvec(pvec, cnt); + start.x = pvec[cnt]; + cnt++; + start.y = pvec[cnt]; + cnt++; + end.x = pvec[cnt]; + cnt++; + end.y = pvec[cnt]; + cnt++; + startAngle = pvec[cnt]; + cnt++; + endAngle = pvec[cnt]; + cnt++; +} +ArcOfParabola* ArcOfParabola::Copy() +{ + ArcOfParabola* crv = new ArcOfParabola(*this); + return crv; +} + +// bspline +DeriVector2 BSpline::CalculateNormal(const Point& p, const double* derivparam) const +{ + // place holder + DeriVector2 ret; + + // even if this method is call CalculateNormal, the returned vector is not the normal strictu + // sensus but a normal vector, where the vector should point to the left when one walks along + // the curve from start to end. + // + // https://forum.freecad.org/viewtopic.php?f=10&t=26312#p209486 + + if (mult[0] > degree && mult[mult.size() - 1] > degree) { + // if endpoints through end poles + if (*p.x == *start.x && *p.y == *start.y) { + // and you are asking about the normal at start point + // then tangency is defined by first to second poles + DeriVector2 endpt(this->poles[1], derivparam); + DeriVector2 spt(this->poles[0], derivparam); + + DeriVector2 tg = endpt.subtr(spt); + ret = tg.rotate90ccw(); + } + else if (*p.x == *end.x && *p.y == *end.y) { + // and you are asking about the normal at end point + // then tangency is defined by last to last but one poles + DeriVector2 endpt(this->poles[poles.size() - 1], derivparam); + DeriVector2 spt(this->poles[poles.size() - 2], derivparam); + + DeriVector2 tg = endpt.subtr(spt); + ret = tg.rotate90ccw(); + } + else { + // another point and we have no clue until we implement De Boor + ret = DeriVector2(); + } + } + else { + // either periodic or abnormal endpoint multiplicity, we have no clue so currently + // unsupported + ret = DeriVector2(); + } + + + return ret; +} + +DeriVector2 BSpline::CalculateNormal(const double* param, const double* derivparam) const +{ + // TODO: is there any advantage in making this a `static`? + size_t startpole = 0; + for (size_t j = 1; j < mult.size() && *(knots[j]) <= *param; ++j) { + startpole += mult[j]; + } + if (!periodic && startpole >= poles.size()) { + startpole = poles.size() - degree - 1; + } + + auto polexat = [&](size_t i) { + return poles[(startpole + i) % poles.size()].x; + }; + auto poleyat = [&](size_t i) { + return poles[(startpole + i) % poles.size()].y; + }; + auto weightat = [&](size_t i) { + return weights[(startpole + i) % weights.size()]; + }; + + double xsum, xslopesum; + double ysum, yslopesum; + double wsum, wslopesum; + + valueHomogenous(*param, &xsum, &ysum, &wsum, &xslopesum, &yslopesum, &wslopesum); + + // Tangent vector + // This should in principle be identical to error gradient wrt curve parameter in + // point-on-object + DeriVector2 result(wsum * xslopesum - wslopesum * xsum, wsum * yslopesum - wslopesum * ysum); + + size_t numpoints = degree + 1; + + // FIXME: Find an appropriate name for this method + auto doSomething = [&](size_t i, double& factor, double& slopefactor) { + VEC_D d(numpoints); + d[i] = 1; + factor = BSpline::splineValue(*param, startpole + degree, degree, d, flattenedknots); + VEC_D sd(numpoints - 1); + if (i > 0) { + sd[i - 1] = + 1.0 / (flattenedknots[startpole + i + degree] - flattenedknots[startpole + i]); + } + if (i < numpoints - 1) { + sd[i] = -1.0 + / (flattenedknots[startpole + i + 1 + degree] - flattenedknots[startpole + i + 1]); + } + slopefactor = + BSpline::splineValue(*param, startpole + degree, degree - 1, sd, flattenedknots); + }; + + // get dx, dy of the normal as well + for (size_t i = 0; i < numpoints; ++i) { + if (derivparam == polexat(i)) { + double factor, slopefactor; + doSomething(i, factor, slopefactor); + result.dx = *weightat(i) * (wsum * slopefactor - wslopesum * factor); + break; + } + if (derivparam == poleyat(i)) { + double factor, slopefactor; + doSomething(i, factor, slopefactor); + result.dy = *weightat(i) * (wsum * slopefactor - wslopesum * factor); + break; + } + if (derivparam == weightat(i)) { + double factor, slopefactor; + doSomething(i, factor, slopefactor); + result.dx = degree + * (factor * (xslopesum - wslopesum * (*polexat(i))) + - slopefactor * (xsum - wsum * (*polexat(i)))); + result.dy = degree + * (factor * (yslopesum - wslopesum * (*poleyat(i))) + - slopefactor * (ysum - wsum * (*poleyat(i)))); + break; + } + } + + // the curve parameter being used by the constraint is not known to the geometry (there can be + // many tangent constraints on the same curve after all). Assume that this is the param + // provided. + if (derivparam != param) { + return result.rotate90ccw(); + } + + // derivparam == param now. Done this way just to reduce "cognitive complexity". + VEC_D sd(numpoints - 1), ssd(numpoints - 2); + for (size_t i = 1; i < numpoints; ++i) { + sd[i - 1] = (*weightat(i) - *weightat(i - 1)) + / (flattenedknots[startpole + i + degree] - flattenedknots[startpole + i]); + } + for (size_t i = 1; i < numpoints - 1; ++i) { + ssd[i - 1] = (sd[i] - sd[i - 1]) + / (flattenedknots[startpole + i + degree] - flattenedknots[startpole + i]); + } + double wslopeslopesum = degree * (degree - 1) + * BSpline::splineValue(*param, startpole + degree, degree - 2, ssd, flattenedknots); + + for (size_t i = 1; i < numpoints; ++i) { + sd[i - 1] = (*polexat(i) * *weightat(i) - *polexat(i - 1) * *weightat(i - 1)) + / (flattenedknots[startpole + i + degree] - flattenedknots[startpole + i]); + } + for (size_t i = 1; i < numpoints - 1; ++i) { + ssd[i - 1] = (sd[i] - sd[i - 1]) + / (flattenedknots[startpole + i + degree] - flattenedknots[startpole + i]); + } + double xslopeslopesum = degree * (degree - 1) + * BSpline::splineValue(*param, startpole + degree, degree - 2, ssd, flattenedknots); + + for (size_t i = 1; i < numpoints; ++i) { + sd[i - 1] = (*poleyat(i) * *weightat(i) - *poleyat(i - 1) * *weightat(i - 1)) + / (flattenedknots[startpole + i + degree] - flattenedknots[startpole + i]); + } + for (size_t i = 1; i < numpoints - 1; ++i) { + ssd[i - 1] = (sd[i] - sd[i - 1]) + / (flattenedknots[startpole + i + degree] - flattenedknots[startpole + i]); + } + double yslopeslopesum = degree * (degree - 1) + * BSpline::splineValue(*param, startpole + degree, degree - 2, ssd, flattenedknots); + + result.dx = wsum * xslopeslopesum - wslopeslopesum * xsum; + result.dy = wsum * yslopeslopesum - wslopeslopesum * ysum; + + return result.rotate90ccw(); +} + +DeriVector2 BSpline::Value(double u, double /*du*/, const double* /*derivparam*/) const +{ + // TODO: is there any advantage in making this a `static`? + size_t startpole = 0; + for (size_t j = 1; j < mult.size() && *(knots[j]) <= u; ++j) { + startpole += mult[j]; + } + if (!periodic && startpole >= poles.size()) { + startpole = poles.size() - degree - 1; + } + + // double xsum = 0., xslopesum = 0.; + // double ysum = 0., yslopesum = 0.; + // double wsum = 0., wslopesum = 0.; + + auto polexat = [&](size_t i) { + return poles[(startpole + i) % poles.size()].x; + }; + auto poleyat = [&](size_t i) { + return poles[(startpole + i) % poles.size()].y; + }; + auto weightat = [&](size_t i) { + return weights[(startpole + i) % weights.size()]; + }; + + size_t numpoints = degree + 1; + // Tangent vector + // This should in principle be identical to error gradient wrt curve parameter in + // point-on-object + VEC_D d(numpoints); + for (size_t i = 0; i < numpoints; ++i) { + d[i] = *polexat(i) * *weightat(i); + } + double xsum = BSpline::splineValue(u, startpole + degree, degree, d, flattenedknots); + for (size_t i = 0; i < numpoints; ++i) { + d[i] = *poleyat(i) * *weightat(i); + } + double ysum = BSpline::splineValue(u, startpole + degree, degree, d, flattenedknots); + for (size_t i = 0; i < numpoints; ++i) { + d[i] = *weightat(i); + } + double wsum = BSpline::splineValue(u, startpole + degree, degree, d, flattenedknots); + + d.resize(numpoints - 1); + for (size_t i = 1; i < numpoints; ++i) { + d[i - 1] = (*polexat(i) * *weightat(i) - *polexat(i - 1) * *weightat(i - 1)) + / (flattenedknots[startpole + i + degree] - flattenedknots[startpole + i]); + } + double xslopesum = + degree * BSpline::splineValue(u, startpole + degree, degree - 1, d, flattenedknots); + for (size_t i = 1; i < numpoints; ++i) { + d[i - 1] = (*poleyat(i) * *weightat(i) - *poleyat(i - 1) * *weightat(i - 1)) + / (flattenedknots[startpole + i + degree] - flattenedknots[startpole + i]); + } + double yslopesum = + degree * BSpline::splineValue(u, startpole + degree, degree - 1, d, flattenedknots); + for (size_t i = 1; i < numpoints; ++i) { + d[i - 1] = (*weightat(i) - *weightat(i - 1)) + / (flattenedknots[startpole + i + degree] - flattenedknots[startpole + i]); + } + double wslopesum = + degree * BSpline::splineValue(u, startpole + degree, degree - 1, d, flattenedknots); + + // place holder + DeriVector2 ret = DeriVector2(xsum / wsum, + ysum / wsum, + (wsum * xslopesum - wslopesum * xsum) / wsum / wsum, + (wsum * yslopesum - wslopesum * ysum) / wsum / wsum); + + return ret; +} + +void BSpline::valueHomogenous(const double u, + double* xw, + double* yw, + double* w, + double* dxwdu, + double* dywdu, + double* dwdu) const +{ + // TODO: is there any advantage in making this a `static`? + size_t startpole = 0; + for (size_t j = 1; j < mult.size() && *(knots[j]) <= u; ++j) { + startpole += mult[j]; + } + if (!periodic && startpole >= poles.size()) { + startpole = poles.size() - degree - 1; + } + + auto polexat = [&](size_t i) { + return poles[(startpole + i) % poles.size()].x; + }; + auto poleyat = [&](size_t i) { + return poles[(startpole + i) % poles.size()].y; + }; + auto weightat = [&](size_t i) { + return weights[(startpole + i) % weights.size()]; + }; + + size_t numpoints = degree + 1; + VEC_D d(numpoints); + for (size_t i = 0; i < numpoints; ++i) { + d[i] = *polexat(i) * *weightat(i); + } + *xw = BSpline::splineValue(u, startpole + degree, degree, d, flattenedknots); + for (size_t i = 0; i < numpoints; ++i) { + d[i] = *poleyat(i) * *weightat(i); + } + *yw = BSpline::splineValue(u, startpole + degree, degree, d, flattenedknots); + for (size_t i = 0; i < numpoints; ++i) { + d[i] = *weightat(i); + } + *w = BSpline::splineValue(u, startpole + degree, degree, d, flattenedknots); + + d.resize(numpoints - 1); + for (size_t i = 1; i < numpoints; ++i) { + d[i - 1] = (*polexat(i) * *weightat(i) - *polexat(i - 1) * *weightat(i - 1)) + / (flattenedknots[startpole + i + degree] - flattenedknots[startpole + i]); + } + *dxwdu = degree * BSpline::splineValue(u, startpole + degree, degree - 1, d, flattenedknots); + for (size_t i = 1; i < numpoints; ++i) { + d[i - 1] = (*poleyat(i) * *weightat(i) - *poleyat(i - 1) * *weightat(i - 1)) + / (flattenedknots[startpole + i + degree] - flattenedknots[startpole + i]); + } + *dywdu = degree * BSpline::splineValue(u, startpole + degree, degree - 1, d, flattenedknots); + for (size_t i = 1; i < numpoints; ++i) { + d[i - 1] = (*weightat(i) - *weightat(i - 1)) + / (flattenedknots[startpole + i + degree] - flattenedknots[startpole + i]); + } + *dwdu = degree * BSpline::splineValue(u, startpole + degree, degree - 1, d, flattenedknots); +} + +int BSpline::PushOwnParams(VEC_pD& pvec) +{ + std::size_t cnt = 0; + + for (VEC_P::const_iterator it = poles.begin(); it != poles.end(); ++it) { + pvec.push_back((*it).x); + pvec.push_back((*it).y); + } + + cnt = cnt + poles.size() * 2; + + pvec.insert(pvec.end(), weights.begin(), weights.end()); + cnt = cnt + weights.size(); + + pvec.insert(pvec.end(), knots.begin(), knots.end()); + cnt = cnt + knots.size(); + + pvec.push_back(start.x); + cnt++; + pvec.push_back(start.y); + cnt++; + pvec.push_back(end.x); + cnt++; + pvec.push_back(end.y); + cnt++; + + return static_cast(cnt); +} + +void BSpline::ReconstructOnNewPvec(VEC_pD& pvec, int& cnt) +{ + for (VEC_P::iterator it = poles.begin(); it != poles.end(); ++it) { + (*it).x = pvec[cnt]; + cnt++; + (*it).y = pvec[cnt]; + cnt++; + } + + for (VEC_pD::iterator it = weights.begin(); it != weights.end(); ++it) { + (*it) = pvec[cnt]; + cnt++; + } + + for (VEC_pD::iterator it = knots.begin(); it != knots.end(); ++it) { + (*it) = pvec[cnt]; + cnt++; + } + + start.x = pvec[cnt]; + cnt++; + start.y = pvec[cnt]; + cnt++; + end.x = pvec[cnt]; + cnt++; + end.y = pvec[cnt]; + cnt++; +} + +BSpline* BSpline::Copy() +{ + BSpline* crv = new BSpline(*this); + return crv; +} + +double BSpline::getLinCombFactor(double x, size_t k, size_t i, unsigned int p) +{ + // Adapted to C++ from the python implementation in the Wikipedia page for de Boor algorithm + // https://en.wikipedia.org/wiki/De_Boor%27s_algorithm. + + // FIXME: This should probably be guaranteed by now, and done somewhere else + // To elaborate: `flattenedknots` should be set up as soon as `knots` + // and `mult` have been defined after creating the B-spline. + // However, in the future the values of `knots` could go into the solver + // as well, when alternatives may be needed to keep `flattenedknots` updated. + // Slightly more detailed discussion here: + // https://github.com/FreeCAD/FreeCAD/pull/7484#discussion_r1020858392 + if (flattenedknots.empty()) { + setupFlattenedKnots(); + } + + std::vector d(p + 1, 0.0); + // Ensure this is within range + int idxOfPole = static_cast(i) + p - static_cast(k); + if (idxOfPole < 0 || idxOfPole > static_cast(p)) { + return 0.0; + } + d[idxOfPole] = 1.0; + + for (size_t r = 1; r < p + 1; ++r) { + for (size_t j = p; j > r - 1; --j) { + double alpha = (x - flattenedknots[j + k - p]) + / (flattenedknots[j + 1 + k - r] - flattenedknots[j + k - p]); + d[j] = (1.0 - alpha) * d[j - 1] + alpha * d[j]; + } + } + + return d[p]; +} + +double BSpline::splineValue(double x, size_t k, unsigned int p, VEC_D& d, const VEC_D& flatknots) +{ + for (size_t r = 1; r < p + 1; ++r) { + for (size_t j = p; j > r - 1; --j) { + double alpha = + (x - flatknots[j + k - p]) / (flatknots[j + 1 + k - r] - flatknots[j + k - p]); + d[j] = (1.0 - alpha) * d[j - 1] + alpha * d[j]; + } + } + + return p < d.size() ? d[p] : 0.0; +} + +void BSpline::setupFlattenedKnots() +{ + flattenedknots.clear(); + + for (size_t i = 0; i < knots.size(); ++i) { + flattenedknots.insert(flattenedknots.end(), mult[i], *knots[i]); + } + + // Adjust for periodic: see OCC documentation for explanation + if (periodic) { + double period = *knots.back() - *knots.front(); + int c = degree + 1 - mult[0]; // number of knots to pad + + // Add capacity so that iterators remain valid + flattenedknots.reserve(flattenedknots.size() + 2 * c); + + // get iterators first for convenience + auto frontStart = flattenedknots.end() - mult.back() - c; + auto frontEnd = flattenedknots.end() - mult.back(); + auto backStart = flattenedknots.begin() + mult.front(); + auto backEnd = flattenedknots.begin() + mult.front() + c; + + // creating new vectors because above iterators can become invalidated upon insert + std::vector frontNew(frontStart, frontEnd); + std::vector backNew(backStart, backEnd); + + flattenedknots.insert(flattenedknots.end(), backNew.begin(), backNew.end()); + flattenedknots.insert(flattenedknots.begin(), frontNew.begin(), frontNew.end()); + + for (int i = 0; i < c; ++i) { + *(flattenedknots.begin() + i) -= period; + *(flattenedknots.end() - 1 - i) += period; + } + } +} + +} // namespace GCS diff --git a/GCS/Geo.h b/GCS/Geo.h new file mode 100644 index 0000000..33c09f0 --- /dev/null +++ b/GCS/Geo.h @@ -0,0 +1,492 @@ +/*************************************************************************** + * Copyright (c) 2011 Konstantinos Poulios * + * * + * 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 * + * * + ***************************************************************************/ + +#ifndef PLANEGCS_GEO_H +#define PLANEGCS_GEO_H + +#include "Util.h" +#include + +#ifdef _MSC_VER +#pragma warning(disable : 4251) +#endif + +namespace GCS +{ +class Point +{ +public: + Point() + { + x = nullptr; + y = nullptr; + } + + Point(double* px, double* py) + { + x = px; + y = py; + } + + Point(double* px, double* py, int _tag) + { + x = px; + y = py; + tag = _tag; + } + + double* x; + double* y; + + int PushOwnParams(VEC_pD& pvec); + + void ReconstructOnNewPvec(VEC_pD& pvec, int& cnt); + + double get_X() const { + return *(this->x); + } + + double get_Y() const { + return *(this->y); + } + +private: + int tag; +}; + +using VEC_P = std::vector; + +/// Class DeriVector2 holds a vector value and its derivative on the +/// parameter that the derivatives are being calculated for now. x,y is the +/// actual vector (v). dx,dy is a derivative of the vector by a parameter +///(dv/dp). The easiest way to fill the vector in is by passing a point and +/// a derivative parameter pointer to its constructor. x,y are read from the +/// pointers in Point, and dx,dy are set to either 0 or 1 depending on what +/// pointers of Point match the supplied pointer. The derivatives can be set +/// manually as well. The class also provides a bunch of methods to do math +/// on it (and derivatives are calculated implicitly). +/// +class DeriVector2 +{ +public: + DeriVector2() + { + x = 0; + y = 0; + dx = 0; + dy = 0; + } + DeriVector2(double x, double y) + { + this->x = x; + this->y = y; + this->dx = 0; + this->dy = 0; + } + DeriVector2(double x, double y, double dx, double dy) + { + this->x = x; + this->y = y; + this->dx = dx; + this->dy = dy; + } + DeriVector2(const Point& p, const double* derivparam); + double x, dx; + double y, dy; + + double length() const + { + return sqrt(x * x + y * y); + } + // returns length and writes length deriv into the dlength argument. + double length(double& dlength) const; + + // unlike other vectors in FreeCAD, this normalization creates a new vector instead of + // modifying existing one. + DeriVector2 getNormalized() const; // returns zero vector if the original is zero. + // calculates scalar product of two vectors and returns the result. The derivative + // of the result is written into argument dprd. + double scalarProd(const DeriVector2& v2, double* dprd = nullptr) const; + // calculates the norm of the cross product of the two vectors. + // DeriVector2 are considered as 3d vectors with null z. The derivative + // of the result is written into argument dprd. + double crossProdNorm(const DeriVector2& v2, double& dprd) const; + DeriVector2 sum(const DeriVector2& v2) const + { // adds two vectors and returns result + return DeriVector2(x + v2.x, y + v2.y, dx + v2.dx, dy + v2.dy); + } + DeriVector2 subtr(const DeriVector2& v2) const + { // subtracts two vectors and returns result + return DeriVector2(x - v2.x, y - v2.y, dx - v2.dx, dy - v2.dy); + } + DeriVector2 mult(double val) const + { + return DeriVector2(x * val, y * val, dx * val, dy * val); + } // multiplies the vector by a number. Derivatives are scaled. + DeriVector2 multD(double val, double dval) const + { // multiply vector by a variable with a derivative. + return DeriVector2(x * val, y * val, dx * val + x * dval, dy * val + y * dval); + } + // divide vector by a variable with a derivative + DeriVector2 divD(double val, double dval) const; + DeriVector2 rotate90ccw() const + { + return DeriVector2(-y, x, -dy, dx); + } + DeriVector2 rotate90cw() const + { + return DeriVector2(y, -x, dy, -dx); + } + DeriVector2 linCombi(double m1, const DeriVector2& v2, double m2) const + { // linear combination of two vectors + return DeriVector2(x * m1 + v2.x * m2, + y * m1 + v2.y * m2, + dx * m1 + v2.dx * m2, + dy * m1 + v2.dy * m2); + } +}; + +/////////////////////////////////////// +// Geometries +/////////////////////////////////////// + +/// A base class for all curve-based objects (line, circle/arc, ellipse/arc). +class Curve +{ +public: + virtual ~Curve() + {} + // returns normal vector. The vector should point to the left when one + // walks along the curve from start to end. Ellipses and circles are + // assumed to be walked counterclockwise, so the vector should point + // into the shape. + // derivparam is a pointer to a curve parameter (or point coordinate) to + // compute the derivative for. The derivative is returned through dx,dy + // fields of DeriVector2. + virtual DeriVector2 CalculateNormal(const Point& p, + const double* derivparam = nullptr) const = 0; + + // returns normal vector at parameter instead of at the given point. + virtual DeriVector2 CalculateNormal(const double* param, + const double* derivparam = nullptr) const + { + DeriVector2 pointDV = Value(*param, 0.0); + Point p(&pointDV.x, &pointDV.y); + return CalculateNormal(p, derivparam); + } + + /** + * @brief Value: returns point (vector) given the value of parameter + * @param u: value of parameter + * @param du: derivative of parameter by derivparam + * @param derivparam: pointer to sketch parameter to calculate the derivative for + * @return + */ + virtual DeriVector2 Value(double u, double du, const double* derivparam = nullptr) const; + + // adds curve's parameters to pvec (used by constraints) + virtual int PushOwnParams(VEC_pD& pvec) = 0; + // recunstruct curve's parameters reading them from pvec starting from index cnt. + // cnt will be incremented by the same value as returned by PushOwnParams() + virtual void ReconstructOnNewPvec(VEC_pD& pvec, int& cnt) = 0; + // DeepSOIC: I haven't found a way to simply copy a curve object provided pointer to a curve + // object. + virtual Curve* Copy() = 0; +}; + +class Line: public Curve +{ +public: + Line() + {} + Line(Point _p1, Point _p2) { + p1 = _p1; + p2 = _p2; + } + ~Line() override + {} + Point p1; + Point p2; + DeriVector2 CalculateNormal(const Point& p, const double* derivparam = nullptr) const override; + DeriVector2 Value(double u, double du, const double* derivparam = nullptr) const override; + int PushOwnParams(VEC_pD& pvec) override; + void ReconstructOnNewPvec(VEC_pD& pvec, int& cnt) override; + Line* Copy() override; +}; + +class Circle: public Curve +{ +public: + Circle() + { + rad = nullptr; + } + ~Circle() override + {} + Point center; + double* rad; + DeriVector2 CalculateNormal(const Point& p, const double* derivparam = nullptr) const override; + DeriVector2 Value(double u, double du, const double* derivparam = nullptr) const override; + int PushOwnParams(VEC_pD& pvec) override; + void ReconstructOnNewPvec(VEC_pD& pvec, int& cnt) override; + Circle* Copy() override; +}; + +class Arc: public Circle +{ +public: + Arc() + { + startAngle = nullptr; + endAngle = nullptr; + rad = nullptr; + } + ~Arc() override + {} + double* startAngle; + double* endAngle; + // double *rad; //inherited + // start and end points are computed by an ArcRules constraint + Point start; + Point end; + // Point center; //inherited + int PushOwnParams(VEC_pD& pvec) override; + void ReconstructOnNewPvec(VEC_pD& pvec, int& cnt) override; + Arc* Copy() override; +}; + +class MajorRadiusConic: public Curve +{ +public: + ~MajorRadiusConic() override + {} + virtual double getRadMaj(const DeriVector2& center, + const DeriVector2& f1, + double b, + double db, + double& ret_dRadMaj) const = 0; + virtual double getRadMaj(double* derivparam, double& ret_dRadMaj) const = 0; + virtual double getRadMaj() const = 0; + // DeriVector2 CalculateNormal(Point &p, double* derivparam = 0) = 0; +}; + +class Ellipse: public MajorRadiusConic +{ +public: + Ellipse() + { + radmin = nullptr; + } + ~Ellipse() override + {} + Point center; + Point focus1; + double* radmin; + double getRadMaj(const DeriVector2& center, + const DeriVector2& f1, + double b, + double db, + double& ret_dRadMaj) const override; + double getRadMaj(double* derivparam, double& ret_dRadMaj) const override; + double getRadMaj() const override; + DeriVector2 CalculateNormal(const Point& p, const double* derivparam = nullptr) const override; + DeriVector2 Value(double u, double du, const double* derivparam = nullptr) const override; + int PushOwnParams(VEC_pD& pvec) override; + void ReconstructOnNewPvec(VEC_pD& pvec, int& cnt) override; + Ellipse* Copy() override; +}; + +class ArcOfEllipse: public Ellipse +{ +public: + ArcOfEllipse() + { + startAngle = nullptr; + endAngle = nullptr; + radmin = nullptr; + } + ~ArcOfEllipse() override + {} + double* startAngle; + double* endAngle; + // double *radmin; //inherited + Point start; + Point end; + // Point center; //inherited + // double *focus1.x; //inherited + // double *focus1.y; //inherited + int PushOwnParams(VEC_pD& pvec) override; + void ReconstructOnNewPvec(VEC_pD& pvec, int& cnt) override; + ArcOfEllipse* Copy() override; +}; + +class Hyperbola: public MajorRadiusConic +{ +public: + Hyperbola() + { + radmin = nullptr; + } + ~Hyperbola() override + {} + Point center; + Point focus1; + double* radmin; + double getRadMaj(const DeriVector2& center, + const DeriVector2& f1, + double b, + double db, + double& ret_dRadMaj) const override; + double getRadMaj(double* derivparam, double& ret_dRadMaj) const override; + double getRadMaj() const override; + DeriVector2 CalculateNormal(const Point& p, const double* derivparam = nullptr) const override; + DeriVector2 Value(double u, double du, const double* derivparam = nullptr) const override; + int PushOwnParams(VEC_pD& pvec) override; + void ReconstructOnNewPvec(VEC_pD& pvec, int& cnt) override; + Hyperbola* Copy() override; +}; + +class ArcOfHyperbola: public Hyperbola +{ +public: + ArcOfHyperbola() + { + startAngle = nullptr; + endAngle = nullptr; + radmin = nullptr; + } + ~ArcOfHyperbola() override + {} + // parameters + double* startAngle; + double* endAngle; + Point start; + Point end; + // interface helpers + int PushOwnParams(VEC_pD& pvec) override; + void ReconstructOnNewPvec(VEC_pD& pvec, int& cnt) override; + ArcOfHyperbola* Copy() override; +}; + +class Parabola: public Curve +{ +public: + Parabola() + {} + ~Parabola() override + {} + Point vertex; + Point focus1; + DeriVector2 CalculateNormal(const Point& p, const double* derivparam = nullptr) const override; + DeriVector2 Value(double u, double du, const double* derivparam = nullptr) const override; + int PushOwnParams(VEC_pD& pvec) override; + void ReconstructOnNewPvec(VEC_pD& pvec, int& cnt) override; + Parabola* Copy() override; +}; + +class ArcOfParabola: public Parabola +{ +public: + ArcOfParabola() + { + startAngle = nullptr; + endAngle = nullptr; + } + ~ArcOfParabola() override + {} + // parameters + double* startAngle; + double* endAngle; + Point start; + Point end; + // interface helpers + int PushOwnParams(VEC_pD& pvec) override; + void ReconstructOnNewPvec(VEC_pD& pvec, int& cnt) override; + ArcOfParabola* Copy() override; +}; + +class BSpline: public Curve +{ +public: + BSpline() + { + periodic = false; + degree = 2; + } + ~BSpline() override + {} + // parameters + VEC_P poles; // TODO: use better data structures so poles.x and poles.y + VEC_pD weights; + VEC_pD knots; + // dependent parameters (depends on previous parameters, + // but an "arcrules" constraint alike would be required to gain the commodity of simple + // coincident with endpoint constraints) + Point start; + Point end; + // not solver parameters + VEC_I mult; + int degree; + bool periodic; + VEC_I knotpointGeoids; // geoids of knotpoints as to index Geom array + // knot vector with repetitions for multiplicity and "padding" for periodic spline + // interface helpers + VEC_D flattenedknots; + DeriVector2 CalculateNormal(const Point& p, const double* derivparam = nullptr) const override; + // TODO: override parametric version + DeriVector2 CalculateNormal(const double* param, + const double* derivparam = nullptr) const override; + DeriVector2 Value(double u, double du, const double* derivparam = nullptr) const override; + // Returns value in homogeneous coordinates (x*w, y*w, w) at given parameter u + void valueHomogenous(const double u, + double* xw, + double* yw, + double* w, + double* dxwdu, + double* dywdu, + double* dwdu) const; + int PushOwnParams(VEC_pD& pvec) override; + void ReconstructOnNewPvec(VEC_pD& pvec, int& cnt) override; + BSpline* Copy() override; + /// finds the value B_i(x) such that spline(x) = sum(poles[i] * B_i(x)) + /// x is the point at which combination is needed + /// k is the range in `flattenedknots` that contains x + /// i is index of control point + /// p is the degree + double getLinCombFactor(double x, size_t k, size_t i, unsigned int p); + inline double getLinCombFactor(double x, size_t k, size_t i) + { + return getLinCombFactor(x, k, i, degree); + } + void setupFlattenedKnots(); + /// finds spline(x) for the given parameter and knot/pole vector + /// x is the point at which combination is needed + /// k is the range in `flattenedknots` that contains x + /// p is the degree + /// d is the vector of (relevant) poles (note that this is not const and will be changed) + /// flatknots is the vector of knots + static double splineValue(double x, size_t k, unsigned int p, VEC_D& d, const VEC_D& flatknots); +}; + +} // namespace GCS + +#endif // PLANEGCS_GEO_H diff --git a/GCS/SubSystem.cpp b/GCS/SubSystem.cpp new file mode 100644 index 0000000..9d88131 --- /dev/null +++ b/GCS/SubSystem.cpp @@ -0,0 +1,357 @@ +/*************************************************************************** + * Copyright (c) 2011 Konstantinos Poulios * + * * + * This file is part of the FreeCAD CAx development system. * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of the GNU Library General Public * + * License as published by the Free Software Foundation; either * + * version 2 of the License, or (at your option) any later version. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * + * GNU Library General Public License for more details. * + * * + * You should have received a copy of the GNU Library General Public * + * License along with this library; see the file COPYING.LIB. If not, * + * write to the Free Software Foundation, Inc., 59 Temple Place, * + * Suite 330, Boston, MA 02111-1307, USA * + * * + ***************************************************************************/ + +#ifdef _MSC_VER +#pragma warning(disable : 4251) +#endif + +#include +#include + +#include "SubSystem.h" + + +namespace GCS +{ + +// SubSystem +SubSystem::SubSystem(std::vector& clist_, VEC_pD& params) + : clist(clist_) +{ + MAP_pD_pD dummymap; + initialize(params, dummymap); +} + +SubSystem::SubSystem(std::vector& clist_, VEC_pD& params, MAP_pD_pD& reductionmap) + : clist(clist_) +{ + initialize(params, reductionmap); +} + +SubSystem::~SubSystem() +{} + +void SubSystem::initialize(VEC_pD& params, MAP_pD_pD& reductionmap) +{ + csize = static_cast(clist.size()); + + // tmpplist will contain the subset of parameters from params that are + // relevant for the constraints listed in clist + VEC_pD tmpplist; + { + SET_pD s1(params.begin(), params.end()); + SET_pD s2; + for (std::vector::iterator constr = clist.begin(); constr != clist.end(); + ++constr) { + (*constr) + ->revertParams(); // ensure that the constraint points to the original parameters + VEC_pD constr_params = (*constr)->params(); + s2.insert(constr_params.begin(), constr_params.end()); + } + std::set_intersection(s1.begin(), + s1.end(), + s2.begin(), + s2.end(), + std::back_inserter(tmpplist)); + } + + plist.clear(); + MAP_pD_I rindex; + if (!reductionmap.empty()) { + int i = 0; + MAP_pD_I pindex; + for (VEC_pD::const_iterator itt = tmpplist.begin(); itt != tmpplist.end(); ++itt) { + MAP_pD_pD::const_iterator itr = reductionmap.find(*itt); + if (itr != reductionmap.end()) { + MAP_pD_I::const_iterator itp = pindex.find(itr->second); + if (itp + == pindex.end()) { // the reduction target is not in plist yet, so add it now + plist.push_back(itr->second); + rindex[itr->first] = i; + pindex[itr->second] = i; + i++; + } + else { // the reduction target is already in plist, just inform rindex + rindex[itr->first] = itp->second; + } + } + else if (pindex.find(*itt) == pindex.end()) { // not in plist yet, so add it now + plist.push_back(*itt); + pindex[*itt] = i; + i++; + } + } + } + else { + plist = tmpplist; + } + + psize = static_cast(plist.size()); + pvals.resize(psize); + pmap.clear(); + for (int j = 0; j < psize; j++) { + pmap[plist[j]] = &pvals[j]; + pvals[j] = *plist[j]; + } + for (MAP_pD_I::const_iterator itr = rindex.begin(); itr != rindex.end(); ++itr) { + pmap[itr->first] = &pvals[itr->second]; + } + + c2p.clear(); + p2c.clear(); + for (std::vector::iterator constr = clist.begin(); constr != clist.end(); + ++constr) { + (*constr)->revertParams(); // ensure that the constraint points to the original parameters + VEC_pD constr_params_orig = (*constr)->params(); + SET_pD constr_params; + for (VEC_pD::const_iterator p = constr_params_orig.begin(); p != constr_params_orig.end(); + ++p) { + MAP_pD_pD::const_iterator pmapfind = pmap.find(*p); + if (pmapfind != pmap.end()) { + constr_params.insert(pmapfind->second); + } + } + for (SET_pD::const_iterator p = constr_params.begin(); p != constr_params.end(); ++p) { + // jacobi.set(*constr, *p, 0.); + c2p[*constr].push_back(*p); + p2c[*p].push_back(*constr); + } + // (*constr)->redirectParams(pmap); // redirect parameters to pvec + } +} + +void SubSystem::redirectParams() +{ + // copying values to pvals + for (MAP_pD_pD::const_iterator p = pmap.begin(); p != pmap.end(); ++p) { + *(p->second) = *(p->first); + } + + // redirect constraints to point to pvals + for (std::vector::iterator constr = clist.begin(); constr != clist.end(); + ++constr) { + (*constr)->revertParams(); // this line will normally not be necessary + (*constr)->redirectParams(pmap); + } +} + +void SubSystem::revertParams() +{ + for (std::vector::iterator constr = clist.begin(); constr != clist.end(); + ++constr) { + (*constr)->revertParams(); + } +} + +void SubSystem::getParamMap(MAP_pD_pD& pmapOut) +{ + pmapOut = pmap; +} + +void SubSystem::getParamList(VEC_pD& plistOut) +{ + plistOut = plist; +} + +void SubSystem::getParams(VEC_pD& params, Eigen::VectorXd& xOut) +{ + if (xOut.size() != int(params.size())) { + xOut.setZero(params.size()); + } + + for (int j = 0; j < int(params.size()); j++) { + MAP_pD_pD::const_iterator pmapfind = pmap.find(params[j]); + if (pmapfind != pmap.end()) { + xOut[j] = *(pmapfind->second); + } + } +} + +void SubSystem::getParams(Eigen::VectorXd& xOut) +{ + if (xOut.size() != psize) { + xOut.setZero(psize); + } + + for (int i = 0; i < psize; i++) { + xOut[i] = pvals[i]; + } +} + +void SubSystem::setParams(VEC_pD& params, Eigen::VectorXd& xIn) +{ + assert(xIn.size() == int(params.size())); + for (int j = 0; j < int(params.size()); j++) { + MAP_pD_pD::const_iterator pmapfind = pmap.find(params[j]); + if (pmapfind != pmap.end()) { + *(pmapfind->second) = xIn[j]; + } + } +} + +void SubSystem::setParams(Eigen::VectorXd& xIn) +{ + assert(xIn.size() == psize); + for (int i = 0; i < psize; i++) { + pvals[i] = xIn[i]; + } +} + +void SubSystem::getConstraintList(std::vector& clist_) +{ + clist_ = clist; +} + +double SubSystem::error() +{ + double err = 0.; + for (std::vector::const_iterator constr = clist.begin(); constr != clist.end(); + ++constr) { + double tmp = (*constr)->error(); + err += tmp * tmp; + } + err *= 0.5; + return err; +} + +void SubSystem::calcResidual(Eigen::VectorXd& r) +{ + assert(r.size() == csize); + + int i = 0; + for (std::vector::const_iterator constr = clist.begin(); constr != clist.end(); + ++constr, i++) { + r[i] = (*constr)->error(); + } +} + +void SubSystem::calcResidual(Eigen::VectorXd& r, double& err) +{ + assert(r.size() == csize); + + int i = 0; + err = 0.; + for (std::vector::const_iterator constr = clist.begin(); constr != clist.end(); + ++constr, i++) { + r[i] = (*constr)->error(); + err += r[i] * r[i]; + } + err *= 0.5; +} + +void SubSystem::calcJacobi(VEC_pD& params, Eigen::MatrixXd& jacobi) +{ + jacobi.setZero(csize, params.size()); + for (int j = 0; j < int(params.size()); j++) { + MAP_pD_pD::const_iterator pmapfind = pmap.find(params[j]); + if (pmapfind != pmap.end()) { + for (int i = 0; i < csize; i++) { + jacobi(i, j) = clist[i]->grad(pmapfind->second); + } + } + } +} + +void SubSystem::calcJacobi(Eigen::MatrixXd& jacobi) +{ + calcJacobi(plist, jacobi); +} + +void SubSystem::calcGrad(VEC_pD& params, Eigen::VectorXd& grad) +{ + assert(grad.size() == int(params.size())); + + grad.setZero(); + for (int j = 0; j < int(params.size()); j++) { + MAP_pD_pD::const_iterator pmapfind = pmap.find(params[j]); + if (pmapfind != pmap.end()) { + std::vector constrs = p2c[pmapfind->second]; + for (std::vector::const_iterator constr = constrs.begin(); + constr != constrs.end(); + ++constr) { + grad[j] += (*constr)->error() * (*constr)->grad(pmapfind->second); + } + } + } +} + +void SubSystem::calcGrad(Eigen::VectorXd& grad) +{ + calcGrad(plist, grad); +} + +double SubSystem::maxStep(VEC_pD& params, Eigen::VectorXd& xdir) +{ + assert(xdir.size() == int(params.size())); + + MAP_pD_D dir; + for (int j = 0; j < int(params.size()); j++) { + MAP_pD_pD::const_iterator pmapfind = pmap.find(params[j]); + if (pmapfind != pmap.end()) { + dir[pmapfind->second] = xdir[j]; + } + } + + double alpha = 1e10; + for (std::vector::iterator constr = clist.begin(); constr != clist.end(); + ++constr) { + alpha = (*constr)->maxStep(dir, alpha); + } + + return alpha; +} + +double SubSystem::maxStep(Eigen::VectorXd& xdir) +{ + return maxStep(plist, xdir); +} + +void SubSystem::applySolution() +{ + for (MAP_pD_pD::const_iterator it = pmap.begin(); it != pmap.end(); ++it) { + *(it->first) = *(it->second); + } +} + +void SubSystem::analyse(Eigen::MatrixXd& /*J*/, Eigen::MatrixXd& /*ker*/, Eigen::MatrixXd& /*img*/) +{} + +void SubSystem::report() +{} + +void SubSystem::printResidual() +{ + Eigen::VectorXd r(csize); + int i = 0; + double err = 0.; + for (std::vector::const_iterator constr = clist.begin(); constr != clist.end(); + ++constr, i++) { + r[i] = (*constr)->error(); + err += r[i] * r[i]; + } + err *= 0.5; + std::cout << "Residual r = " << r.transpose() << std::endl; + std::cout << "Residual err = " << err << std::endl; +} + + +} // namespace GCS diff --git a/GCS/SubSystem.h b/GCS/SubSystem.h new file mode 100644 index 0000000..6c4062d --- /dev/null +++ b/GCS/SubSystem.h @@ -0,0 +1,98 @@ +/*************************************************************************** + * Copyright (c) 2011 Konstantinos Poulios * + * * + * 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 * + * * + ***************************************************************************/ + +#ifndef PLANEGCS_SUBSYSTEM_H +#define PLANEGCS_SUBSYSTEM_H + +#undef min +#undef max + +#include + +#include "Constraints.h" + + +namespace GCS +{ + +class SubSystem +{ +private: + int psize, csize; + std::vector clist; + VEC_pD plist; // pointers to the original parameters + MAP_pD_pD pmap; // redirection map from the original parameters to pvals + VEC_D pvals; // current variables vector (psize) + // JacobianMatrix jacobi; // jacobi matrix of the residuals + std::map c2p; // constraint to parameter adjacency list + std::map> p2c; // parameter to constraint adjacency list + void initialize(VEC_pD& params, MAP_pD_pD& reductionmap); // called by the constructors +public: + SubSystem(std::vector& clist_, VEC_pD& params); + SubSystem(std::vector& clist_, VEC_pD& params, MAP_pD_pD& reductionmap); + ~SubSystem(); + + int pSize() + { + return psize; + }; + int cSize() + { + return csize; + }; + + void redirectParams(); + void revertParams(); + + void getParamMap(MAP_pD_pD& pmapOut); + void getParamList(VEC_pD& plistOut); + + void getParams(VEC_pD& params, Eigen::VectorXd& xOut); + void getParams(Eigen::VectorXd& xOut); + void setParams(VEC_pD& params, Eigen::VectorXd& xIn); + void setParams(Eigen::VectorXd& xIn); + + void getConstraintList(std::vector& clist_); + + double error(); + void calcResidual(Eigen::VectorXd& r); + void calcResidual(Eigen::VectorXd& r, double& err); + void calcJacobi(VEC_pD& params, Eigen::MatrixXd& jacobi); + void calcJacobi(Eigen::MatrixXd& jacobi); + void calcGrad(VEC_pD& params, Eigen::VectorXd& grad); + void calcGrad(Eigen::VectorXd& grad); + + double maxStep(VEC_pD& params, Eigen::VectorXd& xdir); + double maxStep(Eigen::VectorXd& xdir); + + void applySolution(); + void analyse(Eigen::MatrixXd& J, Eigen::MatrixXd& ker, Eigen::MatrixXd& img); + void report(); + + void printResidual(); +}; + +double lineSearch(SubSystem* subsys, Eigen::VectorXd& xdir); + +} // namespace GCS + +#endif // PLANEGCS_SUBSYSTEM_H diff --git a/GCS/Util.h b/GCS/Util.h new file mode 100644 index 0000000..133f911 --- /dev/null +++ b/GCS/Util.h @@ -0,0 +1,44 @@ +/*************************************************************************** + * Copyright (c) 2011 Konstantinos Poulios * + * * + * 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 * + * * + ***************************************************************************/ + +#ifndef PLANEGCS_UTIL_H +#define PLANEGCS_UTIL_H + +#include +#include +#include + + +namespace GCS +{ +using VEC_pD = std::vector; +using VEC_D = std::vector; +using VEC_I = std::vector; +using MAP_pD_pD = std::map; +using MAP_pD_D = std::map; +using MAP_pD_I = std::map; +using SET_pD = std::set; +using SET_I = std::set; +using VEC_FIXED_POINTS_COORDS = std::vector< std::pair< int, std::pair > >; +} // namespace GCS + +#endif // PLANEGCS_UTIL_H diff --git a/GCS/headers/Console.h b/GCS/headers/Console.h new file mode 100644 index 0000000..d1d8a44 --- /dev/null +++ b/GCS/headers/Console.h @@ -0,0 +1,24 @@ +#ifndef BASE_H +#define BASE_H + +#include +#include +//#include +// +//EM_JS(void, log_string, (const char* str), { +// console.log(UTF8ToString(str)); +//} ); +//class Console { +//public: +// static void Log(const char* format, ...) { +// va_list args; +// va_start(args, format); +// char buffer[512]; +// vsprintf(buffer, format, args); +// va_end(args); +// log_string(buffer); +// } +//}; + + +#endif // BASE_H \ No newline at end of file diff --git a/GCS/headers/FCConfig.h b/GCS/headers/FCConfig.h new file mode 100644 index 0000000..507b1a0 --- /dev/null +++ b/GCS/headers/FCConfig.h @@ -0,0 +1,281 @@ +/*************************************************************************** + * Copyright (c) 2004 Jürgen Riegel * + * * + * 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 * + * * + ***************************************************************************/ +/** \file FCConfig.h + * \brief Include all needed defines and macros + * Here all defines and macro switching is done for FreeCAD. + * Every used library has its own section to define the configuration. + * This file keeps the makefiles and project files cleaner. + */ + + +#ifndef FC_CONFIG_H +#define FC_CONFIG_H + + +//************************************************************************** +// switching the operating systems + +// First check for *WIN64* since the *WIN32* are also set on 64-bit platforms +#if defined(WIN64) || defined(_WIN64) || defined(__WIN64__) +# ifndef FC_OS_WIN32 +# define FC_OS_WIN32 +# endif +# ifndef FC_OS_WIN64 +# define FC_OS_WIN64 +# endif +#elif defined(WIN32) || defined(_WIN32) || defined(__WIN32__) +# ifndef FC_OS_WIN32 +# define FC_OS_WIN32 +# endif +# if defined(__MINGW32__) +# if HAVE_CONFIG_H +# include +# endif // HAVE_CONFIG_H + +# endif +#elif defined(__APPLE__) +# ifndef FC_OS_MACOSX +# define FC_OS_MACOSX +# endif +#elif defined(unix) || defined(__linux) || defined(__linux__) || defined(__GLIBC__) +# ifndef FC_OS_LINUX +# define FC_OS_LINUX +# endif +#elif defined(__NetBSD__) || defined(__FreeBSD__) || defined(__OpenBSD__) +# ifndef FC_OS_BSD +# define FC_OS_BSD +# endif +#elif defined(__CYGWIN__) +# ifndef FC_OS_CYGWIN +# define FC_OS_CYGWIN +// Avoid conflicts with Inventor +# define HAVE_INT8_T +# define HAVE_UINT8_T +# define HAVE_INT16_T +# define HAVE_UINT16_T +# define HAVE_INT32_T +# define HAVE_UINT32_T +# define HAVE_INT64_T +# define HAVE_UINT64_T +# define HAVE_INTPTR_T +# define HAVE_UINTPTR_T +#endif + +#else +# error "FreeCAD is not ported to this OS yet. For help see www.freecad.org" +#endif + +#ifdef FC_OS_WIN32 +# define PATHSEP '\\' +#else +# define PATHSEP '/' +#endif + +//************************************************************************** +// Standard types for Windows + +#if defined(__MINGW32__) +// Do not remove this line! +#elif defined (FC_OS_WIN64) || defined (FC_OS_WIN32) + +#ifndef HAVE_INT8_T +#define HAVE_INT8_T +typedef signed char int8_t; +#endif + +#ifndef HAVE_UINT8_T +#define HAVE_UINT8_T +typedef unsigned char uint8_t; +#endif + +#ifndef HAVE_INT16_T +#define HAVE_INT16_T +typedef short int16_t; +#endif + +#ifndef HAVE_UINT16_T +#define HAVE_UINT16_T +typedef unsigned short uint16_t; +#endif + +#ifndef HAVE_INT32_T +#define HAVE_INT32_T +typedef int int32_t; +#endif + +#ifndef HAVE_UINT32_T +#define HAVE_UINT32_T +typedef unsigned int uint32_t; +#endif + +#ifndef HAVE_INT64_T +#define HAVE_INT64_T +typedef __int64 int64_t; +#endif + +#ifndef HAVE_UINT64_T +#define HAVE_UINT64_T +typedef unsigned __int64 uint64_t; +#endif + +/* avoid to redefine the HAVE_* in Coin's inttypes.h file */ +#define COIN_CONFIGURE_BUILD +/* The header file. */ +#define HAVE_INTTYPES_H 1 +/* The header file. */ +#define HAVE_STDINT_H 1 +/* The header file. */ +#define HAVE_SYS_TYPES_H 1 +/* The header file. */ +#define HAVE_STDDEF_H 1 + +#endif + + +//FIXME: Move to modules where OCC is needed +//************************************************************************** +// Open CasCade + +#ifdef _MSC_VER +# ifndef WNT +# define WNT +# endif +# ifndef WIN32 +# define WIN32 +# endif +# ifndef _WINDOWS +# define _WINDOWS +# endif +#endif + +#ifdef FC_OS_LINUX +# define LIN +# define LININTEL +#endif + +#define CSFDB + +/// enables the use of the OCC DocumentBrowser +#ifndef FC_OS_LINUX +# define FC_USE_OCAFBROWSER +#endif + + +#ifdef FC_OCC_DEBUG +# ifdef FC_DEBUG +# define DEBUG 1 +# else +# undef DEBUG +# ifndef NDEBUG +# define NDEBUG +# endif +# endif +#endif + + +//************************************************************************** +// Qt + +// Make sure to explicitly use the correct conversion +#ifndef QT_NO_CAST_FROM_ASCII +# define QT_NO_CAST_FROM_ASCII +#endif + +#ifndef QT_NO_KEYWORDS +# define QT_NO_KEYWORDS +#endif + +#if defined (FC_OS_WIN32) +# ifndef QT_DLL +# define QT_DLL +# endif +#endif + +#ifndef QT_THREAD_SUPPORT +# define QT_THREAD_SUPPORT +#endif + +#ifndef QT_ALTERNATE_QTSMANIP +# define QT_ALTERNATE_QTSMANIP +#endif + + +//************************************************************************** +// Coin3D +#if defined (FC_OS_WIN32) +# ifndef FCGui //COIN_DLL is defined in the FreeCADGui target +# ifndef COIN_DLL +# define COIN_DLL +# endif +# endif +#endif + +//************************************************************************** +// Quarter +#if defined (FC_OS_WIN32) +# ifndef QUARTER_INTERNAL +# ifndef QUARTER_DLL +# define QUARTER_DLL +# endif +# endif +#endif + +//************************************************************************** +// Boost +#ifndef BOOST_SIGNALS_NO_DEPRECATION_WARNING +#define BOOST_SIGNALS_NO_DEPRECATION_WARNING +#endif + + +//************************************************************************** +// Exception handling + +// Don't catch C++ exceptions in DEBUG! +#ifdef FC_DEBUG +# define DONT_CATCH_CXX_EXCEPTIONS 1 +# define DBG_TRY +# define DBG_CATCH(X) +#else +/// used to switch a catch with the debug state +# define DBG_TRY try { +/// see docu DBGTRY +# define DBG_CATCH(X) } catch (...) { X } +#endif + + +//************************************************************************** +// Windows import export DLL defines +#include + +//************************************************************************** +// point at which warnings of overly long specifiers disabled (needed for VC6) +#ifdef _MSC_VER +# pragma warning( disable : 4251 ) +# pragma warning( disable : 4996 ) // suppress deprecated warning for e.g. open() +#if defined(WIN64) || defined(_WIN64) || defined(__WIN64__) +# pragma warning( disable : 4244 ) +# pragma warning( disable : 4267 ) +#endif + +#endif + +#endif //FC_CONFIG_H diff --git a/GCS/headers/FCGlobal.h b/GCS/headers/FCGlobal.h new file mode 100644 index 0000000..71d76be --- /dev/null +++ b/GCS/headers/FCGlobal.h @@ -0,0 +1,93 @@ +/*************************************************************************** + * Copyright (c) 2019 Werner Mayer * + * * + * 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 * + * * + ***************************************************************************/ +/** \file FCGlobal.h + * \brief Include export or import macros. + */ + + +#ifndef FC_GLOBAL_H +#define FC_GLOBAL_H + + +#if defined(WIN64) || defined(_WIN64) || defined(__WIN64__) || defined(__CYGWIN__) +# define FREECAD_DECL_EXPORT __declspec(dllexport) +# define FREECAD_DECL_IMPORT __declspec(dllimport) +#else +# define FREECAD_DECL_EXPORT +# define FREECAD_DECL_IMPORT +#endif + +// FreeCADBase +#ifdef FreeCADBase_EXPORTS +# define BaseExport FREECAD_DECL_EXPORT +#else +# define BaseExport FREECAD_DECL_IMPORT +#endif + +// FreeCADApp +#ifdef FreeCADApp_EXPORTS +# define AppExport FREECAD_DECL_EXPORT +# define DataExport FREECAD_DECL_EXPORT +#else +# define AppExport FREECAD_DECL_IMPORT +# define DataExport FREECAD_DECL_IMPORT +#endif + +// FreeCADGui +#ifdef FreeCADGui_EXPORTS +# define GuiExport FREECAD_DECL_EXPORT +#else +# define GuiExport FREECAD_DECL_IMPORT +#endif + +// Disable copy/move +#define FC_DISABLE_COPY(Class) \ + Class(const Class &) = delete;\ + Class &operator=(const Class &) = delete; + +#define FC_DISABLE_MOVE(Class) \ + Class(Class &&) = delete; \ + Class &operator=(Class &&) = delete; + +#define FC_DISABLE_COPY_MOVE(Class) \ + FC_DISABLE_COPY(Class) \ + FC_DISABLE_MOVE(Class) + +// Default copy/move +#define FC_DEFAULT_COPY(Class) \ + Class(const Class &) = default;\ + Class &operator=(const Class &) = default; + +#define FC_DEFAULT_MOVE(Class) \ + Class(Class &&) = default; \ + Class &operator=(Class &&) = default; + +#define FC_DEFAULT_COPY_MOVE(Class) \ + FC_DEFAULT_COPY(Class) \ + FC_DEFAULT_MOVE(Class) + + +#ifndef HAVE_Q_DISABLE_COPY_MOVE +#define Q_DISABLE_COPY_MOVE FC_DEFAULT_COPY_MOVE +#endif + +#endif //FC_GLOBAL_H diff --git a/GCS/headers/boost_graph_adjacency_list.hpp b/GCS/headers/boost_graph_adjacency_list.hpp new file mode 100644 index 0000000..313a38c --- /dev/null +++ b/GCS/headers/boost_graph_adjacency_list.hpp @@ -0,0 +1,9 @@ +#ifndef FREECAD_ADJACENCY_LIST_HPP_WORKAROUND +#define FREECAD_ADJACENCY_LIST_HPP_WORKAROUND + +// Workaround for boost >= 1.75 +#define BOOST_ALLOW_DEPRECATED_HEADERS +#include +#undef BOOST_ALLOW_DEPRECATED_HEADERS + +#endif // #ifndef FREECAD_ADJACENCY_LIST_HPP_WORKAROUND diff --git a/GCS/qp_eq.cpp b/GCS/qp_eq.cpp new file mode 100644 index 0000000..4cc37c2 --- /dev/null +++ b/GCS/qp_eq.cpp @@ -0,0 +1,72 @@ +/*************************************************************************** + * Copyright (c) 2011 Konstantinos Poulios * + * * + * 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 : 4244) +#endif + +#include +#include + +using namespace Eigen; + +// minimizes ( 0.5 * x^T * H * x + g^T * x ) under the condition ( A*x + c = 0 ) +// it returns the solution in x, the row-space of A in Y, and the null space of A in Z +int qp_eq(MatrixXd& H, VectorXd& g, MatrixXd& A, VectorXd& c, VectorXd& x, MatrixXd& Y, MatrixXd& Z) +{ + FullPivHouseholderQR qrAT(A.transpose()); + MatrixXd Q = qrAT.matrixQ(); + + size_t params_num = qrAT.rows(); + size_t constr_num = qrAT.cols(); + size_t rank = qrAT.rank(); + + if (rank != constr_num || constr_num > params_num) { + return -1; + } + + // A^T = Q*R*P^T = Q1*R1*P^T + // Q = [Q1,Q2], R=[R1;0] + // Y = Q1 * inv(R^T) * P^T + // Z = Q2 + Y = qrAT.matrixQR() + .topRows(constr_num) + .triangularView() + .transpose() + .solve(Q.leftCols(rank)) + * qrAT.colsPermutation().transpose(); + if (params_num == rank) { + x = -Y * c; + } + else { + Z = Q.rightCols(params_num - rank); + + MatrixXd ZTHZ = Z.transpose() * H * Z; + VectorXd rhs = Z.transpose() * (H * Y * c - g); + + VectorXd y = ZTHZ.colPivHouseholderQr().solve(rhs); + + x = -Y * c + Z * y; + } + + return 0; +} diff --git a/GCS/qp_eq.h b/GCS/qp_eq.h new file mode 100644 index 0000000..fdb85c1 --- /dev/null +++ b/GCS/qp_eq.h @@ -0,0 +1,30 @@ +/*************************************************************************** + * Copyright (c) 2011 Konstantinos Poulios * + * * + * This file is part of the FreeCAD CAx development system. * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of the GNU Library General Public * + * License as published by the Free Software Foundation; either * + * version 2 of the License, or (at your option) any later version. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * + * GNU Library General Public License for more details. * + * * + * You should have received a copy of the GNU Library General Public * + * License along with this library; see the file COPYING.LIB. If not, * + * write to the Free Software Foundation, Inc., 59 Temple Place, * + * Suite 330, Boston, MA 02111-1307, USA * + * * + ***************************************************************************/ +#include + +int qp_eq(Eigen::MatrixXd& H, + Eigen::VectorXd& g, + Eigen::MatrixXd& A, + Eigen::VectorXd& c, + Eigen::VectorXd& x, + Eigen::MatrixXd& Y, + Eigen::MatrixXd& Z); diff --git a/main.cpp b/main.cpp new file mode 100644 index 0000000..31f3950 --- /dev/null +++ b/main.cpp @@ -0,0 +1,9 @@ +#include "DRAWer_2_0.h" +#include +int main(int argc, char *argv[]) +{ + QApplication app(argc, argv); + DRAWer_2_0 window; + window.show(); + return app.exec(); +}