ParkSuMin/DRAwer_2_0
Archived
1
0
Fork 0
Code Issues 1 Pull Requests Actions Packages Projects Releases Wiki Activity

Initial commit

Browse Source
This commit is contained in:
Anton Kamalov
2025-12-06 16:01:44 +03:00
parent 5d3506215c
commit e613b4f004
23 changed files with 14518 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
.gitignore vendored
View File

@@ -361,3 +361,7 @@ MigrationBackup/
# Fody - auto-generated XML schema
FodyWeavers.xsd
/eigen-5.0.0
/include
/src
*.json

50
CMakeLists.txt Normal file
View File

@@ -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
)

83
Canvas.cpp Normal file
View File

@@ -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;
}

34
Canvas.h Normal file
View File

@@ -0,0 +1,34 @@
#pragma once
#include <QWidget>
#include <QMouseEvent>
#ifdef _DEBUG
#include <QDebug>
#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<Line*> lines;
QVector<Point> points;
QVector<double*> params;
Line* current_line;
protected:
void mousePressEvent(QMouseEvent*) override;
public:
Canvas(QWidget *parent);
~Canvas();
};

18
DRAWer_2_0.cpp Normal file
View File

@@ -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++));
}

21
DRAWer_2_0.h Normal file
View File

@@ -0,0 +1,21 @@
#pragma once
#include <QtWidgets/QMainWindow>
#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;
};

135
DRAWer_2_0.ui Normal file
View File

@@ -0,0 +1,135 @@
<?xml version="1.0" encoding="UTF-8"?>
<ui version="4.0">
<class>DRAWer_2_0Class</class>
<widget class="QMainWindow" name="DRAWer_2_0Class">
<property name="geometry">
<rect>
<x>0</x>
<y>0</y>
<width>700</width>
<height>600</height>
</rect>
</property>
<property name="maximumSize">
<size>
<width>1080</width>
<height>1080</height>
</size>
</property>
<property name="windowTitle">
<string>DRAWer_2_0</string>
</property>
<widget class="QWidget" name="centralWidget">
<layout class="QVBoxLayout" name="verticalLayout">
<item>
<layout class="QHBoxLayout" name="horizontalLayout_2">
<property name="bottomMargin">
<number>23</number>
</property>
<item>
<widget class="QPushButton" name="pushButton">
<property name="minimumSize">
<size>
<width>75</width>
<height>24</height>
</size>
</property>
<property name="text">
<string>Click me</string>
</property>
</widget>
</item>
<item>
<widget class="QPushButton" name="pushButton_2">
<property name="minimumSize">
<size>
<width>75</width>
<height>24</height>
</size>
</property>
<property name="text">
<string>PushButton</string>
</property>
</widget>
</item>
<item>
<widget class="QPushButton" name="pushButton_3">
<property name="minimumSize">
<size>
<width>75</width>
<height>24</height>
</size>
</property>
<property name="text">
<string>PushButton</string>
</property>
</widget>
</item>
</layout>
</item>
<item>
<widget class="QLabel" name="label">
<property name="maximumSize">
<size>
<width>100</width>
<height>100</height>
</size>
</property>
<property name="text">
<string>TextLabel</string>
</property>
<property name="alignment">
<set>Qt::AlignmentFlag::AlignCenter</set>
</property>
</widget>
</item>
<item>
<widget class="Canvas" name="widget" native="true">
<property name="sizePolicy">
<sizepolicy hsizetype="Expanding" vsizetype="Expanding">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="maximumSize">
<size>
<width>16777215</width>
<height>16777215</height>
</size>
</property>
</widget>
</item>
</layout>
</widget>
<widget class="QMenuBar" name="menuBar">
<property name="geometry">
<rect>
<x>0</x>
<y>0</y>
<width>700</width>
<height>22</height>
</rect>
</property>
</widget>
<widget class="QToolBar" name="mainToolBar">
<attribute name="toolBarArea">
<enum>RightToolBarArea</enum>
</attribute>
<attribute name="toolBarBreak">
<bool>false</bool>
</attribute>
</widget>
<widget class="QStatusBar" name="statusBar"/>
</widget>
<layoutdefault spacing="6" margin="11"/>
<customwidgets>
<customwidget>
<class>Canvas</class>
<extends>QWidget</extends>
<header>canvas.h</header>
<container>1</container>
</customwidget>
</customwidgets>
<resources/>
<connections/>
</ui>

