Esempio n. 1
0
/***************************************************************
*   void vprintf_tests
*      
*
****************************************************************/
void vprintf_tests() {
    set_col(2); set_row(4);
    vprintf_custom("Esto es una prueba: %x %x %x %x", 1000, 10000, 1000000, 99999999);
    set_col(2); set_row(5);
    vprintf_custom("Esto es una prueba: %d %d %d %d", 1000, 10000, 1000000, 99999999);
    set_col(2); set_row(6);
    vprintf_custom("Esto es una prueba: %x %d %x %d", 1000, 10000, 1000000, 999999);
    set_col(2); set_row(7);
    vprintf_custom("Esto es una prueba: %x %d %s %x", 1000, 10000, "Hello!!", 99999999);
    set_col(2); set_row(10);
    vprintf_custom("Esto es una prueba: %s %s", "dos strings muuuuuuuuuuuy largooooooos", "012345678901234567890123456789001234567890123456789");
    print_vscreen();
}
Esempio n. 2
0
void free_command(char* params) {
    char option[20];
    bool leave = false;
    unsigned int address = 0;
    unsigned int i = 0;
    unsigned int len = 0;
    void* result = 0;

    len = strlen_custom(params);
    memset_custom(option, sizeof(option), 0);

    sscanf_custom(params, "%s", option);

    if( strcmp(option, "info") == 0 ) {
        newline();
        set_col(2);
        vprintf_custom("Usa el comando \'free\' seguido de una direccion"
                " de memoria en base hexadecimal (es decir el numero precedido"
                " por 0x o 0X), para liberar la porcion de memoria que empieza"
                " en ese lugar.");
    } else {
        for(i = 0; i < len; i++) {
            if( ishex(option[i] == false) ) {
                leave = true;
                break;
            }
        }
        //compruebo que el numero esté en hexa
        if( leave == false 
                && option[0] == '0'
                && (option[1] == 'x' || option[1] == 'X')) {
            sscanf_custom(option, "%x", &address);
            
            result = (void*)free((unsigned int*)address);
            if( result == (void*)address && (unsigned int)result > 0xF) {
                newline(); set_col(2);
                vprintf_custom("La memoria con direccion %x fue liberada.", address);
            } else {
                newline(); set_col(2);
                vprintf_custom("No existe ningun segmento de memoria alocado"
                        "en esa direccion.");
            }
        } else {
            newline(); set_col(2);
            vprintf_custom("Debes ingresar un direccion de memoria en "
                    "hexa para liberar esa porcion de memoria.");
        }
    }
}
Esempio n. 3
0
// Matrix-Vector algorithm for multiplying matrices
void mat_vec(float* A, float* B, int m, int p, int n, float* C)
{
  float* x = (float*) calloc(p,sizeof(float));
  float* y = (float*) calloc(m,sizeof(float));
  
  // If memory allocation fails then exit with error
  if(x == NULL || y == NULL) { exit(1); }
  
  int j;
  
  // Loop through columns of B
  for(j=0; j < n; j++)
  {
    // Get column j of B and load into x
    get_col(B,p,n,j,x);
      
    // Get column j of C and load into y
    get_col(C,m,n,j,y);
      
    // Saxpy level 2 operation y <- alpha*A*x + y
    cblas_sgemv(CblasRowMajor,CblasNoTrans,m,p,1.0,A,p,x,1,1.0,y,1);
      
    // Set column j of C to y
    set_col(C,m,n,j,y);
  }

  // Free intermediate values
  free(x);
  free(y);
}
Esempio n. 4
0
void				raytrace(t_env *env)
{
	int		x;
	int		y;
	t_ray	ray;
	t_col	col;

	y = 0;
	while (y <= WIN_Y)
	{
		x = 0;
		while (x <= WIN_X)
		{
			ray.start = OBJ.cam_s;
			ray.start.x += x;
			ray.start.y += y;
			ray.dir = OBJ.cam_dir;
			vector_norm(&ray.dir);
			set_col(&OBJ.col, 0, 0, 0);
			env->br = 0;
			col = shoot_ray(ray, 10, env);
			save_to_img(env, col, x, y);
			x++;
		}
		y++;
	}
}
Esempio n. 5
0
 mat get_cols(int start_col, int end_col){
   assert(start_col < end_offset - start_offset);
   assert(start_offset + end_col <= end_offset);
   mat retmat = zeros(end-start, end_col - start_col);
   for (int i=start_col; i< end_col; i++)
     set_col(retmat, i-start_col, this->operator[](i-start_col).to_vec());
   return retmat;
 }
