kindred/create
1
1
Fork 0
Code Issues 53 Pull Requests Actions Packages Projects 9 Releases 2 Wiki Activity

Sketcher: PlaneGCS C-lang formatting

Browse Source 
====================================

As per:
https://forum.freecad.org/viewtopic.php?t=77205

Please use pre-commit as described in:
https://github.com/FreeCAD/DevelopersHandbook/blob/master/gettingstarted/index.md#setting-up-for-development
This commit is contained in:
Abdullah Tahiri
2023-05-18 08:35:55 +02:00
committed by abdullahtahiriyo
parent 5f82aa42bb
commit 2ae7cc4a94
11 changed files with 3939 additions and 3320 deletions
Download Patch File Download Diff File Expand all files Collapse all files

289
src/Mod/Sketcher/App/planegcs/Constraints.cpp
View File

@@ -23,7 +23,7 @@
#include <algorithm>
#define DEBUG_DERIVS 0
#if DEBUG_DERIVS
# include <cassert>
#include <cassert>
#endif
#include <cmath>
@@ -63,7 +63,7 @@ void Constraint::redirectParams(const MAP_pD_pD& redirectionmap)
void Constraint::revertParams()
{
pvec = origpvec;
pvecChangedFlag=true;
pvecChangedFlag = true;
}
ConstraintType Constraint::getTypeId()
@@ -81,12 +81,12 @@ double Constraint::error()
return 0.0;
}
double Constraint::grad(double * /*param*/)
double Constraint::grad(double* /*param*/)
{
return 0.0;
}
double Constraint::maxStep(MAP_pD_D & /*dir*/, double lim)
double Constraint::maxStep(MAP_pD_D& /*dir*/, double lim)
{
return lim;
}
@@ -106,7 +106,7 @@ int Constraint::findParamInPvec(double* param)
// --------------------------------------------------------
// Equal
ConstraintEqual::ConstraintEqual(double *p1, double *p2, double p1p2ratio)
ConstraintEqual::ConstraintEqual(double* p1, double* p2, double p1p2ratio)
{
ratio = p1p2ratio;
pvec.push_back(p1);
@@ -127,14 +127,16 @@ void ConstraintEqual::rescale(double coef)
double ConstraintEqual::error()
{
return scale * (*param1() - ratio *(*param2()));
return scale * (*param1() - ratio * (*param2()));
}
double ConstraintEqual::grad(double *param)
double ConstraintEqual::grad(double* param)
{
double deriv=0.;
if (param == param1()) deriv += 1;
if (param == param2()) deriv += -1;
double deriv = 0.;
if (param == param1())
deriv += 1;
if (param == param2())
deriv += -1;
return scale * deriv;
}
@@ -248,9 +250,9 @@ double ConstraintCenterOfGravity::error()
return scale * (*thecenter() - sum);
}
double ConstraintCenterOfGravity::grad(double *param)
double ConstraintCenterOfGravity::grad(double* param)
{
double deriv=0.;
double deriv = 0.;
if (param == thecenter())
deriv = 1;
@@ -603,7 +605,7 @@ double ConstraintPointOnBSpline::grad(double* gcsparam)
}
// Difference
ConstraintDifference::ConstraintDifference(double *p1, double *p2, double *d)
ConstraintDifference::ConstraintDifference(double* p1, double* p2, double* d)
{
pvec.push_back(p1);
pvec.push_back(p2);
@@ -642,7 +644,7 @@ double ConstraintDifference::grad(double* param)
// --------------------------------------------------------
// P2PDistance
ConstraintP2PDistance::ConstraintP2PDistance(Point &p1, Point &p2, double *d)
ConstraintP2PDistance::ConstraintP2PDistance(Point& p1, Point& p2, double* d)
{
pvec.push_back(p1.x);
pvec.push_back(p1.y);
@@ -732,8 +734,8 @@ double ConstraintP2PDistance::maxStep(MAP_pD_D& dir, double lim)
// --------------------------------------------------------
// P2PAngle
ConstraintP2PAngle::ConstraintP2PAngle(Point &p1, Point &p2, double *a, double da_)
: da(da_)
ConstraintP2PAngle::ConstraintP2PAngle(Point& p1, Point& p2, double* a, double da_)
: da(da_)
{
pvec.push_back(p1.x);
pvec.push_back(p1.y);
@@ -810,7 +812,7 @@ double ConstraintP2PAngle::maxStep(MAP_pD_D& dir, double lim)
// --------------------------------------------------------
// P2LDistance
ConstraintP2LDistance::ConstraintP2LDistance(Point &p, Line &l, double *d)
ConstraintP2LDistance::ConstraintP2LDistance(Point& p, Line& l, double* d)
{
pvec.push_back(p.x);
pvec.push_back(p.y);
@@ -840,7 +842,7 @@ double ConstraintP2LDistance::error()
double dist = *distance();
double dx = x2 - x1;
double dy = y2 - y1;
double d = sqrt(dx * dx + dy * dy); // line length
double d = sqrt(dx * dx + dy * dy);// line length
double area =
std::abs(-x0 * dy + y0 * dx + x1 * y2
- x2 * y1);// = x1y2 - x2y1 - x0y2 + x2y0 + x0y1 - x1y0 = 2*(triangle area)
@@ -932,7 +934,7 @@ double ConstraintP2LDistance::maxStep(MAP_pD_D& dir, double lim)
// --------------------------------------------------------
// PointOnLine
ConstraintPointOnLine::ConstraintPointOnLine(Point &p, Line &l)
ConstraintPointOnLine::ConstraintPointOnLine(Point& p, Line& l)
{
pvec.push_back(p.x);
pvec.push_back(p.y);
@@ -944,7 +946,7 @@ ConstraintPointOnLine::ConstraintPointOnLine(Point &p, Line &l)
rescale();
}
ConstraintPointOnLine::ConstraintPointOnLine(Point &p, Point &lp1, Point &lp2)
ConstraintPointOnLine::ConstraintPointOnLine(Point& p, Point& lp1, Point& lp2)
{
pvec.push_back(p.x);
pvec.push_back(p.y);
@@ -1012,7 +1014,7 @@ double ConstraintPointOnLine::grad(double* param)
// --------------------------------------------------------
// PointOnPerpBisector
ConstraintPointOnPerpBisector::ConstraintPointOnPerpBisector(Point &p, Line &l)
ConstraintPointOnPerpBisector::ConstraintPointOnPerpBisector(Point& p, Line& l)
{
pvec.push_back(p.x);
pvec.push_back(p.y);
@@ -1024,7 +1026,7 @@ ConstraintPointOnPerpBisector::ConstraintPointOnPerpBisector(Point &p, Line &l)
rescale();
}
ConstraintPointOnPerpBisector::ConstraintPointOnPerpBisector(Point &p, Point &lp1, Point &lp2)
ConstraintPointOnPerpBisector::ConstraintPointOnPerpBisector(Point& p, Point& lp1, Point& lp2)
{
pvec.push_back(p.x);
pvec.push_back(p.y);
@@ -1075,22 +1077,22 @@ double ConstraintPointOnPerpBisector::error()
return scale * err;
}
double ConstraintPointOnPerpBisector::grad(double *param)
double ConstraintPointOnPerpBisector::grad(double* param)
{
//first of all, check that we need to compute anything.
if ( findParamInPvec(param) == -1 )
// 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;
return deriv * scale;
}
// --------------------------------------------------------
// Parallel
ConstraintParallel::ConstraintParallel(Line &l1, Line &l2)
ConstraintParallel::ConstraintParallel(Line& l1, Line& l2)
{
pvec.push_back(l1.p1.x);
pvec.push_back(l1.p1.y);
@@ -1154,7 +1156,7 @@ double ConstraintParallel::grad(double* param)
// --------------------------------------------------------
// Perpendicular
ConstraintPerpendicular::ConstraintPerpendicular(Line &l1, Line &l2)
ConstraintPerpendicular::ConstraintPerpendicular(Line& l1, Line& l2)
{
pvec.push_back(l1.p1.x);
pvec.push_back(l1.p1.y);
@@ -1168,8 +1170,7 @@ ConstraintPerpendicular::ConstraintPerpendicular(Line &l1, Line &l2)
rescale();
}
ConstraintPerpendicular::ConstraintPerpendicular(Point &l1p1, Point &l1p2,
Point &l2p1, Point &l2p2)
ConstraintPerpendicular::ConstraintPerpendicular(Point& l1p1, Point& l1p2, Point& l2p1, Point& l2p2)
{
pvec.push_back(l1p1.x);
pvec.push_back(l1p1.y);
@@ -1233,7 +1234,7 @@ double ConstraintPerpendicular::grad(double* param)
// --------------------------------------------------------
// L2LAngle
ConstraintL2LAngle::ConstraintL2LAngle(Line &l1, Line &l2, double *a)
ConstraintL2LAngle::ConstraintL2LAngle(Line& l1, Line& l2, double* a)
{
pvec.push_back(l1.p1.x);
pvec.push_back(l1.p1.y);
@@ -1248,8 +1249,8 @@ ConstraintL2LAngle::ConstraintL2LAngle(Line &l1, Line &l2, double *a)
rescale();
}
ConstraintL2LAngle::ConstraintL2LAngle(Point &l1p1, Point &l1p2,
Point &l2p1, Point &l2p2, double *a)
ConstraintL2LAngle::ConstraintL2LAngle(Point& l1p1, Point& l1p2, Point& l2p1, Point& l2p2,
double* a)
{
pvec.push_back(l1p1.x);
pvec.push_back(l1p1.y);
@@ -1347,7 +1348,7 @@ double ConstraintL2LAngle::maxStep(MAP_pD_D& dir, double lim)
// --------------------------------------------------------
// MidpointOnLine
ConstraintMidpointOnLine::ConstraintMidpointOnLine(Line &l1, Line &l2)
ConstraintMidpointOnLine::ConstraintMidpointOnLine(Line& l1, Line& l2)
{
pvec.push_back(l1.p1.x);
pvec.push_back(l1.p1.y);
@@ -1509,7 +1510,7 @@ double ConstraintTangentCircumf::grad(double* param)
// --------------------------------------------------------
// ConstraintPointOnEllipse
ConstraintPointOnEllipse::ConstraintPointOnEllipse(Point &p, Ellipse &e)
ConstraintPointOnEllipse::ConstraintPointOnEllipse(Point& p, Ellipse& e)
{
pvec.push_back(p.x);
pvec.push_back(p.y);
@@ -1597,13 +1598,13 @@ double ConstraintPointOnEllipse::grad(double* param)
// --------------------------------------------------------
// ConstraintEllipseTangentLine
ConstraintEllipseTangentLine::ConstraintEllipseTangentLine(Line &l, Ellipse &e)
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
this->e.PushOwnParams(pvec);// DeepSOIC: hopefully, this won't push arc's parameters
origpvec = pvec;
pvecChangedFlag = true;
rescale();
@@ -1611,7 +1612,7 @@ ConstraintEllipseTangentLine::ConstraintEllipseTangentLine(Line &l, Ellipse &e)
void ConstraintEllipseTangentLine::ReconstructGeomPointers()
{
int i=0;
int i = 0;
l.ReconstructOnNewPvec(pvec, i);
e.ReconstructOnNewPvec(pvec, i);
pvecChangedFlag = false;
@@ -1849,8 +1850,10 @@ ConstraintInternalAlignmentPoint2Hyperbola::ConstraintInternalAlignmentPoint2Hyp
void ConstraintInternalAlignmentPoint2Hyperbola::ReconstructGeomPointers()
{
int i = 0;
p.x = pvec[i]; i++;
p.y = pvec[i]; i++;
p.x = pvec[i];
i++;
p.y = pvec[i];
i++;
e.ReconstructOnNewPvec(pvec, i);
pvecChangedFlag = false;
}
@@ -1982,9 +1985,10 @@ void ConstraintEqualMajorAxesConic::rescale(double coef)
scale = coef * 1;
}
void ConstraintEqualMajorAxesConic::errorgrad(double *err, double *grad, double *param)
void ConstraintEqualMajorAxesConic::errorgrad(double* err, double* grad, double* param)
{
if (pvecChangedFlag) ReconstructGeomPointers();
if (pvecChangedFlag)
ReconstructGeomPointers();
double a1, da1;
a1 = e1->getRadMaj(param, da1);
double a2, da2;
@@ -2028,7 +2032,7 @@ ConstraintEqualFocalDistance::ConstraintEqualFocalDistance(ArcOfParabola* a1, Ar
void ConstraintEqualFocalDistance::ReconstructGeomPointers()
{
int i =0;
int i = 0;
e1->ReconstructOnNewPvec(pvec, i);
e2->ReconstructOnNewPvec(pvec, i);
pvecChangedFlag = false;
@@ -2044,9 +2048,10 @@ void ConstraintEqualFocalDistance::rescale(double coef)
scale = coef * 1;
}
void ConstraintEqualFocalDistance::errorgrad(double *err, double *grad, double *param)
void ConstraintEqualFocalDistance::errorgrad(double* err, double* grad, double* param)
{
if (pvecChangedFlag) ReconstructGeomPointers();
if (pvecChangedFlag)
ReconstructGeomPointers();
DeriVector2 focus1(this->e1->focus1, param);
DeriVector2 vertex1(this->e1->vertex, param);
@@ -2075,7 +2080,7 @@ void ConstraintEqualFocalDistance::errorgrad(double *err, double *grad, double *
double ConstraintEqualFocalDistance::error()
{
double err;
errorgrad(&err,nullptr,nullptr);
errorgrad(&err, nullptr, nullptr);
return scale * err;
}
@@ -2094,7 +2099,7 @@ double ConstraintEqualFocalDistance::grad(double* param)
// --------------------------------------------------------
// ConstraintCurveValue
ConstraintCurveValue::ConstraintCurveValue(Point &p, double* pcoord, Curve& crv, double *u)
ConstraintCurveValue::ConstraintCurveValue(Point& p, double* pcoord, Curve& crv, double* u)
{
pvec.push_back(p.x);
pvec.push_back(p.y);
@@ -2109,7 +2114,8 @@ ConstraintCurveValue::ConstraintCurveValue(Point &p, double* pcoord, Curve& crv,
ConstraintCurveValue::~ConstraintCurveValue()
{
delete this->crv; this->crv = nullptr;
delete this->crv;
this->crv = nullptr;
}
void ConstraintCurveValue::ReconstructGeomPointers()
@@ -2135,40 +2141,43 @@ void ConstraintCurveValue::rescale(double coef)
scale = coef * 1;
}
void ConstraintCurveValue::errorgrad(double *err, double *grad, double *param)
void ConstraintCurveValue::errorgrad(double* err, double* grad, double* param)
{
if (pvecChangedFlag) ReconstructGeomPointers();
if (pvecChangedFlag)
ReconstructGeomPointers();
double u, du;
u = *(this->u()); du = ( param == this->u() ) ? 1.0 : 0.0;
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_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 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 (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
}
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..*/);
}
else {
assert(false /*this constraint is neither X nor Y. Nothing to do..*/);
}
}
double ConstraintCurveValue::error()
{
double err;
errorgrad(&err,nullptr,nullptr);
errorgrad(&err, nullptr, nullptr);
return scale * err;
}
@@ -2184,7 +2193,7 @@ double ConstraintCurveValue::grad(double* param)
return deriv * scale;
}
double ConstraintCurveValue::maxStep(MAP_pD_D &/*dir*/, double lim)
double ConstraintCurveValue::maxStep(MAP_pD_D& /*dir*/, double lim)
{
// step(angle()) <= pi/18 = 10°
/* TODO: curve-dependent parameter change limiting??
@@ -2201,7 +2210,7 @@ double ConstraintCurveValue::maxStep(MAP_pD_D &/*dir*/, double lim)
// --------------------------------------------------------
// ConstraintPointOnHyperbola
ConstraintPointOnHyperbola::ConstraintPointOnHyperbola(Point &p, Hyperbola &e)
ConstraintPointOnHyperbola::ConstraintPointOnHyperbola(Point& p, Hyperbola& e)
{
pvec.push_back(p.x);
pvec.push_back(p.y);
@@ -2214,7 +2223,7 @@ ConstraintPointOnHyperbola::ConstraintPointOnHyperbola(Point &p, Hyperbola &e)
rescale();
}
ConstraintPointOnHyperbola::ConstraintPointOnHyperbola(Point &p, ArcOfHyperbola &e)
ConstraintPointOnHyperbola::ConstraintPointOnHyperbola(Point& p, ArcOfHyperbola& e)
{
pvec.push_back(p.x);
pvec.push_back(p.y);
@@ -2315,7 +2324,7 @@ double ConstraintPointOnHyperbola::grad(double* param)
// --------------------------------------------------------
// ConstraintPointOnParabola
ConstraintPointOnParabola::ConstraintPointOnParabola(Point &p, Parabola &e)
ConstraintPointOnParabola::ConstraintPointOnParabola(Point& p, Parabola& e)
{
pvec.push_back(p.x);
pvec.push_back(p.y);
@@ -2326,7 +2335,7 @@ ConstraintPointOnParabola::ConstraintPointOnParabola(Point &p, Parabola &e)
rescale();
}
ConstraintPointOnParabola::ConstraintPointOnParabola(Point &p, ArcOfParabola &e)
ConstraintPointOnParabola::ConstraintPointOnParabola(Point& p, ArcOfParabola& e)
{
pvec.push_back(p.x);
pvec.push_back(p.y);
@@ -2339,14 +2348,17 @@ ConstraintPointOnParabola::ConstraintPointOnParabola(Point &p, ArcOfParabola &e)
ConstraintPointOnParabola::~ConstraintPointOnParabola()
{
delete this->parab; this->parab = nullptr;
delete this->parab;
this->parab = nullptr;
}
void ConstraintPointOnParabola::ReconstructGeomPointers()
{
int i=0;
p.x=pvec[i]; i++;
p.y=pvec[i]; i++;
int i = 0;
p.x = pvec[i];
i++;
p.y = pvec[i];
i++;
this->parab->ReconstructOnNewPvec(pvec, i);
pvecChangedFlag = false;
}
@@ -2361,14 +2373,15 @@ void ConstraintPointOnParabola::rescale(double coef)
scale = coef * 1;
}
void ConstraintPointOnParabola::errorgrad(double *err, double *grad, double *param)
void ConstraintPointOnParabola::errorgrad(double* err, double* grad, double* param)
{
if (pvecChangedFlag) ReconstructGeomPointers();
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 point(this->p, param);// point to be constrained to parabola
DeriVector2 focalvect = focus.subtr(vertex);
@@ -2389,15 +2402,15 @@ void ConstraintPointOnParabola::errorgrad(double *err, double *grad, double *par
proj = point_to_focus.scalarProd(xdir, &dproj);
if (err)
*err = pf - 2*focal - proj;
*err = pf - 2 * focal - proj;
if (grad)
*grad = dpf - 2*dfocal - dproj;
*grad = dpf - 2 * dfocal - dproj;
}
double ConstraintPointOnParabola::error()
{
double err;
errorgrad(&err,nullptr,nullptr);
errorgrad(&err, nullptr, nullptr);
return scale * err;
}
@@ -2405,7 +2418,7 @@ double ConstraintPointOnParabola::grad(double* param)
{
// first of all, check that we need to compute anything.
if (findParamInPvec(param) == -1)
return 0.0;
return 0.0;
double deriv;
errorgrad(nullptr, &deriv, param);
@@ -2432,19 +2445,23 @@ ConstraintAngleViaPoint::ConstraintAngleViaPoint(Curve& acrv1, Curve& acrv2, Poi
ConstraintAngleViaPoint::~ConstraintAngleViaPoint()
{
delete crv1; crv1 = nullptr;
delete crv2; crv2 = nullptr;
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;
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()
@@ -2477,21 +2494,23 @@ double ConstraintAngleViaPoint::error()
return scale * err;
}
double ConstraintAngleViaPoint::grad(double *param)
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.;
double deriv = 0.;
if (pvecChangedFlag) ReconstructGeomPointers();
if (pvecChangedFlag)
ReconstructGeomPointers();
if (param == angle()) deriv += -1.0;
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) );
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));
// use numeric for testing
@@ -2541,9 +2560,12 @@ ConstraintSnell::ConstraintSnell(Curve& ray1, Curve& ray2, Curve& boundary, Poin
ConstraintSnell::~ConstraintSnell()
{
delete ray1; ray1 = nullptr;
delete ray2; ray2 = nullptr;
delete boundary; boundary = nullptr;
delete ray1;
ray1 = nullptr;
delete ray2;
ray2 = nullptr;
delete boundary;
boundary = nullptr;
}
void ConstraintSnell::ReconstructGeomPointers()
@@ -2606,7 +2628,7 @@ double ConstraintSnell::error()
return scale * err;
}
double ConstraintSnell::grad(double *param)
double ConstraintSnell::grad(double* param)
{
// first of all, check that we need to compute anything.
if (findParamInPvec(param) == -1)
@@ -2652,7 +2674,7 @@ ConstraintEqualLineLength::ConstraintEqualLineLength(Line& l1, Line& l2)
void ConstraintEqualLineLength::ReconstructGeomPointers()
{
int i=0;
int i = 0;
l1.ReconstructOnNewPvec(pvec, i);
l2.ReconstructOnNewPvec(pvec, i);
pvecChangedFlag = false;
@@ -2668,14 +2690,15 @@ void ConstraintEqualLineLength::rescale(double coef)
scale = coef * 1;
}
void ConstraintEqualLineLength::errorgrad(double *err, double *grad, double *param)
void ConstraintEqualLineLength::errorgrad(double* err, double* grad, double* param)
{
if (pvecChangedFlag) ReconstructGeomPointers();
if (pvecChangedFlag)
ReconstructGeomPointers();
DeriVector2 p1 (l1.p1, param);
DeriVector2 p2 (l1.p2, param);
DeriVector2 p3 (l2.p1, param);
DeriVector2 p4 (l2.p2, param);
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);
@@ -2691,12 +2714,13 @@ void ConstraintEqualLineLength::errorgrad(double *err, double *grad, double *par
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.
// 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
// 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)
@@ -2775,20 +2799,21 @@ void ConstraintC2CDistance::rescale(double coef)
scale = coef * 1;
}
void ConstraintC2CDistance::errorgrad(double *err, double *grad, double *param)
void ConstraintC2CDistance::errorgrad(double* err, double* grad, double* param)
{
if (pvecChangedFlag) ReconstructGeomPointers();
if (pvecChangedFlag)
ReconstructGeomPointers();
DeriVector2 ct1 (c1.center, param);
DeriVector2 ct2 (c2.center, param);
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.
// 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());
@@ -2799,8 +2824,8 @@ void ConstraintC2CDistance::errorgrad(double *err, double *grad, double *param)
}
}
else {
double * bigradius = (*c1.rad >= *c2.rad)?c1.rad:c2.rad;
double * smallradius = (*c1.rad >= *c2.rad)?c2.rad:c1.rad;
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();
@@ -2832,11 +2857,11 @@ void ConstraintC2CDistance::errorgrad(double *err, double *grad, double *param)
double ConstraintC2CDistance::error()
{
double err;
errorgrad(&err,nullptr,nullptr);
errorgrad(&err, nullptr, nullptr);
return scale * err;
}
double ConstraintC2CDistance::grad(double *param)
double ConstraintC2CDistance::grad(double* param)
{
if (findParamInPvec(param) == -1)
return 0.0;
@@ -2849,7 +2874,7 @@ double ConstraintC2CDistance::grad(double *param)
// --------------------------------------------------------
// ConstraintC2LDistance
ConstraintC2LDistance::ConstraintC2LDistance(Circle &c, Line &l, double *d)
ConstraintC2LDistance::ConstraintC2LDistance(Circle& c, Line& l, double* d)
{
this->d = d;
pvec.push_back(d);
@@ -2884,19 +2909,20 @@ void ConstraintC2LDistance::ReconstructGeomPointers()
pvecChangedFlag = false;
}
void ConstraintC2LDistance::errorgrad(double *err, double *grad, double *param)
void ConstraintC2LDistance::errorgrad(double* err, double* grad, double* param)
{
if (pvecChangedFlag) ReconstructGeomPointers();
if (pvecChangedFlag)
ReconstructGeomPointers();
DeriVector2 ct (circle.center, param);
DeriVector2 p1 (line.p1, param);
DeriVector2 p2 (line.p2, param);
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)
// 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 area = v_line.crossProdNorm(v_p1ct, darea);// parallelogram oriented area
double dlength;
double length = v_line.length(dlength);
@@ -2915,15 +2941,16 @@ void ConstraintC2LDistance::errorgrad(double *err, double *grad, double *param)
// and a positive value makes it decrease.
darea = std::signbit(area) ? -darea : darea;
double dh = (darea - h * dlength) / length;
double dh = (darea - h * dlength) / length;
if (err) {
*err = *distance() + *circle.rad - h;
}
else if (grad) {
if ( param == distance() || param == circle.rad) {
if (param == distance() || param == circle.rad) {
*grad = 1.0;
} else {
}
else {
*grad = -dh;
}
}
@@ -2932,11 +2959,11 @@ void ConstraintC2LDistance::errorgrad(double *err, double *grad, double *param)
double ConstraintC2LDistance::error()
{
double err;
errorgrad(&err,nullptr,nullptr);
errorgrad(&err, nullptr, nullptr);
return scale * err;
}
double ConstraintC2LDistance::grad(double *param)
double ConstraintC2LDistance::grad(double* param)
{
if (findParamInPvec(param) == -1)
return 0.0;
@@ -2947,4 +2974,4 @@ double ConstraintC2LDistance::grad(double *param)
return deriv * scale;
}
} //namespace GCS
}// namespace GCS

1828
src/Mod/Sketcher/App/planegcs/Constraints.h
View File

File diff suppressed because it is too large Load Diff

2811
src/Mod/Sketcher/App/planegcs/GCS.cpp
View File

File diff suppressed because it is too large Load Diff

765
src/Mod/Sketcher/App/planegcs/GCS.h
View File

@@ -25,32 +25,31 @@
#include <Eigen/QR>
#include "SubSystem.h"
#include "../../SketcherGlobal.h"
#include "SubSystem.h"
#define EIGEN_VERSION (EIGEN_WORLD_VERSION * 10000 \
+ EIGEN_MAJOR_VERSION * 100 \
+ EIGEN_MINOR_VERSION)
#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>
#define EIGEN_SPARSEQR_COMPATIBLE
#include <Eigen/Sparse>
#endif
namespace GCS
{
///////////////////////////////////////
// Other BFGS Solver parameters
///////////////////////////////////////
#define XconvergenceRough 1e-8
#define smallF 1e-20
///////////////////////////////////////
// Other BFGS Solver parameters
///////////////////////////////////////
#define XconvergenceRough 1e-8
#define smallF 1e-20
///////////////////////////////////////
// Solver
///////////////////////////////////////
///////////////////////////////////////
// Solver
///////////////////////////////////////
enum SolveStatus
enum SolveStatus
{
Success = 0, // Found a solution zeroing the error function
Converged = 1, // Found a solution minimizing the error function
@@ -59,422 +58,418 @@ namespace GCS
// resulting geometry is OCE-invalid
};
enum Algorithm {
BFGS = 0,
LevenbergMarquardt = 1,
DogLeg = 2
};
enum Algorithm
{
BFGS = 0,
LevenbergMarquardt = 1,
DogLeg = 2
};
enum DogLegGaussStep {
FullPivLU = 0,
LeastNormFullPivLU = 1,
LeastNormLdlt = 2
};
enum DogLegGaussStep
{
FullPivLU = 0,
LeastNormFullPivLU = 1,
LeastNormLdlt = 2
};
enum QRAlgorithm {
EigenDenseQR = 0,
EigenSparseQR = 1
};
enum QRAlgorithm
{
EigenDenseQR = 0,
EigenSparseQR = 1
};
enum DebugMode {
NoDebug = 0,
Minimal = 1,
IterationLevel = 2
};
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
};
// 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 SketcherExport System
{
class SketcherExport 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;
private:
VEC_pD plist; // list of the unknown parameters
VEC_pD pdrivenlist;// list of parameters of driven constraints
MAP_pD_I pIndex;
VEC_pD pDependentParameters; // list of dependent parameters by the system
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;
// 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<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();
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
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
std::vector<std::vector<Constraint*>>
clists; // partitioned clist except equality constraints
std::vector<MAP_pD_pD> reductionmaps;// for simplification of equality constraints
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;
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 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.
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);
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 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);
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);
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);
// 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,
template<typename T>
void identifyConflictingRedundantConstraints(Algorithm alg, const T& qrJT,
const std::map<int, int>& jacobianconstraintmap,
const GCS::VEC_pD& pdiagnoselist,
bool silent = true);
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);
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);
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;
#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();
public:
System();
/*System(std::vector<Constraint *> clist_);*/
~System();
void clear();
void clearByTag(int tagId);
void clear();
void clearByTag(int tagId);
int addConstraint(Constraint *constr);
void removeConstraint(Constraint *constr);
int addConstraint(Constraint* constr);
void removeConstraint(Constraint* constr);
// basic constraints
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,
// basic constraints
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 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 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,
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 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);
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);
// 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);
// 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 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);
// 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;
void calculateNormalAtPoint(const Curve& crv, const Point& p, double& rtnX,
double& rtnY) const;
double calculateAngleViaPoint(const Curve& crv1, const Curve& crv2, Point& p) const;
double calculateAngleViaPoint(const Curve& crv1, const Curve& crv2, Point& p1, Point& p2) 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);
// 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 rescaleConstraint(int id, double coeff);
void declareUnknowns(VEC_pD &params);
void declareDrivenParams(VEC_pD &params);
void initSolution(Algorithm alg=DogLeg);
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);
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;
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();
}
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;
});
}
};
return std::count_if(clist.begin(), clist.end(), [tagID](Constraint* constraint) {
return constraint->getTag() == tagID;
});
}
};
///////////////////////////////////////
// Helper elements
///////////////////////////////////////
///////////////////////////////////////
// Helper elements
///////////////////////////////////////
void free(VEC_pD &doublevec);
void free(std::vector<Constraint *> &constrvec);
void free(std::vector<SubSystem *> &subsysvec);
void free(VEC_pD& doublevec);
void free(std::vector<Constraint*>& constrvec);
void free(std::vector<SubSystem*>& subsysvec);
} //namespace GCS
}// namespace GCS
#endif // PLANEGCS_GCS_H
#endif// PLANEGCS_GCS_H

