+GCS::SolveStatus PlaneGCSSolver_Solver::solveWithoutTangent()
+{
+ std::shared_ptr<GCS::System> aSystemWithoutTangent(new GCS::System);
+
+ // Remove tangency which leads to redundant or conflicting constraints
+ GCS::VEC_I aConflicting, aRedundant;
+ myEquationSystem->getRedundant(aRedundant);
+ size_t aNbRemove = myTangent.size(); // number of tangent constraints which can be removed
+ myEquationSystem->getConflicting(aConflicting);
+ aRedundant.insert(aRedundant.end(), aConflicting.begin(), aConflicting.end());
+
+ GCS::SET_I aTangentToRemove;
+ GCS::VEC_I::iterator aCIt = aRedundant.begin();
+ for (; aCIt != aRedundant.end() && aNbRemove > 0; ++aCIt)
+ if (myTangent.find(*aCIt) != myTangent.end()) {
+ aTangentToRemove.insert(*aCIt);
+ --aNbRemove;
+ }
+
+ std::set<GCS::Constraint*> aRemovedTangent;
+ ConstraintMap::const_iterator aConstrIt = myConstraints.begin();
+ while (aConstrIt != myConstraints.end()) {
+ GCS::Constraint* aConstraint = aConstrIt->first;
+ int anID = aConstraint->getTag();
+ ++aConstrIt;
+ if (aTangentToRemove.find(anID) == aTangentToRemove.end())
+ aSystemWithoutTangent->addConstraint(aConstraint);
+ else
+ aRemovedTangent.insert(aConstraint);
+ }
+
+ myTangent.clear();
+ GCS::SolveStatus aResult = (GCS::SolveStatus)aSystemWithoutTangent->solve(myParameters);
+ if (aResult == GCS::Success) {
+ GCS::VEC_I aRedundant;
+ aSystemWithoutTangent->getRedundant(aRedundant);
+ if (aRedundant.empty())
+ myEquationSystem = aSystemWithoutTangent;
+ else
+ aResult = GCS::Failed;
+ }
+
+ // additional check that removed constraints are still correct
+ if (aResult == GCS::Success) {
+ std::set<GCS::Constraint*>::const_iterator aRemIt = aRemovedTangent.begin();
+ for (; aRemIt != aRemovedTangent.end(); ++aRemIt)
+ if (!isTangentTruth(*aRemIt))
+ break;
+ if (aRemIt != aRemovedTangent.end())
+ aResult = GCS::Failed;
+ }
+
+ // Add IDs of removed tangent to the list of conflicting constraints
+ if (aResult == GCS::Failed) {
+ std::set<GCS::Constraint*>::const_iterator aRemIt = aRemovedTangent.begin();
+ for (; aRemIt != aRemovedTangent.end(); ++aRemIt)
+ myConflictingIDs.insert((*aRemIt)->getTag());
+ }
+
+ return aResult;
+}
+
+bool PlaneGCSSolver_Solver::isTangentTruth(GCS::Constraint* theTangent) const
+{
+ static const double aTol = 1e-5;
+ double aTol2 = aTol *aTol;
+
+ if (theTangent->getTypeId() == GCS::TangentCircumf) {
+ GCS::VEC_pD aParams = theTangent->params();
+ double dx = *(aParams[2]) - *(aParams[0]);
+ double dy = *(aParams[3]) - *(aParams[1]);
+ double aDist2 = dx * dx + dy * dy;
+ double aRadSum = *(aParams[4]) + *(aParams[5]);
+ double aRadDiff = *(aParams[4]) - *(aParams[5]);
+ aTol2 *= aDist2 > 1.0 ? aDist2 : 1.0;
+ return fabs(aDist2 - aRadSum * aRadSum) <= aTol2 ||
+ fabs(aDist2 - aRadDiff * aRadDiff) <= aTol2;
+ }
+ if (theTangent->getTypeId() == GCS::P2LDistance) {
+ GCS::VEC_pD aParams = theTangent->params();
+ double aDist2 = *(aParams[6]) * *(aParams[6]);
+ // orthogonal line direction
+ double aDirX = *(aParams[5]) - *(aParams[3]);
+ double aDirY = *(aParams[2]) - *(aParams[4]);
+ double aLen2 = aDirX * aDirX + aDirY * aDirY;
+ // vector from line's start to point
+ double aVecX = *(aParams[0]) - *(aParams[2]);
+ double aVecY = *(aParams[1]) - *(aParams[3]);
+
+ double aDot = aVecX * aDirX + aVecY * aDirY;
+ return fabs(aDot * aDot - aDist2 * aLen2) <= aTol2 * aLen2;
+ }
+ return false;
+}
+
+bool PlaneGCSSolver_Solver::isTangentTruth(int theTagID) const
+{
+ ConstraintMap::const_iterator anIt = myConstraints.begin();
+ for (; anIt != myConstraints.end(); ++anIt)
+ if (anIt->first->getTag() == theTagID)
+ return isTangentTruth(anIt->first);
+ return false;
+}
+