Esempio n. 6
0
/***************************************************************
* int malloc_custom
*
****************************************************************/
void init_malloc_data() {

    if(system_memory_size == 0) {
        vprintf_custom("System memory size is 0.");
        print_vscreen();
        while(1) {}
    }

    bool show_data = true;
    unsigned int i = 0;
    unsigned int table_size = 0;
    unsigned int check = 0;

    memset_custom((char*)info, sizeof(info), 0);
    
    // Aloco el 10% de mi memoria total para mi 
    // registro de headers de memoria.
    table_size = (unsigned int)(system_memory_size * 0.1);
    malloc_registry = (memory_segment*)SAFE_ADDRESS;

    user_space_start = (byte*)(SAFE_ADDRESS + table_size + sizeof(memory_segment));
    user_memory_size = (unsigned int)(system_memory_size - (unsigned int)user_space_start);

    segments_amount = (unsigned int)(table_size / sizeof(memory_segment));
    minimum_segment_size = (unsigned int)(user_memory_size / segments_amount);

    memset_custom((char*)malloc_registry, table_size, 0);

    // Imprime las características del mapa de memoria.
    if(show_data == true) {
        clear_vscreen();
        set_row(3); set_col(0);
        vprintf_custom("Memory Table Size: %x", table_size);
        set_row(4); set_col(0);
        vprintf_custom("Malloc Registry Start: %x", malloc_registry);
        set_row(5); set_col(0);
        vprintf_custom("Total Memory Size: %x", system_memory_size);
        set_row(6); set_col(0);
        vprintf_custom("User Space Start: %x", user_space_start);
        set_row(7); set_col(0);
        vprintf_custom("User Memory Size: %x", user_memory_size);
        set_row(8); set_col(0);
        vprintf_custom("Number of Table Segments: %d", segments_amount);
        set_row(9); set_col(0);
        vprintf_custom("Minimum Allocable Space: %d(bytes)", minimum_segment_size);
        set_row(10); set_col(0);
        vprintf_custom("Memory Header Size: %x", sizeof(memory_segment));
        print_vscreen();
    }

    q_to_continue(SCREEN_LENGTH - 1, 2, 1);
}
Esempio n. 7
0
MAT	*m_inverse(const MAT *A, MAT *out)
{
  unsigned int	i;
  char MatrixTempBuffer[ 4000 ];
  VEC	*tmp = VNULL, *tmp2 = VNULL;
  MAT	*A_cp = MNULL;
  PERM	*pivot = PNULL;

  if ( ! A )
    error(E_NULL,"m_inverse");
  if ( A->m != A->n )
    error(E_SQUARE,"m_inverse");
  if ( ! out || out->m < A->m || out->n < A->n )
    out = m_resize(out,A->m,A->n);

  if( SET_MAT_SIZE( A->m, A->n ) < 1000 )
    mat_get( &A_cp, (void *)( MatrixTempBuffer + 2000 ), A->m, A->n );
  else
    A_cp = matrix_get( A->m, A->n  );

  A_cp = m_copy( A, A_cp );
  if( SET_VEC_SIZE( A->m ) < 1000 ) {
    vec_get( &tmp, (void *)MatrixTempBuffer, A->m );
    vec_get( &tmp2, (void *)(MatrixTempBuffer + 1000), A->m );
  } else {
    tmp   = v_get( A->m );
    tmp2  = v_get( A->m );
  }

  if( SET_PERM_SIZE( A->m ) < 1000 ) {
    perm_get( &pivot, (void *)( MatrixTempBuffer + 3000 ), A->m );
  } else {
    pivot   = px_get( A->m );
  }

  LUfactor(A_cp,pivot);
  //tracecatch_matrix(LUfactor(A_cp,pivot),"m_inverse");
  for ( i = 0; i < A->n; i++ ){
    v_zero(tmp);
    tmp->ve[i] = 1.0;
    LUsolve(A_cp,pivot,tmp,tmp2);
    //tracecatch_matrix(LUsolve(A_cp,pivot,tmp,tmp2),"m_inverse");
    set_col(out,i,tmp2);
  }
  if( tmp != (VEC *)(MatrixTempBuffer ) ) // память выделялась, надо освободить
    V_FREE(tmp);
  if( tmp2 != (VEC *)(MatrixTempBuffer + 1000) ) // память выделялась, надо освободить
    V_FREE(tmp2);
  if( A_cp != (MAT *)(MatrixTempBuffer + 2000) ) // память выделялась, надо освободить
    M_FREE(A_cp);
  if( pivot != (PERM *)(MatrixTempBuffer + 3000) ) // память выделялась, надо освободить
    PX_FREE( pivot );
  return out;
}
Esempio n. 8
0
void col_gradient_green(void)
{

    int x;
    for (x=0; x<WIDTH; x++)
    {
        set_col(x,0, 0,(int)(255.0*(float)x/(float)WIDTH));
        //set_col(x, 0,(int)(255.0*(float)x/(float)WIDTH),0);
        //set_col(x, (int)(255.0*(float)x/(float)WIDTH),0,0);
    }

    send();
}
Esempio n. 9
0
MAT	*makeQ(const MAT *QR,const VEC *diag, MAT *Qout)
#endif
{
    STATIC	VEC	*tmp1=VNULL,*tmp2=VNULL;
    unsigned int	i, limit;
    Real	beta, r_ii, tmp_val;
    int	j;
    
    limit = min(QR->m,QR->n);
    if ( ! QR || ! diag )
	error(E_NULL,"makeQ");
    if ( diag->dim < limit )
	error(E_SIZES,"makeQ");
    if ( Qout==(MAT *)NULL || Qout->m < QR->m || Qout->n < QR->m )
	Qout = m_get(QR->m,QR->m);
    
    tmp1 = v_resize(tmp1,QR->m);	/* contains basis vec & columns of Q */
    tmp2 = v_resize(tmp2,QR->m);	/* contains H/holder vectors */
    MEM_STAT_REG(tmp1,TYPE_VEC);
    MEM_STAT_REG(tmp2,TYPE_VEC);
    
    for ( i=0; i<QR->m ; i++ )
    {	/* get i-th column of Q */
	/* set up tmp1 as i-th basis vector */
	for ( j=0; j<QR->m ; j++ )
	    tmp1->ve[j] = 0.0;
	tmp1->ve[i] = 1.0;
	
	/* apply H/h transforms in reverse order */
	for ( j=limit-1; j>=0; j-- )
	{
	    get_col(QR,j,tmp2);
	    r_ii = fabs(tmp2->ve[j]);
	    tmp2->ve[j] = diag->ve[j];
	    tmp_val = (r_ii*fabs(diag->ve[j]));
	    beta = ( tmp_val == 0.0 ) ? 0.0 : 1.0/tmp_val;
	    /* hhtrvec(tmp2,beta->ve[j],j,tmp1,tmp1); */
	    hhtrvec(tmp2,beta,j,tmp1,tmp1);
	}
	
	/* insert into Q */
	set_col(Qout,i,tmp1);
    }

#ifdef	THREADSAFE
    V_FREE(tmp1);	V_FREE(tmp2);
#endif

    return (Qout);
}
Esempio n. 10
0
static MAT *calc_VinvIminAw(MAT *Vw, MAT *X, MAT *VinvIminAw, int calc_Aw) {
/*
 * calculate V_w^-1(I-A_w) (==VinvIminAw),
 * A = X(X'X)^-1 X' (AY = XBeta; Beta = (X'X)^-1 X'Y)
 *
 * on second thought (Nov 1998 -- more than 4 years later :-))
 * calc (I-Aw) only once and keep this constant during iteration.
 */
 	MAT *tmp = MNULL, *V = MNULL;
 	VEC *b = VNULL, *rhs = VNULL;
 	int i, j;

	if (X->m != Vw->n || VinvIminAw->m != X->m)
		ErrMsg(ER_IMPOSVAL, "calc_VinvIminAw: sizes don't match");
	
	if (calc_Aw) {
		IminAw = m_resize(IminAw, X->m, X->m);
		tmp = m_resize(tmp, X->n, X->n);
		tmp = mtrm_mlt(X, X, tmp); /* X'X */
		m_inverse(tmp, tmp); /* (X'X)-1 */
		/* X(X'X)-1 -> X(X'X)-1 X') */
		IminAw = XVXt_mlt(X, tmp, IminAw);
		for (i = 0; i < IminAw->m; i++) /* I - Aw */
			for (j = 0; j <= i; j++)
				if (i == j)
					IminAw->me[i][j] = 1.0 - IminAw->me[i][j];
				else
					IminAw->me[i][j] = IminAw->me[j][i] = -IminAw->me[i][j];
	}

	V = m_copy(Vw, V);
	LDLfactor(V);

	rhs = v_resize(rhs, X->m);
	b = v_resize(b, X->m);

	for (i = 0; i < X->m; i++) { /* solve Vw X = (I-A) for X -> V-1(I-A) */
		rhs = get_col(IminAw, i, rhs);
		LDLsolve(V, rhs, b);
		set_col(VinvIminAw, i, b);
	}
	v_free(rhs);
	v_free(b);
	m_free(V);

	if (tmp) 
		m_free(tmp);

	return VinvIminAw;
}
Esempio n. 11
0
MAT	*makeHQ(MAT *H, VEC *diag, VEC *beta, MAT *Qout)
#endif
{
	int	i, j, limit;
	STATIC	VEC	*tmp1 = VNULL, *tmp2 = VNULL;

	if ( H==(MAT *)NULL || diag==(VEC *)NULL || beta==(VEC *)NULL )
		error(E_NULL,"makeHQ");
	limit = H->m - 1;
	if ( diag->dim < limit || beta->dim < limit )
		error(E_SIZES,"makeHQ");
	if ( H->m != H->n )
		error(E_SQUARE,"makeHQ");
	Qout = m_resize(Qout,H->m,H->m);

	tmp1 = v_resize(tmp1,H->m);
	tmp2 = v_resize(tmp2,H->m);
	MEM_STAT_REG(tmp1,TYPE_VEC);
	MEM_STAT_REG(tmp2,TYPE_VEC);

	for ( i = 0; i < H->m; i++ )
	{
		/* tmp1 = i'th basis vector */
		for ( j = 0; j < H->m; j++ )
			/* tmp1->ve[j] = 0.0; */
		    v_set_val(tmp1,j,0.0);
		/* tmp1->ve[i] = 1.0; */
		v_set_val(tmp1,i,1.0);

		/* apply H/h transforms in reverse order */
		for ( j = limit-1; j >= 0; j-- )
		{
			get_col(H,(unsigned int)j,tmp2);
			/* tmp2->ve[j+1] = diag->ve[j]; */
			v_set_val(tmp2,j+1,v_entry(diag,j));
			hhtrvec(tmp2,beta->ve[j],j+1,tmp1,tmp1);
		}

		/* insert into Qout */
		set_col(Qout,(unsigned int)i,tmp1);
	}

#ifdef THREADSAFE
	V_FREE(tmp1);	V_FREE(tmp2);
#endif

	return (Qout);
}
Esempio n. 12
0
int main(int argc, char **argv){
  char b[SIZE];
  int **board;
  FILE *f;

  board = calloc(SIZE, sizeof(int));
  int i;
  for(i=0; i<SIZE; i++){
    if (board == NULL) {
      printf("Memory allocation failed\n");
      exit(-1);
    }
    board[i] = calloc(SIZE, sizeof(int));
  }

  f = fopen(argv[1], "r");
  while ( (fgets(b, SIZE , f) != NULL) ){
    char *comm = strtok(b, " ");
    int roc = 0;
    int val = 0;
    if( (strspn(comm, sc) == strlen(sc))){
      roc = atoi(strtok(NULL, " "));
      val = atoi(strtok(NULL, " "));
      set_col(board, roc, val);
    }else if( (strspn(comm, sr)) == strlen(sr) ){
      roc = atoi(strtok(NULL, " "));
      val = atoi(strtok(NULL, " "));
      set_row(board, roc, val);
    }else if( (strspn(comm, qc)) == strlen(qc) ){
      roc = atoi(strtok(NULL, " "));
      printf("%d\n", query_col(board, roc));
    }else if( (strspn(comm, qr)) == strlen(qr) ){
      roc = atoi(strtok(NULL, " "));
      printf("%d\n", query_row(board, roc));
    }

  }

  for(i=0; i<SIZE; i++){
    free(board[i]);
  }
  free(board);
  fclose(f);
}
Esempio n. 13
0
MAT	*m_inverse(const MAT *A, MAT *out)
#endif
{
	int	i;
	STATIC VEC	*tmp = VNULL, *tmp2 = VNULL;
	STATIC MAT	*A_cp = MNULL;
	STATIC PERM	*pivot = PNULL;

	if ( ! A )
	    error(E_NULL,"m_inverse");
	if ( A->m != A->n )
	    error(E_SQUARE,"m_inverse");
	if ( ! out || out->m < A->m || out->n < A->n )
	    out = m_resize(out,A->m,A->n);

	A_cp = m_resize(A_cp,A->m,A->n);
	A_cp = m_copy(A,A_cp);
	tmp = v_resize(tmp,A->m);
	tmp2 = v_resize(tmp2,A->m);
	pivot = px_resize(pivot,A->m);
	MEM_STAT_REG(A_cp,TYPE_MAT);
	MEM_STAT_REG(tmp, TYPE_VEC);
	MEM_STAT_REG(tmp2,TYPE_VEC);
	MEM_STAT_REG(pivot,TYPE_PERM);
	tracecatch(LUfactor(A_cp,pivot),"m_inverse");
	for ( i = 0; i < A->n; i++ )
	{
	    v_zero(tmp);
	    tmp->ve[i] = 1.0;
	    tracecatch(LUsolve(A_cp,pivot,tmp,tmp2),"m_inverse");
	    set_col(out,i,tmp2);
	}

#ifdef	THREADSAFE
	V_FREE(tmp);	V_FREE(tmp2);
	M_FREE(A_cp);	PX_FREE(pivot);
#endif

	return out;
}
Esempio n. 14
0
int call_tabla(void *nombre, int argc, char **argv, char **azColName)
{
		char tmp[255];
		char *formato="<L></B/5>%s";
		char *coltitle[10], *rowtitle[10], *mesg[10];
		int colwidth[10], colvalue[10];
		int i=0;
	    //char *titulo="<L></B/5>%s";
	    char titulo[255];
	    //sprintf(&titulo,nombre);
		CDKSCREEN *pantalla;
		initscr();
		pantalla=initCDKScreen(stdscr);
	CDKMATRIX *matriz;
	sprintf(titulo,formato,nombre);
	for(i=0;i<argc;i++)
	{
	sprintf(tmp,formato,azColName[i]);
	rowtitle[i+1]=copyChar(tmp);

	printf(tmp);
	//	set_row(i+1,azColName[i]);
	}
set_col(1,57,titulo);
	matriz=newCDKMatrix(
						pantalla,
						CENTER,CENTER,
						argc,1,argc,1,
						"",rowtitle,coltitle,colwidth,colvalue,-1,-1,'.',COL,TRUE,FALSE,FALSE

					);
	activateCDKMatrix(matriz,0);
/* 	for(i=0;i<argc;i++)
	{
		printf("->%s: %s\n",azColName[i],argv[i]);
	}
	printf("%s",(char*)nombre);*/
return 0;
}
Esempio n. 15
0
void ClearScreen(){
  unsigned char page;
  u16 i,j;
  page=0xB0;  		
  for(i=0;i<1024;i+=128)  {
      P5OUT &= ~AA0;	  	
      P4OUT=page;           	//page number Setting
      P5OUT &= ~AWRN;	
      P5OUT |= AWRN;	
      set_col();             	// setup low/high column
      P5OUT |= AA0;
      for(j=0;j<128;j++){	
        //page_data=pic[pic_index][j+i*128];
        //page_data=DC[j+(i<<7)];
        P4OUT=0x00;
        //P4OUT=0X03;
        P5OUT &= ~AWRN;	
        P5OUT |= AWRN;	 	
      }
      page++;
  }
}
Esempio n. 16
0
// Saxpy operations for multiplying matrices
void mat_saxpy(float* A, float* B, int m, int p, int n, float* C)
{
  float* x = (float*) calloc(m,sizeof(float));
  float* y = (float*) calloc(m,sizeof(float));
  
  // If memory allocation fails then exit with error
  if(x == NULL || y == NULL) { exit(1); }
  
  int j,k;
  
  // Loop through columns of B
  for(j=0; j < n; j++)
  {
    // Loop through columns of A
    for(k=0; k < p; k++)
    {
      // Get column k of A and load into x
      get_col(A,m,p,k,x);
      
      // Get column j of C and load into y
      get_col(C,m,n,j,y);
      
      // Get alpha
      float alpha = B[k*n+j];
      
      // Saxpy level 1 operation y <- alpha*x + y
      cblas_saxpy(m,alpha,x,1,y,1);
      
      // Set column j of C to y
      set_col(C,m,n,j,y);
    }
  }

  // Free intermediate values
  free(x);
  free(y);
}
Esempio n. 17
0
void ShowProgress(u8 progress){
  unsigned char page;
  u16 j;
  //ClearScreen();
  page=0xB0;  		
  P5OUT &= ~AA0;	  	
  P4OUT=page;           	//page number Setting
  P5OUT &= ~AWRN;	
  P5OUT |= AWRN;	
  set_col();             	// setup low/high column
  P5OUT |= AA0;
  for(j=0;j<128;j++){	
    //page_data=pic[pic_index][j+i*128];
    //page_data=DC[j+(i<<7)];
    if (j<=progress){
      P4OUT=0xFF;
    }else{
      P4OUT=0x00;
    };
    //P4OUT=0X03;
    P5OUT &= ~AWRN;	
    P5OUT |= AWRN;	 	
  }
}
Esempio n. 18
0
void	get_col(t_env *env, t_col *col)
{
	if (env->info.wallside == 0)
	{
		if (env->wall_type == 0)
			if (env->info.step.x < 0)
				set_col(col, 51, 0, 102);
			else
				set_col(col, 0, 0, 102);
		else
			set_col(col, 255, 215, 0);
	}
	else if (env->info.wallside == 1)
	{
		if (env->wall_type == 0)
			if (env->info.step.y < 0)
				set_col(col, 0, 51, 0);
			else
				set_col(col, 192, 192, 192);
		else
			set_col(col, 255, 215, 0);
	}
}
Esempio n. 19
0
VEC	*iter_gmres(ITER *ip)
#endif
{
    STATIC VEC *u=VNULL, *r=VNULL, *rhs = VNULL;
    STATIC VEC *givs=VNULL, *givc=VNULL, *z = VNULL;
    STATIC MAT *Q = MNULL, *R = MNULL;
    VEC *rr, v, v1;   /* additional pointers (not real vectors) */
    int	i,j, done;
    Real	nres;
    /*   Real last_h;  */

    if (ip == INULL)
        error(E_NULL,"iter_gmres");
    if ( ! ip->Ax || ! ip->b )
        error(E_NULL,"iter_gmres");
    if ( ! ip->stop_crit )
        error(E_NULL,"iter_gmres");
    if ( ip->k <= 0 )
        error(E_BOUNDS,"iter_gmres");
    if (ip->x != VNULL && ip->x->dim != ip->b->dim)
        error(E_SIZES,"iter_gmres");
    if (ip->eps <= 0.0) ip->eps = MACHEPS;

    r = v_resize(r,ip->k+1);
    u = v_resize(u,ip->b->dim);
    rhs = v_resize(rhs,ip->k+1);
    givs = v_resize(givs,ip->k);  /* Givens rotations */
    givc = v_resize(givc,ip->k);

    MEM_STAT_REG(r,TYPE_VEC);
    MEM_STAT_REG(u,TYPE_VEC);
    MEM_STAT_REG(rhs,TYPE_VEC);
    MEM_STAT_REG(givs,TYPE_VEC);
    MEM_STAT_REG(givc,TYPE_VEC);

    R = m_resize(R,ip->k+1,ip->k);
    Q = m_resize(Q,ip->k,ip->b->dim);
    MEM_STAT_REG(R,TYPE_MAT);
    MEM_STAT_REG(Q,TYPE_MAT);

    if (ip->x == VNULL) {  /* ip->x == 0 */
        ip->x = v_get(ip->b->dim);
        ip->shared_x = FALSE;
    }

    v.dim = v.max_dim = ip->b->dim;      /* v and v1 are pointers to rows */
    v1.dim = v1.max_dim = ip->b->dim;  	/* of matrix Q */

    if (ip->Bx != (Fun_Ax)NULL) {    /* if precondition is defined */
        z = v_resize(z,ip->b->dim);
        MEM_STAT_REG(z,TYPE_VEC);
    }

    done = FALSE;
    for (ip->steps = 0; ip->steps < ip->limit; ) {

        /* restart */

        ip->Ax(ip->A_par,ip->x,u);    		/* u = A*x */
        v_sub(ip->b,u,u);		 		/* u = b - A*x */
        rr = u;				/* rr is a pointer only */

        if (ip->Bx) {
            (ip->Bx)(ip->B_par,u,z);            /* tmp = B*(b-A*x)  */
            rr = z;
        }

        nres = v_norm2(rr);
        if (ip->steps == 0) {
            if (ip->info) ip->info(ip,nres,VNULL,VNULL);
            ip->init_res = nres;
        }

        if ( nres == 0.0 ) {
            done = TRUE;
            break;
        }

        v.ve = Q->me[0];
        sv_mlt(1.0/nres,rr,&v);

        v_zero(r);
        v_zero(rhs);
        rhs->ve[0] = nres;

        for ( i = 0; i < ip->k && ip->steps < ip->limit; i++ ) {
            ip->steps++;
            v.ve = Q->me[i];
            (ip->Ax)(ip->A_par,&v,u);
            rr = u;
            if (ip->Bx) {
                (ip->Bx)(ip->B_par,u,z);
                rr = z;
            }

            if (i < ip->k - 1) {
                v1.ve = Q->me[i+1];
                v_copy(rr,&v1);
                for (j = 0; j <= i; j++) {
                    v.ve = Q->me[j];
                    /* r->ve[j] = in_prod(&v,rr); */
                    /* modified Gram-Schmidt algorithm */
                    r->ve[j] = in_prod(&v,&v1);
                    v_mltadd(&v1,&v,-r->ve[j],&v1);
                }

                r->ve[i+1] = nres = v_norm2(&v1);
                if (nres <= MACHEPS*ip->init_res) {
                    for (j = 0; j < i; j++)
                        rot_vec(r,j,j+1,givc->ve[j],givs->ve[j],r);
                    set_col(R,i,r);
                    done = TRUE;
                    break;
                }
                sv_mlt(1.0/nres,&v1,&v1);
            }
            else {  /* i == ip->k - 1 */
                /* Q->me[ip->k] need not be computed */

                for (j = 0; j <= i; j++) {
                    v.ve = Q->me[j];
                    r->ve[j] = in_prod(&v,rr);
                }

                nres = in_prod(rr,rr) - in_prod(r,r);
                if (sqrt(fabs(nres)) <= MACHEPS*ip->init_res) {
                    for (j = 0; j < i; j++)
                        rot_vec(r,j,j+1,givc->ve[j],givs->ve[j],r);
                    set_col(R,i,r);
                    done = TRUE;
                    break;
                }
                if (nres < 0.0) { /* do restart */
                    i--;
                    ip->steps--;
                    break;
                }
                r->ve[i+1] = sqrt(nres);
            }

            /* QR update */

            /* last_h = r->ve[i+1]; */ /* for test only */
            for (j = 0; j < i; j++)
                rot_vec(r,j,j+1,givc->ve[j],givs->ve[j],r);
            givens(r->ve[i],r->ve[i+1],&givc->ve[i],&givs->ve[i]);
            rot_vec(r,i,i+1,givc->ve[i],givs->ve[i],r);
            rot_vec(rhs,i,i+1,givc->ve[i],givs->ve[i],rhs);

            set_col(R,i,r);

            nres = fabs((double) rhs->ve[i+1]);
            if (ip->info) ip->info(ip,nres,VNULL,VNULL);
            if ( ip->stop_crit(ip,nres,VNULL,VNULL) ) {
                done = TRUE;
                break;
            }
        }

        /* use ixi submatrix of R */

        if (i >= ip->k) i = ip->k - 1;

        R = m_resize(R,i+1,i+1);
        rhs = v_resize(rhs,i+1);

        /* test only */
        /* test_gmres(ip,i,Q,R,givc,givs,last_h);  */

        Usolve(R,rhs,rhs,0.0); 	 /* solve a system: R*x = rhs */

        /* new approximation */

        for (j = 0; j <= i; j++) {
            v.ve = Q->me[j];
            v_mltadd(ip->x,&v,rhs->ve[j],ip->x);
        }

        if (done) break;

        /* back to old dimensions */

        rhs = v_resize(rhs,ip->k+1);
        R = m_resize(R,ip->k+1,ip->k);

    }

#ifdef THREADSAFE
    V_FREE(u);
    V_FREE(r);
    V_FREE(rhs);
    V_FREE(givs);
    V_FREE(givc);
    V_FREE(z);
    M_FREE(Q);
    M_FREE(R);
#endif

    return ip->x;
}
Esempio n. 20
0
MAT	*iter_arnoldi(ITER *ip, Real *h_rem, MAT *Q, MAT *H)
#endif
{
    STATIC VEC *u=VNULL, *r=VNULL;
    VEC v;     /* auxiliary vector */
    int	i,j;
    Real	h_val, c;

    if (ip == INULL)
        error(E_NULL,"iter_arnoldi");
    if ( ! ip->Ax || ! Q || ! ip->x )
        error(E_NULL,"iter_arnoldi");
    if ( ip->k <= 0 )
        error(E_BOUNDS,"iter_arnoldi");
    if ( Q->n != ip->x->dim ||	Q->m != ip->k )
        error(E_SIZES,"iter_arnoldi");

    m_zero(Q);
    H = m_resize(H,ip->k,ip->k);
    m_zero(H);

    u = v_resize(u,ip->x->dim);
    r = v_resize(r,ip->k);
    MEM_STAT_REG(u,TYPE_VEC);
    MEM_STAT_REG(r,TYPE_VEC);

    v.dim = v.max_dim = ip->x->dim;

    c = v_norm2(ip->x);
    if ( c <= 0.0)
        return H;
    else {
        v.ve = Q->me[0];
        sv_mlt(1.0/c,ip->x,&v);
    }

    v_zero(r);
    for ( i = 0; i < ip->k; i++ )
    {
        v.ve = Q->me[i];
        u = (ip->Ax)(ip->A_par,&v,u);
        for (j = 0; j <= i; j++) {
            v.ve = Q->me[j];
            /* modified Gram-Schmidt */
            r->ve[j] = in_prod(&v,u);
            v_mltadd(u,&v,-r->ve[j],u);
        }
        h_val = v_norm2(u);
        /* if u == 0 then we have an exact subspace */
        if ( h_val <= 0.0 )
        {
            *h_rem = h_val;
            return H;
        }
        set_col(H,i,r);
        if ( i == ip->k-1 )
        {
            *h_rem = h_val;
            continue;
        }
        /* H->me[i+1][i] = h_val; */
        m_set_val(H,i+1,i,h_val);
        v.ve = Q->me[i+1];
        sv_mlt(1.0/h_val,u,&v);
    }

#ifdef THREADSAFE
    V_FREE(u);
    V_FREE(r);
#endif

    return H;
}
Esempio n. 21
0
MAT	*iter_arnoldi_iref(ITER *ip, Real *h_rem, MAT *Q, MAT *H)
#endif
{
    STATIC VEC *u=VNULL, *r=VNULL, *s=VNULL, *tmp=VNULL;
    VEC v;     /* auxiliary vector */
    int	i,j;
    Real	h_val, c;

    if (ip == INULL)
        error(E_NULL,"iter_arnoldi_iref");
    if ( ! ip->Ax || ! Q || ! ip->x )
        error(E_NULL,"iter_arnoldi_iref");
    if ( ip->k <= 0 )
        error(E_BOUNDS,"iter_arnoldi_iref");
    if ( Q->n != ip->x->dim ||	Q->m != ip->k )
        error(E_SIZES,"iter_arnoldi_iref");

    m_zero(Q);
    H = m_resize(H,ip->k,ip->k);
    m_zero(H);

    u = v_resize(u,ip->x->dim);
    r = v_resize(r,ip->k);
    s = v_resize(s,ip->k);
    tmp = v_resize(tmp,ip->x->dim);
    MEM_STAT_REG(u,TYPE_VEC);
    MEM_STAT_REG(r,TYPE_VEC);
    MEM_STAT_REG(s,TYPE_VEC);
    MEM_STAT_REG(tmp,TYPE_VEC);

    v.dim = v.max_dim = ip->x->dim;

    c = v_norm2(ip->x);
    if ( c <= 0.0)
        return H;
    else {
        v.ve = Q->me[0];
        sv_mlt(1.0/c,ip->x,&v);
    }

    v_zero(r);
    v_zero(s);
    for ( i = 0; i < ip->k; i++ )
    {
        v.ve = Q->me[i];
        u = (ip->Ax)(ip->A_par,&v,u);
        for (j = 0; j <= i; j++) {
            v.ve = Q->me[j];
            /* modified Gram-Schmidt */
            r->ve[j] = in_prod(&v,u);
            v_mltadd(u,&v,-r->ve[j],u);
        }
        h_val = v_norm2(u);
        /* if u == 0 then we have an exact subspace */
        if ( h_val <= 0.0 )
        {
            *h_rem = h_val;
            return H;
        }
        /* iterative refinement -- ensures near orthogonality */
        do {
            v_zero(tmp);
            for (j = 0; j <= i; j++) {
                v.ve = Q->me[j];
                s->ve[j] = in_prod(&v,u);
                v_mltadd(tmp,&v,s->ve[j],tmp);
            }
            v_sub(u,tmp,u);
            v_add(r,s,r);
        } while ( v_norm2(s) > 0.1*(h_val = v_norm2(u)) );
        /* now that u is nearly orthogonal to Q, update H */
        set_col(H,i,r);
        /* check once again if h_val is zero */
        if ( h_val <= 0.0 )
        {
            *h_rem = h_val;
            return H;
        }
        if ( i == ip->k-1 )
        {
            *h_rem = h_val;
            continue;
        }
        /* H->me[i+1][i] = h_val; */
        m_set_val(H,i+1,i,h_val);
        v.ve = Q->me[i+1];
        sv_mlt(1.0/h_val,u,&v);
    }

#ifdef THREADSAFE
    V_FREE(u);
    V_FREE(r);
    V_FREE(s);
    V_FREE(tmp);
#endif

    return H;
}
Esempio n. 22
0
/***************************************************************
*   void malloc_tests
*      
*
****************************************************************/
void malloc_tests() {
    
    unsigned int row = 3;
    unsigned int col = 3;

    unsigned int size1 = 30000000;
    unsigned int* test_ptr1 = malloc_custom(size1, 0);
    set_row(row); set_col(col);
    vprintf_custom("1 Asked: %d, starts at: %x", size1, test_ptr1);
    set_row(row++); set_col(col + 40);
    vprintf_custom("mem left: %d", user_memory_size - allocated_space);

    unsigned int size2 = 1000;
    unsigned int* test_ptr2 = malloc_custom(size2, 0);
    set_row(row); set_col(col);
    vprintf_custom("2 Asked: %d, starts at: %x", size2, test_ptr2);
    set_row(row++); set_col(col + 40);
    vprintf_custom("mem left: %d", user_memory_size - allocated_space);

    unsigned int size3 = 0x00A00000; // 10MB
    unsigned int* test_ptr3 = malloc_custom(size3, 0);
    set_row(row); set_col(col);
    vprintf_custom("3 Asked: %d, starts at: %x", size3, test_ptr3);
    set_row(row++); set_col(col + 40);
    vprintf_custom("mem left: %d", user_memory_size - allocated_space);

    unsigned int size4 = 0x00A00000; // 10MB
    unsigned int* test_ptr4 = malloc_custom(size4, 0);
    set_row(row); set_col(col);
    vprintf_custom("4 Asked: %d, starts at: %x", size4, test_ptr4);
    set_row(row++); set_col(col + 40);
    vprintf_custom("mem left: %d", user_memory_size - allocated_space);

    unsigned int size5 = 0x00100000; // 1MB
    unsigned int* test_ptr5 = malloc_custom(size5, 1);
    set_row(row); set_col(col);
    vprintf_custom("5 Asked: %d, starts at: %x", size5, test_ptr5);
    set_row(row++); set_col(col + 40);
    vprintf_custom("mem left: %d", user_memory_size - allocated_space);

    unsigned int size6 = 0x00100000; // 1MB
    unsigned int* test_ptr6 = malloc_custom(size6, 0);
    set_row(row); set_col(col);
    vprintf_custom("6 Asked: %d, starts at: %x", size6, test_ptr6);
    set_row(row++); set_col(col + 40);
    vprintf_custom("mem left: %d", user_memory_size - allocated_space);

    unsigned int size7 = 0x00100000; // 1MB
    unsigned int* test_ptr7 = malloc_custom(size7, 0);
    set_row(row); set_col(col);
    vprintf_custom("7 Asked: %d, starts at: %x", size7, test_ptr7);
    set_row(row++); set_col(col + 40);
    vprintf_custom("mem left: %d", user_memory_size - allocated_space);

    unsigned int size8 = 0x00400000; // 4MB
    unsigned int* test_ptr8 = malloc_custom(size8, 0);
    set_row(row); set_col(col);
    vprintf_custom("8 Asked: %d, starts at: %x", size8, test_ptr8);
    set_row(row++); set_col(col + 40);
    vprintf_custom("mem left: %d", user_memory_size - allocated_space);

    unsigned int size9 = 0x00100000; // 64KB
    unsigned int* test_ptr9 = malloc_custom(size9, 0);
    set_row(row); set_col(col);
    vprintf_custom("9 Asked: %d, starts at: %x", size9, test_ptr9);
    set_row(row++); set_col(col + 40);
    vprintf_custom("mem left: %d", user_memory_size - allocated_space);

    unsigned int size10 = 0x00100000; // 64KB
    unsigned int* test_ptr10 = malloc_custom(size10, 0);
    set_row(row); set_col(col);
    vprintf_custom("10 Asked: %d, starts at: %x", size10, test_ptr10);
    set_row(row++); set_col(col + 40);
    vprintf_custom("mem left: %d", user_memory_size - allocated_space);

    unsigned int size11 = 0x00100000; // 64KB
    unsigned int* test_ptr11 = malloc_custom(size11, 0);
    set_row(row); set_col(col);
    vprintf_custom("11 Asked: %d, starts at: %x", size11, test_ptr11);
    set_row(row++); set_col(col + 40);
    vprintf_custom("mem left: %d", user_memory_size - allocated_space);

    free(test_ptr6);
    set_row(row); set_col(col);
    vprintf_custom("Released: %x (bytes)", size7);
    set_row(row++); set_col(col + 40);
    vprintf_custom("mem left: %d", user_memory_size - allocated_space);

    free(test_ptr7);
    set_row(row); set_col(col);
    vprintf_custom("Released: %x (bytes)", size7);
    set_row(row++); set_col(col + 40);
    vprintf_custom("mem left: %d", user_memory_size - allocated_space);

    test_ptr9 = malloc_custom(size9, 0);
    set_row(row); set_col(col);
    vprintf_custom("12 Asked: %d, starts at: %x", size9, test_ptr9);
    set_row(row++); set_col(col + 40);
    vprintf_custom("mem left: %d", user_memory_size - allocated_space);


    print_vscreen();
}
Esempio n. 23
0
void Data_Elise_Gra_Win::set_fill_style(Data_Fill_St * dfst)
{
     set_active();
     set_col(dfst->dcp());
}
Esempio n. 24
0
void Data_Elise_Gra_Win::set_line_style(Data_Line_St * dlst)
{
     set_active();
     _degd->set_line_witdh(dlst->witdh());
      set_col(dlst->dcp());
}
Esempio n. 25
0
/*
 * This program demonstrates the Cdk matrix widget.
 */
