-
Andrea Censi authored
No commit message
Andrea Censi authoredNo commit message
egsl.c 7.79 KiB
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_linalg.h>
#include <assert.h>
#include <math.h>
#include "egsl.h"
#define MAX_VALS 1024
#define MAX_CONTEXTS 1024
#define INVALID (val_from_context_and_index(1000,1000))
struct egsl_variable {
gsl_matrix * gsl_m;
};
gsl_matrix * egsl_gslm(val v);
struct egsl_context {
int nvars;
struct egsl_variable vars[MAX_VALS];
};
int cid=0;
struct egsl_context egsl_contexts[MAX_CONTEXTS];
void error() {
assert(0);
}
void egsl_expect_size(val v, size_t rows, size_t cols) {
gsl_matrix * m = egsl_gslm(v);
int bad = (rows && (m->size1!=rows)) || (cols && (m->size2!=cols));
if(bad) {
fprintf(stderr, "Matrix size is %d,%d while I expect %d,%d",
m->size1,m->size2,rows,cols);
error();
}
}
void egsl_init_context(int i) {
}
void egsl_free_context(int i) {
struct egsl_context * c = egsl_contexts+i;
int v;
for(v=0;v<c->nvars;v++) {
gsl_matrix_free(c->vars[v].gsl_m);
}
c->nvars = 0;
}
void egsl_push() {
cid++;
assert(cid<MAX_CONTEXTS);// "Maximum number of contexts reached");
egsl_init_context(cid);
// printf("Now context is %d\n",cid);
}
void egsl_pop() {
assert(cid>=0);//, "No egsl_push before?");
egsl_free_context(cid);
cid--;
// printf("pop: Now context is %d\n",cid);
}
int its_context(val v) {
return (v>>16) & 0x0fff;
}
int its_var_index(val v) {
return v & 0x0fff;
}
val val_from_context_and_index(int cid, int index) {
return ((cid& 0x0fff)<<16) | (index& 0x0fff);
}
void check_valid_val(val v) {
int context = its_context(v);
if(context>cid) {
printf("Val %d is from invalid context (%d>%d)\n",v,context,cid);
error();
}
int var_index = its_var_index(v);
if(var_index >= egsl_contexts[context].nvars) {
printf("Val %d is invalid (%d>%d)\n",v,var_index,
egsl_contexts[context].nvars);
error();
}
}
gsl_matrix * egsl_gslm(val v) {
check_valid_val(v);
int context = its_context(v);
int var_index = its_var_index(v);
return egsl_contexts[context].vars[var_index].gsl_m;
}
void egsl_print(const char*str, val v) {
gsl_matrix * m = egsl_gslm(v);
size_t i,j;
int context = its_context(v);
int var_index = its_var_index(v);
printf("%s = (%d x %d) val=%d context=%d index=%d\n",
str,m->size1,m->size2, v, context, var_index);
for(i=0;i<m->size1;i++) {
if(i==0)
printf(" [ ");
else
printf(" ");
for(j=0;j<m->size2;j++)
printf("%f ", gsl_matrix_get(m,i,j));
if(i==m->size1-1)
printf("] \n");
else
printf("; \n");
}
}
val egsl_alloc(size_t rows, size_t columns) {
struct egsl_context*c = egsl_contexts+cid;
if(c->nvars>=MAX_VALS) {
printf("Limit reached\n");
error();
}
int index = c->nvars;
c->vars[index].gsl_m = gsl_matrix_alloc((size_t)rows,(size_t)columns);
val v = val_from_context_and_index(cid,index);
c->nvars++;
//printf("Allocated %d\n",v);
return v;
}
double* egsl_atmp(val v, size_t i, size_t j) {
gsl_matrix * m = egsl_gslm(v);
return gsl_matrix_ptr(m,(size_t)i,(size_t)j);
}
val egsl_vFda(size_t rows, size_t cols, const double *a) {
val v = egsl_alloc(rows, cols);
size_t i; size_t j;
for(i=0;i<rows;i++)
for(j=0;j<cols;j++) {
*egsl_atmp(v,i,j) = a[i+j*cols];
}
return v;
}
val egsl_vFa(size_t rows, const double*a) {
val v = egsl_alloc(rows,1);
size_t i;
for(i=0;i<rows;i++)
*egsl_atmp(v,i,0) = a[i];
return v;
}
val egsl_rot(double theta) {
double R[2*2] = {
cos(theta), -sin(theta),
sin(theta), cos(theta)
};
return egsl_vFda(2,2,R);
}
val egsl_zeros(size_t rows, size_t columns) {
val v = egsl_alloc(rows,columns);
gsl_matrix * m = egsl_gslm(v);
gsl_matrix_set_all(m,0.0);
return v;
}
val egsl_ones(size_t rows, size_t columns) {
val v = egsl_alloc(rows,columns);
gsl_matrix * m = egsl_gslm(v);
gsl_matrix_set_all(m,1.0);
return v;
}
val egsl_copy_val(val v1) {
gsl_matrix * m1 = egsl_gslm(v1);
val v2 = egsl_alloc(m1->size1,m1->size2);
gsl_matrix * m2 = egsl_gslm(v2);
gsl_matrix_memcpy(m2,m1);
return v2;
}
val egsl_scale(double s, val v1){
val v2 = egsl_copy_val(v1);
gsl_matrix * m2 = egsl_gslm(v2);
gsl_matrix_scale(m2, s);
return v2;
}
val egsl_sum(val v1, val v2){
gsl_matrix * m1 = egsl_gslm(v1);
gsl_matrix * m2 = egsl_gslm(v2);
val v3 = egsl_alloc(m1->size1,m1->size2);
gsl_matrix * m3 = egsl_gslm(v3);
gsl_matrix_memcpy(m3,m1);
gsl_matrix_add(m3,m2);
return v3;
}
val egsl_sum3(val v1, val v2, val v3){
return egsl_sum(v1, egsl_sum(v2,v3));
}
val egsl_mult(val v1, val v2){
gsl_matrix * a = egsl_gslm(v1);
gsl_matrix * b = egsl_gslm(v2);
val v = egsl_alloc(a->size1,b->size2);
gsl_matrix * ab = egsl_gslm(v);
gsl_blas_dgemm(CblasNoTrans,CblasNoTrans,1.0,a,b,0.0,ab);
return v;
}
val egsl_transpose(val v1){
gsl_matrix * m1 = egsl_gslm(v1);
val v2 = egsl_alloc(m1->size2,m1->size1);
gsl_matrix * m2 = egsl_gslm(v2);
gsl_matrix_transpose_memcpy(m2,m1);
return v2;
}
val egsl_inverse(val v1){
gsl_matrix*A = egsl_gslm(v1);
val v2 = egsl_alloc(A->size1,A->size1);
gsl_matrix*invA = egsl_gslm(v2);
size_t n = A->size1;
gsl_matrix * m = gsl_matrix_alloc(n,n);
gsl_matrix_memcpy(m,A);
gsl_permutation * perm = gsl_permutation_alloc (n);
// Make LU decomposition of matrix m
int s;
gsl_linalg_LU_decomp (m, perm, &s);
// Invert the matrix m
gsl_linalg_LU_invert (m, perm, invA);
gsl_permutation_free(perm);
gsl_matrix_free(m);
return v2;
}
double egsl_norm(val v1){
egsl_expect_size(v1, 0, 1);
double n=0;
size_t i;
gsl_matrix * m = egsl_gslm(v1);
for(i=0;i<m->size1;i++) {
double v = gsl_matrix_get(m,i,0);
n += v * v;
}
return sqrt(n);
}
double egsl_atv(val v1, size_t i){
return *egsl_atmp(v1, i, 0);
}
double egsl_atm(val v1, size_t i, size_t j){
return *egsl_atmp(v1, i, j);
}
val egsl_sub(val v1,val v2){
return egsl_sum(v1, egsl_scale(-1.0,v2));
}
val egsl_compose_col(val v1, val v2){
gsl_matrix *m1 = egsl_gslm(v1);
gsl_matrix *m2 = egsl_gslm(v2);
egsl_expect_size(v2, 0, m1->size2);
val v3 = egsl_alloc(m1->size1+m2->size1,m1->size2);
gsl_matrix *m3 = egsl_gslm(v3);
size_t i,j;
for(j=0;j<m1->size2;j++) {
for(i=0;i<m1->size1;i++)
gsl_matrix_set(m3, i, j, gsl_matrix_get(m1,i,j));
for(i=0;i<m2->size1;i++)
gsl_matrix_set(m3, m1->size1+i, j, gsl_matrix_get(m2,i,j));
}
return v3;
}
val egsl_compose_row(val v1, val v2){
gsl_matrix *m1 = egsl_gslm(v1);
gsl_matrix *m2 = egsl_gslm(v2);
egsl_expect_size(v2, m1->size1, 0);
val v3 = egsl_alloc(m1->size1, m1->size2 + m2->size2);
gsl_matrix *m3 = egsl_gslm(v3);
size_t i,j;
for(i=0;i<m1->size1;i++) {
for(j=0;j<m1->size2;j++)
gsl_matrix_set(m3, i, j, gsl_matrix_get(m1,i,j));
for(j=0;j<m2->size2;j++)
gsl_matrix_set(m3, i, m1->size2+j, gsl_matrix_get(m2,i,j));
}
return v3;
}
val egsl_vers(double theta){
double v[2] = { cos(theta), sin(theta)};
return egsl_vFa(2,v);
}
void egsl_v2da(val v, double*a){
gsl_matrix *m = egsl_gslm(v);
size_t i,j;
for(i=0;i<m->size1;i++)
for(j=0;j<m->size2;j++)
a[j*m->size1 +i] = gsl_matrix_get(m,i,j);
}
val egsl_vFgslv(const gsl_vector*vec){
val v = egsl_alloc(vec->size,1);
size_t i;
for(i=0;i<vec->size;i++)
*egsl_atmp(v,i,0) = gsl_vector_get(vec,i);
return v;
}
val egsl_vFgslm(const gsl_matrix*m){
val v = egsl_alloc(m->size1,m->size2);
gsl_matrix * m2 = egsl_gslm(v);
gsl_matrix_memcpy(m2,m);
return v;
}
gsl_matrix* egsl_v2gslm(val v){
gsl_matrix * m = egsl_gslm(v);
gsl_matrix * m2 = gsl_matrix_alloc(m->size1,m->size2);
gsl_matrix_memcpy(m2,m);
return m;
}
void egsl_add_to(val v1, val v2) {
gsl_matrix * m1 = egsl_gslm(v1);
gsl_matrix * m2 = egsl_gslm(v2);
gsl_matrix_add(m1,m2);
}
void egsl_add_to_col(val v1, size_t j, val v2) {
// egsl_print("m1",v1);
// egsl_print("m2",v2);
gsl_matrix * m1 = egsl_gslm(v1);
gsl_matrix * m2 = egsl_gslm(v2);
// printf("m1 size = %d,%d j = %d\n",m1->size1,m1->size2,j);
egsl_expect_size(v2, m1->size1, 1);
size_t i;
for(i=0;i<m1->size1;i++) {
*gsl_matrix_ptr(m1, i, j) += gsl_matrix_get(m2,i,0);
}
}