+ if (myConstraints.empty())
+ theFreeParams.insert(myParameters.begin(), myParameters.end());
+ else {
+ GCS::VEC_pD aParametersCopy = myParameters;
+ ConstraintMap aConstraintCopy = myConstraints;
+
+ // clear the set of equations
+ clear();
+ // reset constraints
+ myParameters = aParametersCopy;
+ for (ConstraintMap::iterator anIt = aConstraintCopy.begin();
+ anIt != aConstraintCopy.end(); ++anIt)
+ addConstraint(anIt->first, anIt->second);
+
+ // parameters detection works for Dense QR only
+ GCS::QRAlgorithm aQRAlgo = myEquationSystem->qrAlgorithm;
+ myEquationSystem->qrAlgorithm = GCS::EigenDenseQR;
+ diagnose();
+ GCS::VEC_pD aFreeParams;
+ myEquationSystem->getDependentParams(aFreeParams);
+ theFreeParams.insert(aFreeParams.begin(), aFreeParams.end());
+ // revert QR decomposition algorithm
+ myEquationSystem->qrAlgorithm = aQRAlgo;
+ }
+
+ if (theFreeParams.empty())
+ return;
+
+ // find all equal parameters too
+ struct EqualParameters
+ {
+ typedef std::map<double*, std::list<GCS::SET_pD>::iterator> MapParamGroup;
+
+ std::list<GCS::SET_pD> myEqualParams;
+ MapParamGroup myGroups;
+
+ void add(double* theParam1, double* theParam2)
+ {
+ MapParamGroup::iterator aFound1 = myGroups.find(theParam1);
+ MapParamGroup::iterator aFound2 = myGroups.find(theParam2);
+
+ if (aFound1 == myGroups.end()) {
+ if (aFound2 == myGroups.end()) {
+ // create new group
+ myEqualParams.push_back(GCS::SET_pD());
+ std::list<GCS::SET_pD>::iterator aGroup = --myEqualParams.end();
+ aGroup->insert(theParam1);
+ aGroup->insert(theParam2);
+ myGroups[theParam1] = aGroup;
+ myGroups[theParam2] = aGroup;
+ }
+ else {
+ // add first parameter to the second group
+ aFound2->second->insert(theParam1);
+ myGroups[theParam1] = aFound2->second;
+ }
+ }
+ else {
+ if (aFound2 == myGroups.end()) {
+ // add second parameter to the first group
+ aFound1->second->insert(theParam2);
+ myGroups[theParam2] = aFound1->second;
+ }
+ else if (aFound1 != aFound2) {
+ // merge two groups
+ GCS::SET_pD aCopy = *(aFound2->second);
+ myEqualParams.erase(aFound2->second);
+ for (GCS::SET_pD::iterator anIt = aCopy.begin(); anIt != aCopy.end(); ++anIt)
+ myGroups[*anIt] = aFound1->second;
+ aFound1->second->insert(aCopy.begin(), aCopy.end());
+ }
+ }
+ }
+ } anEqualParams;