Sketcher Ellipse and ArcOfEllipse: Extra features and bug fixes
- Minor changes to alignment constraint selection to avoid to create objects just for simple calculations - Equality constraint of Ellipse, ArcOfEllipse and combinations of those elements - Bugfix for internalalignment creation of major and minor of ellipse and extension to arcofellipse - Trim support for ellipses - Solver threshold for rank calculation set at 1e-13. - Trim support for arc of ellipse - Ellipses and ArcOfEllipses as external geometry - Validate Sketch now supports arcs of ellipse - Tangents of Ellipse or ArcOfEllipse to any of ArcOfcircle, circle and Ellipse (using construction elements) - Perpendicularity constraint - Bug fix: Show/hide internal geometry not working for external geometry - Visualization of Internal alignment constraints when selecting in the constraint widget - Equality for ellipses and arc of ellipses
This commit is contained in:
Abdullah Tahiri
@@ -1761,6 +1761,26 @@ int Sketch::addEqualConstraint(int geoId1, int geoId2)
|
||||
GCSsys.addConstraintEqualRadius(a1, a2, tag);
|
||||
return ConstraintsCounter;
|
||||
}
|
||||
|
||||
if (Geoms[geoId2].type == ArcOfEllipse) {
|
||||
if (Geoms[geoId1].type == ArcOfEllipse) {
|
||||
GCS::ArcOfEllipse &a1 = ArcsOfEllipse[Geoms[geoId1].index];
|
||||
GCS::ArcOfEllipse &a2 = ArcsOfEllipse[Geoms[geoId2].index];
|
||||
int tag = ++ConstraintsCounter;
|
||||
GCSsys.addConstraintEqualRadii(a1, a2, tag);
|
||||
return ConstraintsCounter;
|
||||
}
|
||||
}
|
||||
|
||||
if (Geoms[geoId1].type == Ellipse) {
|
||||
GCS::Ellipse &e1 = Ellipses[Geoms[geoId1].index];
|
||||
if (Geoms[geoId2].type == ArcOfEllipse) {
|
||||
GCS::ArcOfEllipse &a2 = ArcsOfEllipse[Geoms[geoId2].index];
|
||||
int tag = ++ConstraintsCounter;
|
||||
GCSsys.addConstraintEqualRadii(a2, e1, tag);
|
||||
return ConstraintsCounter;
|
||||
}
|
||||
}
|
||||
|
||||
Base::Console().Warning("Equality constraints between %s and %s are not supported.\n",
|
||||
nameByType(Geoms[geoId1].type), nameByType(Geoms[geoId2].type));
|
||||
|
||||
@@ -30,11 +30,13 @@
|
||||
# include <gp_Pln.hxx>
|
||||
# include <gp_Ax3.hxx>
|
||||
# include <gp_Circ.hxx>
|
||||
# include <gp_Elips.hxx>
|
||||
# include <BRepAdaptor_Surface.hxx>
|
||||
# include <BRepAdaptor_Curve.hxx>
|
||||
# include <BRep_Tool.hxx>
|
||||
# include <Geom_Plane.hxx>
|
||||
# include <Geom_Circle.hxx>
|
||||
# include <Geom_Ellipse.hxx>
|
||||
# include <Geom_TrimmedCurve.hxx>
|
||||
# include <GeomAPI_ProjectPointOnSurf.hxx>
|
||||
# include <BRepOffsetAPI_NormalProjection.hxx>
|
||||
@@ -957,8 +959,95 @@ int SketchObject::trim(int GeoId, const Base::Vector3d& point)
|
||||
return 0;
|
||||
}
|
||||
} else if (geo->getTypeId() == Part::GeomEllipse::getClassTypeId()) {
|
||||
// TODO: Ellipse Trim support
|
||||
const Part::GeomEllipse *ellipse = dynamic_cast<const Part::GeomEllipse*>(geo);
|
||||
Base::Vector3d center = ellipse->getCenter();
|
||||
double theta0 = Base::fmod(
|
||||
atan2(-ellipse->getMajorRadius()*((point.x-center.x)*sin(ellipse->getAngleXU())-(point.y-center.y)*cos(ellipse->getAngleXU())),
|
||||
ellipse->getMinorRadius()*((point.x-center.x)*cos(ellipse->getAngleXU())+(point.y-center.y)*sin(ellipse->getAngleXU()))
|
||||
), 2.f*M_PI);
|
||||
if (GeoId1 >= 0 && GeoId2 >= 0) {
|
||||
double theta1 = Base::fmod(
|
||||
atan2(-ellipse->getMajorRadius()*((point1.x-center.x)*sin(ellipse->getAngleXU())-(point1.y-center.y)*cos(ellipse->getAngleXU())),
|
||||
ellipse->getMinorRadius()*((point1.x-center.x)*cos(ellipse->getAngleXU())+(point1.y-center.y)*sin(ellipse->getAngleXU()))
|
||||
), 2.f*M_PI);
|
||||
double theta2 = Base::fmod(
|
||||
atan2(-ellipse->getMajorRadius()*((point2.x-center.x)*sin(ellipse->getAngleXU())-(point2.y-center.y)*cos(ellipse->getAngleXU())),
|
||||
ellipse->getMinorRadius()*((point2.x-center.x)*cos(ellipse->getAngleXU())+(point2.y-center.y)*sin(ellipse->getAngleXU()))
|
||||
), 2.f*M_PI);
|
||||
if (Base::fmod(theta1 - theta0, 2.f*M_PI) > Base::fmod(theta2 - theta0, 2.f*M_PI)) {
|
||||
std::swap(GeoId1,GeoId2);
|
||||
std::swap(point1,point2);
|
||||
std::swap(theta1,theta2);
|
||||
}
|
||||
if (theta1 == theta0 || theta1 == theta2)
|
||||
return -1;
|
||||
else if (theta1 > theta2)
|
||||
theta2 += 2.f*M_PI;
|
||||
|
||||
// Trim Point between intersection points
|
||||
|
||||
// Create a new arc to substitute Circle in geometry list and set parameters
|
||||
Part::GeomArcOfEllipse *geoNew = new Part::GeomArcOfEllipse();
|
||||
geoNew->setCenter(center);
|
||||
geoNew->setMajorRadius(ellipse->getMajorRadius());
|
||||
geoNew->setMinorRadius(ellipse->getMinorRadius());
|
||||
geoNew->setAngleXU(ellipse->getAngleXU());
|
||||
geoNew->setRange(theta1, theta2);
|
||||
|
||||
std::vector< Part::Geometry * > newVals(geomlist);
|
||||
newVals[GeoId] = geoNew;
|
||||
Geometry.setValues(newVals);
|
||||
Constraints.acceptGeometry(getCompleteGeometry());
|
||||
delete geoNew;
|
||||
rebuildVertexIndex();
|
||||
|
||||
PointPos secondPos1 = Sketcher::none, secondPos2 = Sketcher::none;
|
||||
ConstraintType constrType1 = Sketcher::PointOnObject, constrType2 = Sketcher::PointOnObject;
|
||||
for (std::vector<Constraint *>::const_iterator it=constraints.begin();
|
||||
it != constraints.end(); ++it) {
|
||||
Constraint *constr = *(it);
|
||||
if (secondPos1 == Sketcher::none && (constr->First == GeoId1 && constr->Second == GeoId)) {
|
||||
constrType1= Sketcher::Coincident;
|
||||
secondPos1 = constr->FirstPos;
|
||||
} else if(secondPos2 == Sketcher::none && (constr->First == GeoId2 && constr->Second == GeoId)) {
|
||||
constrType2 = Sketcher::Coincident;
|
||||
secondPos2 = constr->FirstPos;
|
||||
}
|
||||
}
|
||||
|
||||
// constrain the trimming points on the corresponding geometries
|
||||
Sketcher::Constraint *newConstr = new Sketcher::Constraint();
|
||||
newConstr->Type = constrType1;
|
||||
newConstr->First = GeoId;
|
||||
newConstr->FirstPos = start;
|
||||
newConstr->Second = GeoId1;
|
||||
|
||||
if (constrType1 == Sketcher::Coincident) {
|
||||
newConstr->SecondPos = secondPos1;
|
||||
delConstraintOnPoint(GeoId1, secondPos1, false);
|
||||
}
|
||||
|
||||
addConstraint(newConstr);
|
||||
|
||||
// Reset secondpos in case it was set previously
|
||||
newConstr->SecondPos = Sketcher::none;
|
||||
|
||||
// Add Second Constraint
|
||||
newConstr->First = GeoId;
|
||||
newConstr->FirstPos = end;
|
||||
newConstr->Second = GeoId2;
|
||||
|
||||
if (constrType2 == Sketcher::Coincident) {
|
||||
newConstr->SecondPos = secondPos2;
|
||||
delConstraintOnPoint(GeoId2, secondPos2, false);
|
||||
}
|
||||
|
||||
addConstraint(newConstr);
|
||||
|
||||
delete newConstr;
|
||||
|
||||
return 0;
|
||||
}
|
||||
} else if (geo->getTypeId() == Part::GeomArcOfCircle::getClassTypeId()) {
|
||||
const Part::GeomArcOfCircle *aoc = dynamic_cast<const Part::GeomArcOfCircle*>(geo);
|
||||
Base::Vector3d center = aoc->getCenter();
|
||||
@@ -1106,6 +1195,176 @@ int SketchObject::trim(int GeoId, const Base::Vector3d& point)
|
||||
newConstr->FirstPos = end;
|
||||
newConstr->Second = GeoId1;
|
||||
|
||||
if (constrType == Sketcher::Coincident)
|
||||
newConstr->SecondPos = secondPos;
|
||||
|
||||
addConstraint(newConstr);
|
||||
delete newConstr;
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
} else if (geo->getTypeId() == Part::GeomArcOfEllipse::getClassTypeId()) {
|
||||
const Part::GeomArcOfEllipse *aoe = dynamic_cast<const Part::GeomArcOfEllipse*>(geo);
|
||||
Base::Vector3d center = aoe->getCenter();
|
||||
double startAngle, endAngle;
|
||||
aoe->getRange(startAngle, endAngle);
|
||||
double dir = (startAngle < endAngle) ? 1 : -1; // this is always == 1
|
||||
double arcLength = (endAngle - startAngle)*dir;
|
||||
double theta0 = Base::fmod(
|
||||
atan2(-aoe->getMajorRadius()*((point.x-center.x)*sin(aoe->getAngleXU())-(point.y-center.y)*cos(aoe->getAngleXU())),
|
||||
aoe->getMinorRadius()*((point.x-center.x)*cos(aoe->getAngleXU())+(point.y-center.y)*sin(aoe->getAngleXU()))
|
||||
)- startAngle, 2.f*M_PI); // x0
|
||||
if (GeoId1 >= 0 && GeoId2 >= 0) {
|
||||
double theta1 = Base::fmod(
|
||||
atan2(-aoe->getMajorRadius()*((point1.x-center.x)*sin(aoe->getAngleXU())-(point1.y-center.y)*cos(aoe->getAngleXU())),
|
||||
aoe->getMinorRadius()*((point1.x-center.x)*cos(aoe->getAngleXU())+(point1.y-center.y)*sin(aoe->getAngleXU()))
|
||||
)- startAngle, 2.f*M_PI) * dir; // x1
|
||||
double theta2 = Base::fmod(
|
||||
atan2(-aoe->getMajorRadius()*((point2.x-center.x)*sin(aoe->getAngleXU())-(point2.y-center.y)*cos(aoe->getAngleXU())),
|
||||
aoe->getMinorRadius()*((point2.x-center.x)*cos(aoe->getAngleXU())+(point2.y-center.y)*sin(aoe->getAngleXU()))
|
||||
)- startAngle, 2.f*M_PI) * dir; // x2
|
||||
|
||||
if (theta1 > theta2) {
|
||||
std::swap(GeoId1,GeoId2);
|
||||
std::swap(point1,point2);
|
||||
std::swap(theta1,theta2);
|
||||
}
|
||||
if (theta1 >= 0.001*arcLength && theta2 <= 0.999*arcLength) {
|
||||
// Trim Point between intersection points
|
||||
if (theta1 < theta0 && theta2 > theta0) {
|
||||
int newGeoId = addGeometry(geo);
|
||||
// go through all constraints and replace the point (GeoId,end) with (newGeoId,end)
|
||||
transferConstraints(GeoId, end, newGeoId, end);
|
||||
|
||||
Part::GeomArcOfEllipse *aoe1 = dynamic_cast<Part::GeomArcOfEllipse*>(geomlist[GeoId]);
|
||||
Part::GeomArcOfEllipse *aoe2 = dynamic_cast<Part::GeomArcOfEllipse*>(geomlist[newGeoId]);
|
||||
aoe1->setRange(startAngle, startAngle + theta1);
|
||||
aoe2->setRange(startAngle + theta2, endAngle);
|
||||
|
||||
// constrain the trimming points on the corresponding geometries
|
||||
Sketcher::Constraint *newConstr = new Sketcher::Constraint();
|
||||
|
||||
// Build Constraints associated with new pair of arcs
|
||||
newConstr->Type = Sketcher::Equal;
|
||||
newConstr->First = GeoId;
|
||||
newConstr->Second = newGeoId;
|
||||
addConstraint(newConstr);
|
||||
|
||||
PointPos secondPos1 = Sketcher::none, secondPos2 = Sketcher::none;
|
||||
ConstraintType constrType1 = Sketcher::PointOnObject, constrType2 = Sketcher::PointOnObject;
|
||||
|
||||
for (std::vector<Constraint *>::const_iterator it=constraints.begin();
|
||||
it != constraints.end(); ++it) {
|
||||
Constraint *constr = *(it);
|
||||
if (secondPos1 == Sketcher::none &&
|
||||
(constr->First == GeoId1 && constr->Second == GeoId)) {
|
||||
constrType1= Sketcher::Coincident;
|
||||
secondPos1 = constr->FirstPos;
|
||||
} else if (secondPos2 == Sketcher::none &&
|
||||
(constr->First == GeoId2 && constr->Second == GeoId)) {
|
||||
constrType2 = Sketcher::Coincident;
|
||||
secondPos2 = constr->FirstPos;
|
||||
}
|
||||
}
|
||||
|
||||
newConstr->Type = constrType1;
|
||||
newConstr->First = GeoId;
|
||||
newConstr->FirstPos = end;
|
||||
newConstr->Second = GeoId1;
|
||||
|
||||
if (constrType1 == Sketcher::Coincident) {
|
||||
newConstr->SecondPos = secondPos1;
|
||||
delConstraintOnPoint(GeoId1, secondPos1, false);
|
||||
}
|
||||
|
||||
addConstraint(newConstr);
|
||||
|
||||
// Reset secondpos in case it was set previously
|
||||
newConstr->SecondPos = Sketcher::none;
|
||||
|
||||
newConstr->Type = constrType2;
|
||||
newConstr->First = newGeoId;
|
||||
newConstr->FirstPos = start;
|
||||
newConstr->Second = GeoId2;
|
||||
|
||||
if (constrType2 == Sketcher::Coincident) {
|
||||
newConstr->SecondPos = secondPos2;
|
||||
delConstraintOnPoint(GeoId2, secondPos2, false);
|
||||
}
|
||||
|
||||
addConstraint(newConstr);
|
||||
|
||||
newConstr->Type = Sketcher::Coincident;
|
||||
newConstr->First = GeoId;
|
||||
newConstr->FirstPos = Sketcher::mid;
|
||||
newConstr->Second = newGeoId;
|
||||
newConstr->SecondPos = Sketcher::mid;
|
||||
addConstraint(newConstr);
|
||||
|
||||
delete newConstr;
|
||||
|
||||
return 0;
|
||||
} else
|
||||
return -1;
|
||||
} else if (theta1 < 0.001*arcLength) { // drop the second intersection point
|
||||
std::swap(GeoId1,GeoId2);
|
||||
std::swap(point1,point2);
|
||||
} else if (theta2 > 0.999*arcLength) {
|
||||
}
|
||||
else
|
||||
return -1;
|
||||
}
|
||||
|
||||
if (GeoId1 >= 0) {
|
||||
|
||||
ConstraintType constrType = Sketcher::PointOnObject;
|
||||
PointPos secondPos = Sketcher::none;
|
||||
for (std::vector<Constraint *>::const_iterator it=constraints.begin();
|
||||
it != constraints.end(); ++it) {
|
||||
Constraint *constr = *(it);
|
||||
if ((constr->First == GeoId1 && constr->Second == GeoId)) {
|
||||
constrType = Sketcher::Coincident;
|
||||
secondPos = constr->FirstPos;
|
||||
delConstraintOnPoint(GeoId1, constr->FirstPos, false);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
double theta1 = Base::fmod(
|
||||
atan2(-aoe->getMajorRadius()*((point1.x-center.x)*sin(aoe->getAngleXU())-(point1.y-center.y)*cos(aoe->getAngleXU())),
|
||||
aoe->getMinorRadius()*((point1.x-center.x)*cos(aoe->getAngleXU())+(point1.y-center.y)*sin(aoe->getAngleXU()))
|
||||
)- startAngle, 2.f*M_PI) * dir; // x1
|
||||
|
||||
if (theta1 >= 0.001*arcLength && theta1 <= 0.999*arcLength) {
|
||||
if (theta1 > theta0) { // trim arc start
|
||||
delConstraintOnPoint(GeoId, start, false);
|
||||
Part::GeomArcOfEllipse *aoe1 = dynamic_cast<Part::GeomArcOfEllipse*>(geomlist[GeoId]);
|
||||
aoe1->setRange(startAngle + theta1, endAngle);
|
||||
// constrain the trimming point on the corresponding geometry
|
||||
Sketcher::Constraint *newConstr = new Sketcher::Constraint();
|
||||
newConstr->Type = constrType;
|
||||
newConstr->First = GeoId;
|
||||
newConstr->FirstPos = start;
|
||||
newConstr->Second = GeoId1;
|
||||
|
||||
if (constrType == Sketcher::Coincident)
|
||||
newConstr->SecondPos = secondPos;
|
||||
|
||||
addConstraint(newConstr);
|
||||
delete newConstr;
|
||||
return 0;
|
||||
}
|
||||
else { // trim arc end
|
||||
delConstraintOnPoint(GeoId, end, false);
|
||||
Part::GeomArcOfEllipse *aoe1 = dynamic_cast<Part::GeomArcOfEllipse*>(geomlist[GeoId]);
|
||||
aoe1->setRange(startAngle, startAngle + theta1);
|
||||
Sketcher::Constraint *newConstr = new Sketcher::Constraint();
|
||||
newConstr->Type = constrType;
|
||||
newConstr->First = GeoId;
|
||||
newConstr->FirstPos = end;
|
||||
newConstr->Second = GeoId1;
|
||||
|
||||
if (constrType == Sketcher::Coincident)
|
||||
newConstr->SecondPos = secondPos;
|
||||
|
||||
@@ -1438,6 +1697,35 @@ void SketchObject::rebuildExternalGeometry(void)
|
||||
ExternalGeo.push_back(arc);
|
||||
}
|
||||
}
|
||||
else if (projCurve.GetType() == GeomAbs_Ellipse) {
|
||||
gp_Elips e = projCurve.Ellipse();
|
||||
gp_Pnt p = e.Location();
|
||||
gp_Pnt P1 = projCurve.Value(projCurve.FirstParameter());
|
||||
gp_Pnt P2 = projCurve.Value(projCurve.LastParameter());
|
||||
|
||||
gp_Dir normal = e.Axis().Direction();
|
||||
gp_Dir xdir = e.XAxis().Direction();
|
||||
gp_Ax2 xdirref(p, normal);
|
||||
|
||||
if (P1.SquareDistance(P2) < Precision::Confusion()) {
|
||||
Part::GeomEllipse* ellipse = new Part::GeomEllipse();
|
||||
ellipse->setMajorRadius(e.MajorRadius());
|
||||
ellipse->setMinorRadius(e.MinorRadius());
|
||||
ellipse->setCenter(Base::Vector3d(p.X(),p.Y(),p.Z()));
|
||||
ellipse->setAngleXU(-xdir.AngleWithRef(xdirref.XDirection(),normal));
|
||||
ellipse->Construction = true;
|
||||
ExternalGeo.push_back(ellipse);
|
||||
}
|
||||
else {
|
||||
Part::GeomArcOfEllipse* aoe = new Part::GeomArcOfEllipse();
|
||||
Handle_Geom_Curve curve = new Geom_Ellipse(e);
|
||||
Handle_Geom_TrimmedCurve tCurve = new Geom_TrimmedCurve(curve, projCurve.FirstParameter(),
|
||||
projCurve.LastParameter());
|
||||
aoe->setHandle(tCurve);
|
||||
aoe->Construction = true;
|
||||
ExternalGeo.push_back(aoe);
|
||||
}
|
||||
}
|
||||
else {
|
||||
throw Base::Exception("Not yet supported geometry for external geometry");
|
||||
}
|
||||
|
||||
@@ -1792,7 +1792,7 @@ double ConstraintInternalAlignmentPoint2Ellipse::grad(double *param)
|
||||
}
|
||||
|
||||
// ConstraintEqualMajorAxesEllipse
|
||||
ConstraintEqualMajorAxesEllipse:: ConstraintEqualMajorAxesEllipse(Ellipse &e1, Ellipse &e2)
|
||||
ConstraintEqualMajorAxesEllipse:: ConstraintEqualMajorAxesEllipse(Ellipse &e1, Ellipse &e2)
|
||||
{
|
||||
pvec.push_back(e1.center.x);
|
||||
pvec.push_back(e1.center.y);
|
||||
@@ -1808,6 +1808,38 @@ double ConstraintInternalAlignmentPoint2Ellipse::grad(double *param)
|
||||
rescale();
|
||||
}
|
||||
|
||||
ConstraintEqualMajorAxesEllipse:: ConstraintEqualMajorAxesEllipse(ArcOfEllipse &a1, Ellipse &e2)
|
||||
{
|
||||
pvec.push_back(a1.center.x);
|
||||
pvec.push_back(a1.center.y);
|
||||
pvec.push_back(a1.focus1X);
|
||||
pvec.push_back(a1.focus1Y);
|
||||
pvec.push_back(a1.radmin);
|
||||
pvec.push_back(e2.center.x);
|
||||
pvec.push_back(e2.center.y);
|
||||
pvec.push_back(e2.focus1X);
|
||||
pvec.push_back(e2.focus1Y);
|
||||
pvec.push_back(e2.radmin);
|
||||
origpvec = pvec;
|
||||
rescale();
|
||||
}
|
||||
|
||||
ConstraintEqualMajorAxesEllipse:: ConstraintEqualMajorAxesEllipse(ArcOfEllipse &a1, ArcOfEllipse &a2)
|
||||
{
|
||||
pvec.push_back(a1.center.x);
|
||||
pvec.push_back(a1.center.y);
|
||||
pvec.push_back(a1.focus1X);
|
||||
pvec.push_back(a1.focus1Y);
|
||||
pvec.push_back(a1.radmin);
|
||||
pvec.push_back(a2.center.x);
|
||||
pvec.push_back(a2.center.y);
|
||||
pvec.push_back(a2.focus1X);
|
||||
pvec.push_back(a2.focus1Y);
|
||||
pvec.push_back(a2.radmin);
|
||||
origpvec = pvec;
|
||||
rescale();
|
||||
}
|
||||
|
||||
ConstraintType ConstraintEqualMajorAxesEllipse::getTypeId()
|
||||
{
|
||||
return EqualMajorAxesEllipse;
|
||||
|
||||
@@ -400,6 +400,8 @@ namespace GCS
|
||||
inline double* e2rmin() { return pvec[9]; }
|
||||
public:
|
||||
ConstraintEqualMajorAxesEllipse(Ellipse &e1, Ellipse &e2);
|
||||
ConstraintEqualMajorAxesEllipse(ArcOfEllipse &a1, Ellipse &e2);
|
||||
ConstraintEqualMajorAxesEllipse(ArcOfEllipse &a1, ArcOfEllipse &a2);
|
||||
virtual ConstraintType getTypeId();
|
||||
virtual void rescale(double coef=1.);
|
||||
virtual double error();
|
||||
|
||||
@@ -798,6 +798,24 @@ int System::addConstraintEqualRadii(Ellipse &e1, Ellipse &e2, int tagId)
|
||||
return addConstraint(constr);
|
||||
}
|
||||
|
||||
int System::addConstraintEqualRadii(ArcOfEllipse &a1, ArcOfEllipse &a2, int tagId)
|
||||
{
|
||||
addConstraintEqual(a1.radmin, a2.radmin, tagId);
|
||||
|
||||
Constraint *constr = new ConstraintEqualMajorAxesEllipse(a1,a2);
|
||||
constr->setTag(tagId);
|
||||
return addConstraint(constr);
|
||||
}
|
||||
|
||||
int System::addConstraintEqualRadii(ArcOfEllipse &a1, Ellipse &e2, int tagId)
|
||||
{
|
||||
addConstraintEqual(a1.radmin, e2.radmin, tagId);
|
||||
|
||||
Constraint *constr = new ConstraintEqualMajorAxesEllipse(a1,e2);
|
||||
constr->setTag(tagId);
|
||||
return addConstraint(constr);
|
||||
}
|
||||
|
||||
int System::addConstraintEqualRadius(Circle &c1, Arc &a2, int tagId)
|
||||
{
|
||||
return addConstraintEqual(c1.rad, a2.rad, tagId);
|
||||
@@ -828,71 +846,81 @@ int System::addConstraintInternalAlignmentPoint2Ellipse(Ellipse &e, Point &p1, I
|
||||
}
|
||||
|
||||
int System::addConstraintInternalAlignmentEllipseMajorDiameter(Ellipse &e, Point &p1, Point &p2, int tagId)
|
||||
{
|
||||
//chechk which of the points is closer to satisfying positivemajor
|
||||
double err1=0.0, err2=0.0;
|
||||
{
|
||||
ConstraintInternalAlignmentPoint2Ellipse constr (e,p1,EllipsePositiveMajorX);
|
||||
err1+=abs(constr.error());
|
||||
};
|
||||
{
|
||||
ConstraintInternalAlignmentPoint2Ellipse constr (e,p1,EllipsePositiveMajorY);
|
||||
err1+=abs(constr.error());
|
||||
};
|
||||
{
|
||||
ConstraintInternalAlignmentPoint2Ellipse constr (e,p2,EllipsePositiveMajorX);
|
||||
err2+=abs(constr.error());
|
||||
};
|
||||
{
|
||||
ConstraintInternalAlignmentPoint2Ellipse constr (e,p2,EllipsePositiveMajorY);
|
||||
err2+=abs(constr.error());
|
||||
};
|
||||
if(err1<=err2){
|
||||
{
|
||||
double X_1=*p1.x;
|
||||
double Y_1=*p1.y;
|
||||
double X_2=*p2.x;
|
||||
double Y_2=*p2.y;
|
||||
double X_c=*e.center.x;
|
||||
double Y_c=*e.center.y;
|
||||
double X_F1=*e.focus1X;
|
||||
double Y_F1=*e.focus1Y;
|
||||
double b=*e.radmin;
|
||||
|
||||
// P1=vector([X_1,Y_1])
|
||||
// P2=vector([X_2,Y_2])
|
||||
// dF1= (F1-C)/sqrt((F1-C)*(F1-C))
|
||||
// print "these are the extreme points of the major axis"
|
||||
// PA = C + a * dF1
|
||||
// PN = C - a * dF1
|
||||
// print "this is a simple function to know which point is closer to the positive edge of the ellipse"
|
||||
// DMC=(P1-PA)*(P1-PA)-(P2-PA)*(P2-PA)
|
||||
double closertopositivemajor=pow(X_1 - X_c - (X_F1 - X_c)*sqrt(pow(b, 2) + pow(X_F1 - X_c,
|
||||
2) + pow(Y_F1 - Y_c, 2))/sqrt(pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2)),
|
||||
2) - pow(X_2 - X_c - (X_F1 - X_c)*sqrt(pow(b, 2) + pow(X_F1 - X_c, 2) +
|
||||
pow(Y_F1 - Y_c, 2))/sqrt(pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2)), 2) +
|
||||
pow(Y_1 - Y_c - (Y_F1 - Y_c)*sqrt(pow(b, 2) + pow(X_F1 - X_c, 2) +
|
||||
pow(Y_F1 - Y_c, 2))/sqrt(pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2)), 2) -
|
||||
pow(Y_2 - Y_c - (Y_F1 - Y_c)*sqrt(pow(b, 2) + pow(X_F1 - X_c, 2) +
|
||||
pow(Y_F1 - Y_c, 2))/sqrt(pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2)), 2);
|
||||
|
||||
if(closertopositivemajor>0){
|
||||
//p2 is closer to positivemajor. Assign constraints back-to-front.
|
||||
addConstraintInternalAlignmentPoint2Ellipse(e,p2,EllipsePositiveMajorX,tagId);
|
||||
addConstraintInternalAlignmentPoint2Ellipse(e,p2,EllipsePositiveMajorY,tagId);
|
||||
addConstraintInternalAlignmentPoint2Ellipse(e,p1,EllipseNegativeMajorX,tagId);
|
||||
return addConstraintInternalAlignmentPoint2Ellipse(e,p1,EllipseNegativeMajorY,tagId);
|
||||
}
|
||||
else{
|
||||
//p1 is closer to positivemajor
|
||||
addConstraintInternalAlignmentPoint2Ellipse(e,p1,EllipsePositiveMajorX,tagId);
|
||||
addConstraintInternalAlignmentPoint2Ellipse(e,p1,EllipsePositiveMajorY,tagId);
|
||||
addConstraintInternalAlignmentPoint2Ellipse(e,p2,EllipseNegativeMajorX,tagId);
|
||||
return addConstraintInternalAlignmentPoint2Ellipse(e,p2,EllipseNegativeMajorY,tagId);
|
||||
} else {
|
||||
//p2 is closer to positivemajor. Assign constraints back-to-front.
|
||||
addConstraintInternalAlignmentPoint2Ellipse(e,p2,EllipsePositiveMajorX,tagId);
|
||||
addConstraintInternalAlignmentPoint2Ellipse(e,p2,EllipsePositiveMajorY,tagId);
|
||||
addConstraintInternalAlignmentPoint2Ellipse(e,p1,EllipseNegativeMajorX,tagId);
|
||||
return addConstraintInternalAlignmentPoint2Ellipse(e,p1,EllipseNegativeMajorY,tagId);
|
||||
}
|
||||
}
|
||||
|
||||
int System::addConstraintInternalAlignmentEllipseMinorDiameter(Ellipse &e, Point &p1, Point &p2, int tagId)
|
||||
{
|
||||
//chechk which of the points is closer to satisfying positivemajor
|
||||
double err1=0.0, err2=0.0;
|
||||
{
|
||||
ConstraintInternalAlignmentPoint2Ellipse constr (e,p1,EllipsePositiveMinorX);
|
||||
err1+=abs(constr.error());
|
||||
};
|
||||
{
|
||||
ConstraintInternalAlignmentPoint2Ellipse constr (e,p1,EllipsePositiveMinorY);
|
||||
err1+=abs(constr.error());
|
||||
};
|
||||
{
|
||||
ConstraintInternalAlignmentPoint2Ellipse constr (e,p2,EllipsePositiveMinorX);
|
||||
err2+=abs(constr.error());
|
||||
};
|
||||
{
|
||||
ConstraintInternalAlignmentPoint2Ellipse constr (e,p2,EllipsePositiveMinorY);
|
||||
err2+=abs(constr.error());
|
||||
};
|
||||
if(err1<=err2){
|
||||
double X_1=*p1.x;
|
||||
double Y_1=*p1.y;
|
||||
double X_2=*p2.x;
|
||||
double Y_2=*p2.y;
|
||||
double X_c=*e.center.x;
|
||||
double Y_c=*e.center.y;
|
||||
double X_F1=*e.focus1X;
|
||||
double Y_F1=*e.focus1Y;
|
||||
double b=*e.radmin;
|
||||
|
||||
// Same idea as for major above, but for minor
|
||||
// DMC=(P1-PA)*(P1-PA)-(P2-PA)*(P2-PA)
|
||||
double closertopositiveminor= pow(X_1 - X_c + b*(Y_F1 - Y_c)/sqrt(pow(X_F1 - X_c, 2) +
|
||||
pow(Y_F1 - Y_c, 2)), 2) - pow(X_2 - X_c + b*(Y_F1 - Y_c)/sqrt(pow(X_F1 -
|
||||
X_c, 2) + pow(Y_F1 - Y_c, 2)), 2) + pow(-Y_1 + Y_c + b*(X_F1 -
|
||||
X_c)/sqrt(pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2)), 2) - pow(-Y_2 + Y_c
|
||||
+ b*(X_F1 - X_c)/sqrt(pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2)), 2);
|
||||
|
||||
if(closertopositiveminor>0){
|
||||
addConstraintInternalAlignmentPoint2Ellipse(e,p2,EllipsePositiveMinorX,tagId);
|
||||
addConstraintInternalAlignmentPoint2Ellipse(e,p2,EllipsePositiveMinorY,tagId);
|
||||
addConstraintInternalAlignmentPoint2Ellipse(e,p1,EllipseNegativeMinorX,tagId);
|
||||
return addConstraintInternalAlignmentPoint2Ellipse(e,p1,EllipseNegativeMinorY,tagId);
|
||||
} else {
|
||||
addConstraintInternalAlignmentPoint2Ellipse(e,p1,EllipsePositiveMinorX,tagId);
|
||||
addConstraintInternalAlignmentPoint2Ellipse(e,p1,EllipsePositiveMinorY,tagId);
|
||||
addConstraintInternalAlignmentPoint2Ellipse(e,p2,EllipseNegativeMinorX,tagId);
|
||||
return addConstraintInternalAlignmentPoint2Ellipse(e,p2,EllipseNegativeMinorY,tagId);
|
||||
} else {
|
||||
addConstraintInternalAlignmentPoint2Ellipse(e,p2,EllipsePositiveMinorX,tagId);
|
||||
addConstraintInternalAlignmentPoint2Ellipse(e,p2,EllipsePositiveMinorY,tagId);
|
||||
addConstraintInternalAlignmentPoint2Ellipse(e,p1,EllipseNegativeMinorX,tagId);
|
||||
return addConstraintInternalAlignmentPoint2Ellipse(e,p1,EllipseNegativeMinorY,tagId);
|
||||
};
|
||||
}
|
||||
}
|
||||
|
||||
int System::addConstraintInternalAlignmentEllipseFocus1(Ellipse &e, Point &p1, int tagId)
|
||||
@@ -916,18 +944,80 @@ int System::addConstraintInternalAlignmentPoint2Ellipse(ArcOfEllipse &a, Point &
|
||||
|
||||
int System::addConstraintInternalAlignmentEllipseMajorDiameter(ArcOfEllipse &a, Point &p1, Point &p2, int tagId)
|
||||
{
|
||||
addConstraintInternalAlignmentPoint2Ellipse(a,p1,EllipsePositiveMajorX,tagId);
|
||||
addConstraintInternalAlignmentPoint2Ellipse(a,p1,EllipsePositiveMajorY,tagId);
|
||||
addConstraintInternalAlignmentPoint2Ellipse(a,p2,EllipseNegativeMajorX,tagId);
|
||||
return addConstraintInternalAlignmentPoint2Ellipse(a,p2,EllipseNegativeMajorY,tagId);
|
||||
double X_1=*p1.x;
|
||||
double Y_1=*p1.y;
|
||||
double X_2=*p2.x;
|
||||
double Y_2=*p2.y;
|
||||
double X_c=*a.center.x;
|
||||
double Y_c=*a.center.y;
|
||||
double X_F1=*a.focus1X;
|
||||
double Y_F1=*a.focus1Y;
|
||||
double b=*a.radmin;
|
||||
|
||||
// P1=vector([X_1,Y_1])
|
||||
// P2=vector([X_2,Y_2])
|
||||
// dF1= (F1-C)/sqrt((F1-C)*(F1-C))
|
||||
// print "these are the extreme points of the major axis"
|
||||
// PA = C + a * dF1
|
||||
// PN = C - a * dF1
|
||||
// print "this is a simple function to know which point is closer to the positive edge of the ellipse"
|
||||
// DMC=(P1-PA)*(P1-PA)-(P2-PA)*(P2-PA)
|
||||
double closertopositivemajor=pow(X_1 - X_c - (X_F1 - X_c)*sqrt(pow(b, 2) + pow(X_F1 - X_c,
|
||||
2) + pow(Y_F1 - Y_c, 2))/sqrt(pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2)),
|
||||
2) - pow(X_2 - X_c - (X_F1 - X_c)*sqrt(pow(b, 2) + pow(X_F1 - X_c, 2) +
|
||||
pow(Y_F1 - Y_c, 2))/sqrt(pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2)), 2) +
|
||||
pow(Y_1 - Y_c - (Y_F1 - Y_c)*sqrt(pow(b, 2) + pow(X_F1 - X_c, 2) +
|
||||
pow(Y_F1 - Y_c, 2))/sqrt(pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2)), 2) -
|
||||
pow(Y_2 - Y_c - (Y_F1 - Y_c)*sqrt(pow(b, 2) + pow(X_F1 - X_c, 2) +
|
||||
pow(Y_F1 - Y_c, 2))/sqrt(pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2)), 2);
|
||||
|
||||
if(closertopositivemajor>0){
|
||||
//p2 is closer to positivemajor. Assign constraints back-to-front.
|
||||
addConstraintInternalAlignmentPoint2Ellipse(a,p2,EllipsePositiveMajorX,tagId);
|
||||
addConstraintInternalAlignmentPoint2Ellipse(a,p2,EllipsePositiveMajorY,tagId);
|
||||
addConstraintInternalAlignmentPoint2Ellipse(a,p1,EllipseNegativeMajorX,tagId);
|
||||
return addConstraintInternalAlignmentPoint2Ellipse(a,p1,EllipseNegativeMajorY,tagId);
|
||||
}
|
||||
else{
|
||||
//p1 is closer to positivemajor
|
||||
addConstraintInternalAlignmentPoint2Ellipse(a,p1,EllipsePositiveMajorX,tagId);
|
||||
addConstraintInternalAlignmentPoint2Ellipse(a,p1,EllipsePositiveMajorY,tagId);
|
||||
addConstraintInternalAlignmentPoint2Ellipse(a,p2,EllipseNegativeMajorX,tagId);
|
||||
return addConstraintInternalAlignmentPoint2Ellipse(a,p2,EllipseNegativeMajorY,tagId);
|
||||
}
|
||||
}
|
||||
|
||||
int System::addConstraintInternalAlignmentEllipseMinorDiameter(ArcOfEllipse &a, Point &p1, Point &p2, int tagId)
|
||||
{
|
||||
addConstraintInternalAlignmentPoint2Ellipse(a,p1,EllipsePositiveMinorX,tagId);
|
||||
addConstraintInternalAlignmentPoint2Ellipse(a,p1,EllipsePositiveMinorY,tagId);
|
||||
addConstraintInternalAlignmentPoint2Ellipse(a,p2,EllipseNegativeMinorX,tagId);
|
||||
return addConstraintInternalAlignmentPoint2Ellipse(a,p2,EllipseNegativeMinorY,tagId);
|
||||
double X_1=*p1.x;
|
||||
double Y_1=*p1.y;
|
||||
double X_2=*p2.x;
|
||||
double Y_2=*p2.y;
|
||||
double X_c=*a.center.x;
|
||||
double Y_c=*a.center.y;
|
||||
double X_F1=*a.focus1X;
|
||||
double Y_F1=*a.focus1Y;
|
||||
double b=*a.radmin;
|
||||
|
||||
// Same idea as for major above, but for minor
|
||||
// DMC=(P1-PA)*(P1-PA)-(P2-PA)*(P2-PA)
|
||||
double closertopositiveminor= pow(X_1 - X_c + b*(Y_F1 - Y_c)/sqrt(pow(X_F1 - X_c, 2) +
|
||||
pow(Y_F1 - Y_c, 2)), 2) - pow(X_2 - X_c + b*(Y_F1 - Y_c)/sqrt(pow(X_F1 -
|
||||
X_c, 2) + pow(Y_F1 - Y_c, 2)), 2) + pow(-Y_1 + Y_c + b*(X_F1 -
|
||||
X_c)/sqrt(pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2)), 2) - pow(-Y_2 + Y_c
|
||||
+ b*(X_F1 - X_c)/sqrt(pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2)), 2);
|
||||
|
||||
if(closertopositiveminor>0){
|
||||
addConstraintInternalAlignmentPoint2Ellipse(a,p2,EllipsePositiveMinorX,tagId);
|
||||
addConstraintInternalAlignmentPoint2Ellipse(a,p2,EllipsePositiveMinorY,tagId);
|
||||
addConstraintInternalAlignmentPoint2Ellipse(a,p1,EllipseNegativeMinorX,tagId);
|
||||
return addConstraintInternalAlignmentPoint2Ellipse(a,p1,EllipseNegativeMinorY,tagId);
|
||||
} else {
|
||||
addConstraintInternalAlignmentPoint2Ellipse(a,p1,EllipsePositiveMinorX,tagId);
|
||||
addConstraintInternalAlignmentPoint2Ellipse(a,p1,EllipsePositiveMinorY,tagId);
|
||||
addConstraintInternalAlignmentPoint2Ellipse(a,p2,EllipseNegativeMinorX,tagId);
|
||||
return addConstraintInternalAlignmentPoint2Ellipse(a,p2,EllipseNegativeMinorY,tagId);
|
||||
}
|
||||
}
|
||||
|
||||
int System::addConstraintInternalAlignmentEllipseFocus1(ArcOfEllipse &a, Point &p1, int tagId)
|
||||
@@ -1749,7 +1839,7 @@ int System::diagnose()
|
||||
Eigen::MatrixXd Q = qrJT.matrixQ ();
|
||||
int paramsNum = qrJT.rows();
|
||||
int constrNum = qrJT.cols();
|
||||
qrJT.setThreshold(1e-10);
|
||||
qrJT.setThreshold(1e-13);
|
||||
int rank = qrJT.rank();
|
||||
|
||||
Eigen::MatrixXd R;
|
||||
|
||||
@@ -165,6 +165,8 @@ namespace GCS
|
||||
int addConstraintEqualLength(Line &l1, Line &l2, double *length, int tagId=0);
|
||||
int addConstraintEqualRadius(Circle &c1, Circle &c2, int tagId=0);
|
||||
int addConstraintEqualRadii(Ellipse &e1, Ellipse &e2, int tagId=0);
|
||||
int addConstraintEqualRadii(ArcOfEllipse &a1, ArcOfEllipse &a2, int tagId=0);
|
||||
int addConstraintEqualRadii(ArcOfEllipse &a1, Ellipse &e2, int tagId=0);
|
||||
int addConstraintEqualRadius(Circle &c1, Arc &a2, int tagId=0);
|
||||
int addConstraintEqualRadius(Arc &a1, Arc &a2, int tagId=0);
|
||||
int addConstraintP2PSymmetric(Point &p1, Point &p2, Line &l, int tagId=0);
|
||||
|
||||
Reference in New Issue
Block a user