int main (int argc, char **argv)
{
   /* *INDENT-EQLS* */
   CDKSCREEN *cdkscreen = 0;
   CDKMATRIX *courseList = 0;
   WINDOW *cursesWin    = 0;
   const char *title    = 0;
   int rows             = 8;
   int cols             = 5;
   int vrows            = 3;
   int vcols            = 5;

   bool use_coltitles;
   bool use_rowtitles;

   const char *coltitle[MY_COLS];
   const char *rowtitle[MY_COLS];
   const char *mesg[MY_COLS];

   int colwidth[MY_COLS];
   int colvalue[MY_COLS];

   CDK_PARAMS params;

   CDKparseParams (argc, argv, &params, "trcT:" CDK_MIN_PARAMS);

   /* invert, so giving -S causes the shadow to turn off */
   params.Shadow = !params.Shadow;

   /* cancel the default title, or supply a new one */
   if (CDKparamValue (&params, 't', FALSE))
   {
      title = 0;
   }
   else if ((title = CDKparamString (&params, 'T')) == 0)
   {
      title = "<C>This is the CDK\n<C>matrix widget.\n<C><#LT><#HL(30)><#RT>";
   }

   /* allow cancelling of column and/or row titles with -c and/or -r */
   use_coltitles = !CDKparamValue (&params, 'c', FALSE);
   use_rowtitles = !CDKparamValue (&params, 'r', FALSE);

   /* Set up CDK. */
   cursesWin = initscr ();
   cdkscreen = initCDKScreen (cursesWin);

   /* Start CDK Colors. */
   initCDKColor ();

   /* Create the horizontal and vertical matrix labels. */
#define set_col(n, width, string) \
   coltitle[n] = use_coltitles ? string : 0 ;\
   colwidth[n] = width ;\
   colvalue[n] = vUMIXED

   set_col (1, 7, "</B/5>Course");
   set_col (2, 7, "</B/33>Lec 1");
   set_col (3, 7, "</B/33>Lec 2");
   set_col (4, 7, "</B/33>Lec 3");
   set_col (5, 1, "</B/7>Flag");

#define set_row(n, string) \
   rowtitle[n] = use_rowtitles ? "<C></B/6>" string : 0

   set_row (1, "Course 1");
   set_row (2, "Course 2");
   set_row (3, "Course 3");
   set_row (4, "Course 4");
   set_row (5, "Course 5");
   set_row (6, "Course 6");
   set_row (7, "Course 7");
   set_row (8, "Course 8");

   /* Create the matrix object. */
   courseList = newCDKMatrix (cdkscreen,
			      CDKparamValue (&params, 'X', CENTER),
			      CDKparamValue (&params, 'Y', CENTER),
			      rows, cols, vrows, vcols,
			      title,
			      (CDK_CSTRING2) rowtitle,
			      (CDK_CSTRING2) coltitle,
			      colwidth, colvalue,
			      -1, -1, '.',
			      COL, params.Box,
			      params.Box,
			      params.Shadow);

   /* Check to see if the matrix is null. */
   if (courseList == 0)
   {
      /* Clean up. */
      destroyCDKScreen (cdkscreen);
      endCDK ();

      printf ("Cannot create the matrix widget.\n");
      printf ("Is the window too small ?\n");
      ExitProgram (EXIT_FAILURE);
   }

   /* Activate the matrix. */
   activateCDKMatrix (courseList, 0);

   /* Check if the user hit escape or not. */
   if (courseList->exitType == vESCAPE_HIT)
   {
      mesg[0] = "<C>You hit escape. No information passed back.";
      mesg[1] = "";
      mesg[2] = "<C>Press any key to continue.";
      popupLabel (cdkscreen, (CDK_CSTRING2) mesg, 3);
   }
   else if (courseList->exitType == vNORMAL)
   {
      char temp[80];

      sprintf (temp, "Current cell (%d,%d)", courseList->crow, courseList->ccol);
      mesg[0] = "<L>You exited the matrix normally.";
      mesg[1] = temp;
      mesg[2] = "<L>To get the contents of the matrix cell, you can";
      mesg[3] = "<L>use getCDKMatrixCell():";
      mesg[4] = getCDKMatrixCell (courseList, courseList->crow, courseList->ccol);
      mesg[5] = "";
      mesg[6] = "<C>Press any key to continue.";
      popupLabel (cdkscreen, (CDK_CSTRING2) mesg, 7);
   }

   /* Clean up. */
   destroyCDKMatrix (courseList);
   destroyCDKScreen (cdkscreen);
   endCDK ();
   ExitProgram (EXIT_SUCCESS);
}
Esempio n. 26
0
void Ukf(VEC *omega, VEC *mag_vec, VEC *mag_vec_I, VEC *sun_vec, VEC *sun_vec_I, VEC *Torq_ext, double t, double h, int eclipse, VEC *state, VEC *st_error, VEC *residual, int *P_flag, double sim_time)
{
    static VEC *omega_prev = VNULL, *mag_vec_prev = VNULL, *sun_vec_prev = VNULL, *q_s_c = VNULL, *x_prev = VNULL, *Torq_prev, *x_m_o;
    static MAT *Q = {MNULL}, *R = {MNULL}, *Pprev = {MNULL};
    static double alpha, kappa, lambda, sqrt_lambda, w_m_0, w_c_0, w_i, beta;
    static int n_states, n_sig_pts, n_err_states, iter_num, initialize=0;
    
    VEC *x = VNULL, *x_priori = VNULL,  *x_err_priori = VNULL,  *single_sig_pt = VNULL, *v_temp = VNULL, *q_err_quat = VNULL,
            *err_vec = VNULL, *v_temp2 = VNULL, *x_ang_vel = VNULL, *meas = VNULL, *meas_priori = VNULL,
            *v_temp3 = VNULL, *x_posteriori_err = VNULL, *x_b_m = VNULL, *x_b_g = VNULL;
    MAT *sqrt_P = {MNULL}, *P = {MNULL}, *P_priori = {MNULL}, *sig_pt = {MNULL}, *sig_vec_mat = {MNULL},
            *err_sig_pt_mat = {MNULL}, *result = {MNULL}, *result_larger = {MNULL}, *result1 = {MNULL}, *Meas_err_mat = {MNULL},
            *P_zz = {MNULL}, *iP_vv = {MNULL}, *P_xz = {MNULL}, *K = {MNULL}, *result2 = {MNULL}, *result3 = {MNULL}, *C = {MNULL};
    
    int update_mag_vec, update_sun_vec, update_omega, i, j;
    double d_res;

    if (inertia == MNULL)
	{
		inertia = m_get(3,3);
		m_ident(inertia);
		inertia->me[0][0] = 0.007;
		inertia->me[1][1] = 0.014;
		inertia->me[2][2] = 0.015;
	}

    if (initialize == 0){
        iter_num = 1;
		n_states = (7+6);
        n_err_states = (6+6);
        n_sig_pts = 2*n_err_states+1;
        alpha = sqrt(3);
        kappa = 3 - n_states;
        lambda = alpha*alpha * (n_err_states+kappa) - n_err_states;
        beta = -(1-(alpha*alpha)); 
        w_m_0 = (lambda)/(n_err_states + lambda);
        w_c_0 = (lambda/(n_err_states + lambda)) + (1 - (alpha*alpha) + beta);
        w_i = 0.5/(n_err_states +lambda);
        initialize = 1;
        sqrt_lambda = (lambda+n_err_states);
        if(q_s_c == VNULL)
        {
            q_s_c = v_get(4);
            
            q_s_c->ve[0] = -0.020656;
            q_s_c->ve[1] = 0.71468;
            q_s_c->ve[2] = -0.007319;
            q_s_c->ve[3] = 0.6991;
        }
        if(Torq_prev == VNULL)
        {
            Torq_prev = v_get(3);
            v_zero(Torq_prev);
        }
        
        quat_normalize(q_s_c);
		
    }
      

    result = m_get(9,9);
    m_zero(result);
        
    result1 = m_get(n_err_states, n_err_states);
    m_zero(result1);
        
    if(x_m_o == VNULL)
	{
		x_m_o = v_get(n_states);
		v_zero(x_m_o);     
	}
	
	x = v_get(n_states);
    v_zero(x);
    
    
    x_err_priori = v_get(n_err_states);
    v_zero(x_err_priori);
    
    x_ang_vel = v_get(3);
    v_zero(x_ang_vel);
    
    sig_pt = m_get(n_states, n_err_states);
    m_zero(sig_pt);
    
    
	if (C == MNULL)
    {
        C = m_get(9, 12);
        m_zero(C);
    }    

    
    if (P_priori == MNULL)
    {
        P_priori = m_get(n_err_states, n_err_states);
        m_zero(P_priori);
    }
    
	
    if (Q == MNULL)
    {
        Q = m_get(n_err_states, n_err_states); 
        m_ident(Q);
        //
        Q->me[0][0] = 0.0001;
        Q->me[1][1] = 0.0001;
        Q->me[2][2] = 0.0001;
		
        Q->me[3][3] = 0.0001;
        Q->me[4][4] = 0.0001;
        Q->me[5][5] = 0.0001;

        Q->me[6][6] = 0.000001;
        Q->me[7][7] = 0.000001;
        Q->me[8][8] = 0.000001;

        Q->me[9][9]   = 0.000001;
        Q->me[10][10] = 0.000001;
        Q->me[11][11] = 0.000001;
	}

    

    if( Pprev == MNULL)
    {
        Pprev = m_get(n_err_states, n_err_states); 
        m_ident(Pprev);
		
        Pprev->me[0][0] = 1e-3;
        Pprev->me[1][1] = 1e-3;
        Pprev->me[2][2] = 1e-3;
        Pprev->me[3][3] = 1e-3;
        Pprev->me[4][4] = 1e-3;
        Pprev->me[5][5] = 1e-3;
        Pprev->me[6][6] = 1e-4;
        Pprev->me[7][7] = 1e-4;
        Pprev->me[8][8] = 1e-4;
        Pprev->me[9][9] =	1e-3;
        Pprev->me[10][10] = 1e-3;
        Pprev->me[11][11] = 1e-3;
    }



    if (R == MNULL)
    {
        R = m_get(9,9);
        m_ident(R);
    
        R->me[0][0] = 0.034;
        R->me[1][1] = 0.034;
        R->me[2][2] = 0.034;
        
        R->me[3][3] = 0.00027;
        R->me[4][4] = 0.00027;
        R->me[5][5] = 0.00027;
        
        R->me[6][6] = 0.000012;
        R->me[7][7] = 0.000012;
        R->me[8][8] = 0.000012;
    }

	if(eclipse==0)
	{
		R->me[0][0] = 0.00034;
        R->me[1][1] = 0.00034;
        R->me[2][2] = 0.00034;
        
        R->me[3][3] = 0.00027;
        R->me[4][4] = 0.00027;
        R->me[5][5] = 0.00027;
        
        R->me[6][6] = 0.0000012;
        R->me[7][7] = 0.0000012;
        R->me[8][8] = 0.0000012;


		Q->me[0][0] =	0.00001;
        Q->me[1][1] =	0.00001;
        Q->me[2][2] =	0.00001;

        Q->me[3][3] =	0.0001;//0.000012;//0.0175;//1e-3; 
        Q->me[4][4] =	0.0001;//0.0175;//1e-3;
        Q->me[5][5] =	0.0001;//0.0175;//1e-3;

        Q->me[6][6] =	0.0000000001;//1e-6;
        Q->me[7][7] =	0.0000000001;
        Q->me[8][8] =	0.0000000001;

        Q->me[9][9]   =	0.0000000001;
        Q->me[10][10] = 0.0000000001;
        Q->me[11][11] = 0.0000000001;
	}    
	else
	{
		R->me[0][0] = 0.34;
        R->me[1][1] = 0.34;
        R->me[2][2] = 0.34;

        R->me[3][3] =	0.0027;
        R->me[4][4] =	0.0027;
        R->me[5][5] =	0.0027;
        
        R->me[6][6] =	0.0000012;
        R->me[7][7] =	0.0000012;
        R->me[8][8] =	0.0000012;


		Q->me[0][0] =	0.00001;
        Q->me[1][1] =	0.00001;
        Q->me[2][2] =	0.00001;
		
        Q->me[3][3] =	0.0001;
        Q->me[4][4] =	0.0001;
        Q->me[5][5] =	0.0001;

        Q->me[6][6] =	0.0000000001;
        Q->me[7][7] =	0.0000000001;
        Q->me[8][8] =	0.0000000001;

        Q->me[9][9]   = 0.0000000001;
        Q->me[10][10] = 0.0000000001;
        Q->me[11][11] = 0.0000000001;
	}
    
    if(omega_prev == VNULL)
    {
        omega_prev = v_get(3);
        v_zero(omega_prev);
        
    }
    
    if(mag_vec_prev == VNULL)
    {
        mag_vec_prev = v_get(3);
        v_zero(mag_vec_prev);     
    }
    
    if(sun_vec_prev == VNULL)
    {
        sun_vec_prev = v_get(3);
        v_zero(sun_vec_prev);
    }
    
   
    if (err_sig_pt_mat == MNULL)
    {
        err_sig_pt_mat = m_get(n_err_states, n_sig_pts); 
        m_zero(err_sig_pt_mat);        
    }
    
    
    if(q_err_quat == VNULL)
    {
        q_err_quat = v_get(4);
//         q_err_quat = v_resize(q_err_quat,4);
        v_zero(q_err_quat);
    }
    
    if(err_vec == VNULL)
    {
        err_vec = v_get(3);
        v_zero(err_vec);
    }
    
    
    v_temp = v_get(9);
    
    v_resize(v_temp,3);

     
    if(x_prev == VNULL)
    {
        x_prev = v_get(n_states);
        v_zero(x_prev);
        x_prev->ve[3] = 1;
        
        quat_mul(x_prev,q_s_c,x_prev);
        
        x_prev->ve[4] = 0.0;
        x_prev->ve[5] = 0.0;
        x_prev->ve[6] = 0.0;
        
        x_prev->ve[7] = 0.0;
        x_prev->ve[8] = 0.0;
        x_prev->ve[9] = 0.0;
        
        x_prev->ve[10] = 0.0;
        x_prev->ve[11] = 0.0;
        x_prev->ve[12] = 0.0;
    }


    
    sqrt_P = m_get(n_err_states, n_err_states);
    m_zero(sqrt_P);


    //result = m_resize(result, n_err_states, n_err_states);
    result_larger = m_get(n_err_states, n_err_states);
    int n, m;
    for(n = 0; n < result->n; n++)
    {
    	for(m = 0; m < result->m; m++)
		{
			result_larger->me[m][n] = result->me[m][n];
		}
    }
    


	
	
 	//v_resize(v_temp, n_err_states);
 	V_FREE(v_temp);
 	v_temp = v_get(n_err_states);

	symmeig(Pprev, result_larger, v_temp);

	i = 0;
	for (j=0;j<n_err_states;j++){
		if(v_temp->ve[j]>=0);
		else{
			i = 1;
		}
		
	}
		
	m_copy(Pprev, result1);
	sm_mlt(sqrt_lambda, result1, result_larger);
	catchall(CHfactor(result_larger), printerr(sim_time));
	
	
	for(i=0; i<n_err_states; i++){
		for(j=i+1; j<n_err_states; j++){
			result_larger->me[i][j] = 0;
		}
	}

	expandstate(result_larger, x_prev, sig_pt);

    sig_vec_mat = m_get(n_states, n_sig_pts);
    m_zero(sig_vec_mat);
    
    
    for(j = 0; j<(n_err_states+1); j++)
    {
        
        for(i = 0; i<n_states; i++)
        {
			if(j==0)
			{
				sig_vec_mat->me[i][j] = x_prev->ve[i];
			}
            else if(j>0) 
			{
				sig_vec_mat->me[i][j] = sig_pt->me[i][j-1];
			}
		}
	}
	
	sm_mlt(-1,result_larger,result_larger);
    
    expandstate(result_larger, x_prev, sig_pt);
    
	for(j = (n_err_states+1); j<n_sig_pts; j++)
    {
        for(i = 0; i<n_states; i++)
        {
			sig_vec_mat->me[i][j] = sig_pt->me[i][j-(n_err_states+1)];
	    }
    }

    single_sig_pt = v_get(n_states); 

    
    quat_rot_vec(q_s_c, Torq_ext);
    
               
    for(j=0; j<(n_sig_pts); j++)
    {   
        //v_temp = v_resize(v_temp,n_states);
        V_FREE(v_temp);
        v_temp = v_get(n_states);
        get_col(sig_vec_mat, j, single_sig_pt);
        v_copy(single_sig_pt, v_temp);
        rk4(t, v_temp, h, Torq_prev);
        set_col(sig_vec_mat, j, v_temp);

    }
    
    v_copy(Torq_ext, Torq_prev);
    
    x_priori = v_get(n_states);
    v_zero(x_priori);
    
    
    v_resize(v_temp,n_states);
    v_zero(v_temp);
    
    for(j=0; j<n_sig_pts; j++)
    {
        get_col( sig_vec_mat, j, v_temp);
        if(j == 0)
        {
            v_mltadd(x_priori, v_temp, w_m_0, x_priori);
        }
        else 
        {
            v_mltadd(x_priori, v_temp, w_i, x_priori);
        }
        
    }

    
    v_copy(x_priori, v_temp);

    v_resize(v_temp,4);
    quat_normalize(v_temp);//zaroori hai ye
	
	
    for(i=0; i<4; i++)
    {
        x_priori->ve[i] = v_temp->ve[i];
    }
   

    v_resize(v_temp, n_states);
    v_copy(x_priori, v_temp);
    
    v_resize(v_temp, 4);
    
    quat_inv(v_temp, v_temp);
        
    
    for(i=0; i<3; i++)
    {
        x_ang_vel->ve[i] = x_priori->ve[i+4];
    }
     
    
   
    x_b_m = v_get(3);
    v_zero(x_b_m);
    x_b_g = v_get(3);
    v_zero(x_b_g);
    /////////////////////////check it!!!!!!!! checked... doesnt change much the estimate
    for(i=0; i<3; i++)
    {
        x_b_m->ve[i] = x_priori->ve[i+7];
        x_b_g->ve[i] = x_priori->ve[i+10];
    }
    
    v_temp2 = v_get(n_states);
    v_zero(v_temp2);


    
    for(j=0; j<n_sig_pts; j++)
    {
        v_resize(v_temp2, n_states);
        get_col( sig_vec_mat, j, v_temp2);

        for(i=0; i<3; i++)
        {
            err_vec->ve[i] = v_temp2->ve[i+4];
        }
        
        v_resize(v_temp2, 4);
        quat_mul(v_temp2, v_temp, q_err_quat);

        v_resize(q_err_quat, n_err_states);
        
        v_sub(err_vec, x_ang_vel, err_vec);
        for(i=3; i<6; i++)
        {
            q_err_quat->ve[i] = err_vec->ve[i-3];
        }
        
        for(i=0; i<3; i++)
        {
            err_vec->ve[i] = v_temp2->ve[i+7];
        }
        v_sub(err_vec, x_b_m, err_vec);
        for(i=6; i<9; i++)
        {
            q_err_quat->ve[i] = err_vec->ve[i-6];
        }
        
        for(i=0; i<3; i++)
        {
            err_vec->ve[i] = v_temp2->ve[i+10];
        }
        v_sub(err_vec, x_b_g, err_vec);
        for(i=9; i<12; i++)
        {
            q_err_quat->ve[i] = err_vec->ve[i-9];
        }
        
                
        set_col(err_sig_pt_mat, j, q_err_quat); 

        if(j==0){
            v_mltadd(x_err_priori, q_err_quat, w_m_0, x_err_priori);  
        }
        else{
            v_mltadd(x_err_priori, q_err_quat, w_i, x_err_priori);     
        }

    }
    
    v_resize(v_temp,n_err_states);
    for (j=0;j<13;j++)
    {
        get_col(err_sig_pt_mat, j, v_temp);
        v_sub(v_temp, x_err_priori, v_temp);
        get_dyad(v_temp, v_temp, result_larger);
        
        if(j==0){
            sm_mlt(w_c_0, result_larger, result_larger);
        }
        else{
            sm_mlt(w_i, result_larger, result_larger);
        }
        m_add(P_priori, result_larger, P_priori);
    }
    

	symmeig(P_priori, result_larger, v_temp);

	i = 0;
	for (j=0;j<n_err_states;j++){
		if(v_temp->ve[j]>=0);
		else{
			i = 1;
		}
		
	}


	m_add(P_priori, Q, P_priori);
	
	

   v_resize(v_temp,3);    
  
   meas = v_get(9);
   if (!(is_vec_equal(sun_vec, sun_vec_prev)) /*&& (eclipse==0)*/ ){
        update_sun_vec =1;
        v_copy(sun_vec, sun_vec_prev);
        v_copy(sun_vec, v_temp);
    
        normalize_vec(v_temp);
        quat_rot_vec(q_s_c, v_temp);  
        normalize_vec(v_temp);
        
        
        for(i = 0; i<3;i++){
            meas->ve[i] = v_temp->ve[i];
        }
    }
   else{
       update_sun_vec =0;
       for(i = 0; i<3;i++){
            meas->ve[i] = 0;
        }
    }
   
    
    if (!(is_vec_equal(mag_vec, mag_vec_prev)) ){
        update_mag_vec =1;
        v_copy(mag_vec, mag_vec_prev);
        v_copy(mag_vec, v_temp);
              
        normalize_vec(v_temp);
        quat_rot_vec(q_s_c, v_temp);
        normalize_vec(v_temp); 
        for(i=3; i<6; i++){
            meas->ve[i] = v_temp->ve[i-3];
        }
    }
    else{
        update_mag_vec =0;
        for(i=3; i<6; i++){
            meas->ve[i] = 0;//mag_vec_prev->ve[i-3];
        }
    }
     
    if (!(is_vec_equal(omega, omega_prev) ) ){
        update_omega =1;
        v_copy(omega, omega_prev);
        v_copy(omega, v_temp);
        
      
        quat_rot_vec(q_s_c, v_temp);
        for(i=6; i<9; i++){
            meas->ve[i] = v_temp->ve[i-6];
        }
    }
    else{
        update_omega =0;
        for(i=6; i<9; i++){
            meas->ve[i] = 0;
        }
    }    
    

    v_resize(v_temp, 9);
    v_resize(v_temp2, n_states);
    v_temp3 = v_get(3);
    
    Meas_err_mat = m_get(9, n_sig_pts);
    m_zero(Meas_err_mat);
    
    meas_priori = v_get(9);
    v_zero(meas_priori);
    
	
	    
    for(j=0; j<n_sig_pts; j++)
    {
        get_col( sig_vec_mat, j, v_temp2);
        
        if(update_omega){
           
            for(i=6;i<9;i++){
                v_temp->ve[i] = v_temp2->ve[i-2] + x_b_g->ve[i-6];
                
            }
        }
        else{
            for(i=6;i<9;i++){
                v_temp->ve[i] = 0;
            }
        }

        v_resize(v_temp2, 4); 

        if(update_sun_vec){
            for(i=0;i<3;i++){
                v_temp3->ve[i] = sun_vec_I->ve[i];
            }
            quat_rot_vec(v_temp2, v_temp3);
            normalize_vec(v_temp3);
            
            for(i=0;i<3;i++){
                v_temp->ve[i] = v_temp3->ve[i]; 
            }
			
			
        }
        else{
            for(i=0;i<3;i++){
                v_temp->ve[i] = 0;
            }
        }
        if(update_mag_vec){
            for(i=0;i<3;i++){
                v_temp3->ve[i] = mag_vec_I->ve[i];
            }
            normalize_vec(v_temp3);
            for(i=0;i<3;i++){
                v_temp3->ve[i] = v_temp3->ve[i] + x_b_m->ve[i];
            } 
            quat_rot_vec(v_temp2, v_temp3);
            normalize_vec(v_temp3);
           
            for(i=3;i<6;i++){
                v_temp->ve[i] = v_temp3->ve[i-3];
            }

			           
        }
        else{
            for(i=3;i<6;i++){
                v_temp->ve[i] = 0;
            }
        }
        
   
        set_col(Meas_err_mat, j, v_temp); 
        
        if(j==0){
            v_mltadd(meas_priori, v_temp, w_m_0, meas_priori);
        }
        else{
            v_mltadd(meas_priori, v_temp, w_i, meas_priori);  
        }
    }
	
	

	
	v_resize(v_temp, 9);

    m_resize(result_larger, 9, 9);
    m_zero(result_larger);
    
    P_zz = m_get(9, 9);
    m_zero(P_zz);
    
    iP_vv = m_get(9, 9);
    m_zero(iP_vv);
    
   
    P_xz = m_get(n_err_states, 9);
    m_zero(P_xz);
    
    v_resize(v_temp2, n_err_states);
    
    result1 = m_resize(result1,n_err_states,9);    
    
	for (j=0; j<n_sig_pts; j++)
    {
        get_col( Meas_err_mat, j, v_temp);
        
        get_col( err_sig_pt_mat, j, v_temp2);
        
	
        v_sub(v_temp, meas_priori, v_temp); 
        
        get_dyad(v_temp, v_temp, result_larger);
        
        get_dyad(v_temp2, v_temp, result1);
               
        if(j==0){
            sm_mlt(w_c_0, result_larger, result_larger);
            sm_mlt(w_c_0, result1, result1);
        }
        else{
            sm_mlt(w_i, result_larger, result_larger);
            sm_mlt(w_i, result1, result1);
        }
      
			
		m_add(P_zz, result_larger, P_zz);
        m_add(P_xz, result1, P_xz);
        
    }
	




	symmeig(P_zz, result_larger, v_temp);

	i = 0;
	for (j=0; j<9; j++){
		if(v_temp->ve[j]>=0);
		else{
			i = 1;
		}
	}


	m_add(P_zz, R, P_zz);
	
	m_inverse(P_zz, iP_vv);

	
    K = m_get(n_err_states, 9);
    m_zero(K);

    m_mlt(P_xz, iP_vv, K); 
	
	

    
    if(x_posteriori_err == VNULL)
    {
        x_posteriori_err = v_get(n_err_states);
        v_zero(x_posteriori_err);
    }
    v_resize(v_temp,9);
    
    v_sub(meas, meas_priori, v_temp);
    
    v_copy(v_temp, residual);
    mv_mlt(K, v_temp, x_posteriori_err);
     
    v_resize(v_temp2,3);
    for(i=0;i<3;i++){
        v_temp2->ve[i] = x_posteriori_err->ve[i];
    }
    
    
    for(i=4; i<n_states; i++){
       
        x_prev->ve[i] = (x_posteriori_err->ve[i-1] + x_priori->ve[i]);
    }
    
     
    
    d_res = v_norm2(v_temp2);
    v_resize(v_temp2,4);
	

	
    if(d_res<=1 /*&& d_res!=0*/){


        v_temp2->ve[0] = v_temp2->ve[0];
        v_temp2->ve[1] = v_temp2->ve[1];
        v_temp2->ve[2] = v_temp2->ve[2];
        v_temp2->ve[3] = sqrt(1-d_res); 

    }
	else//baad main daala hai
	{
		v_temp2->ve[0] = (v_temp2->ve[0])/(sqrt(1+d_res));
        v_temp2->ve[1] = (v_temp2->ve[1])/(sqrt(1+d_res));
        v_temp2->ve[2] = (v_temp2->ve[2])/(sqrt(1+d_res));
        v_temp2->ve[3] = 1/sqrt(1 + d_res);
	}
    
    v_resize(x_posteriori_err, n_states);

    for(i=(n_states-1); i>3; i--){
        x_posteriori_err->ve[i] = x_posteriori_err->ve[i-1];
    }
    for(i=0; i<4; i++){
        x_posteriori_err->ve[i] = v_temp2->ve[i];
    }

    
    quat_mul(v_temp2, x_priori, v_temp2);
   
    for(i=0;i<4;i++){
        x_prev->ve[i] = v_temp2->ve[i];
    }
   
     m_resize(result_larger, n_err_states, 9);
       
     m_mlt(K, P_zz, result_larger);
     result2 = m_get(9, n_err_states);
     
	m_transp(K,result2);
  
		
     m_resize(result1, n_err_states, n_err_states);
     m_mlt(result_larger, result2,  result1);
     v_resize(v_temp, n_err_states);
	
	 
	 m_sub(P_priori, result1, Pprev);

	 symmeig(Pprev, result1 , v_temp);

	 i = 0;
	 
     for (j=0;j<n_err_states;j++){
		 if(v_temp->ve[j]>=0);
		 else{
			 i = 1;
		 }
     }


    
	v_copy(x_prev, v_temp);
	v_resize(v_temp,4);
	v_copy(x_prev, v_temp2);
	v_resize(v_temp2,4);

	
	v_copy(x_prev, x_m_o);
	//v_resize(x_m_o, 4);

     v_resize(v_temp,3);
     quat_inv(q_s_c, v_temp2);
     v_copy( x_prev, state); 
     quat_mul(state, v_temp2, state);
		


     for(i=0; i<3; i++){
         v_temp->ve[i] = state->ve[i+4];
     }
     quat_rot_vec(v_temp2, v_temp);
     
     for(i=0; i<3; i++){
         state->ve[i+4] = v_temp->ve[i];
     }
     
    v_copy( x_posteriori_err, st_error);
    

		

    iter_num++;
    
	V_FREE(x);
	V_FREE(x_priori);
	V_FREE(x_err_priori);
	V_FREE(single_sig_pt);
	V_FREE(v_temp);
	V_FREE(q_err_quat);
	V_FREE(err_vec);
	V_FREE(v_temp2);
	V_FREE(x_ang_vel);
	V_FREE(meas);
	V_FREE(meas_priori);
	V_FREE(v_temp3);
	V_FREE(x_posteriori_err);
	V_FREE(x_b_m);
	V_FREE(x_b_g);
	
 
	M_FREE(sqrt_P);
	M_FREE(P);
	M_FREE(P_priori);
	M_FREE(sig_pt);
	M_FREE(sig_vec_mat);
	M_FREE(err_sig_pt_mat);
	M_FREE(result);
	M_FREE(result_larger);
	M_FREE(result1);
	M_FREE(Meas_err_mat);
	M_FREE(P_zz);
	M_FREE(iP_vv);
	M_FREE(P_xz);
	M_FREE(K);
	M_FREE(result2);
	M_FREE(result3);
     
}
Esempio n. 27
0
void turtle_t::exec(turtle_com_t *com)
{
  if (com->cname==F)
    {
      turtle_fwd_t* fcom = dynamic_cast<turtle_fwd_t*>(com);
      if (fcom) forward(fcom->dist);
    }
  else if (com->cname==B)
    {
      turtle_bck_t* bcom = dynamic_cast<turtle_bck_t*>(com);
      if (bcom) back(bcom->dist);
    }
  else if (com->cname==L)
    {
      turtle_lft_t* lcom = dynamic_cast<turtle_lft_t*>(com);
      if (lcom) turn_left(lcom->angl);
    }
  else if (com->cname==R)
    {
      turtle_rht_t* rcom = dynamic_cast<turtle_rht_t*>(com);
      if (rcom) turn_right(rcom->angl);
    }
  else if (com->cname==MF)
    {
      turtle_mfwd_t* mfcom = dynamic_cast<turtle_mfwd_t*>(com);
      if (mfcom) forward_move(mfcom->dist);
    }
  else if (com->cname==MB)
    {
      turtle_mbck_t* mbcom = dynamic_cast<turtle_mbck_t*>(com);
      if (mbcom) backward_move(mbcom->dist);
    }
  else if (com->cname==CLS)
    {
      turtle_cls_t* clscom = dynamic_cast<turtle_cls_t*>(com);
      if (clscom) clear();
    }
  else if (com->cname==RESET)
    {
      turtle_rst_t* rstcom = dynamic_cast<turtle_rst_t*>(com);
      if (rstcom) reset();
    }
  else if (com->cname==COL)
    {
      turtle_col_t* colcom = dynamic_cast<turtle_col_t*>(com);
      if (colcom) set_col(colcom->r, colcom->g, colcom->b);
    }
  else if (com->cname==BGCOL)
    {
      turtle_bgcol_t* bgcolcom = dynamic_cast<turtle_bgcol_t*>(com);
      if (bgcolcom) set_bgcol(bgcolcom->r, bgcolcom->g, bgcolcom->b);
    }
  else if (com->cname==SCALE)
    {
      turtle_scale_t* scalecom = dynamic_cast<turtle_scale_t*>(com);
      if (scalecom) scale(scalecom->s);
    }
  else if (com->cname==PAUSE)
    {
      turtle_pause_t* pausecom = dynamic_cast<turtle_pause_t*>(com);
      if (pausecom) pause(pausecom->t);
    }
  else if (com->cname==REPEAT)
    {
      turtle_rep_t *repcom = dynamic_cast<turtle_rep_t*>(com);

      if (repcom)
	{
	  unsigned int times = repcom->times;
	  turtle_com_list_t sublist = repcom->replist;
	  repeat(times, sublist);
	}
     }
  else if ((com->cname==ENDREP) || (com->cname==END) || (com->cname==BEGIN)) 
    {
      //These commands are place holders and used for program structure
      //But no execution is necessary - generate a NoOP
      ;
    }
  else
    {
      std::cerr<<"Unknown Command: Ignoring"<<std::endl;
      exit(-1);
    }
}
Esempio n. 28
0
VEC *iter_mgcr(ITER *ip)
#endif
{
    STATIC VEC *As=VNULL, *beta=VNULL, *alpha=VNULL, *z=VNULL;
    STATIC MAT *N=MNULL, *H=MNULL;

    VEC *rr, v, s;  /* additional pointer and structures */
    Real nres;      /* norm of a residual */
    Real dd;        /* coefficient d_i */
    int i,j;
    int done;      /* if TRUE then stop the iterative process */
    int dim;       /* dimension of the problem */

    /* ip cannot be NULL */
    if (ip == INULL) error(E_NULL,"mgcr");
    /* Ax, b and stopping criterion must be given */
    if (! ip->Ax || ! ip->b || ! ip->stop_crit)
        error(E_NULL,"mgcr");
    /* at least one direction vector must exist */
    if ( ip->k <= 0) error(E_BOUNDS,"mgcr");
    /* if the vector x is given then b and x must have the same dimension */
    if ( ip->x && ip->x->dim != ip->b->dim)
        error(E_SIZES,"mgcr");
    if (ip->eps <= 0.0) ip->eps = MACHEPS;

    dim = ip->b->dim;
    As = v_resize(As,dim);
    alpha = v_resize(alpha,ip->k);
    beta = v_resize(beta,ip->k);

    MEM_STAT_REG(As,TYPE_VEC);
    MEM_STAT_REG(alpha,TYPE_VEC);
    MEM_STAT_REG(beta,TYPE_VEC);

    H = m_resize(H,ip->k,ip->k);
    N = m_resize(N,ip->k,dim);

    MEM_STAT_REG(H,TYPE_MAT);
    MEM_STAT_REG(N,TYPE_MAT);

    /* if a preconditioner is defined */
    if (ip->Bx) {
        z = v_resize(z,dim);
        MEM_STAT_REG(z,TYPE_VEC);
    }

    /* if x is NULL then it is assumed that x has
       entries with value zero */
    if ( ! ip->x ) {
        ip->x = v_get(ip->b->dim);
        ip->shared_x = FALSE;
    }

    /* v and s are additional pointers to rows of N */
    /* they must have the same dimension as rows of N */
    v.dim = v.max_dim = s.dim = s.max_dim = dim;


    done = FALSE;
    for (ip->steps = 0; ip->steps < ip->limit; ) {
        (*ip->Ax)(ip->A_par,ip->x,As);         /* As = A*x */
        v_sub(ip->b,As,As);                    /* As = b - A*x */
        rr = As;                               /* rr is an additional pointer */

        /* if a preconditioner is defined */
        if (ip->Bx) {
            (*ip->Bx)(ip->B_par,As,z);               /* z = B*(b-A*x)  */
            rr = z;
        }

        /* norm of the residual */
        nres = v_norm2(rr);
        dd = nres;                            /* dd = ||r_i||  */

        /* check if the norm of the residual is zero */
        if (ip->steps == 0) {
            /* information for a user */
            if (ip->info) (*ip->info)(ip,nres,As,rr);
            ip->init_res = fabs(nres);
        }

        if (nres == 0.0) {
            /* iterative process is finished */
            done = TRUE;
            break;
        }

        /* save this residual in the first row of N */
        v.ve = N->me[0];
        v_copy(rr,&v);

        for (i = 0; i < ip->k && ip->steps < ip->limit; i++) {
            ip->steps++;
            v.ve = N->me[i];                /* pointer to a row of N (=s_i) */
            /* note that we must use here &v, not v */
            (*ip->Ax)(ip->A_par,&v,As);
            rr = As;                        /* As = A*s_i */
            if (ip->Bx) {
                (*ip->Bx)(ip->B_par,As,z);    /* z = B*A*s_i  */
                rr = z;
            }

            if (i < ip->k - 1) {
                s.ve = N->me[i+1];         /* pointer to a row of N (=s_{i+1}) */
                v_copy(rr,&s);                   /* s_{i+1} = B*A*s_i */
                for (j = 0; j <= i-1; j++) {
                    v.ve = N->me[j+1];      /* pointer to a row of N (=s_{j+1}) */
                    /* beta->ve[j] = in_prod(&v,rr); */      /* beta_{j,i} */
                    /* modified Gram-Schmidt algorithm */
                    beta->ve[j] = in_prod(&v,&s);  	         /* beta_{j,i} */
                    /* s_{i+1} -= beta_{j,i}*s_{j+1} */
                    v_mltadd(&s,&v,- beta->ve[j],&s);
                }

                /* beta_{i,i} = ||s_{i+1}||_2 */
                beta->ve[i] = nres = v_norm2(&s);
                if ( nres <= MACHEPS*ip->init_res) {
                    /* s_{i+1} == 0 */
                    i--;
                    done = TRUE;
                    break;
                }
                sv_mlt(1.0/nres,&s,&s);           /* normalize s_{i+1} */

                v.ve = N->me[0];
                alpha->ve[i] = in_prod(&v,&s);     /* alpha_i = (s_0 , s_{i+1}) */

            }
            else {
                for (j = 0; j <= i-1; j++) {
                    v.ve = N->me[j+1];      /* pointer to a row of N (=s_{j+1}) */
                    beta->ve[j] = in_prod(&v,rr);       /* beta_{j,i} */
                }

                nres = in_prod(rr,rr);                 /* rr = B*A*s_{k-1} */
                for (j = 0; j <= i-1; j++)
                    nres -= beta->ve[j]*beta->ve[j];

                if (sqrt(fabs(nres)) <= MACHEPS*ip->init_res)  {
                    /* s_k is zero */
                    i--;
                    done = TRUE;
                    break;
                }
                if (nres < 0.0) { /* do restart */
                    i--;
                    ip->steps--;
                    break;
                }
                beta->ve[i] = sqrt(nres);         /* beta_{k-1,k-1} */

                v.ve = N->me[0];
                alpha->ve[i] = in_prod(&v,rr);
                for (j = 0; j <= i-1; j++)
                    alpha->ve[i] -= beta->ve[j]*alpha->ve[j];
                alpha->ve[i] /= beta->ve[i];                /* alpha_{k-1} */

            }

            set_col(H,i,beta);

            /* other method of computing dd */
            /* if (fabs((double)alpha->ve[i]) > dd)  {
                nres = - dd*dd + alpha->ve[i]*alpha->ve[i];
                nres = sqrt((double) nres);
                if (ip->info) (*ip->info)(ip,-nres,VNULL,VNULL);
                break;
             }  */
            /* to avoid overflow/underflow in computing dd */
            /* dd *= cos(asin((double)(alpha->ve[i]/dd))); */

            nres = alpha->ve[i]/dd;
            if (fabs(nres-1.0) <= MACHEPS*ip->init_res)
                dd = 0.0;
            else {
                nres = 1.0 - nres*nres;
                if (nres < 0.0) {
                    nres = sqrt((double) -nres);
                    if (ip->info) (*ip->info)(ip,-dd*nres,VNULL,VNULL);
                    break;
                }
                dd *= sqrt((double) nres);
            }

            if (ip->info) (*ip->info)(ip,dd,VNULL,VNULL);
            if ( ip->stop_crit(ip,dd,VNULL,VNULL) ) {
                /* stopping criterion is satisfied */
                done = TRUE;
                break;
            }

        } /* end of for */

        if (i >= ip->k) i = ip->k - 1;

        /* use (i+1) by (i+1) submatrix of H */
        H = m_resize(H,i+1,i+1);
        alpha = v_resize(alpha,i+1);
        Usolve(H,alpha,alpha,0.0);       /* c_i is saved in alpha */

        for (j = 0; j <= i; j++) {
            v.ve = N->me[j];
            v_mltadd(ip->x,&v,alpha->ve[j],ip->x);
        }


        if (done) break;              /* stop the iterative process */
        alpha = v_resize(alpha,ip->k);
        H = m_resize(H,ip->k,ip->k);

    }  /* end of while */

#ifdef THREADSAFE
    V_FREE(As);
    V_FREE(beta);
    V_FREE(alpha);
    V_FREE(z);
    M_FREE(N);
    M_FREE(H);
#endif

    return ip->x;                    /* return the solution */
}
Esempio n. 29
0
/*
 * This program demonstrates the Cdk matrix widget.
 */