3583
GCS/Constraints.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

1413
GCS/Constraints.h Normal file
View File

File diff suppressed because it is too large Load Diff

5839
GCS/GCS.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

645
GCS/GCS.h Normal file
View File

@@ -0,0 +1,645 @@
/***************************************************************************
* Copyright (c) 2011 Konstantinos Poulios <logari81@gmail.com> *
* *
* This file is part of the FreeCAD CAx development system. *
* *
* This library is free software; you can redistribute it and/or *
* modify it under the terms of the GNU Library General Public *
* License as published by the Free Software Foundation; either *
* version 2 of the License, or (at your option) any later version. *
* *
* This library is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
* GNU Library General Public License for more details. *
* *
* You should have received a copy of the GNU Library General Public *
* License along with this library; see the file COPYING.LIB. If not, *
* write to the Free Software Foundation, Inc., 59 Temple Place, *
* Suite 330, Boston, MA 02111-1307, USA *
* *
***************************************************************************/
#ifndef PLANEGCS_GCS_H
#define PLANEGCS_GCS_H
#include <Eigen/QR>
#include <memory>
#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 <Eigen/Sparse>
#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<std::vector<double*>> pDependentParametersGroups;
std::vector<Constraint*> clist;
std::map<Constraint*, VEC_pD> c2p; // constraint to parameter adjacency list
std::map<double*, std::vector<Constraint*>> p2c; // parameter to constraint adjacency list
std::vector<SubSystem*> 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<VEC_pD> plists; // partitioned plist except equality constraints
// partitioned clist except equality constraints
std::vector<std::vector<Constraint*>> clists;
std::vector<MAP_pD_pD> reductionmaps; // for simplification of equality constraints
int dofs;
std::set<Constraint*> 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<int, int>& jacobianconstraintmap,
GCS::VEC_pD& pdiagnoselist,
std::map<int, int>& tagmultiplicity);
void makeDenseQRDecomposition(const Eigen::MatrixXd& J,
const std::map<int, int>& jacobianconstraintmap,
Eigen::FullPivHouseholderQR<Eigen::MatrixXd>& 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<int, int>& jacobianconstraintmap,
Eigen::SparseQR<Eigen::SparseMatrix<double>, Eigen::COLAMDOrdering<int>>& 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<Eigen::MatrixXd>& qrJT,
const GCS::VEC_pD& pdiagnoselist,
int paramsNum,
int rank);
template<typename T>
void identifyConflictingRedundantConstraints(Algorithm alg,
const T& qrJT,
const std::map<int, int>& jacobianconstraintmap,
const std::map<int, int>& tagmultiplicity,
GCS::VEC_pD& pdiagnoselist,
Eigen::MatrixXd& R,
int constrNum,
int rank,
int& nonredundantconstrNum);
void eliminateNonZerosOverPivotInUpperTriangularMatrix(Eigen::MatrixXd& R, int rank);
#ifdef EIGEN_SPARSEQR_COMPATIBLE
void identifyDependentParametersSparseQR(const Eigen::MatrixXd& J,
const std::map<int, int>& jacobianconstraintmap,
const GCS::VEC_pD& pdiagnoselist,
bool silent = true);
#endif
void identifyDependentParametersDenseQR(const Eigen::MatrixXd& J,
const std::map<int, int>& jacobianconstraintmap,
const GCS::VEC_pD& pdiagnoselist,
bool silent = true);
template<typename T>
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<Constraint *> 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<std::vector<double*>>& 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<Constraint*>& constrvec);
void deleteAllContent(std::vector<SubSystem*>& subsysvec);
} // namespace GCS
#endif // PLANEGCS_GCS_H