565
src/Mod/Sketcher/App/planegcs/Geo.cpp
View File

@@ -29,9 +29,10 @@
#include "Geo.h"
namespace GCS{
namespace GCS
{
DeriVector2::DeriVector2(const Point &p, const double *derivparam)
DeriVector2::DeriVector2(const Point& p, const double* derivparam)
{
x = *p.x;
y = *p.y;
@@ -43,7 +44,7 @@ DeriVector2::DeriVector2(const Point &p, const double *derivparam)
dy = 1.0;
}
double DeriVector2::length(double &dlength) const
double DeriVector2::length(double& dlength) const
{
double l = length();
if (l == 0) {
@@ -77,7 +78,7 @@ DeriVector2 DeriVector2::getNormalized() const
}
}
double DeriVector2::scalarProd(const DeriVector2 &v2, double *dprd) const
double DeriVector2::scalarProd(const DeriVector2& v2, double* dprd) const
{
if (dprd) {
*dprd = dx * v2.x + x * v2.dx + dy * v2.y + y * v2.dy;
@@ -91,10 +92,10 @@ DeriVector2 DeriVector2::divD(double val, double dval) const
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
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;
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
@@ -105,9 +106,9 @@ DeriVector2 Curve::Value(double /*u*/, double /*du*/, const double* /*derivparam
//----------------Line
DeriVector2 Line::CalculateNormal(const Point &p, const double* derivparam) const
DeriVector2 Line::CalculateNormal(const Point& p, const double* derivparam) const
{
(void) p;
(void)p;
DeriVector2 p1v(p1, derivparam);
DeriVector2 p2v(p2, derivparam);
@@ -120,24 +121,32 @@ DeriVector2 Line::Value(double u, double du, const double* derivparam) const
DeriVector2 p2v(p2, derivparam);
DeriVector2 line_vec = p2v.subtr(p1v);
return p1v.sum(line_vec.multD(u,du));
return p1v.sum(line_vec.multD(u, du));
}
int Line::PushOwnParams(VEC_pD &pvec)
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++;
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)
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++;
p1.x = pvec[cnt];
cnt++;
p1.y = pvec[cnt];
cnt++;
p2.x = pvec[cnt];
cnt++;
p2.y = pvec[cnt];
cnt++;
}
Line* Line::Copy()
{
@@ -148,10 +157,10 @@ Line* Line::Copy()
//---------------circle
DeriVector2 Circle::CalculateNormal(const Point &p, const double* derivparam) const
DeriVector2 Circle::CalculateNormal(const Point& p, const double* derivparam) const
{
DeriVector2 cv (center, derivparam);
DeriVector2 pv (p, derivparam);
DeriVector2 cv(center, derivparam);
DeriVector2 pv(p, derivparam);
return cv.subtr(pv);
}
@@ -172,19 +181,25 @@ DeriVector2 Circle::Value(double u, double du, const double* derivparam) const
return cv.sum(ex.multD(co, dco).sum(ey.multD(si, dsi)));
}
int Circle::PushOwnParams(VEC_pD &pvec)
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++;
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)
void Circle::ReconstructOnNewPvec(VEC_pD& pvec, int& cnt)
{
center.x=pvec[cnt]; cnt++;
center.y=pvec[cnt]; cnt++;
rad=pvec[cnt]; cnt++;
center.x = pvec[cnt];
cnt++;
center.y = pvec[cnt];
cnt++;
rad = pvec[cnt];
cnt++;
}
Circle* Circle::Copy()
{
@@ -193,27 +208,39 @@ Circle* Circle::Copy()
}
//------------arc
int Arc::PushOwnParams(VEC_pD &pvec)
int Arc::PushOwnParams(VEC_pD& pvec)
{
int cnt=0;
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++;
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)
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++;
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()
{
@@ -239,40 +266,40 @@ double Ellipse::getRadMaj(const DeriVector2& center, const DeriVector2& f1, doub
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
// 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);
return getRadMaj(c, f1, *radmin, radmin == derivparam ? 1.0 : 0.0, ret_dRadMaj);
}
//returns the major radius (plain value, no derivatives)
// returns the major radius (plain value, no derivatives)
double Ellipse::getRadMaj() const
{
double dradmaj;//dummy
return getRadMaj(nullptr,dradmaj);
double dradmaj;// dummy
return getRadMaj(nullptr, dradmaj);
}
DeriVector2 Ellipse::CalculateNormal(const Point &p, const double* derivparam) const
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);
// 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
// calculation.
// focus2:
DeriVector2 f2v = cv.linCombi(2.0, f1v, -1.0);// 2*cv - f1v
//pf1, pf2 = vectors from p to focus1,focus2
// pf1, pf2 = vectors from p to focus1,focus2
DeriVector2 pf1 = f1v.subtr(pv);
DeriVector2 pf2 = f2v.subtr(pv);
//return sum of normalized pf2, pf2
// return sum of normalized pf2, pf2
DeriVector2 ret = pf1.getNormalized().sum(pf2.getNormalized());
//numeric derivatives for testing
#if 0 //make sure to enable DEBUG_DERIVS when enabling
// numeric derivatives for testing
#if 0// make sure to enable DEBUG_DERIVS when enabling
if(derivparam) {
double const eps = 0.00001;
double oldparam = *derivparam;
@@ -290,18 +317,18 @@ DeriVector2 Ellipse::CalculateNormal(const Point &p, const double* derivparam) c
assert(ret.dy <= std::max(numretl.y,numretr.y) );
assert(ret.dy >= std::min(numretl.y,numretr.y) );
}
#endif
#endif
return ret;
return ret;
}
DeriVector2 Ellipse::Value(double u, double du, const double* derivparam) const
{
//In local coordinate system, value() of ellipse is:
// 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.
// In global, it is (vector formula):
// center + a_vec*cos(u) + b_vec*sin(u).
// That's what is being computed here.
// <construct a_vec, b_vec>
DeriVector2 c(this->center, derivparam);
@@ -310,41 +337,53 @@ DeriVector2 Ellipse::Value(double u, double du, const double* derivparam) const
DeriVector2 emaj = f1.subtr(c).getNormalized();
DeriVector2 emin = emaj.rotate90ccw();
double b, db;
b = *(this->radmin); db = this->radmin==derivparam ? 1.0 : 0.0;
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);
a = this->getRadMaj(c, f1, b, db, da);
DeriVector2 a_vec = emaj.multD(a, da);
DeriVector2 b_vec = emin.multD(b, db);
// </construct a_vec, b_vec>
// 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;
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);
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 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++;
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)
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++;
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()
{
@@ -354,28 +393,39 @@ Ellipse* Ellipse::Copy()
//---------------arc of ellipse
int ArcOfEllipse::PushOwnParams(VEC_pD &pvec)
int ArcOfEllipse::PushOwnParams(VEC_pD& pvec)
{
int cnt=0;
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++;
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)
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++;
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()
{
@@ -398,19 +448,19 @@ double Hyperbola::getRadMaj(const DeriVector2& center, const DeriVector2& f1, do
return a;
}
//returns major radius. The derivative by derivparam is returned into ret_dRadMaj argument.
double Hyperbola::getRadMaj(double *derivparam, double &ret_dRadMaj) const
// 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);
return getRadMaj(c, f1, *radmin, radmin == derivparam ? 1.0 : 0.0, ret_dRadMaj);
}
//returns the major radius (plain value, no derivatives)
// returns the major radius (plain value, no derivatives)
double Hyperbola::getRadMaj() const
{
double dradmaj;//dummy
return getRadMaj(nullptr,dradmaj);
double dradmaj;// dummy
return getRadMaj(nullptr, dradmaj);
}
DeriVector2 Hyperbola::CalculateNormal(const Point& p, const double* derivparam) const
@@ -437,11 +487,11 @@ DeriVector2 Hyperbola::CalculateNormal(const Point& p, const double* derivparam)
DeriVector2 Hyperbola::Value(double u, double du, const double* derivparam) const
{
//In local coordinate system, value() of hyperbola is:
// 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.
// In global, it is (vector formula):
// center + a_vec*cosh(u) + b_vec*sinh(u).
// That's what is being computed here.
// <construct a_vec, b_vec>
DeriVector2 c(this->center, derivparam);
@@ -450,40 +500,53 @@ DeriVector2 Hyperbola::Value(double u, double du, const double* derivparam) cons
DeriVector2 emaj = f1.subtr(c).getNormalized();
DeriVector2 emin = emaj.rotate90ccw();
double b, db;
b = *(this->radmin); db = this->radmin==derivparam ? 1.0 : 0.0;
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);
a = this->getRadMaj(c, f1, b, db, da);
DeriVector2 a_vec = emaj.multD(a, da);
DeriVector2 b_vec = emin.multD(b, db);
// </construct a_vec, b_vec>
// 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;
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);
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 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++;
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)
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++;
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()
{
@@ -492,28 +555,39 @@ Hyperbola* Hyperbola::Copy()
}
//--------------- arc of hyperbola
int ArcOfHyperbola::PushOwnParams(VEC_pD &pvec)
int ArcOfHyperbola::PushOwnParams(VEC_pD& pvec)
{
int cnt=0;
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++;
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)
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++;
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()
{
@@ -543,49 +617,57 @@ DeriVector2 Parabola::CalculateNormal(const Point& p, const double* derivparam)
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
// 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;
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 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
DeriVector2 ret;// point of parabola at parameter value of u, in global coordinates
ret = c.sum( dir );
ret = c.sum(dir);
return ret;
}
int Parabola::PushOwnParams(VEC_pD &pvec)
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++;
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)
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++;
vertex.x = pvec[cnt];
cnt++;
vertex.y = pvec[cnt];
cnt++;
focus1.x = pvec[cnt];
cnt++;
focus1.y = pvec[cnt];
cnt++;
}
Parabola* Parabola::Copy()
@@ -595,28 +677,39 @@ Parabola* Parabola::Copy()
}
//--------------- arc of hyperbola
int ArcOfParabola::PushOwnParams(VEC_pD &pvec)
int ArcOfParabola::PushOwnParams(VEC_pD& pvec)
{
int cnt=0;
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++;
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)
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++;
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()
{
@@ -625,7 +718,7 @@ ArcOfParabola* ArcOfParabola::Copy()
}
// bspline
DeriVector2 BSpline::CalculateNormal(const Point &p, const double* derivparam) const
DeriVector2 BSpline::CalculateNormal(const Point& p, const double* derivparam) const
{
// place holder
DeriVector2 ret;
@@ -636,9 +729,9 @@ DeriVector2 BSpline::CalculateNormal(const Point &p, const double* derivparam) c
//
// 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) {
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);
@@ -647,21 +740,23 @@ DeriVector2 BSpline::CalculateNormal(const Point &p, const double* derivparam) c
DeriVector2 tg = endpt.subtr(spt);
ret = tg.rotate90ccw();
}
else if(*p.x == *end.x && *p.y == *end.y) {
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 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
}
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
// either periodic or abnormal endpoint multiplicity, we have no clue so currently
// unsupported
ret = DeriVector2();
}
@@ -677,13 +772,13 @@ DeriVector2 BSpline::Value(double /*u*/, double /*du*/, const double* /*derivpar
return ret;
}
int BSpline::PushOwnParams(VEC_pD &pvec)
int BSpline::PushOwnParams(VEC_pD& pvec)
{
std::size_t cnt=0;
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 );
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;
@@ -694,33 +789,45 @@ int BSpline::PushOwnParams(VEC_pD &pvec)
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++;
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<int>(cnt);
}
void BSpline::ReconstructOnNewPvec(VEC_pD &pvec, int &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_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 = 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++;
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++;
start.x = pvec[cnt];
cnt++;
start.y = pvec[cnt];
cnt++;
end.x = pvec[cnt];
cnt++;
end.y = pvec[cnt];
cnt++;
}
BSpline* BSpline::Copy()
@@ -785,10 +892,10 @@ void BSpline::setupFlattenedKnots()
// 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
int c = degree + 1 - mult[0];// number of knots to pad
// Add capacity so that iterators remain valid
flattenedknots.reserve(flattenedknots.size() + 2*c);
flattenedknots.reserve(flattenedknots.size() + 2 * c);
// get iterators first for convenience
auto frontStart = flattenedknots.end() - mult.back() - c;
@@ -810,4 +917,4 @@ void BSpline::setupFlattenedKnots()
}
}
}//namespace GCS
}// namespace GCS

689
src/Mod/Sketcher/App/planegcs/Geo.h
View File

@@ -30,345 +30,400 @@
namespace GCS
{
class Point
class Point
{
public:
Point()
{
public:
Point()
{
x = nullptr;
y = nullptr;
}
Point(double* px, double* py)
{
x = px;
y = py;
}
double* x;
double* y;
};
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
x = nullptr;
y = nullptr;
}
Point(double* px, double* py)
{
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;
x = px;
y = py;
}
double* x;
double* y;
};
double length() const
{
return sqrt(x * x + y * y);
}
double length(double& dlength)
const;// returns length and writes length deriv into the dlength argument.
using VEC_P = std::vector<Point>;
// 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.
double scalarProd(const DeriVector2& v2, double* dprd = nullptr)
const;// calculates scalar product of two vectors and returns the result. The derivative
// of the result is written into argument dprd.
double crossProdNorm(const DeriVector2& v2, double& dprd)
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.
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);
}
DeriVector2 divD(double val,
double dval) const;// divide vector by a variable with a derivative
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
///////////////////////////////////////
class Curve //a base class for all curve-based objects (line, circle/arc, ellipse/arc)
/// 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()
{
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;
/**
* @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;
virtual Curve* Copy() = 0; //DeepSOIC: I haven't found a way to simply copy a curve object provided pointer to a curve object.
};
class Line: public Curve
x = 0;
y = 0;
dx = 0;
dy = 0;
}
DeriVector2(double x, double y)
{
public:
Line(){}
~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
this->x = x;
this->y = y;
this->dx = 0;
this->dy = 0;
}
DeriVector2(double x, double y, double dx, double dy)
{
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;
};
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;
class Arc: public Circle
double length() const
{
public:
Arc(){startAngle=nullptr;endAngle=nullptr;rad=nullptr;}
~Arc() override{}
double *startAngle;
double *endAngle;
//double *rad; //inherited
Point start;
Point end;
//Point center; //inherited
int PushOwnParams(VEC_pD &pvec) override;
void ReconstructOnNewPvec (VEC_pD &pvec, int &cnt) override;
Arc* Copy() override;
};
return sqrt(x * x + y * y);
}
// returns length and writes length deriv into the dlength argument.
double length(double& dlength) const;
class MajorRadiusConic: public Curve
// 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
{
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
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
{
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
return DeriVector2(-y, x, -dy, dx);
}
DeriVector2 rotate90cw() const
{
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;
};
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);
}
};
class Hyperbola: public MajorRadiusConic
///////////////////////////////////////
// Geometries
///////////////////////////////////////
class Curve// a base class for all curve-based objects (line, circle/arc, ellipse/arc)
{
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;
/**
* @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() 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()
{
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;
};
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 ArcOfHyperbola: public Hyperbola
class Arc: public Circle
{
public:
Arc()
{
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;
};
startAngle = nullptr;
endAngle = nullptr;
rad = nullptr;
}
~Arc() override
{}
double* startAngle;
double* endAngle;
// double *rad; //inherited
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 Parabola: public Curve
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()
{
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;
};
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 ArcOfParabola: public Parabola
class ArcOfEllipse: public Ellipse
{
public:
ArcOfEllipse()
{
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;
};
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 BSpline: public Curve
class Hyperbola: public MajorRadiusConic
{
public:
Hyperbola()
{
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
VEC_D flattenedknots; // knot vector with repetitions for multiplicity and "padding" for periodic spline
// interface helpers
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;
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 (this 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);
};
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;
};
} //namespace GCS
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;
};
#endif // PLANEGCS_GEO_H
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;
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;
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 (this 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

157
src/Mod/Sketcher/App/planegcs/SubSystem.cpp
View File

@@ -30,25 +30,23 @@ namespace GCS
{
// SubSystem
SubSystem::SubSystem(std::vector<Constraint *> &clist_, VEC_pD &params)
: clist(clist_)
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_)
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)
void SubSystem::initialize(VEC_pD& params, MAP_pD_pD& reductionmap)
{
csize = static_cast<int>(clist.size());
@@ -101,146 +99,141 @@ void SubSystem::initialize(VEC_pD &params, MAP_pD_pD &reductionmap)
psize = static_cast<int>(plist.size());
pvals.resize(psize);
pmap.clear();
for (int j=0; j < psize; j++) {
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)
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
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) {
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.);
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
// (*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)
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
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)
for (std::vector<Constraint*>::iterator constr = clist.begin(); constr != clist.end(); ++constr)
(*constr)->revertParams();
}
void SubSystem::getParamMap(MAP_pD_pD &pmapOut)
void SubSystem::getParamMap(MAP_pD_pD& pmapOut)
{
pmapOut = pmap;
}
void SubSystem::getParamList(VEC_pD &plistOut)
void SubSystem::getParamList(VEC_pD& plistOut)
{
plistOut = plist;
}
void SubSystem::getParams(VEC_pD &params, Eigen::VectorXd &xOut)
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]);
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)
void SubSystem::getParams(Eigen::VectorXd& xOut)
{
if (xOut.size() != psize)
xOut.setZero(psize);
for (int i=0; i < psize; i++)
for (int i = 0; i < psize; i++)
xOut[i] = pvals[i];
}
void SubSystem::setParams(VEC_pD &params, Eigen::VectorXd &xIn)
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]);
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)
void SubSystem::setParams(Eigen::VectorXd& xIn)
{
assert(xIn.size() == psize);
for (int i=0; i < psize; i++)
for (int i = 0; i < psize; i++)
pvals[i] = xIn[i];
}
void SubSystem::getConstraintList(std::vector<Constraint *> &clist_)
void SubSystem::getConstraintList(std::vector<Constraint*>& clist_)
{
clist_= clist;
clist_ = clist;
}
double SubSystem::error()
{
double err = 0.;
for (std::vector<Constraint *>::const_iterator constr=clist.begin();
constr != clist.end(); ++constr) {
for (std::vector<Constraint*>::const_iterator constr = clist.begin(); constr != clist.end();
++constr) {
double tmp = (*constr)->error();
err += tmp*tmp;
err += tmp * tmp;
}
err *= 0.5;
return err;
}
void SubSystem::calcResidual(Eigen::VectorXd &r)
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++) {
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)
void SubSystem::calcResidual(Eigen::VectorXd& r, double& err)
{
assert(r.size() == csize);
int i=0;
int i = 0;
err = 0.;
for (std::vector<Constraint *>::const_iterator constr=clist.begin();
constr != clist.end(); ++constr, i++) {
for (std::vector<Constraint*>::const_iterator constr = clist.begin(); constr != clist.end();
++constr, i++) {
r[i] = (*constr)->error();
err += r[i]*r[i];
err += r[i] * r[i];
}
err *= 0.5;
}
@@ -261,74 +254,71 @@ void SubSystem::calcJacobi()
}
*/
void SubSystem::calcJacobi(VEC_pD &params, Eigen::MatrixXd &jacobi)
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]);
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);
for (int i = 0; i < csize; i++)
jacobi(i, j) = clist[i]->grad(pmapfind->second);
}
}
void SubSystem::calcJacobi(Eigen::MatrixXd &jacobi)
void SubSystem::calcJacobi(Eigen::MatrixXd& jacobi)
{
calcJacobi(plist, jacobi);
}
void SubSystem::calcGrad(VEC_pD &params, Eigen::VectorXd &grad)
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]);
for (int j = 0; j < int(params.size()); j++) {
MAP_pD_pD::const_iterator pmapfind = pmap.find(params[j]);
if (pmapfind != pmap.end()) {
// assert(p2c.find(pmapfind->second) != p2c.end());
std::vector<Constraint *> constrs=p2c[pmapfind->second];
for (std::vector<Constraint *>::const_iterator constr = constrs.begin();
constr != constrs.end(); ++constr)
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)
void SubSystem::calcGrad(Eigen::VectorXd& grad)
{
calcGrad(plist, grad);
}
double SubSystem::maxStep(VEC_pD &params, Eigen::VectorXd &xdir)
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++) {
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)
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)
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)
for (MAP_pD_pD::const_iterator it = pmap.begin(); it != pmap.end(); ++it)
*(it->first) = *(it->second);
}
@@ -336,18 +326,17 @@ void SubSystem::analyse(Eigen::MatrixXd& /*J*/, Eigen::MatrixXd& /*ker*/, Eigen:
{}
void SubSystem::report()
{
}
{}
void SubSystem::printResidual()
{
Eigen::VectorXd r(csize);
int i=0;
int i = 0;
double err = 0.;
for (std::vector<Constraint *>::const_iterator constr=clist.begin();
constr != clist.end(); ++constr, i++) {
for (std::vector<Constraint*>::const_iterator constr = clist.begin(); constr != clist.end();
++constr, i++) {
r[i] = (*constr)->error();
err += r[i]*r[i];
err += r[i] * r[i];
}
err *= 0.5;
std::cout << "Residual r = " << r.transpose() << std::endl;
@@ -355,4 +344,4 @@ void SubSystem::printResidual()
}
} //namespace GCS
}// namespace GCS