int main (int argc, char **argv)
{
   /* Declare local variables. */
   CDKSCREEN *cdkscreen     = 0;
   CDKMATRIX *form_prov    = 0;
   WINDOW *cursesWin        = 0;
   char *title          = 0;
   int rows         = 5;
   int cols         = 1;
   int vrows            = 5;
   int vcols            = 1;
   char *coltitle[10], *rowtitle[10], *mesg[10];
   int colwidth[10], colvalue[10];

 //  CDK_PARAMS params;

 //  CDKparseParams (argc, argv, &params, CDK_MIN_PARAMS);

   /* Set up CDK. */
   cursesWin = initscr();
   cdkscreen = initCDKScreen (cursesWin);

   /* Start CDK Colors. */
   initCDKColor();

   /* Create the horizontal and vertical matrix labels. */
#define set_col(n, width, string) \
   coltitle[n] = string;   colwidth[n] = width ; colvalue[n] = vUMIXED

   set_col(1, 7, "</B/5>Proveedor");
   set_col(2, 7, "</B/33>Lec 1");
   set_col(3, 7, "</B/33>Lec 2");
   set_col(4, 7, "</B/33>Lec 3");
   set_col(5, 1, "</B/7>Flag");

#define set_row(n, string) \
   rowtitle[n] = "</B/8>" string

   set_row(1, "ID");
   set_row(2, "Nombre");
   set_row(3, "Apellido");
   set_row(4, "Correo");
   set_row(5, "Direccion");
   set_row(6, "Course 6");
   set_row(7, "Course 7");
   set_row(8, "Course 8");

   /* Create the title. */
   /* Create the matrix object. */
   form_prov = newCDKMatrix (cdkscreen,
                   CENTER,
                   CENTER,
                  rows, cols, vrows, vcols,
                  title, rowtitle, coltitle,
                  colwidth, colvalue,
                  -1, -1, '.',
                  COL, TRUE,
                  TRUE,
                   FALSE);

   /* Check to see if the matrix is null. */
   if (form_prov == 0)
   {
      /* Clean up. */
      destroyCDKScreen (cdkscreen);
      endCDK();

      /* Print out a little message. */
      printf ("Oops. Can't seem to create the matrix widget. Is the window too small ?\n");
      exit (EXIT_FAILURE);
   }

   /* Activate the matrix. */
   activateCDKMatrix (form_prov, 0);

   /* Check if the user hit escape or not. */
   if (form_prov->exitType == vESCAPE_HIT)
   {
      mesg[0] = "<C>You hit escape. No information passed back.";
      mesg[1] = "",
      mesg[2] = "<C>Press any key to continue.";
      popupLabel (cdkscreen, mesg, 3);
   }
   else if (form_prov->exitType == vNORMAL)
   {
      char temp[80];

      sprintf(temp, "Current cell (%d,%d)", form_prov->crow, form_prov->ccol);
      mesg[0] = "<L>You exited the matrix normally.";
      mesg[1] = temp;
      mesg[2] = "<L>To get the contents of the matrix cell, you can";
      mesg[3] = "<L>use getCDKMatrixCell():";
      mesg[4] = getCDKMatrixCell(form_prov, form_prov->crow, form_prov->ccol);
      mesg[5] = "";
      mesg[6] = "<C>Press any key to continue.";
      popupLabel (cdkscreen, mesg, 7);
   }

   /* Clean up. */
   destroyCDKMatrix (form_prov);
   destroyCDKScreen (cdkscreen);
   endCDK();
   exit (EXIT_SUCCESS);
}
Esempio n. 30
0
/***************************************************************
*   void malloc_command
*       Calcula la cantidad de RAM que tiene la máquina.
*
****************************************************************/
void malloc_command(char* params) {
    char option[20];
    unsigned int size = 0;
    unsigned int i = 0;

    memset_custom(option, sizeof(option), 0);

    sscanf_custom(params, "%s %d", option, &size);

    if( strcmp(option, "get") == 0 ) {
        if( size > minimum_segment_size ) {
            void* ptr = malloc_custom(size, 0);
            newline();
            if(ptr == 0) {
                vprintf_custom("No hay espacio para alocar %d bytes.", size);
            } else {
                for(i = 0; info[i].ptr != 0; i++) {}

                if( i < INFO_TABLE_SIZE ) {
                    vprintf_custom("Se aloco la memoria. Esta en la direccion %x", ptr);

                    info[i].ptr = ptr;
                    info[i].size = size;
                } else {
                    newline();
                    vprintf_custom("No puedes alocar mas memoria porque llenaste la"
                            "tabla que tiene las referencias de las alocaciones, y que"
                            "usa el comando malloc info.");
                }
            }
        } else {
            newline();
            vprintf_custom("Debes pedir %d bytes o mas.", minimum_segment_size);
        }
    } else if( strcmp(option, "info") == 0) {
        clear_vscreen();
        set_col(2); set_row(1);
        vprintf_custom("Los valores de la izquierda son la direcciones "
                "en donde empieza cada segmento, los de la derecha son los "
                "tamanos de los mismos.");
        set_col(2); set_row(3);
        for(i = 0; i < INFO_TABLE_SIZE; i++) {
            if( info[i].ptr != 0 ) {
                set_col(2);
                vprintf_custom("%x %d", info[i].ptr, info[i].size);
                newline();
                if( i > SCREEN_WIDTH - 3 ) {
                    set_col(20); set_row(3);
                }
            }
        }
        if (i == 0) {
            vprintf_custom("Aun no alocaste ninguna porcion de memoria.");
        }
    } else if( strcmp(option, "") == 0 ) {
        newline();
        vprintf_custom("Debes ingresar al menos una opcion para"
                " ser usada por esta funcion.");
    } else if(strcmp(option, "test") == 0) {
        malloc_tests(); 
        q_to_continue(SCREEN_LENGTH - 1, 2, 1);
    } else {
        newline();
        vprintf_custom("El comando que ingresaste no es valido.");
    }
    print_vscreen();

}