1183
GCS/Geo.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

492
GCS/Geo.h Normal file
View File

@@ -0,0 +1,492 @@
/***************************************************************************
* Copyright (c) 2011 Konstantinos Poulios <logari81@gmail.com> *
* *
* This file is part of the FreeCAD CAx development system. *
* *
* This library is free software; you can redistribute it and/or *
* modify it under the terms of the GNU Library General Public *
* License as published by the Free Software Foundation; either *
* version 2 of the License, or (at your option) any later version. *
* *
* This library is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
* GNU Library General Public License for more details. *
* *
* You should have received a copy of the GNU Library General Public *
* License along with this library; see the file COPYING.LIB. If not, *
* write to the Free Software Foundation, Inc., 59 Temple Place, *
* Suite 330, Boston, MA 02111-1307, USA *
* *
***************************************************************************/
#ifndef PLANEGCS_GEO_H
#define PLANEGCS_GEO_H
#include "Util.h"
#include <boost/math/constants/constants.hpp>
#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<Point>;
/// 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

357
GCS/SubSystem.cpp Normal file
View File

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

98
GCS/SubSystem.h Normal file
View File

@@ -0,0 +1,98 @@
/***************************************************************************
* Copyright (c) 2011 Konstantinos Poulios <logari81@gmail.com> *
* *
* This file is part of the FreeCAD CAx development system. *
* *
* This library is free software; you can redistribute it and/or *
* modify it under the terms of the GNU Library General Public *
* License as published by the Free Software Foundation; either *
* version 2 of the License, or (at your option) any later version. *
* *
* This library is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
* GNU Library General Public License for more details. *
* *
* You should have received a copy of the GNU Library General Public *
* License along with this library; see the file COPYING.LIB. If not, *
* write to the Free Software Foundation, Inc., 59 Temple Place, *
* Suite 330, Boston, MA 02111-1307, USA *
* *
***************************************************************************/
#ifndef PLANEGCS_SUBSYSTEM_H
#define PLANEGCS_SUBSYSTEM_H
#undef min
#undef max
#include <Eigen/Core>
#include "Constraints.h"
namespace GCS
{
class SubSystem
{
private:
int psize, csize;
std::vector<Constraint*> 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<Constraint*, VEC_pD> c2p; // constraint to parameter adjacency list
std::map<double*, std::vector<Constraint*>> p2c; // parameter to constraint adjacency list
void initialize(VEC_pD& params, MAP_pD_pD& reductionmap); // called by the constructors
public:
SubSystem(std::vector<Constraint*>& clist_, VEC_pD& params);
SubSystem(std::vector<Constraint*>& 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<Constraint*>& 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

44
GCS/Util.h Normal file
View File

@@ -0,0 +1,44 @@
/***************************************************************************
* Copyright (c) 2011 Konstantinos Poulios <logari81@gmail.com> *
* *
* This file is part of the FreeCAD CAx development system. *
* *
* This library is free software; you can redistribute it and/or *
* modify it under the terms of the GNU Library General Public *
* License as published by the Free Software Foundation; either *
* version 2 of the License, or (at your option) any later version. *
* *
* This library is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
* GNU Library General Public License for more details. *
* *
* You should have received a copy of the GNU Library General Public *
* License along with this library; see the file COPYING.LIB. If not, *
* write to the Free Software Foundation, Inc., 59 Temple Place, *
* Suite 330, Boston, MA 02111-1307, USA *
* *
***************************************************************************/
#ifndef PLANEGCS_UTIL_H
#define PLANEGCS_UTIL_H
#include <map>
#include <set>
#include <vector>
namespace GCS
{
using VEC_pD = std::vector<double*>;
using VEC_D = std::vector<double>;
using VEC_I = std::vector<int>;
using MAP_pD_pD = std::map<double*, double*>;
using MAP_pD_D = std::map<double*, double>;
using MAP_pD_I = std::map<double*, int>;
using SET_pD = std::set<double*>;
using SET_I = std::set<int>;
using VEC_FIXED_POINTS_COORDS = std::vector< std::pair< int, std::pair<double*, double*> > >;
} // namespace GCS
#endif // PLANEGCS_UTIL_H

24
GCS/headers/Console.h Normal file
View File

@@ -0,0 +1,24 @@
#ifndef BASE_H
#define BASE_H
#include <stdio.h>
#include <stdarg.h>
//#include <emscripten.h>
//
//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

281
GCS/headers/FCConfig.h Normal file
View File

@@ -0,0 +1,281 @@
/***************************************************************************
* Copyright (c) 2004 Jürgen Riegel <juergen.riegel@web.de> *
* *
* 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 <config.h>
# 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 <inttypes.h> header file. */
#define HAVE_INTTYPES_H 1
/* The <stdint.h> header file. */
#define HAVE_STDINT_H 1
/* The <sys/types.h> header file. */
#define HAVE_SYS_TYPES_H 1
/* The <stddef.h> 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 <FCGlobal.h>
//**************************************************************************
// 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

93
GCS/headers/FCGlobal.h Normal file
View File

@@ -0,0 +1,93 @@
/***************************************************************************
* Copyright (c) 2019 Werner Mayer <wmayer[at]users.sourceforge.net> *
* *
* 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

9
GCS/headers/boost_graph_adjacency_list.hpp Normal file
View File

@@ -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 <boost/graph/adjacency_list.hpp>
#undef BOOST_ALLOW_DEPRECATED_HEADERS
#endif // #ifndef FREECAD_ADJACENCY_LIST_HPP_WORKAROUND

72
GCS/qp_eq.cpp Normal file
View File

@@ -0,0 +1,72 @@
/***************************************************************************
* Copyright (c) 2011 Konstantinos Poulios <logari81@gmail.com> *
* *
* This file is part of the FreeCAD CAx development system. *
* *
* This library is free software; you can redistribute it and/or *
* modify it under the terms of the GNU Library General Public *
* License as published by the Free Software Foundation; either *
* version 2 of the License, or (at your option) any later version. *
* *
* This library is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
* GNU Library General Public License for more details. *
* *
* You should have received a copy of the GNU Library General Public *
* License along with this library; see the file COPYING.LIB. If not, *
* write to the Free Software Foundation, Inc., 59 Temple Place, *
* Suite 330, Boston, MA 02111-1307, USA *
* *
***************************************************************************/
#ifdef _MSC_VER
#pragma warning(disable : 4244)
#endif
#include <Eigen/QR>
#include <iostream>
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<MatrixXd> 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<Upper>()
.transpose()
.solve<OnTheRight>(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;
}

30
GCS/qp_eq.h Normal file
View File

@@ -0,0 +1,30 @@
/***************************************************************************
* Copyright (c) 2011 Konstantinos Poulios <logari81@gmail.com> *
* *
* This file is part of the FreeCAD CAx development system. *
* *
* This library is free software; you can redistribute it and/or *
* modify it under the terms of the GNU Library General Public *
* License as published by the Free Software Foundation; either *
* version 2 of the License, or (at your option) any later version. *
* *
* This library is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
* GNU Library General Public License for more details. *
* *
* You should have received a copy of the GNU Library General Public *
* License along with this library; see the file COPYING.LIB. If not, *
* write to the Free Software Foundation, Inc., 59 Temple Place, *
* Suite 330, Boston, MA 02111-1307, USA *
* *
***************************************************************************/
#include <Eigen/Dense>
int qp_eq(Eigen::MatrixXd& H,
Eigen::VectorXd& g,
Eigen::MatrixXd& A,
Eigen::VectorXd& c,
Eigen::VectorXd& x,
Eigen::MatrixXd& Y,
Eigen::MatrixXd& Z);

9
main.cpp Normal file
View File

@@ -0,0 +1,9 @@
#include "DRAWer_2_0.h"
#include <QtWidgets/QApplication>
int main(int argc, char *argv[])
{
QApplication app(argc, argv);
DRAWer_2_0 window;
window.show();
return app.exec();
}