109
src/Mod/Sketcher/App/planegcs/SubSystem.h
View File

@@ -34,60 +34,65 @@
namespace GCS
{
class SubSystem
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()
{
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();
return psize;
};
int cSize()
{
return csize;
};
double lineSearch(SubSystem *subsys, Eigen::VectorXd &xdir);
void redirectParams();
void revertParams();
} //namespace GCS
void getParamMap(MAP_pD_pD& pmapOut);
void getParamList(VEC_pD& plistOut);
#endif // PLANEGCS_SUBSYSTEM_H
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

20
src/Mod/Sketcher/App/planegcs/Util.h
View File

@@ -30,19 +30,19 @@
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_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>;
#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif
} //namespace GCS
}// namespace GCS
#endif // PLANEGCS_UTIL_H
#endif// PLANEGCS_UTIL_H

22
src/Mod/Sketcher/App/planegcs/qp_eq.cpp
View File

@@ -24,18 +24,17 @@
#pragma warning(disable : 4244)
#endif
#include <iostream>
#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)
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 ();
MatrixXd Q = qrAT.matrixQ();
size_t params_num = qrAT.rows();
size_t constr_num = qrAT.cols();
@@ -48,22 +47,23 @@ int qp_eq(MatrixXd &H, VectorXd &g, MatrixXd &A, VectorXd &c,
// 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))
Y = qrAT.matrixQR()
.topRows(constr_num)
.triangularView<Upper>()
.transpose()
.solve<OnTheRight>(Q.leftCols(rank))
* qrAT.colsPermutation().transpose();
if (params_num == rank)
x = - Y * c;
x = -Y * c;
else {
Z = Q.rightCols(params_num-rank);
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;
x = -Y * c + Z * y;
}
return 0;

4
src/Mod/Sketcher/App/planegcs/qp_eq.h
View File

@@ -21,5 +21,5 @@
***************************************************************************/
#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);
int qp_eq(Eigen::MatrixXd& H, Eigen::VectorXd& g, Eigen::MatrixXd& A, Eigen::VectorXd& c,
Eigen::VectorXd& x, Eigen::MatrixXd& Y, Eigen::MatrixXd& Z);