float
CRebuildGraph::compareMatrix(gsl_matrix* matrixA, gsl_matrix*matrixB){
float delta;
gsl_vector *work ,*s;
if (matrixA->size1 != matrixB->size1)
throw runtime_error(" size 1 and size 2 are different");
gsl_matrix *U1, *U2,*V1,*V2;
InitMatrix((int)matrixA->size1,&work,&s,&U1,&U2,&V1,&V2);
gsl_matrix_memcpy (U1, matrixA);
//gsl_linalg_SV_decomp (gsl_matrix * A, gsl_matrix * V, gsl_vector * S, gsl_vector * work)
//La matriu A es substitueix per U a la sortida
gsl_linalg_SV_decomp(U1,V1,s,work);
gsl_matrix_memcpy (U2, matrixB);
gsl_linalg_SV_decomp(U2,V2,s,work);
//F = U1 VS2 V1^T = U1 U2^T A2 V2 V1^T
gsl_matrix *F=gsl_matrix_alloc(matrixA->size1,matrixA->size1);
gsl_matrix_transpose(U2);
multiplica(F,U1,U2);
multiplica(F,F,matrixB);
multiplica(F,F,V2);
gsl_matrix_transpose(V1);
multiplica(F,F,V1);
//F ja esta calculada. Calculem la norma.
delta=0;
for(int i=0; i<matrixA->size1; i++){
for(int j=0; j<matrixA->size1; j++){
delta+=pow(gsl_matrix_get(matrixA,i,j)-gsl_matrix_get(F,i,j),2);
}
}
delta=std::pow(delta,0.5f);
delta/=matrixA->size1;
printingCompareMatrixResults(delta,F,matrixA);
FreeMatrix(&work,
&s,
&U1,
&U2,
&V1,
&V2,
&F );
return delta;
}
gsl_matrix * SgFilter::pseudoInverse(gsl_matrix *A, int n_row, int n_col){
gsl_matrix * A_t = gsl_matrix_alloc (n_col, n_row); //A_t is transpose
gsl_matrix_transpose_memcpy (A_t, A);
gsl_matrix * U = gsl_matrix_alloc (n_col, n_row);
gsl_matrix * V= gsl_matrix_alloc (n_row, n_row);
gsl_vector * S = gsl_vector_alloc (n_row);
// Computing the SVD of the transpose of A
gsl_vector * work = gsl_vector_alloc (n_row);
gsl_linalg_SV_decomp (A_t, V, S, work);
gsl_vector_free(work);
gsl_matrix_memcpy (U, A_t);
//Inverting S
gsl_matrix * Sp = gsl_matrix_alloc (n_row, n_row);
gsl_matrix_set_zero (Sp);
for (int i = 0; i < n_row; i++)
gsl_matrix_set (Sp, i, i, gsl_vector_get(S, i)); // Vector 'S' to matrix 'Sp'
gsl_permutation * p = gsl_permutation_alloc (n_row);
int signum;
gsl_linalg_LU_decomp (Sp, p, &signum); // Computing the LU decomposition
// Compute the inverse
gsl_matrix * SI = gsl_matrix_calloc (n_row, n_row);
for (int i = 0; i < n_row; i++) {
if (gsl_vector_get (S, i) > 0.0000000001)
gsl_matrix_set (SI, i, i, 1.0 / gsl_vector_get (S, i));
}
gsl_matrix * VT = gsl_matrix_alloc (n_row, n_row);
gsl_matrix_transpose_memcpy (VT, V); // Tranpose of V
//THE PSEUDOINVERSE
//Computation of the pseudoinverse of trans(A) as pinv(A) = U·inv(S).trans(V) with trans(A) = U.S.trans(V)
gsl_matrix * SIpVT = gsl_matrix_alloc (n_row, n_row);
gsl_blas_dgemm (CblasNoTrans, CblasNoTrans, // Calculating inv(S).trans(V)
1.0, SI, VT,
0.0, SIpVT);
gsl_matrix * pinv = gsl_matrix_alloc (n_col, n_row); // Calculating U·inv(S).trans(V)
gsl_blas_dgemm (CblasNoTrans, CblasNoTrans,
1.0, U, SIpVT,
0.0, pinv);
gsl_matrix_free(VT);
gsl_matrix_free(SI);
gsl_matrix_free(SIpVT);
gsl_matrix_free(A_t);
gsl_matrix_free(U);
gsl_matrix_free(A);
gsl_matrix_free(V);
gsl_vector_free(S);
return pinv;
}
int wrap_gsl_linalg_SV_decomp(gsl_matrix* A, gsl_matrix* V, gsl_matrix* S,
gsl_matrix* work)
{
gsl_vector_view _S = gsl_matrix_diagonal(S);
gsl_vector_view _work = gsl_matrix_row(work, 0);
return gsl_linalg_SV_decomp(A, V, &_S.vector, &_work.vector);
}
int svd3(double A[3][3], double V[3][3], double s[3]) {
gsl_matrix *A_gsl = gsl_matrix_alloc(3,3);
for (unsigned int i = 0; i < 3; i++) {
for (unsigned int j = 0; j < 3; j++)
gsl_matrix_set(A_gsl, i, j, A[i][j]);
}
gsl_matrix *V_gsl = gsl_matrix_alloc(3,3);
gsl_vector *s_gsl = gsl_vector_alloc(3);
gsl_vector *work = gsl_vector_alloc(3);
int ret_val = gsl_linalg_SV_decomp(A_gsl, V_gsl, s_gsl, work);
for (unsigned int i = 0; i < 3; i++) {
s[i] = gsl_vector_get(s_gsl, i);
for (unsigned int j = 0; j < 3; j++) {
V[i][j] = gsl_matrix_get(V_gsl, i, j);
A[i][j] = gsl_matrix_get(A_gsl, i, j);
}
}
gsl_matrix_free(A_gsl);
gsl_matrix_free(V_gsl);
gsl_vector_free(s_gsl);
gsl_vector_free(work);
return ret_val;
}
// if A is M x N, then A_ps is NxM
void mygsl_linalg_pseudoinverse(const gsl_matrix * A, gsl_matrix * A_ps)
{
size_t M = A->size1, N = A->size2;
// A = U D V' where U is MxN and V is NxN
gsl_matrix * U = gsl_matrix_alloc(M, N), * V = gsl_matrix_alloc(N, N);
gsl_vector * D_diag = gsl_vector_alloc(N),
* work = gsl_vector_alloc(N);
gsl_matrix_memcpy(U, A);
gsl_linalg_SV_decomp(U, V, D_diag, work);
// V D^(-1)
gsl_matrix * VDinv = gsl_matrix_alloc(N, N);
mygsl_vector_pow(D_diag, -1.0);
gsl_matrix * D_inv = mygsl_matrix_diagalloc(D_diag, 0.0);
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, V, D_inv, 0.0, VDinv);
// A_ps = VDinv U'
gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1.0, VDinv, U, 0.0, A_ps);
gsl_matrix_free(U);
gsl_matrix_free(V);
gsl_vector_free(D_diag);
gsl_vector_free(work);
gsl_matrix_free(VDinv);
gsl_matrix_free(D_inv);
}
int
linalg_svd (lua_State *L)
{
const gsl_matrix *a = matrix_check (L, 1);
int sm = a->size1, sn = a->size2;
gsl_matrix *u, *v, *s;
gsl_vector *s_vec, *work;
int k;
u = matrix_push_raw (L, sm, sn);
s = matrix_push_raw (L, sn, sn);
v = matrix_push_raw (L, sn, sn);
s_vec = gsl_vector_alloc (sn);
work = gsl_vector_alloc (sn);
gsl_matrix_memcpy (u, a);
gsl_linalg_SV_decomp (u, v, s_vec, work);
for (k = 0; k < sn; k++)
{
double z = gsl_vector_get (s_vec, k);
gsl_matrix_set (s, k, k, z);
}
gsl_vector_free (s_vec);
gsl_vector_free (work);
return 3;
}
gsl_matrix *pseudo_inverse(gsl_matrix* input) {
int m = input->size1;
int n = input->size2;
gsl_matrix *U = gsl_matrix_calloc(m,n);
gsl_matrix_memcpy(U, input);
gsl_matrix *V = gsl_matrix_calloc(n,n);
gsl_vector *sigma = gsl_vector_calloc(n);
gsl_vector *tmp = gsl_vector_calloc(n);
gsl_linalg_SV_decomp(U, V, sigma, tmp);
for (int i = 0; i < n; i++) {
double s = gsl_vector_get(sigma, i);
if (s > 0.0000000001) {
gsl_vector_set(sigma, i, 1/s);
} else if (s > 0) {
gsl_vector_set(sigma, i, 0);
}
}
gsl_matrix *tmpa = gsl_matrix_calloc(n,n);
gsl_matrix *tmpb = gsl_matrix_calloc(n,m);
gsl_matrix *tmpc = gsl_matrix_calloc(n,m);
for (int i = 0; i < n; i++) {
gsl_matrix_set(tmpa, i, i, gsl_vector_get(sigma, i));
}
gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1, tmpa, U, 0, tmpb);
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1, V, tmpb, 0, tmpc);
return tmpc;
}
int rank_double_tab(double** matrix_in, int x, int y)
{
int i,j,n,m;
int rank;
gsl_matrix * A;
gsl_matrix * V;
gsl_vector * S;
gsl_vector * work;
if(x>y)
{
m = x;
n = y;
}
else
{
m = y;
n = x;
}
A = gsl_matrix_alloc(m, n);
V = gsl_matrix_alloc(n, n);
S = gsl_vector_alloc(n);
work = gsl_vector_alloc(n);
for(i=0; i<x; i++)
{
for(j=0; j<y; j++)
{
if(x>y)
{
gsl_matrix_set(A, i, j, matrix_in[i][j]);
}
else
{
gsl_matrix_set(A, j, i, matrix_in[i][j]);
}
}
}
gsl_linalg_SV_decomp(A,V,S,work);
rank = 0;
for(i=0; i<n; i++)
{
if(fabs(gsl_vector_get(S,i))> 1e-30)
{
rank ++;
}
}
gsl_matrix_free(A);
gsl_matrix_free(V);
gsl_vector_free(S);
gsl_vector_free(work);
return rank;
}
int is_mirror_image(double* X, int X_dim0, int X_dim1, int X_dim1_mem,
double* Y, int Y_dim0, int Y_dim1, int Y_dim1_mem) {
// Check if two configurations X and Y are mirror images
// (i.e. does their optimal superposition involve a reflection?)
//
// Parameters
// ----------
// X : double*, shape=(X_dim0, X_dim1)
// Pointer to the upper left corner of matrix X.
// X_dim0 : int
// The number of rows in matrix X. Should be 3.
// X_dim1 : int
// The number of columns in matrix X. Corresponds to number of atoms
// X_dim1_mem : int
// number of columns of X in memory. corresponds to the number of padded atoms.
// such that the (i,j)-th element of X is accessed at X[i*X_dim1*mem + j]
// Y : double*, shape=(X_dim0, X_dim1)
// Pointer to the upper left corner of matrix X.
// Y_dim0 : int
// The number of rows in matrix Y. Should be 3.
// Y_dim1 : int
// The number of columns in matrix Y. Corresponds to number of atoms
// Y_dim1_mem : int
// number of columns of Y in memory. corresponds to the number of padded atoms.
// such that the (i,j)-th element of Y is accessed at Y[i*Y_dim1*mem + j] //
//
// Returns
// -------
// mirror : int
// = 1 if they are mirror images
// = 0 if they are not mirror images
if ((X_dim0 != Y_dim0) || (X_dim1 != Y_dim1) || (X_dim0 != 3)){
fprintf(stderr, "is_mirror_image called with wrong shape\n");
exit(1);
}
// covariance = np.dot(X, Y.T)
gsl_matrix* covariance = gsl_matrix_alloc(3, 3);
gsl_matrix_view mX = gsl_matrix_view_array_with_tda(X, X_dim0, X_dim1, X_dim1_mem);
gsl_matrix_view mY = gsl_matrix_view_array_with_tda(Y, Y_dim0, Y_dim1, Y_dim1_mem);
gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1.0, &mX.matrix, &mY.matrix, 0.0, covariance);
gsl_matrix* U = gsl_matrix_alloc(3, 3);
gsl_vector* S = gsl_vector_alloc(3);
gsl_vector* work = gsl_vector_alloc(3);
gsl_linalg_SV_decomp(covariance, U, S, work);
double determinant = ddet(covariance->data) * ddet(U->data);
gsl_matrix_free(covariance);
gsl_matrix_free(U);
gsl_vector_free(S);
gsl_vector_free(work);
return determinant < 0;
}
int ap_svd(gsl_matrix * u, gsl_vector * s, gsl_matrix * v,
gsl_matrix const * a){
const size_t n = a->size2;
gsl_vector * w = gsl_vector_alloc(n);
gsl_matrix_memcpy(u, a);
assert(GSL_SUCCESS == gsl_linalg_SV_decomp(u, v, s, w));
gsl_vector_free(w);
return GSL_SUCCESS;
}
/* Singular Value Decomposition */
CAMLprim value ml_gsl_linalg_SV_decomp(value A, value V, value S, value WORK)
{
_DECLARE_MATRIX2(A, V);
_DECLARE_VECTOR2(S, WORK);
_CONVERT_MATRIX2(A, V);
_CONVERT_VECTOR2(S, WORK);
gsl_linalg_SV_decomp(&m_A, &m_V, &v_S, &v_WORK);
return Val_unit;
}
int main(int argc, char** argv) {
const gsl_rng_type *rngType;
gsl_rng_env_setup();
rngType = gsl_rng_default;
rng = gsl_rng_alloc(rngType);
int matrixSize1 = atoi(argv[1]);
int matrixSize2 = atoi(argv[2]);
#ifdef DEBUG
printf("%5d %5d\n", matrixSize1, matrixSize2);
#endif
gsl_matrix *dataSet = gsl_matrix_alloc(matrixSize1, matrixSize2);
int i = 0;
int j = 0;
#ifdef DEBUG
printf("Generating.......");
#endif
for (i= 0; i < matrixSize1; i++) {
for (j = 0; j < matrixSize2; j++) {
gsl_matrix_set(dataSet, i, j, gsl_ran_ugaussian(rng));
#ifdef DEBUG
printf("%e\n", gsl_matrix_get(dataSet, i, j));
#endif
}
}
#ifdef DEBUG
printf("OK!\n");
#endif
gsl_matrix *svdVmatrix = gsl_matrix_alloc(matrixSize2, matrixSize2);
gsl_vector *svdSvector = gsl_vector_alloc(matrixSize2);
gsl_vector *svdWorkspace = gsl_vector_alloc(matrixSize2);
#ifdef DEBUG
for (j = 0; j < matrixSize2; j++) printf("%e\n", gsl_matrix_get(dataSet, 5, j));
#endif
if (!(gsl_linalg_SV_decomp(dataSet, svdVmatrix, svdSvector, svdWorkspace) == 0))
printf("Error!\n");
gsl_rng_free(rng);
return 0;
}
void SVD(
matrix& A,
matrix& V,
vector& S,
vector& work) {
int ret = gsl_linalg_SV_decomp(
A.ptr_,
V.ptr_,
S.ptr_,
work.ptr_);
}
/* SVD of any matrix */
int svdAnyMat(gsl_matrix * X,
gsl_matrix * U,
gsl_matrix * V,
gsl_vector * D)
{
// SVD part
gsl_vector * work;
int n = X->size1;
int p = X->size2;
if(p > n)
{
work = gsl_vector_alloc(n);
gsl_matrix * tmpV = gsl_matrix_alloc(n, n);
gsl_matrix * tmpU = gsl_matrix_alloc(p, n);
// Transpose the matrix X
gsl_matrix_transpose_memcpy(tmpU, X);
// TmpU contians t(X)
// There is linear dependence in X
// Need to replace the NaNs with zeros in the matrix
// Or something
gsl_linalg_SV_decomp(tmpU, tmpV, D, work);
gsl_vector_free(work);
// Swap back
gsl_matrix * tmp1 = gsl_matrix_alloc(tmpU->size1, tmpU->size2);
gsl_matrix * tmp2 = gsl_matrix_alloc(tmpV->size1, tmpV->size2);
gsl_matrix_memcpy(tmp1, tmpU);
gsl_matrix_memcpy(tmp2, tmpV);
gsl_matrix_free(tmpU);
gsl_matrix_free(tmpV);
gsl_matrix_memcpy(V, tmp1);
gsl_matrix_memcpy(U, tmp2);
gsl_matrix_free(tmp1);
gsl_matrix_free(tmp2);
} else {
work = gsl_vector_alloc(p);
gsl_matrix_memcpy(U, X);
gsl_linalg_SV_decomp(U, V, D, work);
gsl_vector_free(work);
}
return 0;
}
void ap_singular_values(gsl_matrix const * m){
auto u = ap_gsl_clone(m);
auto s = gsl_vector_alloc(m->size2);
auto w = gsl_vector_alloc(m->size2);
auto v = gsl_matrix_alloc(m->size2, m->size2);
gsl_linalg_SV_decomp(u, v, s, w);
ap_gsl_show(s);
ap_gsl_free(u);
ap_gsl_free(s);
ap_gsl_free(w);
ap_gsl_free(v);
return;
}
//------------------------------------------------------------------------------------------
int svd_solve(gsl_matrix * x, gsl_matrix const * a, gsl_matrix const * b,
double const tol){
// solve for X in AX=B by SVD decomposition method.
// m == numRows A == numRows B
// n == numCols A == numRows X
// p == numCols X == numCols B
std::cout << "\n" __FILE__ << " --> " << __func__ << "() --> " << __LINE__ << "\n";
const size_t m = a->size1;
const size_t n = a->size2;
const size_t p = b->size2;
gsl_matrix * U = gsl_matrix_alloc(m, n);
gsl_matrix * V = gsl_matrix_alloc(n, n);
gsl_vector * s = gsl_vector_alloc(n);
gsl_vector * w = gsl_vector_alloc(n);
gsl_matrix_memcpy(U, a);
assert(GSL_SUCCESS == gsl_linalg_SV_decomp(U, V, s, w));
{
//double norm = gsl_vector_get(s, 0); // largest singular value
//SHOW(std::max(m,n)*norm*std::numeric_limits<double>::epsilon());
//SHOW(gsl_vector_get(s, 0)/gsl_vector_get(s, n-1));
}
//AP_GSL_SHOW(s);
for(size_t i = 0; i < n; ++i){
if(tol > gsl_vector_get(s, i)){
gsl_vector_set(s, i, 0.0);
}
}
for(size_t i = 0; i < p; ++i){
gsl_vector_view xcol = gsl_matrix_column(x,i);
gsl_vector_const_view bcol = gsl_matrix_const_column(b, i);
gsl_linalg_SV_solve(U, V, s, &bcol.vector, &xcol.vector);
}
gsl_matrix_free(U);
gsl_matrix_free(V);
gsl_vector_free(s);
gsl_vector_free(w);
return GSL_SUCCESS;
}
inline void SVD::solve(std::vector<std::vector<double> > &src){
if(src.size()<=0)
{ std::cerr << "invalid matrix" <<std::endl; return;}
//--- matrix allocation ---
Allocation(src[0].size(), src.size());
// copy into gsl_matrix* A
for(size_t i=0; i<m; ++i) for(size_t j=0; j<n; ++j) gsl_matrix_set(A, i, j, src[j][i]);
if(m<n) { std::cerr << "invalid matrix" <<std::endl; return;}
//--- the condition of m >> n is assumed as m > 100*n
if(m < 100*n) // Golub Reinsch method;
gsl_linalg_SV_decomp(A, V, S, work);
else // modified Golub Reinsch method;
gsl_linalg_SV_decomp_mod(A, X, V, S, work);
}
int main (int argc, char** argv, char** env) {
int i, j;
const gsl_rng_type *T;
gsl_rng *r;
gsl_rng_env_setup();
T = gsl_rng_default;
r = gsl_rng_alloc (T);
gsl_matrix *x = gsl_matrix_alloc (SIZE1, SIZE2);
#ifdef DEBUG
printf ("Start allocating matrix...\n");
#endif
for (i = 0; i < (x -> size1); i++) {
for (j = 0; j < (x -> size2); j++) {
gsl_matrix_set(x, i, j, gsl_ran_gaussian(r, 1.0));
}
}
#ifdef DEBUG
printf ("matrix allocation finished\n");
#endif
gsl_matrix *v = gsl_matrix_alloc (SIZE2, SIZE2);
gsl_vector *s = gsl_vector_alloc (SIZE2);
gsl_vector *work = gsl_vector_alloc (SIZE2);
gsl_linalg_SV_decomp (x, v, s, work);
#ifdef DEBUG
printf("%e\n", gsl_matrix_get(x, rand() % SIZE1 + 1, rand() % SIZE2 + 2));
#endif
#ifdef SVD_MOD
gsl_matrix *mx = gsl_matrix_alloc (SIZE2, SIZE2);
gsl_linalg_SV_decomp_mod (x, mx, v, s, work);
#endif
#iddef CLAPACK_SVD
dgesvd
#endif
return 0;
}
void PseudoInverter::invert (Matrix& inv, const Matrix& src, double threshold)
{
SVD_S->zero();
SVD_U->copy(src);
if (gsl_linalg_SV_decomp(SVD_U->get_gsl_matrix(), SVD_V->get_gsl_matrix(), singular_values, SVD_work))
throw Exception ("error computing SVD for pseudo-inverse");
for (guint x = 0; x < src.columns(); x++)
(*SVD_S)(x,x) = gsl_vector_get(singular_values, x) > threshold ?
1.0/gsl_vector_get(singular_values, x) : 0.0;
// VAR(gsl_vector_get(singular_values, 0)/ gsl_vector_get(singular_values,src.columns()-1));
SVD_Ut->transpose(*SVD_U);
SVD_D->multiply(*SVD_S, *SVD_Ut);
inv.multiply(*SVD_V, *SVD_D);
}
// f = I(||X||_2 <= 1)
void project_spectral_norm_ball(gsl_matrix *X)
{
gsl_matrix *V = gsl_matrix_alloc(X->size1, X->size2);
gsl_vector *d = gsl_vector_alloc(X->size2);
gsl_vector *tmp = gsl_vector_alloc(X->size2);
gsl_linalg_SV_decomp(X, W, d, tmp);
int i;
double d_i;
gsl_matrix_set_zero(X);
for (i = 0; i < X->size2; i++) {
d_i = fmax(1, gsl_vector_get(d, i));
gsl_vector_view U_i = gsl_matrix_column(X, i);
gsl_vector_view V_i = gsl_matrix_column(V, i);
gsl_blas_dger(d_i, &U_i.vector, &V_i.vector, X);
}
gsl_vector_free(d);
gsl_matrix_free(V);
gsl_vector_free(tmp);
}
int gsl_svd(
const struct tc_mat *A,
struct tc_mat *U,
struct tc_mat *D,
struct tc_mat *V)
{
if (!A || !U || !D || !V)
return -1;
if ((U->nr != A->nr) || (U->nc != A->nc))
tc_mat_resize(U, A->nr, A->nc);
if ((V->nr != A->nc) || (V->nc != A->nc))
tc_mat_resize(V, A->nc, A->nc);
if ((D->nr != A->nc) || (D->nc != A->nc))
tc_mat_resize(D, A->nc, A->nc);
gsl_vector *gwork = gsl_vector_alloc(A->nc);
gsl_vector *gS = gsl_vector_alloc(A->nc);
gsl_matrix *gA = gsl_matrix_alloc(A->nr, A->nc);
gsl_matrix *gV = gsl_matrix_alloc(A->nc, A->nc);
for(uint32_t i=0; i < A->nr; i++)
for(uint32_t j=0; j < A->nc; j++)
gsl_matrix_set(gA, i, j, A->a[i][j]);
gsl_set_error_handler_off();
int status = gsl_linalg_SV_decomp(gA, gV, gS, gwork);
if (status != GSL_SUCCESS)
return -2;
for(uint32_t i=0; i < U->nr; i++)
for(uint32_t j=0; j < U->nc; j++)
U->a[i][j] = gsl_matrix_get(gA, i, j);
for(uint32_t i=0; i < D->nr; i++)
for(uint32_t j=0; j < D->nc; j++)
D->a[i][j] = (i==j)? gsl_vector_get(gS, i): 0.0;
for(uint32_t i=0; i < V->nr; i++)
for(uint32_t j=0; j < V->nc; j++)
V->a[i][j] = gsl_matrix_get(gV, i, j);
gsl_vector_free(gwork);
gsl_vector_free(gS);
gsl_matrix_free(gA);
gsl_matrix_free(gV);
return 0;
}
/** 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 );
}
static
int fit_tan_wcs_solve(const double* starxyz,
const double* fieldxy,
const double* weights,
int N,
const double* crpix,
const tan_t* tanin,
tan_t* tanout,
double* p_scale) {
int i, j, k;
double field_cm[2] = {0, 0};
double cov[4] = {0, 0, 0, 0};
double R[4] = {0, 0, 0, 0};
double scale;
// projected star coordinates
double* p;
// relative field coordinates
double* f;
double pcm[2] = {0, 0};
double w = 0;
double totalw;
gsl_matrix* A;
gsl_matrix* U;
gsl_matrix* V;
gsl_vector* S;
gsl_vector* work;
gsl_matrix_view vcov;
gsl_matrix_view vR;
double crxyz[3];
double star_cm[3] = {0, 0, 0};
assert(((tanin != NULL) && (crpix != NULL)) || ((tanin == NULL) && (crpix == NULL)));
if (tanin) {
// default vals...
memcpy(tanout, tanin, sizeof(tan_t));
} else {
memset(tanout, 0, sizeof(tan_t));
}
// -allocate and fill "p" and "f" arrays. ("projected" and "field")
p = malloc(N * 2 * sizeof(double));
f = malloc(N * 2 * sizeof(double));
// -get field center-of-mass
totalw = 0.0;
for (i=0; i<N; i++) {
w = (weights ? weights[i] : 1.0);
field_cm[0] += w * fieldxy[i*2 + 0];
field_cm[1] += w * fieldxy[i*2 + 1];
totalw += w;
}
field_cm[0] /= totalw;
field_cm[1] /= totalw;
// Subtract it out.
for (i=0; i<N; i++) {
f[2*i+0] = fieldxy[2*i+0] - field_cm[0];
f[2*i+1] = fieldxy[2*i+1] - field_cm[1];
}
if (tanin) {
// Use original WCS to set the center of projection to the new crpix.
tan_pixelxy2xyzarr(tanin, crpix[0], crpix[1], crxyz);
for (i=0; i<N; i++) {
Unused anbool ok;
// -project the stars around crval
ok = star_coords(starxyz + i*3, crxyz, TRUE, p + 2*i, p + 2*i + 1);
assert(ok);
}
} else {
// -get the star center-of-mass (this will become the tangent point CRVAL)
for (i=0; i<N; i++) {
w = (weights ? weights[i] : 1.0);
star_cm[0] += w * starxyz[i*3 + 0];
star_cm[1] += w * starxyz[i*3 + 1];
star_cm[2] += w * starxyz[i*3 + 2];
}
normalize_3(star_cm);
// -project the stars around their center of mass
for (i=0; i<N; i++) {
Unused anbool ok;
ok = star_coords(starxyz + i*3, star_cm, TRUE, p + 2*i, p + 2*i + 1);
assert(ok);
}
}
// -compute the center of mass of the projected stars and subtract it out.
for (i=0; i<N; i++) {
w = (weights ? weights[i] : 1.0);
pcm[0] += w * p[2*i + 0];
pcm[1] += w * p[2*i + 1];
}
pcm[0] /= totalw;
pcm[1] /= totalw;
for (i=0; i<N; i++) {
p[2*i + 0] -= pcm[0];
p[2*i + 1] -= pcm[1];
}
// -compute the covariance between field positions and projected
// positions of the corresponding stars.
for (i=0; i<N; i++)
for (j=0; j<2; j++)
for (k=0; k<2; k++)
cov[j*2 + k] += p[i*2 + k] * f[i*2 + j];
for (i=0; i<4; i++)
assert(isfinite(cov[i]));
// -run SVD
V = gsl_matrix_alloc(2, 2);
S = gsl_vector_alloc(2);
work = gsl_vector_alloc(2);
vcov = gsl_matrix_view_array(cov, 2, 2);
vR = gsl_matrix_view_array(R, 2, 2);
A = &(vcov.matrix);
// The Jacobi version doesn't always compute an orthonormal U if S has zeros.
//gsl_linalg_SV_decomp_jacobi(A, V, S);
gsl_linalg_SV_decomp(A, V, S, work);
// the U result is written to A.
U = A;
gsl_vector_free(S);
gsl_vector_free(work);
// R = V U'
gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1.0, V, U, 0.0, &(vR.matrix));
gsl_matrix_free(V);
for (i=0; i<4; i++)
assert(isfinite(R[i]));
// -compute scale: make the variances equal.
{
double pvar, fvar;
pvar = fvar = 0.0;
for (i=0; i<N; i++) {
w = (weights ? weights[i] : 1.0);
for (j=0; j<2; j++) {
pvar += w * square(p[i*2 + j]);
fvar += w * square(f[i*2 + j]);
}
}
scale = sqrt(pvar / fvar);
}
// -compute WCS parameters.
scale = rad2deg(scale);
tanout->cd[0][0] = R[0] * scale; // CD1_1
tanout->cd[0][1] = R[1] * scale; // CD1_2
tanout->cd[1][0] = R[2] * scale; // CD2_1
tanout->cd[1][1] = R[3] * scale; // CD2_2
assert(isfinite(tanout->cd[0][0]));
assert(isfinite(tanout->cd[0][1]));
assert(isfinite(tanout->cd[1][0]));
assert(isfinite(tanout->cd[1][1]));
if (tanin) {
// CRPIX is fixed.
tanout->crpix[0] = crpix[0];
tanout->crpix[1] = crpix[1];
// Set CRVAL temporarily...
tan_pixelxy2radec(tanin, crpix[0], crpix[1],
tanout->crval+0, tanout->crval+1);
// Shift CRVAL so that the center of the quad is in the right place.
{
double ix,iy;
double dx,dy;
double dxyz[3];
tan_pixelxy2iwc(tanout, field_cm[0], field_cm[1], &ix, &iy);
dx = rad2deg(pcm[0]) - ix;
dy = rad2deg(pcm[1]) - iy;
tan_iwc2xyzarr(tanout, dx, dy, dxyz);
xyzarr2radecdeg(dxyz, tanout->crval + 0, tanout->crval + 1);
}
} else {
tanout->crpix[0] = field_cm[0];
tanout->crpix[1] = field_cm[1];
xyzarr2radecdegarr(star_cm, tanout->crval);
// FIXME -- we ignore pcm. It should get added back in (after
// multiplication by CD in the appropriate units) to either crval or
// crpix. It's a very small correction probably of the same size
// as the other approximations we're making.
}
if (p_scale) *p_scale = scale;
free(p);
free(f);
return 0;
}
int main(int argc, char **argv){
int row = atoi(argv[2]);
int col = atoi(argv[3]);
printf("%d %d\n", row, col);
gsl_matrix* data = gsl_matrix_alloc(row, col);
//gsl_matrix* data = gsl_matrix_alloc(col, row);
FILE* f = fopen(argv[1], "r");
gsl_matrix_fscanf(f, data);
//gsl_matrix_fread(f, data);
//gsl_matrix_transpose_memcpy(data, data_raw);
fclose(f);
//printf("%f %f", gsl_matrix_get(data,0,0), gsl_matrix_get(data,0,1));
//f = fopen("test.dat", "w");
//gsl_matrix_fprintf(f, data, "%f");
//fclose(f);
// data centering, subtract the mean in each dimension (col.-wise)
int i, j;
double mean, sum, std;
gsl_vector_view col_vector;
for (i = 0; i < col; ++i){
col_vector = gsl_matrix_column(data, i);
mean = gsl_stats_mean((&col_vector.vector)->data, 1,
(&col_vector.vector)->size);
gsl_vector_add_constant(&col_vector.vector, -mean);
gsl_matrix_set_col(data, i, &col_vector.vector);
}
char filename[50];
//sprintf(filename, "%s.zscore", argv[1]);
//print2file(filename, data);
gsl_matrix* u;
if (col > row) {
u = gsl_matrix_alloc(data->size2, data->size1);
gsl_matrix_transpose_memcpy(u, data);
}
else {
u = gsl_matrix_alloc(data->size1, data->size2);
gsl_matrix_memcpy(u, data);
}
// svd
gsl_matrix* X = gsl_matrix_alloc(col, col);
gsl_matrix* V = gsl_matrix_alloc(u->size2, u->size2);
gsl_vector* S = gsl_vector_alloc(u->size2);
gsl_vector* work = gsl_vector_alloc(u->size2);
gsl_linalg_SV_decomp(u, V, S, work);
//gsl_linalg_SV_decomp_jacobi(u, V, S);
// mode coefficient
//print2file("u.dat", u);
/*
// characteristic mode
gsl_matrix* diag = diag_alloc(S);
gsl_matrix* mode = gsl_matrix_alloc(diag->size1, V->size1);
gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1.0, diag, V, 0.0, mode);
gsl_matrix_transpose(mode);
print2file("mode.dat", mode);
gsl_matrix_transpose(mode);
*/
// reconstruction
gsl_matrix *recons = gsl_matrix_alloc(u->size2, data->size1);
if (col > row) {
gsl_matrix_view data_sub = gsl_matrix_submatrix(data, 0, 0,
u->size2, u->size2);
gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1.0, V,
&data_sub.matrix, 0.0, recons);
}
else
gsl_blas_dgemm(CblasTrans, CblasTrans, 1.0, V, data, 0.0, recons);
//gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, u, mode, 0.0,
// recons);
gsl_matrix *recons_trans = gsl_matrix_alloc(recons->size2,
recons->size1);
gsl_matrix_transpose_memcpy(recons_trans, recons);
// take the first two eigenvectors
gsl_matrix_view final = gsl_matrix_submatrix(recons_trans, 0, 0,
recons_trans->size1, 2);
print2file(argv[4], &final.matrix);
// eigenvalue
gsl_vector_mul(S, S);
f = fopen("eigenvalue.dat", "w");
//gsl_vector_fprintf(f, S, "%f");
fclose(f);
gsl_matrix_free(data);
gsl_matrix_free(X);
gsl_matrix_free(V);
//gsl_matrix_free(diag);
//gsl_matrix_free(mode);
gsl_matrix_free(recons);
gsl_matrix_free(recons_trans);
gsl_matrix_free(u);
gsl_vector_free(S);
gsl_vector_free(work);
//gsl_vector_free(zero);
//gsl_vector_free(corrcoef);
//gsl_vector_free(corrcoef_mean);
return 0;
}
/** \brief Principal Components analysis.
\ingroup grplinalg
This implementation uses SVD to calculate the PCA.
Example:
\code
Array *X = get_data_from_somewhere();
Array *var, *X_pca;
matrix_pca( X, &var, FALSE ); // overwrite X
X_pca = matrix_pca( X, &var, TRUE ); // do not touch X
\endcode
\param X a 2D array observations x variables containing the data
\param var output: vector of eigenvalues of XX^T in decreasing order; if you
pass NULL, it is ignored;
\param alloc if true, new memory is allocated and returned. Else
X is overwritten.
\return pointer to the PCA'd matrix
*/
Array* matrix_pca( Array *X, Array **var, bool alloc ){
Array *out=NULL;
int i,j;
bool ismatrix;
matrix_CHECK( ismatrix, X );
if( !ismatrix ) return NULL;
int N,K; /* N observations, K variables */
N = X->size[0];
K = X->size[1];
Array *tmp = array_copy( X, TRUE );
/* subtract mean from observations */
Array *mean=matrix_mean( X, 0 );
for( i=0; i<N; i++ ){
for( j=0; j<K; j++ ){
array_INDEX2( tmp, double, i, j ) -=
array_INDEX1( mean, double, j );
}
}
array_scale( tmp, 1.0/sqrt((double) N-1 ) );
gsl_matrix *A=matrix_to_gsl( tmp, TRUE ); /* copy */
gsl_matrix *V=gsl_matrix_alloc( K, K );
gsl_vector *S=gsl_vector_alloc( K );
gsl_vector *workspace=gsl_vector_alloc( K );
/* A->U, V->V, S->S */
gsl_linalg_SV_decomp( A, V, S, workspace);
gsl_matrix_transpose( V );
if( var ){
(*var)=array_fromptr2( DOUBLE, 1, S->data, S->size );
S->owner=0; /* transfer ownership to array */
(*var)->free_data=1;
for( i=0; i<array_NUMEL( *var ); i++ ){
array_INDEX1( *var, double, i ) = SQR( array_INDEX1( *var, double, i ) );
}
}
Array *Vp=array_fromptr2( DOUBLE, 2, V->data, V->size1, V->size2 );
matrix_transpose( tmp, FALSE );
out=matrix_mult( Vp, tmp ); /* PCA'd data */
matrix_transpose( out, FALSE );
if( out->size[0]!=X->size[0] || out->size[1]!=X->size[1] ){
errprintf("Input/Output dimension mismatch: (%i,%i) vs. (%i,%i)\n",
X->size[0], X->size[1], out->size[0], out->size[1] );
}
if( !alloc ){ /* write back out->X */
memcpy( X->data, out->data, out->nbytes );
array_free( out );
out = X;
}
/* clean up */
gsl_matrix_free( A );
gsl_matrix_free( V );
gsl_vector_free( S );
gsl_vector_free( workspace );
array_free( mean );
array_free( Vp );
array_free( tmp );
return out;
}
int SolveSVD (double a[], double b[], double x[], int neq, int nvar)
{
// get A
gsl_matrix_view av = gsl_matrix_view_array (a, neq, nvar);
if (neq < nvar) { // M < N ... do the transposed matrix
gsl_matrix *atrans = gsl_matrix_alloc (nvar, neq);
gsl_matrix_transpose_memcpy (atrans, &av.matrix);
gsl_matrix *v = gsl_matrix_alloc (neq, neq);
gsl_vector *s = gsl_vector_alloc (neq);
gsl_vector *work = gsl_vector_alloc (neq);
gsl_linalg_SV_decomp (atrans, v, s, work);
// x = A+ b
gsl_matrix *splus = gsl_matrix_calloc (neq, neq);
// compute sigma_plus
for (int i = 0; i < neq; i++) {
double sigma;
if ((sigma = gsl_vector_get (s,i)) != 0.0)
gsl_matrix_set (splus, i,i, 1.0/sigma);
}
gsl_linalg_matmult (atrans, splus, atrans);
gsl_linalg_matmult_mod (atrans, GSL_LINALG_MOD_NONE, v, GSL_LINALG_MOD_TRANSPOSE, atrans);
gsl_vector_view bv = gsl_vector_view_array (b, neq);
gsl_matrix_view bmv = gsl_matrix_view_vector (&bv.vector, neq, 1);
gsl_matrix *xx = gsl_matrix_alloc (nvar,1);
gsl_linalg_matmult (atrans, &bmv.matrix, xx);
// gsl_matrix_fprintf (stdout, xx, "%g");
for (int i = 0; i < nvar; i++) {
x[i] = gsl_matrix_get(xx,i,0);
}
gsl_matrix_free (splus); gsl_matrix_free (xx);
gsl_matrix_free (atrans);
gsl_matrix_free (v); gsl_vector_free (s); gsl_vector_free (work);
} else { // M >= N
gsl_matrix *v = gsl_matrix_alloc (nvar, nvar);
gsl_vector *s = gsl_vector_alloc (nvar);
gsl_vector *work = gsl_vector_alloc (nvar);
gsl_linalg_SV_decomp (&av.matrix, v, s, work);
// x = A+ b
gsl_vector_view bv = gsl_vector_view_array (b, neq);
gsl_vector *xx = gsl_vector_alloc (nvar);
gsl_linalg_SV_solve (&av.matrix, v, s, &bv.vector, xx);
// gsl_vector_fprintf (stdout, xx, "%g");
for (int i = 0; i < nvar; i++)
x[i] = gsl_vector_get (xx, i);
gsl_vector_free (xx);
gsl_matrix_free (v); gsl_vector_free (s); gsl_vector_free (work);
}
return 1;
}
/*....................................................................*/
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;
}
int GMRFLib_gsl_ginv(gsl_matrix * A, double tol, int rankdef)
{
/*
* replace n x n matrix A with its generlized inverse. if TOL > 0, use that tolerance. If rankdef is set, use that. If both are set, give an error.
*/
assert(A && (A->size1 == A->size2));
gsl_matrix *U = GMRFLib_gsl_duplicate_matrix(A);
gsl_matrix *V = gsl_matrix_alloc(A->size1, A->size2);
gsl_vector *S = gsl_vector_alloc(A->size1);
gsl_vector *work = gsl_vector_alloc(A->size1);
gsl_linalg_SV_decomp(U, V, S, work);
size_t i;
double one = 1.0, zero = 0.0;
double s_max = gsl_vector_get(S, 0);
gsl_matrix *M1 = gsl_matrix_alloc(A->size1, A->size2);
gsl_matrix *M2 = gsl_matrix_alloc(A->size1, A->size2);
assert(!(tol > 0.0 && (rankdef >= 0 && rankdef <= (int) A->size1)));
if (tol > 0.0){
for(i = 0; i < A->size1; i++){
double s = gsl_vector_get(S, i);
if (s < tol * s_max) {
gsl_matrix_set(M2, i, i, 0.0);
} else {
gsl_matrix_set(M2, i, i, 1.0/s);
}
}
} else {
assert(rankdef >= 0);
assert(rankdef <= (int)A->size1);
double first = gsl_vector_get(S, 0);
double last = gsl_vector_get(S, A->size1-1);
for(i = 0; i < A->size1; i++){
double s = gsl_vector_get(S, i);
if (first > last){
// do not use the last 'rdef's
if (i < (int) A->size1 - rankdef){
gsl_matrix_set(M2, i, i, 1.0/s);
} else {
gsl_matrix_set(M2, i, i, 0.0);
}
} else {
// do not use the first 'rdef's
if (i < rankdef){
gsl_matrix_set(M2, i, i, 0.0);
} else {
gsl_matrix_set(M2, i, i, 1.0/s);
}
}
}
}
gsl_blas_dgemm(CblasNoTrans, CblasTrans, one, M2, U, zero, M1);
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, one, V, M1, zero, M2);
gsl_matrix_memcpy(A, M2);
gsl_matrix_free(U);
gsl_matrix_free(V);
gsl_matrix_free(M1);
gsl_matrix_free(M2);
gsl_vector_free(S);
gsl_vector_free(work);
return GMRFLib_SUCCESS;
}
/**
* C++ version of gsl_linalg_SV_decomp().
* @param A A matrix
* @param V A matrix (part of SVD)
* @param S A vector
* @param work A vector
* @return Error code on failure
*/
inline int SV_decomp( matrix& A, matrix& V, vector& S, vector& work ){
return gsl_linalg_SV_decomp( A.get(), V.get(), S.get(), work.get() ); }
/** 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;
}
