ConvexConstraintsPtr ZMPConstraint::convex(const DblVec& x, Model* model) {
DblVec curvals = getDblVec(x, m_vars);
m_rad->SetDOFValues(curvals);
DblMatrix jacmoment = DblMatrix::Zero(3, curvals.size());
OR::Vector moment(0,0,0);
float totalmass = 0;
BOOST_FOREACH(const KinBody::LinkPtr& link, m_rad->GetRobot()->GetLinks()) {
if (!link->GetGeometries().empty()) {
OR::Vector cm = link->GetGlobalCOM();
moment += cm * link->GetMass();
jacmoment += m_rad->PositionJacobian(link->GetIndex(), cm) * link->GetMass();
totalmass += link->GetMass();
}
}
moment /= totalmass;
jacmoment /= totalmass;
AffExpr x_expr = AffExpr(moment.x) + varDot(jacmoment.row(0), m_vars) - jacmoment.row(0).dot(toVectorXd(curvals));
AffExpr y_expr = AffExpr(moment.y) + varDot(jacmoment.row(1), m_vars) - jacmoment.row(1).dot(toVectorXd(curvals));
ConvexConstraintsPtr out(new ConvexConstraints(model));
for (int i=0; i < m_ab.rows(); ++i) {
out->addIneqCnt(m_ab(i,0) * x_expr + m_ab(i,1) * y_expr + m_c(i));
}
return out;
}
/**
* @param equality A matrix of equality constraints \f$A_{eq}\f$(the number of
* columns must match number of parameters)
* where \f$A_{eq} x = 0\f$
* @param inequality A matrix of inequality constraints (the number of columns
* must match number of parameters
* where \f$A_{eq} x \geq 0\f$
*/
void AugmentedLagrangianOptimizer::checkConstraints(
const DblMatrix &equality, const DblMatrix &inequality) {
const size_t totalNumConstr =
numEqualityConstraints() + numInequalityConstraints();
if (totalNumConstr == 0)
return;
// Sanity checks on matrix sizes
for (size_t i = 0; i < 2; ++i) {
size_t ncols(0);
std::string matrix("");
if (i == 0) {
ncols = equality.numCols();
matrix = "equality";
} else {
ncols = inequality.numCols();
matrix = "inequality";
}
if (ncols > 0 && ncols != numParameters()) {
std::ostringstream os;
os << "AugmentedLagrangianOptimizer::initializeConstraints - Invalid "
<< matrix << " constraint matrix. Number of columns must match number "
"of parameters. ncols=" << ncols
<< ", nparams=" << numParameters();
throw std::invalid_argument(os.str());
}
}
}
// x given
int FitLine(const DblVector& PointsY, const DblVector& PointsX, double& a, double& b)
{
assert(PointsX.size()==PointsY.size());
DblMatrix S;
S.Assign(3, 2, 0.0);
for(unsigned int i=0; i<PointsX.size(); i++)
{
S[0][0] += PointsX[i] * PointsX[i];
S[0][1] += PointsX[i];
S[0][2] += PointsX[i] * PointsY[i];
S[1][2] += PointsY[i];
}
S[1][1] = (double)PointsX.size();
S.Diagonalize();
DblVector Solutions;
int Ret = S.SolveLinearCramer(Solutions);
if(Ret)
{
a = Solutions[0];
b = Solutions[1];
}
else std::cout << "Error in line fitting" << std::endl;
return Ret;
}
// x given
int FitParabola(const DblVector& PointsY, const DblVector& PointsX, double& a, double& b, double& c)
{
// fit parabola
assert(PointsX.size()==PointsY.size());
DblMatrix S;
S.Assign(4, 3, 0.0);
for(unsigned int i=0; i<PointsX.size(); i++)
{
S[0][0] += PointsX[i] * PointsX[i] * PointsX[i] * PointsX[i];
S[0][1] += PointsX[i] * PointsX[i] * PointsX[i];
S[0][2] += PointsX[i] * PointsX[i];
S[0][3] += PointsX[i] * PointsX[i] * PointsY[i];
S[1][2] += PointsX[i];
S[1][3] += PointsX[i] * PointsY[i];
S[2][3] += PointsY[i];
}
S[1][1] = S[0][2];
S[2][2] = (double)PointsX.size();
S.Diagonalize();
DblVector Solutions;
int Ret = S.SolveLinearCramer(Solutions);
if(Ret==-1) return -1;
else
{
a = Solutions[0];
b = Solutions[1];
c = Solutions[2];
}
return Ret;
}
/** Sets the U matrix
@param newU :: the new U matrix
@param force :: If true, do not check that U matrix is valid
*/
void OrientedLattice::setU(const DblMatrix &newU, const bool force) {
// determinant ==1 or (determinant == +/-1 and force)
if (newU.isRotation() || (force && newU.isOrthogonal())) {
U = newU;
UB = U * getB();
} else
throw std::invalid_argument("U is not a proper rotation");
}
int DblMatrix::qtClustering(double thresh, std::vector<DblMatrix>& clusters)
{
/// initialize the members
IntMatrix members;
members.assign(size(), IntVector());
///// each entry is a neighbor of itself
for(unsigned int i=0; i<size(); i++)
members[i].push_back(i);
/// get the other members
for(unsigned int j=0; j<size()-1; j++)
{
for(unsigned int k=j+1; k<size(); k++)
{
double diff = (*this)[j].EuclDist((*this)[k]);
if(diff <= thresh)
{
members[j].push_back(k);
members[k].push_back(j);
}
}
}
/// QT clustering
std::sort(members.begin(), members.end(), IntVectorGreater);
IntMatrix::iterator it1;
int n=0;
for(it1=members.begin(); it1!=members.end(); it1++, n++)
{
for(unsigned int j=1; j<it1->size(); j++)
{
IntMatrix::iterator it2=it1;
it2++;
for(; it2!=members.end();)
{
if((*it1)[j]==it2->front()) it2=members.erase(it2);
else it2++;
}
}
}
/// build groups
clusters.clear();
for(unsigned int i=0; i<members.size(); i++)
{
DblMatrix cluster;
for(unsigned int j=0; j<members[i].size(); j++)
{
cluster.push_back((*this)[members[i][j]]);
}
clusters.push_back(cluster);
}
return 0;
}
// fit a two dimensional quadratic surface
int FitQuadraticSurface(const DblVector& PX, const DblVector& PY,
const DblVector& PZ, DblVector& Parameters)
{
// fit parabola
assert(PX.size()==PY.size());
DblMatrix S;
S.Assign(7, 6, 0.0);
for(unsigned int i=0; i<PX.size(); i++)
{
double X2 = PX[i] * PX[i];
double Y2 = PY[i] * PY[i];
double X3 = X2 * PX[i];
double Y3 = Y2 * PY[i];
double X4 = X3 * PX[i];
double Y4 = Y3 * PY[i];
S[0][1] += PY[i];
S[1][1] += Y2;
S[0][2] += PX[i];
S[1][2] += PX[i]*PY[i];
S[2][2] += X2;
S[0][3] += PX[i]*PY[i];
S[1][3] += PX[i]*Y2;
S[2][3] += X2*PY[i];
S[3][3] += X2*Y2;
S[0][4] += Y2;
S[1][4] += Y3;
S[2][4] += PX[i]*Y2;
S[3][4] += PX[i]*Y3;
S[4][4] += Y4;
S[0][5] += X2;
S[1][5] += X2*PY[i];
S[2][5] += X3;
S[3][5] += X3*PY[i];
S[4][5] += X2*Y2;
S[5][5] += X4;
// function values
S[0][6]+=PZ[i];
S[1][6]+=PY[i]*PZ[i];
S[2][6]+=PX[i]*PZ[i];
S[3][6]+=PX[i]*PY[i]*PZ[i];
S[4][6]+=Y2*PZ[i];
S[5][6]+=X2*PZ[i];
}
S[0][0] = (double)PX.size();
S.Diagonalize();
return S.SolveLinearCramer(Parameters);
}
/** Sets the UB matrix and recalculates lattice parameters
@param newUB :: the new UB matrix*/
void OrientedLattice::setUB(const DblMatrix &newUB) {
// check if determinant is close to 0. The 1e-10 value is arbitrary
if (std::fabs(newUB.determinant()) > 1e-10) {
UB = newUB;
DblMatrix newGstar, B;
newGstar = newUB.Tprime() * newUB;
this->recalculateFromGstar(newGstar);
B = this->getB();
B.Invert();
U = newUB * B;
} else
throw std::invalid_argument("determinant of UB is too close to 0");
}
void SharedImageSequence::ExportSequenceAsPointClouds(const std::string& Name)
{
SharedImageSequence::iterator it;
int i=0;
for(it=begin(); it!=end(); it++)
{
std::stringstream FileNameStream;
FileNameStream << Name << i << m_PCloudAttachement;
DblMatrix M;
it->ExportToPointCloud(M);
M.Save(FileNameStream.str());
i++;
}
}
/** Default constructor
@param Umatrix :: orientation matrix U. By default this will be identity matrix
*/
NiggliCell::NiggliCell(const DblMatrix &Umatrix) : UnitCell() {
if (Umatrix.isRotation() == true) {
U = Umatrix;
UB = U * getB();
} else
throw std::invalid_argument("U is not a proper rotation");
}
/** Sets the U matrix
@param newU :: the new U matrix
@param force :: If true, do not check that U matrix is valid
*/
void OrientedLattice::setU(const DblMatrix &newU, const bool force) {
if (force || newU.isRotation()) {
U = newU;
UB = U * getB();
} else
throw std::invalid_argument("U is not a proper rotation");
}
/** Default constructor
@param Umatrix :: orientation matrix U. By default this will be identity matrix
*/
OrientedLattice::OrientedLattice(const DblMatrix &Umatrix) : UnitCell() {
if (Umatrix.isRotation()) {
U = Umatrix;
UB = U * getB();
} else
throw std::invalid_argument("U is not a proper rotation");
}
/**
Get the UB matrix corresponding to the real space edge vectors a,b,c.
The inverse of the matrix with vectors a,b,c as rows will be stored in UB.
@param UB A 3x3 matrix that will be set to the UB matrix.
@param a_dir The real space edge vector for side a of the unit cell
@param b_dir The real space edge vector for side b of the unit cell
@param c_dir The real space edge vector for side c of the unit cell
@return true if UB was set to the new matrix and false if UB could not be
set since the matrix with a,b,c as rows could not be inverted.
*/
bool OrientedLattice::GetUB(DblMatrix &UB, const V3D &a_dir, const V3D &b_dir,
const V3D &c_dir) {
if (UB.numRows() != 3 || UB.numCols() != 3) {
throw std::invalid_argument("Find_UB(): UB matrix NULL or not 3X3");
}
UB.setRow(0, a_dir);
UB.setRow(1, b_dir);
UB.setRow(2, c_dir);
try {
UB.Invert();
} catch (...) {
return false;
}
return true;
}
/**
@param inname Name of workspace containing peaks
@param params optimized cell parameters
@param out residuals from optimization
*/
void OptimizeLatticeForCellType::optLattice(std::string inname,
std::vector<double> ¶ms,
double *out) {
PeaksWorkspace_sptr ws = boost::dynamic_pointer_cast<PeaksWorkspace>(
AnalysisDataService::Instance().retrieve(inname));
const std::vector<Peak> &peaks = ws->getPeaks();
size_t n_peaks = ws->getNumberPeaks();
std::vector<V3D> q_vector;
std::vector<V3D> hkl_vector;
for (size_t i = 0; i < params.size(); i++)
params[i] = std::abs(params[i]);
for (size_t i = 0; i < n_peaks; i++) {
q_vector.push_back(peaks[i].getQSampleFrame());
hkl_vector.push_back(peaks[i].getHKL());
}
Mantid::API::IAlgorithm_sptr alg = createChildAlgorithm("CalculateUMatrix");
alg->setPropertyValue("PeaksWorkspace", inname);
alg->setProperty("a", params[0]);
alg->setProperty("b", params[1]);
alg->setProperty("c", params[2]);
alg->setProperty("alpha", params[3]);
alg->setProperty("beta", params[4]);
alg->setProperty("gamma", params[5]);
alg->executeAsChildAlg();
ws = alg->getProperty("PeaksWorkspace");
OrientedLattice latt = ws->mutableSample().getOrientedLattice();
DblMatrix UB = latt.getUB();
DblMatrix A = aMatrix(params);
DblMatrix Bc = A;
Bc.Invert();
DblMatrix U1_B1 = UB * A;
OrientedLattice o_lattice;
o_lattice.setUB(U1_B1);
DblMatrix U1 = o_lattice.getU();
DblMatrix U1_Bc = U1 * Bc;
for (size_t i = 0; i < hkl_vector.size(); i++) {
V3D error = U1_Bc * hkl_vector[i] - q_vector[i] / (2.0 * M_PI);
out[i] = error.norm2();
}
return;
}
/**
Get the real space edge vectors a,b,c corresponding to the UB matrix.
The rows of the inverse of the matrix with will be stored in a_dir,
b_dir, c_dir.
@param UB A 3x3 matrix containing a UB matrix.
@param a_dir Will be set to the real space edge vector for side a
of the unit cell
@param b_dir Will be set to the real space edge vector for side b
of the unit cell
@param c_dir Will be set to the real space edge vector for side c
of the unit cell
@return true if the inverse of the matrix UB could be found and the
a_dir, b_dir and c_dir vectors have been set to the rows of
UB inverse.
*/
bool OrientedLattice::GetABC(const DblMatrix &UB, V3D &a_dir, V3D &b_dir,
V3D &c_dir) {
if (UB.numRows() != 3 || UB.numCols() != 3) {
throw std::invalid_argument("GetABC(): UB matrix NULL or not 3X3");
}
DblMatrix UB_inverse(UB);
try {
UB_inverse.Invert();
} catch (...) {
return false;
}
a_dir(UB_inverse[0][0], UB_inverse[0][1], UB_inverse[0][2]);
b_dir(UB_inverse[1][0], UB_inverse[1][1], UB_inverse[1][2]);
c_dir(UB_inverse[2][0], UB_inverse[2][1], UB_inverse[2][2]);
return true;
}
// sub-determinant
double DblMatrix::GetHelpDeterminant(int k)
{
int h=GetHeight();
assert(h+1==GetWidth());
DblMatrix HelpDet;
// replace the
for(int i=0; i<h; i++)
{
HelpDet.push_back(DblVector());
for(int j=0; j<h; j++)
{
if(j==k)
HelpDet[i].push_back((*this)[i][h]);
else
HelpDet[i].push_back((*this)[i][j]);
}
}
return HelpDet.GetDeterminantNxN();
}
/// Constructor from a rotation matrix
/// @param rot :: DblMatrix matrix that is going to be the internal rotation
/// matrix of the goniometer. Cannot push additional axes
Goniometer::Goniometer(DblMatrix rot) {
DblMatrix ide(3, 3), rtr(3, 3);
rtr = rot.Tprime() * rot;
ide.identityMatrix();
if (rtr == ide) {
R = rot;
initFromR = true;
} else
throw std::invalid_argument("rot is not a rotation matrix");
}
/** Constructor
@param _a :: lattice parameter \f$ a \f$
@param _b :: lattice parameter \f$ b \f$
@param _c :: lattice parameter \f$ c \f$
@param _alpha :: lattice parameter \f$ \alpha \f$
@param _beta :: lattice parameter \f$ \beta \f$
@param _gamma :: lattice parameter \f$ \gamma \f$
@param angleunit :: units for angle, of type #AngleUnits. Default is degrees.
@param Umatrix :: orientation matrix U
*/
OrientedLattice::OrientedLattice(const double _a, const double _b,
const double _c, const double _alpha,
const double _beta, const double _gamma,
const DblMatrix &Umatrix, const int angleunit)
: UnitCell(_a, _b, _c, _alpha, _beta, _gamma, angleunit) {
if (Umatrix.isRotation()) {
U = Umatrix;
UB = U * getB();
} else
throw std::invalid_argument("U is not a proper rotation");
}
/** Sets the UB matrix and recalculates lattice parameters
@param newUB :: the new UB matrix*/
void OrientedLattice::setUB(const DblMatrix &newUB) {
if (UB.determinant() > 0) {
UB = newUB;
DblMatrix newGstar, B;
newGstar = newUB.Tprime() * newUB;
this->recalculateFromGstar(newGstar);
B = this->getB();
B.Invert();
U = newUB * B;
} else
throw std::invalid_argument("determinant of UB is not greater than 0");
}
double DblMatrix::GetDeterminantNxN()
{
int w=GetWidth();
assert(w=GetHeight());
if(w==1) return (*this)[0][0];
if(w==2) return GetDeterminant2x2();
if(w==3) return GetDeterminant3x3();
double Det=0.0;
// laplace's method (recursion)
double s=1.0;
if(w%2==1) s=-1.0;
for(int i=0; i<w; i++)
{
DblMatrix SubDet;
SubDet.assign(w-1, DblVector());
for(int j=1; j<w; j++)
{
for(int k=0; k<w; k++)
{
if(k==i)
{
continue;
}
else
{
SubDet[j-1].push_back((*this)[j][k]);
}
}
}
double v = (*this)[0][i];
double a = s * v * SubDet.GetDeterminantNxN();
//std::cout << "s " << s << std::endl;
Det += a;
s*=-1.0;
}
return Det;
}
// solve linear NxN equation system (cramer)
int DblMatrix::SolveLinearCramer(DblVector& Solution)
{
// get main determinant
DblMatrix DetMat;
double Det=0.0;
int w = GetWidth();
int h = GetHeight();
for(int i=0; i<h; i++)
{
DetMat.push_back(DblVector());
for(int j=0; j<w-1; j++)
DetMat[i].push_back((*this)[i][j]);
}
Det = DetMat.GetDeterminantNxN();
if(fabs(Det)<THE_EPS_DEF) return 0;
// get solutions
Solution.clear();
for(int k=0; k<h; k++)
Solution.push_back(this->GetHelpDeterminant(k)/Det);
return 1;
}
unsigned long DblMatrixList::Load(std::string PathName, std::string FileName, std::string InfoFileName)
{
clear();
/// load info header
std::stringstream FileNameStream;
FileNameStream << PathName << InfoFileName;
int s=0;
std::ifstream f(FileNameStream.str().c_str());
if(!f.is_open()) return RET_FAILED;
f >> s;
f.close();
for(int i=0; i<s; i++)
{
DblMatrix Temp;
std::stringstream Num;
Num << PathName << FileName << i << ".txt";
if(Temp.Load(Num.str()) & RET_FAILED) return RET_FAILED;
push_back(Temp);
}
return RET_OK;
}
void LoadIsawUB::readModulatedUB(std::ifstream &in, DblMatrix &ub) {
int ModDim = 0;
Kernel::DblMatrix modub(3, 3);
Kernel::DblMatrix ModVecErr(3, 3);
int maxorder = 0;
bool crossterm = false;
std::string s;
double val;
s = getWord(in, true);
if (!convert(s, val)) {
readToEndOfLine(in, true);
for (size_t row = 0; row < 3; row++) {
for (size_t col = 0; col < 3; col++) {
s = getWord(in, true);
if (!convert(s, val))
throw std::runtime_error(
"The string '" + s +
"' in the file was not understood as a number.");
modub[row][col] = val;
}
readToEndOfLine(in, true);
if (modub[row][0] != 0.0 || modub[row][1] != 0.0 || modub[row][2] != 0.0)
ModDim++;
}
}
readToEndOfLine(in, true);
double latVals[6];
for (double &latVal : latVals) {
s = getWord(in, true);
if (!convert(s, val))
throw std::runtime_error("The string '" + s +
"' in the file was not understood as a number.");
latVal = val;
}
if (ModDim > 0) {
readToEndOfLine(in, true);
readToEndOfLine(in, true);
for (int i = 0; i < ModDim; i++) {
readToEndOfLine(in, true);
for (int j = 0; j < 4; j++)
s = getWord(in, true);
for (int j = 0; j < 3; j++) {
s = getWord(in, true);
if (!convert(s, val))
throw std::runtime_error(
"The string '" + s +
"' in the file was not understood as a number.");
ModVecErr[i][j] = val;
}
readToEndOfLine(in, true);
}
readToEndOfLine(in, true);
for (int j = 0; j < 3; j++)
s = getWord(in, true);
if (!convert(s, val))
throw std::runtime_error("The string '" + s +
"' in the file was not understood as a number.");
maxorder = static_cast<int>(val);
readToEndOfLine(in, true);
for (int j = 0; j < 3; j++)
s = getWord(in, true);
bool valBool;
if (!convert(s, valBool))
throw std::runtime_error("The string '" + s +
"' in the file was not understood as a number.");
crossterm = valBool;
}
// Adjust the UB by transposing
ub = ub.Transpose();
modub = modub.Transpose();
/* The method in OrientedLattice gets both the lattice parameters and the U
* matrix from the UB matrix.
* This is compatible (same results) with the ISAW lattice parameters */
OrientedLattice *latt = new OrientedLattice();
latt->setUB(ub);
latt->setError(latVals[0], latVals[1], latVals[2], latVals[3], latVals[4],
latVals[5]);
latt->setModUB(modub);
for (int i = 0; i < ModDim; i++)
latt->setModerr(i, ModVecErr[i][0], ModVecErr[i][1], ModVecErr[i][2]);
latt->setMaxOrder(maxorder);
latt->setCrossTerm(crossterm);
DblMatrix U = latt->getU();
// Swap rows around to accound for IPNS convention
DblMatrix U2 = U;
// Swap rows around
for (size_t r = 0; r < 3; r++) {
U2[2][r] = U[0][r];
U2[1][r] = U[2][r];
U2[0][r] = U[1][r];
}
U = U2;
const bool checkU = getProperty("CheckUMatrix");
latt->setU(U, !checkU);
// In and Out workspace.
Workspace_sptr ws1 = getProperty("InputWorkspace");
ExperimentInfo_sptr ws;
MultipleExperimentInfos_sptr MDWS =
boost::dynamic_pointer_cast<MultipleExperimentInfos>(ws1);
if (MDWS != nullptr) {
ws = MDWS->getExperimentInfo(0);
} else {
ws = boost::dynamic_pointer_cast<ExperimentInfo>(ws1);
}
if (!ws)
throw std::invalid_argument("Must specify either a MatrixWorkspace or a "
"PeaksWorkspace or a MDWorkspace.");
// Save it into the workspace
ws->mutableSample().setOrientedLattice(latt);
// Save it to every experiment info in MD workspaces
if ((MDWS != nullptr) && (MDWS->getNumExperimentInfo() > 1)) {
for (uint16_t i = 1; i < MDWS->getNumExperimentInfo(); i++) {
ws = MDWS->getExperimentInfo(i);
ws->mutableSample().setOrientedLattice(latt);
}
}
delete latt;
this->setProperty("InputWorkspace", ws1);
}
/** gets a vector in the horizontal plane, perpendicular to the beam direction
when
goniometers are at 0. Note, this vector is not unique, but all vectors can be
obtaineb by multiplying with a scalar
@return v :: V3D vector perpendicular to the beam direction, in the horizontal
plane*/
Kernel::V3D OrientedLattice::getvVector() {
V3D x(1, 0, 0);
DblMatrix UBinv = UB;
UBinv.Invert();
return UBinv * x;
}
/** gets a vector along beam direction when goniometers are at 0. Note, this
vector is not unique, but
all vectors can be obtaineb by multiplying with a scalar
@return u :: V3D vector along beam direction*/
Kernel::V3D OrientedLattice::getuVector() {
V3D z(0, 0, 1);
DblMatrix UBinv = UB;
UBinv.Invert();
return UBinv * z;
}
void DblPtrMat::ImportFromDblMatrix(DblMatrix& m)
{
for(int i=0; i<m.GetHeight(); i++)
this->push_back(&(m[i]));
}
/** Calculate the hkl corresponding to a given Q-vector
* @param Q :: Q-vector in $AA^-1 in the sample frame
* @return a V3D with H,K,L
*/
V3D OrientedLattice::hklFromQ(const V3D &Q) const {
DblMatrix UBinv = this->getUB();
UBinv.Invert();
V3D out = UBinv * Q / TWO_PI; // transform back to HKL
return out;
}
/** Solves A*x = B using SVD
* @param A :: [input] The matrix A
* @param B :: [input] The vector B
* @return :: The solution x
*/
std::vector<double> MaxEnt::solveSVD(const DblMatrix &A, const DblMatrix &B) {
size_t dim = A.size().first;
gsl_matrix *a = gsl_matrix_alloc(dim, dim);
gsl_matrix *v = gsl_matrix_alloc(dim, dim);
gsl_vector *s = gsl_vector_alloc(dim);
gsl_vector *w = gsl_vector_alloc(dim);
gsl_vector *x = gsl_vector_alloc(dim);
gsl_vector *b = gsl_vector_alloc(dim);
// Need to copy from DblMatrix to gsl matrix
for (size_t k = 0; k < dim; k++)
for (size_t l = 0; l < dim; l++)
gsl_matrix_set(a, k, l, A[k][l]);
for (size_t k = 0; k < dim; k++)
gsl_vector_set(b, k, B[k][0]);
// Singular value decomposition
gsl_linalg_SV_decomp(a, v, s, w);
// A could be singular or ill-conditioned. We can use SVD to obtain a least
// squares
// solution by setting the small (compared to the maximum) singular values to
// zero
// Find largest sing value
double max = gsl_vector_get(s, 0);
for (size_t i = 0; i < dim; i++) {
if (max < gsl_vector_get(s, i))
max = gsl_vector_get(s, i);
}
// Apply a threshold to small singular values
const double THRESHOLD = 1E-6;
double threshold = THRESHOLD * max;
for (size_t i = 0; i < dim; i++)
if (gsl_vector_get(s, i) > threshold)
gsl_vector_set(s, i, gsl_vector_get(s, i));
else
gsl_vector_set(s, i, 0);
// Solve A*x = B
gsl_linalg_SV_solve(a, v, s, b, x);
// From gsl_vector to vector
std::vector<double> delta(dim);
for (size_t k = 0; k < dim; k++)
delta[k] = gsl_vector_get(x, k);
gsl_matrix_free(a);
gsl_matrix_free(v);
gsl_vector_free(s);
gsl_vector_free(w);
gsl_vector_free(x);
gsl_vector_free(b);
return delta;
}