int main ()
{
double a_data[] = { 0.18, 0.60, 0.57, 0.96,
0.41, 0.24, 0.99, 0.58,
0.14, 0.30, 0.97, 0.66,
0.51, 0.13, 0.19, 0.85 };
double b_data[] = { 1.0, 2.0, 3.0, 4.0 };
gsl_matrix_view m
= gsl_matrix_view_array (a_data, 4, 4);
gsl_vector_view b
= gsl_vector_view_array (b_data, 4);
gsl_vector *x = gsl_vector_alloc (4);
int s;
gsl_permutation * p = gsl_permutation_alloc (4);
gsl_linalg_LU_decomp (&m.matrix, p, &s);
gsl_linalg_LU_solve (&m.matrix, p, &b.vector, x);
printf ("x = \n");
gsl_vector_fprintf (stdout, x, "%g");
gsl_permutation_free (p);
gsl_vector_free (x);
return 0;
}
/* solve M x = V: x = M:solve(V) */
static int
md_solve(lua_State *L)
{
mMatReal *m = qlua_checkMatReal(L, 1);
mVecReal *v = qlua_checkVecReal(L, 2);
mMatReal *lu;
mVecReal *x;
gsl_permutation *p;
gsl_vector_view vb = gsl_vector_view_array(v->val, v->size);
gsl_vector_view vx;
int signum;
if (m->l_size != m->r_size)
return luaL_error(L, "square matrix expected");
if (m->l_size != v->size)
return luaL_error(L, "vector dim mismatch matrix size");
p = new_permutation(L, m->l_size);
lu = qlua_newMatReal(L, m->l_size, m->l_size);
x = qlua_newVecReal(L, m->l_size);
vx = gsl_vector_view_array(x->val, x->size);
gsl_matrix_memcpy(lu->m, m->m);
gsl_linalg_LU_decomp(lu->m, p, &signum);
if (gsl_linalg_LU_solve(lu->m, p, &vb.vector, &vx.vector))
luaL_error(L, "matrix:solve() failed");
gsl_permutation_free(p);
return 1;
}
// center must be allocated to ndims vals at least.
// coord are the vertice coords
// center is the center of the resulting ndims-sphere
// function returns its radius squared
double SimplexSphere(double **coord, int ndims,double *center)
{
double a_data[ndims*ndims];
double b_data[ndims];
gsl_matrix_view m=gsl_matrix_view_array (a_data, ndims, ndims);
gsl_vector_view b=gsl_vector_view_array (b_data, ndims);
gsl_vector_view x = gsl_vector_view_array (center,ndims);
gsl_permutation * p = gsl_permutation_alloc (ndims);
int s;
int i,j,k;
double d;
for (i=0,k=0;i<ndims;i++)
{
b_data[i]=0;
for (j=0;j<ndims;j++,k++)
{
a_data[k]=((double)coord[0][j]-(double)coord[i+1][j]);
b_data[i]+=((double)coord[0][j]*(double)coord[0][j]-(double)coord[i+1][j]*(double)coord[i+1][j]);
}
b_data[i]*=0.5;
}
gsl_linalg_LU_decomp (&m.matrix, p, &s);
gsl_linalg_LU_solve (&m.matrix, p, &b.vector, &x.vector);
gsl_permutation_free (p);
for (i=0,d=0;i<ndims;i++)
d+=((double)center[i]-(double)coord[0][i])*((double)center[i]-(double)coord[0][i]);
return d;
}
int main(int ac, char** av)
{
grid_t g;
gsl_matrix* a;
gsl_vector* x;
gsl_vector* b;
gsl_permutation* p;
int s;
int i;
grid_init_once(&g);
alloc_linear_system(&g, &a, &x, &b);
generate_a(&g, a);
p = gsl_permutation_alloc(a->size1);
gsl_linalg_LU_decomp(a, p, &s);
for (i = 1; i < ac; ++i)
{
grid_load_string(&g, av[i]);
generate_b(&g, b);
gsl_linalg_LU_solve(a, p, b, x);
print_vector(x);
printf("---\n");
}
gsl_permutation_free(p);
free_linear_system(a, x, b);
return 0;
}
void StrainForce::solve(double time) {
if (nodes.size() < 1)
return;
if (nodes.size() < 2) {
if (auto sys = system.lock()) {
auto node = sys->getNode(nodes.front());
node->applyForce(appliedForce);
return;
}
}
if (!initialized) {
setup();
initialized = true;
}
if (auto sys = system.lock()) {
size_t size = nodes.size()-1;
auto mainNode = sys->getNode(mainId);
mainNode->applyForce(appliedForce);
for (size_t i = 0; i < size; ++i) {
auto node = sys->getNode(otherIds[i]);
auto right = mainNode->getForce() - mainNode->getMass() / node->getMass() * node->getForce();
gsl_vector_set(vecB[0], i, right.x);
gsl_vector_set(vecB[1], i, right.y);
gsl_vector_set(vecB[2], i, right.z);
}
for (int i = 0; i < 3; ++i)
gsl_linalg_LU_solve(matA, permutation, vecB[i], vecX[i]);
for (size_t i = 0; i < size; ++i) {
auto node = sys->getNode(otherIds[i]);
glm::dvec3 f = {
gsl_vector_get(vecX[0], i),
gsl_vector_get(vecX[1], i),
gsl_vector_get(vecX[2], i),
};
node->applyForce(f);
mainNode->applyForce(-f);
}
if(fixedAxe)
for (size_t i = 0; i < nodes.size(); ++i)
{
auto node = sys->getNode(nodes[i]);
node->FixDirection(appliedForce);
}
}
}
CAMLprim value ml_gsl_linalg_LU_solve(value LU, value P, value B, value X)
{
GSL_PERMUT_OF_BIGARRAY(P);
_DECLARE_MATRIX(LU);
_DECLARE_VECTOR2(B,X);
_CONVERT_MATRIX(LU);
_CONVERT_VECTOR2(B,X);
gsl_linalg_LU_solve(&m_LU, &perm_P, &v_B, &v_X);
return Val_unit;
}
int tGSLSolve::solveLU(void)
{
int s;
gsl_permutation * p = gsl_permutation_alloc (N);
gsl_linalg_LU_decomp(MATRIX, p, &s);
for (int i=0;i<RHS.count() && i<x.count(); i++){
gsl_linalg_LU_solve (MATRIX, p, RHS.at(i), x.at(i));
}
return s;
}
int update_circuit(Circuit* c)
{
gsl_matrix* A;
gsl_vector* b;
to_matrix(c,&A,&b);
if(A==NULL){
return -1;
}
gsl_permutation * p=gsl_permutation_alloc(b->size);
gsl_vector* x =gsl_vector_alloc(b->size);
if(p==NULL||x==NULL){
printf("unable to allocate memory (update_circuit()), freeing and halting\n");
gsl_matrix_free(A);
gsl_vector_free(b);
if(p!=NULL){
gsl_permutation_free(p);
}
if(x!=NULL){
gsl_vector_free(x);
}
return -1;
}
int i,j;
for(i=0;i<A->size1;i++){
printf("[ ");
for(j=0;j<A->size2;j++)
{
printf("%6.3g ",gsl_matrix_get(A,i,j));
}
printf("] [ %6.3g ]\n",gsl_vector_get(b,i));
}
int s;
gsl_linalg_LU_decomp(A,p,&s);
double det=gsl_linalg_LU_det(A,s);
printf("\ndeterminant is %g\n",det);
if(det==0.0||(det<0x1p-16&&det>-0x1p-16)){
printf("ERROR, NON-TRIVIAL SOLUTION\nFREEING MEMORY AND HALTING COMPUTATION\n...");
gsl_vector_free(x);
gsl_vector_free(b);
gsl_matrix_free(A);
gsl_permutation_free(p);
printf("DONE\n");
return -1;
}
gsl_linalg_LU_solve(A,p,b,x);
printf("\nthe solution is:\n");
print_vector(x);
to_circuit(x,c);
gsl_vector_free(x);
gsl_matrix_free(A);
gsl_vector_free(b);
gsl_permutation_free(p);
return 0;
}
void QgsLeastSquares::helmert( std::vector<QgsPoint> mapCoords,
std::vector<QgsPoint> pixelCoords,
QgsPoint& origin, double& pixelSize,
double& rotation )
{
int n = mapCoords.size();
if ( n < 2 )
{
throw std::domain_error( QObject::tr( "Fit to a Helmert transform requires at least 2 points." ).toLocal8Bit().constData() );
}
double A = 0, B = 0, C = 0, D = 0, E = 0, F = 0, G = 0, H = 0, I = 0, J = 0;
for ( int i = 0; i < n; ++i )
{
A += pixelCoords[i].x();
B += pixelCoords[i].y();
C += mapCoords[i].x();
D += mapCoords[i].y();
E += mapCoords[i].x() * pixelCoords[i].x();
F += mapCoords[i].y() * pixelCoords[i].y();
G += std::pow( pixelCoords[i].x(), 2 );
H += std::pow( pixelCoords[i].y(), 2 );
I += mapCoords[i].x() * pixelCoords[i].y();
J += pixelCoords[i].x() * mapCoords[i].y();
}
/* The least squares fit for the parameters { a, b, x0, y0 } is the solution
to the matrix equation Mx = b, where M and b is given below. I *think*
that this is correct but I derived it myself late at night. Look at
helmert.jpg if you suspect bugs. */
double MData[] = { A, -B, n, 0,
B, A, 0, n,
G + H, 0, A, B,
0, G + H, -B, A
};
double bData[] = { C, D, E + F, J - I };
// we want to solve the equation M*x = b, where x = [a b x0 y0]
gsl_matrix_view M = gsl_matrix_view_array( MData, 4, 4 );
gsl_vector_view b = gsl_vector_view_array( bData, 4 );
gsl_vector* x = gsl_vector_alloc( 4 );
gsl_permutation* p = gsl_permutation_alloc( 4 );
int s;
gsl_linalg_LU_decomp( &M.matrix, p, &s );
gsl_linalg_LU_solve( &M.matrix, p, &b.vector, x );
gsl_permutation_free( p );
origin.setX( gsl_vector_get( x, 2 ) );
origin.setY( gsl_vector_get( x, 3 ) );
pixelSize = std::sqrt( std::pow( gsl_vector_get( x, 0 ), 2 ) +
std::pow( gsl_vector_get( x, 1 ), 2 ) );
rotation = std::atan2( gsl_vector_get( x, 1 ), gsl_vector_get( x, 0 ) );
}
/*! \brief Discrete Cepstrum Transform
*
* method for computing cepstrum aenalysis from a discrete
* set of partial peaks (frequency and amplitude)
*
* This implementation is owed to the help of Jordi Janer (thanks!) from the MTG,
* along with the following paper:
* "Regularization Techniques for Discrete Cepstrum Estimation"
* Olivier Cappe and Eric Moulines, IEEE Signal Processing Letters, Vol. 3
* No.4, April 1996
*
* \todo add anchor point add at frequency = 0 with the same magnitude as the first
* peak in pMag. This does not change the size of the cepstrum, only helps to smoothen it
* at the very beginning.
*
* \param sizeCepstrum order+1 of the discrete cepstrum
* \param pCepstrum pointer to output array of cepstrum coefficients
* \param sizeFreq number of partials peaks (the size of pFreq should be the same as pMag
* \param pFreq pointer to partial peak frequencies (hertz)
* \param pMag pointer to partial peak magnitudes (linear)
* \param fLambda regularization factor
* \param iMaxFreq maximum frequency of cepstrum
*/
void sms_dCepstrum( int sizeCepstrum, sfloat *pCepstrum, int sizeFreq, sfloat *pFreq, sfloat *pMag,
sfloat fLambda, int iMaxFreq)
{
int i, k;
sfloat factor;
sfloat fNorm = PI / (float)iMaxFreq; /* value to normalize frequencies to 0:0.5 */
//static sizeCepstrumStatic
static CepstrumMatrices m;
//printf("nPoints: %d, nCoeff: %d \n", m.nPoints, m.nCoeff);
if(m.nPoints != sizeCepstrum || m.nCoeff != sizeFreq)
AllocateDCepstrum(sizeFreq, sizeCepstrum, &m);
int s; /* signum: "(-1)^n, where n is the number of interchanges in the permutation." */
/* compute matrix M (eq. 4)*/
for (i=0; i<sizeFreq; i++)
{
gsl_matrix_set (m.pM, i, 0, 1.); // first colum is all 1
for (k=1; k <sizeCepstrum; k++)
gsl_matrix_set (m.pM, i, k , 2.*sms_sine(PI_2 + fNorm * k * pFreq[i]) );
}
/* compute transpose of M */
gsl_matrix_transpose_memcpy (m.pMt, m.pM);
/* compute R diagonal matrix (for eq. 7)*/
factor = COEF * (fLambda / (1.-fLambda)); /* \todo why is this divided like this again? */
for (k=0; k<sizeCepstrum; k++)
gsl_matrix_set(m.pR, k, k, factor * powf((sfloat) k,2.));
/* MtM = Mt * M, later will add R */
gsl_blas_dgemm (CblasNoTrans, CblasNoTrans, 1., m.pMt, m.pM, 0.0, m.pMtMR);
/* add R to make MtMR */
gsl_matrix_add (m.pMtMR, m.pR);
/* set pMag in X and multiply with Mt to get pMtXk */
for(k = 0; k <sizeFreq; k++)
gsl_vector_set(m.pXk, k, log(pMag[k]));
gsl_blas_dgemv (CblasNoTrans, 1., m.pMt, m.pXk, 0., m.pMtXk);
/* solve x (the cepstrum) in Ax = b, where A=MtMR and b=pMtXk */
/* ==== the Cholesky Decomposition way ==== */
/* MtM is 'symmetric and positive definite?' */
//gsl_linalg_cholesky_decomp (m.pMtMR);
//gsl_linalg_cholesky_solve (m.pMtMR, m.pMtXk, m.pC);
/* ==== the LU decomposition way ==== */
gsl_linalg_LU_decomp (m.pMtMR, m.pPerm, &s);
gsl_linalg_LU_solve (m.pMtMR, m.pPerm, m.pMtXk, m.pC);
/* copy pC to pCepstrum */
for(i = 0; i < sizeCepstrum; i++)
pCepstrum[i] = gsl_vector_get (m.pC, i);
}
static void __attribute__((unused)) solve_linear_system
(gsl_matrix* a, gsl_vector* x, const gsl_vector* b)
{
gsl_permutation* const p = gsl_permutation_alloc(a->size1);
int s;
gsl_linalg_LU_decomp(a, p, &s);
gsl_linalg_LU_solve(a, p, b, x);
gsl_permutation_free(p);
}
void QgsLeastSquares::affine( std::vector<QgsPoint> mapCoords,
std::vector<QgsPoint> pixelCoords )
{
int n = mapCoords.size();
if ( n < 4 )
{
throw std::domain_error( QObject::tr( "Fit to an affine transform requires at least 4 points." ).toLocal8Bit().constData() );
}
double A = 0, B = 0, C = 0, D = 0, E = 0, F = 0,
G = 0, H = 0, I = 0, J = 0, K = 0;
for ( int i = 0; i < n; ++i )
{
A += pixelCoords[i].x();
B += pixelCoords[i].y();
C += mapCoords[i].x();
D += mapCoords[i].y();
E += std::pow( pixelCoords[i].x(), 2 );
F += std::pow( pixelCoords[i].y(), 2 );
G += pixelCoords[i].x() * pixelCoords[i].y();
H += pixelCoords[i].x() * mapCoords[i].x();
I += pixelCoords[i].y() * mapCoords[i].y();
J += pixelCoords[i].x() * mapCoords[i].y();
K += mapCoords[i].x() * pixelCoords[i].y();
}
/* The least squares fit for the parameters { a, b, c, d, x0, y0 } is the
solution to the matrix equation Mx = b, where M and b is given below.
I *think* that this is correct but I derived it myself late at night.
Look at affine.jpg if you suspect bugs. */
double MData[] = { A, B, 0, 0, n, 0,
0, 0, A, B, 0, n,
E, G, 0, 0, A, 0,
G, F, 0, 0, B, 0,
0, 0, E, G, 0, A,
0, 0, G, F, 0, B
};
double bData[] = { C, D, H, K, J, I };
// we want to solve the equation M*x = b, where x = [a b c d x0 y0]
gsl_matrix_view M = gsl_matrix_view_array( MData, 6, 6 );
gsl_vector_view b = gsl_vector_view_array( bData, 6 );
gsl_vector* x = gsl_vector_alloc( 6 );
gsl_permutation* p = gsl_permutation_alloc( 6 );
int s;
gsl_linalg_LU_decomp( &M.matrix, p, &s );
gsl_linalg_LU_solve( &M.matrix, p, &b.vector, x );
gsl_permutation_free( p );
}
void fred2_solver(double a,
double b,
gsl_vector* t,
gsl_vector* f,
gsl_vector* w)
{
gsl_integration_glfixed_table* glgrid = gsl_integration_glfixed_table_alloc(MAX);
gsl_permutation* p = gsl_permutation_alloc(MAX);
gsl_matrix* lhs = gsl_matrix_alloc(MAX, MAX);
gsl_matrix* ktilde = gsl_matrix_alloc(MAX, MAX);
gsl_matrix* ak = gsl_matrix_alloc(MAX, MAX);
gsl_vector* g = gsl_vector_alloc(MAX);
int i, j, error, s;
double ptsi, wghtsi;
// set Gauss-Legendre integration points and weights
for (i = 0; i < MAX; i++)
{
error = gsl_integration_glfixed_point(a, b, i, &ptsi, &wghtsi, glgrid);
gsl_vector_set(t, i, ptsi);
gsl_vector_set(w, i, wghtsi);
}
kernel(ak, t, t);
aux_g(g, t);
// fill in unit matrix first
gsl_matrix_set_identity(lhs);
for (i = 0; i < MAX; i++)
{
for (j = 0; j < MAX; j++)
{
gsl_matrix_set(ktilde, i, j, gsl_matrix_get(ak, i, j) * gsl_vector_get(w, j));
}
}
// set up LHS matrix
error = gsl_matrix_sub(lhs, ktilde);
gsl_linalg_LU_decomp(lhs, p, &s);
gsl_linalg_LU_solve(lhs, p, g, f);
gsl_integration_glfixed_table_free(glgrid);
gsl_permutation_free(p);
gsl_matrix_free(lhs);
gsl_matrix_free(ktilde);
gsl_vector_free(g);
gsl_matrix_free(ak);
return;
}
double gaussian_pdf_log(const gaussian_t* dist,
const gsl_vector* x) {
double r = 0.0;
double logdet = 0.0;
if (gaussian_isdiagonal(dist)) {
size_t i;
double dx, dd;
for (i = 0; i < dist->dim; i++) {
dx = gsl_vector_get(x, i) - gsl_vector_get(dist->mean, i);
dd = gsl_vector_get(dist->diag, i);
r += dx * dx / dd;
logdet += DEBUG_LOG(dd);
}
}
else {
int signum;
gsl_vector* w1 = gsl_vector_alloc(dist->dim);
gsl_vector* w2 = gsl_vector_alloc(dist->dim);
gsl_vector_memcpy(w1, x);
gsl_vector_sub(w1, dist->mean);
gsl_matrix* v = gsl_matrix_alloc(dist->dim, dist->dim);
gsl_matrix_memcpy(v, dist->cov);
gsl_permutation* p = gsl_permutation_alloc(dist->dim);
gsl_linalg_LU_decomp(v, p, &signum);
gsl_linalg_LU_solve(v, p, w1, w2);
gsl_blas_ddot(w1, w2, &r);
logdet = gsl_linalg_LU_lndet(v);
assert(gsl_linalg_LU_sgndet(v, signum) == 1.0);
gsl_vector_free(w1);
gsl_vector_free(w2);
gsl_matrix_free(v);
gsl_permutation_free(p);
}
/* Use log to avoid underflow !
here
r = (x - mean)^T * cov^-1 * (x - mean)
logdet = log(det(cov))
then
logpdf = -.5 * (k * log(2*pi) + logdet + r);
*/
r = r + dist->dim * DEBUG_LOG(2 * M_PI) + logdet;
r = -0.5 * r;
assert(!isnan(r));
return r;
}
int main()
{
int i,j;
srand(42);
for (i=0;i<dimension;i++)
{
for (j=0;j<dimension;j++)
{
a_data[i*dimension+j]=(rand()%100)/100.0;
/*
if ((rand()%2))
a_data[i*dimension+j]+=0.2;
else
a_data[i*dimension+j]-=0.5;
//printf("%g ",a_data[i*dimension+j]);
*/
}
//printf("\n");
b_data[i]=(rand()%15)+4;
}
gsl_matrix_view m
= gsl_matrix_view_array (a_data, dimension, dimension);
gsl_vector_view b
= gsl_vector_view_array (b_data, dimension);
gsl_vector *x = gsl_vector_alloc (dimension);
int s;
gsl_permutation * p = gsl_permutation_alloc (dimension);
gsl_linalg_LU_decomp (&m.matrix, p, &s);
gsl_linalg_LU_solve (&m.matrix, p, &b.vector, x);
if (PRINT_RES)
{
printf ("x = \n");
gsl_vector_fprintf (stdout, x, "%g");
}
gsl_permutation_free (p);
gsl_vector_free (x);
return 0;
}
static int
dnewton_iterate (void * vstate, gsl_multiroot_function * function, gsl_vector * x, gsl_vector * f, gsl_vector * dx)
{
dnewton_state_t * state = (dnewton_state_t *) vstate;
int signum ;
size_t i;
size_t n = function->n ;
gsl_matrix_memcpy (state->lu, state->J);
{
int status = gsl_linalg_LU_decomp (state->lu, state->permutation, &signum);
if (status)
return status;
}
{
int status = gsl_linalg_LU_solve (state->lu, state->permutation, f, dx);
if (status)
return status;
}
for (i = 0; i < n; i++)
{
double e = gsl_vector_get (dx, i);
double y = gsl_vector_get (x, i);
gsl_vector_set (dx, i, -e);
gsl_vector_set (x, i, y - e);
}
{
int status = GSL_MULTIROOT_FN_EVAL (function, x, f);
if (status != GSL_SUCCESS)
{
return GSL_EBADFUNC;
}
}
gsl_multiroot_fdjacobian (function, x, f, GSL_SQRT_DBL_EPSILON, state->J);
return GSL_SUCCESS;
}
/// Solve system of linear equations M*x == rhs, M is this matrix
/// This matrix is destroyed.
/// @param rhs :: The right-hand-side vector
/// @param x :: The solution vector
void GSLMatrix::solve(const GSLVector &rhs, GSLVector &x) {
if (size1() != size2()) {
throw std::runtime_error(
"System of linear equations: the matrix must be square.");
}
size_t n = size1();
if (rhs.size() != n) {
throw std::runtime_error(
"System of linear equations: right-hand side vector has wrong size.");
}
x.resize(n);
int s;
gsl_permutation *p = gsl_permutation_alloc(n);
gsl_linalg_LU_decomp(gsl(), p, &s); // matrix is modified at this moment
gsl_linalg_LU_solve(gsl(), p, rhs.gsl(), x.gsl());
gsl_permutation_free(p);
}
Real DAESolver::solve()
{
if (the_system_size_ == 0)
{
return 0.0;
}
const VariableArray::size_type a_size(the_system_size_);
gsl_linalg_LU_solve(the_jacobian_matrix1_, the_permutation1_,
the_velocity_vector1_, the_solution_vector1_);
gsl_linalg_complex_LU_solve(the_jacobian_matrix2_, the_permutation2_,
the_velocity_vector2_, the_solution_vector2_);
Real a_norm(0.0);
Real delta_w(0.0);
gsl_complex comp;
VariableArray a_tmp_value_buffer = the_value_differential_buffer_;
a_tmp_value_buffer.insert(
a_tmp_value_buffer.end(), the_value_algebraic_buffer_.begin(),
the_value_algebraic_buffer_.end());
for (VariableArray::size_type c(0); c < a_size; ++c)
{
Real a_tolerance2(rtoler_ * fabs(a_tmp_value_buffer[c]) + atoler_);
a_tolerance2 = a_tolerance2 * a_tolerance2;
delta_w = gsl_vector_get(the_solution_vector1_, c);
the_w_[c] += delta_w;
a_norm += delta_w * delta_w / a_tolerance2;
comp = gsl_vector_complex_get(the_solution_vector2_, c);
delta_w = GSL_REAL(comp);
the_w_[c + a_size] += delta_w;
a_norm += delta_w * delta_w / a_tolerance2;
delta_w = GSL_IMAG(comp);
the_w_[c + a_size * 2] += delta_w;
a_norm += delta_w * delta_w / a_tolerance2;
}
return sqrt(a_norm / (3 * a_size));
}
static int
newton_iterate (void * vstate, gsl_multiroot_function_fdf * fdf, gsl_vector * x, gsl_vector * f, gsl_matrix * J, gsl_vector * dx)
{
newton_state_t * state = (newton_state_t *) vstate;
int signum;
size_t i;
size_t n = fdf->n ;
gsl_matrix_memcpy (state->lu, J);
gsl_linalg_LU_decomp (state->lu, state->permutation, &signum);
{
int status = gsl_linalg_LU_solve (state->lu, state->permutation, f, dx);
if (status)
return status;
}
for (i = 0; i < n; i++)
{
double e = gsl_vector_get (dx, i);
double y = gsl_vector_get (x, i);
gsl_vector_set (dx, i, -e);
gsl_vector_set (x, i, y - e);
}
{
int status = GSL_MULTIROOT_FN_EVAL_F_DF (fdf, x, f, J);
if (status != GSL_SUCCESS)
{
return GSL_EBADFUNC;
}
}
return GSL_SUCCESS;
}
void Module_Rectifier::linear_multiple_regression(double a_data[NUM_EQU*8], double b_data[NUM_EQU]){
gsl_matrix_view m = gsl_matrix_view_array (a_data, 8, 8);
gsl_vector_view b = gsl_vector_view_array (b_data, 8);
gsl_vector *x = gsl_vector_alloc (8);
int s;
gsl_permutation * p = gsl_permutation_alloc (8);
gsl_linalg_LU_decomp (&m.matrix, p, &s);
gsl_linalg_LU_solve (&m.matrix, p, &b.vector, x);
for (int i=0; i<8; i++)
{
solution_matrix[i] = gsl_vector_get(x,i);
}
solution_matrix[8] = 1;
gsl_permutation_free (p);
gsl_vector_free (x);
}
/// Solve system of linear equations M*x == rhs, M is this matrix
/// This matrix is destroyed.
/// @param rhs :: The right-hand-side vector
/// @param x :: The solution vector
/// @throws std::invalid_argument if the input vectors have wrong sizes.
/// @throws std::runtime_error if the GSL fails to solve the equations.
void GSLMatrix::solve(const GSLVector &rhs, GSLVector &x) {
if (size1() != size2()) {
throw std::invalid_argument(
"System of linear equations: the matrix must be square.");
}
size_t n = size1();
if (rhs.size() != n) {
throw std::invalid_argument(
"System of linear equations: right-hand side vector has wrong size.");
}
x.resize(n);
int s;
gsl_permutation *p = gsl_permutation_alloc(n);
gsl_linalg_LU_decomp(gsl(), p, &s); // matrix is modified at this moment
int res = gsl_linalg_LU_solve(gsl(), p, rhs.gsl(), x.gsl());
gsl_permutation_free(p);
if (res != GSL_SUCCESS) {
std::string message = "Failed to solve system of linear equations.\n"
"Error message returned by the GSL:\n" +
std::string(gsl_strerror(res));
throw std::runtime_error(message);
}
}
static int init(void * vstate, const double xa[], const double ya[], size_t size)
{
struct state_t * state = (struct state_t *) vstate;
switch (size)
{
case 2:
{
double A[16];
double x[4];
double b[4];
/* Set up the system */
{
int i;
double h = xa[1] - xa[0];
for (i=0; i<16; i++)
A[i] = 0.0;
/* Zeroth-order, all points have the same value */
/* a = y0 */
A[0*4+0] = 1.0;
b[0] = ya[0];
/* a, b, c, d = y1 */
A[1*4+0] = 1.0;
A[1*4+1] = h;
A[1*4+2] = h*h;
A[1*4+3] = h*h*h;
b[1] = ya[1];
/* The last two equations depend on the endpoint types */
if (state->ltype == BT_INTERP_SLOPE)
{
/* b = alpha */
A[2*4+1] = 1.0;
b[2] = state->alpha;
}
else /* state->ltype == WAM_INTERP_NATURAL */
{
/* c = 0 */
A[2*4+2] = 2.0;
b[2] = 0.0;
}
if (state->rtype == BT_INTERP_SLOPE)
{
/* b, c, d = beta */
A[3*4+1] = 1.0;
A[3*4+2] = 2*h;
A[3*4+3] = 3*h*h;
b[3] = state->beta;
}
else /* state->ltype == WAM_INTERP_NATURAL */
{
/* c, d = 0 */
A[3*4+2] = 2.0;
A[3*4+3] = 6*h;
b[3] = 0.0;
}
}
/* Solve the system */
{
gsl_matrix_view A_view = gsl_matrix_view_array(A,4,4);
gsl_vector_view b_view = gsl_vector_view_array(b,4);
gsl_vector_view x_view = gsl_vector_view_array(x,4);
gsl_permutation * p = gsl_permutation_alloc(4);
int s;
gsl_linalg_LU_decomp( &A_view.matrix, p, &s);
gsl_linalg_LU_solve( &A_view.matrix, p, &b_view.vector, &x_view.vector );
gsl_permutation_free(p);
}
/* Save the solved values */
state->s_2p->a = x[0];
state->s_2p->b = x[1];
state->s_2p->c = x[2];
state->s_2p->d = x[3];
} break;
case 3:
{
double A[64];
double x[8];
double b[8];
/* Set up the system */
{
int i;
double hL = xa[1] - xa[0];
double hR = xa[2] - xa[1];
for (i=0; i<64; i++)
A[i] = 0.0;
/* Zeroth-order, all points have the same value */
/* aL = y0 */
A[0*8+0] = 1.0;
b[0] = ya[0];
/* aL, bL, cL, dL = y1 */
A[1*8+0] = 1.0;
A[1*8+1] = hL;
A[1*8+2] = hL*hL;
A[1*8+3] = hL*hL*hL;
b[1] = ya[1];
/* aR = y1 */
A[2*8+4] = 1.0;
b[2] = ya[1];
/* aR, bR, cR, dR = y2 */
A[3*8+4] = 1.0;
A[3*8+5] = hR;
A[3*8+6] = hR*hR;
A[3*8+7] = hR*hR*hR;
b[3] = ya[2];
/* First-order, the slopes are the same at the midpoint */
/* bL, cL, dL, -bR = 0 */
A[4*8+1] = 1.0;
A[4*8+2] = 2*hL;
A[4*8+3] = 3*hL*hL;
A[4*8+5] = -1.0;
b[4] = 0.0;
/* Second-order, the concavity is the same at the midpoint */
/* cL, dL, -cR = 0 */
A[5*8+2] = 2.0;
A[5*8+3] = 6*hL;
A[5*8+6] = -2.0;
b[5] = 0.0;
/* The last two equations depend on the endpoint types */
if (state->ltype == BT_INTERP_SLOPE)
{
/* bL = alpha */
A[6*8+1] = 1.0;
b[6] = state->alpha;
}
else /* state->ltype == WAM_INTERP_NATURAL */
{
/* cL = 0 */
A[6*8+2] = 2.0;
b[6] = 0.0;
}
if (state->rtype == BT_INTERP_SLOPE)
{
/* bR, cR, dR = beta */
A[7*8+5] = 1.0;
A[7*8+6] = 2*hR;
A[7*8+7] = 3*hR*hR;
b[7] = state->beta;
}
else /* state->rtype == WAM_INTERP_NATURAL */
{
/* cR, dR = 0 */
A[7*8+6] = 2.0;
A[7*8+7] = 6*hL;
b[7] = 0.0;
}
}
/* Solve the system */
{
gsl_matrix_view A_view = gsl_matrix_view_array(A,8,8);
gsl_vector_view b_view = gsl_vector_view_array(b,8);
gsl_vector_view x_view = gsl_vector_view_array(x,8);
gsl_permutation * p = gsl_permutation_alloc(8);
int s;
gsl_linalg_LU_decomp( &A_view.matrix, p, &s);
gsl_linalg_LU_solve( &A_view.matrix, p, &b_view.vector, &x_view.vector );
gsl_permutation_free(p);
}
/* Save the solved values */
state->s_3p->aL = x[0];
state->s_3p->bL = x[1];
state->s_3p->cL = x[2];
state->s_3p->dL = x[3];
state->s_3p->aR = x[4];
state->s_3p->bR = x[5];
state->s_3p->cR = x[6];
state->s_3p->dR = x[7];
} break;
default:
{
/* spline calculation with natural boundary conditions
* or with defined first and/or last derivatives
* see [Engeln-Mullges + Uhlig, p. 258]
*/
/* Note - this is mostly duplication. Oh well. */
size_t i;
size_t num_points = size;
size_t max_index = num_points - 1; /* Engeln-Mullges + Uhlig "n" */
size_t sys_size = max_index - 1; /* linear system is sys_size x sys_size */
/* Note - moved outer c setting to below */
/* Set up the system for the inner c's */
for (i = 0; i < sys_size; i++)
{
const double h_i = xa[i + 1] - xa[i];
const double h_ip1 = xa[i + 2] - xa[i + 1];
const double ydiff_i = ya[i + 1] - ya[i];
const double ydiff_ip1 = ya[i + 2] - ya[i + 1];
const double g_i = (h_i != 0.0) ? 1.0 / h_i : 0.0;
const double g_ip1 = (h_ip1 != 0.0) ? 1.0 / h_ip1 : 0.0;
state->cstate->offdiag[i] = h_ip1;
/* added in here ######### */
if (state->ltype == BT_INTERP_SLOPE && i==0)
{
state->cstate->diag[i] = 1.5 * h_i + 2.0 * h_ip1;
state->cstate->g[i] = 3.0 * (ydiff_ip1 * g_ip1 - 0.5 * ( 3.0*(ydiff_i*g_i) - state->alpha ) );
continue;
}
if (state->rtype == BT_INTERP_SLOPE && i == sys_size-1)
{
state->cstate->diag[i] = 2.0 * h_i + 1.5 * h_ip1;
state->cstate->g[i] = 3.0 * ( 0.5 * ( 3.0*(ydiff_ip1*g_ip1) - state->beta ) - ydiff_i * g_i );
continue;
}
/* ####################### */
state->cstate->diag[i] = 2.0 * (h_ip1 + h_i);
state->cstate->g[i] = 3.0 * (ydiff_ip1 * g_ip1 - ydiff_i * g_i);
}
/* Solve the system for the inner c's */
{
gsl_vector_view g_vec = gsl_vector_view_array(state->cstate->g, sys_size);
gsl_vector_view diag_vec = gsl_vector_view_array(state->cstate->diag, sys_size);
gsl_vector_view offdiag_vec = gsl_vector_view_array(state->cstate->offdiag, sys_size - 1);
gsl_vector_view solution_vec = gsl_vector_view_array ((state->cstate->c) + 1, sys_size);
int status = gsl_linalg_solve_symm_tridiag(&diag_vec.vector,
&offdiag_vec.vector,
&g_vec.vector,
&solution_vec.vector);
if (status != GSL_SUCCESS) return status;
}
/* Set the outer c's. */
if (state->ltype == BT_INTERP_SLOPE)
{
const double h_0 = xa[1] - xa[0];
const double ydiff_0 = ya[1] - ya[0];
state->cstate->c[0] = 1.0/(2.0*h_0) * ( (3.0/h_0)*ydiff_0 - 3.0*state->alpha - state->cstate->c[1]*h_0 );
}
else
state->cstate->c[0] = 0.0;
if (state->rtype == BT_INTERP_SLOPE)
{
const double h_nm1 = xa[max_index] - xa[max_index-1];
const double ydiff_nm1 = ya[max_index] - ya[max_index-1];
state->cstate->c[max_index] = - 1.0/(2.0*h_nm1) * ( (3.0/h_nm1)*ydiff_nm1 - 3.0*state->beta + state->cstate->c[max_index-1]*h_nm1 );
}
else
state->cstate->c[max_index] = 0.0;
} break;
}
return GSL_SUCCESS;
}
void make_cof(points_t *pts, coefficients_t *cof)
{
gsl_matrix *A, *A_inverse, *BF, *wsp;
gsl_permutation * perm, *permtest;
gsl_vector *wspt, *bf;
double *x = pts->x;
double *y = pts->y;
double a = x[0];
double b = x[pts->n - 1];
int m = pts->n - 2 > 10 ? 10 : pts->n - 2;
int i, j, k;
double tmp;
char *mEnv = getenv("APPROX_BASE_SIZE");
if (mEnv != NULL && atoi(mEnv) > 0)
m = atoi(mEnv);
m++;
wspt = gsl_vector_alloc(m);
bf = gsl_vector_alloc(m);
permtest = gsl_permutation_alloc(m);
gsl_vector_set_zero (bf);
for (i = 0; i < m; i++)
{
tmp = 0.0;
for (k = 0; k < pts->n; k++)
tmp += legendre(i, x[k]) * y[k];
gsl_vector_set(bf, i, tmp);
//printf("%g\n", gsl_vector_get(bf, i));
}
A = gsl_matrix_alloc(m, m);
A_inverse = gsl_matrix_alloc(m, m);
perm = gsl_permutation_alloc(m);
BF = gsl_matrix_alloc(m, 1);
wsp = gsl_matrix_alloc(m, 1);
gsl_matrix_set_zero (A);
for (i = 0; i < m; i++)
{
for (j = 0; j < m; j++)
{
for (k = 0; k < pts->n; k++)
{
A->data[i * A->tda + j] += legendre(i, x[k]) * legendre(j, x[k]);
}
}
}
gsl_linalg_LU_decomp(A, permtest, &i);
gsl_linalg_LU_solve(A, permtest, bf, wspt);
gsl_vector_fprintf(stdout, wspt, "%g");
//macierz odwrotna
gsl_linalg_LU_decomp(A, perm, &i);
gsl_linalg_LU_invert(A, perm, A_inverse);
gsl_matrix_free(A);
gsl_permutation_free(perm);
gsl_matrix_set_zero (BF);
for (i = 0; i < m; i++)
{
for (k = 0; k < pts->n; k++)
BF->data[i * BF->tda + 0] += legendre(i, x[k]) * y[k];
}
//mnozenie macierzy
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, A_inverse, BF, 0.0, wsp);
gsl_matrix_free(A_inverse);
gsl_matrix_free(BF);
if (alloc_cof(cof, m) == 0)
{
for (i = 0; i < cof->base; i++)
cof->coefficients[i] = gsl_matrix_get(wsp, i, 0);
}
return;
}
/***************************************************************************************************************
Iterative NLLS fit algorithm
****************************************************************************************************************/
int iteration(const unsigned char *data, int w, int h, fit_t * results)
{
fprintf(fitDebug, "IMMAGINE: %dx%d \nSTARTING STRUCT: \n", w, h);
printFit(fitDebug, results);
fflush(fitDebug);
int npixels = w * h;
double *diff = new double[npixels];
double min = 255;
double max = -255;
int temp;
int x, y;
int index;
gsl_vector *delta = gsl_vector_alloc(8);
gsl_vector *vettore = gsl_vector_alloc(8);
fprintf(fitDebug, "Done most of it\n");
fflush(fitDebug);
/** variables used to keep track of the square error*/
double square;
int iteration = 0;
double squares[10];
for (int i = 0; i < 10; i++)
squares[i] = 0;
fprintf(fitDebug, "Starting with with algebra stuff..");
fflush(fitDebug);
int dimension = 8 * npixels;
fprintf(fitDebug, "dimension..");
fflush(fitDebug);
double *M = new double[dimension];
fprintf(fitDebug, "M..");
fflush(fitDebug);
double matrix[8 * 8];
fprintf(fitDebug, "matrix..");
fflush(fitDebug);
double vector[8];
fprintf(fitDebug, "Done with algebra stuff\n");
fflush(fitDebug);
double initial_error = 0;
double diff_x, diff_y;
double frac_x, frac_y, sig2x, sig2y, dexp;
int base;
double test;
fprintf(fitDebug, "Done with inizialization\n");
fflush(fitDebug);
/**ITERATION LOOP */
while (iteration < 1) {
/*
printf("LOOP %d\n",iteration+1);
fflush(stdout);
*/
/* fake inizialization to follow the weird MATLAB PATTERN
TOPLEFT-COLUMNWISE-STARTING DOWN */
int row = h;
int column = 1;
/** ITERATIVE PROCEDURE OVER THE IMAGE*/
fprintf(fitDebug, "Iteration: %d\n", iteration);
fflush(fitDebug);
for (int i = 0; i < npixels; i++) {
//WEIRD ! !!!I 'M USING THE ROWS AS OFFSET TO IMPLEMENT THE COLUMWISE FASHION
x = (i + 1) % h;
y = (i + 1) / h;
//WEIRD INDEX CALCULATION
int index = (row - 1) * w + (column - 1);
base = i * 8;
test = evaluateGaussian(results, x, y);
diff[i] = data[index] - test;
diff_x = x - results->x_0;
diff_y = y - results->y_0;
sig2x = pow(results->sigma_x, 2);
sig2y = pow(results->sigma_y, 2);
frac_x = pow(diff_x, 2) / sig2x;
frac_y = pow(diff_y, 2) / sig2y;
dexp = exp(frac_x + frac_y);
M[base] = 1 / dexp;
M[base + 1] = (results->A * (2 * x - 2 * results->x_0)) / (sig2x * dexp);
M[base + 2] = (results->A * (2 * y - 2 * results->y_0)) / (sig2y * dexp);
M[base + 3] = (2 * results->A * pow(diff_x, 2)) / (pow(results->sigma_x, 3) * dexp);
M[base + 4] = (2 * results->A * pow(diff_y, 2)) / (pow(results->sigma_y, 3) * dexp);
//derivative of the slopePlan !
M[base + 5] = x;
M[base + 6] = y;
M[base + 7] = 1.0;
//WEIRD CIRCULAR INCREMENT
row--;
if (row == 0) {
row = h;
column++;
}
}
square = 0.0;
/* square calculation and array adjustment */
for (int index1 = 0; index1 < npixels; index1++) {
square = square + pow(diff[index1], 2);
}
squares[iteration] = square;
iteration++;
/**initial error print*/
if (iteration == 1) {
initial_error = square;
fprintf(fitDebug, "ERRORE INIZIA: %09.0f\n", initial_error);
fflush(fitDebug);
}
fflush(stdout);
/**
FILE* fp = fopen("matrice.mat","w");
for (int index1 = 0;index1<500;index1++){
for (int index2=0;index2<8;index2++){
fprintf(fp,"%08f\t",M[index1*8+index2]);
}
fprintf(fp,"\n");
}
*/
gsl_matrix_view gsl_M = gsl_matrix_view_array(M, npixels, 8);
gsl_matrix_view matrice = gsl_matrix_view_array(matrix, 8, 8);
gsl_vector_view differenze = gsl_vector_view_array(diff, npixels);
//printf("\nMM\n");
/* Compute matrix = M'*M */
gsl_blas_dgemm(CblasTrans, CblasNoTrans, 1.0, &gsl_M.matrix, &gsl_M.matrix, 0.0, &matrice.matrix);
/**
for (int index1 = 0;index1<8;index1++){
for (int index2 =0;index2<8;index2++){
// printf("%8f\t",matrix[index1*8 + index2]);
}
// printf("\n");
}
*/
/** Compute matrix = M'*M
printf("MM2\n");
for (int index1 = 0;index1<8;index1++){
for(int index2 = 0;index2<8;index2++){
matrix[index1*8 + index2] = 0;
for (int index3 = 0;index3<npixels;index3++){
matrix[index1*8 + index2] = matrix[index1*8 + index2] + M[index3*8+index1]*M[index3*8+index2];
}
}
}
for (int index1 = 0;index1<8;index1++){
for (int index2 =0;index2<8;index2++){
printf("%08.2f\t",matrix[index1*8 + index2]);
}
printf("\n");
}
*/
//printf("\nVECTOR\n");
/* Compute vector = M'*diff for (int i =8; i<16 ;i++){ for (int
k=0;k<npixels;k++){ vector[i] = vector[i] + M[k*8 + i]*diff[k];
} } for (int index1 = 0;index1<8;index1++){
printf("%08.2f\t",vector[index1]); } printf("\n"); */
/* Compute vector = M'*diff */
gsl_blas_dgemv(CblasTrans, 1.0, &gsl_M.matrix, &differenze.vector, 0.0, vettore);
//gsl_vector_fprintf(stdout, vettore, "%g");
//printf("\n");
/* Compute the delta vector of deviation */
int s;
gsl_permutation *p = gsl_permutation_alloc(8);
gsl_linalg_LU_decomp(&matrice.matrix, p, &s);
gsl_linalg_LU_solve(&matrice.matrix, p, vettore, delta);
//printf("delta = \n");
//gsl_vector_fprintf(stdout, delta, "%g");
/** result adjustment */
results->A = results->A + gsl_vector_get(delta, 0);
//printf("New A is: %f\n", results->A);
//ADDING
results->x_0 = results->x_0 + gsl_vector_get(delta, 1);
results->y_0 = results->y_0 + gsl_vector_get(delta, 2);
results->sigma_x = results->sigma_x + gsl_vector_get(delta, 3);
results->sigma_y = results->sigma_y + gsl_vector_get(delta, 4);
results->a = results->a + gsl_vector_get(delta, 5);
results->b = results->b + gsl_vector_get(delta, 6);
results->c = results->c + +gsl_vector_get(delta, 7);
//RIPROVA ! !!
//printf("New x_0 is %f\n", results->x_0);
}
#if DEBUG
fprintf(fitDebug, "ERRORE FINALE: %09.0f\n", square);
fprintf(fitDebug, "STRUCT FINALE: \n");
printFit(fitDebug, results);
fflush(fitDebug);
#endif
//printResults(results);
/* FREE!!!!!!!! */
if (M)
delete M;
if (diff)
delete diff;
gsl_vector_free(vettore);
gsl_vector_free(delta);
if (square > initial_error) {
return 0;
} else {
return 1;
}
}
/**
* C++ version of gsl_linalg_LU_solve().
* @param LU An LU decomposition matrix
* @param p A permutation
* @param b A vector
* @param x A vector
* @return Error code on failure
*/
inline int LU_solve( matrix const& LU, permutation const& p, vector const& b, vector& x ){
return gsl_linalg_LU_solve( LU.get(), p.get(), b.get(), x.get() ); }
/*....................................................................*/
intersectType
intersectLineWithFace(const int numDims, double *x, double *dir, faceType *face, const double epsilon){
/*
This function calculates the intersection between a line and the face of a simplex oriented in that space. Obviously the number of dimensions of the space must be >=2. The intersection point may be expressed as
px_ = x_ + a*dir_
where px_, x_ and dir_ are vectors and 'a' is a scalar. The routine returns the following information:
- The value of 'a'.
- The so-called barycentric coordinates (BC) of px_ in the face.
The scalar 'a' is found as follows. We need the additional point y_ which can be any of the face vertices. We state the vector identity
a*dir_ = (y_ - x_) + (px_ - y_).
Clearly (px_ - y_) is parallel to the face. If we take the scalar product of both sides with the vector n_ which is normal to the face we arrive at
a*dir_.n_ = (y_ - x_).n_ + (px_ - y_).n_.
= (y_ - x_).n_ + 0.
Thus
(y_ - x_).n_
a = --------------.
dir_.n_
Notes:
* This routine works best when the sides of the face are not too disparate in size.
* There is, of course, no guarantee that the line actually intersects the face, even if the line and the face are non-parallel. There are also borderline cases, i.e. where the line passes close to a vertex, in which an exact calculation would show that the intersection occurs (or doesn't occur), but the imprecise computed value claims that it doesn't (or does). Intersection may be judged via the values of the barycentric coordinates (BC): if the BC all fall in the interval [0,1], the line intersects the face; if one of the BC is negative, it doesn't.
*/
const double oneOnEpsilon=1.0/epsilon;
double vs[numDims-1][numDims],norm[numDims],normDotDx, numerator, pxInFace[numDims-1], tMat[numDims-1][numDims-1], bVec[numDims-1], det;
int i,j,k,di,ci,ri,vi,ciOfMax,ciOfMin;
double testSumForCW=0.0,maxSingularValue,singularValue;
facePlusBasisType facePlusBasis;
char errStr[80];
intersectType intcpt;
for(vi=0;vi<numDims-1;vi++){
for(di=0;di<numDims;di++)
vs[vi][di] = (*face).r[vi+1][di]-(*face).r[0][di];
}
/* First calculate a normal vector to the face (note that it doesn't need to be of length==1).
*/
if(numDims==2){
norm[0] = -vs[0][1];
norm[1] = vs[0][0];
}else if(numDims==3){
/* Calculate norm via cross product. */
for(di=0;di<numDims;di++){
j = (di+1)%numDims;
k = (di+2)%numDims;
norm[di] = vs[0][j]*vs[1][k] - vs[0][k]*vs[1][j];
}
}else{ /* Assume numDims>3 */
/* Calculate norm via SVD. */
int status=0;
gsl_matrix *matrix = gsl_matrix_alloc(numDims-1, numDims);
gsl_matrix *svv = gsl_matrix_alloc(numDims, numDims);
gsl_vector *svs = gsl_vector_alloc(numDims);
gsl_vector *work = gsl_vector_alloc(numDims);
for(ci=0;ci<numDims;ci++){
for(ri=0;ri<numDims-1;ri++)
gsl_matrix_set(matrix, ri, ci, vs[ri][ci]);
}
status = gsl_linalg_SV_decomp(matrix, svv, svs, work);
if(status){
sprintf(errStr, "SVD decomposition failed (GSL error %d).", status);
error(RTC_ERR_SVD_FAIL, errStr);
}
/*
Since we have N-1 equations in N unknowns, we would expect at least one of the N elements of svs to be zero (within rounding error). The column of svv which corresponds to this value should then be the normal vector which we require. We'll just check however that not more than 1 value of svs is zero.
The GSL doco says that SV_decomp returns sorted svs values, but I prefer not to rely on that.
*/
ci = 0;
ciOfMax = ci;
maxSingularValue = gsl_vector_get(svs,ci);
for(ci=1;ci<numDims;ci++){
singularValue = gsl_vector_get(svs,ci);
if(singularValue>maxSingularValue){
ciOfMax = ci;
maxSingularValue = singularValue;
}
}
ciOfMin = -1; /* Impossible default. */
for(ci=0;ci<numDims;ci++){
if(ci==ciOfMax) continue;
singularValue = gsl_vector_get(svs,ci);
if(singularValue*oneOnEpsilon<maxSingularValue){
if(ciOfMin>=0){
/* This is an error because it indicates that >1 singular values are 'small'. */
sprintf(errStr, "Simplex face does not span an N-1 subspace.");
error(RTC_ERR_NON_SPAN, errStr);
}
ciOfMin = ci;
}
}
for(di=0;di<numDims;di++)
norm[di] = gsl_matrix_get(svv,di,ciOfMin);
gsl_vector_free(work);
gsl_vector_free(svs);
gsl_matrix_free(svv);
gsl_matrix_free(matrix);
}
/* Since we don't know a priori whether the vertices of the face are listed CW or ACW seen from inside the simplex, we will work this out by dotting the normal vector with a vector from the centre of the simplex to vertex 0 of the face. (A simplex is always convex so this ought to work.)
*/
testSumForCW = 0.0;
for(di=0;di<numDims;di++)
testSumForCW += norm[di]*((*face).r[0][di] - (*face).simplexCentre[di]);
if(testSumForCW<0.0){
for(di=0;di<numDims;di++)
norm[di] *= -1.0;
}
/* Calculate the scalar (or dot) product between norm and dir.
*/
normDotDx = 0.0;
for(di=0;di<numDims;di++)
normDotDx += norm[di]*dir[di];
if(normDotDx>0.0){ /* it is an exit face. */
intcpt.orientation = 1;
}else if(normDotDx<0.0){ /* it is an entry face. */
intcpt.orientation = -1;
}else{ /* normDotDx==0.0, i.e. line and face are parallel. */
intcpt.orientation = 0;
for(di=0;di<numDims;di++)
intcpt.bary[di] = 0.0;
intcpt.dist = 0.0;
intcpt.collPar = 0.0;
return intcpt;
}
/* If we've got to here, we can be sure the line and the face are not parallel, and that we therefore expect meaningful results for the calculation of 'a'.
*/
numerator = 0.0;
for(di=0;di<numDims;di++)
numerator += norm[di]*((*face).r[0][di] - x[di]); /* n_.(y_ - x_) */
intcpt.dist = numerator/normDotDx;
/* In order to calculate the barycentric coordinates, we need to set up a N-1 coordinate basis in the plane of the face.
*/
facePlusBasis = calcFaceInNMinus1(numDims, numDims, face);
/* Now we want to express the intersection point in these coordinates:
*/
for(i=0;i<numDims-1;i++){
pxInFace[i] = 0.0;
for(di=0;di<numDims;di++)
pxInFace[i] += (x[di] + intcpt.dist*dir[di] - facePlusBasis.origin[di])*facePlusBasis.axes[i][di];
}
/*
The barycentric coordinates x_ = {L_1,L_2,...,L_{N-1}} are given by
T x_ = b_
where T is an (N-1)*(N-1) matrix with entries
T_{i,j} = facePlusBasis.r[j+1][i] - facePlusBasis.r[0][i]
and
b_i = pxInFace[i] - facePlusBasis.r[0][i].
The final BC L_0 is given by
_N-1
\
L_0 = 1 - > L_i.
/_i=1
*/
if(numDims==2 || numDims==3){
for(i=0;i<numDims-1;i++){
for(j=0;j<numDims-1;j++)
tMat[i][j] = facePlusBasis.r[j+1][i] - facePlusBasis.r[0][i];
bVec[i] = pxInFace[i] - facePlusBasis.r[0][i];
}
if(numDims==2)
intcpt.bary[1] = bVec[0]/tMat[0][0];
else{ /* numDims==3 */
det = tMat[0][0]*tMat[1][1] - tMat[0][1]*tMat[1][0];
/*** We're assuming that the triangle is not pathological, i.e that det!=0. */
intcpt.bary[1] = ( tMat[1][1]*bVec[0] - tMat[0][1]*bVec[1])/det;
intcpt.bary[2] = (-tMat[1][0]*bVec[0] + tMat[0][0]*bVec[1])/det;
}
}else{ /* Assume numDims>3 */
int dummySignum,status=0;
gsl_matrix *gslT = gsl_matrix_alloc(numDims-1, numDims-1);
gsl_vector *gsl_x = gsl_vector_alloc(numDims-1);
gsl_vector *gsl_b = gsl_vector_alloc(numDims-1);
gsl_permutation *p = gsl_permutation_alloc(numDims-1);
for(i=0;i<numDims-1;i++){
for(j=0;j<numDims-1;j++)
gsl_matrix_set(gslT, i, j, facePlusBasis.r[j+1][i] - facePlusBasis.r[0][i]);
gsl_vector_set(gsl_b, i, pxInFace[i] - facePlusBasis.r[0][i]);
}
status = gsl_linalg_LU_decomp(gslT,p,&dummySignum);
if(status){
sprintf(errStr, "LU decomposition failed (GSL error %d).", status);
error(RTC_ERR_LU_DECOMP_FAIL, errStr);
}
status = gsl_linalg_LU_solve(gslT,p,gsl_b,gsl_x);
if(status){
sprintf(errStr, "LU solver failed (GSL error %d).", status);
error(RTC_ERR_LU_SOLVE_FAIL, errStr);
}
for(i=0;i<numDims-1;i++)
intcpt.bary[i+1] = gsl_vector_get(gsl_x,i);
gsl_permutation_free(p);
gsl_vector_free(gsl_b);
gsl_vector_free(gsl_x);
gsl_matrix_free(gslT);
}
intcpt.bary[0] = 1.0;
for(i=1;i<numDims;i++)
intcpt.bary[0] -= intcpt.bary[i];
/* Finally, calculate the 'collision parameter':
*/
di = 0;
if(intcpt.bary[di] < 0.5)
intcpt.collPar = intcpt.bary[di];
else
intcpt.collPar = 1.0 - intcpt.bary[di];
for(di=1;di<numDims;di++){
if(intcpt.bary[di] < 0.5){
if(intcpt.bary[di] < intcpt.collPar)
intcpt.collPar = intcpt.bary[di];
}else{ /* intcpt.bary[di]>=0.5 */
if(1.0 - intcpt.bary[di] < intcpt.collPar)
intcpt.collPar = 1.0 - intcpt.bary[di];
}
}
return intcpt;
}
/* solve system with given lambda and L = diag(L) and test against
* normal equations solution */
static void
test_reg3(const double lambda, const gsl_vector * L, const gsl_matrix * X,
const gsl_vector * y, const gsl_vector * wts, const double tol,
gsl_multifit_linear_workspace * w, const char * desc)
{
const size_t n = X->size1;
const size_t p = X->size2;
double rnorm0, snorm0;
double rnorm1, snorm1;
gsl_vector *c0 = gsl_vector_alloc(p);
gsl_vector *c1 = gsl_vector_alloc(p);
gsl_matrix *XTX = gsl_matrix_alloc(p, p); /* X^T W X + lambda^2 L^T L */
gsl_vector *XTy = gsl_vector_alloc(p); /* X^T W y */
gsl_matrix *Xs = gsl_matrix_alloc(n, p); /* standard form X~ */
gsl_vector *ys = gsl_vector_alloc(n); /* standard form y~ */
gsl_vector *Lc = gsl_vector_alloc(p);
gsl_vector *r = gsl_vector_alloc(n);
gsl_permutation *perm = gsl_permutation_alloc(p);
int signum;
size_t j;
/* compute Xs = sqrt(W) X, ys = sqrt(W) y */
gsl_multifit_linear_wstdform1(NULL, X, wts, y, Xs, ys, w);
/* construct XTy = X^T W y */
gsl_blas_dgemv(CblasTrans, 1.0, Xs, ys, 0.0, XTy);
/* construct XTX = X^T W X + lambda^2 L^T L */
gsl_blas_dgemm(CblasTrans, CblasNoTrans, 1.0, Xs, Xs, 0.0, XTX);
for (j = 0; j < p; ++j)
{
double lj = gsl_vector_get(L, j);
*gsl_matrix_ptr(XTX, j, j) += pow(lambda * lj, 2.0);
}
/* solve XTX c = XTy with LU decomp */
gsl_linalg_LU_decomp(XTX, perm, &signum);
gsl_linalg_LU_solve(XTX, perm, XTy, c0);
/* solve with reg routine */
gsl_multifit_linear_wstdform1(L, X, wts, y, Xs, ys, w);
gsl_multifit_linear_svd(Xs, w);
gsl_multifit_linear_solve(lambda, Xs, ys, c1, &rnorm0, &snorm0, w);
gsl_multifit_linear_genform1(L, c1, c1, w);
/* test snorm = ||L c1|| */
gsl_vector_memcpy(Lc, c1);
gsl_vector_mul(Lc, L);
snorm1 = gsl_blas_dnrm2(Lc);
gsl_test_rel(snorm0, snorm1, tol, "test_reg3: %s, snorm lambda=%g n=%zu p=%zu",
desc, lambda, n, p);
/* test rnorm = ||y - X c1||, compute again Xs = sqrt(W) X and ys = sqrt(W) y */
gsl_multifit_linear_wstdform1(NULL, X, wts, y, Xs, ys, w);
gsl_vector_memcpy(r, ys);
gsl_blas_dgemv(CblasNoTrans, -1.0, Xs, c1, 1.0, r);
rnorm1 = gsl_blas_dnrm2(r);
gsl_test_rel(rnorm0, rnorm1, tol, "test_reg3: %s, rnorm lambda=%g n=%zu p=%zu",
desc, lambda, n, p);
/* test c0 = c1 */
for (j = 0; j < p; ++j)
{
double c0j = gsl_vector_get(c0, j);
double c1j = gsl_vector_get(c1, j);
gsl_test_rel(c1j, c0j, tol, "test_reg3: %s, c0/c1 j=%zu lambda=%g n=%zu p=%zu",
desc, j, lambda, n, p);
}
gsl_matrix_free(Xs);
gsl_matrix_free(XTX);
gsl_vector_free(XTy);
gsl_vector_free(c0);
gsl_vector_free(c1);
gsl_vector_free(Lc);
gsl_vector_free(ys);
gsl_vector_free(r);
gsl_permutation_free(perm);
}
/** From a vector-field dataset, compute the vector-valued weighting factors,
* \f$\vec{c}_j\f$. Info is returned in the rbf structure.
*
*
* \param[in] v - pointer to an array of position vectors.
* \param[in] B - pointer to array of corresponding field vectors.
* \param[in] n - number of (v, B) pairs defined.
* \param[in] eps - smoothing factor in scalar RBF.
* \param[in] RadialBasisFunction - RBF to use. Can be LGM_RBF_GAUSSIAN, LGM_RBF_MULTIQUADRIC
*
* \return pointer to structure containing info for RBF interpolation. User
* is responsible for freeing with Lgm_DFI_RBF_Free().
*
* \author M. G. Henderson
* date January 24, 2012
*
*
*/
Lgm_DFI_RBF_Info *Lgm_DFI_RBF_Init( unsigned long int *I_data, Lgm_Vector *v, Lgm_Vector *B, int n, double eps, int RadialBasisFunction ) {
int i, j, ii, jj, p, q, n3, s;
double *d, **a, Phi[3][3], val;
gsl_matrix *A, *V;
gsl_vector *D, *c, *S, *Work;
Lgm_DFI_RBF_Info *rbf;
n3 = 3*n;
A = gsl_matrix_calloc( n3, n3 );
c = gsl_vector_alloc( n3 );
D = gsl_vector_calloc( n3 );
/*
* Save info needed to do an evaluation.
*/
rbf = ( Lgm_DFI_RBF_Info *)calloc( 1, sizeof(*rbf) );
rbf->RadialBasisFunction = RadialBasisFunction;
rbf->eps = eps;
rbf->n = n;
rbf->n3 = n3;
LGM_ARRAY_1D( rbf->LookUpKey, n, unsigned long int);
LGM_ARRAY_1D( rbf->v, n, Lgm_Vector);
LGM_ARRAY_1D( rbf->c, n, Lgm_Vector);
for ( i=0; i<n; i++ ) {
rbf->LookUpKey[i] = I_data[i];
rbf->v[i] = v[i];
}
// This subtraction doesntm seem to work out very well...
// rbf->Bx0 = B[0].x;
// rbf->By0 = B[0].y;
// rbf->Bz0 = B[0].z;
double Bbkg;
for ( Bbkg = 0.0, i=0; i<n; i++ ) Bbkg += B[i].x; rbf->Bx0 = Bbkg/(double)n;
for ( Bbkg = 0.0, i=0; i<n; i++ ) Bbkg += B[i].y; rbf->By0 = Bbkg/(double)n;
for ( Bbkg = 0.0, i=0; i<n; i++ ) Bbkg += B[i].z; rbf->Bz0 = Bbkg/(double)n;
rbf->Bx0 = 0.0;
rbf->By0 = 0.0;
rbf->Bz0 = 0.0;
/*
* Fill d array. (Subtract off the field at the nearest point v[0] -- See
* McNally [2011].) We add this field back on later.
*/
for (i=0; i<n; i++){
gsl_vector_set( D, 3*i+0, B[i].x - rbf->Bx0 );
gsl_vector_set( D, 3*i+1, B[i].y - rbf->By0 );
gsl_vector_set( D, 3*i+2, B[i].z - rbf->Bz0 );
}
/*
* [ row0 ]
* Fill A matrix. In C, order is A[row][col] = [ row1 ]
* [ row2 ]
*/
for ( i=0; i<n; i++ ) { // locate start row for subarray
ii = 3*i;
for ( j=0; j<n; j++ ) { // locate start column for subarray
jj = 3*j;
// Get Phi( v_i - v_j )
Lgm_DFI_RBF_Phi( &v[i], &v[j], Phi, rbf );
for ( p=0; p<3; p++ ){ // subarray row
for ( q=0; q<3; q++ ){ // subarray column
gsl_matrix_set( A, ii+p, jj+q, Phi[p][q] );
}
}
}
}
/*
for (i=0; i<n; i++ ) {
printf("v%02d = %8g %8g %8g B%02d = %8g %8g %8g\n", i, v[i].x, v[i].y, v[i].z, i, B[i].x, B[i].y, B[i].z );
}
for (i=0; i<n3; i++){
for (j=0; j<n3; j++){
printf("%8g ", gsl_matrix_get(A, i, j ) );
}
printf("\n");
}
*/
/*
* Now we need to solve the system of equation;
*
* d = ac
*
* for c.
*
* First create gsl_vector and gsl_matrix views of the d and A arrays.
* Then compute Cholesky decomposition of the a array. Then solve the
* system to get c.
*
*/
if ( LGM_DFI_RBF_SOLVER == LGM_CHOLESKY_DECOMP ){
gsl_linalg_cholesky_decomp( A );
gsl_linalg_cholesky_solve( A, D, c );
} else if ( LGM_DFI_RBF_SOLVER == LGM_PLU_DECOMP ){
gsl_permutation *P = gsl_permutation_alloc( n3 );
gsl_linalg_LU_decomp( A, P, &s );
gsl_linalg_LU_solve( A, P, D, c );
gsl_permutation_free( P );
} else if ( LGM_DFI_RBF_SOLVER == LGM_SVD ){
V = gsl_matrix_calloc( n3, n3 );
S = gsl_vector_alloc( n3 );
Work = gsl_vector_alloc( n3 );
gsl_linalg_SV_decomp( A, V, S, Work );
gsl_linalg_SV_solve( A, V, S, D, c );
gsl_vector_free( Work );
gsl_vector_free( S );
gsl_matrix_free( V );
}
for (i=0; i<n; i++){
rbf->c[i].x = gsl_vector_get( c, 3*i+0 );
rbf->c[i].y = gsl_vector_get( c, 3*i+1 );
rbf->c[i].z = gsl_vector_get( c, 3*i+2 );
}
gsl_vector_free( D );
gsl_vector_free( c );
gsl_matrix_free( A );
return( rbf );
}
/* solve system with given lambda and L and test against
* normal equations solution */
static void
test_reg4(const double lambda, const gsl_matrix * L, const gsl_matrix * X,
const gsl_vector * y, const gsl_vector * wts, const double tol,
gsl_multifit_linear_workspace * w, const char *desc)
{
const size_t m = L->size1;
const size_t n = X->size1;
const size_t p = X->size2;
double rnorm0, snorm0;
double rnorm1, snorm1;
gsl_vector *c0 = gsl_vector_alloc(p);
gsl_vector *c1 = gsl_vector_alloc(p);
gsl_matrix *LTL = gsl_matrix_alloc(p, p); /* L^T L */
gsl_matrix *XTX = gsl_matrix_alloc(p, p); /* X^T W X + lambda^2 L^T L */
gsl_vector *XTy = gsl_vector_alloc(p); /* X^T W y */
gsl_permutation *perm = gsl_permutation_alloc(p);
gsl_matrix *Xs = (m < p) ? gsl_matrix_alloc(n - (p - m), m) : gsl_matrix_alloc(n, p);
gsl_vector *ys = (m < p) ? gsl_vector_alloc(n - (p - m)) : gsl_vector_alloc(n);
gsl_matrix *M = (m < p) ? gsl_matrix_alloc(n, p) : gsl_matrix_alloc(m, p);
gsl_vector *cs = (m < p) ? gsl_vector_alloc(m) : gsl_vector_alloc(p);
gsl_matrix *WX = gsl_matrix_alloc(n, p);
gsl_vector *Wy = gsl_vector_alloc(n);
gsl_vector *Lc = gsl_vector_alloc(m);
gsl_vector *r = gsl_vector_alloc(n);
gsl_matrix *LQR = gsl_matrix_alloc(m, p);
gsl_vector *Ltau = gsl_vector_alloc(GSL_MIN(m, p));
int signum;
size_t j;
/* compute WX = sqrt(W) X, Wy = sqrt(W) y */
gsl_multifit_linear_wstdform1(NULL, X, wts, y, WX, Wy, w);
/* construct XTy = X^T W y */
gsl_blas_dgemv(CblasTrans, 1.0, WX, Wy, 0.0, XTy);
/* construct XTX = X^T W X + lambda^2 L^T L */
gsl_blas_dgemm(CblasTrans, CblasNoTrans, 1.0, L, L, 0.0, LTL);
gsl_matrix_scale(LTL, lambda * lambda);
gsl_blas_dgemm(CblasTrans, CblasNoTrans, 1.0, WX, WX, 0.0, XTX);
gsl_matrix_add(XTX, LTL);
/* solve XTX c = XTy with LU decomp */
gsl_linalg_LU_decomp(XTX, perm, &signum);
gsl_linalg_LU_solve(XTX, perm, XTy, c0);
/* solve with reg routine */
gsl_matrix_memcpy(LQR, L);
gsl_multifit_linear_L_decomp(LQR, Ltau);
gsl_multifit_linear_wstdform2(LQR, Ltau, X, wts, y, Xs, ys, M, w);
gsl_multifit_linear_svd(Xs, w);
gsl_multifit_linear_solve(lambda, Xs, ys, cs, &rnorm0, &snorm0, w);
gsl_multifit_linear_wgenform2(LQR, Ltau, X, wts, y, cs, M, c1, w);
/* test snorm = ||L c1|| */
gsl_blas_dgemv(CblasNoTrans, 1.0, L, c1, 0.0, Lc);
snorm1 = gsl_blas_dnrm2(Lc);
gsl_test_rel(snorm0, snorm1, tol, "test_reg4: %s snorm lambda=%g", desc, lambda);
/* test rnorm = ||y - X c1||_W */
gsl_vector_memcpy(r, Wy);
gsl_blas_dgemv(CblasNoTrans, -1.0, WX, c1, 1.0, r);
rnorm1 = gsl_blas_dnrm2(r);
gsl_test_rel(rnorm0, rnorm1, tol, "test_reg4: %s rnorm lambda=%g", desc, lambda);
/* test c0 = c1 */
for (j = 0; j < p; ++j)
{
double c0j = gsl_vector_get(c0, j);
double c1j = gsl_vector_get(c1, j);
gsl_test_rel(c1j, c0j, tol, "test_reg4: %s lambda=%g n=%zu p=%zu j=%zu",
desc, lambda, n, p, j);
}
gsl_matrix_free(LTL);
gsl_matrix_free(XTX);
gsl_vector_free(XTy);
gsl_vector_free(c0);
gsl_vector_free(c1);
gsl_permutation_free(perm);
gsl_matrix_free(Xs);
gsl_vector_free(ys);
gsl_vector_free(cs);
gsl_matrix_free(M);
gsl_vector_free(Lc);
gsl_matrix_free(WX);
gsl_vector_free(Wy);
gsl_vector_free(r);
gsl_matrix_free(LQR);
gsl_vector_free(Ltau);
}
void chapeau_update_peaks ( chapeau * ch ) {
int i,j,J,I;
int s;
double ninv;
double lo,lb,alpha;
gsl_matrix * Abar;
gsl_vector * bbar;
gsl_vector * lambar;
gsl_permutation * p;
int nred;
//DB//for (i=0;i<ch->m;i++) fprintf(stderr,"AAA %i %i\n",i,ch->hits[i]); exit(1);
//// parameters that are allowed to evolve lie between indices for which
//// ch->hits[] is non-zero so extract the proper subspace
nred=0;
for (i=0;i<ch->m;i++) if (ch->hits[i]) nred++;
// If the nred is small, singular matrix can occur
//fprintf(stderr,"Warning: No chapeau update: only %i non cero elements\n",nred);
if (nred<10) return;
Abar=gsl_matrix_alloc(nred,nred);
bbar=gsl_vector_alloc(nred);
lambar=gsl_vector_alloc(nred);
p=gsl_permutation_alloc(nred);
// Add the new and old statistic to the reduced matrix
I=0;
for (i=0;i<ch->m;i++) {
if (!ch->hits[i]) continue;
lb=gsl_vector_get(ch->b,i)+gsl_vector_get(ch->bfull,i);
gsl_vector_set(bbar,I,-lb); //TODO: Trace back the origin of the minus. See fes1D procedure.
J=0;
for (j=0;j<ch->m;j++) {
if (!ch->hits[j]) continue;
lb=gsl_matrix_get(ch->A,i,j)+gsl_matrix_get(ch->Afull,i,j);
gsl_matrix_set(Abar,I,J,lb);
J++;
}
I++;
}
//XXX: What is alpha?
alpha=0.0;//1.0-exp(-1.0e4/nsamples);
gsl_linalg_LU_decomp(Abar,p,&s);
gsl_linalg_LU_solve(Abar,p,bbar,lambar);
// update the vector of coefficients
I=0;
for (i=0;i<ch->m;i++) {
if (!ch->hits[i]) continue;
lb=gsl_vector_get(lambar,I);
if (lb!=lb) {fprintf(stderr,"PARANOIA) Tripped at 3333. Too many chapeau additions?\n");exit(-1);}
gsl_vector_set(ch->lam,i,lb);
//XXX: What is alpha?
//lo=gsl_vector_get(ch->lam,i);
//gsl_vector_set(ch->lam,i,alpha*lo+(1-alpha)*lb);
I++;
}
gsl_matrix_free(Abar);
gsl_vector_free(bbar);
gsl_vector_free(lambar);
gsl_permutation_free(p);
}
